Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: allow for fallback to DCM in quadplanes 

this adds the necessary functions to allow for flying a quadplane on
DCM as an emergency fallback. It sets the NavGainScalar to 0.5 to
reduce the VTOL controller gains to allow planes to cope with the
higher lag of DCM
This commit is contained in:
Andrew Tridgell 2021-08-14 13:03:42 +10:00
parent d64621e349
commit 53e7436525
6 changed files with 121 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -1332,8 +1332,14 @@ bool AP_AHRS::get_mag_field_correction(Vector3f &vec) const
bool AP_AHRS::get_vert_pos_rate(float &velocity) const
{
switch (active_EKF_type()) {
case EKFType::NONE:
return false;
case EKFType::NONE: {
Vector3f velned;
if (!AP_AHRS_DCM::get_velocity_NED(velned)) {
return false;
}
velocity = velned.z;
return true;
}
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
@ -1409,7 +1415,7 @@ bool AP_AHRS::get_relative_position_NED_origin(Vector3f &vec) const
{
switch (active_EKF_type()) {
case EKFType::NONE:
return false;
return AP_AHRS_DCM::get_relative_position_NED_origin(vec);
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO: {
@ -1502,7 +1508,7 @@ bool AP_AHRS::get_relative_position_NE_origin(Vector2f &posNE) const
{
switch (active_EKF_type()) {
case EKFType::NONE:
return false;
return AP_AHRS_DCM::get_relative_position_NE_origin(posNE);
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO: {
@ -1572,7 +1578,7 @@ bool AP_AHRS::get_relative_position_D_origin(float &posD) const
{
switch (active_EKF_type()) {
case EKFType::NONE:
return false;
return AP_AHRS_DCM::get_relative_position_D_origin(posD);
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO: {
@ -1738,17 +1744,15 @@ AP_AHRS::EKFType AP_AHRS::active_EKF_type(void) const
}
/*
fixed wing and rover when in fly_forward mode will fall back to
DCM if the EKF doesn't have GPS. This is the safest option as
DCM is very robust. Note that we also check the filter status
when fly_forward is false and we are disarmed. This is to ensure
that the arming checks do wait for good GPS position on fixed
wing and rover
fixed wing and rover will fall back to DCM if the EKF doesn't
have GPS. This is the safest option as DCM is very robust. Note
that we also check the filter status when fly_forward is false
and we are disarmed. This is to ensure that the arming checks do
wait for good GPS position on fixed wing and rover
*/
if (ret != EKFType::NONE &&
(_vehicle_class == VehicleClass::FIXED_WING ||
_vehicle_class == VehicleClass::GROUND) &&
(fly_forward || !hal.util->get_soft_armed())) {
_vehicle_class == VehicleClass::GROUND)) {
bool should_use_gps = true;
nav_filter_status filt_state;
#if HAL_NAVEKF2_AVAILABLE
@ -2096,11 +2100,14 @@ void AP_AHRS::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeSt
#endif
}
// get speed limit
void AP_AHRS::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const
// get speed limit and XY navigation gain scale factor
void AP_AHRS::getControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const
{
switch (ekf_type()) {
switch (active_EKF_type()) {
case EKFType::NONE:
// lower gains in VTOL controllers when flying on DCM
ekfGndSpdLimit = 50.0;
ekfNavVelGainScaler = 0.5;
break;
#if HAL_NAVEKF2_AVAILABLE
@ -2124,12 +2131,27 @@ void AP_AHRS::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainSca
#endif
#if HAL_EXTERNAL_AHRS_ENABLED
case EKFType::EXTERNAL:
// no limits
// no limit on gains, large vel limit
ekfGndSpdLimit = 400;
ekfNavVelGainScaler = 1;
break;
#endif
}
}
/*
get gain factor for Z controllers
*/
float AP_AHRS::getControlScaleZ(void) const
{
if (active_EKF_type() == EKFType::NONE) {
// when flying on DCM lower gains by 4x to cope with the high
// lag
return 0.25;
}
return 1;
}
// get compass offset estimates
// true if offsets are valid
bool AP_AHRS::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
@ -2298,7 +2320,7 @@ bool AP_AHRS::attitudes_consistent(char *failure_msg, const uint8_t failure_msg_
// returns the time of the last yaw angle reset or 0 if no reset has ever occurred
uint32_t AP_AHRS::getLastYawResetAngle(float &yawAng)
{
switch (ekf_type()) {
switch (active_EKF_type()) {
case EKFType::NONE:
return 0;
@ -2329,7 +2351,7 @@ uint32_t AP_AHRS::getLastYawResetAngle(float &yawAng)
// returns the time of the last reset or 0 if no reset has ever occurred
uint32_t AP_AHRS::getLastPosNorthEastReset(Vector2f &pos)
{
switch (ekf_type()) {
switch (active_EKF_type()) {
case EKFType::NONE:
return 0;
@ -2360,7 +2382,7 @@ uint32_t AP_AHRS::getLastPosNorthEastReset(Vector2f &pos)
// returns the time of the last reset or 0 if no reset has ever occurred
uint32_t AP_AHRS::getLastVelNorthEastReset(Vector2f &vel) const
{
switch (ekf_type()) {
switch (active_EKF_type()) {
case EKFType::NONE:
return 0;
@ -2392,7 +2414,7 @@ uint32_t AP_AHRS::getLastVelNorthEastReset(Vector2f &vel) const
// returns the time of the last reset or 0 if no reset has ever occurred
uint32_t AP_AHRS::getLastPosDownReset(float &posDelta)
{
switch (ekf_type()) {
switch (active_EKF_type()) {
case EKFType::NONE:
return 0;
@ -2523,7 +2545,7 @@ bool AP_AHRS::get_origin(Location &ret) const
{
switch (ekf_type()) {
case EKFType::NONE:
return false;
return AP_AHRS_DCM::get_origin_fallback(ret);
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
@ -2624,13 +2646,12 @@ void AP_AHRS::set_terrain_hgt_stable(bool stable)
}
// get_location - updates the provided location with the latest calculated location
// returns true on success (i.e. the EKF knows it's latest position), false on failure
// returns true on success (i.e. the backend knows it's latest position), false on failure
bool AP_AHRS::get_location(struct Location &loc) const
{
switch (ekf_type()) {
switch (active_EKF_type()) {
case EKFType::NONE:
// We are not using an EKF so no data
return false;
return get_position(loc);
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:

5
libraries/AP_AHRS/AP_AHRS.h
View File

@ -203,7 +203,8 @@ public:
void writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms) override;
// get speed limit
void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const;
void getControlLimits(float &ekfGndSpdLimit, float &controlScaleXY) const;
float getControlScaleZ(void) const;
// is the AHRS subsystem healthy?
bool healthy() const override;
@ -330,7 +331,7 @@ public:
void set_alt_measurement_noise(float noise) override;
// active EKF type for logging
uint8_t get_active_AHRS_type(void) const override {
uint8_t get_active_AHRS_type(void) const {
return uint8_t(active_EKF_type());
}

3
libraries/AP_AHRS/AP_AHRS_Backend.h
View File

@ -429,9 +429,6 @@ public:
return _rsem;
}
// active AHRS type for logging
virtual uint8_t get_active_AHRS_type(void) const { return 0; }
// Logging to disk functions
void Write_AHRS2(void) const;
void Write_Attitude(const Vector3f &targets) const;

62
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -108,6 +108,9 @@ AP_AHRS_DCM::update(bool skip_ins_update)
update_trig();
backup_attitude();
// remember the last origin for fallback support
IGNORE_RETURN(AP::ahrs().get_origin(last_origin));
}
/*
@ -744,6 +747,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
// use GPS for positioning with any fix, even a 2D fix
_last_lat = _gps.location().lat;
_last_lng = _gps.location().lng;
_last_pos_ms = AP_HAL::millis();
_position_offset_north = 0;
_position_offset_east = 0;
@ -1047,10 +1051,13 @@ bool AP_AHRS_DCM::get_position(struct Location &loc) const
loc.relative_alt = 0;
loc.terrain_alt = 0;
loc.offset(_position_offset_north, _position_offset_east);
if (AP::ahrs().get_fly_forward() && _have_position) {
float gps_delay_sec = 0;
gps.get_lag(gps_delay_sec);
loc.offset_bearing(gps.ground_course(), gps.ground_speed() * gps_delay_sec);
if (_have_position) {
const uint32_t now = AP_HAL::millis();
float dt = 0;
gps.get_lag(dt);
dt += constrain_float((now - _last_pos_ms) * 0.001, 0, 0.5);
Vector2f dpos = _last_velocity.xy() * dt;
loc.offset(dpos.x, dpos.y);
}
return _have_position;
}
@ -1177,3 +1184,50 @@ bool AP_AHRS_DCM::pre_arm_check(bool requires_position, char *failure_msg, uint8
}
return true;
}
/*
relative-origin functions for fallback in AP_InertialNav
*/
bool AP_AHRS_DCM::get_origin_fallback(Location &ret) const
{
ret = last_origin;
if (ret.is_zero()) {
// use home if we never have had an origin
ret = AP::ahrs().get_home();
}
return !ret.is_zero();
}
bool AP_AHRS_DCM::get_relative_position_NED_origin(Vector3f &posNED) const
{
Location origin;
if (!AP_AHRS_DCM::get_origin_fallback(origin)) {
return false;
}
Location loc;
if (!AP_AHRS_DCM::get_position(loc)) {
return false;
}
posNED = origin.get_distance_NED(loc);
return true;
}
bool AP_AHRS_DCM::get_relative_position_NE_origin(Vector2f &posNE) const
{
Vector3f posNED;
if (!AP_AHRS_DCM::get_relative_position_NED_origin(posNED)) {
return false;
}
posNE = posNED.xy();
return true;
}
bool AP_AHRS_DCM::get_relative_position_D_origin(float &posD) const
{
Vector3f posNED;
if (!AP_AHRS_DCM::get_relative_position_NED_origin(posNED)) {
return false;
}
posD = posNED.z;
return true;
}

11
libraries/AP_AHRS/AP_AHRS_DCM.h
View File

@ -35,7 +35,6 @@ public:
AP_AHRS_DCM(const AP_AHRS_DCM &other) = delete;
AP_AHRS_DCM &operator=(const AP_AHRS_DCM&) = delete;
// return the smoothed gyro vector corrected for drift
const Vector3f &get_gyro() const override {
return _omega;
@ -118,6 +117,12 @@ public:
// 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;
// relative-origin functions for fallback in AP_InertialNav
bool get_origin_fallback(Location &ret) const;
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;
private:
// these are experimentally derived from the simulator
@ -198,6 +203,7 @@ private:
// the lat/lng where we last had GPS lock
int32_t _last_lat;
int32_t _last_lng;
uint32_t _last_pos_ms;
// position offset from last GPS lock
float _position_offset_north;
@ -223,4 +229,7 @@ private:
// time when DCM was last reset
uint32_t _last_startup_ms;
// last origin we returned, for DCM fallback from EKF
Location last_origin;
};

5
libraries/AP_AHRS/AP_AHRS_Logging.cpp
View File

@ -58,7 +58,7 @@ void AP_AHRS_Backend::Write_Attitude(const Vector3f &targets) const
yaw : (uint16_t)wrap_360_cd(yaw_sensor),
error_rp : (uint16_t)(get_error_rp() * 100),
error_yaw : (uint16_t)(get_error_yaw() * 100),
active : get_active_AHRS_type(),
active : AP::ahrs().get_active_AHRS_type(),
};
AP::logger().WriteBlock(&pkt, sizeof(pkt));
}
@ -110,7 +110,8 @@ void AP_AHRS_View::Write_AttitudeView(const Vector3f &targets) const
control_yaw : (uint16_t)wrap_360_cd(targets.z),
yaw : (uint16_t)wrap_360_cd(yaw_sensor),
error_rp : (uint16_t)(get_error_rp() * 100),
error_yaw : (uint16_t)(get_error_yaw() * 100)
error_yaw : (uint16_t)(get_error_yaw() * 100),
active : AP::ahrs().get_active_AHRS_type()
};
AP::logger().WriteBlock(&pkt, sizeof(pkt));
}