APM: adapt ArduPlane for AHRS framework

ArduPlane/ArduPlane.pde

@@ -40,8 +40,7 @@ version 2.1 of the License, or (at your option) any later version.
 #include <AP_Math.h>        // ArduPilot Mega Vector/Matrix math Library
 #include <AP_InertialSensor.h> // Inertial Sensor (uncalibated IMU) Library
 #include <AP_IMU.h>         // ArduPilot Mega IMU Library
-#include <AP_DCM.h>         // ArduPilot Mega DCM Library
-#include <AP_Quaternion.h>  // Madgwick quaternion system
+#include <AP_AHRS.h>         // ArduPilot Mega DCM Library
 #include <PID.h>            // PID library
 #include <RC_Channel.h>     // RC Channel Library
 #include <AP_RangeFinder.h>	// Range finder library
@@ -194,12 +193,9 @@ AP_GPS_None     g_gps_driver(NULL);
 AP_IMU_INS imu( &ins );
 
 #if QUATERNION_ENABLE == ENABLED
-  // this shouldn't be called dcm of course, but until we
-  // decide to actually use something else, I don't want the patch
-  // size to be huge
-  AP_Quaternion dcm(&imu, g_gps);
+  AP_AHRS_Quaternion ahrs(&imu, g_gps);
 #else
- AP_DCM  dcm(&imu, g_gps);
+  AP_AHRS_DCM  ahrs(&imu, g_gps);
 #endif
 
 #elif HIL_MODE == HIL_MODE_SENSORS
@@ -210,14 +206,14 @@ AP_Compass_HIL          compass;
 AP_GPS_HIL              g_gps_driver(NULL);
 AP_InertialSensor_Oilpan ins( &adc );
 AP_IMU_Shim imu;
-AP_DCM  dcm(&imu, g_gps);
+AP_AHRS_DCM  ahrs(&imu, g_gps);
 
 #elif HIL_MODE == HIL_MODE_ATTITUDE
 AP_ADC_HIL              adc;
-AP_DCM_HIL              dcm;
+AP_IMU_Shim             imu; // never used
+AP_AHRS_HIL             ahrs(&imu, g_gps);
 AP_GPS_HIL              g_gps_driver(NULL);
 AP_Compass_HIL          compass; // never used
-AP_IMU_Shim             imu; // never used
 AP_Baro_BMP085_HIL      barometer;
 
 #else
@@ -261,7 +257,7 @@ AP_Relay relay;
 // Camera/Antenna mount tracking and stabilisation stuff
 // --------------------------------------
 #if MOUNT == ENABLED
-AP_Mount camera_mount(g_gps, &dcm);
+AP_Mount camera_mount(g_gps, &ahrs);
 #endif
 
 
@@ -719,18 +715,18 @@ static void fast_loop()
 	}
 
 	#if HIL_MODE == HIL_MODE_SENSORS
-		// update hil before dcm update
+		// update hil before AHRS update
 		gcs_update();
 	#endif
 
-    dcm.update_DCM();
+    ahrs.update();
 
 	// uses the yaw from the DCM to give more accurate turns
 	calc_bearing_error();
 
 	# if HIL_MODE == HIL_MODE_DISABLED
 		if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
-			Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (uint16_t)dcm.yaw_sensor);
+			Log_Write_Attitude((int)ahrs.roll_sensor, (int)ahrs.pitch_sensor, (uint16_t)ahrs.yaw_sensor);
 
 		if (g.log_bitmask & MASK_LOG_RAW)
 			Log_Write_Raw();
@@ -784,19 +780,19 @@ static void medium_loop()
 
 			#if HIL_MODE != HIL_MODE_ATTITUDE
             if (g.compass_enabled && compass.read()) {
-                dcm.set_compass(&compass);
+                ahrs.set_compass(&compass);
                 // Calculate heading
-                Matrix3f m = dcm.get_dcm_matrix();
+                Matrix3f m = ahrs.get_dcm_matrix();
                 compass.calculate(m);
                 compass.null_offsets(m);
             } else {
-                dcm.set_compass(NULL);
+                ahrs.set_compass(NULL);
             }
 			#endif
 /*{
-Serial.print(dcm.roll_sensor, DEC);	Serial.printf_P(PSTR("\t"));
-Serial.print(dcm.pitch_sensor, DEC);	Serial.printf_P(PSTR("\t"));
-Serial.print(dcm.yaw_sensor, DEC);	Serial.printf_P(PSTR("\t"));
+Serial.print(ahrs.roll_sensor, DEC);	Serial.printf_P(PSTR("\t"));
+Serial.print(ahrs.pitch_sensor, DEC);	Serial.printf_P(PSTR("\t"));
+Serial.print(ahrs.yaw_sensor, DEC);	Serial.printf_P(PSTR("\t"));
 Vector3f tempaccel = imu.get_accel();
 Serial.print(tempaccel.x, DEC);	Serial.printf_P(PSTR("\t"));
 Serial.print(tempaccel.y, DEC);	Serial.printf_P(PSTR("\t"));
@@ -850,7 +846,7 @@ Serial.println(tempaccel.z, DEC);
 
 			#if HIL_MODE != HIL_MODE_ATTITUDE
 				if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_ATTITUDE_FAST))
-					Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (uint16_t)dcm.yaw_sensor);
+					Log_Write_Attitude((int)ahrs.roll_sensor, (int)ahrs.pitch_sensor, (uint16_t)ahrs.yaw_sensor);
 
 				if (g.log_bitmask & MASK_LOG_CTUN)
 					Log_Write_Control_Tuning();

ArduPlane/Attitude.pde

@@ -38,7 +38,7 @@ static void stabilize()
         // handle this is to ensure both go in the same direction from
         // zero
         nav_roll += 18000;
-        if (dcm.roll_sensor < 0) nav_roll -= 36000;
+        if (ahrs.roll_sensor < 0) nav_roll -= 36000;
     }
 
 	// For Testing Only
@@ -48,11 +48,11 @@ static void stabilize()
 
 	// Calculate dersired servo output for the roll
 	// ---------------------------------------------
-	g.channel_roll.servo_out = g.pidServoRoll.get_pid((nav_roll - dcm.roll_sensor), delta_ms_fast_loop, speed_scaler);
+	g.channel_roll.servo_out = g.pidServoRoll.get_pid((nav_roll - ahrs.roll_sensor), delta_ms_fast_loop, speed_scaler);
 	long tempcalc = nav_pitch +
-	        fabs(dcm.roll_sensor * g.kff_pitch_compensation) +
+	        fabs(ahrs.roll_sensor * g.kff_pitch_compensation) +
 	        (g.channel_throttle.servo_out * g.kff_throttle_to_pitch) -
-	        (dcm.pitch_sensor - g.pitch_trim);
+	        (ahrs.pitch_sensor - g.pitch_trim);
     if (inverted_flight) {
         // when flying upside down the elevator control is inverted
         tempcalc = -tempcalc;
@@ -120,7 +120,7 @@ static void stabilize()
 
 static void crash_checker()
 {
-	if(dcm.pitch_sensor < -4500){
+	if(ahrs.pitch_sensor < -4500){
 		crash_timer = 255;
 	}
 	if(crash_timer > 0)
@@ -215,7 +215,7 @@ static void calc_nav_roll()
 	nav_roll = constrain(nav_roll, -g.roll_limit.get(), g.roll_limit.get());
 
 	Vector3f omega;
-	omega = dcm.get_gyro();
+	omega = ahrs.get_gyro();
 
 	// rate limiter
 	long rate		= degrees(omega.z) * 100; 										// 3rad = 17188 , 6rad = 34377

ArduPlane/GCS_Mavlink.pde

@@ -108,13 +108,13 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan)
 
 static NOINLINE void send_attitude(mavlink_channel_t chan)
 {
-    Vector3f omega = dcm.get_gyro();
+    Vector3f omega = ahrs.get_gyro();
     mavlink_msg_attitude_send(
         chan,
         micros(),
-        dcm.roll,
-        dcm.pitch - radians(g.pitch_trim*0.01),
-        dcm.yaw,
+        ahrs.roll,
+        ahrs.pitch - radians(g.pitch_trim*0.01),
+        ahrs.yaw,
         omega.x,
         omega.y,
         omega.z);
@@ -306,7 +306,7 @@ static void NOINLINE send_meminfo(mavlink_channel_t chan)
 
 static void NOINLINE send_location(mavlink_channel_t chan)
 {
-    Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
+    Matrix3f rot = ahrs.get_dcm_matrix(); // neglecting angle of attack for now
     mavlink_msg_global_position_int_send(
         chan,
         millis(),
@@ -434,7 +434,7 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
         chan,
         (float)airspeed / 100.0,
         (float)g_gps->ground_speed / 100.0,
-        (dcm.yaw_sensor / 100) % 360,
+        (ahrs.yaw_sensor / 100) % 360,
         (uint16_t)(100 * (g.channel_throttle.norm_output() / 2.0 + 0.5)), // scale -1,1 to 0-100
         current_loc.alt / 100.0,
         0);
@@ -486,18 +486,18 @@ static void NOINLINE send_raw_imu3(mavlink_channel_t chan)
                                     imu.ax(), imu.ay(), imu.az());
 }
 
-static void NOINLINE send_dcm(mavlink_channel_t chan)
+static void NOINLINE send_ahrs(mavlink_channel_t chan)
 {
-    Vector3f omega_I = dcm.get_gyro_drift();
-    mavlink_msg_dcm_send(
+    Vector3f omega_I = ahrs.get_gyro_drift();
+    mavlink_msg_ahrs_send(
         chan,
         omega_I.x,
         omega_I.y,
         omega_I.z,
-        dcm.get_accel_weight(),
-        dcm.get_renorm_val(),
-        dcm.get_error_rp(),
-        dcm.get_error_yaw());
+        0,
+        0,
+        ahrs.get_error_rp(),
+        ahrs.get_error_yaw());
 }
 
 #endif // HIL_MODE != HIL_MODE_ATTITUDE
@@ -679,16 +679,16 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
         break;
 #endif
 
-    case MSG_DCM:
+    case MSG_AHRS:
 #if HIL_MODE != HIL_MODE_ATTITUDE
-        CHECK_PAYLOAD_SIZE(DCM);
-        send_dcm(chan);
+        CHECK_PAYLOAD_SIZE(AHRS);
+        send_ahrs(chan);
 #endif
         break;
 
     case MSG_SIMSTATE:
 #ifdef DESKTOP_BUILD
-        CHECK_PAYLOAD_SIZE(DCM);
+        CHECK_PAYLOAD_SIZE(SIMSTATE);
         send_simstate(chan);
 #endif
         break;
@@ -932,7 +932,7 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
 		}
 
 		if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){
-			send_message(MSG_DCM);
+			send_message(MSG_AHRS);
 			send_message(MSG_HWSTATUS);
 		}
 	}
@@ -1928,8 +1928,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 			
 			#else
 
-			// set dcm hil sensor
-            dcm.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed,
+			// set AHRS hil sensor
+            ahrs.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed,
             packet.pitchspeed,packet.yawspeed);
 
 			#endif
@@ -1945,8 +1945,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             mavlink_attitude_t packet;
             mavlink_msg_attitude_decode(msg, &packet);
 
-            // set dcm hil sensor
-            dcm.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed,
+            // set AHRS hil sensor
+            ahrs.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed,
             packet.pitchspeed,packet.yawspeed);
             break;
         }

ArduPlane/Log.pde

@@ -239,15 +239,15 @@ static void Log_Write_Performance()
 	DataFlash.WriteLong(millis()- perf_mon_timer);
 	DataFlash.WriteInt((int16_t)mainLoop_count);
 	DataFlash.WriteInt(G_Dt_max);
-	DataFlash.WriteByte(dcm.gyro_sat_count);
+	DataFlash.WriteByte(0);
 	DataFlash.WriteByte(imu.adc_constraints);
-	DataFlash.WriteByte(dcm.renorm_range_count);
-	DataFlash.WriteByte(dcm.renorm_blowup_count);
+	DataFlash.WriteByte(ahrs.renorm_range_count);
+	DataFlash.WriteByte(ahrs.renorm_blowup_count);
 	DataFlash.WriteByte(gps_fix_count);
-	DataFlash.WriteInt((int)(dcm.get_health() * 1000));
-	DataFlash.WriteInt((int)(dcm.get_gyro_drift().x * 1000));
-	DataFlash.WriteInt((int)(dcm.get_gyro_drift().y * 1000));
-	DataFlash.WriteInt((int)(dcm.get_gyro_drift().z * 1000));
+	DataFlash.WriteInt(1); // AHRS health
+	DataFlash.WriteInt((int)(ahrs.get_gyro_drift().x * 1000));
+	DataFlash.WriteInt((int)(ahrs.get_gyro_drift().y * 1000));
+	DataFlash.WriteInt((int)(ahrs.get_gyro_drift().z * 1000));
 	DataFlash.WriteInt(pmTest1);
 	DataFlash.WriteByte(END_BYTE);
 }
@@ -300,10 +300,10 @@ static void Log_Write_Control_Tuning()
 	DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG);
 	DataFlash.WriteInt((int)(g.channel_roll.servo_out));
 	DataFlash.WriteInt((int)nav_roll);
-	DataFlash.WriteInt((int)dcm.roll_sensor);
+	DataFlash.WriteInt((int)ahrs.roll_sensor);
 	DataFlash.WriteInt((int)(g.channel_pitch.servo_out));
 	DataFlash.WriteInt((int)nav_pitch);
-	DataFlash.WriteInt((int)dcm.pitch_sensor);
+	DataFlash.WriteInt((int)ahrs.pitch_sensor);
 	DataFlash.WriteInt((int)(g.channel_throttle.servo_out));
 	DataFlash.WriteInt((int)(g.channel_rudder.servo_out));
 	DataFlash.WriteInt((int)(accel.y * 10000));
@@ -317,7 +317,7 @@ static void Log_Write_Nav_Tuning()
 	DataFlash.WriteByte(HEAD_BYTE1);
 	DataFlash.WriteByte(HEAD_BYTE2);
 	DataFlash.WriteByte(LOG_NAV_TUNING_MSG);
-	DataFlash.WriteInt((uint16_t)dcm.yaw_sensor);
+	DataFlash.WriteInt((uint16_t)ahrs.yaw_sensor);
 	DataFlash.WriteInt((int)wp_distance);
 	DataFlash.WriteInt((uint16_t)target_bearing);
 	DataFlash.WriteInt((uint16_t)nav_bearing);

ArduPlane/commands_logic.pde

@@ -279,7 +279,7 @@ static bool verify_takeoff()
 		if (hold_course == -1) {
 			// save our current course to take off
 			if(g.compass_enabled) {
-				hold_course = dcm.yaw_sensor;
+				hold_course = ahrs.yaw_sensor;
 			} else {
 				hold_course = g_gps->ground_course;
 			}
@@ -326,7 +326,7 @@ static bool verify_land()
 			// be quite large at this point, and that could induce a
 			// sudden large roll correction which is very nasty at
 			// this point in the landing.
-			hold_course = dcm.yaw_sensor;
+			hold_course = ahrs.yaw_sensor;
 		}
 	}
 

ArduPlane/defines.h

@@ -123,7 +123,7 @@ enum ap_message {
     MSG_NEXT_PARAM,
     MSG_STATUSTEXT,
     MSG_FENCE_STATUS,
-    MSG_DCM,
+    MSG_AHRS,
     MSG_SIMSTATE,
     MSG_HWSTATUS,
     MSG_RETRY_DEFERRED // this must be last

ArduPlane/navigation.pde

@@ -115,7 +115,7 @@ static void calc_bearing_error()
           correction using the GPS typically takes 10 seconds or so
           for a 180 degree correction.
          */
-		bearing_error = nav_bearing - dcm.yaw_sensor;
+		bearing_error = nav_bearing - ahrs.yaw_sensor;
 	} else {
 
 		// TODO: we need to use the Yaw gyro for in between GPS reads,

ArduPlane/system.pde

@@ -183,7 +183,7 @@ static void init_ardupilot()
             Serial.println_P(PSTR("Compass initialisation failed!"));
             g.compass_enabled = false;
         } else {
-            dcm.set_compass(&compass);
+            ahrs.set_compass(&compass);
             compass.null_offsets_enable();
         }
 	}
@@ -263,7 +263,7 @@ static void init_ardupilot()
 		//read_EEPROM_airstart_critical();
 #if HIL_MODE != HIL_MODE_ATTITUDE
 		imu.init(IMU::WARM_START, mavlink_delay, flash_leds, &timer_scheduler);
-		dcm.set_centripetal(1);
+		ahrs.set_centripetal(1);
 #endif
 
 		// This delay is important for the APM_RC library to work.
@@ -455,8 +455,8 @@ static void startup_IMU_ground(void)
 
 	imu.init(IMU::COLD_START, mavlink_delay, flash_leds, &timer_scheduler);
 	imu.init_accel(mavlink_delay, flash_leds);
-	dcm.set_centripetal(1);
-    dcm.matrix_reset();
+	ahrs.set_centripetal(1);
+    ahrs.reset();
 
 	// read Baro pressure at ground
 	//-----------------------------
@@ -510,10 +510,9 @@ static void update_GPS_light(void)
 static void resetPerfData(void) {
 	mainLoop_count 			= 0;
 	G_Dt_max 				= 0;
-	dcm.gyro_sat_count 		= 0;
 	imu.adc_constraints 	= 0;
-	dcm.renorm_range_count 	= 0;
-	dcm.renorm_blowup_count = 0;
+	ahrs.renorm_range_count 	= 0;
+	ahrs.renorm_blowup_count = 0;
 	gps_fix_count 			= 0;
 	pmTest1					= 0;
 	perf_mon_timer 			= millis();

ArduPlane/test.pde

@@ -514,7 +514,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
 {
 	//Serial.printf_P(PSTR("Calibrating."));
 	imu.init(IMU::COLD_START, delay, flash_leds, &timer_scheduler);
-    dcm.matrix_reset();
+    ahrs.reset();
 
 	print_hit_enter();
 	delay(1000);
@@ -528,14 +528,14 @@ test_imu(uint8_t argc, const Menu::arg *argv)
 
 			// IMU
 			// ---
-			dcm.update_DCM();
+			ahrs.update();
 
 			if(g.compass_enabled) {
 				medium_loopCounter++;
 				if(medium_loopCounter == 5){
 					if (compass.read()) {
                         // Calculate heading
-                        compass.calculate(dcm.get_dcm_matrix());
+                        compass.calculate(ahrs.get_dcm_matrix());
                     }
                     medium_loopCounter = 0;
 				}
@@ -546,9 +546,9 @@ test_imu(uint8_t argc, const Menu::arg *argv)
             Vector3f gyros 	= imu.get_gyro();
             Vector3f accels = imu.get_accel();
 			Serial.printf_P(PSTR("r:%4d  p:%4d  y:%3d  g=(%5.1f %5.1f %5.1f)  a=(%5.1f %5.1f %5.1f)\n"),
-                            (int)dcm.roll_sensor / 100,
-                            (int)dcm.pitch_sensor / 100,
-                            (uint16_t)dcm.yaw_sensor / 100,
+                            (int)ahrs.roll_sensor / 100,
+                            (int)ahrs.pitch_sensor / 100,
+                            (uint16_t)ahrs.yaw_sensor / 100,
                             gyros.x, gyros.y, gyros.z,
                             accels.x, accels.y, accels.z);
 		}
@@ -574,12 +574,12 @@ test_mag(uint8_t argc, const Menu::arg *argv)
         return 0;
     }
     compass.null_offsets_enable();
-    dcm.set_compass(&compass);
+    ahrs.set_compass(&compass);
     report_compass();
 
-    // we need the DCM initialised for this test
+    // we need the AHRS initialised for this test
 	imu.init(IMU::COLD_START, delay, flash_leds, &timer_scheduler);
-    dcm.matrix_reset();
+    ahrs.reset();
 
 	int counter = 0;
 		//Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION);
@@ -595,13 +595,13 @@ test_mag(uint8_t argc, const Menu::arg *argv)
 
 			// IMU
 			// ---
-			dcm.update_DCM();
+			ahrs.update();
 
             medium_loopCounter++;
             if(medium_loopCounter == 5){
                 if (compass.read()) {
                     // Calculate heading
-                    Matrix3f m = dcm.get_dcm_matrix();
+                    Matrix3f m = ahrs.get_dcm_matrix();
                     compass.calculate(m);
                     compass.null_offsets(m);
                 }