Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Mount: topotek: Change the type of gimbal angle acquisition 

... also convert the lowercase characters in the command to uppercase
This commit is contained in:
zhou 2024-10-23 18:03:36 +08:00 committed by Randy Mackay
parent 8085c44840
commit 3bc0207f22
2 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_Mount/AP_Mount_Topotek.cpp
View File

@ -33,7 +33,7 @@ extern const AP_HAL::HAL& hal;
# define AP_MOUNT_TOPOTEK_ID3CHAR_START_TRACKING "LOC" // start image tracking # define AP_MOUNT_TOPOTEK_ID3CHAR_START_TRACKING "LOC" // start image tracking
# define AP_MOUNT_TOPOTEK_ID3CHAR_LRF "LRF" // laser rangefinder control, data bytes: 00:ranging stop, 01:ranging start, 02:single measurement, 03:continuous measurement # define AP_MOUNT_TOPOTEK_ID3CHAR_LRF "LRF" // laser rangefinder control, data bytes: 00:ranging stop, 01:ranging start, 02:single measurement, 03:continuous measurement
# define AP_MOUNT_TOPOTEK_ID3CHAR_PIP "PIP" // set picture-in-picture setting, data bytes: // 00:main only, 01:main+sub, 02:sub+main, 03:sub only, 0A:next # define AP_MOUNT_TOPOTEK_ID3CHAR_PIP "PIP" // set picture-in-picture setting, data bytes: // 00:main only, 01:main+sub, 02:sub+main, 03:sub only, 0A:next
# define AP_MOUNT_TOPOTEK_ID3CHAR_GIMBAL_ATT "GAA" // get gimbal attitude, data bytes: 00:stop, 01:start # define AP_MOUNT_TOPOTEK_ID3CHAR_GIMBAL_ATT "GIA" // get gimbal attitude, data bytes: 00:stop, 01:start
# define AP_MOUNT_TOPOTEK_ID3CHAR_SD_CARD "SDC" // get SD card state, data bytes: 00:get remaining capacity, 01:get total capacity # define AP_MOUNT_TOPOTEK_ID3CHAR_SD_CARD "SDC" // get SD card state, data bytes: 00:get remaining capacity, 01:get total capacity
# define AP_MOUNT_TOPOTEK_ID3CHAR_TIME "UTC" // set time and date, data bytes: HHMMSSDDMMYY # define AP_MOUNT_TOPOTEK_ID3CHAR_TIME "UTC" // set time and date, data bytes: HHMMSSDDMMYY
# define AP_MOUNT_TOPOTEK_ID3CHAR_GET_VERSION "VSN" // get firmware version, data bytes always 00 # define AP_MOUNT_TOPOTEK_ID3CHAR_GET_VERSION "VSN" // get firmware version, data bytes always 00
@ -756,7 +756,7 @@ void AP_Mount_Topotek::read_incoming_packets()
// request gimbal attitude // request gimbal attitude
void AP_Mount_Topotek::request_gimbal_attitude() void AP_Mount_Topotek::request_gimbal_attitude()
{ {
// sample command: #TPUG2wGAA01 // sample command: #TPUG2wGIA01
send_fixedlen_packet(AddressByte::GIMBAL, AP_MOUNT_TOPOTEK_ID3CHAR_GIMBAL_ATT, true, 1); send_fixedlen_packet(AddressByte::GIMBAL, AP_MOUNT_TOPOTEK_ID3CHAR_GIMBAL_ATT, true, 1);
} }
@ -802,7 +802,7 @@ void AP_Mount_Topotek::send_angle_target(const MountTarget& angle_rad)
// calculate and send yaw target // calculate and send yaw target
// sample command #tpUG6wGIY // sample command #tpUG6wGIY
const char* format_str = "%04x%02x"; const char* format_str = "%04X%02X";
const uint8_t speed = 99; const uint8_t speed = 99;
const uint16_t yaw_angle_cd = (uint16_t)constrain_int16(degrees(angle_rad.get_bf_yaw()) * 100, MAX(-18000, _params.yaw_angle_min * 100), MIN(18000, _params.yaw_angle_max * 100)); const uint16_t yaw_angle_cd = (uint16_t)constrain_int16(degrees(angle_rad.get_bf_yaw()) * 100, MAX(-18000, _params.yaw_angle_min * 100), MIN(18000, _params.yaw_angle_max * 100));
@ -863,7 +863,7 @@ void AP_Mount_Topotek::send_rate_target(const MountTarget& rate_rads)
// prepare and send command // prepare and send command
// sample command: #tpUG6wYPR // sample command: #tpUG6wYPR
uint8_t databuff[7]; uint8_t databuff[7];
hal.util->snprintf((char *)databuff, ARRAY_SIZE(databuff), "%02x%02x%02x", yaw_angle_speed, pitch_angle_speed, roll_angle_speed); hal.util->snprintf((char *)databuff, ARRAY_SIZE(databuff), "%02X%02X%02X", yaw_angle_speed, pitch_angle_speed, roll_angle_speed);
send_variablelen_packet(HeaderType::VARIABLE_LEN, AddressByte::GIMBAL, AP_MOUNT_TOPOTEK_ID3CHAR_YPR_RATE, true, databuff, ARRAY_SIZE(databuff)-1); send_variablelen_packet(HeaderType::VARIABLE_LEN, AddressByte::GIMBAL, AP_MOUNT_TOPOTEK_ID3CHAR_YPR_RATE, true, databuff, ARRAY_SIZE(databuff)-1);
} }
@ -1141,7 +1141,7 @@ int16_t AP_Mount_Topotek::hexchar4_to_int16(char high, char mid_high, char mid_l
bool AP_Mount_Topotek::send_fixedlen_packet(AddressByte address, const Identifier id, bool write, uint8_t value) bool AP_Mount_Topotek::send_fixedlen_packet(AddressByte address, const Identifier id, bool write, uint8_t value)
{ {
uint8_t databuff[3]; uint8_t databuff[3];
hal.util->snprintf((char *)databuff, ARRAY_SIZE(databuff), "%02x", value); hal.util->snprintf((char *)databuff, ARRAY_SIZE(databuff), "%02X", value);
return send_variablelen_packet(HeaderType::FIXED_LEN, address, id, write, databuff, ARRAY_SIZE(databuff)-1); return send_variablelen_packet(HeaderType::FIXED_LEN, address, id, write, databuff, ARRAY_SIZE(databuff)-1);
} }

2
libraries/AP_Mount/AP_Mount_Topotek.h
View File

@ -272,7 +272,7 @@ private:
// stores command ID and corresponding member functions that are compared with the command received by the gimbal // stores command ID and corresponding member functions that are compared with the command received by the gimbal
UartCmdFunctionHandler uart_recv_cmd_compare_list[AP_MOUNT_RECV_GIMBAL_CMD_CATEGORIES_NUM] = { UartCmdFunctionHandler uart_recv_cmd_compare_list[AP_MOUNT_RECV_GIMBAL_CMD_CATEGORIES_NUM] = {
{{"GAC"}, &AP_Mount_Topotek::gimbal_angle_analyse}, {{"GIA"}, &AP_Mount_Topotek::gimbal_angle_analyse},
{{"REC"}, &AP_Mount_Topotek::gimbal_record_analyse}, {{"REC"}, &AP_Mount_Topotek::gimbal_record_analyse},
{{"SDC"}, &AP_Mount_Topotek::gimbal_sdcard_analyse}, {{"SDC"}, &AP_Mount_Topotek::gimbal_sdcard_analyse},
{{"LRF"}, &AP_Mount_Topotek::gimbal_dist_info_analyse}, {{"LRF"}, &AP_Mount_Topotek::gimbal_dist_info_analyse},