AP_Mount: Topotek retrieves model name

Co-authored-by: Randy Mackay <rmackay9@yahoo.com>

libraries/AP_Mount/AP_Mount_Topotek.cpp

@@ -35,6 +35,7 @@ extern const AP_HAL::HAL& hal;
 # 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
+# define AP_MOUNT_TOPOTEK_ID3CHAR_GET_MODEL_NAME "PA2"          // get model name, data bytes always 00
 # define AP_MOUNT_TOPOTEK_ID3CHAR_GIMBAL_MODE   "PTZ"           // set gimbal mode, data bytes: 00:stop, 01:up, 02:down, 03:left, 04:right, 05:home position, 06:lock, 07:follow, 08:lock/follow toggle, 09:calibration, 0A:one button down
 # define AP_MOUNT_TOPOTEK_ID3CHAR_YPR_RATE      "YPR"           // set the rate yaw, pitch and roll targets of the gimbal yaw in range -99 ~ +99
 # define AP_MOUNT_TOPOTEK_ID3CHAR_YAW_ANGLE     "GIY"           // set the yaw angle target in the range -150 ~ 150, speed 0 ~ 99 (0.1deg/sec)
@@ -111,6 +112,12 @@ void AP_Mount_Topotek::update()
             request_track_status();
         }
         break;
+    case 8:
+        // get gimbal model name
+        if (!_got_gimbal_model_name) {
+            request_gimbal_model_name();
+        }
+        break;
     }
 
     // change to RC_TARGETING mode if RC input has changed
@@ -510,6 +517,11 @@ void AP_Mount_Topotek::send_camera_information(mavlink_channel_t chan) const
     static uint8_t model_name[32] {};
     const char cam_definition_uri[140] {};
 
+    // copy model name if available
+    if (_got_gimbal_model_name) {
+        strncpy((char*)model_name, (const char*)_model_name, ARRAY_SIZE(model_name));
+    }
+
     // capability flags
     const uint32_t flags = CAMERA_CAP_FLAGS_CAPTURE_VIDEO |
                            CAMERA_CAP_FLAGS_CAPTURE_IMAGE |
@@ -675,8 +687,8 @@ void AP_Mount_Topotek::read_incoming_packets()
         case ParseState::WAITING_FOR_ID1:
         case ParseState::WAITING_FOR_ID2:
         case ParseState::WAITING_FOR_ID3:
-            // sanity check all capital letters.  eg 'GAC'
+            // check all uppercase letters and numbers.  eg 'GAC'
-            if (b >= 'A' && b <= 'Z') {
+            if ((b >= 'A' && b <= 'Z') || (b >= '0' && b <= '9')) {
                 // advance to next state
                 _parser.state = (ParseState)((uint8_t)_parser.state+1);
                 break;
@@ -774,6 +786,13 @@ void AP_Mount_Topotek::request_gimbal_version()
     send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_GET_VERSION, false, 0);
 }
 
+// request gimbal model name
+void AP_Mount_Topotek::request_gimbal_model_name()
+{
+    // sample command: #TPUG2rPA200
+    send_fixedlen_packet(AddressByte::GIMBAL, AP_MOUNT_TOPOTEK_ID3CHAR_GET_MODEL_NAME, false, 0);
+}
+
 // send angle target in radians to gimbal
 void AP_Mount_Topotek::send_angle_target(const MountTarget& angle_rad)
 {
@@ -1072,6 +1091,17 @@ void AP_Mount_Topotek::gimbal_version_analyse()
     _got_gimbal_version = true;
 }
 
+// gimbal model name message analysis
+void AP_Mount_Topotek::gimbal_model_name_analyse()
+{
+    strncpy((char *)_model_name, (const char *)_msg_buff + 10, char_to_hex(_msg_buff[5]));
+
+    // display gimbal model name to user
+    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %s", send_message_prefix, _model_name);
+
+    _got_gimbal_model_name = true;
+}
+
 // calculate checksum
 uint8_t AP_Mount_Topotek::calculate_crc(const uint8_t *cmd, uint8_t len) const
 {

libraries/AP_Mount/AP_Mount_Topotek.h

@@ -27,7 +27,7 @@
 #include <AP_Common/AP_Common.h>
 
 #define AP_MOUNT_TOPOTEK_PACKETLEN_MAX              36          // maximum number of bytes in a packet sent to or received from the gimbal
-#define AP_MOUNT_RECV_GIMBAL_CMD_CATEGORIES_NUM     6           // parse the number of gimbal command types
+#define AP_MOUNT_RECV_GIMBAL_CMD_CATEGORIES_NUM     7           // parse the number of gimbal command types
 
 class AP_Mount_Topotek : public AP_Mount_Backend_Serial
 {
@@ -168,6 +168,9 @@ private:
     // request gimbal version
     void request_gimbal_version();
 
+    // request gimbal model name
+    void request_gimbal_model_name();
+
     // send angle target in radians to gimbal
     void send_angle_target(const MountTarget& angle_rad);
 
@@ -195,6 +198,9 @@ private:
     // gimbal basic information analysis
     void gimbal_version_analyse();
 
+    // gimbal model name message analysis
+    void gimbal_model_name_analyse();
+
     // gimbal distance information analysis
     void gimbal_dist_info_analyse();
 
@@ -229,8 +235,10 @@ private:
     bool _sdcard_status;                                        // memory card status (received from gimbal)
     bool _last_lock;                                            // last lock mode sent to gimbal
     bool _got_gimbal_version;                                   // true if gimbal's version has been received
+    bool _got_gimbal_model_name;                                // true if gimbal's model name has been received
     bool _last_zoom_stop;                                       // true if zoom has been stopped (used to re-send in order to handle lost packets)
     bool _last_focus_stop;                                      // true if focus has been stopped (used to re-sent in order to handle lost packets)
+    uint8_t _model_name[16];                                    // gimbal model name
     uint8_t _sent_time_count;                                   // count of current time messages sent to gimbal
     uint32_t _firmware_ver;                                     // firmware version
     Vector3f _current_angle_rad;                                // current angles in radians received from gimbal (x=roll, y=pitch, z=yaw)
@@ -260,6 +268,7 @@ private:
         {{"LRF"}, &AP_Mount_Topotek::gimbal_dist_info_analyse},
         {{"TRC"}, &AP_Mount_Topotek::gimbal_track_analyse},
         {{"VSN"}, &AP_Mount_Topotek::gimbal_version_analyse},
+        {{"PA2"}, &AP_Mount_Topotek::gimbal_model_name_analyse}
     };
 };