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

AP_Mount: Topotek retrieves model name 

Co-authored-by: Randy Mackay <rmackay9@yahoo.com>
This commit is contained in:
xiaozhou 2024-07-16 19:56:19 +09:00 committed by Andrew Tridgell
parent 858aff5f4f
commit 69e4005bb6
2 changed files with 42 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
libraries/AP_Mount/AP_Mount_Topotek.cpp
View File

@ -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'
if (b >= 'A' && b <= 'Z') {
// check all uppercase letters and numbers. eg 'GAC'
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
{

11
libraries/AP_Mount/AP_Mount_Topotek.h
View File

@ -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}
};
};