From 8afd1969079cdfa3c8a9b390098d8128801acadc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 11 Mar 2012 19:13:31 +1100 Subject: [PATCH] APM: adapt ArduPlane for AHRS framework --- ArduPlane/ArduPlane.pde | 38 ++++++++++++++----------------- ArduPlane/Attitude.pde | 12 +++++----- ArduPlane/GCS_Mavlink.pde | 44 ++++++++++++++++++------------------ ArduPlane/Log.pde | 20 ++++++++-------- ArduPlane/commands_logic.pde | 4 ++-- ArduPlane/defines.h | 2 +- ArduPlane/navigation.pde | 2 +- ArduPlane/system.pde | 13 +++++------ ArduPlane/test.pde | 22 +++++++++--------- 9 files changed, 76 insertions(+), 81 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 22d5d5cf86..676a8720bc 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -40,8 +40,7 @@ version 2.1 of the License, or (at your option) any later version. #include // ArduPilot Mega Vector/Matrix math Library #include // Inertial Sensor (uncalibated IMU) Library #include // ArduPilot Mega IMU Library -#include // ArduPilot Mega DCM Library -#include // Madgwick quaternion system +#include // ArduPilot Mega DCM Library #include // PID library #include // RC Channel Library #include // 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(); diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index e8b0e70216..93c1db0bff 100644 --- a/ArduPlane/Attitude.pde +++ b/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 diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index f1d4cda07d..d39345a159 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/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; } diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde index ece99ea6c2..069f0220f7 100644 --- a/ArduPlane/Log.pde +++ b/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); diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index 2156a0c2f6..e9e9de4a20 100644 --- a/ArduPlane/commands_logic.pde +++ b/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; } } diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 8161638686..9144bf3cfd 100644 --- a/ArduPlane/defines.h +++ b/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 diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde index 4aa68f3ad9..df95caab05 100644 --- a/ArduPlane/navigation.pde +++ b/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, diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index 9bcc101e4f..0dc50c54d9 100644 --- a/ArduPlane/system.pde +++ b/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(); diff --git a/ArduPlane/test.pde b/ArduPlane/test.pde index 85c8e4ae3f..ec988bbdd3 100644 --- a/ArduPlane/test.pde +++ b/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); }