Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include <AP_HAL/AP_HAL_Boards.h>
       4             : #include <GCS_MAVLink/GCS_config.h>
       5             : 
       6             : #ifndef HAL_INS_ACCELCAL_ENABLED
       7             : #if HAL_GCS_ENABLED
       8             : #include <AP_InertialSensor/AP_InertialSensor_config.h>
       9             : #define HAL_INS_ACCELCAL_ENABLED AP_INERTIALSENSOR_ENABLED
      10             : #else
      11             : #define HAL_INS_ACCELCAL_ENABLED 0
      12             : #endif
      13             : #endif
      14             : 
      15             : #include <GCS_MAVLink/GCS_MAVLink.h>
      16             : #include "AccelCalibrator.h"
      17             : 
      18             : #define AP_ACCELCAL_MAX_NUM_CLIENTS 4
      19             : class GCS_MAVLINK;
      20             : class AP_AccelCal_Client;
      21             : 
      22             : class AP_AccelCal {
      23             : public:
      24           2 :     AP_AccelCal():
      25           2 :     _use_gcs_snoop(true),
      26           2 :     _started(false),
      27           2 :     _saving(false)
      28           2 :     { update_status(); }
      29             : 
      30             :     // start all the registered calibrations
      31             :     void start(GCS_MAVLINK *gcs, uint8_t sysid, uint8_t compid);
      32             : 
      33             :     // called on calibration cancellation
      34             :     void cancel();
      35             : 
      36             :     // Run an iteration of all registered calibrations
      37             :     void update();
      38             : 
      39             :     // get the status of the calibrator server as a whole
      40           0 :     accel_cal_status_t get_status() { return _status; }
      41             :     
      42             :     // Set vehicle position sent by the GCS
      43             :     bool gcs_vehicle_position(float position);
      44             : 
      45             :     // interface to the clients for registration
      46             :     static void register_client(AP_AccelCal_Client* client);
      47             : 
      48             : #if HAL_GCS_ENABLED
      49             :     void handle_command_ack(const mavlink_command_ack_t &packet, uint8_t src_sysid, uint8_t src_compid);
      50             : #endif
      51             : 
      52             :     // true if we are in a calibration process
      53             :     bool running(void) const;
      54             : 
      55             : private:
      56             :     class GCS_MAVLINK *_gcs;
      57             :     uint8_t _sysid;
      58             :     uint8_t _compid;
      59             :     bool _use_gcs_snoop;
      60             :     bool _waiting_for_mavlink_ack = false;
      61             :     uint32_t _last_position_request_ms;
      62             :     uint8_t _step;
      63             :     accel_cal_status_t _status;
      64             :     accel_cal_status_t _last_result;
      65             : 
      66             :     static uint8_t _num_clients;
      67             :     static AP_AccelCal_Client* _clients[AP_ACCELCAL_MAX_NUM_CLIENTS];
      68             : 
      69             :     // called on calibration success
      70             :     void success();
      71             : 
      72             :     // called on calibration failure
      73             :     void fail();
      74             : 
      75             :     // reset all the calibrators to there pre calibration stage so as to make them ready for next calibration request
      76             :     void clear();
      77             : 
      78             :     // proceed through the collection step for each of the registered calibrators
      79             :     void collect_sample();
      80             : 
      81             :     // update the state of the Accel calibrator server
      82             :     void update_status();
      83             : 
      84             :     // checks if no new sample has been received for considerable amount of time
      85             :     bool check_for_timeout();
      86             : 
      87             :     // check if client's calibrator is active
      88             :     bool client_active(uint8_t client_num);
      89             : 
      90             :     bool _started;
      91             :     bool _saving;
      92             : 
      93             :     uint8_t _num_active_calibrators;
      94             : 
      95             :     AccelCalibrator* get_calibrator(uint8_t i);
      96             : };
      97             : 
      98             : class AP_AccelCal_Client {
      99             : friend class AP_AccelCal;
     100             : private:
     101             :     // getters
     102           1 :     virtual bool _acal_get_saving() { return false; }
     103           6 :     virtual bool _acal_get_ready_to_sample() { return true; }
     104           1 :     virtual bool _acal_get_fail() { return false; }
     105             :     virtual AccelCalibrator* _acal_get_calibrator(uint8_t instance) = 0;
     106             : 
     107             :     // events
     108             :     virtual void _acal_save_calibrations() = 0;
     109           1 :     virtual void _acal_event_success() {};
     110           0 :     virtual void _acal_event_cancellation() {};
     111           0 :     virtual void _acal_event_failure() {};
     112             : };