aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Baro: auto-set field elevation from origin

Browse Source
This commit is contained in:
Andrew Tridgell 2022-05-29 10:03:39 +10:00
parent e5e96eb4d8
commit 0f04611970
2 changed files with 42 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
libraries/AP_Baro/AP_Baro.cpp
View File

@ -226,7 +226,7 @@ const AP_Param::GroupInfo AP_Baro::var_info[] = {
#ifndef HAL_BUILD_AP_PERIPH #ifndef HAL_BUILD_AP_PERIPH
// @Param: _FIELD_ELV // @Param: _FIELD_ELV
// @DisplayName: field elevation // @DisplayName: field elevation
// @Description: User provided field elevation in meters. This is used to improve the calculation of the altitude the vehicle is at. This parameter is not persistent and will be reset to 0 every time the vehicle is rebooted. A value of 0 means no correction for takeoff height above sea level is performed. // @Description: User provided field elevation in meters. This is used to improve the calculation of the altitude the vehicle is at. This parameter is not persistent and will be reset to 0 every time the vehicle is rebooted. Changes to this parameter will only be used when disarmed. A value of 0 means the EKF origin height is used for takeoff height above sea level.
// @Units: m // @Units: m
// @Increment: 0.1 // @Increment: 0.1
// @Volatile: True // @Volatile: True
@ -961,21 +961,7 @@ void AP_Baro::update(void)
} }
} }
#ifndef HAL_BUILD_AP_PERIPH #ifndef HAL_BUILD_AP_PERIPH
const uint32_t now_ms = AP_HAL::millis(); update_field_elevation();
if (now_ms - _field_elevation_last_ms >= 1000 && fabsf(_field_elevation_active-_field_elevation) > 1.0) {
if (!AP::arming().is_armed()) {
_field_elevation_last_ms = now_ms;
_field_elevation_active = _field_elevation;
AP::ahrs().resetHeightDatum();
update_calibration();
BARO_SEND_TEXT(MAV_SEVERITY_INFO, "Barometer Field Elevation Set: %.0fm",_field_elevation_active);
}
else {
_field_elevation.set(_field_elevation_active);
_field_elevation.notify();
BARO_SEND_TEXT(MAV_SEVERITY_ALERT, "Failed to Set Field Elevation: Armed");
}
}
#endif #endif
// logging // logging
@ -996,6 +982,44 @@ void AP_Baro::update(void)
#endif #endif
} }
/*
update field elevation value
*/
void AP_Baro::update_field_elevation(void)
{
const uint32_t now_ms = AP_HAL::millis();
bool new_field_elev = false;
const bool armed = hal.util->get_soft_armed();
if (now_ms - _field_elevation_last_ms >= 1000) {
if (is_zero(_field_elevation_active) &&
is_zero(_field_elevation)) {
// auto-set based on origin
Location origin;
if (!armed && AP::ahrs().get_origin(origin)) {
_field_elevation_active = origin.alt * 0.01;
new_field_elev = true;
}
} else if (fabsf(_field_elevation_active-_field_elevation) > 1.0 &&
!is_zero(_field_elevation)) {
// user has set field elevation
if (!armed) {
_field_elevation_active = _field_elevation;
new_field_elev = true;
} else {
_field_elevation.set(_field_elevation_active);
_field_elevation.notify();
BARO_SEND_TEXT(MAV_SEVERITY_ALERT, "Failed to Set Field Elevation: Armed");
}
}
}
if (new_field_elev && !armed) {
_field_elevation_last_ms = now_ms;
AP::ahrs().resetHeightDatum();
update_calibration();
BARO_SEND_TEXT(MAV_SEVERITY_INFO, "Field Elevation Set: %.0fm", _field_elevation_active);
}
}
/* /*
call accumulate on all drivers call accumulate on all drivers
*/ */

3
libraries/AP_Baro/AP_Baro.h
View File

@ -324,7 +324,8 @@ private:
// Logging function // Logging function
void Write_Baro(void); void Write_Baro(void);
void Write_Baro_instance(uint64_t time_us, uint8_t baro_instance); void Write_Baro_instance(uint64_t time_us, uint8_t baro_instance);
void update_field_elevation();
}; };
namespace AP { namespace AP {