Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-11 17:13:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: create and use AP_AHRS_SIM_ENABLED

Browse Source
This commit is contained in:
Peter Barker 2021-10-30 11:38:13 +11:00 committed by Peter Barker
parent ed608d37f6
commit a31ff08f23
2 changed files with 61 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

108
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -304,7 +304,7 @@ void AP_AHRS::update(bool skip_ins_update)
// update takeoff/touchdown flags
update_flags();
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
update_SITL();
#endif
@ -365,7 +365,7 @@ void AP_AHRS::update(bool skip_ins_update)
case EKFType::NONE:
shortname = "DCM";
break;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
shortname = "SITL";
break;
@ -596,7 +596,7 @@ void AP_AHRS::update_EKF3(void)
}
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
void AP_AHRS::update_SITL(void)
{
if (_sitl == nullptr) {
@ -740,7 +740,7 @@ bool AP_AHRS::get_position(struct Location &loc) const
break;
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM: {
if (_sitl) {
const struct SITL::sitl_fdm &fdm = _sitl->state;
@ -788,7 +788,7 @@ Vector3f AP_AHRS::wind_estimate(void) const
wind = dcm.wind_estimate();
break;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
wind.zero();
break;
@ -851,7 +851,7 @@ bool AP_AHRS::airspeed_estimate(float &airspeed_ret) const
case EKFType::NONE:
return dcm.airspeed_estimate(get_active_airspeed_index(), airspeed_ret);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
if (!_sitl) {
return false;
@ -911,7 +911,7 @@ bool AP_AHRS::airspeed_estimate_true(float &airspeed_ret) const
#if HAL_NAVEKF3_AVAILABLE
case EKFType::THREE:
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
#endif
#if HAL_EXTERNAL_AHRS_ENABLED
@ -944,7 +944,7 @@ bool AP_AHRS::airspeed_vector_true(Vector3f &vec) const
return EKF3.getAirSpdVec(-1, vec);
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
break;
#endif
@ -982,7 +982,7 @@ bool AP_AHRS::use_compass(void)
return EKF3.use_compass();
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
return true;
#endif
@ -1023,7 +1023,7 @@ bool AP_AHRS::get_quaternion(Quaternion &quat) const
EKF3.getQuaternion(-1, quat);
break;
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM: {
if (!_sitl) {
return false;
@ -1076,7 +1076,7 @@ bool AP_AHRS::get_secondary_attitude(Vector3f &eulers) const
return _ekf3_started;
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
// SITL is secondary (should never happen)
return false;
@ -1137,7 +1137,7 @@ bool AP_AHRS::get_secondary_quaternion(Quaternion &quat) const
break;
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
// SITL is secondary (should never happen)
return false;
@ -1184,7 +1184,7 @@ bool AP_AHRS::get_secondary_position(struct Location &loc) const
return _ekf3_started;
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
// SITL is secondary (should never happen)
return false;
@ -1222,7 +1222,7 @@ Vector2f AP_AHRS::groundspeed_vector(void)
return Vector2f(vec.x, vec.y);
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM: {
if (_sitl) {
const struct SITL::sitl_fdm &fdm = _sitl->state;
@ -1257,7 +1257,7 @@ float AP_AHRS::groundspeed(void)
case EKFType::EXTERNAL:
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
#endif
break;
@ -1293,7 +1293,7 @@ bool AP_AHRS::set_origin(const Location &loc)
return ret3;
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
// never allow origin set in SITL. The origin is set by the
// simulation backend
@ -1335,7 +1335,7 @@ bool AP_AHRS::get_velocity_NED(Vector3f &vec) const
return true;
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM: {
if (!_sitl) {
return false;
@ -1372,7 +1372,7 @@ bool AP_AHRS::get_mag_field_NED(Vector3f &vec) const
return true;
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
return false;
#endif
@ -1403,7 +1403,7 @@ bool AP_AHRS::get_mag_field_correction(Vector3f &vec) const
return true;
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
return false;
#endif
@ -1436,7 +1436,7 @@ bool AP_AHRS::get_vert_pos_rate(float &velocity) const
return true;
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
if (_sitl) {
const struct SITL::sitl_fdm &fdm = _sitl->state;
@ -1472,7 +1472,7 @@ bool AP_AHRS::get_hagl(float &height) const
return EKF3.getHAGL(height);
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM: {
if (!_sitl) {
return false;
@ -1530,7 +1530,7 @@ bool AP_AHRS::get_relative_position_NED_origin(Vector3f &vec) const
}
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM: {
if (!_sitl) {
return false;
@ -1607,7 +1607,7 @@ bool AP_AHRS::get_relative_position_NE_origin(Vector2f &posNE) const
}
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM: {
Location loc, orgn;
if (!get_position(loc) ||
@ -1677,7 +1677,7 @@ bool AP_AHRS::get_relative_position_D_origin(float &posD) const
}
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM: {
if (!_sitl) {
return false;
@ -1736,7 +1736,7 @@ AP_AHRS::EKFType AP_AHRS::ekf_type(void) const
{
EKFType type = (EKFType)_ekf_type.get();
switch (type) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
return type;
#endif
@ -1820,7 +1820,7 @@ AP_AHRS::EKFType AP_AHRS::active_EKF_type(void) const
}
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
ret = EKFType::SIM;
break;
@ -1856,7 +1856,7 @@ AP_AHRS::EKFType AP_AHRS::active_EKF_type(void) const
should_use_gps = EKF3.configuredToUseGPSForPosXY();
}
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
if (ret == EKFType::SIM) {
get_filter_status(filt_state);
}
@ -1941,7 +1941,7 @@ bool AP_AHRS::get_secondary_EKF_type(EKFType &secondary_ekf_type) const
#if HAL_NAVEKF3_AVAILABLE
case EKFType::THREE:
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
#endif
#if HAL_EXTERNAL_AHRS_ENABLED
@ -2002,7 +2002,7 @@ bool AP_AHRS::healthy(void) const
}
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
return true;
#endif
@ -2028,7 +2028,7 @@ bool AP_AHRS::pre_arm_check(bool requires_position, char *failure_msg, uint8_t f
}
switch (ekf_type()) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
return ret;
#endif
@ -2083,7 +2083,7 @@ bool AP_AHRS::initialised(void) const
return (_ekf3_started && (AP_HAL::millis() - start_time_ms > AP_AHRS_NAVEKF_SETTLE_TIME_MS));
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
return true;
#endif
@ -2114,7 +2114,7 @@ bool AP_AHRS::get_filter_status(nav_filter_status &status) const
return true;
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
memset(&status, 0, sizeof(status));
status.flags.attitude = 1;
@ -2212,7 +2212,7 @@ void AP_AHRS::getControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler
break;
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
// same as EKF2 for no optical flow
ekfGndSpdLimit = 400.0f;
@ -2260,7 +2260,7 @@ bool AP_AHRS::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
return EKF3.getMagOffsets(mag_idx, magOffsets);
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
magOffsets.zero();
return true;
@ -2294,7 +2294,7 @@ void AP_AHRS::getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const
EKF3.getAccelBias(-1,accel_bias);
break;
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
break;
#endif
@ -2429,7 +2429,7 @@ uint32_t AP_AHRS::getLastYawResetAngle(float &yawAng)
return EKF3.getLastYawResetAngle(yawAng);
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
return 0;
#endif
@ -2460,7 +2460,7 @@ uint32_t AP_AHRS::getLastPosNorthEastReset(Vector2f &pos)
return EKF3.getLastPosNorthEastReset(pos);
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
return 0;
#endif
@ -2491,7 +2491,7 @@ uint32_t AP_AHRS::getLastVelNorthEastReset(Vector2f &vel) const
return EKF3.getLastVelNorthEastReset(vel);
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
return 0;
#endif
@ -2522,7 +2522,7 @@ uint32_t AP_AHRS::getLastPosDownReset(float &posDelta)
return EKF3.getLastPosDownReset(posDelta);
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
return 0;
#endif
@ -2571,7 +2571,7 @@ bool AP_AHRS::resetHeightDatum(void)
return EKF3.resetHeightDatum();
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
return false;
#endif
@ -2592,7 +2592,7 @@ void AP_AHRS::send_ekf_status_report(mavlink_channel_t chan) const
dcm.send_ekf_status_report(chan);
break;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
{
// send status report with everything looking good
@ -2657,7 +2657,7 @@ bool AP_AHRS::get_origin(Location &ret) const
return true;
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM: {
if (!_sitl) {
return false;
@ -2696,7 +2696,7 @@ bool AP_AHRS::get_hgt_ctrl_limit(float& limit) const
return EKF3.getHeightControlLimit(limit);
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
return false;
#endif
@ -2760,7 +2760,7 @@ bool AP_AHRS::get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &
return EKF3.getInnovations(-1, velInnov, posInnov, magInnov, tasInnov, yawInnov);
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
velInnov.zero();
posInnov.zero();
@ -2791,7 +2791,7 @@ bool AP_AHRS::is_vibration_affected() const
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
#endif
#if HAL_EXTERNAL_AHRS_ENABLED
@ -2829,7 +2829,7 @@ bool AP_AHRS::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3
}
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
velVar = 0;
posVar = 0;
@ -2869,7 +2869,7 @@ bool AP_AHRS::get_vel_innovations_and_variances_for_source(uint8_t source, Vecto
return EKF3.getVelInnovationsAndVariancesForSource(-1, (AP_NavEKF_Source::SourceXY)source, innovations, variances);
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
// SITL does not support source level variances
return false;
@ -2924,7 +2924,7 @@ uint8_t AP_AHRS::get_primary_IMU_index() const
imu = EKF3.getPrimaryCoreIMUIndex();
break;
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
break;
#endif
@ -2950,7 +2950,7 @@ int8_t AP_AHRS::get_primary_core_index() const
{
switch (active_EKF_type()) {
case EKFType::NONE:
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
#endif
#if HAL_EXTERNAL_AHRS_ENABLED
@ -3000,7 +3000,7 @@ void AP_AHRS::check_lane_switch(void)
case EKFType::NONE:
break;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
break;
#endif
@ -3031,7 +3031,7 @@ void AP_AHRS::request_yaw_reset(void)
case EKFType::NONE:
break;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
break;
#endif
@ -3086,7 +3086,7 @@ bool AP_AHRS::using_noncompass_for_yaw(void) const
case EKFType::THREE:
return EKF3.using_noncompass_for_yaw();
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
#endif
#if HAL_EXTERNAL_AHRS_ENABLED
@ -3111,7 +3111,7 @@ bool AP_AHRS::using_extnav_for_yaw(void) const
case EKFType::THREE:
return EKF3.using_extnav_for_yaw();
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
#endif
#if HAL_EXTERNAL_AHRS_ENABLED
@ -3155,7 +3155,7 @@ const EKFGSF_yaw *AP_AHRS::get_yaw_estimator(void) const
case EKFType::THREE:
return EKF3.get_yawEstimator();
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
#endif
#if HAL_EXTERNAL_AHRS_ENABLED

10
libraries/AP_AHRS/AP_AHRS.h
View File

@ -32,9 +32,13 @@
#define HAL_NAVEKF3_AVAILABLE 1
#endif
#ifndef AP_AHRS_SIM_ENABLED
#define AP_AHRS_SIM_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
#endif
#include "AP_AHRS.h"
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
#include <SITL/SITL.h>
#endif
@ -665,7 +669,7 @@ private:
#if HAL_NAVEKF2_AVAILABLE
,TWO = 2
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
,SIM = 10
#endif
#if HAL_EXTERNAL_AHRS_ENABLED
@ -760,7 +764,7 @@ private:
EKFType last_active_ekf_type;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if AP_AHRS_SIM_ENABLED
SITL::SIM *_sitl;
uint32_t _last_body_odm_update_ms;
void update_SITL(void);