AP_Compass: make compass_cal_status_t enum class Status within CompassCalibrator

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<COMPASS_MAX_INSTANCES; i++) {
         switch(_calibrator[i].get_status()) {
-            case COMPASS_CAL_NOT_STARTED:
-            case COMPASS_CAL_SUCCESS:
-            case COMPASS_CAL_FAILED:
-            case COMPASS_CAL_BAD_ORIENTATION:
+            case CompassCalibrator::Status::NOT_STARTED:
+            case CompassCalibrator::Status::SUCCESS:
+            case CompassCalibrator::Status::FAILED:
+            case CompassCalibrator::Status::BAD_ORIENTATION:
                 break;
             default:
                 return true;
@@ -285,7 +285,7 @@ uint8_t Compass::_get_cal_mask() const
 {
     uint8_t cal_mask = 0;
     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;
         }
     }

libraries/AP_Compass/CompassCalibrator.cpp

@@ -33,7 +33,7 @@
  * origin-centered sphere.
  *
  * 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
  * 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
@@ -82,7 +82,7 @@ CompassCalibrator::CompassCalibrator():
 
 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)
@@ -105,12 +105,12 @@ void CompassCalibrator::start(bool retry, float delay, uint16_t offset_max, uint
     _delay_start_sec = delay;
     _start_time_ms = AP_HAL::millis();
     _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)
 {
-    if (_status != COMPASS_CAL_SUCCESS) {
+    if (_status != Status::SUCCESS) {
         return;
     }
 
@@ -123,23 +123,24 @@ void CompassCalibrator::get_calibration(Vector3f &offsets, Vector3f &diagonals,
 float CompassCalibrator::get_completion_percent() const
 {
     // 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) {
-        case COMPASS_CAL_NOT_STARTED:
-        case COMPASS_CAL_WAITING_TO_START:
+        case Status::NOT_STARTED:
+        case Status::WAITING_TO_START:
             return 0.0f;
-        case COMPASS_CAL_RUNNING_STEP_ONE:
+        case Status::RUNNING_STEP_ONE:
             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));
-        case COMPASS_CAL_SUCCESS:
+        case Status::SUCCESS:
             return 100.0f;
-        case COMPASS_CAL_FAILED:
-        case COMPASS_CAL_BAD_ORIENTATION:
-        case COMPASS_CAL_BAD_RADIUS:
-        default:
+        case Status::FAILED:
+        case Status::BAD_ORIENTATION:
+        case Status::BAD_RADIUS:
             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
@@ -173,7 +174,7 @@ bool CompassCalibrator::check_for_timeout()
     uint32_t tnow = AP_HAL::millis();
     if (running() && tnow - _last_sample_ms > 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;
     }
 

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