AP_AHRS: create an AP_AHRS_SIM backend
This commit is contained in:
Peter Barker
Andrew Tridgell
parent
c79672b96c
commit
c42754b691
@ -478,6 +478,18 @@ void AP_AHRS::update_DCM()
|
||||
// }
|
||||
}
|
||||
|
||||
#if AP_AHRS_SIM_ENABLED
|
||||
void AP_AHRS::update_SITL(void)
|
||||
{
|
||||
sim.update();
|
||||
sim.get_results(sim_estimates);
|
||||
|
||||
if (active_EKF_type() == EKFType::SIM) {
|
||||
copy_estimates_from_backend_estimates(sim_estimates);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
void AP_AHRS::update_notify_from_filter_status(const nav_filter_status &status)
|
||||
{
|
||||
AP_Notify::flags.gps_fusion = status.flags.using_gps; // Drives AP_Notify flag for usable GPS.
|
||||
@ -620,7 +632,8 @@ void AP_AHRS::update_EKF3(void)
|
||||
#endif
|
||||
|
||||
#if AP_AHRS_SIM_ENABLED
|
||||
void AP_AHRS::update_SITL(void)
|
||||
|
||||
void AP_AHRS_SIM::get_results(AP_AHRS_Backend::Estimates &results)
|
||||
{
|
||||
if (_sitl == nullptr) {
|
||||
_sitl = AP::sitl();
|
||||
@ -632,22 +645,15 @@ void AP_AHRS::update_SITL(void)
|
||||
const struct SITL::sitl_fdm &fdm = _sitl->state;
|
||||
const AP_InertialSensor &_ins = AP::ins();
|
||||
|
||||
if (active_EKF_type() == EKFType::SIM) {
|
||||
fdm.quaternion.rotation_matrix(results.dcm_matrix);
|
||||
results.dcm_matrix = results.dcm_matrix * AP::ahrs().get_rotation_vehicle_body_to_autopilot_body();
|
||||
results.dcm_matrix.to_euler(&results.roll_rad, &results.pitch_rad, &results.yaw_rad);
|
||||
|
||||
fdm.quaternion.rotation_matrix(_dcm_matrix);
|
||||
_dcm_matrix = _dcm_matrix * get_rotation_vehicle_body_to_autopilot_body();
|
||||
_dcm_matrix.to_euler(&roll, &pitch, &yaw);
|
||||
results.gyro_estimate = _ins.get_gyro();
|
||||
results.gyro_drift.zero();
|
||||
|
||||
update_cd_values();
|
||||
update_trig();
|
||||
|
||||
_gyro_drift.zero();
|
||||
|
||||
_gyro_estimate = _ins.get_gyro();
|
||||
|
||||
const Vector3f &accel = _ins.get_accel();
|
||||
_accel_ef = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * accel;
|
||||
}
|
||||
const Vector3f &accel = _ins.get_accel();
|
||||
results.accel_ef = results.dcm_matrix * AP::ahrs().get_rotation_autopilot_body_to_vehicle_body() * accel;
|
||||
|
||||
#if HAL_NAVEKF3_AVAILABLE
|
||||
if (_sitl->odom_enable) {
|
||||
@ -673,7 +679,7 @@ void AP_AHRS::update_SITL(void)
|
||||
}
|
||||
#endif // HAL_NAVEKF3_AVAILABLE
|
||||
}
|
||||
#endif // CONFIG_HAL_BOARD
|
||||
#endif // AP_AHRS_SIM_ENABLED
|
||||
|
||||
#if HAL_EXTERNAL_AHRS_ENABLED
|
||||
void AP_AHRS::update_external(void)
|
||||
@ -708,6 +714,9 @@ void AP_AHRS::reset()
|
||||
WITH_SEMAPHORE(_rsem);
|
||||
|
||||
dcm.reset();
|
||||
#if AP_AHRS_SIM_ENABLED
|
||||
sim.reset();
|
||||
#endif
|
||||
|
||||
#if HAL_NAVEKF2_AVAILABLE
|
||||
if (_ekf2_started) {
|
||||
@ -745,17 +754,8 @@ bool AP_AHRS::get_location(struct Location &loc) const
|
||||
#endif
|
||||
|
||||
#if AP_AHRS_SIM_ENABLED
|
||||
case EKFType::SIM: {
|
||||
if (_sitl) {
|
||||
const struct SITL::sitl_fdm &fdm = _sitl->state;
|
||||
loc = {};
|
||||
loc.lat = fdm.latitude * 1e7;
|
||||
loc.lng = fdm.longitude * 1e7;
|
||||
loc.alt = fdm.altitude*100;
|
||||
return true;
|
||||
}
|
||||
break;
|
||||
}
|
||||
case EKFType::SIM:
|
||||
return sim.get_location(loc);
|
||||
#endif
|
||||
|
||||
#if HAL_EXTERNAL_AHRS_ENABLED
|
||||
@ -794,10 +794,7 @@ Vector3f AP_AHRS::wind_estimate(void) const
|
||||
|
||||
#if AP_AHRS_SIM_ENABLED
|
||||
case EKFType::SIM:
|
||||
if (_sitl) {
|
||||
const auto &fdm = _sitl->state;
|
||||
wind = fdm.wind_ef;
|
||||
}
|
||||
wind = sim.wind_estimate();
|
||||
break;
|
||||
#endif
|
||||
|
||||
@ -885,11 +882,7 @@ bool AP_AHRS::airspeed_estimate(float &airspeed_ret) const
|
||||
|
||||
#if AP_AHRS_SIM_ENABLED
|
||||
case EKFType::SIM:
|
||||
if (!_sitl) {
|
||||
return false;
|
||||
}
|
||||
airspeed_ret = _sitl->state.airspeed;
|
||||
return true;
|
||||
return sim.airspeed_estimate(airspeed_ret);
|
||||
#endif
|
||||
|
||||
#if HAL_NAVEKF2_AVAILABLE
|
||||
@ -1046,7 +1039,7 @@ bool AP_AHRS::use_compass(void)
|
||||
|
||||
#if AP_AHRS_SIM_ENABLED
|
||||
case EKFType::SIM:
|
||||
return true;
|
||||
return sim.use_compass();
|
||||
#endif
|
||||
|
||||
#if HAL_EXTERNAL_AHRS_ENABLED
|
||||
@ -1086,14 +1079,11 @@ bool AP_AHRS::get_quaternion(Quaternion &quat) const
|
||||
break;
|
||||
#endif
|
||||
#if AP_AHRS_SIM_ENABLED
|
||||
case EKFType::SIM: {
|
||||
if (!_sitl) {
|
||||
case EKFType::SIM:
|
||||
if (!sim.get_quaternion(quat)) {
|
||||
return false;
|
||||
}
|
||||
const struct SITL::sitl_fdm &fdm = _sitl->state;
|
||||
quat = fdm.quaternion;
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
#if HAL_EXTERNAL_AHRS_ENABLED
|
||||
case EKFType::EXTERNAL:
|
||||
@ -1285,13 +1275,8 @@ Vector2f AP_AHRS::groundspeed_vector(void)
|
||||
#endif
|
||||
|
||||
#if AP_AHRS_SIM_ENABLED
|
||||
case EKFType::SIM: {
|
||||
if (_sitl) {
|
||||
const struct SITL::sitl_fdm &fdm = _sitl->state;
|
||||
return Vector2f(fdm.speedN, fdm.speedE);
|
||||
}
|
||||
break;
|
||||
}
|
||||
case EKFType::SIM:
|
||||
return sim.groundspeed_vector();
|
||||
#endif
|
||||
#if HAL_EXTERNAL_AHRS_ENABLED
|
||||
case EKFType::EXTERNAL: {
|
||||
@ -1398,14 +1383,8 @@ bool AP_AHRS::get_velocity_NED(Vector3f &vec) const
|
||||
#endif
|
||||
|
||||
#if AP_AHRS_SIM_ENABLED
|
||||
case EKFType::SIM: {
|
||||
if (!_sitl) {
|
||||
return false;
|
||||
}
|
||||
const struct SITL::sitl_fdm &fdm = _sitl->state;
|
||||
vec = Vector3f(fdm.speedN, fdm.speedE, fdm.speedD);
|
||||
return true;
|
||||
}
|
||||
case EKFType::SIM:
|
||||
return sim.get_velocity_NED(vec);
|
||||
#endif
|
||||
#if HAL_EXTERNAL_AHRS_ENABLED
|
||||
case EKFType::EXTERNAL:
|
||||
@ -1500,13 +1479,7 @@ bool AP_AHRS::get_vert_pos_rate(float &velocity) const
|
||||
|
||||
#if AP_AHRS_SIM_ENABLED
|
||||
case EKFType::SIM:
|
||||
if (_sitl) {
|
||||
const struct SITL::sitl_fdm &fdm = _sitl->state;
|
||||
velocity = fdm.speedD;
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
return sim.get_vert_pos_rate(velocity);
|
||||
#endif
|
||||
#if HAL_EXTERNAL_AHRS_ENABLED
|
||||
case EKFType::EXTERNAL:
|
||||
@ -1535,14 +1508,8 @@ bool AP_AHRS::get_hagl(float &height) const
|
||||
#endif
|
||||
|
||||
#if AP_AHRS_SIM_ENABLED
|
||||
case EKFType::SIM: {
|
||||
if (!_sitl) {
|
||||
return false;
|
||||
}
|
||||
const struct SITL::sitl_fdm &fdm = _sitl->state;
|
||||
height = fdm.altitude - get_home().alt*0.01f;
|
||||
return true;
|
||||
}
|
||||
case EKFType::SIM:
|
||||
return sim.get_hagl(height);
|
||||
#endif
|
||||
#if HAL_EXTERNAL_AHRS_ENABLED
|
||||
case EKFType::EXTERNAL: {
|
||||
@ -1593,21 +1560,8 @@ bool AP_AHRS::get_relative_position_NED_origin(Vector3f &vec) const
|
||||
#endif
|
||||
|
||||
#if AP_AHRS_SIM_ENABLED
|
||||
case EKFType::SIM: {
|
||||
if (!_sitl) {
|
||||
return false;
|
||||
}
|
||||
Location loc, orgn;
|
||||
if (!get_location(loc) ||
|
||||
!get_origin(orgn)) {
|
||||
return false;
|
||||
}
|
||||
const Vector2f diff2d = orgn.get_distance_NE(loc);
|
||||
const struct SITL::sitl_fdm &fdm = _sitl->state;
|
||||
vec = Vector3f(diff2d.x, diff2d.y,
|
||||
-(fdm.altitude - orgn.alt*0.01f));
|
||||
return true;
|
||||
}
|
||||
case EKFType::SIM:
|
||||
return sim.get_relative_position_NED_origin(vec);
|
||||
#endif
|
||||
#if HAL_EXTERNAL_AHRS_ENABLED
|
||||
case EKFType::EXTERNAL: {
|
||||
@ -1671,13 +1625,7 @@ bool AP_AHRS::get_relative_position_NE_origin(Vector2f &posNE) const
|
||||
|
||||
#if AP_AHRS_SIM_ENABLED
|
||||
case EKFType::SIM: {
|
||||
Location loc, orgn;
|
||||
if (!get_location(loc) ||
|
||||
!get_origin(orgn)) {
|
||||
return false;
|
||||
}
|
||||
posNE = orgn.get_distance_NE(loc);
|
||||
return true;
|
||||
return sim.get_relative_position_NE_origin(posNE);
|
||||
}
|
||||
#endif
|
||||
#if HAL_EXTERNAL_AHRS_ENABLED
|
||||
@ -1740,18 +1688,8 @@ bool AP_AHRS::get_relative_position_D_origin(float &posD) const
|
||||
#endif
|
||||
|
||||
#if AP_AHRS_SIM_ENABLED
|
||||
case EKFType::SIM: {
|
||||
if (!_sitl) {
|
||||
return false;
|
||||
}
|
||||
const struct SITL::sitl_fdm &fdm = _sitl->state;
|
||||
Location orgn;
|
||||
if (!get_origin(orgn)) {
|
||||
return false;
|
||||
}
|
||||
posD = -(fdm.altitude - orgn.alt*0.01f);
|
||||
return true;
|
||||
}
|
||||
case EKFType::SIM:
|
||||
return sim.get_relative_position_D_origin(posD);
|
||||
#endif
|
||||
#if HAL_EXTERNAL_AHRS_ENABLED
|
||||
case EKFType::EXTERNAL: {
|
||||
@ -2078,7 +2016,7 @@ bool AP_AHRS::healthy(void) const
|
||||
|
||||
#if AP_AHRS_SIM_ENABLED
|
||||
case EKFType::SIM:
|
||||
return true;
|
||||
return sim.healthy();
|
||||
#endif
|
||||
#if HAL_EXTERNAL_AHRS_ENABLED
|
||||
case EKFType::EXTERNAL:
|
||||
@ -2210,17 +2148,7 @@ bool AP_AHRS::get_filter_status(nav_filter_status &status) const
|
||||
|
||||
#if AP_AHRS_SIM_ENABLED
|
||||
case EKFType::SIM:
|
||||
memset(&status, 0, sizeof(status));
|
||||
status.flags.attitude = 1;
|
||||
status.flags.horiz_vel = 1;
|
||||
status.flags.vert_vel = 1;
|
||||
status.flags.horiz_pos_rel = 1;
|
||||
status.flags.horiz_pos_abs = 1;
|
||||
status.flags.vert_pos = 1;
|
||||
status.flags.pred_horiz_pos_rel = 1;
|
||||
status.flags.pred_horiz_pos_abs = 1;
|
||||
status.flags.using_gps = 1;
|
||||
return true;
|
||||
return sim.get_filter_status(status);
|
||||
#endif
|
||||
#if HAL_EXTERNAL_AHRS_ENABLED
|
||||
case EKFType::EXTERNAL:
|
||||
@ -2298,9 +2226,7 @@ void AP_AHRS::getControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler
|
||||
{
|
||||
switch (active_EKF_type()) {
|
||||
case EKFType::NONE:
|
||||
// lower gains in VTOL controllers when flying on DCM
|
||||
ekfGndSpdLimit = 50.0;
|
||||
ekfNavVelGainScaler = 0.5;
|
||||
dcm.get_control_limits(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
break;
|
||||
|
||||
#if HAL_NAVEKF2_AVAILABLE
|
||||
@ -2317,9 +2243,7 @@ void AP_AHRS::getControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler
|
||||
|
||||
#if AP_AHRS_SIM_ENABLED
|
||||
case EKFType::SIM:
|
||||
// same as EKF2 for no optical flow
|
||||
ekfGndSpdLimit = 400.0f;
|
||||
ekfNavVelGainScaler = 1.0f;
|
||||
sim.get_control_limits(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
break;
|
||||
#endif
|
||||
#if HAL_EXTERNAL_AHRS_ENABLED
|
||||
@ -2365,8 +2289,7 @@ bool AP_AHRS::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
|
||||
|
||||
#if AP_AHRS_SIM_ENABLED
|
||||
case EKFType::SIM:
|
||||
magOffsets.zero();
|
||||
return true;
|
||||
return sim.get_mag_offsets(mag_idx, magOffsets);
|
||||
#endif
|
||||
#if HAL_EXTERNAL_AHRS_ENABLED
|
||||
case EKFType::EXTERNAL:
|
||||
@ -2534,7 +2457,7 @@ uint32_t AP_AHRS::getLastYawResetAngle(float &yawAng)
|
||||
|
||||
#if AP_AHRS_SIM_ENABLED
|
||||
case EKFType::SIM:
|
||||
return 0;
|
||||
return sim.getLastYawResetAngle(yawAng);
|
||||
#endif
|
||||
#if HAL_EXTERNAL_AHRS_ENABLED
|
||||
case EKFType::EXTERNAL:
|
||||
@ -2565,7 +2488,7 @@ uint32_t AP_AHRS::getLastPosNorthEastReset(Vector2f &pos)
|
||||
|
||||
#if AP_AHRS_SIM_ENABLED
|
||||
case EKFType::SIM:
|
||||
return 0;
|
||||
return sim.getLastPosNorthEastReset(pos);
|
||||
#endif
|
||||
#if HAL_EXTERNAL_AHRS_ENABLED
|
||||
case EKFType::EXTERNAL:
|
||||
@ -2596,7 +2519,7 @@ uint32_t AP_AHRS::getLastVelNorthEastReset(Vector2f &vel) const
|
||||
|
||||
#if AP_AHRS_SIM_ENABLED
|
||||
case EKFType::SIM:
|
||||
return 0;
|
||||
return sim.getLastVelNorthEastReset(vel);
|
||||
#endif
|
||||
#if HAL_EXTERNAL_AHRS_ENABLED
|
||||
case EKFType::EXTERNAL:
|
||||
@ -2627,7 +2550,7 @@ uint32_t AP_AHRS::getLastPosDownReset(float &posDelta)
|
||||
|
||||
#if AP_AHRS_SIM_ENABLED
|
||||
case EKFType::SIM:
|
||||
return 0;
|
||||
return sim.getLastPosDownReset(posDelta);
|
||||
#endif
|
||||
#if HAL_EXTERNAL_AHRS_ENABLED
|
||||
case EKFType::EXTERNAL:
|
||||
@ -2676,7 +2599,7 @@ bool AP_AHRS::resetHeightDatum(void)
|
||||
|
||||
#if AP_AHRS_SIM_ENABLED
|
||||
case EKFType::SIM:
|
||||
return false;
|
||||
return sim.resetHeightDatum();
|
||||
#endif
|
||||
#if HAL_EXTERNAL_AHRS_ENABLED
|
||||
case EKFType::EXTERNAL:
|
||||
@ -2697,21 +2620,7 @@ void AP_AHRS::send_ekf_status_report(GCS_MAVLINK &link) const
|
||||
|
||||
#if AP_AHRS_SIM_ENABLED
|
||||
case EKFType::SIM:
|
||||
{
|
||||
// send status report with everything looking good
|
||||
const uint16_t flags =
|
||||
EKF_ATTITUDE | /* Set if EKF's attitude estimate is good. | */
|
||||
EKF_VELOCITY_HORIZ | /* Set if EKF's horizontal velocity estimate is good. | */
|
||||
EKF_VELOCITY_VERT | /* Set if EKF's vertical velocity estimate is good. | */
|
||||
EKF_POS_HORIZ_REL | /* Set if EKF's horizontal position (relative) estimate is good. | */
|
||||
EKF_POS_HORIZ_ABS | /* Set if EKF's horizontal position (absolute) estimate is good. | */
|
||||
EKF_POS_VERT_ABS | /* Set if EKF's vertical position (absolute) estimate is good. | */
|
||||
EKF_POS_VERT_AGL | /* Set if EKF's vertical position (above ground) estimate is good. | */
|
||||
//EKF_CONST_POS_MODE | /* EKF is in constant position mode and does not know it's absolute or relative position. | */
|
||||
EKF_PRED_POS_HORIZ_REL | /* Set if EKF's predicted horizontal position (relative) estimate is good. | */
|
||||
EKF_PRED_POS_HORIZ_ABS; /* Set if EKF's predicted horizontal position (absolute) estimate is good. | */
|
||||
mavlink_msg_ekf_status_report_send(link.get_chan(), flags, 0, 0, 0, 0, 0, 0);
|
||||
}
|
||||
sim.send_ekf_status_report(link);
|
||||
break;
|
||||
#endif
|
||||
|
||||
@ -2753,14 +2662,8 @@ bool AP_AHRS::get_origin(EKFType type, Location &ret) const
|
||||
#endif
|
||||
|
||||
#if AP_AHRS_SIM_ENABLED
|
||||
case EKFType::SIM: {
|
||||
if (!_sitl) {
|
||||
return false;
|
||||
}
|
||||
const struct SITL::sitl_fdm &fdm = _sitl->state;
|
||||
ret = fdm.home;
|
||||
return true;
|
||||
}
|
||||
case EKFType::SIM:
|
||||
return sim.get_origin(ret);
|
||||
#endif
|
||||
#if HAL_EXTERNAL_AHRS_ENABLED
|
||||
case EKFType::EXTERNAL:
|
||||
@ -2811,7 +2714,7 @@ bool AP_AHRS::get_hgt_ctrl_limit(float& limit) const
|
||||
|
||||
#if AP_AHRS_SIM_ENABLED
|
||||
case EKFType::SIM:
|
||||
return false;
|
||||
return sim.get_hgt_ctrl_limit(limit);
|
||||
#endif
|
||||
#if HAL_EXTERNAL_AHRS_ENABLED
|
||||
case EKFType::EXTERNAL:
|
||||
@ -2875,12 +2778,7 @@ bool AP_AHRS::get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &
|
||||
|
||||
#if AP_AHRS_SIM_ENABLED
|
||||
case EKFType::SIM:
|
||||
velInnov.zero();
|
||||
posInnov.zero();
|
||||
magInnov.zero();
|
||||
tasInnov = 0.0f;
|
||||
yawInnov = 0.0f;
|
||||
return true;
|
||||
return sim.get_innovations(velInnov, posInnov, magInnov, tasInnov, yawInnov);
|
||||
#endif
|
||||
|
||||
#if HAL_EXTERNAL_AHRS_ENABLED
|
||||
@ -2944,12 +2842,7 @@ bool AP_AHRS::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3
|
||||
|
||||
#if AP_AHRS_SIM_ENABLED
|
||||
case EKFType::SIM:
|
||||
velVar = 0;
|
||||
posVar = 0;
|
||||
hgtVar = 0;
|
||||
magVar.zero();
|
||||
tasVar = 0;
|
||||
return true;
|
||||
return sim.get_variances(velVar, posVar, hgtVar, magVar, tasVar);
|
||||
#endif
|
||||
|
||||
#if HAL_EXTERNAL_AHRS_ENABLED
|
||||
|
||||
@ -25,15 +25,12 @@
|
||||
|
||||
#include <AP_HAL/Semaphores.h>
|
||||
|
||||
#if AP_AHRS_SIM_ENABLED
|
||||
#include <SITL/SITL.h>
|
||||
#endif
|
||||
|
||||
#include <AP_NavEKF2/AP_NavEKF2.h>
|
||||
#include <AP_NavEKF3/AP_NavEKF3.h>
|
||||
#include <AP_NavEKF/AP_Nav_Common.h> // definitions shared by inertial and ekf nav filters
|
||||
|
||||
#include "AP_AHRS_DCM.h"
|
||||
#include "AP_AHRS_SIM.h"
|
||||
|
||||
// forward declare view class
|
||||
class AP_AHRS_View;
|
||||
@ -744,10 +741,8 @@ private:
|
||||
EKFType last_active_ekf_type;
|
||||
|
||||
#if AP_AHRS_SIM_ENABLED
|
||||
SITL::SIM *_sitl;
|
||||
uint32_t _last_body_odm_update_ms;
|
||||
void update_SITL(void);
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#if HAL_EXTERNAL_AHRS_ENABLED
|
||||
void update_external(void);
|
||||
@ -805,6 +800,14 @@ private:
|
||||
*/
|
||||
AP_AHRS_DCM dcm{_kp_yaw, _kp, gps_gain, beta, _gps_use, _gps_minsats};
|
||||
struct AP_AHRS_Backend::Estimates dcm_estimates;
|
||||
#if AP_AHRS_SIM_ENABLED
|
||||
#if HAL_NAVEKF3_AVAILABLE
|
||||
AP_AHRS_SIM sim{EKF3};
|
||||
#else
|
||||
AP_AHRS_SIM sim;
|
||||
#endif
|
||||
struct AP_AHRS_Backend::Estimates sim_estimates;
|
||||
#endif
|
||||
|
||||
/*
|
||||
* copy results from a backend over AP_AHRS canonical results.
|
||||
|
||||
@ -140,12 +140,6 @@ public:
|
||||
return false;
|
||||
}
|
||||
|
||||
// return a synthetic airspeed estimate (one derived from sensors
|
||||
// other than an actual airspeed sensor), if available. return
|
||||
// true if we have a synthetic airspeed. ret will not be modified
|
||||
// on failure.
|
||||
virtual bool synthetic_airspeed(float &ret) const WARN_IF_UNUSED = 0;
|
||||
|
||||
// get apparent to true airspeed ratio
|
||||
float get_EAS2TAS(void) const;
|
||||
|
||||
@ -325,4 +319,5 @@ public:
|
||||
// this is not related to terrain following
|
||||
virtual void set_terrain_hgt_stable(bool stable) {}
|
||||
|
||||
virtual void get_control_limits(float &ekfGndSpdLimit, float &controlScaleXY) const = 0;
|
||||
};
|
||||
|
||||
@ -1260,3 +1260,10 @@ bool AP_AHRS_DCM::yaw_source_available(void) const
|
||||
{
|
||||
return AP::compass().use_for_yaw();
|
||||
}
|
||||
|
||||
void AP_AHRS_DCM::get_control_limits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const
|
||||
{
|
||||
// lower gains in VTOL controllers when flying on DCM
|
||||
ekfGndSpdLimit = 50.0;
|
||||
ekfNavVelGainScaler = 0.5;
|
||||
}
|
||||
|
||||
@ -88,7 +88,7 @@ public:
|
||||
// other than an actual airspeed sensor), if available. return
|
||||
// true if we have a synthetic airspeed. ret will not be modified
|
||||
// on failure.
|
||||
bool synthetic_airspeed(float &ret) const override WARN_IF_UNUSED {
|
||||
bool synthetic_airspeed(float &ret) const WARN_IF_UNUSED {
|
||||
ret = _last_airspeed;
|
||||
return true;
|
||||
}
|
||||
@ -127,6 +127,8 @@ public:
|
||||
// return true if DCM has a yaw source
|
||||
bool yaw_source_available(void) const;
|
||||
|
||||
void get_control_limits(float &ekfGndSpdLimit, float &controlScaleXY) const override;
|
||||
|
||||
private:
|
||||
|
||||
// settable parameters
|
||||
|
||||
233
libraries/AP_AHRS/AP_AHRS_SIM.cpp
Normal file
233
libraries/AP_AHRS/AP_AHRS_SIM.cpp
Normal file
@ -0,0 +1,233 @@
|
||||
#include "AP_AHRS_SIM.h"
|
||||
|
||||
#if AP_AHRS_SIM_ENABLED
|
||||
|
||||
#include "AP_AHRS.h"
|
||||
|
||||
bool AP_AHRS_SIM::get_location(struct Location &loc) const
|
||||
{
|
||||
if (_sitl == nullptr) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const struct SITL::sitl_fdm &fdm = _sitl->state;
|
||||
loc = {};
|
||||
loc.lat = fdm.latitude * 1e7;
|
||||
loc.lng = fdm.longitude * 1e7;
|
||||
loc.alt = fdm.altitude*100;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool AP_AHRS_SIM::get_velocity_NED(Vector3f &vec) const
|
||||
{
|
||||
if (_sitl == nullptr) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const struct SITL::sitl_fdm &fdm = _sitl->state;
|
||||
vec = Vector3f(fdm.speedN, fdm.speedE, fdm.speedD);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
Vector3f AP_AHRS_SIM::wind_estimate() const
|
||||
{
|
||||
if (_sitl == nullptr) {
|
||||
return Vector3f{};
|
||||
}
|
||||
|
||||
return _sitl->state.wind_ef;
|
||||
}
|
||||
|
||||
bool AP_AHRS_SIM::airspeed_estimate(float &airspeed_ret) const
|
||||
{
|
||||
if (_sitl == nullptr) {
|
||||
return false;
|
||||
}
|
||||
|
||||
airspeed_ret = _sitl->state.airspeed;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool AP_AHRS_SIM::airspeed_estimate(uint8_t index, float &airspeed_ret) const
|
||||
{
|
||||
return airspeed_estimate(airspeed_ret);
|
||||
}
|
||||
|
||||
bool AP_AHRS_SIM::get_quaternion(Quaternion &quat) const
|
||||
{
|
||||
if (_sitl == nullptr) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const struct SITL::sitl_fdm &fdm = _sitl->state;
|
||||
quat = fdm.quaternion;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
Vector2f AP_AHRS_SIM::groundspeed_vector(void)
|
||||
{
|
||||
if (_sitl == nullptr) {
|
||||
return Vector2f{};
|
||||
}
|
||||
|
||||
const struct SITL::sitl_fdm &fdm = _sitl->state;
|
||||
|
||||
return Vector2f(fdm.speedN, fdm.speedE);
|
||||
}
|
||||
|
||||
bool AP_AHRS_SIM::get_vert_pos_rate(float &velocity) const
|
||||
{
|
||||
if (_sitl == nullptr) {
|
||||
return false;
|
||||
}
|
||||
|
||||
velocity = _sitl->state.speedD;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool AP_AHRS_SIM::get_hagl(float &height) const
|
||||
{
|
||||
if (_sitl == nullptr) {
|
||||
return false;
|
||||
}
|
||||
|
||||
height = _sitl->state.altitude - AP::ahrs().get_home().alt*0.01f;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool AP_AHRS_SIM::get_relative_position_NED_origin(Vector3f &vec) const
|
||||
{
|
||||
if (_sitl == nullptr) {
|
||||
return false;
|
||||
}
|
||||
|
||||
Location loc, orgn;
|
||||
if (!get_location(loc) ||
|
||||
!get_origin(orgn)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const Vector2f diff2d = orgn.get_distance_NE(loc);
|
||||
const struct SITL::sitl_fdm &fdm = _sitl->state;
|
||||
vec = Vector3f(diff2d.x, diff2d.y,
|
||||
-(fdm.altitude - orgn.alt*0.01f));
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool AP_AHRS_SIM::get_relative_position_NE_origin(Vector2f &posNE) const
|
||||
{
|
||||
Location loc, orgn;
|
||||
if (!get_location(loc) ||
|
||||
!get_origin(orgn)) {
|
||||
return false;
|
||||
}
|
||||
posNE = orgn.get_distance_NE(loc);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool AP_AHRS_SIM::get_relative_position_D_origin(float &posD) const
|
||||
{
|
||||
if (_sitl == nullptr) {
|
||||
return false;
|
||||
}
|
||||
const struct SITL::sitl_fdm &fdm = _sitl->state;
|
||||
Location orgn;
|
||||
if (!get_origin(orgn)) {
|
||||
return false;
|
||||
}
|
||||
posD = -(fdm.altitude - orgn.alt*0.01f);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool AP_AHRS_SIM::get_filter_status(nav_filter_status &status) const
|
||||
{
|
||||
memset(&status, 0, sizeof(status));
|
||||
status.flags.attitude = 1;
|
||||
status.flags.horiz_vel = 1;
|
||||
status.flags.vert_vel = 1;
|
||||
status.flags.horiz_pos_rel = 1;
|
||||
status.flags.horiz_pos_abs = 1;
|
||||
status.flags.vert_pos = 1;
|
||||
status.flags.pred_horiz_pos_rel = 1;
|
||||
status.flags.pred_horiz_pos_abs = 1;
|
||||
status.flags.using_gps = 1;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void AP_AHRS_SIM::get_control_limits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const
|
||||
{
|
||||
// same as EKF2 for no optical flow
|
||||
ekfGndSpdLimit = 400.0f;
|
||||
ekfNavVelGainScaler = 1.0f;
|
||||
}
|
||||
|
||||
bool AP_AHRS_SIM::get_mag_offsets(uint8_t mag_idx, Vector3f &magOffsets) const
|
||||
{
|
||||
magOffsets.zero();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void AP_AHRS_SIM::send_ekf_status_report(GCS_MAVLINK &link) const
|
||||
{
|
||||
// send status report with everything looking good
|
||||
const uint16_t flags =
|
||||
EKF_ATTITUDE | /* Set if EKF's attitude estimate is good. | */
|
||||
EKF_VELOCITY_HORIZ | /* Set if EKF's horizontal velocity estimate is good. | */
|
||||
EKF_VELOCITY_VERT | /* Set if EKF's vertical velocity estimate is good. | */
|
||||
EKF_POS_HORIZ_REL | /* Set if EKF's horizontal position (relative) estimate is good. | */
|
||||
EKF_POS_HORIZ_ABS | /* Set if EKF's horizontal position (absolute) estimate is good. | */
|
||||
EKF_POS_VERT_ABS | /* Set if EKF's vertical position (absolute) estimate is good. | */
|
||||
EKF_POS_VERT_AGL | /* Set if EKF's vertical position (above ground) estimate is good. | */
|
||||
//EKF_CONST_POS_MODE | /* EKF is in constant position mode and does not know it's absolute or relative position. | */
|
||||
EKF_PRED_POS_HORIZ_REL | /* Set if EKF's predicted horizontal position (relative) estimate is good. | */
|
||||
EKF_PRED_POS_HORIZ_ABS; /* Set if EKF's predicted horizontal position (absolute) estimate is good. | */
|
||||
mavlink_msg_ekf_status_report_send(link.get_chan(), flags, 0, 0, 0, 0, 0, 0);
|
||||
}
|
||||
|
||||
bool AP_AHRS_SIM::get_origin(Location &ret) const
|
||||
{
|
||||
if (_sitl == nullptr) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ret = _sitl->state.home;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// return the innovations for the specified instance
|
||||
// An out of range instance (eg -1) returns data for the primary instance
|
||||
bool AP_AHRS_SIM::get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const
|
||||
{
|
||||
velInnov.zero();
|
||||
posInnov.zero();
|
||||
magInnov.zero();
|
||||
tasInnov = 0.0f;
|
||||
yawInnov = 0.0f;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool AP_AHRS_SIM::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const
|
||||
{
|
||||
velVar = 0;
|
||||
posVar = 0;
|
||||
hgtVar = 0;
|
||||
magVar.zero();
|
||||
tasVar = 0;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
#endif // AP_AHRS_SIM_ENABLED
|
||||
133
libraries/AP_AHRS/AP_AHRS_SIM.h
Normal file
133
libraries/AP_AHRS/AP_AHRS_SIM.h
Normal file
@ -0,0 +1,133 @@
|
||||
#pragma once
|
||||
|
||||
/*
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/*
|
||||
* SITL-based AHRS (Attitude Heading Reference System) interface for
|
||||
* ArduPilot
|
||||
*
|
||||
*/
|
||||
|
||||
#include "AP_AHRS_config.h"
|
||||
|
||||
#if AP_AHRS_SIM_ENABLED
|
||||
|
||||
#include "AP_AHRS_Backend.h"
|
||||
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
#include <SITL/SITL.h>
|
||||
|
||||
#if HAL_NAVEKF3_AVAILABLE
|
||||
#include <AP_NavEKF3/AP_NavEKF3.h>
|
||||
#endif
|
||||
|
||||
class AP_AHRS_SIM : public AP_AHRS_Backend {
|
||||
public:
|
||||
|
||||
#if HAL_NAVEKF3_AVAILABLE
|
||||
AP_AHRS_SIM(NavEKF3 &_EKF3) :
|
||||
AP_AHRS_Backend(),
|
||||
EKF3(_EKF3)
|
||||
{ }
|
||||
~AP_AHRS_SIM() {}
|
||||
#else
|
||||
// a version of the constructor which doesn't take a non-existant
|
||||
// NavEKF3 class instance as a parameter.
|
||||
AP_AHRS_SIM() : AP_AHRS_Backend() { }
|
||||
#endif
|
||||
|
||||
CLASS_NO_COPY(AP_AHRS_SIM);
|
||||
|
||||
// reset the current gyro drift estimate
|
||||
// should be called if gyro offsets are recalculated
|
||||
void reset_gyro_drift() override {};
|
||||
|
||||
// Methods
|
||||
void update() override { }
|
||||
void get_results(Estimates &results) override;
|
||||
void reset() override { return; }
|
||||
|
||||
// dead-reckoning support
|
||||
virtual bool get_location(struct Location &loc) const override;
|
||||
|
||||
// get latest altitude estimate above ground level in meters and validity flag
|
||||
bool get_hagl(float &hagl) const override WARN_IF_UNUSED;
|
||||
|
||||
// return a wind estimation vector, in m/s
|
||||
Vector3f wind_estimate() const override;
|
||||
|
||||
// return an airspeed estimate if available. return true
|
||||
// if we have an estimate
|
||||
bool airspeed_estimate(float &airspeed_ret) const override;
|
||||
|
||||
// return an airspeed estimate if available. return true
|
||||
// if we have an estimate from a specific sensor index
|
||||
bool airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const override;
|
||||
|
||||
// return a ground vector estimate in meters/second, in North/East order
|
||||
Vector2f groundspeed_vector() override;
|
||||
|
||||
bool use_compass() override { return true; }
|
||||
|
||||
// return the quaternion defining the rotation from NED to XYZ (body) axes
|
||||
bool get_quaternion(Quaternion &quat) const override WARN_IF_UNUSED;
|
||||
|
||||
// is the AHRS subsystem healthy?
|
||||
bool healthy() const override { return true; }
|
||||
|
||||
bool get_velocity_NED(Vector3f &vec) const override;
|
||||
|
||||
// Get a derivative of the vertical position in m/s which is kinematically consistent with the vertical position is required by some control loops.
|
||||
// This is different to the vertical velocity from the EKF which is not always consistent with the vertical position due to the various errors that are being corrected for.
|
||||
bool get_vert_pos_rate(float &velocity) const override;
|
||||
|
||||
// returns false if we fail arming checks, in which case the buffer will be populated with a failure message
|
||||
// requires_position should be true if horizontal position configuration should be checked (not used)
|
||||
bool pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const override { return true; }
|
||||
|
||||
// get_filter_status - returns filter status as a series of flags
|
||||
bool get_filter_status(nav_filter_status &status) const override;
|
||||
|
||||
// get compass offset estimates
|
||||
// true if offsets are valid
|
||||
bool get_mag_offsets(uint8_t mag_idx, Vector3f &magOffsets) const override;
|
||||
|
||||
// relative-origin functions for fallback in AP_InertialNav
|
||||
bool get_origin(Location &ret) const override;
|
||||
bool get_relative_position_NED_origin(Vector3f &vec) const override;
|
||||
bool get_relative_position_NE_origin(Vector2f &posNE) const override;
|
||||
bool get_relative_position_D_origin(float &posD) const override;
|
||||
|
||||
void send_ekf_status_report(class GCS_MAVLINK &link) const override;
|
||||
|
||||
void get_control_limits(float &ekfGndSpdLimit, float &controlScaleXY) const override;
|
||||
bool get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const override;
|
||||
bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const override;
|
||||
|
||||
private:
|
||||
|
||||
#if HAL_NAVEKF3_AVAILABLE
|
||||
// a reference to the EKF3 backend that we can use to send in
|
||||
// body-frame-odometry data into the EKF. Rightfully there should
|
||||
// be something over in the SITL directory doing this.
|
||||
NavEKF3 &EKF3;
|
||||
#endif
|
||||
|
||||
class SITL::SIM *_sitl;
|
||||
uint32_t _last_body_odm_update_ms;
|
||||
};
|
||||
|
||||
#endif // AP_AHRS_SIM_ENABLED
|
||||
Loading…
Reference in New Issue
Block a user