mirror of https://github.com/ArduPilot/ardupilot
Plane: ensure home is up to date on arming
remove any discrepancy which has crept in over the last few seconds this also ensures that relative_altitude is updated, and copes with the EKF refusing the resetHeightDatum call
This commit is contained in:
parent
4bef9b7164
commit
67e1fde31f
|
@ -306,6 +306,18 @@ bool AP_Arming_Plane::arm(const AP_Arming::Method method, const bool do_arming_c
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (plane.update_home()) {
|
||||||
|
// after update_home the home position could still be
|
||||||
|
// different from the current_loc if the EKF refused the
|
||||||
|
// resetHeightDatum call. If we are updating home then we want
|
||||||
|
// to force the home to be the current_loc so relative alt
|
||||||
|
// takeoffs work correctly
|
||||||
|
if (plane.ahrs.set_home(plane.current_loc)) {
|
||||||
|
// update current_loc
|
||||||
|
plane.update_current_loc();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
change_arm_state();
|
change_arm_state();
|
||||||
|
|
||||||
// rising edge of delay_arming oneshot
|
// rising edge of delay_arming oneshot
|
||||||
|
|
|
@ -406,10 +406,7 @@ void Plane::update_GPS_50Hz(void)
|
||||||
{
|
{
|
||||||
gps.update();
|
gps.update();
|
||||||
|
|
||||||
// get position from AHRS
|
update_current_loc();
|
||||||
have_position = ahrs.get_location(current_loc);
|
|
||||||
ahrs.get_relative_position_D_home(relative_altitude);
|
|
||||||
relative_altitude *= -1.0f;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
@ -816,4 +813,16 @@ void Plane::get_osd_roll_pitch_rad(float &roll, float &pitch) const
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
update current_loc Location
|
||||||
|
*/
|
||||||
|
void Plane::update_current_loc(void)
|
||||||
|
{
|
||||||
|
have_position = plane.ahrs.get_location(plane.current_loc);
|
||||||
|
|
||||||
|
// re-calculate relative altitude
|
||||||
|
ahrs.get_relative_position_D_home(plane.relative_altitude);
|
||||||
|
relative_altitude *= -1.0f;
|
||||||
|
}
|
||||||
|
|
||||||
AP_HAL_MAIN_CALLBACKS(&plane);
|
AP_HAL_MAIN_CALLBACKS(&plane);
|
||||||
|
|
|
@ -946,7 +946,13 @@ private:
|
||||||
|
|
||||||
// commands.cpp
|
// commands.cpp
|
||||||
void set_guided_WP(const Location &loc);
|
void set_guided_WP(const Location &loc);
|
||||||
void update_home();
|
|
||||||
|
// update home position. Return true if update done
|
||||||
|
bool update_home();
|
||||||
|
|
||||||
|
// update current_loc
|
||||||
|
void update_current_loc(void);
|
||||||
|
|
||||||
// set home location and store it persistently:
|
// set home location and store it persistently:
|
||||||
bool set_home_persistently(const Location &loc) WARN_IF_UNUSED;
|
bool set_home_persistently(const Location &loc) WARN_IF_UNUSED;
|
||||||
|
|
||||||
|
|
|
@ -105,11 +105,13 @@ void Plane::set_guided_WP(const Location &loc)
|
||||||
update home location from GPS
|
update home location from GPS
|
||||||
this is called as long as we have 3D lock and the arming switch is
|
this is called as long as we have 3D lock and the arming switch is
|
||||||
not pushed
|
not pushed
|
||||||
|
|
||||||
|
returns true if home is changed
|
||||||
*/
|
*/
|
||||||
void Plane::update_home()
|
bool Plane::update_home()
|
||||||
{
|
{
|
||||||
if (hal.util->was_watchdog_armed()) {
|
if (hal.util->was_watchdog_armed()) {
|
||||||
return;
|
return false;
|
||||||
}
|
}
|
||||||
if ((g2.home_reset_threshold == -1) ||
|
if ((g2.home_reset_threshold == -1) ||
|
||||||
((g2.home_reset_threshold > 0) &&
|
((g2.home_reset_threshold > 0) &&
|
||||||
|
@ -118,24 +120,30 @@ void Plane::update_home()
|
||||||
// significantly. This allows us to cope with slow baro drift
|
// significantly. This allows us to cope with slow baro drift
|
||||||
// but not re-do home and the baro if we have changed height
|
// but not re-do home and the baro if we have changed height
|
||||||
// significantly
|
// significantly
|
||||||
return;
|
return false;
|
||||||
}
|
}
|
||||||
if (ahrs.home_is_set() && !ahrs.home_is_locked()) {
|
bool ret = false;
|
||||||
|
if (ahrs.home_is_set() && !ahrs.home_is_locked() && gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
|
||||||
Location loc;
|
Location loc;
|
||||||
if(ahrs.get_location(loc) && gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
|
if (ahrs.get_location(loc)) {
|
||||||
// we take the altitude directly from the GPS as we are
|
// we take the altitude directly from the GPS as we are
|
||||||
// about to reset the baro calibration. We can't use AHRS
|
// about to reset the baro calibration. We can't use AHRS
|
||||||
// altitude or we can end up perpetuating a bias in
|
// altitude or we can end up perpetuating a bias in
|
||||||
// altitude, as AHRS alt depends on home alt, which means
|
// altitude, as AHRS alt depends on home alt, which means
|
||||||
// we would have a circular dependency
|
// we would have a circular dependency
|
||||||
loc.alt = gps.location().alt;
|
loc.alt = gps.location().alt;
|
||||||
if (!AP::ahrs().set_home(loc)) {
|
ret = AP::ahrs().set_home(loc);
|
||||||
// silently fail
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// even if home is not updated we do a baro reset to stop baro
|
||||||
|
// drift errors while disarmed
|
||||||
barometer.update_calibration();
|
barometer.update_calibration();
|
||||||
ahrs.resetHeightDatum();
|
ahrs.resetHeightDatum();
|
||||||
|
|
||||||
|
update_current_loc();
|
||||||
|
|
||||||
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool Plane::set_home_persistently(const Location &loc)
|
bool Plane::set_home_persistently(const Location &loc)
|
||||||
|
|
Loading…
Reference in New Issue