Skip to content

Commit

Permalink
AP_Mount: Topotek formatting fixes
Browse files Browse the repository at this point in the history
  • Loading branch information
@rmackay9
rmackay9 committed Feb 29, 2024
1 parent adbcbb4 commit ee45e69
Show file tree
Hide file tree
Showing 2 changed files with 38 additions and 75 deletions.
107 changes: 34 additions & 73 deletions libraries/AP_Mount/AP_Mount_Topotek.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,6 @@ extern const AP_HAL::HAL& hal;
#define TRACK_RANGE 60 // the size of the image at point tracking
#define AP_MOUNT_TOPOTEK_UPDATE_INTERVAL_MS 100 // resend angle or rate targets to gimbal at this interval
#define AP_MOUNT_TOPOTEK_HEALTH_TIMEOUT_MS 1000 // timeout for health and rangefinder readings
#define UNUSED(x) (void)x; // function argument not used

#define AP_MOUNT_TOPOTEK_DEBUG 0
#define debug(fmt, args ...) do { if (AP_MOUNT_TOPOTEK_DEBUG) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Topotek: " fmt, ## args); } } while (0)
Expand Down Expand Up @@ -72,25 +71,26 @@ void AP_Mount_Topotek::update()

// request gimbal attitude information within the specified time
if ((now_ms - _last_req_current_info_ms) >= 100) {

_last_req_current_info_ms = now_ms;
static uint8_t count;
// request memory card information within the specified time
if (count%3 == 0) {

if (_last_req_count % 3 == 0) {
// request memory card information within the specified time
request_gimbal_sdcard_info();

// if trace mode is enabled, trace status information is requested within a specified period of time
if (_is_tracking) {
request_track_status();
}
}

// the gimbal attitude is obtained every second, and the gimbal will continue to send attitude information during the next period
if (count%11 == 0) {
if (_last_req_count % 11 == 0) {
request_gimbal_attitude();
if (!count) {
count = 0;
if (!_last_req_count) {
_last_req_count = 0;
}
}
count++;
_last_req_count++;
}

// zoom control
Expand All @@ -104,12 +104,9 @@ void AP_Mount_Topotek::update()
request_gimbal_basic_info();
}



#if AP_RTC_ENABLED
// send UTC time to the camera
if (_sent_time_count < 7)
{
if (_sent_time_count < 7) {
uint64_t utc_usec ;
if (AP::rtc().get_utc_usec(utc_usec) && send_time_to_gimbal(utc_usec)) {
_sent_time_count++;
Expand Down Expand Up @@ -226,7 +223,6 @@ bool AP_Mount_Topotek::take_picture()
return false;
}


return send_packet(_take_pic, 12);
}

Expand Down Expand Up @@ -255,7 +251,7 @@ bool AP_Mount_Topotek::record_video(bool start_recording)
_start_record_video[11] = '0';
}

return send_packet(_start_record_video, 12);;
return send_packet(_start_record_video, 12);
}

// set zoom specified as a rate
Expand Down Expand Up @@ -334,7 +330,6 @@ SetFocusResult AP_Mount_Topotek::set_focus(FocusType focus_type, float focus_val
return SetFocusResult::INVALID_PARAMETERS;
}


// set tracking to none, point or rectangle (see TrackingType enum)
// if POINT only p1 is used, if RECTANGLE then p1 is top-left, p2 is bottom-right
// p1,p2 are in range 0 to 1. 0 is left or top, 1 is right or bottom
Expand All @@ -345,12 +340,10 @@ bool AP_Mount_Topotek::set_tracking(TrackingType tracking_type, const Vector2f&
return false;
}


switch (tracking_type) {
case TrackingType::TRK_NONE:
_is_tracking = false;
return send_packet(_stop_track_cmd, 12);
break;
case TrackingType::TRK_POINT: {
// the converted tracking center
int16_t track_center_x = (int16_t)((p1.x*TRACK_TOTAL_WIDTH - 960) / 0.96);
Expand All @@ -376,7 +369,6 @@ bool AP_Mount_Topotek::set_tracking(TrackingType tracking_type, const Vector2f&

_is_tracking = true;
return send_packet(_begin_track_cmd, 20);
break;
}
case TrackingType::TRK_RECTANGLE:
// upper left point
Expand Down Expand Up @@ -442,7 +434,6 @@ bool AP_Mount_Topotek::set_tracking(TrackingType tracking_type, const Vector2f&

_is_tracking = true;
return send_packet(_begin_track_cmd, 20);
break;

}

Expand All @@ -458,8 +449,6 @@ bool AP_Mount_Topotek::set_lens(uint8_t lens)
return false;
}

UNUSED(lens);

return (send_packet(_next_pip_mode, 12));
}

Expand Down Expand Up @@ -548,15 +537,11 @@ void AP_Mount_Topotek::read_incoming_packets()
}

if (_msg_buff_len != 0 || b == '#') {

if (b == '#' && _msg_buff_len != 0) {
analyse_packet_data();

}

_msg_buff[_msg_buff_len++] = b;
}

}
analyse_packet_data();
}
Expand Down Expand Up @@ -592,11 +577,8 @@ void AP_Mount_Topotek::send_angle_target(float roll_rad, float pitch_rad, float
return;
}
uint8_t speed = 99;

int16_t roll_angle_hund = (int16_t)(RAD_TO_DEG*roll_rad*100);

int16_t pitch_angle_hund = (int16_t)(RAD_TO_DEG*pitch_rad*100);

int16_t yaw_angle_hund = (int16_t)(RAD_TO_DEG*yaw_rad*100);

_set_yaw_pitch_angle_cmd[10] = hex2char((yaw_angle_hund >> 12) & 0x0F);
Expand Down Expand Up @@ -637,36 +619,32 @@ void AP_Mount_Topotek::send_rate_target(float roll_rads, float pitch_rads, float
return;
}

static uint8_t roll_angle_speed;

static uint8_t pitch_angle_speed;

static uint8_t yaw_angle_speed;

static uint8_t gimbal_stop_order_count;
// send stop rotation command three times if target roll, pitch and yaw are near zero
if (roll_rads >= -0.0001 && roll_rads <= 0.0001 && pitch_rads >= -0.0001 && pitch_rads <= 0.0001 && yaw_rads >= -0.0001 && yaw_rads <= 0.0001) {

if (gimbal_stop_order_count < 3) {
if (_stop_order_count < 3) {
send_packet(_gimbal_control_stop, 12);
gimbal_stop_order_count++;
_stop_order_count++;
}

return;
}
gimbal_stop_order_count = 0;
_stop_order_count = 0;

uint8_t roll_angle_speed;
if (roll_rads >= 0) {
roll_angle_speed = RAD_TO_DEG*roll_rads*ANGULAR_VELOCITY_CONVERSION> 99 ? 99 : (uint8_t)(RAD_TO_DEG*roll_rads*ANGULAR_VELOCITY_CONVERSION);
} else {
roll_angle_speed = RAD_TO_DEG*roll_rads*ANGULAR_VELOCITY_CONVERSION < -99 ? -99 : (uint8_t)(RAD_TO_DEG*roll_rads*ANGULAR_VELOCITY_CONVERSION);
}

uint8_t pitch_angle_speed;
if (pitch_rads >= 0) {
pitch_angle_speed = RAD_TO_DEG*pitch_rads*ANGULAR_VELOCITY_CONVERSION > 99 ? 99 : (uint8_t)(RAD_TO_DEG*pitch_rads*ANGULAR_VELOCITY_CONVERSION);
} else {
pitch_angle_speed = RAD_TO_DEG*pitch_rads*ANGULAR_VELOCITY_CONVERSION < -99 ? -99 : (uint8_t)(RAD_TO_DEG*pitch_rads*ANGULAR_VELOCITY_CONVERSION);
}

uint8_t yaw_angle_speed;
if (yaw_rads >= 0) {
yaw_angle_speed = RAD_TO_DEG*yaw_rads*ANGULAR_VELOCITY_CONVERSION > 99 ? 99 : (uint8_t)(RAD_TO_DEG*yaw_rads*ANGULAR_VELOCITY_CONVERSION);
} else {
Expand All @@ -687,7 +665,7 @@ void AP_Mount_Topotek::send_rate_target(float roll_rads, float pitch_rads, float
return;
}

//send time to gimbal
// send time to gimbal
bool AP_Mount_Topotek::send_time_to_gimbal(uint64_t &utc_time)
{
time_t now_second = utc_time / 1000000;
Expand Down Expand Up @@ -734,22 +712,18 @@ void AP_Mount_Topotek::gimbal_angle_analyse(void)
// Angle[0] represents the gimbal yaw angle multiplied by 100
// Angle[1] represents the gimbal pitch angle multiplied by 100
// Angle[2] represents the gimbal roll angle multiplied by 100
int16_t Angle[3] = {0};
int16_t angle[3];

Angle[0] = char_to_number(_msg_buff[10])<<12 | char_to_number(_msg_buff[11])<<8 | char_to_number(_msg_buff[12])<<4 | char_to_number(_msg_buff[13]);
Angle[1] = char_to_number(_msg_buff[14])<<12 | char_to_number(_msg_buff[15])<<8 | char_to_number(_msg_buff[16])<<4 | char_to_number(_msg_buff[17]);
Angle[2] = char_to_number(_msg_buff[18])<<12 | char_to_number(_msg_buff[19])<<8 | char_to_number(_msg_buff[20])<<4 | char_to_number(_msg_buff[21]);
angle[0] = char_to_number(_msg_buff[10])<<12 | char_to_number(_msg_buff[11])<<8 | char_to_number(_msg_buff[12])<<4 | char_to_number(_msg_buff[13]);
angle[1] = char_to_number(_msg_buff[14])<<12 | char_to_number(_msg_buff[15])<<8 | char_to_number(_msg_buff[16])<<4 | char_to_number(_msg_buff[17]);
angle[2] = char_to_number(_msg_buff[18])<<12 | char_to_number(_msg_buff[19])<<8 | char_to_number(_msg_buff[20])<<4 | char_to_number(_msg_buff[21]);

// handle when the yaw angle is too large
if (18000 < Angle[0]) {
Angle[0] = Angle[0] - 36000;
} else if (-18000 > Angle[0]) {
Angle[0] = Angle[0] + 36000;
}
angle[0] = wrap_180_cd(angle[0]);

_current_angle_rad.x = (float)Angle[2] / 100 * DEG_TO_RAD;
_current_angle_rad.y = (float)Angle[1] / 100 * DEG_TO_RAD;
_current_angle_rad.z = (float)Angle[0] / 100 * DEG_TO_RAD;
_current_angle_rad.x = (float)angle[2] / 100 * DEG_TO_RAD;
_current_angle_rad.y = (float)angle[1] / 100 * DEG_TO_RAD;
_current_angle_rad.z = (float)angle[0] / 100 * DEG_TO_RAD;

return;
}
Expand All @@ -759,9 +733,7 @@ void AP_Mount_Topotek::gimbal_record_analyse(void)
{
if (_msg_buff[10] == '1' || _msg_buff[11] == '1') {
_recording = true;

GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s recording ON", send_message_prefix);

return;
}
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s recording OFF", send_message_prefix);
Expand Down Expand Up @@ -795,7 +767,6 @@ void AP_Mount_Topotek::gimbal_track_analyse(void)
case '2':
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s be tracking", send_message_prefix);
break;

}
}

Expand All @@ -808,7 +779,7 @@ void AP_Mount_Topotek::gimbal_basic_info_analyse(void)
uint8_t patch_ver = 0;

// analyze gimbal model and firmware version
for ( count = 0; count < char_to_number(_msg_buff[5]); count++ ) {
for (count = 0; count < char_to_number(_msg_buff[5]); count++) {
if (count == 0) {
major_ver = char_to_number(_msg_buff[10 + count]);
} else if (count == 1) {
Expand All @@ -819,15 +790,13 @@ void AP_Mount_Topotek::gimbal_basic_info_analyse(void)
strncpy(( char*)_model_name, (const char*)(_msg_buff+13), 10);
break;
}

}
_firmware_ver = (patch_ver << 16) | (minor_ver << 8) | (major_ver);

if (strlen((const char*)_model_name) == 0) {
strcpy((char*)_model_name, "Unknown");
}


_got_gimbal_basic_info = true;
}

Expand All @@ -836,43 +805,35 @@ void AP_Mount_Topotek::update_zoom_control(void)
if (_zoom_type == ZoomType::RATE) {
// only send zoom rate target if it's non-zero because if zero it has already been sent
// and sending zero rate also triggers autofocus
//zoom stop
// zoom stop
_zoom_cmd[11] = '0';
if (_zoom_value < 0) {
//zoom out
// zoom out
_zoom_cmd[11] = '1';
} else if (_zoom_value > 0) {
//zoom in
// zoom in
_zoom_cmd[11] = '2';
}
send_packet(_zoom_cmd, 12);
}
}

//add check
// add check
void AP_Mount_Topotek::add_check(int8_t *cmd, uint8_t len)
{
int8_t crc = 0;
crc = calculate_crc(cmd, len);

cmd[len] = hex2char((crc >> 4) & 0x0f);

cmd[len+1] = hex2char(crc & 0x0f);

}

// calculate checksum
int8_t AP_Mount_Topotek::calculate_crc(int8_t *cmd, uint8_t len)
{
char crc;
int i;

crc = 0;
//calculate
for (i = 0; i<len; i++) {
uint8_t crc = 0;
for (uint16_t i = 0; i<len; i++) {
crc += cmd[i];
}

return(crc);
}

Expand Down
6 changes: 4 additions & 2 deletions libraries/AP_Mount/AP_Mount_Topotek.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -178,7 +178,9 @@ class AP_Mount_Topotek : public AP_Mount_Backend
Vector3f _current_angle_rad; // current angles in radians received from gimbal (x=roll, y=pitch, z=yaw)
uint32_t _last_current_angle_ms = 0; // system time (in milliseconds) that angle information received from the gimbal
uint32_t _last_req_current_info_ms = 0; // system time that this driver last requested current gimbal infomation
uint8_t _last_req_count; // last request count used to slow request to gimbal
uint32_t _last_zoom_control_ms = 0; // system time (in milliseconds) that control gimbal zoom
uint8_t _stop_order_count = 0; // number of stop commands sent since target rates became zero

// buffer holding bytes from latest packet. This is only used to calculate the crc
int8_t _msg_buff[AP_MOUNT_TOPOTEK_PACKETLEN_MAX]{};
Expand All @@ -204,8 +206,8 @@ class AP_Mount_Topotek : public AP_Mount_Backend
static int8_t _gimbal_lock[15]; // set whether the gimbal is locked or followed command
// stores strings and member function pointers
typedef struct {
int8_t uart_cmd_key[4]; // gimbal command key;
void (AP_Mount_Topotek::*func)(void); // the gimbal command corresponding operation
int8_t uart_cmd_key[4]; // gimbal command key;
void (AP_Mount_Topotek::*func)(void); // the gimbal command corresponding operation
} UartCmdFunctionHandler;

// stores command ID and corresponding member functions that are compared with the command received by the gimbal
Expand Down

0 comments on commit ee45e69

Please sign in to comment.