Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             :    This program is distributed in the hope that it will be useful,
       7             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       8             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
       9             :    GNU General Public License for more details.
      10             :    You should have received a copy of the GNU General Public License
      11             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      12             : */
      13             : 
      14             : #include "AP_AccelCal.h"
      15             : 
      16             : #if HAL_INS_ACCELCAL_ENABLED
      17             : 
      18             : #include <stdarg.h>
      19             : #include <AP_HAL/AP_HAL.h>
      20             : #include <GCS_MAVLink/GCS.h>
      21             : 
      22             : #define AP_ACCELCAL_POSITION_REQUEST_INTERVAL_MS 1000
      23             : 
      24             : #define _printf(fmt, args ...) do {                                     \
      25             :         if (_gcs != nullptr) {                                          \
      26             :             _gcs->send_text(MAV_SEVERITY_CRITICAL, fmt, ## args);       \
      27             :         }                                                               \
      28             :     } while (0)
      29             : 
      30             : 
      31             : const extern AP_HAL::HAL& hal;
      32             : static bool _start_collect_sample;
      33             : 
      34             : uint8_t AP_AccelCal::_num_clients = 0;
      35             : AP_AccelCal_Client* AP_AccelCal::_clients[AP_ACCELCAL_MAX_NUM_CLIENTS] {};
      36             : 
      37         691 : void AP_AccelCal::update()
      38             : {
      39         691 :     if (!get_calibrator(0)) {
      40             :         // no calibrators
      41           0 :         return;
      42             :     }
      43             : 
      44         691 :     if (_started) {
      45         468 :         update_status();
      46             : 
      47         468 :         uint8_t num_active_calibrators = 0;
      48        1404 :         for(uint8_t i=0; get_calibrator(i) != nullptr; i++) {
      49         936 :             num_active_calibrators++;
      50             :         }
      51         468 :         if (num_active_calibrators != _num_active_calibrators) {
      52           0 :             fail();
      53           0 :             return;
      54             :         }
      55         468 :         if(_start_collect_sample) {
      56           6 :             collect_sample();
      57             :         }
      58             :         AccelCalibrator *cal;
      59         468 :         switch(_status) {
      60           0 :             case ACCEL_CAL_NOT_STARTED:
      61           0 :                 fail();
      62           0 :                 return;
      63         430 :             case ACCEL_CAL_WAITING_FOR_ORIENTATION: {
      64             :                 // if we're waiting for orientation, first ensure that all calibrators are on the same step
      65             :                 uint8_t step;
      66         430 :                 if ((cal = get_calibrator(0)) == nullptr) {
      67           0 :                     fail();
      68           0 :                     return;
      69             :                 }
      70         430 :                 step = cal->get_num_samples_collected()+1;
      71             : 
      72         860 :                 for(uint8_t i=1 ; (cal = get_calibrator(i))  ; i++) {
      73         430 :                     if (step != cal->get_num_samples_collected()+1) {
      74           0 :                         fail();
      75           0 :                         return;
      76             :                     }
      77             :                 }
      78             :                 // if we're on a new step, print a message describing the step
      79         430 :                 if (step != _step) {
      80           7 :                     _step = step;
      81             : 
      82           7 :                     if(_use_gcs_snoop) {
      83             :                         const char *msg;
      84           7 :                         switch (step) {
      85           2 :                             case ACCELCAL_VEHICLE_POS_LEVEL:
      86           2 :                                 msg = "level";
      87           2 :                                 break;
      88           1 :                             case ACCELCAL_VEHICLE_POS_LEFT:
      89           1 :                                 msg = "on its LEFT side";
      90           1 :                                 break;
      91           1 :                             case ACCELCAL_VEHICLE_POS_RIGHT:
      92           1 :                                 msg = "on its RIGHT side";
      93           1 :                                 break;
      94           1 :                             case ACCELCAL_VEHICLE_POS_NOSEDOWN:
      95           1 :                                 msg = "nose DOWN";
      96           1 :                                 break;
      97           1 :                             case ACCELCAL_VEHICLE_POS_NOSEUP:
      98           1 :                                 msg = "nose UP";
      99           1 :                                 break;
     100           1 :                             case ACCELCAL_VEHICLE_POS_BACK:
     101           1 :                                 msg = "on its BACK";
     102           1 :                                 break;
     103           0 :                             default:
     104           0 :                                 fail();
     105           0 :                                 return;
     106             :                         }
     107           7 :                         _printf("Place vehicle %s and press any key.", msg);
     108           7 :                         _waiting_for_mavlink_ack = true;
     109             :                     }
     110             :                 }
     111             : 
     112         430 :                 uint32_t now = AP_HAL::millis();
     113         430 :                 if (now - _last_position_request_ms > AP_ACCELCAL_POSITION_REQUEST_INTERVAL_MS) {
     114          43 :                     _last_position_request_ms = now;
     115          43 :                     _gcs->send_accelcal_vehicle_position(step);
     116             :                 }
     117         430 :                 break;
     118             :             }
     119          36 :             case ACCEL_CAL_COLLECTING_SAMPLE:
     120             :                 // check for timeout
     121             : 
     122         108 :                 for(uint8_t i=0; (cal = get_calibrator(i)); i++) {
     123          72 :                     cal->check_for_timeout();
     124             :                 }
     125             : 
     126          36 :                 update_status();
     127             : 
     128          36 :                 if (_status == ACCEL_CAL_FAILED) {
     129           0 :                     fail();
     130             :                 }
     131          36 :                 return;
     132           2 :             case ACCEL_CAL_SUCCESS:
     133             :                 // save
     134           2 :                 if (_saving) {
     135           1 :                     bool done = true;
     136           2 :                     for(uint8_t i=0; i<_num_clients; i++) {
     137           1 :                         if (client_active(i) && _clients[i]->_acal_get_saving()) {
     138           0 :                             done = false;
     139           0 :                             break;
     140             :                         }
     141             :                     }
     142           1 :                     if (done) {
     143           1 :                         success();
     144             :                     }
     145           1 :                     return;
     146             :                 } else {
     147           2 :                     for(uint8_t i=0; i<_num_clients; i++) {
     148           1 :                         if(client_active(i) && _clients[i]->_acal_get_fail()) {
     149           0 :                             fail();
     150           0 :                             return;
     151             :                         }
     152             :                     }
     153           2 :                     for(uint8_t i=0; i<_num_clients; i++) {
     154           1 :                         if(client_active(i)) {
     155           1 :                             _clients[i]->_acal_save_calibrations();
     156             :                         }
     157             :                     }
     158           1 :                     _saving = true;
     159             :                 }
     160           1 :                 return;
     161           0 :             default:
     162             :             case ACCEL_CAL_FAILED:
     163           0 :                 fail();
     164           0 :                 return;
     165             :         }
     166         223 :     } else if (_last_result != ACCEL_CAL_NOT_STARTED) {
     167             :         // only continuously report if we have ever completed a calibration
     168         223 :         uint32_t now = AP_HAL::millis();
     169         223 :         if (now - _last_position_request_ms > AP_ACCELCAL_POSITION_REQUEST_INTERVAL_MS) {
     170          22 :             _last_position_request_ms = now;
     171          22 :             switch (_last_result) {
     172          22 :                 case ACCEL_CAL_SUCCESS:
     173          22 :                     _gcs->send_accelcal_vehicle_position(ACCELCAL_VEHICLE_POS_SUCCESS);
     174          22 :                     break;
     175           0 :                 case ACCEL_CAL_FAILED:
     176           0 :                     _gcs->send_accelcal_vehicle_position(ACCELCAL_VEHICLE_POS_FAILED);
     177           0 :                     break;
     178           0 :                 default:
     179             :                     // should never hit this state
     180           0 :                     break;
     181             :             }
     182             :         }
     183             :     }
     184             : }
     185             : 
     186           2 : void AP_AccelCal::start(GCS_MAVLINK *gcs, uint8_t sysid, uint8_t compid)
     187             : {
     188           2 :     if (gcs == nullptr || _started) {
     189           0 :         return;
     190             :     }
     191           2 :     _start_collect_sample = false;
     192           2 :     _num_active_calibrators = 0;
     193             : 
     194             :     AccelCalibrator *cal;
     195           6 :     for(uint8_t i=0; (cal = get_calibrator(i)); i++) {
     196           4 :         cal->clear();
     197           4 :         cal->start(ACCEL_CAL_AXIS_ALIGNED_ELLIPSOID, 6, 0.5f);
     198           4 :         _num_active_calibrators++;
     199             :     }
     200             : 
     201           2 :     _started = true;
     202           2 :     _saving = false;
     203           2 :     _gcs = gcs;
     204           2 :     _sysid = sysid;
     205           2 :     _compid = compid;
     206           2 :     _use_gcs_snoop = true;
     207           2 :     _last_position_request_ms = 0;
     208           2 :     _step = 0;
     209             : 
     210           2 :     _last_result = ACCEL_CAL_NOT_STARTED;
     211             : 
     212           2 :     update_status();
     213             : }
     214             : 
     215           1 : void AP_AccelCal::success()
     216             : {
     217           1 :     _printf("Calibration successful");
     218             : 
     219           2 :     for(uint8_t i=0 ; i < _num_clients ; i++) {
     220           1 :         _clients[i]->_acal_event_success();
     221             :     }
     222             : 
     223           1 :     _last_result = ACCEL_CAL_SUCCESS;
     224             : 
     225           1 :     clear();
     226           1 : }
     227             : 
     228           0 : void AP_AccelCal::cancel()
     229             : {
     230           0 :     _printf("Calibration cancelled");
     231             : 
     232           0 :     for(uint8_t i=0 ; i < _num_clients ; i++) {
     233           0 :         _clients[i]->_acal_event_cancellation();
     234             :     }
     235             : 
     236           0 :     _last_result = ACCEL_CAL_NOT_STARTED;
     237             : 
     238           0 :     clear();
     239           0 : }
     240             : 
     241           0 : void AP_AccelCal::fail()
     242             : {
     243           0 :     _printf("Calibration FAILED");
     244             : 
     245           0 :     for(uint8_t i=0 ; i < _num_clients ; i++) {
     246           0 :         _clients[i]->_acal_event_failure();
     247             :     }
     248             : 
     249           0 :     _last_result = ACCEL_CAL_FAILED;
     250             : 
     251           0 :     clear();
     252           0 : }
     253             : 
     254           1 : void AP_AccelCal::clear()
     255             : {
     256           1 :     if (!_started) {
     257           0 :         return;
     258             :     }
     259             : 
     260             :     AccelCalibrator *cal;
     261           3 :     for(uint8_t i=0 ; (cal = get_calibrator(i))  ; i++) {
     262           2 :         cal->clear();
     263             :     }
     264             : 
     265           1 :     _step = 0;
     266           1 :     _started = false;
     267           1 :     _saving = false;
     268             : 
     269           1 :     update_status();
     270             : }
     271             : 
     272           6 : void AP_AccelCal::collect_sample()
     273             : {
     274           6 :     if (_status != ACCEL_CAL_WAITING_FOR_ORIENTATION) {
     275           0 :         return;
     276             :     }
     277             : 
     278          12 :     for(uint8_t i=0; i<_num_clients; i++) {
     279           6 :         if (client_active(i) && !_clients[i]->_acal_get_ready_to_sample()) {
     280           0 :             _printf("Not ready to sample");
     281           0 :             return;
     282             :         }
     283             :     }
     284             : 
     285             :     AccelCalibrator *cal;
     286          18 :     for(uint8_t i=0 ; (cal = get_calibrator(i))  ; i++) {
     287          12 :         cal->collect_sample();
     288             :     }
     289           6 :     _start_collect_sample = false;
     290           6 :     update_status();
     291             : }
     292             : 
     293        2099 : void AP_AccelCal::register_client(AP_AccelCal_Client* client) {
     294        2099 :     if (client == nullptr || _num_clients >= AP_ACCELCAL_MAX_NUM_CLIENTS) {
     295           0 :         return;
     296             :     }
     297             : 
     298             : 
     299        2100 :     for(uint8_t i=0; i<_num_clients; i++) {
     300           1 :         if(_clients[i] == client) {
     301           0 :             return;
     302             :         }
     303             :     }
     304        2099 :     _clients[_num_clients] = client;
     305        2099 :     _num_clients++;
     306             : }
     307             : 
     308        7425 : AccelCalibrator* AP_AccelCal::get_calibrator(uint8_t index) {
     309             :     AccelCalibrator* ret;
     310        9329 :     for(uint8_t i=0; i<_num_clients; i++) {
     311       13133 :         for(uint8_t j=0 ; (ret = _clients[i]->_acal_get_calibrator(j)) ; j++) {
     312       11229 :             if (index == 0) {
     313        5521 :                 return ret;
     314             :             }
     315        5708 :             index--;
     316             :         }
     317             :     }
     318        1904 :     return nullptr;
     319             : }
     320             : 
     321         515 : void AP_AccelCal::update_status() {
     322             :     AccelCalibrator *cal;
     323             : 
     324         515 :     if (!get_calibrator(0)) {
     325             :         // no calibrators
     326           2 :         _status = ACCEL_CAL_NOT_STARTED;
     327           2 :         return;
     328             :     }
     329             : 
     330        1539 :     for(uint8_t i=0 ; (cal = get_calibrator(i))  ; i++) {
     331        1026 :         if (cal->get_status() == ACCEL_CAL_FAILED) {
     332           0 :             _status = ACCEL_CAL_FAILED;         //fail if even one of the calibration has
     333           0 :             return;
     334             :         }
     335             :     }
     336             : 
     337        1397 :     for(uint8_t i=0 ; (cal = get_calibrator(i))  ; i++) {
     338         956 :         if (cal->get_status() == ACCEL_CAL_COLLECTING_SAMPLE) {
     339          72 :             _status = ACCEL_CAL_COLLECTING_SAMPLE;          // move to Collecting sample state if all the callibrators have
     340          72 :             return;
     341             :         }
     342             :     }
     343             : 
     344         447 :     for(uint8_t i=0 ; (cal = get_calibrator(i))  ; i++) {
     345         444 :         if (cal->get_status() == ACCEL_CAL_WAITING_FOR_ORIENTATION) {
     346         438 :             _status = ACCEL_CAL_WAITING_FOR_ORIENTATION;    // move to waiting for user ack for orientation confirmation
     347         438 :             return;
     348             :         }
     349             :     }
     350             : 
     351           7 :     for(uint8_t i=0 ; (cal = get_calibrator(i))  ; i++) {
     352           5 :         if (cal->get_status() == ACCEL_CAL_NOT_STARTED) {
     353           1 :             _status = ACCEL_CAL_NOT_STARTED;    // we haven't started if all the calibrators haven't
     354           1 :             return;
     355             :         }
     356             :     }
     357             : 
     358           2 :     _status = ACCEL_CAL_SUCCESS;    // we have succeeded calibration if all the calibrators have
     359             : }
     360             : 
     361           9 : bool AP_AccelCal::client_active(uint8_t client_num)
     362             : {
     363           9 :     return (bool)_clients[client_num]->_acal_get_calibrator(0);
     364             : }
     365             : 
     366             : #if HAL_GCS_ENABLED
     367           6 : void AP_AccelCal::handle_command_ack(const mavlink_command_ack_t &packet, uint8_t src_sysid, uint8_t src_compid)
     368             : {
     369           6 :     if(_sysid != src_sysid || _compid != src_compid) {
     370           0 :         return;
     371             :     }
     372             : 
     373           6 :     if (!_waiting_for_mavlink_ack) {
     374           0 :         return;
     375             :     }
     376             :     // this is support for the old, non-accelcal-specific calibration.
     377             :     // The GCS is expected to send back a COMMAND_ACK when the vehicle
     378             :     // is posed, but we placed no constraints on the result code or
     379             :     // the command field in the ack packet.  That meant that any ACK
     380             :     // would move the cal process forward - and since we don't even
     381             :     // check the source system/component here the process could easily
     382             :     // fail due to other ACKs floating around the mavlink network.
     383             :     // GCSs should be moved to using the non-gcs-snoop method.  As a
     384             :     // round-up:
     385             :     // MAVProxy: command=1-6 depending on pose, result=1
     386             :     // QGC: command=0, result=1
     387             :     // MissionPlanner: uses new ACCELCAL_VEHICLE_POS
     388           6 :     if (packet.command > 6) {
     389             :         // not an acknowledgement for a vehicle position
     390           0 :         return;
     391             :     }
     392           6 :     if (packet.result != MAV_RESULT_TEMPORARILY_REJECTED) {
     393             :         // not an acknowledgement for a vehicle position
     394           0 :         return;
     395             :     }
     396           6 :     _waiting_for_mavlink_ack = false;
     397           6 :     _start_collect_sample = true;
     398             : }
     399             : 
     400           0 : bool AP_AccelCal::gcs_vehicle_position(float position)
     401             : {
     402           0 :     _use_gcs_snoop = false;
     403             : 
     404           0 :     if (_status == ACCEL_CAL_WAITING_FOR_ORIENTATION && is_equal((float) _step, position)) {
     405           0 :         _start_collect_sample = true;
     406           0 :         return true;
     407             :     }
     408             : 
     409           0 :     return false;
     410             : }
     411             : #endif
     412             : 
     413             : // true if we are in a calibration process
     414      125549 : bool AP_AccelCal::running(void) const
     415             : {
     416      125549 :     return _status == ACCEL_CAL_WAITING_FOR_ORIENTATION || _status == ACCEL_CAL_COLLECTING_SAMPLE;
     417             : }
     418             : #endif //HAL_INS_ACCELCAL_ENABLED