Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include "AP_Mount_config.h"
       4             : 
       5             : #if HAL_SOLO_GIMBAL_ENABLED
       6             : 
       7             : #include <AP_Math/AP_Math.h>
       8             : #include <AP_Common/AP_Common.h>
       9             : #include <GCS_MAVLink/GCS_MAVLink.h>
      10             : 
      11             : enum gmb_param_state_t {
      12             :     GMB_PARAMSTATE_NOT_YET_READ=0, // parameter has yet to be initialized
      13             :     GMB_PARAMSTATE_FETCH_AGAIN=1, // parameter is being fetched
      14             :     GMB_PARAMSTATE_ATTEMPTING_TO_SET=2, // parameter is being set
      15             :     GMB_PARAMSTATE_CONSISTENT=3, // parameter is consistent
      16             :     GMB_PARAMSTATE_NONEXISTANT=4 // parameter does not seem to exist
      17             : };
      18             : 
      19             : enum gmb_param_t {
      20             :     GMB_PARAM_GMB_OFF_ACC_X=0,
      21             :     GMB_PARAM_GMB_OFF_ACC_Y,
      22             :     GMB_PARAM_GMB_OFF_ACC_Z,
      23             :     GMB_PARAM_GMB_GN_ACC_X,
      24             :     GMB_PARAM_GMB_GN_ACC_Y,
      25             :     GMB_PARAM_GMB_GN_ACC_Z,
      26             :     GMB_PARAM_GMB_OFF_GYRO_X,
      27             :     GMB_PARAM_GMB_OFF_GYRO_Y,
      28             :     GMB_PARAM_GMB_OFF_GYRO_Z,
      29             :     GMB_PARAM_GMB_OFF_JNT_X,
      30             :     GMB_PARAM_GMB_OFF_JNT_Y,
      31             :     GMB_PARAM_GMB_OFF_JNT_Z,
      32             :     GMB_PARAM_GMB_K_RATE,
      33             :     GMB_PARAM_GMB_POS_HOLD,
      34             :     GMB_PARAM_GMB_MAX_TORQUE,
      35             :     GMB_PARAM_GMB_SND_TORQUE,
      36             :     GMB_PARAM_GMB_SYSID,
      37             :     GMB_PARAM_GMB_FLASH,
      38             :     MAVLINK_GIMBAL_NUM_TRACKED_PARAMS
      39             : };
      40             : 
      41             : enum gmb_flashing_step_t {
      42             :     GMB_PARAM_NOT_FLASHING=0,
      43             :     GMB_PARAM_FLASHING_WAITING_FOR_SET,
      44             :     GMB_PARAM_FLASHING_WAITING_FOR_ACK
      45             : };
      46             : 
      47             : class SoloGimbal_Parameters
      48             : {
      49             : public:
      50             :     SoloGimbal_Parameters();
      51             :     void reset();
      52             : 
      53             :     bool initialized();
      54             :     bool received_all();
      55             :     void fetch_params();
      56             : 
      57             :     void get_param(gmb_param_t param, float& value, float def_val = 0.0f);
      58             :     void set_param(gmb_param_t param, float value);
      59             : 
      60             :     void update();
      61             :     void handle_param_value(const mavlink_message_t &msg);
      62             : 
      63             :     Vector3f get_accel_bias();
      64             :     Vector3f get_accel_gain();
      65             :     void set_accel_bias(const Vector3f& bias);
      66             :     void set_accel_gain(const Vector3f& gain);
      67             :     Vector3f get_gyro_bias();
      68             :     void set_gyro_bias(const Vector3f& bias);
      69             :     Vector3f get_joint_bias();
      70             : 
      71             :     float get_K_rate();
      72             :     void flash();
      73             :     bool flashing();
      74             : 
      75       18195 :     void set_channel(mavlink_channel_t chan) { _chan = chan; }
      76             : 
      77             : private:
      78             :     static const char* get_param_name(gmb_param_t param);
      79             : 
      80             :     static const uint32_t _retry_period;
      81             :     static const uint8_t _max_fetch_attempts;
      82             : 
      83             :     struct {
      84             :         float value;
      85             :         gmb_param_state_t state;
      86             :         uint8_t fetch_attempts;
      87             :         bool seen;
      88             :     } _params[MAVLINK_GIMBAL_NUM_TRACKED_PARAMS];
      89             : 
      90             :     uint32_t _last_request_ms;
      91             :     uint32_t _last_set_ms;
      92             :     gmb_flashing_step_t _flashing_step;
      93             : 
      94             :     mavlink_channel_t _chan;
      95             : };
      96             : 
      97             : #endif // HAL_SOLO_GIMBAL_ENABLED