diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 9d52939a30..ae771368ad 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -285,7 +285,9 @@ 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; @@ -805,7 +807,9 @@ 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(); diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index e61a197425..e805617e4b 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -530,6 +530,7 @@ 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 Vector3f euler; struct Location loc; NavEKF.getEulerAngles(euler); @@ -541,6 +542,7 @@ static void NOINLINE send_ekf(mavlink_channel_t chan) loc.alt*1.0e-2f, loc.lat, loc.lng); +#endif } static void NOINLINE send_hwstatus(mavlink_channel_t chan) diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde index b5bbfac70f..762af324aa 100644 --- a/ArduPlane/Log.pde +++ b/ArduPlane/Log.pde @@ -203,6 +203,7 @@ struct PACKED log_EKF { // Write an EKF packet static void Log_Write_EKF(void) { +#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 Vector3f euler; struct Location loc; NavEKF.getEulerAngles(euler); @@ -218,6 +219,7 @@ static void Log_Write_EKF(void) lon : loc.lng }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); +#endif } struct PACKED log_Performance { diff --git a/ArduPlane/commands.pde b/ArduPlane/commands.pde index 0f535012c7..42d3494bed 100644 --- a/ArduPlane/commands.pde +++ b/ArduPlane/commands.pde @@ -241,7 +241,9 @@ static void init_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 } /*