guided AHRS calibration procedure

iltis42 · Jan 21, 2024 · 12599da · 12599da
1 parent 346d3e0
commit 12599da
Show file tree

Hide file tree

Showing 3 changed files with 57 additions and 8 deletions.
diff --git a/main/KalmanMPU6050.cpp b/main/KalmanMPU6050.cpp
@@ -479,4 +479,41 @@ void IMU::applyImuReference(const float gAA, const Quaternion& basic)
 {
 	ref_rot = Quaternion(deg2rad(gAA), vector_ijk(0,1,0)) * basic; // rotate positive around Y
 	ref_rot.normalize();
-}
+}
+
+void IMU::doImuCalibration( SetupMenuSelect *p ){
+	p->ucg->setFont( ucg_font_ncenR14_hr, true );
+	p->clear();
+	p->ucg->setPrintPos( 1, 30 );
+	p->ucg->printf( "AHRS calibration" );
+	p->ucg->setPrintPos( 1, 60 );
+	p->ucg->printf( "Ensure ground is flat," );
+	p->ucg->setPrintPos( 1, 90 );
+	p->ucg->printf( "with no inclination." );
+	p->ucg->setPrintPos( 1, 120 );
+	p->ucg->printf( "Press button to start" );
+	while( !p->readSwitch() ){ delay( 100 ); }
+	p->clear();
+	p->ucg->setPrintPos( 1, 30 );
+	p->ucg->printf( "Now put down RIGHT wing" );
+	p->ucg->setPrintPos( 1, 60 );
+	p->ucg->printf( "on the ground," );
+	p->ucg->setPrintPos( 1, 90 );
+	p->ucg->printf( "then press button.." );
+	while( !p->readSwitch() ){ delay( 100 ); }
+	IMU::getAccelSamplesAndCalib(IMU_RIGHT);
+	p->clear();
+	p->ucg->setPrintPos( 1, 30 );
+	p->ucg->printf( "Now put down LEFT wing" );
+	p->ucg->setPrintPos( 1, 60 );
+	p->ucg->printf( "on the ground," );
+	p->ucg->setPrintPos( 1, 90 );
+	p->ucg->printf( "then press button.." );
+	while( !p->readSwitch() ){ delay( 100 ); }
+	IMU::getAccelSamplesAndCalib(IMU_LEFT);
+	p->ucg->setPrintPos( 1, 130 );
+	p->ucg->printf( "Finished!" );
+	delay(2000);
+}
+
+
diff --git a/main/KalmanMPU6050.h b/main/KalmanMPU6050.h
@@ -21,6 +21,7 @@
 
 #include "Arduino.h"
 #include "vector_3d.h"
+#include "SetupMenuSelect.h"
 
 class Quaternion;
 
@@ -35,6 +36,8 @@ class Quaternion;
 #define DEG_TO_RAD (M_PI/180.0)
 #endif // DEG_TO_RAD
 
+typedef enum e_imudir { IMU_RIGHT=1, IMU_LEFT=2 } e_imudir_t;
+
 typedef struct kalman_t
 {
 	double Q_angle;   // Process noise variance for the accelerometer
@@ -154,6 +157,7 @@ class IMU
   static void getGyroSamplesAndZero();
   static void defaultImuReference();
   static void applyImuReference(const float gAA, const Quaternion& basic);
+  static void doImuCalibration( SetupMenuSelect *p );
 
 private:
   static float getGyroYawDelta();

diff --git a/main/SetupMenu.cpp b/main/SetupMenu.cpp
@@ -283,9 +283,8 @@ static int imu_calib( SetupMenuSelect *p )
 		case 0:
 			break; // cancel
 		case 1:
-		case 2:
 			// collect samples
-			IMU::getAccelSamplesAndCalib(sel);
+			IMU::doImuCalibration(p);
 			break;
 		case 3:
 			// reset to default
@@ -294,7 +293,7 @@ static int imu_calib( SetupMenuSelect *p )
 		default:
 			break;
 	}
-
+	p->setSelect(0);
 	return 0;
 }
 
@@ -471,6 +470,15 @@ static int compassSensorCalibrateAction( SetupMenuSelect *p )
 	return 0;
 }
 
+static int ahrsCalibrateAction( SetupMenuSelect *p )
+{
+	if( p->getSelect() == 1 ){ // Start
+		IMU::doImuCalibration(p);
+	}
+	p->setSelect( 0 );
+	return 0;
+}
+
 SetupMenu::SetupMenu() : MenuEntry() {
 	highlight = -1;
 	_parent = 0;
@@ -1749,12 +1757,12 @@ void SetupMenu::system_menu_create_hardware_rotary( MenuEntry *top ){
 
 
 void SetupMenu::system_menu_create_ahrs_calib( MenuEntry *top ){
-	SetupMenuSelect* ahrs_calib_collect = new SetupMenuSelect( PROGMEM"Wing down samples", RST_NONE, imu_calib, false);
-	ahrs_calib_collect->setHelp(PROGMEM"Collect samples with right and left wing on the ground in any sequence. Start sampling by selecting the proper side and pressing the button.");
+	SetupMenuSelect* ahrs_calib_collect = new SetupMenuSelect( PROGMEM"Axis calibration", RST_NONE, imu_calib, false);
+	ahrs_calib_collect->setHelp(PROGMEM"Calibrate IMU axis on flat leveled ground ground with no inclination. Run the procedure by selecting Start.");
 	ahrs_calib_collect->addEntry("Cancel");
-	ahrs_calib_collect->addEntry("Right");
-	ahrs_calib_collect->addEntry("Left");
+	ahrs_calib_collect->addEntry("Start");
 	ahrs_calib_collect->addEntry("Reset");
+
 	SetupMenuValFloat* ahrs_ground_aa = new SetupMenuValFloat( PROGMEM"Ground angle of attack", "°", -5, 20, 1, imu_gaa, false, &glider_ground_aa);
 	ahrs_ground_aa->setHelp(PROGMEM"Angle of attack with tail skid on the ground to adjust the AHRS reference. Change this any time to correct the AHRS horizon level.");
 	ahrs_ground_aa->setPrecision( 0 );