From 9ac886f58dbf5a296fba459c954e44a6aad6666d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 2 Jan 2014 12:16:20 +1100 Subject: [PATCH] Plane: use AP_AHRS_NavEKF when available --- ArduPlane/ArduPlane.pde | 19 ++++------- ArduPlane/GCS_Mavlink.pde | 6 ++-- ArduPlane/Log.pde | 68 ++++++++++++++++++--------------------- ArduPlane/commands.pde | 5 --- 4 files changed, 41 insertions(+), 57 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 9b09a77e7d..398fff24a7 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -273,7 +273,12 @@ AP_InertialSensor_L3G4200D ins; #error Unrecognised CONFIG_INS_TYPE setting. #endif // CONFIG_INS_TYPE +// Inertial Navigation EKF +#if AP_AHRS_NAVEKF_AVAILABLE +AP_AHRS_NavEKF ahrs(ins, g_gps, barometer); +#else AP_AHRS_DCM ahrs(ins, g_gps); +#endif static AP_L1_Control L1_controller(ahrs); static AP_TECS TECS_controller(ahrs, aparm); @@ -284,11 +289,6 @@ static AP_PitchController pitchController(ahrs, aparm); static AP_YawController yawController(ahrs, aparm); static AP_SteerController steerController(ahrs); -// Inertial Navigation EKF -#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 -static NavEKF NavEKF(ahrs, barometer); -#endif - #if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL SITL sitl; #endif @@ -807,14 +807,10 @@ static void ahrs_update() #endif ahrs.update(); -#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 - NavEKF.UpdateFilter(); -#endif if (should_log(MASK_LOG_ATTITUDE_FAST)) { Log_Write_Attitude(); - Log_Write_EKF1(); - Log_Write_EKF2(); + Log_Write_EKF(); } if (should_log(MASK_LOG_IMU)) @@ -898,8 +894,7 @@ static void update_logging1(void) { if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) { Log_Write_Attitude(); - Log_Write_EKF1(); - Log_Write_EKF2(); + Log_Write_EKF(); } if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_IMU)) diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index e805617e4b..86900b4385 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -530,11 +530,11 @@ static void NOINLINE send_simstate(mavlink_channel_t chan) // report NavEKF state static void NOINLINE send_ekf(mavlink_channel_t chan) { -#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 +#if AP_AHRS_NAVEKF_AVAILABLE Vector3f euler; struct Location loc; - NavEKF.getEulerAngles(euler); - NavEKF.getLLH(loc); + ahrs.get_NavEKF().getEulerAngles(euler); + ahrs.get_NavEKF().getLLH(loc); mavlink_msg_ekf_send(chan, euler.x, euler.y, diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde index 4b03f612e8..9f81b00efc 100644 --- a/ArduPlane/Log.pde +++ b/ArduPlane/Log.pde @@ -206,20 +206,37 @@ struct PACKED log_EKF1 { int8_t gyrZ; }; +struct PACKED log_EKF2 { + LOG_PACKET_HEADER; + uint32_t time_ms; + int8_t accX; + int8_t accY; + int8_t accZ; + int16_t windN; + int16_t windE; + int16_t magN; + int16_t magE; + int16_t magD; + int16_t magX; + int16_t magY; + int16_t magZ; +}; + + // Write first EKF packet -static void Log_Write_EKF1(void) +static void Log_Write_EKF(void) { -#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 +#if AP_AHRS_NAVEKF_AVAILABLE Vector3f euler; Vector3f posNED; Vector3f velNED; Vector3f dAngBias; Vector3f dVelBias; Vector3f gyroBias; - NavEKF.getEulerAngles(euler); - NavEKF.getVelNED(velNED); - NavEKF.getPosNED(posNED); - NavEKF.getGyroBias(gyroBias); + ahrs.get_NavEKF().getEulerAngles(euler); + ahrs.get_NavEKF().getVelNED(velNED); + ahrs.get_NavEKF().getPosNED(posNED); + ahrs.get_NavEKF().getGyroBias(gyroBias); struct log_EKF1 pkt = { LOG_PACKET_HEADER_INIT(LOG_EKF1_MSG), time_ms : hal.scheduler->millis(), @@ -237,39 +254,16 @@ static void Log_Write_EKF1(void) gyrZ : (int8_t)(gyroBias.z) // deg/min }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); -#endif -} -struct PACKED log_EKF2 { - LOG_PACKET_HEADER; - uint32_t time_ms; - int8_t accX; - int8_t accY; - int8_t accZ; - int16_t windN; - int16_t windE; - int16_t magN; - int16_t magE; - int16_t magD; - int16_t magX; - int16_t magY; - int16_t magZ; -}; - -// Write second EKF packet -static void Log_Write_EKF2(void) -{ -#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 - Vector3f euler; Vector3f accelBias; Vector3f wind; Vector3f magNED; Vector3f magXYZ; - NavEKF.getAccelBias(accelBias); - NavEKF.getWind(wind); - NavEKF.getMagNED(magNED); - NavEKF.getMagXYZ(magXYZ); - struct log_EKF2 pkt = { + ahrs.get_NavEKF().getAccelBias(accelBias); + ahrs.get_NavEKF().getWind(wind); + ahrs.get_NavEKF().getMagNED(magNED); + ahrs.get_NavEKF().getMagXYZ(magXYZ); + struct log_EKF2 pkt2 = { LOG_PACKET_HEADER_INIT(LOG_EKF2_MSG), time_ms : hal.scheduler->millis(), accX : (int8_t)(100*accelBias.x), @@ -284,10 +278,11 @@ static void Log_Write_EKF2(void) magY : (int16_t)(magXYZ.y), magZ : (int16_t)(magXYZ.z) }; - DataFlash.WriteBlock(&pkt, sizeof(pkt)); + DataFlash.WriteBlock(&pkt2, sizeof(pkt2)); #endif } + struct PACKED log_Performance { LOG_PACKET_HEADER; uint32_t loop_time; @@ -726,8 +721,7 @@ static void Log_Write_Nav_Tuning() {} static void Log_Write_TECS_Tuning() {} static void Log_Write_Performance() {} static void Log_Write_Attitude() {} -static void Log_Write_EKF1() {} -static void Log_Write_EKF2() {} +static void Log_Write_EKF() {} static void Log_Write_Control_Tuning() {} static void Log_Write_Camera() {} static void Log_Write_Mode(uint8_t mode) {} diff --git a/ArduPlane/commands.pde b/ArduPlane/commands.pde index 42d3494bed..e9a137bb73 100644 --- a/ArduPlane/commands.pde +++ b/ArduPlane/commands.pde @@ -239,11 +239,6 @@ static void init_home() // ------------- guided_WP = home; guided_WP.alt += g.RTL_altitude_cm; - - // startup EKF now we have home -#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 - NavEKF.InitialiseFilter(); -#endif } /*