Line data    Source code

       1             : /************************************************************
       2             : * SoloGimbal -- library to control a 3 axis rate gimbal.     *
       3             : *                                                           *
       4             : * Author:  Arthur Benemann, Paul Riseborough;               *
       5             : *                                                           *
       6             : ************************************************************/
       7             : #pragma once
       8             : 
       9             : #include "AP_Mount_config.h"
      10             : 
      11             : #if HAL_SOLO_GIMBAL_ENABLED
      12             : 
      13             : #include <AP_AccelCal/AP_AccelCal.h>
      14             : #include <AP_Common/AP_Common.h>
      15             : #include <AP_HAL/AP_HAL_Boards.h>
      16             : #include <AP_Math/AP_Math.h>
      17             : #include <GCS_MAVLink/GCS_MAVLink.h>
      18             : 
      19             : #include "SoloGimbal_Parameters.h"
      20             : #include "SoloGimbalEKF.h"
      21             : 
      22             : enum gimbal_state_t {
      23             :     GIMBAL_STATE_NOT_PRESENT = 0,
      24             :     GIMBAL_STATE_PRESENT_INITIALIZING,
      25             :     GIMBAL_STATE_PRESENT_ALIGNING,
      26             :     GIMBAL_STATE_PRESENT_RUNNING
      27             : };
      28             : 
      29             : enum gimbal_mode_t {
      30             :     GIMBAL_MODE_IDLE = 0,
      31             :     GIMBAL_MODE_POS_HOLD,
      32             :     GIMBAL_MODE_POS_HOLD_FF,
      33             :     GIMBAL_MODE_STABILIZE
      34             : };
      35             : 
      36             : class SoloGimbal : AP_AccelCal_Client
      37             : {
      38             : public:
      39             :     //Constructor
      40           1 :     SoloGimbal() :
      41           1 :         _ekf(),
      42           1 :         _state(GIMBAL_STATE_NOT_PRESENT),
      43           1 :         _vehicle_yaw_rate_ef_filt(0.0f),
      44           1 :         _vehicle_to_gimbal_quat(),
      45           1 :         _vehicle_to_gimbal_quat_filt(),
      46           1 :         _filtered_joint_angles(),
      47           1 :         _last_report_msg_ms(0),
      48           1 :         _max_torque(5000.0f),
      49           1 :         _ang_vel_mag_filt(0),
      50           1 :         _lockedToBody(false),
      51           1 :         _log_dt(0),
      52           1 :         _log_del_ang(),
      53           3 :         _log_del_vel()
      54             :     {
      55             : #if HAL_INS_ACCELCAL_ENABLED
      56           1 :         AP_AccelCal::register_client(this);
      57             : #endif
      58           1 :     }
      59             : 
      60             :     void    update_target(const Vector3f &newTarget);
      61             :     void    receive_feedback(mavlink_channel_t chan, const mavlink_message_t &msg);
      62             : 
      63             :     void update_fast();
      64             : 
      65             :     bool present();
      66             :     bool aligned();
      67             : 
      68        8680 :     void set_lockedToBody(bool val) { _lockedToBody = val; }
      69             : 
      70             :     void write_logs();
      71             : 
      72       18195 :     float get_log_dt() const { return _log_dt; }
      73             : 
      74           0 :     void disable_torque_report() { _gimbalParams.set_param(GMB_PARAM_GMB_SND_TORQUE, 0); }
      75           1 :     void fetch_params() { _gimbalParams.fetch_params(); }
      76             : 
      77          26 :     void handle_param_value(const mavlink_message_t &msg) {
      78          26 :         _gimbalParams.handle_param_value(msg);
      79          26 :     }
      80             : 
      81             : private:
      82             :     // private methods
      83             :     void update_estimators();
      84             :     void send_controls(mavlink_channel_t chan);
      85             :     void extract_feedback(const mavlink_gimbal_report_t& report_msg);
      86             :     void update_joint_angle_est();
      87             : 
      88             :     Vector3f get_ang_vel_dem_yaw(const Quaternion &quatEst);
      89             :     Vector3f get_ang_vel_dem_roll_tilt(const Quaternion &quatEst);
      90             :     Vector3f get_ang_vel_dem_feedforward(const Quaternion &quatEst) const;
      91             :     Vector3f get_ang_vel_dem_gyro_bias();
      92             :     Vector3f get_ang_vel_dem_body_lock();
      93             : 
      94             :     void gimbal_ang_vel_to_joint_rates(const Vector3f& ang_vel, Vector3f& joint_rates) const;
      95             :     void joint_rates_to_gimbal_ang_vel(const Vector3f& joint_rates, Vector3f& ang_vel) const;
      96             : 
      97             :     void readVehicleDeltaAngle(uint8_t ins_index, Vector3f &dAng);
      98             : 
      99             :     void _acal_save_calibrations() override;
     100             :     bool _acal_get_ready_to_sample() override;
     101             :     bool _acal_get_saving() override;
     102             :     AccelCalibrator* _acal_get_calibrator(uint8_t instance) override;
     103             : 
     104             :     gimbal_mode_t get_mode();
     105             : 
     106             :     bool joints_near_limits() const;
     107             : 
     108             :     // private member variables
     109             :     SoloGimbalEKF            _ekf;      // state of small EKF for gimbal
     110             : 
     111             :     gimbal_state_t _state;
     112             : 
     113             :     struct {
     114             :         float delta_time;
     115             :         Vector3f delta_angles;
     116             :         Vector3f delta_velocity;
     117             :         Vector3f joint_angles;
     118             :     } _measurement;
     119             : 
     120             :     float _vehicle_yaw_rate_ef_filt;
     121             : 
     122             :     static const uint8_t _compid = MAV_COMP_ID_GIMBAL;
     123             : 
     124             :     // joint angle filter states
     125             :     Vector3f _vehicle_delta_angles;
     126             : 
     127             :     Quaternion _vehicle_to_gimbal_quat;
     128             :     Quaternion _vehicle_to_gimbal_quat_filt;
     129             :     Vector3f _filtered_joint_angles;
     130             : 
     131             :     uint32_t _last_report_msg_ms;
     132             : 
     133             :     float _max_torque;
     134             : 
     135             :     float _ang_vel_mag_filt;
     136             : 
     137             :     Vector3f    _ang_vel_dem_rads;       // rad/s
     138             :     Vector3f    _att_target_euler_rad;   // desired earth-frame roll, tilt and pan angles in radians
     139             : 
     140             :     bool _lockedToBody;
     141             : 
     142             :     SoloGimbal_Parameters _gimbalParams;
     143             : 
     144             :     AccelCalibrator _calibrator;
     145             : 
     146             :     float _log_dt;
     147             :     Vector3f _log_del_ang;
     148             :     Vector3f _log_del_vel;
     149             : };
     150             : 
     151             : #endif // HAL_SOLO_GIMBAL_ENABLED