diff --git a/libraries/AP_Compass/AP_Compass_Calibration.cpp b/libraries/AP_Compass/AP_Compass_Calibration.cpp index fcd7f318ce..f8ceee1971 100644 --- a/libraries/AP_Compass/AP_Compass_Calibration.cpp +++ b/libraries/AP_Compass/AP_Compass_Calibration.cpp @@ -31,7 +31,7 @@ void Compass::cal_update() if (_calibrator[i].running()) { 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); } } @@ -119,7 +119,7 @@ void Compass::_cancel_calibration(uint8_t i) { 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; } _cal_saved[i] = false; @@ -143,11 +143,11 @@ void Compass::cancel_calibration_all() bool Compass::_accept_calibration(uint8_t 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; - } else if (cal_status == COMPASS_CAL_SUCCESS) { + } else if (cal_status == CompassCalibrator::Status::SUCCESS) { _cal_complete_requires_reboot = true; _cal_saved[i] = true; @@ -202,11 +202,11 @@ bool Compass::send_mag_cal_progress(const GCS_MAVLINK& link) } 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 || - cal_status == COMPASS_CAL_RUNNING_STEP_ONE || - cal_status == COMPASS_CAL_RUNNING_STEP_TWO) { + if (cal_status == CompassCalibrator::Status::WAITING_TO_START || + cal_status == CompassCalibrator::Status::RUNNING_STEP_ONE || + cal_status == CompassCalibrator::Status::RUNNING_STEP_TWO) { uint8_t completion_pct = calibrator.get_completion_percent(); const CompassCalibrator::completion_mask_t& completion_mask = calibrator.get_completion_mask(); const Vector3f direction; @@ -215,7 +215,7 @@ bool Compass::send_mag_cal_progress(const GCS_MAVLINK& link) mavlink_msg_mag_cal_progress_send( link.get_chan(), 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 ); } @@ -237,10 +237,10 @@ bool Compass::send_mag_cal_report(const GCS_MAVLINK& link) return true; } - uint8_t cal_status = _calibrator[compass_id].get_status(); - if (cal_status == COMPASS_CAL_SUCCESS || - cal_status == COMPASS_CAL_FAILED || - cal_status == COMPASS_CAL_BAD_ORIENTATION) { + const CompassCalibrator::Status cal_status = _calibrator[compass_id].get_status(); + if (cal_status == CompassCalibrator::Status::SUCCESS || + cal_status == CompassCalibrator::Status::FAILED || + cal_status == CompassCalibrator::Status::BAD_ORIENTATION) { float fitness = _calibrator[compass_id].get_fitness(); Vector3f ofs, diag, offdiag; float scale_factor; @@ -250,7 +250,7 @@ bool Compass::send_mag_cal_report(const GCS_MAVLINK& link) mavlink_msg_mag_cal_report_send( link.get_chan(), compass_id, cal_mask, - cal_status, autosaved, + (uint8_t)cal_status, autosaved, fitness, ofs.x, ofs.y, ofs.z, diag.x, diag.y, diag.z, @@ -269,10 +269,10 @@ bool Compass::is_calibrating() const { for (uint8_t i=0; i 1000) { _retry = false; - set_status(COMPASS_CAL_FAILED); + set_status(Status::FAILED); return true; } return false; @@ -183,8 +184,8 @@ void CompassCalibrator::new_sample(const Vector3f& sample) { _last_sample_ms = AP_HAL::millis(); - if (_status == COMPASS_CAL_WAITING_TO_START) { - set_status(COMPASS_CAL_RUNNING_STEP_ONE); + if (_status == Status::WAITING_TO_START) { + set_status(Status::RUNNING_STEP_ONE); } if (running() && _samples_collected < COMPASS_CAL_NUM_SAMPLES && accept_sample(sample)) { @@ -204,13 +205,13 @@ void CompassCalibrator::update(bool &failure) return; } - if (_status == COMPASS_CAL_RUNNING_STEP_ONE) { + if (_status == Status::RUNNING_STEP_ONE) { if (_fit_step >= 10) { 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; } else { - set_status(COMPASS_CAL_RUNNING_STEP_TWO); + set_status(Status::RUNNING_STEP_TWO); } } else { if (_fit_step == 0) { @@ -219,12 +220,12 @@ void CompassCalibrator::update(bool &failure) run_sphere_fit(); _fit_step++; } - } else if (_status == COMPASS_CAL_RUNNING_STEP_TWO) { + } else if (_status == Status::RUNNING_STEP_TWO) { if (_fit_step >= 35) { if (fit_acceptable() && fix_radius() && calculate_orientation()) { - set_status(COMPASS_CAL_SUCCESS); + set_status(Status::SUCCESS); } else { - set_status(COMPASS_CAL_FAILED); + set_status(Status::FAILED); failure = true; } } else if (_fit_step < 15) { @@ -242,7 +243,7 @@ void CompassCalibrator::update(bool &failure) ///////////////////////////////////////////////////////////// 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 @@ -278,30 +279,30 @@ void CompassCalibrator::reset_state() 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; } switch (status) { - case COMPASS_CAL_NOT_STARTED: + case Status::NOT_STARTED: reset_state(); - _status = COMPASS_CAL_NOT_STARTED; + _status = Status::NOT_STARTED; if (_sample_buffer != nullptr) { free(_sample_buffer); _sample_buffer = nullptr; } return true; - case COMPASS_CAL_WAITING_TO_START: + case Status::WAITING_TO_START: reset_state(); - _status = COMPASS_CAL_WAITING_TO_START; - set_status(COMPASS_CAL_RUNNING_STEP_ONE); + _status = Status::WAITING_TO_START; + set_status(Status::RUNNING_STEP_ONE); return true; - case COMPASS_CAL_RUNNING_STEP_ONE: - if (_status != COMPASS_CAL_WAITING_TO_START) { + case Status::RUNNING_STEP_ONE: + if (_status != Status::WAITING_TO_START) { return false; } @@ -315,22 +316,22 @@ bool CompassCalibrator::set_status(compass_cal_status_t status) } if (_sample_buffer != nullptr) { initialize_fit(); - _status = COMPASS_CAL_RUNNING_STEP_ONE; + _status = Status::RUNNING_STEP_ONE; return true; } return false; - case COMPASS_CAL_RUNNING_STEP_TWO: - if (_status != COMPASS_CAL_RUNNING_STEP_ONE) { + case Status::RUNNING_STEP_TWO: + if (_status != Status::RUNNING_STEP_ONE) { return false; } thin_samples(); initialize_fit(); - _status = COMPASS_CAL_RUNNING_STEP_TWO; + _status = Status::RUNNING_STEP_TWO; return true; - case COMPASS_CAL_SUCCESS: - if (_status != COMPASS_CAL_RUNNING_STEP_TWO) { + case Status::SUCCESS: + if (_status != Status::RUNNING_STEP_TWO) { return false; } @@ -339,24 +340,24 @@ bool CompassCalibrator::set_status(compass_cal_status_t status) _sample_buffer = nullptr; } - _status = COMPASS_CAL_SUCCESS; + _status = Status::SUCCESS; return true; - case COMPASS_CAL_FAILED: - if (_status == COMPASS_CAL_BAD_ORIENTATION || - _status == COMPASS_CAL_BAD_RADIUS) { + case Status::FAILED: + if (_status == Status::BAD_ORIENTATION || + _status == Status::BAD_RADIUS) { // don't overwrite bad orientation status return false; } FALLTHROUGH; - - case COMPASS_CAL_BAD_ORIENTATION: - case COMPASS_CAL_BAD_RADIUS: - if (_status == COMPASS_CAL_NOT_STARTED) { + + case Status::BAD_ORIENTATION: + case Status::BAD_RADIUS: + if (_status == Status::NOT_STARTED) { return false; } - if (_retry && set_status(COMPASS_CAL_WAITING_TO_START)) { + if (_retry && set_status(Status::WAITING_TO_START)) { _attempt++; return true; } @@ -891,7 +892,7 @@ bool CompassCalibrator::calculate_orientation(void) } if (!pass) { - set_status(COMPASS_CAL_BAD_ORIENTATION); + set_status(Status::BAD_ORIENTATION); return false; } @@ -904,7 +905,7 @@ bool CompassCalibrator::calculate_orientation(void) // we won't change the orientation, but we set _orientation // for reporting purposes _orientation = besti; - set_status(COMPASS_CAL_BAD_ORIENTATION); + set_status(Status::BAD_ORIENTATION); return false; } @@ -961,7 +962,7 @@ bool CompassCalibrator::fix_radius(void) _compass_idx, _params.radius, expected_radius); - set_status(COMPASS_CAL_BAD_RADIUS); + set_status(Status::BAD_RADIUS); return false; } diff --git a/libraries/AP_Compass/CompassCalibrator.h b/libraries/AP_Compass/CompassCalibrator.h index cd95dc1cf9..4b795a6c24 100644 --- a/libraries/AP_Compass/CompassCalibrator.h +++ b/libraries/AP_Compass/CompassCalibrator.h @@ -7,18 +7,6 @@ #define COMPASS_CAL_NUM_SAMPLES 300 // number of samples required before fitting begins #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_MAX_SCALE_FACTOR 1.3 @@ -45,8 +33,20 @@ public: // running is true if actively calculating offsets, diagonals or offdiagonals 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 - enum compass_cal_status_t get_status() const { return _status; } + Status get_status() const { return _status; } // get calibration outputs (offsets, diagonals, offdiagonals) and fitness void get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals, float &scale_factor); @@ -111,7 +111,7 @@ private: }; // 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 bool accept_sample(const Vector3f &sample, uint16_t skip_index = UINT16_MAX); @@ -164,7 +164,7 @@ private: bool fix_radius(); 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 // values provided by caller