Line data    Source code

       1             : /*
       2             :   Alexmos Serial controlled mount backend class
       3             : */
       4             : #pragma once
       5             : 
       6             : #include "AP_Mount_config.h"
       7             : 
       8             : #if HAL_MOUNT_ALEXMOS_ENABLED
       9             : 
      10             : #include "AP_Mount_Backend.h"
      11             : 
      12             : #include <AP_HAL/AP_HAL.h>
      13             : #include <AP_Param/AP_Param.h>
      14             : #include <AP_Math/AP_Math.h>
      15             : 
      16             : class AP_Mount_Alexmos : public AP_Mount_Backend
      17             : {
      18             : public:
      19             :     //constructor
      20             :     using AP_Mount_Backend::AP_Mount_Backend;
      21             : 
      22             :     // init - performs any required initialisation for this instance
      23             :     void init() override;
      24             : 
      25             :     // update mount position - should be called periodically
      26             :     void update() override;
      27             : 
      28             :     // has_pan_control - returns true if this mount can control its pan (required for multicopters)
      29             :     bool has_pan_control() const override;
      30             : 
      31             : protected:
      32             : 
      33             :     // get attitude as a quaternion.  returns true on success
      34             :     bool get_attitude_quaternion(Quaternion& att_quat) override;
      35             : 
      36             :     // servo only natively supports angles:
      37           0 :     uint8_t natively_supported_mount_target_types() const override {
      38           0 :         return NATIVE_ANGLES_ONLY;
      39             :     };
      40             : 
      41             : private:
      42             :     // allow removing lean angles for pitch and roll locks
      43           0 :     bool apply_bf_roll_pitch_adjustments_in_rc_targeting() const override {
      44           0 :         return true;
      45             :     }
      46             : 
      47             :     // get_angles -
      48             :     void get_angles();
      49             : 
      50             :     // set_motor will activate motors if true, and disable them if false
      51             :     void set_motor(bool on);
      52             : 
      53             :     // get_boardinfo - get board version and firmware version
      54             :     void get_boardinfo();
      55             : 
      56             :     // send new angles to the gimbal at a fixed speed of 30 deg/s
      57             :     void send_target_angles(const MountAngleTarget& angle_rad) override;
      58             : 
      59             :     // read_params - read current profile profile_id and global parameters from the gimbal settings
      60             :     void read_params(uint8_t profile_id);
      61             : 
      62             :     // write_params - write new parameters to the gimbal settings
      63             :     void write_params();
      64             : 
      65             :     bool get_realtimedata(Vector3f& angle);
      66             : 
      67             :     // Alexmos Serial Protocol reading part implementation
      68             :     // send_command - send a command to the Alemox Serial API
      69             :     void send_command(uint8_t cmd, uint8_t* data, uint8_t size);
      70             : 
      71             :     // Parse the body of the message received from the Alexmos gimbal
      72             :     void parse_body();
      73             : 
      74             :     // read_incoming - detect and read the header of the incoming message from the gimbal
      75             :     void read_incoming();
      76             : 
      77             :     // structure for the Serial Protocol
      78             : 
      79             :     // CMD_BOARD_INFO
      80             :     struct PACKED alexmos_version {
      81             :         uint8_t _board_version;
      82             :         uint16_t _firmware_version;
      83             :         uint8_t debug_mode;
      84             :         uint16_t _board_features;
      85             :     };
      86             : 
      87             :     // CMD_GET_ANGLES
      88             :     struct PACKED alexmos_angles {
      89             :         int16_t angle_roll;
      90             :         int16_t rc_angle_roll;
      91             :         int16_t rc_speed_roll;
      92             :         int16_t angle_pitch;
      93             :         int16_t rc_angle_pitch;
      94             :         int16_t rc_speed_pitch;
      95             :         int16_t angle_yaw;
      96             :         int16_t rc_angle_yaw;
      97             :         int16_t rc_speed_yaw;
      98             :     };
      99             : 
     100             :     // CMD_CONTROL
     101             :     struct PACKED alexmos_angles_speed {
     102             :         int8_t mode;
     103             :         int16_t speed_roll;
     104             :         int16_t angle_roll;
     105             :         int16_t speed_pitch;
     106             :         int16_t angle_pitch;
     107             :         int16_t speed_yaw;
     108             :         int16_t angle_yaw;
     109             :     };
     110             : 
     111             :     // CMD_READ_PARAMS
     112             :     struct PACKED alexmos_params {
     113             :         uint8_t profile_id;
     114             :         uint8_t roll_P;
     115             :         uint8_t roll_I;
     116             :         uint8_t roll_D;
     117             :         uint8_t roll_power;
     118             :         uint8_t roll_invert;
     119             :         uint8_t roll_poles;
     120             :         uint8_t pitch_P;
     121             :         uint8_t pitch_I;
     122             :         uint8_t pitch_D;
     123             :         uint8_t pitch_power;
     124             :         uint8_t pitch_invert;
     125             :         uint8_t pitch_poles;
     126             :         uint8_t yaw_P;
     127             :         uint8_t yaw_I;
     128             :         uint8_t yaw_D;
     129             :         uint8_t yaw_power;
     130             :         uint8_t yaw_invert;
     131             :         uint8_t yaw_poles;
     132             :         uint8_t acc_limiter;
     133             :         int8_t ext_fc_gain_roll;
     134             :         int8_t ext_fc_gain_pitch;
     135             :         int16_t roll_rc_min_angle;
     136             :         int16_t roll_rc_max_angle;
     137             :         uint8_t roll_rc_mode;
     138             :         uint8_t roll_rc_lpf;
     139             :         uint8_t roll_rc_speed;
     140             :         uint8_t roll_rc_follow;
     141             :         int16_t pitch_rc_min_angle;
     142             :         int16_t pitch_rc_max_angle;
     143             :         uint8_t pitch_rc_mode;
     144             :         uint8_t pitch_rc_lpf;
     145             :         uint8_t pitch_rc_speed;
     146             :         uint8_t pitch_rc_follow;
     147             :         int16_t yaw_rc_min_angle;
     148             :         int16_t yaw_rc_max_angle;
     149             :         uint8_t yaw_rc_mode;
     150             :         uint8_t yaw_rc_lpf;
     151             :         uint8_t yaw_rc_speed;
     152             :         uint8_t yaw_rc_follow;
     153             :         uint8_t gyro_trust;
     154             :         uint8_t use_model;
     155             :         uint8_t pwm_freq;
     156             :         uint8_t serial_speed;
     157             :         int8_t rc_trim_roll;
     158             :         int8_t rc_trim_pitch;
     159             :         int8_t rc_trim_yaw;
     160             :         uint8_t rc_deadband;
     161             :         uint8_t rc_expo_rate;
     162             :         uint8_t rc_virt_mode;
     163             :         uint8_t rc_map_roll;
     164             :         uint8_t rc_map_pitch;   
     165             :         uint8_t rc_map_yaw;
     166             :         uint8_t rc_map_cmd;
     167             :         uint8_t rc_map_fc_roll;
     168             :         uint8_t rc_map_fc_pitch;
     169             : 
     170             :         uint8_t rc_mix_fc_roll;
     171             :         uint8_t rc_mix_fc_pitch;
     172             : 
     173             :         uint8_t follow_mode;
     174             :         uint8_t follow_deadband;
     175             :         uint8_t follow_expo_rate;
     176             :         int8_t follow_offset_roll;
     177             :         int8_t follow_offset_pitch;
     178             :         int8_t follow_offset_yaw;
     179             : 
     180             :         int8_t axis_top;
     181             :         int8_t axis_right;
     182             : 
     183             :         uint8_t gyro_lpf;
     184             : 
     185             :         uint8_t gyro_sens;
     186             :         uint8_t i2c_internal_pullups;
     187             :         uint8_t sky_gyro_calib;
     188             : 
     189             :         uint8_t rc_cmd_low;
     190             :         uint8_t rc_cmd_mid;
     191             :         uint8_t rc_cmd_high;
     192             : 
     193             :         uint8_t menu_cmd_1;
     194             :         uint8_t menu_cmd_2;
     195             :         uint8_t menu_cmd_3;
     196             :         uint8_t menu_cmd_4;
     197             :         uint8_t menu_cmd_5;
     198             :         uint8_t menu_cmd_long;
     199             : 
     200             :         uint8_t output_roll;
     201             :         uint8_t output_pitch;
     202             :         uint8_t output_yaw;
     203             : 
     204             :         int16_t bat_threshold_alarm;
     205             :         int16_t bat_threshold_motors;
     206             :         int16_t bat_comp_ref;
     207             : 
     208             :         uint8_t beeper_modes;
     209             : 
     210             :         uint8_t follow_roll_mix_start;
     211             :         uint8_t follow_roll_mix_range;
     212             : 
     213             :         uint8_t booster_power_roll;
     214             :         uint8_t booster_power_pitch;
     215             :         uint8_t booster_power_yaw;
     216             : 
     217             :         uint8_t follow_speed_roll;
     218             :         uint8_t follow_speed_pitch;
     219             :         uint8_t follow_speed_yaw;
     220             : 
     221             :         uint8_t frame_angle_from_motors;
     222             : 
     223             :         uint8_t cur_profile_id;
     224             : 
     225             :     };
     226             :     union PACKED alexmos_parameters {
     227           0 :         DEFINE_BYTE_ARRAY_METHODS
     228             :         alexmos_version version;
     229             :         alexmos_angles angles;
     230             :         alexmos_params params;
     231             :         alexmos_angles_speed angle_speed;
     232             :     } _buffer,_current_parameters;
     233             : 
     234             :     AP_HAL::UARTDriver *_port;
     235             :     bool _initialised;
     236             : 
     237             :     // result of the get_boardinfo
     238             :     uint8_t _board_version;
     239             :     float _current_firmware_version;
     240             :     uint8_t _firmware_beta_version;
     241             :     bool _gimbal_3axis : 1;
     242             :     bool _gimbal_bat_monitoring : 1;
     243             : 
     244             :     // keep the last _current_angle values
     245             :     Vector3f _current_angle;
     246             : 
     247             :     // CMD_READ_PARAMS has been called once
     248             :     bool _param_read_once;
     249             : 
     250             :     // Serial Protocol Variables
     251             :     uint8_t _checksum;
     252             :     uint8_t _step;
     253             :     uint8_t _command_id;
     254             :     uint8_t _payload_length;
     255             :     uint8_t _payload_counter;
     256             : 
     257             :     // confirmed that last command was ok
     258             :     bool _last_command_confirmed;
     259             : };
     260             : #endif // HAL_MOUNT_ALEXMOS_ENABLED