Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 04:03:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

Compass_cal: implement the concept of auto and mandatory user reboot after cal

Browse Source
This commit is contained in:
Siddharth Bharat Purohit 2015-08-27 19:44:33 -07:00 committed by Andrew Tridgell
parent 9cf2998bba
commit 6a603eb594
4 changed files with 56 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_Arming/AP_Arming.cpp
View File

@ -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) {

25
libraries/AP_Compass/AP_Compass_Calibration.cpp
View File

@ -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<COMPASS_MAX_INSTANCES; i++) {
if ((1<<i) & mask) {
if (!start_calibration(i,retry,autosave,delay)) {
if (!start_calibration(i,retry,autosave,delay,autoreboot)) {
cancel_calibration_mask(mask);
return false;
}
@ -60,11 +69,11 @@ Compass::start_calibration_mask(uint8_t mask, bool retry, bool autosave, float d
}
bool
Compass::start_calibration_all(bool retry, bool autosave, float delay)
Compass::start_calibration_all(bool retry, bool autosave, float delay, bool autoreboot)
{
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
if (healthy(i) && use_for_yaw(i)) {
if (!start_calibration(i,retry,autosave,delay)) {
if (!start_calibration(i,retry,autosave,delay,autoreboot)) {
cancel_calibration_all();
return false;
}
@ -104,6 +113,7 @@ Compass::accept_calibration(uint8_t i)
uint8_t cal_status = cal.get_status();
if (cal_status == COMPASS_CAL_SUCCESS) {
_cal_complete_requires_reboot = true;
Vector3f ofs, diag, offdiag;
cal.get_calibration(ofs, diag, offdiag);
cal.clear();
@ -194,8 +204,8 @@ void Compass::send_mag_cal_report(mavlink_channel_t chan)
uint8_t& compass_id = i;
uint8_t cal_status = cal.get_status();
if (cal_status == COMPASS_CAL_SUCCESS ||
cal_status == COMPASS_CAL_FAILED) {
if ((cal_status == COMPASS_CAL_SUCCESS ||
cal_status == COMPASS_CAL_FAILED) && ((_reports_sent[i] < MAX_CAL_REPORTS) || CONTINUOUS_REPORTS)) {
float fitness = cal.get_fitness();
Vector3f ofs, diag, offdiag;
cal.get_calibration(ofs, diag, offdiag);
@ -210,6 +220,7 @@ void Compass::send_mag_cal_report(mavlink_channel_t chan)
diag.x, diag.y, diag.z,
offdiag.x, offdiag.y, offdiag.z
);
_reports_sent[i]++;
}
if (cal_status == COMPASS_CAL_SUCCESS && cal.get_autosave()) {

5
libraries/AP_Compass/Compass.cpp
View File

@ -378,12 +378,15 @@ Compass::Compass(void) :
_board_orientation(ROTATION_NONE),
_null_init_done(false),
_thr_or_curr(0.0f),
_hil_mode(false)
_hil_mode(false),
_cal_complete_requires_reboot(false),
_cal_has_run(false)
{
AP_Param::setup_object_defaults(this, var_info);
for (uint8_t i=0; i<COMPASS_MAX_BACKEND; i++) {
_backends[i] = NULL;
_state[i].last_update_usec = 0;
_reports_sent[i] = 0;
}
#if COMPASS_MAX_INSTANCES > 1

20
libraries/AP_Compass/Compass.h
View File

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