Plane: only include NavEKF on fast CPUs

This commit is contained in:
Andrew Tridgell 2013-12-31 10:24:38 +11:00
parent 4adf6000f3
commit 006b9da6be
4 changed files with 10 additions and 0 deletions

View File

@ -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();

View File

@ -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)

View File

@ -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 {

View File

@ -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
}
/*