Browse Source

AP_AHRS_NavEKF: add setTakeoffExpected, setTouchdownExpected

mission-4.1.18
Jonathan Challinger 9 years ago committed by Randy Mackay
parent
commit
a2999ece54
  1. 32
      libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
  2. 3
      libraries/AP_AHRS/AP_AHRS_NavEKF.h

32
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

@ -1098,6 +1098,38 @@ bool AP_AHRS_NavEKF::get_variances(float &velVar, float &posVar, float &hgtVar, @@ -1098,6 +1098,38 @@ bool AP_AHRS_NavEKF::get_variances(float &velVar, float &posVar, float &hgtVar,
}
}
void AP_AHRS_NavEKF::setTakeoffExpected(bool val)
{
switch (ekf_type()) {
case EKF_TYPE1:
EKF1.setTakeoffExpected(val);
break;
case EKF_TYPE2:
EKF2.setTakeoffExpected(val);
break;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL:
break;
#endif
}
}
void AP_AHRS_NavEKF::setTouchdownExpected(bool val)
{
switch (ekf_type()) {
case EKF_TYPE1:
EKF1.setTouchdownExpected(val);
break;
case EKF_TYPE2:
EKF2.setTouchdownExpected(val);
break;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL:
break;
#endif
}
}
#endif // AP_AHRS_NAVEKF_AVAILABLE

3
libraries/AP_AHRS/AP_AHRS_NavEKF.h

@ -197,6 +197,9 @@ public: @@ -197,6 +197,9 @@ public:
// boolean false is returned if variances are not available
bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const;
void setTakeoffExpected(bool val);
void setTouchdownExpected(bool val);
private:
enum EKF_TYPE {EKF_TYPE_NONE=0,
EKF_TYPE1=1,

Loading…
Cancel
Save