diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index 2a71b2852f..dc284c4468 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -242,6 +242,22 @@ bool AP_Arming::compass_checks(bool report) return false; } + //check if compass is calibrating + if(_compass.is_calibrating()) { + if (report) { + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Compass calibration running")); + } + return false; + } + + //check if compass has calibrated and requires reboot + if(_compass.compass_cal_requires_reboot()) { + if (report) { + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Compass calibrated requires reboot")); + } + return false; + } + // check for unreasonable compass offsets Vector3f offsets = _compass.get_offsets(); if (offsets.length() > 600) { diff --git a/libraries/AP_Compass/AP_Compass_Calibration.cpp b/libraries/AP_Compass/AP_Compass_Calibration.cpp index 11cc163791..8a6f0a34b5 100644 --- a/libraries/AP_Compass/AP_Compass_Calibration.cpp +++ b/libraries/AP_Compass/AP_Compass_Calibration.cpp @@ -24,12 +24,20 @@ Compass::compass_cal_update() AP_Notify::flags.compass_cal_running = 1; } } + if (is_calibrating()) { + _cal_has_run = true; + return; + } else if (_cal_has_run && auto_reboot()) { + hal.scheduler->delay(1000); + hal.scheduler->reboot(false); + } } bool -Compass::start_calibration(uint8_t i, bool retry, bool autosave, float delay) +Compass::start_calibration(uint8_t i, bool retry, bool autosave, float delay, bool autoreboot) { if (healthy(i)) { + memset(_reports_sent,0,sizeof(_reports_sent)); if (!is_calibrating() && delay > 0.5f) { AP_Notify::events.initiated_compass_cal = 1; } @@ -39,6 +47,7 @@ Compass::start_calibration(uint8_t i, bool retry, bool autosave, float delay) _calibrator[i].set_tolerance(16); } _calibrator[i].start(retry, autosave, delay); + _compass_cal_autoreboot = autoreboot; return true; } else { return false; @@ -46,11 +55,11 @@ Compass::start_calibration(uint8_t i, bool retry, bool autosave, float delay) } bool -Compass::start_calibration_mask(uint8_t mask, bool retry, bool autosave, float delay) +Compass::start_calibration_mask(uint8_t mask, bool retry, bool autosave, float delay, bool autoreboot) { for (uint8_t i=0; i 1 // default device ids to zero. init() method will overwrite with the actual device ids diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h index 0df63065fc..73758b3e85 100644 --- a/libraries/AP_Compass/Compass.h +++ b/libraries/AP_Compass/Compass.h @@ -46,6 +46,10 @@ #define COMPASS_MAX_BACKEND 1 #endif +//MAXIMUM COMPASS REPORTS +#define MAX_CAL_REPORTS 10 +#define CONTINUOUS_REPORTS 0 + class Compass { friend class AP_Compass_Backend; @@ -131,9 +135,9 @@ public: // compass calibrator interface void compass_cal_update(); - bool start_calibration(uint8_t i, bool retry=false, bool autosave=false, float delay_sec=0.0f); - bool start_calibration_all(bool retry=false, bool autosave=false, float delay_sec=0.0f); - bool start_calibration_mask(uint8_t mask, bool retry=false, bool autosave=false, float delay_sec=0.0f); + bool start_calibration(uint8_t i, bool retry=false, bool autosave=false, float delay_sec=0.0f, bool autoreboot = false); + bool start_calibration_all(bool retry=false, bool autosave=false, float delay_sec=0.0f, bool autoreboot = false); + bool start_calibration_mask(uint8_t mask, bool retry=false, bool autosave=false, float delay_sec=0.0f, bool autoreboot=false); void cancel_calibration(uint8_t i); void cancel_calibration_all(); @@ -143,6 +147,8 @@ public: bool accept_calibration_all(); bool accept_calibration_mask(uint8_t mask); + bool compass_cal_requires_reboot() { return _cal_complete_requires_reboot; } + bool auto_reboot() { return _compass_cal_autoreboot; } uint8_t get_cal_mask() const; bool is_calibrating() const; @@ -299,6 +305,14 @@ private: void _add_backend(AP_Compass_Backend *backend); void _detect_backends(void); + //keep track of number of calibration reports sent + uint8_t _reports_sent[COMPASS_MAX_INSTANCES]; + + //autoreboot after compass calibration + bool _compass_cal_autoreboot; + bool _cal_complete_requires_reboot; + bool _cal_has_run; + // backend objects AP_Compass_Backend *_backends[COMPASS_MAX_BACKEND]; uint8_t _backend_count;