Line data    Source code

       1             : #include "AP_Mount_config.h"
       2             : 
       3             : #if HAL_MOUNT_CADDX_ENABLED
       4             : 
       5             : #include "AP_Mount_CADDX.h"
       6             : #include <AP_HAL/AP_HAL.h>
       7             : 
       8             : #define SET_ATTITUDE_HEADER1 0xA5
       9             : #define SET_ATTITUDE_HEADER2 0x5A
      10             : #define SET_ATTITUDE_BUF_SIZE 10
      11             : #define AXIS_MIN 0
      12             : #define AXIS_MAX 4096
      13             : 
      14             : // update mount position - should be called periodically
      15           0 : void AP_Mount_CADDX::update()
      16             : {
      17           0 :     AP_Mount_Backend::update();
      18             : 
      19             :     // exit immediately if not initialised
      20           0 :     if (!_initialised) {
      21           0 :         return;
      22             :     }
      23             : 
      24           0 :     AP_Mount_Backend::update_mnt_target();
      25             : 
      26             :     // send target angles (which may be derived from other target types)
      27           0 :     AP_Mount_Backend::send_target_to_gimbal();
      28             : }
      29             : 
      30             : // get attitude as a quaternion.  returns true on success
      31           0 : bool AP_Mount_CADDX::get_attitude_quaternion(Quaternion& att_quat)
      32             : {
      33             :     // gimbal does not provide attitude so simply return targets
      34           0 :     att_quat.from_euler(mnt_target.angle_rad.roll, mnt_target.angle_rad.pitch, mnt_target.angle_rad.get_bf_yaw());
      35           0 :     return true;
      36             : }
      37             : 
      38             : // send_target_angles
      39           0 : void AP_Mount_CADDX::send_target_angles(const MountAngleTarget& angle_target_rad)
      40             : {
      41             :     // exit immediately if not initialised
      42           0 :     if (!_initialised) {
      43           0 :         return;
      44             :     }
      45             : 
      46             :     // ensure we have enough space to send the packet
      47           0 :     if (_uart->txspace() < SET_ATTITUDE_BUF_SIZE) {
      48           0 :         return;
      49             :     }
      50             : 
      51             :     // calculate roll, pitch, yaw angles in range 0 to 4096
      52           0 :     const float scalar = AXIS_MAX / M_2PI;
      53           0 :     const uint16_t roll_target_cmd = constrain_uint16(wrap_2PI(angle_target_rad.roll) * scalar, AXIS_MIN, AXIS_MAX);
      54           0 :     const uint16_t pitch_target_cmd = constrain_uint16(wrap_2PI(angle_target_rad.pitch) * scalar, AXIS_MIN, AXIS_MAX);
      55           0 :     const uint16_t yaw_target_cmd = constrain_uint16(wrap_2PI(angle_target_rad.get_bf_yaw()) * scalar, AXIS_MIN, AXIS_MAX);
      56             :     
      57             :     // prepare packet to send to gimbal
      58           0 :     uint8_t set_attitude_cmd_buf[SET_ATTITUDE_BUF_SIZE] {};
      59             : 
      60             :     // first two bytes hold the header
      61           0 :     set_attitude_cmd_buf[0] = SET_ATTITUDE_HEADER1;
      62           0 :     set_attitude_cmd_buf[1] = SET_ATTITUDE_HEADER2;
      63             : 
      64             :     // byte 2's lower 3 bits are mode
      65             :     // lower 5 bits are sensitivity but always left as zero
      66           0 :     uint8_t mode = 0; //start with axes in bf
      67             :     // CADDX yaw lock is not required and is duplication since we get yaw via get_bf_yaw, the gimbal accs would be fighting our yaw target potentially
      68             :     // but we need to reset roll and pitch locks to body frame if set by RP_LOCK aux switch or by FPV mnt option
      69           0 :     if (angle_target_rad.pitch_is_ef) {
      70           0 :         mode |= (uint8_t)LockMode::PITCH_LOCK;
      71             :     }
      72           0 :     if (angle_target_rad.roll_is_ef) {
      73           0 :         mode |= (uint8_t)LockMode::ROLL_LOCK;
      74             :     }
      75           0 :     set_attitude_cmd_buf[2] = mode & 0x07;
      76             : 
      77             :     // byte 3's lower 4 bits are reserved
      78             :     // upper 4 bits are roll's lower 4 bits
      79           0 :     set_attitude_cmd_buf[3] = (roll_target_cmd << 4) & 0xF0;
      80             : 
      81             :     // byte 4 is roll's upper 8 bits
      82           0 :     set_attitude_cmd_buf[4] = (roll_target_cmd >> 4) & 0xFF;
      83             : 
      84             :     // byte 5 is pitch's lower 8 bits
      85           0 :     set_attitude_cmd_buf[5] = pitch_target_cmd & 0xFF;
      86             : 
      87             :     // byte 6's lower 4 bits are pitch's upper 4 bits
      88             :     // upper 4 bits are yaw's lower 4 bits
      89           0 :     set_attitude_cmd_buf[6] = (pitch_target_cmd >> 8) & 0x0F;
      90           0 :     set_attitude_cmd_buf[6] |= (yaw_target_cmd << 4) & 0xF0;
      91             : 
      92             :     // byte 7 is yaw's upper 8 bits
      93           0 :     set_attitude_cmd_buf[7] = (yaw_target_cmd >> 4) & 0xFF;
      94             : 
      95             :     // calculate CRC
      96           0 :     const uint16_t crc16 = crc16_ccitt(set_attitude_cmd_buf, sizeof(set_attitude_cmd_buf) - 2, 0);
      97           0 :     set_attitude_cmd_buf[8] = HIGHBYTE(crc16);
      98           0 :     set_attitude_cmd_buf[9] = LOWBYTE(crc16);
      99             : 
     100             :     // send packet to gimbal
     101           0 :     _uart->write(set_attitude_cmd_buf, sizeof(set_attitude_cmd_buf));
     102             : }
     103             : 
     104             : #endif // HAL_MOUNT_CADDX_ENABLED