Skip to content

Commit

Permalink
AP_Mount: caddx protocol debug
Browse files Browse the repository at this point in the history
  • Loading branch information
@rmackay9
rmackay9 committed Dec 23, 2024
1 parent cf152ce commit 12666d3
Showing 1 changed file with 53 additions and 15 deletions.
68 changes: 53 additions & 15 deletions libraries/AP_Mount/AP_Mount_CADDX.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,8 +7,10 @@
#define SET_ATTITUDE_HEADER1 0xA5
#define SET_ATTITUDE_HEADER2 0x5A
#define SET_ATTITUDE_BUF_SIZE 10
#define AXIS_MIN -2048
#define AXIS_MAX 2047
//#define AXIS_MIN -2048
//#define AXIS_MAX 2047
#define AXIS_MIN -512
#define AXIS_MAX 512

// update mount position - should be called periodically
void AP_Mount_CADDX::update()
Expand Down Expand Up @@ -127,43 +129,56 @@ void AP_Mount_CADDX::send_target_angles(const MountTarget& angle_target_rad)
}

// calculate roll, pitch, yaw angles in range -2048 to 2047
int16_t roll_target_cmd = int16_t(linear_interpolate(AXIS_MIN, AXIS_MAX, degrees(angle_target_rad.roll), -180, 180));
int16_t pitch_target_cmd = int16_t(linear_interpolate(AXIS_MIN, AXIS_MAX, degrees(angle_target_rad.pitch), -180, 180));
int16_t yaw_target_cmd = int16_t(linear_interpolate(AXIS_MIN, AXIS_MAX, degrees(angle_target_rad.get_bf_yaw()), -180, 180));
//int16_t roll_target_cmd = int16_t(linear_interpolate(AXIS_MIN, AXIS_MAX, degrees(angle_target_rad.roll), -180, 180));
//int16_t pitch_target_cmd = int16_t(linear_interpolate(AXIS_MIN, AXIS_MAX, -degrees(angle_target_rad.pitch), -180, 180));
//int16_t yaw_target_cmd = int16_t(linear_interpolate(AXIS_MIN, AXIS_MAX, degrees(angle_target_rad.get_bf_yaw()), -180, 180));

// prepare packet to send to gimbal
uint8_t set_attitude_cmd_buf[SET_ATTITUDE_BUF_SIZE] {};

// copy in overrides
if (_params.override_byte.get() >= 0 && _params.override_byte.get() < 8) {
set_attitude_cmd_buf[_params.override_byte.get()] = _params.override_value.get();
}

// add header
set_attitude_cmd_buf[0] = SET_ATTITUDE_HEADER1; // 0
set_attitude_cmd_buf[1] = SET_ATTITUDE_HEADER2; // 1

// mode is top 3 bites of byte 2
uint8_t mode = (uint8_t)LockMode::GIMBAL_MODE_TILT_LOCK | (uint8_t)LockMode::GIMBAL_MODE_ROLL_LOCK;
/*uint8_t mode = (uint8_t)LockMode::GIMBAL_MODE_TILT_LOCK | (uint8_t)LockMode::GIMBAL_MODE_ROLL_LOCK;
if (angle_target_rad.yaw_is_ef) {
mode |= (uint8_t)LockMode::GIMBAL_MODE_YAW_LOCK;
}
set_attitude_cmd_buf[2] = mode << 5;
set_attitude_cmd_buf[2] = mode << 5;*/

// sensitivity is bottom 5 bits of byte 2
const uint8_t sensitivity = 7;
set_attitude_cmd_buf[2] |= 0x1F & sensitivity;
//const uint8_t sensitivity = 0;
//set_attitude_cmd_buf[2] |= 0x1F & sensitivity;

// byte 3's top 4 bits are reserved
// bottom 4 bits are top 4 bits of roll
set_attitude_cmd_buf[3] = (roll_target_cmd >> 8) & 0x0F;
//set_attitude_cmd_buf[3] = (roll_target_cmd >> 8) & 0x0F;

// byte 4 is bottom 8 bits of roll
set_attitude_cmd_buf[4] = roll_target_cmd & 0xFF;
//set_attitude_cmd_buf[4] = roll_target_cmd & 0xFF; //<-- large control of roll, direction seems correct so must be high byte?
//set_attitude_cmd_buf[4] = roll_target_cmd & 0xF0; //<-- large control of roll

// byte 5 is top 8 bits of pitch
set_attitude_cmd_buf[5] = (pitch_target_cmd >> 4) & 0xFF;
//uint8_t pitch_top8 = (pitch_target_cmd >> 4) & 0xFF;
//uint8_t pitch_bot4 = pitch_target_cmd & 0x0F;
//set_attitude_cmd_buf[5] = pitch_top8; <-- small impact on pitch
//set_attitude_cmd_buf[5] = uint8_t((pitch_target_cmd >> 4) & 0xFF);

// byte 6's top 4 bits are bottom 4 bits of pitch
// bottom 4 bits are top 4 bits of yaw
set_attitude_cmd_buf[6] = (pitch_target_cmd << 4) & 0xF0;
set_attitude_cmd_buf[6] |= (yaw_target_cmd >> 8) & 0x0F;
//set_attitude_cmd_buf[6] = uint8_t(pitch_target_cmd & 0x0F) << 4;
//set_attitude_cmd_buf[6] |= (yaw_target_cmd >> 8) & 0x0F;
//set_attitude_cmd_buf[6] |= yaw_target_cmd & 0x0F;
//set_attitude_cmd_buf[6] |= (yaw_target_cmd >> 4) & 0xF0; <-- low bits of yaw

// byte 7 is bottom 8 bits of yaw
set_attitude_cmd_buf[7] = yaw_target_cmd & 0xFF;
//set_attitude_cmd_buf[7] = yaw_target_cmd & 0xFF; <-- all yaw

// calculate CRC
const uint16_t crc16 = crc16_ccitt(set_attitude_cmd_buf, sizeof(set_attitude_cmd_buf) - 2, 0);
Expand All @@ -175,6 +190,29 @@ void AP_Mount_CADDX::send_target_angles(const MountTarget& angle_target_rad)

// store time of send
_last_send_ms = AP_HAL::millis();

// debug
static uint32_t last_print_ms = 0;
uint32_t now_ms = AP_HAL::millis();
if (now_ms - last_print_ms > 1000) {
last_print_ms = now_ms;
/*GCS_SEND_TEXT(MAV_SEVERITY_INFO, "CADDX: pitch tdec:%4.1f thex:%x out:%x %x",
(float)degrees(angle_target_rad.pitch),
(int)pitch_target_cmd,
(int)set_attitude_cmd_buf[5],
(int)set_attitude_cmd_buf[6]);*/
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "CADDX: H:%x %x 2:%x 3:%x 4:%x 5:%x 6:%x 7:%x C:%x %x",
(int)set_attitude_cmd_buf[0],
(int)set_attitude_cmd_buf[1],
(int)set_attitude_cmd_buf[2],
(int)set_attitude_cmd_buf[3],
(int)set_attitude_cmd_buf[4],
(int)set_attitude_cmd_buf[5],
(int)set_attitude_cmd_buf[6],
(int)set_attitude_cmd_buf[7],
(int)set_attitude_cmd_buf[8],
(int)set_attitude_cmd_buf[9]);
}
}

#endif // HAL_MOUNT_CADDX_ENABLED

0 comments on commit 12666d3

Please sign in to comment.