diff --git a/libraries/AP_AccelCal/AP_AccelCal.cpp b/libraries/AP_AccelCal/AP_AccelCal.cpp index ebab633157..f75d6e94c9 100644 --- a/libraries/AP_AccelCal/AP_AccelCal.cpp +++ b/libraries/AP_AccelCal/AP_AccelCal.cpp @@ -22,6 +22,9 @@ const extern AP_HAL::HAL& hal; static bool _start_collect_sample; static void _snoop(const mavlink_message_t* msg); +uint8_t AP_AccelCal::_num_clients = 0; +AP_AccelCal_Client* AP_AccelCal::_clients[AP_ACCELCAL_MAX_NUM_CLIENTS] {}; + void AP_AccelCal::update() { if (!get_calibrator(0)) { @@ -245,13 +248,10 @@ void AP_AccelCal::collect_sample() } void AP_AccelCal::register_client(AP_AccelCal_Client* client) { - if (client == NULL || _num_clients == AP_ACCELCAL_MAX_NUM_CLIENTS) { + if (client == NULL || _num_clients >= AP_ACCELCAL_MAX_NUM_CLIENTS) { return; } - if (_started) { - fail(); - } for(uint8_t i=0; i<_num_clients; i++) { if(_clients[i] == client) { diff --git a/libraries/AP_AccelCal/AP_AccelCal.h b/libraries/AP_AccelCal/AP_AccelCal.h index 6ace5761e1..192ec52441 100644 --- a/libraries/AP_AccelCal/AP_AccelCal.h +++ b/libraries/AP_AccelCal/AP_AccelCal.h @@ -11,7 +11,6 @@ class AP_AccelCal_Client; class AP_AccelCal { public: AP_AccelCal(): - _num_clients(0), _started(false), _saving(false) { update_status(); } @@ -25,18 +24,19 @@ public: // Run an iteration of all registered calibrations void update(); - // interface to the clients for registration - void register_client(AP_AccelCal_Client* client); - // get the status of the calibrator server as a whole accel_cal_status_t get_status() { return _status; } + // interface to the clients for registration + static void register_client(AP_AccelCal_Client* client); + private: GCS_MAVLINK *_gcs; uint8_t _step; accel_cal_status_t _status; - uint8_t _num_clients; - AP_AccelCal_Client* _clients[AP_ACCELCAL_MAX_NUM_CLIENTS]; + + static uint8_t _num_clients; + static AP_AccelCal_Client* _clients[AP_ACCELCAL_MAX_NUM_CLIENTS]; // called on calibration success void success();