Browse Source

AP_AHRS: add set_alt_measurement_noise

These calls the EKF2 and EKF3's set_baro_alt_noise
c415-sdk
Randy Mackay 5 years ago
parent
commit
8caf7d5811
  1. 3
      libraries/AP_AHRS/AP_AHRS.h
  2. 11
      libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
  3. 3
      libraries/AP_AHRS/AP_AHRS_NavEKF.h

3
libraries/AP_AHRS/AP_AHRS.h

@ -562,6 +562,9 @@ public:
// return current vibration vector for primary IMU // return current vibration vector for primary IMU
Vector3f get_vibration(void) const; Vector3f get_vibration(void) const;
// set and save the alt noise parameter value
virtual void set_alt_measurement_noise(float noise) {};
// allow threads to lock against AHRS update // allow threads to lock against AHRS update
HAL_Semaphore &get_semaphore(void) { HAL_Semaphore &get_semaphore(void) {
return _rsem; return _rsem;

11
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

@ -2290,4 +2290,15 @@ bool AP_AHRS_NavEKF::is_ext_nav_used_for_yaw(void) const
return false; return false;
} }
// set and save the alt noise parameter value
void AP_AHRS_NavEKF::set_alt_measurement_noise(float noise)
{
#if HAL_NAVEKF2_AVAILABLE
EKF2.set_baro_alt_noise(noise);
#endif
#if HAL_NAVEKF3_AVAILABLE
EKF3.set_baro_alt_noise(noise);
#endif
}
#endif // AP_AHRS_NAVEKF_AVAILABLE #endif // AP_AHRS_NAVEKF_AVAILABLE

3
libraries/AP_AHRS/AP_AHRS_NavEKF.h

@ -298,6 +298,9 @@ public:
// check whether external navigation is providing yaw. Allows compass pre-arm checks to be bypassed // check whether external navigation is providing yaw. Allows compass pre-arm checks to be bypassed
bool is_ext_nav_used_for_yaw(void) const override; bool is_ext_nav_used_for_yaw(void) const override;
// set and save the ALT_M_NSE parameter value
void set_alt_measurement_noise(float noise) override;
// these are only out here so vehicles can reference them for parameters // these are only out here so vehicles can reference them for parameters
#if HAL_NAVEKF2_AVAILABLE #if HAL_NAVEKF2_AVAILABLE
NavEKF2 EKF2; NavEKF2 EKF2;

Loading…
Cancel
Save