mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-22 15:53:56 -04:00
AP_AHRS: remove ability to use DCM as AHRS
This commit is contained in:
parent
a7f14efef2
commit
0d391533b0
@ -127,14 +127,12 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] = {
|
|||||||
|
|
||||||
// 13 was the old EKF_USE
|
// 13 was the old EKF_USE
|
||||||
|
|
||||||
#if AP_AHRS_NAVEKF_AVAILABLE
|
|
||||||
// @Param: EKF_TYPE
|
// @Param: EKF_TYPE
|
||||||
// @DisplayName: Use NavEKF Kalman filter for attitude and position estimation
|
// @DisplayName: Use NavEKF Kalman filter for attitude and position estimation
|
||||||
// @Description: This controls which NavEKF Kalman filter version is used for attitude and position estimation
|
// @Description: This controls which NavEKF Kalman filter version is used for attitude and position estimation
|
||||||
// @Values: 0:Disabled,2:Enable EKF2,3:Enable EKF3,11:ExternalAHRS
|
// @Values: 0:Disabled,2:Enable EKF2,3:Enable EKF3,11:ExternalAHRS
|
||||||
// @User: Advanced
|
// @User: Advanced
|
||||||
AP_GROUPINFO("EKF_TYPE", 14, AP_AHRS, _ekf_type, HAL_AHRS_EKF_TYPE_DEFAULT),
|
AP_GROUPINFO("EKF_TYPE", 14, AP_AHRS, _ekf_type, HAL_AHRS_EKF_TYPE_DEFAULT),
|
||||||
#endif
|
|
||||||
|
|
||||||
// @Param: CUSTOM_ROLL
|
// @Param: CUSTOM_ROLL
|
||||||
// @DisplayName: Board orientation roll offset
|
// @DisplayName: Board orientation roll offset
|
||||||
@ -171,7 +169,7 @@ void AP_AHRS::init()
|
|||||||
{
|
{
|
||||||
update_orientation();
|
update_orientation();
|
||||||
|
|
||||||
#if !HAL_MINIMIZE_FEATURES && AP_AHRS_NAVEKF_AVAILABLE
|
#if !HAL_MINIMIZE_FEATURES
|
||||||
_nmea_out = AP_NMEA_Output::probe();
|
_nmea_out = AP_NMEA_Output::probe();
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
@ -476,12 +474,10 @@ void AP_AHRS::Log_Write_Home_And_Origin()
|
|||||||
if (logger == nullptr) {
|
if (logger == nullptr) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
#if AP_AHRS_NAVEKF_AVAILABLE
|
|
||||||
Location ekf_orig;
|
Location ekf_orig;
|
||||||
if (get_origin(ekf_orig)) {
|
if (get_origin(ekf_orig)) {
|
||||||
Write_Origin(LogOriginType::ekf_origin, ekf_orig);
|
Write_Origin(LogOriginType::ekf_origin, ekf_orig);
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
|
|
||||||
if (home_is_set()) {
|
if (home_is_set()) {
|
||||||
Write_Origin(LogOriginType::ahrs_home, _home);
|
Write_Origin(LogOriginType::ahrs_home, _home);
|
||||||
@ -495,7 +491,7 @@ float AP_AHRS::get_EAS2TAS(void) const {
|
|||||||
|
|
||||||
void AP_AHRS::update_nmea_out()
|
void AP_AHRS::update_nmea_out()
|
||||||
{
|
{
|
||||||
#if !HAL_MINIMIZE_FEATURES && AP_AHRS_NAVEKF_AVAILABLE
|
#if !HAL_MINIMIZE_FEATURES
|
||||||
if (_nmea_out != nullptr) {
|
if (_nmea_out != nullptr) {
|
||||||
_nmea_out->update();
|
_nmea_out->update();
|
||||||
}
|
}
|
||||||
|
@ -732,10 +732,6 @@ private:
|
|||||||
|
|
||||||
namespace AP {
|
namespace AP {
|
||||||
AP_AHRS &ahrs();
|
AP_AHRS &ahrs();
|
||||||
|
// ahrs_navekf only exists to avoid code churn
|
||||||
// use ahrs_navekf() only where the AHRS interface doesn't expose the
|
|
||||||
// functionality you require:
|
|
||||||
#if AP_AHRS_NAVEKF_AVAILABLE
|
|
||||||
AP_AHRS_NavEKF &ahrs_navekf();
|
AP_AHRS_NavEKF &ahrs_navekf();
|
||||||
#endif
|
|
||||||
};
|
};
|
||||||
|
@ -31,8 +31,6 @@
|
|||||||
#include <AP_Vehicle/AP_Vehicle_Type.h>
|
#include <AP_Vehicle/AP_Vehicle_Type.h>
|
||||||
#include <GCS_MAVLink/GCS.h>
|
#include <GCS_MAVLink/GCS.h>
|
||||||
|
|
||||||
#if AP_AHRS_NAVEKF_AVAILABLE
|
|
||||||
|
|
||||||
#define ATTITUDE_CHECK_THRESH_ROLL_PITCH_RAD radians(10)
|
#define ATTITUDE_CHECK_THRESH_ROLL_PITCH_RAD radians(10)
|
||||||
#define ATTITUDE_CHECK_THRESH_YAW_RAD radians(20)
|
#define ATTITUDE_CHECK_THRESH_YAW_RAD radians(20)
|
||||||
|
|
||||||
@ -173,7 +171,7 @@ void AP_AHRS_NavEKF::update(bool skip_ins_update)
|
|||||||
_view->update(skip_ins_update);
|
_view->update(skip_ins_update);
|
||||||
}
|
}
|
||||||
|
|
||||||
#if !HAL_MINIMIZE_FEATURES && AP_AHRS_NAVEKF_AVAILABLE
|
#if !HAL_MINIMIZE_FEATURES
|
||||||
// update NMEA output
|
// update NMEA output
|
||||||
update_nmea_out();
|
update_nmea_out();
|
||||||
#endif
|
#endif
|
||||||
@ -2802,5 +2800,3 @@ void AP_AHRS_NavEKF::set_alt_measurement_noise(float noise)
|
|||||||
EKF3.set_baro_alt_noise(noise);
|
EKF3.set_baro_alt_noise(noise);
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif // AP_AHRS_NAVEKF_AVAILABLE
|
|
||||||
|
@ -32,7 +32,6 @@
|
|||||||
#define HAL_NAVEKF3_AVAILABLE 1
|
#define HAL_NAVEKF3_AVAILABLE 1
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#define AP_AHRS_NAVEKF_AVAILABLE 1
|
|
||||||
#include "AP_AHRS.h"
|
#include "AP_AHRS.h"
|
||||||
|
|
||||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||||
|
Loading…
Reference in New Issue
Block a user