Line data    Source code

       1             : /*
       2             :   SToRM32 mount using serial protocol backend class
       3             :  */
       4             : #pragma once
       5             : 
       6             : #include "AP_Mount_config.h"
       7             : 
       8             : #if HAL_MOUNT_STORM32SERIAL_ENABLED
       9             : 
      10             : #include "AP_Mount_Backend_Serial.h"
      11             : 
      12             : #include <AP_HAL/AP_HAL.h>
      13             : #include <AP_Math/AP_Math.h>
      14             : #include <AP_Common/AP_Common.h>
      15             : 
      16             : // timeout for no-response-received (gimbal only sends data when we
      17             : // send it data first)
      18             : #define AP_MOUNT_STORM32_SERIAL_TIMEOUT_MS   2000
      19             : 
      20             : class AP_Mount_SToRM32_serial : public AP_Mount_Backend_Serial
      21             : {
      22             : 
      23             : public:
      24             :     // Constructor
      25             :     using AP_Mount_Backend_Serial::AP_Mount_Backend_Serial;
      26             : 
      27             :     // update mount position - should be called periodically
      28             :     void update() override;
      29             : 
      30             :     // has_pan_control - returns true if this mount can control its pan (required for multicopters)
      31           0 :     bool has_pan_control() const override { return yaw_range_valid(); };
      32             : 
      33             : protected:
      34             : 
      35             :     // get attitude as a quaternion.  returns true on success
      36             :     bool get_attitude_quaternion(Quaternion& att_quat) override;
      37             : 
      38             : private:
      39             : 
      40             :     // SToRM32-serial can only send angles
      41           0 :     uint8_t natively_supported_mount_target_types() const override {
      42           0 :         return NATIVE_ANGLES_ONLY;
      43             :     };
      44             :     
      45             :     // allow removing lean angles for pitch and roll locks
      46           0 :     bool apply_bf_roll_pitch_adjustments_in_rc_targeting() const override {
      47           0 :         return true;
      48             :     }
      49             : 
      50             :     // send_target_angles
      51             :     void send_target_angles(const MountAngleTarget& angle_target_rad) override;
      52             : 
      53             :     // send read data request
      54             :     void get_angles();
      55             : 
      56             :     // read_incoming
      57             :     void read_incoming();
      58             :     void parse_reply();
      59             : 
      60             :     enum ReplyType {
      61             :         ReplyType_UNKNOWN = 0,
      62             :         ReplyType_DATA,
      63             :         ReplyType_ACK
      64             :     };
      65             : 
      66             :     //void add_next_reply(ReplyType reply_type);
      67             :     uint8_t get_reply_size(ReplyType reply_type);
      68             :     bool can_send();
      69             : 
      70             :     struct PACKED SToRM32_reply_data_struct {
      71             :         uint16_t state;
      72             :         uint16_t status;
      73             :         uint16_t status2;
      74             : 
      75             :         uint16_t i2c_errors;
      76             :         uint16_t lipo_voltage;
      77             :         uint16_t systicks;
      78             :         uint16_t cycle_time;
      79             : 
      80             :         int16_t imu1_gx;
      81             :         int16_t imu1_gy;
      82             :         int16_t imu1_gz;
      83             : 
      84             :         int16_t imu1_ax;
      85             :         int16_t imu1_ay;
      86             :         int16_t imu1_az;
      87             : 
      88             :         int16_t ahrs_x;
      89             :         int16_t ahrs_y;
      90             :         int16_t ahrs_z;
      91             : 
      92             :         int16_t imu1_pitch;
      93             :         int16_t imu1_roll;
      94             :         int16_t imu1_yaw;
      95             : 
      96             :         int16_t cpid_pitch;
      97             :         int16_t cpid_roll;
      98             :         int16_t cpid_yaw;
      99             : 
     100             :         uint16_t input_pitch;
     101             :         uint16_t input_roll;
     102             :         uint16_t input_yaw;
     103             : 
     104             :         int16_t imu2_pitch;
     105             :         int16_t imu2_roll;
     106             :         int16_t imu2_yaw;
     107             : 
     108             :         int16_t mag2_yaw;
     109             :         int16_t mag2_pitch;
     110             : 
     111             :         int16_t ahrs_imu_confidence;
     112             : 
     113             :         uint16_t function_input_values;
     114             : 
     115             :         uint16_t crc;
     116             :         uint8_t magic;
     117             :     };
     118             : 
     119             :     struct PACKED SToRM32_reply_ack_struct {
     120             :         uint8_t byte1;
     121             :         uint8_t byte2;
     122             :         uint8_t byte3;
     123             :         uint8_t data;
     124             :         uint16_t crc;
     125             :     };
     126             : 
     127             :     struct PACKED cmd_set_angles_struct {
     128             :         uint8_t byte1;
     129             :         uint8_t byte2;
     130             :         uint8_t byte3;
     131             :         float pitch;
     132             :         float roll;
     133             :         float yaw;
     134             :         uint8_t flags;
     135             :         uint8_t type;
     136             :         uint16_t crc;
     137             :     };
     138             : 
     139             : 
     140             :     // internal variables
     141             :     uint32_t _last_send;            // system time of last do_mount_control sent to gimbal
     142             :     uint8_t _reply_length;
     143             :     uint8_t _reply_counter;
     144             :     ReplyType _reply_type = ReplyType_UNKNOWN;
     145             : 
     146             : 
     147             :     union PACKED SToRM32_reply {
     148           0 :         DEFINE_BYTE_ARRAY_METHODS
     149             :         SToRM32_reply_data_struct data;
     150             :         SToRM32_reply_ack_struct ack;
     151             :     } _buffer;
     152             : 
     153             :     // keep the last _current_angle values
     154             :     Vector3l _current_angle;
     155             : };
     156             : #endif // HAL_MOUNT_STORM32SERIAL_ENABLED