Copter: remove AP_AHRS_NAVEKF_AVAILABLE checks

Copter requires an EKF
This commit is contained in:
Randy Mackay 2015-06-10 12:14:13 +09:00
parent e3183babde
commit ce1031a5bd
6 changed files with 2 additions and 19 deletions

View File

@ -665,10 +665,8 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
CHECK_PAYLOAD_SIZE(SIMSTATE);
copter.send_simstate(chan);
#endif
#if AP_AHRS_NAVEKF_AVAILABLE
CHECK_PAYLOAD_SIZE(AHRS2);
copter.gcs[chan-MAVLINK_COMM_0].send_ahrs2(copter.ahrs);
#endif
break;
case MSG_HWSTATUS:
@ -703,10 +701,8 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
break;
case MSG_EKF_STATUS_REPORT:
#if AP_AHRS_NAVEKF_AVAILABLE
CHECK_PAYLOAD_SIZE(EKF_STATUS_REPORT);
copter.ahrs.get_NavEKF().send_status_report(chan);
#endif
break;
case MSG_FENCE_STATUS:

View File

@ -365,14 +365,12 @@ void Copter::Log_Write_Attitude()
Vector3f targets = attitude_control.angle_ef_targets();
DataFlash.Log_Write_Attitude(ahrs, targets);
#if AP_AHRS_NAVEKF_AVAILABLE
#if OPTFLOW == ENABLED
DataFlash.Log_Write_EKF(ahrs,optflow.enabled());
#else
DataFlash.Log_Write_EKF(ahrs,false);
#endif
DataFlash.Log_Write_AHRS2(ahrs);
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
sitl.Log_Write_SIMSTATE(DataFlash);
#endif

View File

@ -1010,11 +1010,9 @@ const AP_Param::Info Copter::var_info[] PROGMEM = {
// @Path: ../libraries/AP_RCMapper/AP_RCMapper.cpp
GOBJECT(rcmap, "RCMAP_", RCMapper),
#if AP_AHRS_NAVEKF_AVAILABLE
// @Group: EKF_
// @Path: ../libraries/AP_NavEKF/AP_NavEKF.cpp
GOBJECTN(EKF, NavEKF, "EKF_", NavEKF),
#endif
// @Group: MIS_
// @Path: ../libraries/AP_Mission/AP_Mission.cpp

View File

@ -311,11 +311,7 @@
//////////////////////////////////////////////////////////////////////////////
// OPTICAL_FLOW
#ifndef OPTFLOW
#if AP_AHRS_NAVEKF_AVAILABLE
# define OPTFLOW ENABLED
#else
# define OPTFLOW DISABLED
#endif
# define OPTFLOW ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////

View File

@ -85,7 +85,6 @@ void Copter::ekf_check()
// ekf_over_threshold - returns true if the ekf's variance are over the tolerance
bool Copter::ekf_over_threshold()
{
#if AP_AHRS_NAVEKF_AVAILABLE
// return false immediately if disabled
if (g.fs_ekf_thresh <= 0.0f) {
return false;
@ -107,9 +106,6 @@ bool Copter::ekf_over_threshold()
// return true if compass and velocity variance over the threshold
return (compass_variance >= g.fs_ekf_thresh && vel_variance >= g.fs_ekf_thresh);
#else
return false;
#endif
}

View File

@ -131,10 +131,9 @@ bool Copter::set_mode(uint8_t mode)
// called at 100hz or more
void Copter::update_flight_mode()
{
#if AP_AHRS_NAVEKF_AVAILABLE
// Update EKF speed limit - used to limit speed when we are using optical flow
ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
#endif
switch (control_mode) {
case ACRO:
#if FRAME_CONFIG == HELI_FRAME