AP_Mount: topotek: Change the type of gimbal angle acquisition

... also convert the lowercase characters in the command to uppercase
ArduPilot · Nov 8, 2024 · e80de49 · e80de49
1 parent 874d268
commit e80de49
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 6 deletions.
diff --git a/libraries/AP_Mount/AP_Mount_Topotek.cpp b/libraries/AP_Mount/AP_Mount_Topotek.cpp
@@ -31,7 +31,7 @@ extern const AP_HAL::HAL& hal;
 # 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_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_TIME          "UTC"           // set time and date, data bytes: HHMMSSDDMMYY
 # define AP_MOUNT_TOPOTEK_ID3CHAR_GET_VERSION   "VSN"           // get firmware version, data bytes always 00
@@ -765,7 +765,7 @@ void AP_Mount_Topotek::read_incoming_packets()
 // 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);
 }
 
@@ -811,7 +811,7 @@ void AP_Mount_Topotek::send_angle_target(const MountTarget& angle_rad)
 
     // calculate and send yaw target
     // sample command #tpUG6wGIY
-    const char* format_str = "%04x%02x";
+    const char* format_str = "%04X%02X";
     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));
 
@@ -872,7 +872,7 @@ void AP_Mount_Topotek::send_rate_target(const MountTarget& rate_rads)
     // prepare and send command
     // sample command: #tpUG6wYPR
     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);
 }
 
@@ -1150,7 +1150,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)
 {
     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);
 }
 

diff --git a/libraries/AP_Mount/AP_Mount_Topotek.h b/libraries/AP_Mount/AP_Mount_Topotek.h
@@ -272,7 +272,7 @@ class AP_Mount_Topotek : public AP_Mount_Backend_Serial
 
     // 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] = {
-        {{"GAC"}, &AP_Mount_Topotek::gimbal_angle_analyse},
+        {{"GIA"}, &AP_Mount_Topotek::gimbal_angle_analyse},
         {{"REC"}, &AP_Mount_Topotek::gimbal_record_analyse},
         {{"SDC"}, &AP_Mount_Topotek::gimbal_sdcard_analyse},
         {{"LRF"}, &AP_Mount_Topotek::gimbal_dist_info_analyse},