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:59:16 +10:00 committed by Randy Mackay
parent 7b38bdb455
commit 4000983884
5 changed files with 115 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -119,6 +119,9 @@ AP_AHRS_DCM::update(bool skip_ins_update)
// update takeoff/touchdown flags
update_flags();
// remember the last origin for fallback support
IGNORE_RETURN(AP::ahrs().get_origin(last_origin));
}
/*
@ -739,6 +742,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;
@ -1036,10 +1040,13 @@ bool AP_AHRS_DCM::get_position(struct Location &loc) const
loc.terrain_alt = 0;
loc.offset(_position_offset_north, _position_offset_east);
const AP_GPS &_gps = AP::gps();
if (_flags.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) {
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;
}
@ -1160,3 +1167,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;
}

9
libraries/AP_AHRS/AP_AHRS_DCM.h
View File

@ -121,6 +121,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:
float _ki;
float _ki_yaw;
@ -187,6 +193,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;
@ -212,4 +219,6 @@ private:
// time when DCM was last reset
uint32_t _last_startup_ms;
Location last_origin;
};

3
libraries/AP_AHRS/AP_AHRS_Logging.cpp
View File

@ -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));
}

67
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -1122,8 +1122,14 @@ bool AP_AHRS_NavEKF::get_mag_field_correction(Vector3f &vec) const
bool AP_AHRS_NavEKF::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:
@ -1199,7 +1205,7 @@ bool AP_AHRS_NavEKF::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: {
@ -1292,7 +1298,7 @@ bool AP_AHRS_NavEKF::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: {
@ -1362,7 +1368,7 @@ bool AP_AHRS_NavEKF::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: {
@ -1528,17 +1534,15 @@ AP_AHRS_NavEKF::EKFType AP_AHRS_NavEKF::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 == AHRS_VEHICLE_FIXED_WING ||
_vehicle_class == AHRS_VEHICLE_GROUND) &&
(_flags.fly_forward || !hal.util->get_soft_armed())) {
_vehicle_class == AHRS_VEHICLE_GROUND)) {
bool should_use_gps = true;
nav_filter_status filt_state;
#if HAL_NAVEKF2_AVAILABLE
@ -1889,8 +1893,11 @@ void AP_AHRS_NavEKF::writeExtNavVelData(const Vector3f &vel, float err, uint32_t
// get speed limit
void AP_AHRS_NavEKF::getEkfControlLimits(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
@ -1914,12 +1921,27 @@ void AP_AHRS_NavEKF::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVel
#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 recuction factor for Z controllers
*/
float AP_AHRS_NavEKF::getEkfControlScaleZ(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_NavEKF::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
@ -2088,7 +2110,7 @@ bool AP_AHRS_NavEKF::attitudes_consistent(char *failure_msg, const uint8_t failu
// returns the time of the last yaw angle reset or 0 if no reset has ever occurred
uint32_t AP_AHRS_NavEKF::getLastYawResetAngle(float &yawAng)
{
switch (ekf_type()) {
switch (active_EKF_type()) {
case EKFType::NONE:
return 0;
@ -2119,7 +2141,7 @@ uint32_t AP_AHRS_NavEKF::getLastYawResetAngle(float &yawAng)
// returns the time of the last reset or 0 if no reset has ever occurred
uint32_t AP_AHRS_NavEKF::getLastPosNorthEastReset(Vector2f &pos)
{
switch (ekf_type()) {
switch (active_EKF_type()) {
case EKFType::NONE:
return 0;
@ -2150,7 +2172,7 @@ uint32_t AP_AHRS_NavEKF::getLastPosNorthEastReset(Vector2f &pos)
// returns the time of the last reset or 0 if no reset has ever occurred
uint32_t AP_AHRS_NavEKF::getLastVelNorthEastReset(Vector2f &vel) const
{
switch (ekf_type()) {
switch (active_EKF_type()) {
case EKFType::NONE:
return 0;
@ -2182,7 +2204,7 @@ uint32_t AP_AHRS_NavEKF::getLastVelNorthEastReset(Vector2f &vel) const
// returns the time of the last reset or 0 if no reset has ever occurred
uint32_t AP_AHRS_NavEKF::getLastPosDownReset(float &posDelta)
{
switch (ekf_type()) {
switch (active_EKF_type()) {
case EKFType::NONE:
return 0;
@ -2313,7 +2335,7 @@ bool AP_AHRS_NavEKF::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:
@ -2417,10 +2439,9 @@ void AP_AHRS_NavEKF::set_terrain_hgt_stable(bool stable)
// returns true on success (i.e. the EKF knows it's latest position), false on failure
bool AP_AHRS_NavEKF::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:

3
libraries/AP_AHRS/AP_AHRS_NavEKF.h
View File

@ -191,8 +191,9 @@ public:
// Write velocity data from an external navigation system
void writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms) override;
// get speed limit
// get speed limits and controller scaling
void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const;
float getEkfControlScaleZ(void) const;
// is the AHRS subsystem healthy?
bool healthy() const override;