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

AP_Compass: make compass_cal_status_t enum class Status within CompassCalibrator

Browse Source
This commit is contained in:
Peter Barker 2019-11-22 14:57:17 +11:00 committed by Peter Barker
parent 9c17fdb851
commit 823f2bb217
3 changed files with 87 additions and 86 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
libraries/AP_Compass/AP_Compass_Calibration.cpp
View File

@ -31,7 +31,7 @@ void Compass::cal_update()
if (_calibrator[i].running()) { if (_calibrator[i].running()) {
running = true; running = true;
} else if (_cal_autosave && !_cal_saved[i] && _calibrator[i].get_status() == COMPASS_CAL_SUCCESS) { } else if (_cal_autosave && !_cal_saved[i] && _calibrator[i].get_status() == CompassCalibrator::Status::SUCCESS) {
_accept_calibration(i); _accept_calibration(i);
} }
} }
@ -119,7 +119,7 @@ void Compass::_cancel_calibration(uint8_t i)
{ {
AP_Notify::events.initiated_compass_cal = 0; AP_Notify::events.initiated_compass_cal = 0;
if (_calibrator[i].running() || _calibrator[i].get_status() == COMPASS_CAL_WAITING_TO_START) { if (_calibrator[i].running() || _calibrator[i].get_status() == CompassCalibrator::Status::WAITING_TO_START) {
AP_Notify::events.compass_cal_canceled = 1; AP_Notify::events.compass_cal_canceled = 1;
} }
_cal_saved[i] = false; _cal_saved[i] = false;
@ -143,11 +143,11 @@ void Compass::cancel_calibration_all()
bool Compass::_accept_calibration(uint8_t i) bool Compass::_accept_calibration(uint8_t i)
{ {
CompassCalibrator& cal = _calibrator[i]; CompassCalibrator& cal = _calibrator[i];
uint8_t cal_status = cal.get_status(); const CompassCalibrator::Status cal_status = cal.get_status();
if (_cal_saved[i] || cal_status == COMPASS_CAL_NOT_STARTED) { if (_cal_saved[i] || cal_status == CompassCalibrator::Status::NOT_STARTED) {
return true; return true;
} else if (cal_status == COMPASS_CAL_SUCCESS) { } else if (cal_status == CompassCalibrator::Status::SUCCESS) {
_cal_complete_requires_reboot = true; _cal_complete_requires_reboot = true;
_cal_saved[i] = true; _cal_saved[i] = true;
@ -202,11 +202,11 @@ bool Compass::send_mag_cal_progress(const GCS_MAVLINK& link)
} }
auto& calibrator = _calibrator[compass_id]; auto& calibrator = _calibrator[compass_id];
uint8_t cal_status = calibrator.get_status(); const CompassCalibrator::Status cal_status = calibrator.get_status();
if (cal_status == COMPASS_CAL_WAITING_TO_START || if (cal_status == CompassCalibrator::Status::WAITING_TO_START ||
cal_status == COMPASS_CAL_RUNNING_STEP_ONE || cal_status == CompassCalibrator::Status::RUNNING_STEP_ONE ||
cal_status == COMPASS_CAL_RUNNING_STEP_TWO) { cal_status == CompassCalibrator::Status::RUNNING_STEP_TWO) {
uint8_t completion_pct = calibrator.get_completion_percent(); uint8_t completion_pct = calibrator.get_completion_percent();
const CompassCalibrator::completion_mask_t& completion_mask = calibrator.get_completion_mask(); const CompassCalibrator::completion_mask_t& completion_mask = calibrator.get_completion_mask();
const Vector3f direction; const Vector3f direction;
@ -215,7 +215,7 @@ bool Compass::send_mag_cal_progress(const GCS_MAVLINK& link)
mavlink_msg_mag_cal_progress_send( mavlink_msg_mag_cal_progress_send(
link.get_chan(), link.get_chan(),
compass_id, cal_mask, compass_id, cal_mask,
cal_status, attempt, completion_pct, completion_mask, (uint8_t)cal_status, attempt, completion_pct, completion_mask,
direction.x, direction.y, direction.z direction.x, direction.y, direction.z
); );
} }
@ -237,10 +237,10 @@ bool Compass::send_mag_cal_report(const GCS_MAVLINK& link)
return true; return true;
} }
uint8_t cal_status = _calibrator[compass_id].get_status(); const CompassCalibrator::Status cal_status = _calibrator[compass_id].get_status();
if (cal_status == COMPASS_CAL_SUCCESS || if (cal_status == CompassCalibrator::Status::SUCCESS ||
cal_status == COMPASS_CAL_FAILED || cal_status == CompassCalibrator::Status::FAILED ||
cal_status == COMPASS_CAL_BAD_ORIENTATION) { cal_status == CompassCalibrator::Status::BAD_ORIENTATION) {
float fitness = _calibrator[compass_id].get_fitness(); float fitness = _calibrator[compass_id].get_fitness();
Vector3f ofs, diag, offdiag; Vector3f ofs, diag, offdiag;
float scale_factor; float scale_factor;
@ -250,7 +250,7 @@ bool Compass::send_mag_cal_report(const GCS_MAVLINK& link)
mavlink_msg_mag_cal_report_send( mavlink_msg_mag_cal_report_send(
link.get_chan(), link.get_chan(),
compass_id, cal_mask, compass_id, cal_mask,
cal_status, autosaved, (uint8_t)cal_status, autosaved,
fitness, fitness,
ofs.x, ofs.y, ofs.z, ofs.x, ofs.y, ofs.z,
diag.x, diag.y, diag.z, diag.x, diag.y, diag.z,
@ -269,10 +269,10 @@ bool Compass::is_calibrating() const
{ {
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) { for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
switch(_calibrator[i].get_status()) { switch(_calibrator[i].get_status()) {
case COMPASS_CAL_NOT_STARTED: case CompassCalibrator::Status::NOT_STARTED:
case COMPASS_CAL_SUCCESS: case CompassCalibrator::Status::SUCCESS:
case COMPASS_CAL_FAILED: case CompassCalibrator::Status::FAILED:
case COMPASS_CAL_BAD_ORIENTATION: case CompassCalibrator::Status::BAD_ORIENTATION:
break; break;
default: default:
return true; return true;
@ -285,7 +285,7 @@ uint8_t Compass::_get_cal_mask() const
{ {
uint8_t cal_mask = 0; uint8_t cal_mask = 0;
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) { for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
if (_calibrator[i].get_status() != COMPASS_CAL_NOT_STARTED) { if (_calibrator[i].get_status() != CompassCalibrator::Status::NOT_STARTED) {
cal_mask |= 1 << i; cal_mask |= 1 << i;
} }
} }

103
libraries/AP_Compass/CompassCalibrator.cpp
View File

@ -33,7 +33,7 @@
* origin-centered sphere. * origin-centered sphere.
* *
* The state machine of this library is described entirely by the * The state machine of this library is described entirely by the
* compass_cal_status_t enum, and all state transitions are managed by the * CompassCalibrator::Status enum, and all state transitions are managed by the
* set_status function. Normally, the library is in the NOT_STARTED state. When * set_status function. Normally, the library is in the NOT_STARTED state. When
* the start function is called, the state transitions to WAITING_TO_START, * the start function is called, the state transitions to WAITING_TO_START,
* until two conditions are met: the delay as elapsed, and the memory for the * until two conditions are met: the delay as elapsed, and the memory for the
@ -82,7 +82,7 @@ CompassCalibrator::CompassCalibrator():
void CompassCalibrator::stop() void CompassCalibrator::stop()
{ {
set_status(COMPASS_CAL_NOT_STARTED); set_status(Status::NOT_STARTED);
} }
void CompassCalibrator::set_orientation(enum Rotation orientation, bool is_external, bool fix_orientation) void CompassCalibrator::set_orientation(enum Rotation orientation, bool is_external, bool fix_orientation)
@ -105,12 +105,12 @@ void CompassCalibrator::start(bool retry, float delay, uint16_t offset_max, uint
_delay_start_sec = delay; _delay_start_sec = delay;
_start_time_ms = AP_HAL::millis(); _start_time_ms = AP_HAL::millis();
_compass_idx = compass_idx; _compass_idx = compass_idx;
set_status(COMPASS_CAL_WAITING_TO_START); set_status(Status::WAITING_TO_START);
} }
void CompassCalibrator::get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals, float &scale_factor) void CompassCalibrator::get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals, float &scale_factor)
{ {
if (_status != COMPASS_CAL_SUCCESS) { if (_status != Status::SUCCESS) {
return; return;
} }
@ -123,23 +123,24 @@ void CompassCalibrator::get_calibration(Vector3f &offsets, Vector3f &diagonals,
float CompassCalibrator::get_completion_percent() const float CompassCalibrator::get_completion_percent() const
{ {
// first sampling step is 1/3rd of the progress bar // first sampling step is 1/3rd of the progress bar
// never return more than 99% unless _status is COMPASS_CAL_SUCCESS // never return more than 99% unless _status is Status::SUCCESS
switch (_status) { switch (_status) {
case COMPASS_CAL_NOT_STARTED: case Status::NOT_STARTED:
case COMPASS_CAL_WAITING_TO_START: case Status::WAITING_TO_START:
return 0.0f; return 0.0f;
case COMPASS_CAL_RUNNING_STEP_ONE: case Status::RUNNING_STEP_ONE:
return 33.3f * _samples_collected/COMPASS_CAL_NUM_SAMPLES; return 33.3f * _samples_collected/COMPASS_CAL_NUM_SAMPLES;
case COMPASS_CAL_RUNNING_STEP_TWO: case Status::RUNNING_STEP_TWO:
return 33.3f + 65.7f*((float)(_samples_collected-_samples_thinned)/(COMPASS_CAL_NUM_SAMPLES-_samples_thinned)); return 33.3f + 65.7f*((float)(_samples_collected-_samples_thinned)/(COMPASS_CAL_NUM_SAMPLES-_samples_thinned));
case COMPASS_CAL_SUCCESS: case Status::SUCCESS:
return 100.0f; return 100.0f;
case COMPASS_CAL_FAILED: case Status::FAILED:
case COMPASS_CAL_BAD_ORIENTATION: case Status::BAD_ORIENTATION:
case COMPASS_CAL_BAD_RADIUS: case Status::BAD_RADIUS:
default:
return 0.0f; return 0.0f;
}; };
// will not get here if the compiler is doing its job (no default clause)
return 0.0f;
} }
// update completion mask based on latest sample // update completion mask based on latest sample
@ -173,7 +174,7 @@ bool CompassCalibrator::check_for_timeout()
uint32_t tnow = AP_HAL::millis(); uint32_t tnow = AP_HAL::millis();
if (running() && tnow - _last_sample_ms > 1000) { if (running() && tnow - _last_sample_ms > 1000) {
_retry = false; _retry = false;
set_status(COMPASS_CAL_FAILED); set_status(Status::FAILED);
return true; return true;
} }
return false; return false;
@ -183,8 +184,8 @@ void CompassCalibrator::new_sample(const Vector3f& sample)
{ {
_last_sample_ms = AP_HAL::millis(); _last_sample_ms = AP_HAL::millis();
if (_status == COMPASS_CAL_WAITING_TO_START) { if (_status == Status::WAITING_TO_START) {
set_status(COMPASS_CAL_RUNNING_STEP_ONE); set_status(Status::RUNNING_STEP_ONE);
} }
if (running() && _samples_collected < COMPASS_CAL_NUM_SAMPLES && accept_sample(sample)) { if (running() && _samples_collected < COMPASS_CAL_NUM_SAMPLES && accept_sample(sample)) {
@ -204,13 +205,13 @@ void CompassCalibrator::update(bool &failure)
return; return;
} }
if (_status == COMPASS_CAL_RUNNING_STEP_ONE) { if (_status == Status::RUNNING_STEP_ONE) {
if (_fit_step >= 10) { if (_fit_step >= 10) {
if (is_equal(_fitness, _initial_fitness) || isnan(_fitness)) { // if true, means that fitness is diverging instead of converging if (is_equal(_fitness, _initial_fitness) || isnan(_fitness)) { // if true, means that fitness is diverging instead of converging
set_status(COMPASS_CAL_FAILED); set_status(Status::FAILED);
failure = true; failure = true;
} else { } else {
set_status(COMPASS_CAL_RUNNING_STEP_TWO); set_status(Status::RUNNING_STEP_TWO);
} }
} else { } else {
if (_fit_step == 0) { if (_fit_step == 0) {
@ -219,12 +220,12 @@ void CompassCalibrator::update(bool &failure)
run_sphere_fit(); run_sphere_fit();
_fit_step++; _fit_step++;
} }
} else if (_status == COMPASS_CAL_RUNNING_STEP_TWO) { } else if (_status == Status::RUNNING_STEP_TWO) {
if (_fit_step >= 35) { if (_fit_step >= 35) {
if (fit_acceptable() && fix_radius() && calculate_orientation()) { if (fit_acceptable() && fix_radius() && calculate_orientation()) {
set_status(COMPASS_CAL_SUCCESS); set_status(Status::SUCCESS);
} else { } else {
set_status(COMPASS_CAL_FAILED); set_status(Status::FAILED);
failure = true; failure = true;
} }
} else if (_fit_step < 15) { } else if (_fit_step < 15) {
@ -242,7 +243,7 @@ void CompassCalibrator::update(bool &failure)
///////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////
bool CompassCalibrator::running() const bool CompassCalibrator::running() const
{ {
return _status == COMPASS_CAL_RUNNING_STEP_ONE || _status == COMPASS_CAL_RUNNING_STEP_TWO; return _status == Status::RUNNING_STEP_ONE || _status == Status::RUNNING_STEP_TWO;
} }
bool CompassCalibrator::fitting() const bool CompassCalibrator::fitting() const
@ -278,30 +279,30 @@ void CompassCalibrator::reset_state()
initialize_fit(); initialize_fit();
} }
bool CompassCalibrator::set_status(compass_cal_status_t status) bool CompassCalibrator::set_status(CompassCalibrator::Status status)
{ {
if (status != COMPASS_CAL_NOT_STARTED && _status == status) { if (status != Status::NOT_STARTED && _status == status) {
return true; return true;
} }
switch (status) { switch (status) {
case COMPASS_CAL_NOT_STARTED: case Status::NOT_STARTED:
reset_state(); reset_state();
_status = COMPASS_CAL_NOT_STARTED; _status = Status::NOT_STARTED;
if (_sample_buffer != nullptr) { if (_sample_buffer != nullptr) {
free(_sample_buffer); free(_sample_buffer);
_sample_buffer = nullptr; _sample_buffer = nullptr;
} }
return true; return true;
case COMPASS_CAL_WAITING_TO_START: case Status::WAITING_TO_START:
reset_state(); reset_state();
_status = COMPASS_CAL_WAITING_TO_START; _status = Status::WAITING_TO_START;
set_status(COMPASS_CAL_RUNNING_STEP_ONE); set_status(Status::RUNNING_STEP_ONE);
return true; return true;
case COMPASS_CAL_RUNNING_STEP_ONE: case Status::RUNNING_STEP_ONE:
if (_status != COMPASS_CAL_WAITING_TO_START) { if (_status != Status::WAITING_TO_START) {
return false; return false;
} }
@ -315,22 +316,22 @@ bool CompassCalibrator::set_status(compass_cal_status_t status)
} }
if (_sample_buffer != nullptr) { if (_sample_buffer != nullptr) {
initialize_fit(); initialize_fit();
_status = COMPASS_CAL_RUNNING_STEP_ONE; _status = Status::RUNNING_STEP_ONE;
return true; return true;
} }
return false; return false;
case COMPASS_CAL_RUNNING_STEP_TWO: case Status::RUNNING_STEP_TWO:
if (_status != COMPASS_CAL_RUNNING_STEP_ONE) { if (_status != Status::RUNNING_STEP_ONE) {
return false; return false;
} }
thin_samples(); thin_samples();
initialize_fit(); initialize_fit();
_status = COMPASS_CAL_RUNNING_STEP_TWO; _status = Status::RUNNING_STEP_TWO;
return true; return true;
case COMPASS_CAL_SUCCESS: case Status::SUCCESS:
if (_status != COMPASS_CAL_RUNNING_STEP_TWO) { if (_status != Status::RUNNING_STEP_TWO) {
return false; return false;
} }
@ -339,24 +340,24 @@ bool CompassCalibrator::set_status(compass_cal_status_t status)
_sample_buffer = nullptr; _sample_buffer = nullptr;
} }
_status = COMPASS_CAL_SUCCESS; _status = Status::SUCCESS;
return true; return true;
case COMPASS_CAL_FAILED: case Status::FAILED:
if (_status == COMPASS_CAL_BAD_ORIENTATION || if (_status == Status::BAD_ORIENTATION ||
_status == COMPASS_CAL_BAD_RADIUS) { _status == Status::BAD_RADIUS) {
// don't overwrite bad orientation status // don't overwrite bad orientation status
return false; return false;
} }
FALLTHROUGH; FALLTHROUGH;
case COMPASS_CAL_BAD_ORIENTATION: case Status::BAD_ORIENTATION:
case COMPASS_CAL_BAD_RADIUS: case Status::BAD_RADIUS:
if (_status == COMPASS_CAL_NOT_STARTED) { if (_status == Status::NOT_STARTED) {
return false; return false;
} }
if (_retry && set_status(COMPASS_CAL_WAITING_TO_START)) { if (_retry && set_status(Status::WAITING_TO_START)) {
_attempt++; _attempt++;
return true; return true;
} }
@ -891,7 +892,7 @@ bool CompassCalibrator::calculate_orientation(void)
} }
if (!pass) { if (!pass) {
set_status(COMPASS_CAL_BAD_ORIENTATION); set_status(Status::BAD_ORIENTATION);
return false; return false;
} }
@ -904,7 +905,7 @@ bool CompassCalibrator::calculate_orientation(void)
// we won't change the orientation, but we set _orientation // we won't change the orientation, but we set _orientation
// for reporting purposes // for reporting purposes
_orientation = besti; _orientation = besti;
set_status(COMPASS_CAL_BAD_ORIENTATION); set_status(Status::BAD_ORIENTATION);
return false; return false;
} }
@ -961,7 +962,7 @@ bool CompassCalibrator::fix_radius(void)
_compass_idx, _compass_idx,
_params.radius, _params.radius,
expected_radius); expected_radius);
set_status(COMPASS_CAL_BAD_RADIUS); set_status(Status::BAD_RADIUS);
return false; return false;
} }

30
libraries/AP_Compass/CompassCalibrator.h
View File

@ -7,18 +7,6 @@
#define COMPASS_CAL_NUM_SAMPLES 300 // number of samples required before fitting begins #define COMPASS_CAL_NUM_SAMPLES 300 // number of samples required before fitting begins
#define COMPASS_CAL_DEFAULT_TOLERANCE 5.0f // default RMS tolerance #define COMPASS_CAL_DEFAULT_TOLERANCE 5.0f // default RMS tolerance
// compass calibration states
enum compass_cal_status_t {
COMPASS_CAL_NOT_STARTED = 0,
COMPASS_CAL_WAITING_TO_START = 1,
COMPASS_CAL_RUNNING_STEP_ONE = 2,
COMPASS_CAL_RUNNING_STEP_TWO = 3,
COMPASS_CAL_SUCCESS = 4,
COMPASS_CAL_FAILED = 5,
COMPASS_CAL_BAD_ORIENTATION = 6,
COMPASS_CAL_BAD_RADIUS = 7,
};
#define COMPASS_MIN_SCALE_FACTOR 0.85 #define COMPASS_MIN_SCALE_FACTOR 0.85
#define COMPASS_MAX_SCALE_FACTOR 1.3 #define COMPASS_MAX_SCALE_FACTOR 1.3
@ -45,8 +33,20 @@ public:
// running is true if actively calculating offsets, diagonals or offdiagonals // running is true if actively calculating offsets, diagonals or offdiagonals
bool running() const; bool running() const;
// compass calibration states
enum class Status {
NOT_STARTED = 0,
WAITING_TO_START = 1,
RUNNING_STEP_ONE = 2,
RUNNING_STEP_TWO = 3,
SUCCESS = 4,
FAILED = 5,
BAD_ORIENTATION = 6,
BAD_RADIUS = 7,
};
// get status of calibrations progress // get status of calibrations progress
enum compass_cal_status_t get_status() const { return _status; } Status get_status() const { return _status; }
// get calibration outputs (offsets, diagonals, offdiagonals) and fitness // get calibration outputs (offsets, diagonals, offdiagonals) and fitness
void get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals, float &scale_factor); void get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals, float &scale_factor);
@ -111,7 +111,7 @@ private:
}; };
// set status including any required initialisation // set status including any required initialisation
bool set_status(compass_cal_status_t status); bool set_status(Status status);
// returns true if sample should be added to buffer // returns true if sample should be added to buffer
bool accept_sample(const Vector3f &sample, uint16_t skip_index = UINT16_MAX); bool accept_sample(const Vector3f &sample, uint16_t skip_index = UINT16_MAX);
@ -164,7 +164,7 @@ private:
bool fix_radius(); bool fix_radius();
uint8_t _compass_idx; // index of the compass providing data uint8_t _compass_idx; // index of the compass providing data
enum compass_cal_status_t _status; // current state of calibrator Status _status; // current state of calibrator
uint32_t _last_sample_ms; // system time of last sample received for timeout uint32_t _last_sample_ms; // system time of last sample received for timeout
// values provided by caller // values provided by caller