AP_Compass: added COMPASS_OPTIONS

this allows user to set that calibration requires GPS lock
This commit is contained in:
Andrew Tridgell 2019-11-27 09:45:02 +11:00
parent c6aad9b1ec
commit 421fbef2a0
3 changed files with 21 additions and 1 deletions

View File

@ -533,7 +533,14 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @Range: 0 1.3
AP_GROUPINFO("SCALE3", 42, Compass, _state[2].scale_factor, 0),
#endif
// @Param: OPTIONS
// @DisplayName: Compass options
// @Description: This sets options to change the behaviour of the compass
// @Bitmask: 0:CalRequireGPS
// @User: Advanced
AP_GROUPINFO("OPTIONS", 43, Compass, _options, 0),
AP_GROUPEND
};

View File

@ -472,6 +472,12 @@ private:
AP_Int16 _offset_max;
// bitmask of options
enum class Option : uint16_t {
CAL_REQUIRE_GPS = (1U<<0),
};
AP_Int16 _options;
#if COMPASS_CAL_ENABLED
CompassCalibrator _calibrator[COMPASS_MAX_INSTANCES];
#endif

View File

@ -1,5 +1,6 @@
#include <AP_HAL/AP_HAL.h>
#include <AP_Notify/AP_Notify.h>
#include <AP_GPS/AP_GPS.h>
#include <GCS_MAVLink/GCS.h>
#include "AP_Compass.h"
@ -54,6 +55,12 @@ bool Compass::_start_calibration(uint8_t i, bool retry, float delay)
if (!use_for_yaw(i)) {
return false;
}
if (_options.get() & uint16_t(Option::CAL_REQUIRE_GPS)) {
if (AP::gps().status() < AP_GPS::GPS_OK_FIX_2D) {
gcs().send_text(MAV_SEVERITY_ERROR, "Compass cal requires GPS lock");
return false;
}
}
if (!is_calibrating()) {
AP_Notify::events.initiated_compass_cal = 1;
}