From c52d386117d23b9a889e779e84b97a5ddc24edee Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Sun, 30 Aug 2026 15:03:16 -0600 Subject: [PATCH 01/19] AP_Networking: accept UDP client replies from a different source port NET_Pn set to UDP_CLIENT connect()'d its socket to the configured destination, which makes the kernel filter incoming packets to match that exact source address *and port*. That's fine for the common case of a device that replies from the same socket it was queried on, but some devices (confirmed against a real Topotek KHP415 gimbal) reply from a different, fixed source port instead - every reply was silently dropped before ArduPilot's own code ever saw it, regardless of NET_Pn config being otherwise correct. For a unicast destination, stop calling connect() and use sendto()/an unconnected recv() instead, checking the source IP ourselves (but not the port) before accepting a packet. Broadcast and multicast destinations keep the original connect()-based path unchanged, since connect() also does necessary setup for them (joining the multicast group via IP_ADD_MEMBERSHIP) unrelated to this fix, and neither is a point-to-point relationship that could hit this problem in the first place. Verified against the real KHP415 (which replies from a fixed but different port than it's queried on) by hand-crafting its wire protocol and sending it from an unconnected socket. Regression-tested against the existing TestLogDownloadMAVProxyNetwork suite (unicast/ multicast/broadcast UDP client, UDP server, TCP client/server) and the full AP_Mount network autotest suite - no regressions. --- libraries/AP_Networking/AP_Networking.h | 8 +++ .../AP_Networking/AP_Networking_port.cpp | 60 +++++++++++++++---- 2 files changed, 55 insertions(+), 13 deletions(-) diff --git a/libraries/AP_Networking/AP_Networking.h b/libraries/AP_Networking/AP_Networking.h index 3be7a5539b5752..9d19fa4423959a 100644 --- a/libraries/AP_Networking/AP_Networking.h +++ b/libraries/AP_Networking/AP_Networking.h @@ -277,6 +277,13 @@ class AP_Networking bool init_buffers(const uint32_t size_rx, const uint32_t size_tx); void thread_create(AP_HAL::MemberProc); + // true if addr (host byte order, as returned by AP_Networking_IPV4::get_uint32() + // or SocketAPM::last_recv_address()) is the broadcast or a multicast address + static bool is_broadcast_or_multicast(uint32_t addr) { + const uint8_t first_octet = (addr >> 24) & 0xFF; + return addr == 0xFFFFFFFF || (first_octet >= 224 && first_octet <= 239); + } + uint32_t txspace() override; void _begin(uint32_t b, uint16_t rxS, uint16_t txS) override; size_t _write(const uint8_t *buffer, size_t size) override; @@ -299,6 +306,7 @@ class AP_Networking uint32_t last_size_rx; bool packetise; bool connected; + bool is_udp_client_unicast; // only meaningful when type == UDP_CLIENT, set in udp_client_loop() uint32_t last_udp_connect_address; uint16_t last_udp_connect_port; bool have_received; diff --git a/libraries/AP_Networking/AP_Networking_port.cpp b/libraries/AP_Networking/AP_Networking_port.cpp index 18bc64592b6071..fb58f5754e1b4e 100644 --- a/libraries/AP_Networking/AP_Networking_port.cpp +++ b/libraries/AP_Networking/AP_Networking_port.cpp @@ -181,11 +181,26 @@ void AP_Networking::Port::udp_client_loop(void) AP::network().startup_wait(); const char *dest = ip.get_str(); - if (!sock->connect(dest, port.get())) { - GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "UDP[%u]: Failed to connect to %s", (unsigned)state.idx, dest); - delete sock; - sock = nullptr; - return; + is_udp_client_unicast = !is_broadcast_or_multicast(ip.get_uint32()); + + if (is_udp_client_unicast) { + // deliberately not calling sock->connect(): a connect()'d UDP socket + // has its incoming packets filtered by the kernel to the exact + // address *and port* it connected to, but some devices reply from a + // different source port than the one they were queried on. + // send_receive() uses sendto() for our fixed destination instead, + // and checks the source IP itself (ignoring port) on receive + } else { + // broadcast/multicast: connect() also joins the multicast group + // (IP_ADD_MEMBERSHIP) when the destination is one, which we must + // not skip - and neither of these targets are point-to-point, so + // they don't have the mismatched-reply-port problem above + if (!sock->connect(dest, port.get())) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "UDP[%u]: Failed to connect to %s", (unsigned)state.idx, dest); + delete sock; + sock = nullptr; + return; + } } GCS_SEND_TEXT(MAV_SEVERITY_INFO, "UDP[%u]: connected to %s:%u", (unsigned)state.idx, dest, unsigned(port.get())); @@ -339,15 +354,28 @@ bool AP_Networking::Port::send_receive(void) return false; } if (ret > 0) { - WITH_SEMAPHORE(sem); - readbuffer->write(buf, ret); + bool accept = true; + if (type == NetworkPortType::UDP_CLIENT && is_udp_client_unicast) { + // our socket isn't connect()'d for a unicast destination (see + // udp_client_loop()), so the kernel doesn't filter incoming + // packets for us - check the source IP ourselves. Deliberately + // not checking the source port: some devices reply from a + // different port than the one they were queried on + uint32_t src_addr = 0; + uint16_t src_port = 0; + accept = sock->last_recv_address(src_addr, src_port) && (src_addr == ip.get_uint32()); + } + if (accept) { + WITH_SEMAPHORE(sem); + readbuffer->write(buf, ret); - // Cant track dropped read packets because we only read in what there is space for - // The socket buffer becomes full and data is lost there - rx_stats_bytes += ret; + // Cant track dropped read packets because we only read in what there is space for + // The socket buffer becomes full and data is lost there + rx_stats_bytes += ret; - active = true; - have_received = true; + active = true; + have_received = true; + } } } @@ -410,8 +438,14 @@ bool AP_Networking::Port::send_receive(void) if(last_udp_connect_address != 0 && last_udp_connect_port != 0) { ret = sock->sendto(buf, n, last_udp_connect_address, last_udp_connect_port); } + } else if (type == NetworkPortType::UDP_CLIENT && is_udp_client_unicast) { + // a unicast UDP Client also uses sendto rather than a connect()'d + // send() - see udp_client_loop() and the receive-side comment + // above for why + ret = sock->sendto(buf, n, ip.get_uint32(), port.get()); } else { - // TCP Server and Client and UDP Client use send + // TCP Server and Client, and a broadcast/multicast UDP Client + // (which is connect()'d - see udp_client_loop()), use send() ret = sock->send(buf, n); } From 300176fa49aa015a5fc8565c6e68d0bf6d20302d Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Mon, 24 Aug 2026 08:41:20 -0600 Subject: [PATCH 02/19] AP_Mount: add SkyDroid gimbal driver (C11, C13) Adds AP_Mount_SkyDroid, a new backend for SkyDroid's "TOP protocol" gimbal camera family (MNT1_TYPE=15), communicating over UDP using SkyDroid's own #TP/#tp packet framing. One driver covers the whole family (C11 2-axis, C13 3-axis) - SkyDroid have directly confirmed the gimbal-control commands are identical across models, and the C13's extra features (infrared thermal imaging, laser ranging) aren't used by this driver. Key protocol findings, confirmed on real hardware: - Only the individual-axis speed commands (GSY/GSP) actually move the gimbal - the combined and absolute-angle commands are silently ignored by the firmware. Both rate control and (via a closed-loop P-controller using the gimbal's own attitude feedback) angle control are built on GSY/GSP alone. The real-world speed of these commands (deg/s per wire LSB value) was measured directly from a dataflash log of a sustained full-speed hold and confirmed against SkyDroid's own documentation, after an earlier measurement attempt had been wrong by a factor of 16. - Roll is self-stabilized by the gimbal firmware with no control command on any model - the protocol document describes roll commands (GAR/GSR) but they aren't implemented in firmware. The driver reports has_roll_control() == false and never sends them, while still parsing roll from the gimbal's own attitude reports for telemetry. - Retract/neutral use the gimbal's own dedicated one-shot "center" command rather than the closed-loop controller, so centering doesn't depend on attitude feedback ever having arrived. - The gimbal has no RTC of its own and defaults to 1970-01-01 without the driver periodically pushing UTC time to it. - The connected model name and firmware version are queried and reported through CAMERA_INFORMATION, purely informationally - no control decision depends on either. Tested extensively on real C11 hardware, including RC and MAVLink pitch/yaw control, picture/video capture, digital zoom, and the retract/neutral center command. --- libraries/AP_Mount/AP_Mount.cpp | 10 + libraries/AP_Mount/AP_Mount.h | 4 + libraries/AP_Mount/AP_Mount_Params.cpp | 2 +- libraries/AP_Mount/AP_Mount_SkyDroid.cpp | 850 +++++++++++++++++++++++ libraries/AP_Mount/AP_Mount_SkyDroid.h | 338 +++++++++ libraries/AP_Mount/AP_Mount_config.h | 4 + 6 files changed, 1207 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_Mount/AP_Mount_SkyDroid.cpp create mode 100644 libraries/AP_Mount/AP_Mount_SkyDroid.h diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 6b66439e01b5d8..a751503827cd28 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -20,6 +20,7 @@ #include "AP_Mount_Topotek.h" #include "AP_Mount_CADDX.h" #include "AP_Mount_XFRobot.h" +#include "AP_Mount_SkyDroid.h" #include #include #include @@ -188,6 +189,15 @@ void AP_Mount::init() serial_instance++; break; #endif // HAL_MOUNT_XFROBOT_ENABLED + +#if HAL_MOUNT_SKYDROID_ENABLED + // check for SkyDroid gimbal + case Type::SkyDroid: + _backends[instance] = NEW_NOTHROW AP_Mount_SkyDroid(*this, _params[instance], instance, serial_instance); + _num_instances++; + serial_instance++; + break; +#endif // HAL_MOUNT_SKYDROID_ENABLED } // init new instance diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 2b601562b67700..83fdca535c72bf 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -50,6 +50,7 @@ class AP_Mount_Viewpro; class AP_Mount_Topotek; class AP_Mount_CADDX; class AP_Mount_XFRobot; +class AP_Mount_SkyDroid; /* This is a workaround to allow the MAVLink backend access to the @@ -129,6 +130,9 @@ class AP_Mount #if HAL_MOUNT_XFROBOT_ENABLED XFRobot = 14, /// XFRobot gimbal using a custom serial protocol #endif +#if HAL_MOUNT_SKYDROID_ENABLED + SkyDroid = 15, /// SkyDroid gimbal using a custom serial protocol +#endif // HAL_MOUNT_SKYDROID_ENABLED }; // init - detect and initialise all mounts diff --git a/libraries/AP_Mount/AP_Mount_Params.cpp b/libraries/AP_Mount/AP_Mount_Params.cpp index 317999855a791c..5ed2be0985ed2d 100644 --- a/libraries/AP_Mount/AP_Mount_Params.cpp +++ b/libraries/AP_Mount/AP_Mount_Params.cpp @@ -10,7 +10,7 @@ const AP_Param::GroupInfo AP_Mount_Params::var_info[] = { // @DisplayName: Mount Type // @Description: Mount Type // @SortValues: AlphabeticalZeroAtTop - // @Values: 0:None, 1:Servo, 2:3DR Solo, 3:Alexmos Serial, 4:SToRM32 MAVLink, 5:SToRM32 Serial, 6:MAVLink (Gremsy/AVT), 7:BrushlessPWM, 8:Siyi, 9:Scripting, 10:Xacti, 11:Viewpro, 12:Topotek, 13:CADDX, 14:XFRobot + // @Values: 0:None, 1:Servo, 2:3DR Solo, 3:Alexmos Serial, 4:SToRM32 MAVLink, 5:SToRM32 Serial, 6:MAVLink (Gremsy/AVT), 7:BrushlessPWM, 8:Siyi, 9:Scripting, 10:Xacti, 11:Viewpro, 12:Topotek, 13:CADDX, 14:XFRobot, 15:SkyDroid // @RebootRequired: True // @User: Standard AP_GROUPINFO_FLAGS("_TYPE", 1, AP_Mount_Params, type, 0, AP_PARAM_FLAG_ENABLE), diff --git a/libraries/AP_Mount/AP_Mount_SkyDroid.cpp b/libraries/AP_Mount/AP_Mount_SkyDroid.cpp new file mode 100644 index 00000000000000..9afbf3bb39c4de --- /dev/null +++ b/libraries/AP_Mount/AP_Mount_SkyDroid.cpp @@ -0,0 +1,850 @@ +#include "AP_Mount_config.h" + +#if HAL_MOUNT_SKYDROID_ENABLED + +#include "AP_Mount_SkyDroid.h" + +#include +#include +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +#define AP_MOUNT_SKYDROID_UPDATE_INTERVAL_MS 100 // resend angle or rate targets, and push our attitude, at this interval +#define AP_MOUNT_SKYDROID_HEALTH_TIMEOUT_MS 1000 // timeout for health (based on attitude reports from gimbal) +#define AP_MOUNT_SKYDROID_PACKETLEN_MIN 12 // packet length not including the data segment +#define AP_MOUNT_SKYDROID_DATALEN_MAX (AP_MOUNT_SKYDROID_PACKETLEN_MAX - AP_MOUNT_SKYDROID_PACKETLEN_MIN) // data segment len can be no more than this +#define AP_MOUNT_SKYDROID_ATTITUDE_RATE_HZ 50 // rate we ask the gimbal to stream its attitude to us (matches the 50hz rate AP_Mount::update() is actually called at; doc allows up to 100hz) + +// byte offsets within a received packet - see the packet-format table in this file's +// header comment. Every packet shares this same preamble layout regardless of command; +// only the data segment's own internal layout (if any) differs per command, and is +// documented at each parse function that reads one +#define AP_MOUNT_SKYDROID_MSGOFS_DATALEN 5 // data length, 1 ASCII hex nibble +#define AP_MOUNT_SKYDROID_MSGOFS_ID 7 // 3-character command identifier +#define AP_MOUNT_SKYDROID_MSGOFS_DATA 10 // start of the command-specific data segment + +// 3 character identifiers +#define AP_MOUNT_SKYDROID_ID3CHAR_GIMBAL_MODE "PTZ" // discrete gimbal control, data bytes: 00:stop, 01:up, 02:down, 03:left, 04:right, 05:center, 06:follow, 07:lock head. Only the follow/lock codes (06/07 - see set_gimbal_lock()) and center (05 - see send_center_command()) are used +#define AP_MOUNT_SKYDROID_ID3CHAR_SPEED_YAW "GSY" // individual-axis yaw rate control, data bytes: signed 8bit hex. The only command confirmed to move yaw on real hardware (GAM/GSM/GAY all silently ignored); its sign is also inverted vs the doc - see send_target_rates() +#define AP_MOUNT_SKYDROID_ID3CHAR_SPEED_PITCH "GSP" // individual-axis pitch rate control, data bytes: signed 8bit hex. Confirmed functional on real hardware, sign matches the doc +// matches the protocol doc's and SkyDroid's own RCSDK's documented 0.5deg/s per LSB +// (max speed +/-63.5 deg/s == 127 * 0.5). An earlier real-hardware measurement of +// this driver had it at 1/16th of this (0.03125) - that measurement was wrong, not +// the documentation: a real-C11 dataflash log of a sustained full-deflection GSY +// rate-mode command (MNT1_RC_RATE=90, so comfortably saturating to the max LSB +// value of 127) measured yaw moving at a clean, consistent ~64deg/s across three +// independent full-speed sweeps (e.g. a 179deg sweep in exactly 2.80s = 63.97deg/s) +// - matching 0.5deg/s/LSB almost exactly, not 0.03125 +#define AP_MOUNT_SKYDROID_AXIS_DPS_PER_LSB 0.5f +#define AP_MOUNT_SKYDROID_AXIS_MAX_DPS (127 * AP_MOUNT_SKYDROID_AXIS_DPS_PER_LSB) +// every other 3-character command ID is used in exactly one place, so is a plain +// string literal at its own call site instead of a macro defined here + +#define AP_MOUNT_SKYDROID_DEBUG 0 +#define debug(fmt, args ...) do { if (AP_MOUNT_SKYDROID_DEBUG) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SkyDroid: " fmt, ## args); } } while (0) + +const char* AP_Mount_SkyDroid::send_message_prefix = "Mount: SkyDroid"; + +// update mount position - should be called periodically +void AP_Mount_SkyDroid::update() +{ + AP_Mount_Backend::update(); + + // exit immediately if not initialised + if (!_initialised) { + return; + } + + // reading incoming packets from gimbal + read_incoming_packets(); + + // update based on mount mode, and send target angles or rates depending on the + // target type. Deliberately NOT gated by the 10hz throttle below - AP_Mount::update() + // is actually called at 50hz (see ArduPlane/ArduCopter's SCHED_TASK entry), and the + // closed-loop angle control in send_target_angles() benefits from running its + // P-controller at the full rate rather than being throttled down further + update_mnt_target(); + send_target_to_gimbal(); + + // everything below updates at 10hz + uint32_t now_ms = AP_HAL::millis(); + if ((now_ms - _last_req_current_info_ms) < AP_MOUNT_SKYDROID_UPDATE_INTERVAL_MS) { + return; + } + _last_req_current_info_ms = now_ms; + + // push our own attitude to the gimbal + send_attitude_to_gimbal(); + + // calls below here called at 1hz + _last_req_step++; + if (_last_req_step >= (uint8_t)ReqStep::NUM_STEPS) { + _last_req_step = 0; + } + switch ((ReqStep)_last_req_step) { + case ReqStep::VERSION: + // get gimbal firmware version. Worth retrying until answered rather than + // asking once: the version determines which command sets the gimbal actually + // implements (SkyDroid's RCSDK gates its combined yaw+pitch call on firmware + // >= 0.5, and we've found the combined/absolute-angle commands dead on the + // firmware we have), so it's the single most useful thing to know when + // diagnosing a gimbal that connects but won't move + if (!_got_gimbal_version) { + request_gimbal_version(); + } + break; + case ReqStep::TIME_SYNC: + // (re)send current UTC time so photos/videos are timestamped correctly - see + // send_time_sync() for why this is needed and why it's resent periodically + send_time_sync(); + break; + case ReqStep::ATTITUDE_ENABLE: + // (re)request gimbal attitude streaming. harmless to resend if already enabled, + // and guards against the enable packet being lost over UDP + request_gimbal_attitude(); + break; + case ReqStep::MODEL: + // get the model name. Purely informational (reported to the GCS via + // CAMERA_INFORMATION) - no control decision depends on it, since SkyDroid have + // confirmed the gimbal-control commands are identical across models. That's + // why a slow answer here is harmless and 1hz is plenty: "MOD" has been seen to + // take minutes to reply on real hardware, and control works throughout + if (!_got_model_name) { + request_gimbal_model(); + } + break; + case ReqStep::SDCARD: + // request memory card information + request_gimbal_sdcard_info(); + break; + case ReqStep::ATTITUDE_ACCEPT: + // (re)enable gimbal to accept our attitude pushes + send_attitude_enable(); + break; + default: + // spare steps (5, 7, 8, 9) - nothing to do + break; + } +} + +// return true if healthy +bool AP_Mount_SkyDroid::healthy() const +{ + // exit immediately if not initialised + if (!_initialised) { + return false; + } + + // unhealthy if attitude information not received recently + const uint32_t last_current_angle_ms = _last_current_angle_ms; + return (AP_HAL::millis() - last_current_angle_ms < AP_MOUNT_SKYDROID_HEALTH_TIMEOUT_MS); +} + +// take a picture. returns true on success +bool AP_Mount_SkyDroid::take_picture() +{ + // exit immediately if not initialised + if (!_initialised) { + return false; + } + + // exit immediately if the memory card is abnormal + if (!_sdcard_healthy) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "%s SD card error", send_message_prefix); + return false; + } + + // "CAP": take picture, data bytes: 01. sample command: #TPUD2wCAP01 + return send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, "CAP", true, 1); +} + +// start or stop video recording. returns true on success +// set start_recording = true to start record, false to stop recording +bool AP_Mount_SkyDroid::record_video(bool start_recording) +{ + // exit immediately if not initialised + if (!_initialised) { + return false; + } + + // exit immediately if the memory card is abnormal + if (!_sdcard_healthy) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "%s SD card error", send_message_prefix); + return false; + } + + // "REC": record video, data bytes: 00:stop, 01:start. sample command: #TPUD2wREC01 + if (send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, "REC", true, start_recording ? 1 : 0)) { + // SkyDroid does not push unsolicited recording-state changes to us so track our own request locally + _recording = start_recording; + return true; + } + return false; +} + +// set zoom specified as a rate. SkyDroid's digital zoom is stepped, not continuous: +// there is no "stop" data value, only single-shot zoom-in/zoom-out pulses +bool AP_Mount_SkyDroid::set_zoom(ZoomType zoom_type, float zoom_value) +{ + // exit immediately if not initialised + if (!_initialised) { + return false; + } + + // only rate based zoom is supported + if (zoom_type != ZoomType::RATE) { + return false; + } + + // zero rate has no corresponding command so treat as a successful no-op + if (is_zero(zoom_value)) { + return true; + } + + // "DZM": digital zoom, data bytes: 0A:zoom+ (single step), 0B:zoom- (single step). + // sample command: #TPUM2wDZM0A65 + const uint8_t zoom_cmd = (zoom_value < 0) ? 0x0B : 0x0A; // 0x0B: zoom-, 0x0A: zoom+ + return send_fixedlen_packet(AddressByte::LENS, "DZM", true, zoom_cmd); +} + +// send camera settings message to GCS +void AP_Mount_SkyDroid::send_camera_settings(mavlink_channel_t chan) const +{ + // exit immediately if not initialised + if (!_initialised) { + return; + } + + // send CAMERA_SETTINGS message + mavlink_msg_camera_settings_send( + chan, + AP_HAL::millis(), // time_boot_ms + _recording ? CAMERA_MODE_VIDEO : CAMERA_MODE_IMAGE, // camera mode (0:image, 1:video, 2:image survey) + NaNf, // zoomLevel float, percentage from 0 to 100, NaN if unknown + NaNf); // focusLevel float, percentage from 0 to 100, NaN if unknown +} + +// get attitude as a quaternion. returns true on success +bool AP_Mount_SkyDroid::get_attitude_quaternion(Quaternion& att_quat) +{ + // fail while we've never actually received a "GAC" attitude report - otherwise + // callers (e.g. GIMBAL_DEVICE_ATTITUDE_STATUS reporting) would be given a + // fabricated (0,0,0) attitude as if it were real data. _last_current_angle_ms + // is zero-initialised and only ever set by gimbal_angle_analyse() on receipt of + // a real "GAC" packet, so this is the same "have we ever heard from the gimbal" + // signal healthy() uses for its own timeout check + if (_last_current_angle_ms == 0) { + return false; + } + // x=roll (always zero on models with no roll axis, e.g. C11), y=pitch, z=yaw + att_quat.from_euler(_current_angle_rad.x, _current_angle_rad.y, _current_angle_rad.z); + return true; +} + +// reading incoming packets from gimbal and confirm they are of the correct format +void AP_Mount_SkyDroid::read_incoming_packets() +{ + // check for bytes on the serial port + const uint16_t nbytes = MIN(_uart->available(), 1024U); + if (nbytes == 0) { + return; + } + + // flag to allow cases below to reset parser state + bool reset_parser = false; + + // process bytes received + for (uint16_t i = 0; i < nbytes; i++) { + uint8_t b; + if (!_uart->read(b)) { + continue; + } + + // add latest byte to buffer + _msg_buff[_msg_buff_len++] = b; + + // protect against overly long messages + if (_msg_buff_len >= AP_MOUNT_SKYDROID_PACKETLEN_MAX) { + reset_parser = true; + } + + // process byte depending upon current state + switch (_parser.state) { + + case ParseState::WAITING_FOR_HEADER1: + if (b == '#') { + _parser.state = ParseState::WAITING_FOR_HEADER2; + break; + } + reset_parser = true; + break; + + case ParseState::WAITING_FOR_HEADER2: + // 't'/'T' (and 'p'/'P' below) distinguish HeaderType::VARIABLE_LEN from + // FIXED_LEN - meaningful on transmit (see send_variablelen_packet()), but + // deliberately not tracked here on receive: the packet's own Data_Len + // nibble is authoritative regardless of which header case was used to + // send it, so the parser has no need to remember which one it saw + if (b == 't' || b == 'T') { + _parser.state = ParseState::WAITING_FOR_HEADER3; + break; + } + reset_parser = true; + break; + + case ParseState::WAITING_FOR_HEADER3: + if (b == 'p' || b == 'P') { + _parser.state = ParseState::WAITING_FOR_ADDR1; + break; + } + reset_parser = true; + break; + + case ParseState::WAITING_FOR_ADDR1: + case ParseState::WAITING_FOR_ADDR2: + if (b == 'U' || b == 'M' || b == 'D' || b == 'G') { + // advance to next state + _parser.state = (ParseState)((uint8_t)_parser.state+1); + break; + } + reset_parser = true; + break; + + case ParseState::WAITING_FOR_DATALEN: { + // sanity check data length + uint8_t data_len; + if (hex_char_to_nibble(b, data_len) && data_len <= AP_MOUNT_SKYDROID_DATALEN_MAX) { + _parser.data_len = data_len; + _parser.state = ParseState::WAITING_FOR_CONTROL; + break; + } + reset_parser = true; + break; + } + + case ParseState::WAITING_FOR_CONTROL: + // r or w + if (b == 'r' || b == 'w') { + _parser.state = ParseState::WAITING_FOR_ID1; + break; + } + reset_parser = true; + break; + + case ParseState::WAITING_FOR_ID1: + case ParseState::WAITING_FOR_ID2: + case ParseState::WAITING_FOR_ID3: + // 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; + } + reset_parser = true; + break; + + case ParseState::WAITING_FOR_DATA: { + // normally hex numbers in char form (e.g. '0A') + const uint8_t data_bytes_received = _msg_buff_len - (AP_MOUNT_SKYDROID_PACKETLEN_MIN - 2); + + // sanity check to protect against programming errors + if (data_bytes_received > AP_MOUNT_SKYDROID_DATALEN_MAX) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + reset_parser = true; + break; + } + + // advance parser state once expected number of bytes have been received + if (data_bytes_received == _parser.data_len) { + _parser.state = ParseState::WAITING_FOR_CRC_LOW; + } + break; + } + + case ParseState::WAITING_FOR_CRC_LOW: + _parser.state = ParseState::WAITING_FOR_CRC_HIGH; + break; + + case ParseState::WAITING_FOR_CRC_HIGH: + // this is the last byte in the message so reset the parser + reset_parser = true; + + // sanity check to protect against programming errors + if (_msg_buff_len < AP_MOUNT_SKYDROID_PACKETLEN_MIN) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + break; + } + + // calculate and check CRC + const uint8_t crc_value = calculate_crc(_msg_buff, _msg_buff_len - 2); + const char crc_char1 = hex2char((crc_value >> 4) & 0x0f); + const char crc_char2 = hex2char((crc_value) & 0x0f); + if (crc_char1 != _msg_buff[_msg_buff_len - 2] || crc_char2 != _msg_buff[_msg_buff_len-1]) { + debug("CRC expected:%x got:%c%c", (int)crc_value, crc_char1, crc_char2); + break; + } + + // CRC is OK, dispatch on the 3-character command ID to the function that + // consumes that message + const char *msg_id = (const char*)&_msg_buff[AP_MOUNT_SKYDROID_MSGOFS_ID]; + if (strncmp(msg_id, "GAC", 3) == 0) { + gimbal_angle_analyse(); + } else if (strncmp(msg_id, "REC", 3) == 0) { + gimbal_record_analyse(); + } else if (strncmp(msg_id, "SDC", 3) == 0) { + gimbal_sdcard_analyse(); + } else if (strncmp(msg_id, "VER", 3) == 0) { + gimbal_version_analyse(); + } else if (strncmp(msg_id, "MOD", 3) == 0) { + gimbal_model_analyse(); + } + } + + // handle reset of parser + if (reset_parser) { + _parser.state = ParseState::WAITING_FOR_HEADER1; + _msg_buff_len = 0; + reset_parser = false; + } + } +} + +// request gimbal to (re)start sending us attitude at 10hz +void AP_Mount_SkyDroid::request_gimbal_attitude() +{ + // "GAA": enable/disable gimbal->us attitude streaming, data bytes: 00:off, + // 01-64:rate in Hz. sample command: #TPUG2wGAA0A + send_fixedlen_packet(AddressByte::GIMBAL, "GAA", true, AP_MOUNT_SKYDROID_ATTITUDE_RATE_HZ); +} + +// request gimbal memory card information +void AP_Mount_SkyDroid::request_gimbal_sdcard_info() +{ + // "SDC": get SD card state, data bytes: 00 to query. sample command: #TPUD2rSDC00 + send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, "SDC", false, 0); +} + +// request gimbal version +void AP_Mount_SkyDroid::request_gimbal_version() +{ + // "VER": get firmware version, data bytes always 00. sample command: #TPUD2rVER00 + send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, "VER", false, 0); +} + +// request gimbal model name (e.g. "C11", "C13") +void AP_Mount_SkyDroid::request_gimbal_model() +{ + // "MOD": get model name (e.g. "C11"), data bytes always 00. sample command: #TPUD2rMOD00 + send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, "MOD", false, 0); +} + +// send current UTC date/time to the gimbal so photos/videos are timestamped correctly. +// Confirmed on real hardware that the camera has no RTC of its own and defaults to +// 1970-01-01 without this. Uses UTC since ArduPilot has no local timezone concept - +// this means driver-triggered captures' timestamps will differ from ones taken via +// SkyDroid's own app (which uses the connected device's local time) by the local UTC +// offset. Returns false (without sending) if the vehicle doesn't yet have a valid +// time source (e.g. GPS not locked) +bool AP_Mount_SkyDroid::send_time_sync() +{ + uint16_t year, ms; + uint8_t month, day, hour, min, sec; + if (!AP::rtc().get_date_and_time_utc(year, month, day, hour, min, sec, ms)) { + return false; + } + + // "TIM": set current time, data bytes: hhmmss.ccDDMMYY (15 ASCII chars, + // cc=hundredths of a second). Confirmed on real hardware that the camera has no + // RTC of its own and defaults to 1970-01-01 without this. + // sample command: #tpUDFwTIM142832.00031218 (2018-12-03 14:28:32.00) - data is 15 + // ASCII chars: hhmmss.ccDDMMYY, cc=hundredths of a second, YY=2-digit year + uint8_t databuff[16]; + hal.util->snprintf((char*)databuff, ARRAY_SIZE(databuff), "%02u%02u%02u.%02u%02u%02u%02u", + hour, min, sec, (unsigned)((ms / 10) % 100), + day, month, (unsigned)(year % 100)); + return send_variablelen_packet(HeaderType::VARIABLE_LEN, AddressByte::SYSTEM_AND_IMAGE, "TIM", true, databuff, ARRAY_SIZE(databuff)-1); +} + +// (re)enable the gimbal to accept our attitude pushes +bool AP_Mount_SkyDroid::send_attitude_enable() +{ + // "FAE": enable/disable us->gimbal attitude streaming, data bytes: 00:off, 01:on. + // sample command: #TPUG2wFAE01 + return send_fixedlen_packet(AddressByte::GIMBAL, "FAE", true, 1); +} + +// send our current attitude to the gimbal +bool AP_Mount_SkyDroid::send_attitude_to_gimbal() +{ + const int16_t yaw_cd = wrap_180_cd((int32_t)(AP::ahrs().get_yaw_deg() * 100)); + const int16_t pitch_cd = (int16_t)(AP::ahrs().get_pitch_deg() * 100); + const int16_t roll_cd = (int16_t)(AP::ahrs().get_roll_deg() * 100); + + // 1: fixed-wing, 0: copter/hover - this is SkyDroid's own documented field for + // this command, and APM_BUILD_TYPE(APM_BUILD_ArduPlane) is the obvious compile-time + // proxy for it, but we don't actually know what it changes in the gimbal's own + // firmware, or whether that proxy is the right one (e.g. a Plane holding/loitering + // isn't continuously moving forward either). Raised with SkyDroid to find out what + // this bit actually affects before assuming a "more correct" runtime check would + // really be better rather than just differently wrong + const bool fixed_wing = APM_BUILD_TYPE(APM_BUILD_ArduPlane); + + // "FAI": our attitude sent to gimbal, data bytes: yaw+pitch+roll (4hex each, + // 0.01deg) + mode (1:fixed-wing, 0:hover). sample command: #tpUG0EwFAI + uint8_t databuff[15]; + hal.util->snprintf((char*)databuff, ARRAY_SIZE(databuff), "%04X%04X%04X%02X", + (uint16_t)yaw_cd, (uint16_t)pitch_cd, (uint16_t)roll_cd, fixed_wing ? 1 : 0); + return send_variablelen_packet(HeaderType::VARIABLE_LEN, AddressByte::GIMBAL, "FAI", true, databuff, ARRAY_SIZE(databuff)-1); +} + +// send angle target in radians to gimbal, by closing the loop ourselves using the +// gimbal's own "GAC" attitude feedback and driving GSY/GSP as the rate actuator. +// There is no absolute-angle command that works on this hardware - GAM/GAY/GAP are +// all silently ignored (see this file's header comment) +void AP_Mount_SkyDroid::send_target_angles(const MountAngleTarget& angle_rad) +{ + // set gimbal's lock state (follow the body-frame target) + set_gimbal_lock(false); + + // clamp to the configured MNT1_YAW/PITCH_MIN/MAX range - AP_Mount's frontend does + // not clamp the target itself before calling us. Roll is deliberately absent: + // the gimbal self-stabilizes roll and offers no way to command it (see + // has_roll_control()) + const float yaw_target_rad = radians(constrain_float(degrees(angle_rad.get_bf_yaw()), + _params.yaw_angle_min, _params.yaw_angle_max)); + const float pitch_target_rad = radians(constrain_float(degrees(angle_rad.pitch), + _params.pitch_angle_min, _params.pitch_angle_max)); + + // simple P-controller driving GSY/GSP as the rate actuator, using the GAC + // attitude feedback already parsed by gimbal_angle_analyse(). + // + // Confirmed on real C11 hardware that kP=2.0 sustains a continuous limit-cycle + // oscillation on both axes: at that gain, full-scale rate is reached by just + // ~2deg of error, so the actuator was being driven at max speed for almost any + // real excursion - combined with the real feedback's lag (GAC round-trip plus + // the gimbal's own mechanical response), a fast-reacting/early-saturating + // P-controller overshoots and re-corrects indefinitely instead of settling. + // + // kP=1.0 was chosen (before AP_MOUNT_SKYDROID_AXIS_DPS_PER_LSB's real value was + // known - see that constant's comment) to saturate at ~4deg of error + // (AP_MOUNT_SKYDROID_AXIS_MAX_DPS/kP), staying clear of the ~2deg real-hardware + // oscillation zone confirmed below while converging fast enough to meet + // mount_test_body's tightest check (test_mount_rc_targetting()'s hardcoded + // 0.1deg tolerance). Now that AXIS_MAX_DPS is 16x larger (0.5, not 0.03125 + // deg/s per LSB), this saturates at ~63.5deg instead - even further from the + // 2deg danger zone, so no less safe, but the "~4deg" figure below is no longer + // accurate to the number, just the reasoning. NOT YET RE-VALIDATED ON REAL + // HARDWARE - the original oscillation was only ever found via real-hardware + // testing, not SITL, so confirm this still settles cleanly (no hunting/dither) + // on the real C11 before relying on this + constexpr float kP = 1.0; // (deg/s of rate command) per (deg of angle error) + // + // The deadzone below (stop correcting entirely once close, rather than tapering + // to an ever-smaller command) guarantees a clean stop rather than dither once + // within range - its WIDTH is a separate knob from kP, and was originally set to + // 2.0deg for margin without much thought, wider than mount_test_body's autotest + // tolerances and hence a real bug, not a hardware ceiling. 0.05deg keeps margin + // under the 0.1deg check while staying a "stop dead" cutoff (not tapering). + // + // IMPORTANT: this deadzone is no longer the binding constraint it was designed + // to be. GSY/GSP's wire value is a quantized 8-bit signed LSB - see + // AP_MOUNT_SKYDROID_AXIS_DPS_PER_LSB, now known to be 0.5deg/s per LSB (not the + // 0.03125 this was tuned against) - so kP*error now rounds to 0 LSB at + // send_axis_rate() for any error below ~0.25deg (half an LSB step at this kP), + // which is COARSER than the 0.05deg deadzone below and than the 0.1deg test + // tolerance this was tuned to meet. In other words: the actuator's real, + // confirmed resolution floor may no longer be fine enough to pass + // test_mount_rc_targetting() at all via this fixed-gain approach, regardless of + // deadzone or kP tuning - re-run the autotest and see before assuming this still + // passes + constexpr float deadzone_deg = 0.05; // stop correcting once within this many degrees + const float yaw_error_deg = degrees(wrap_PI(yaw_target_rad - _current_angle_rad.z)); + const float pitch_error_deg = degrees(pitch_target_rad - _current_angle_rad.y); + const float yaw_rate_dps = (fabsf(yaw_error_deg) <= deadzone_deg) ? 0.0f : + constrain_float(yaw_error_deg * kP, + -AP_MOUNT_SKYDROID_AXIS_MAX_DPS, + AP_MOUNT_SKYDROID_AXIS_MAX_DPS); + const float pitch_rate_dps = (fabsf(pitch_error_deg) <= deadzone_deg) ? 0.0f : + constrain_float(pitch_error_deg * kP, + -AP_MOUNT_SKYDROID_AXIS_MAX_DPS, + AP_MOUNT_SKYDROID_AXIS_MAX_DPS); + + // GSY's sign is inverted vs AP_Mount's convention (see send_axis_rate below) + send_axis_rate(AP_MOUNT_SKYDROID_ID3CHAR_SPEED_YAW, -yaw_rate_dps); + send_axis_rate(AP_MOUNT_SKYDROID_ID3CHAR_SPEED_PITCH, pitch_rate_dps); +} + +// send rate target in rad/s to gimbal, directly via GSY/GSP - the only commands +// confirmed to move this hardware (GSM is silently ignored) +void AP_Mount_SkyDroid::send_target_rates(const MountRateTarget& rate_rads) +{ + // set gimbal's lock state if it has changed + set_gimbal_lock(rate_rads.yaw_is_ef); + + // GSY's sign is inverted vs AP_Mount's convention (confirmed on real hardware: a + // positive GSY value moves yaw LEFT, not right) - negate here so callers of this + // function keep using AP_Mount's normal yaw-right-positive convention. GSP's + // sign matches AP_Mount's convention (pitch-up-positive) so is passed straight + // through. rate_rads.roll is deliberately ignored - the gimbal self-stabilizes + // roll and offers no way to command it (see has_roll_control()) + send_axis_rate(AP_MOUNT_SKYDROID_ID3CHAR_SPEED_YAW, -degrees(rate_rads.yaw)); + send_axis_rate(AP_MOUNT_SKYDROID_ID3CHAR_SPEED_PITCH, degrees(rate_rads.pitch)); +} + +// send a single-axis rate command (GSY or GSP) for rate_dps, converted to the wire's +// signed 8bit LSB units using the real-world calibrated scale. Caller is responsible +// for any axis-specific sign compensation (see send_target_rates() above) +void AP_Mount_SkyDroid::send_axis_rate(const Identifier id, float rate_dps) +{ + const int8_t rate_lsb = constrain_int16(roundf(rate_dps / AP_MOUNT_SKYDROID_AXIS_DPS_PER_LSB), -127, 127); + send_fixedlen_packet(AddressByte::GIMBAL, id, true, (uint8_t)rate_lsb); +} + +// attitude information analysis of gimbal (arrives as "GAC" in response to our "GAA" enable request) +void AP_Mount_SkyDroid::gimbal_angle_analyse() +{ + // consume current angles. data is yaw, pitch, roll in that order, each 4 hex chars, 0.01deg units + uint32_t yaw_raw, pitch_raw, roll_raw; + if (!hex_chars_to_uint32((const char*)&_msg_buff[AP_MOUNT_SKYDROID_MSGOFS_DATA], 4, yaw_raw) || + !hex_chars_to_uint32((const char*)&_msg_buff[AP_MOUNT_SKYDROID_MSGOFS_DATA + 4], 4, pitch_raw) || + !hex_chars_to_uint32((const char*)&_msg_buff[AP_MOUNT_SKYDROID_MSGOFS_DATA + 8], 4, roll_raw)) { + return; + } + const int16_t yaw_angle_cd = wrap_180_cd((int16_t)yaw_raw); + const int16_t pitch_angle_cd = (int16_t)pitch_raw; + // roll comes from the gimbal's own self-stabilization - we report it for telemetry + // but cannot command it (see has_roll_control()) + const int16_t roll_angle_cd = (int16_t)roll_raw; + + // convert cd to radians + _current_angle_rad.x = cd_to_rad(roll_angle_cd); + _current_angle_rad.y = cd_to_rad(pitch_angle_cd); + _current_angle_rad.z = cd_to_rad(yaw_angle_cd); + _last_current_angle_ms = AP_HAL::millis(); + + // announce gimbal connection to the user on the first attitude report received. + // this does not depend on the "VER" command (whose model support is undocumented + // for some SkyDroid models) so it is a more reliable connection signal + if (!_announced_connected) { + _announced_connected = true; + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s connected", send_message_prefix); + } +} + +// gimbal video information analysis +void AP_Mount_SkyDroid::gimbal_record_analyse() +{ + // data is 2 ASCII chars ("00" or "01") - only the low digit is ever non-zero, so + // that's the only one we need to check + _recording = (_msg_buff[AP_MOUNT_SKYDROID_MSGOFS_DATA + 1] == '1'); +} + +// information analysis of gimbal storage card +void AP_Mount_SkyDroid::gimbal_sdcard_analyse() +{ + // data is 10 hex chars: 5 for remaining capacity, 5 for total capacity (units MB) + // all zeros means no card inserted. A too-short buffer is treated the same as + // all-zero (no card), matching this function's previous behaviour + static const uint8_t all_zero_chars[10] = {'0','0','0','0','0','0','0','0','0','0'}; + bool all_zero = true; + if (_msg_buff_len >= AP_MOUNT_SKYDROID_MSGOFS_DATA + ARRAY_SIZE(all_zero_chars)) { + all_zero = (memcmp(&_msg_buff[AP_MOUNT_SKYDROID_MSGOFS_DATA], all_zero_chars, ARRAY_SIZE(all_zero_chars)) == 0); + } + _sdcard_healthy = !all_zero; +} + +// gimbal basic information analysis. response data is of the form "VX.X.X" (e.g. "V1.0.78") +void AP_Mount_SkyDroid::gimbal_version_analyse() +{ + uint8_t data_buf_len; + if (!hex_char_to_nibble(_msg_buff[AP_MOUNT_SKYDROID_MSGOFS_DATALEN], data_buf_len) || data_buf_len == 0 || + _msg_buff[AP_MOUNT_SKYDROID_MSGOFS_DATA] != 'V') { + return; + } + + // version array with index 0=major, 1=minor, 2=patch + uint8_t version[3] {}; + uint8_t ver_count = 0; + uint32_t ver_num = 0; + for (uint8_t i = 1; i < data_buf_len && ver_count < ARRAY_SIZE(version); i++) { + const uint8_t c = _msg_buff[AP_MOUNT_SKYDROID_MSGOFS_DATA + i]; + if (c == '.') { + version[ver_count++] = ver_num; + ver_num = 0; + continue; + } + uint8_t digit; + if (!hex_char_to_nibble(c, digit)) { + return; + } + ver_num = ver_num * 10 + digit; + } + if (ver_count < ARRAY_SIZE(version)) { + version[ver_count] = ver_num; + } + _firmware_ver = (version[2] << 16) | (version[1] << 8) | (version[0]); + + // display gimbal firmware version to user. Worth reporting prominently: which + // command sets this gimbal actually implements appears to depend on it (SkyDroid's + // RCSDK gates its combined yaw+pitch call on firmware >= 0.5, and we've found the + // combined/absolute-angle commands dead on the firmware we have), so this is the + // first thing to check when a gimbal connects but won't move + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s firmware v%u.%u.%u", + send_message_prefix, + version[0], // major version + version[1], // minor version + version[2]); // patch version + + _got_gimbal_version = true; +} + +// gimbal model name analysis. response data is raw ASCII text, e.g. "C13" +void AP_Mount_SkyDroid::gimbal_model_analyse() +{ + uint8_t data_buf_len; + if (!hex_char_to_nibble(_msg_buff[AP_MOUNT_SKYDROID_MSGOFS_DATALEN], data_buf_len) || data_buf_len == 0) { + return; + } + memset(_model_name, 0, sizeof(_model_name)); + memcpy(_model_name, _msg_buff + AP_MOUNT_SKYDROID_MSGOFS_DATA, MIN((uint8_t)(sizeof(_model_name)-1), data_buf_len)); + + // display gimbal model name to user. Informational only - no control decision + // depends on it (see this driver's header comment) + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s model %s", send_message_prefix, _model_name); + + _got_model_name = true; +} + +// calculate checksum +uint8_t AP_Mount_SkyDroid::calculate_crc(const uint8_t *cmd, uint8_t len) const +{ + uint8_t crc = 0; + for (uint16_t i = 0; i= data)) { + return (data + '0'); + } else { + return (data - 10 + 'A'); + } +} + +// send a fixed length packet +bool AP_Mount_SkyDroid::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); + return send_variablelen_packet(HeaderType::FIXED_LEN, address, id, write, databuff, ARRAY_SIZE(databuff)-1); +} + +// send variable length packet +bool AP_Mount_SkyDroid::send_variablelen_packet(HeaderType header, AddressByte address, const Identifier id, bool write, const uint8_t* databuff, uint8_t databuff_len) +{ + // exit immediately if not initialised + if (!_initialised) { + return false; + } + + // calculate and sanity check packet size + const uint16_t packet_size = AP_MOUNT_SKYDROID_PACKETLEN_MIN + databuff_len; + if (packet_size > AP_MOUNT_SKYDROID_PACKETLEN_MAX) { + debug("send_packet data buff too large"); + return false; + } + + // check for sufficient space in outgoing buffer + if (_uart->txspace() < packet_size) { + debug("tx buffer full"); + return false; + } + + // create buffer for holding outgoing packet + uint8_t send_buff[packet_size]; + uint8_t send_buff_ofs = 0; + + // packet header (bytes 0 ~ 2) + send_buff[send_buff_ofs++] = '#'; + send_buff[send_buff_ofs++] = (header == HeaderType::FIXED_LEN) ? 'T' : 't'; + send_buff[send_buff_ofs++] = (header == HeaderType::FIXED_LEN) ? 'P' : 'p'; + + // address (bytes 3, 4). source is always the UDP/external-control address for this gimbal + send_buff[send_buff_ofs++] = (uint8_t)AddressByte::UDP; + send_buff[send_buff_ofs++] = (uint8_t)address; + + // data length (byte 5) + send_buff[send_buff_ofs++] = hex2char(databuff_len); + + // control byte (byte 6) + send_buff[send_buff_ofs++] = write ? (uint8_t)ControlByte::WRITE : (uint8_t)ControlByte::READ; + + // identifier (bytes 7 ~ 9) + send_buff[send_buff_ofs++] = id[0]; + send_buff[send_buff_ofs++] = id[1]; + send_buff[send_buff_ofs++] = id[2]; + + // data + if (databuff_len != 0) { + memcpy(&send_buff[send_buff_ofs], databuff, databuff_len); + send_buff_ofs += databuff_len; + } + + // crc + uint8_t crc = calculate_crc(send_buff, send_buff_ofs); + send_buff[send_buff_ofs++] = hex2char((crc >> 4) & 0x0f); + send_buff[send_buff_ofs++] = hex2char(crc & 0x0f); + + // send packet + _uart->write(send_buff, send_buff_ofs); + return true; +} + +// set gimbal's lock vs follow mode +// lock should be true if gimbal should maintain an earth-frame target +// lock is false to follow / maintain a body-frame target +bool AP_Mount_SkyDroid::set_gimbal_lock(bool lock) +{ + if (_last_lock == lock) { + return true; + } + + // send message and update lock state. PTZ data: 0x06 = follow, 0x07 = lock head + if (send_fixedlen_packet(AddressByte::GIMBAL, AP_MOUNT_SKYDROID_ID3CHAR_GIMBAL_MODE, true, lock ? 0x07 : 0x06)) { + _last_lock = lock; + return true; + } + return false; +} + +// send the gimbal's own one-shot "center" command. Deliberately not deduped like +// set_gimbal_lock() above - unlike lock/follow (a persistent mode we don't want to +// keep re-sending), center is a one-shot action that should fire every time the +// mode is (re)selected, same as every other RETRACT/NEUTRAL backend's behaviour +bool AP_Mount_SkyDroid::send_center_command() +{ + return send_fixedlen_packet(AddressByte::GIMBAL, AP_MOUNT_SKYDROID_ID3CHAR_GIMBAL_MODE, true, 0x05); +} + +// move to a "retracted" position - see this file's header declaration for why this +// uses the gimbal's own "center" command rather than falling through to the +// angle-based conversion (which needs GAC attitude feedback to converge) +void AP_Mount_SkyDroid::send_target_retracted() +{ + send_center_command(); +} + +// move to a neutral (forward-pointing) position - see this file's header declaration +void AP_Mount_SkyDroid::send_target_neutral() +{ + send_center_command(); +} + +#endif // HAL_MOUNT_SKYDROID_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_SkyDroid.h b/libraries/AP_Mount/AP_Mount_SkyDroid.h new file mode 100644 index 00000000000000..0c571d2a9e8479 --- /dev/null +++ b/libraries/AP_Mount/AP_Mount_SkyDroid.h @@ -0,0 +1,338 @@ +/* + SkyDroid gimbal driver using custom serial protocol (usually run over UDP) + + Packet format (courtesy of SkyDroid's "TOP" protocol document). This is + the same framing used by SkyDroid's OEM supplier for the Topotek driver + (see AP_Mount_Topotek) but the address bytes, command identifiers and + units used by SkyDroid's own firmware differ, so this is a separate, + independent implementation rather than a subclass. + + ------------------------------------------------------------------------------------------- + Field Index Bytes Description + ------------------------------------------------------------------------------------------- + Frame Header 0 3 #TP (fixed length) or #tp (variable length) + Address Bit 3 2 source address first, destination address second + Data_Len 5 1 data length (hex nibble, max 0x0F) + Control Bit 6 1 r -> query w -> set/control + Identification Bit 7 3 3 character command identifier + Data 10 Data_Len + Check Bit 2 sum of all preceding bytes, output as 2 ASCII hex + characters (high nibble first) + + This one driver covers every model in SkyDroid's "TOP protocol" gimbal camera + family: + - control is over UDP only (no direct UART), source address is always 'U' + - confirmed directly with SkyDroid: the gimbal-control commands are IDENTICAL + across models (C11, C13, ...). There is no model-specific control path, and + this driver deliberately has no model-dependent behaviour. The C13's extra + features over the C11 are infrared thermal imaging and laser ranging, neither + of which this driver currently uses. Do not reintroduce per-model dispatch + without new information from SkyDroid - an earlier version of this driver had + it, and it was wrong + - confirmed directly with SkyDroid: ROLL IS SELF-STABILIZED BY THE GIMBAL AND HAS + NO CONTROL COMMAND AT ALL, on any model. The protocol document does describe + roll commands ("GAR" angle, "GSR" rate) but the firmware does not implement + them, so this driver does not send them and reports has_roll_control() == false. + Roll is still parsed from the gimbal's own attitude reports and passed through + for telemetry, and our vehicle roll is still pushed to the gimbal ("FAI") to + feed its stabilizer + - pitch/yaw ranges are configured via MNT1_PITCH/YAW_MIN/MAX (e.g. the C11 has + pitch -90 to +10 deg, yaw -90 to +90 deg). This driver does not hardcode any + model's limits itself, so it stays correct across the family + - SkyDroid's documented sign convention is yaw-right-positive, pitch-up-positive, which + matches AP_Mount's own convention (no sign flip needed, unlike Topotek's protocol) + - the connected model (e.g. "C11", "C13") is queried at runtime via the "MOD" + command and reported through CAMERA_INFORMATION. This is INFORMATIONAL ONLY - + no control decision depends on it, which matters because "MOD" has been observed + on real hardware to take anywhere from under a second to 8+ minutes to answer + (SkyDroid's own SDK documents that camera-side commands are only effective once + the camera is producing video frames - "需要在出图后设置才有效" - which would + explain it). Gimbal-addressed commands (GSY/GSP/PTZ) have usually been observed + to work well before "MOD" answers - but at least once, on real hardware, NOTHING + worked (no RC, no GAC, no GCS messages of any kind) for over 5 minutes, so this + is not a guarantee - root cause of that particular case is still unknown (raised + with SkyDroid, response pending). Whatever gates it, this driver deliberately + does not wait on it: gimbal-addressed commands are always sent unconditionally, + for whenever the link does come up + - confirmed on real C11 hardware: the combined and absolute-angle commands (GAM, + GSM, GAY, GAP) are all silently ignored - only the individual-axis speed + commands (GSY, GSP) actually move the gimbal, so both rate and (closed-loop) + angle control are driven through those. This is believed to be a FIRMWARE + limitation rather than a model one: SkyDroid's SDK documents its combined + yaw+pitch call as requiring gimbal firmware >= 0.5, so newer firmware may well + accept the commands we found dead here. The gimbal's reported firmware version + is logged at startup (see gimbal_version_analyse()) to make that checkable + */ + +#pragma once + +#include "AP_Mount_config.h" + +#if HAL_MOUNT_SKYDROID_ENABLED + +#include "AP_Mount_Backend_Serial.h" +#include +#include +#include + +#define AP_MOUNT_SKYDROID_PACKETLEN_MAX 28 // maximum number of bytes in a packet sent to or received from the gimbal + +class AP_Mount_SkyDroid : public AP_Mount_Backend_Serial +{ + +public: + // Constructor + using AP_Mount_Backend_Serial::AP_Mount_Backend_Serial; + + // Do not allow copies + CLASS_NO_COPY(AP_Mount_SkyDroid); + + // update mount position - should be called periodically + void update() override; + + // return true if healthy + bool healthy() const override; + + // has_pan_control - returns true if this mount can control its pan (required for multicopters) + bool has_pan_control() const override { return yaw_range_valid(); }; + + // has_roll_control - always false: confirmed directly with SkyDroid that roll is + // self-stabilized by the gimbal and has no control command on any model in this + // family, regardless of what MNT1_ROLL_MIN/MAX is set to + bool has_roll_control() const override { return false; }; + + // + // camera controls + // + + // take a picture. returns true on success + bool take_picture() override; + + // start or stop video recording + // set start_recording = true to start record, false to stop recording + bool record_video(bool start_recording) override; + + // set zoom specified as a rate. SkyDroid's zoom is stepped (not continuous) so + // each non-zero call sends a single zoom-in/zoom-out pulse + bool set_zoom(ZoomType zoom_type, float zoom_value) override; + + bool has_camera_information() const override { return true; } + // return camera vendor name + void get_camera_vendor_name(char *buf, uint8_t buflen) const override { strncpy(buf, "SkyDroid", buflen); } + // return camera model name (e.g. "C11", "C13"), queried from the gimbal via the "MOD" command. + // this same driver supports every model in SkyDroid's "TOP protocol" gimbal camera family; + // the model name lets the GCS show which one is actually connected + void get_camera_model_name(char *buf, uint8_t buflen) const override { + if (!_got_model_name) { + return; + } + strncpy(buf, _model_name, buflen); + } + // return camera firmware version + uint32_t get_camera_firmware_version() const override { return _firmware_ver; } + // return camera capability flags + uint32_t get_camera_cap_flags() const override { + return (CAMERA_CAP_FLAGS_CAPTURE_VIDEO | + CAMERA_CAP_FLAGS_CAPTURE_IMAGE | + CAMERA_CAP_FLAGS_HAS_BASIC_ZOOM); + } + + // send camera settings message to GCS + void send_camera_settings(mavlink_channel_t chan) const override; + +protected: + + // get attitude as a quaternion. returns true on success + bool get_attitude_quaternion(Quaternion& att_quat) override; + + // SkyDroid can send either rates or angles, and also has a dedicated one-shot + // "center" command for retract/neutral (see send_target_retracted()/ + // send_target_neutral()) rather than falling through to the angle-based + // conversion every other target type uses + uint8_t natively_supported_mount_target_types() const override { + return NATIVE_ANGLES_AND_RATES_ONLY | + (1U << uint8_t(MountTargetType::RETRACTED)) | + (1U << uint8_t(MountTargetType::NEUTRAL)); + }; + + // move to a "retracted" position: SkyDroid has no separate stow position, so + // this uses the same one-shot "center" command as send_target_neutral() + void send_target_retracted() override; + + // move to a neutral (forward-pointing) position using the gimbal's own one-shot + // "center" command (PTZ data byte 0x05), rather than the angle-based conversion + // every other target type falls through to (which would drive our own P-controller + // toward _params.neutral_angles, requiring GAC attitude feedback to converge - see + // send_target_angles()). This is deliberately independent of that feedback loop: + // a switch mapped to RC_TARGETING->NEUTRAL should reliably point the gimbal + // forward using the gimbal's own logic, not depend on our closed loop ever + // having received a GAC packet + void send_target_neutral() override; + +private: + + // header type (fixed or variable length) + // first three bytes of packet determined by this value + enum class HeaderType : uint8_t { + FIXED_LEN = 0x00, // #TP will be sent + VARIABLE_LEN = 0x01, // #tp will be sent + }; + + // address (2nd and 3rd bytes of packet) + // first byte is always U (external/UDP controller) for our outgoing packets + enum class AddressByte : uint8_t { + SYSTEM_AND_IMAGE = 68, // 'D' + GIMBAL = 71, // 'G' + LENS = 77, // 'M' + UDP = 85, // 'U' + }; + + // control byte (read or write) + // sent as 7th byte of packet + enum class ControlByte : uint8_t { + READ = 114, // 'r' + WRITE = 119, // 'w' + }; + + // parsing state + enum class ParseState : uint8_t { + WAITING_FOR_HEADER1 = 0,// # + WAITING_FOR_HEADER2, // T or t + WAITING_FOR_HEADER3, // P or p + WAITING_FOR_ADDR1, // normally U + WAITING_FOR_ADDR2, // M, D, G + WAITING_FOR_DATALEN, + WAITING_FOR_CONTROL, // r or w + WAITING_FOR_ID1, // e.g. 'G' + WAITING_FOR_ID2, // e.g. 'A' + WAITING_FOR_ID3, // e.g. 'C' + WAITING_FOR_DATA, // normally hex numbers in char form (e.g. '0A') + WAITING_FOR_CRC_LOW, + WAITING_FOR_CRC_HIGH, + }; + + // steps of the 1hz round-robin housekeeping loop in update() - not every value is + // used every cycle (some requests only fire until answered once), and a few + // numbers are deliberately left spare for anything added later without needing + // to renumber the rest + enum class ReqStep : uint8_t { + VERSION = 0, // request_gimbal_version(), until _got_gimbal_version + TIME_SYNC = 1, // send_time_sync() + ATTITUDE_ENABLE = 2, // request_gimbal_attitude() + MODEL = 3, // request_gimbal_model(), until _got_model_name + SDCARD = 4, // request_gimbal_sdcard_info() + ATTITUDE_ACCEPT = 6, // send_attitude_enable() - note the gap at 5, spare + NUM_STEPS = 10, // wraps back to VERSION after this - note the gap at 7-9, spare + }; + + // identifier bytes + typedef char Identifier[3]; + + // send text prefix string + static const char* send_message_prefix; + + // reading incoming packets from gimbal and confirm they are of the correct format + void read_incoming_packets(); + + // request gimbal to start sending attitude at 10hz + void request_gimbal_attitude(); + + // request gimbal memory card information + void request_gimbal_sdcard_info(); + + // request gimbal version + void request_gimbal_version(); + + // request gimbal model name (e.g. "C11", "C13") + void request_gimbal_model(); + + // send current UTC date/time to the gimbal (TIM command) so it can correctly + // timestamp photos/videos - the camera has no RTC of its own and defaults to + // 1970-01-01 without this (confirmed on real hardware). Resent periodically, + // same as request_gimbal_attitude()/send_attitude_enable(), both as a guard + // against UDP packet loss and to recover if the camera reboots independently + // of the flight controller (also confirmed to happen on real hardware) + bool send_time_sync(); + + // enable the gimbal to receive our attitude (FAE) and send it to us (GAA) + bool send_attitude_enable(); + + // send our current attitude to the gimbal (FAI) + bool send_attitude_to_gimbal(); + + // send angle target in radians to gimbal. Closes the loop ourselves with a + // P-controller over the gimbal's own "GAC" attitude feedback, driving GSY/GSP as + // the rate actuator - there is no absolute-angle command that works on this + // hardware (see this file's header comment) + void send_target_angles(const MountAngleTarget& angle_rad) override; + + // send rate target in rad/s to gimbal, directly via GSY/GSP + void send_target_rates(const MountRateTarget& rate_rads) override; + + // send a single-axis rate command (GSY or GSP) for rate_dps, converted to the + // wire's signed 8bit LSB units using the real-world calibrated scale (see + // AP_MOUNT_SKYDROID_AXIS_DPS_PER_LSB). Caller is responsible for any + // axis-specific sign compensation (GSY's sign is inverted vs AP_Mount's + // convention - see send_target_rates()) + void send_axis_rate(const Identifier id, float rate_dps); + + // attitude information analysis of gimbal (response to GAA, arrives as "GAC") + void gimbal_angle_analyse(); + + // gimbal video information analysis + void gimbal_record_analyse(); + + // information analysis of gimbal storage card + void gimbal_sdcard_analyse(); + + // gimbal basic information analysis + void gimbal_version_analyse(); + + // gimbal model name analysis (raw ASCII text, e.g. "C13") + void gimbal_model_analyse(); + + // calculate checksum + uint8_t calculate_crc(const uint8_t *cmd, uint8_t len) const; + + // hexadecimal to character conversion + uint8_t hex2char(uint8_t data) const; + + // send a fixed length packet to gimbal + // returns true on success, false if serial port initialization failed + bool send_fixedlen_packet(AddressByte address, const Identifier id, bool write, uint8_t value); + + // send a variable length packet to gimbal + // returns true on success, false if serial port initialization failed + bool send_variablelen_packet(HeaderType header, AddressByte address, const Identifier id, bool write, const uint8_t* databuff, uint8_t databuff_len); + + // set gimbal's lock vs follow mode + // lock should be true if gimbal should maintain an earth-frame target + // lock is false to follow / maintain a body-frame target + bool set_gimbal_lock(bool lock); + + // send the gimbal's own one-shot "center" command (PTZ data byte 0x05). Used by + // both send_target_retracted() and send_target_neutral() - see their comments + bool send_center_command(); + + // members + bool _recording; // recording status, tracked locally from commands we've sent + bool _sdcard_healthy; // true if a memory card is present and OK (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_model_name; // true if gimbal's model name has been received + bool _announced_connected; // true once we've told the user the gimbal is connected + uint32_t _firmware_ver; // firmware version + char _model_name[8]; // gimbal model name (e.g. "C11", "C13"), always null-terminated + Vector3f _current_angle_rad; // current angles in radians received from gimbal (x=roll, y=pitch, z=yaw). roll is reported by the gimbal's own self-stabilization and is not controllable - see has_roll_control() + uint32_t _last_current_angle_ms; // system time (in milliseconds) that angle information received from the gimbal + uint32_t _last_req_current_info_ms; // system time that this driver last requested current gimbal information + uint8_t _last_req_step; // 10hz request loop step (different requests are sent at various steps) + uint8_t _msg_buff[AP_MOUNT_SKYDROID_PACKETLEN_MAX]; // buffer holding bytes from latest packet received. only used to calculate crc + uint8_t _msg_buff_len; // number of bytes in the msg buffer + struct { + ParseState state; // parser state + uint8_t data_len; // expected number of data bytes + } _parser; +}; + +#endif // HAL_MOUNT_SKYDROID_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_config.h b/libraries/AP_Mount/AP_Mount_config.h index fcc4239292115b..6b91c5c7c957a8 100644 --- a/libraries/AP_Mount/AP_Mount_config.h +++ b/libraries/AP_Mount/AP_Mount_config.h @@ -74,6 +74,10 @@ #define HAL_MOUNT_TOPOTEK_ENABLED AP_MOUNT_BACKEND_DEFAULT_ENABLED #endif +#ifndef HAL_MOUNT_SKYDROID_ENABLED +#define HAL_MOUNT_SKYDROID_ENABLED AP_MOUNT_BACKEND_DEFAULT_ENABLED && HAL_PROGRAM_SIZE_LIMIT_KB > 1024 +#endif + // set camera source is supported on gimbals that may have more than one lens #ifndef HAL_MOUNT_SET_CAMERA_SOURCE_ENABLED #define HAL_MOUNT_SET_CAMERA_SOURCE_ENABLED HAL_MOUNT_SIYI_ENABLED || HAL_MOUNT_XACTI_ENABLED || HAL_MOUNT_VIEWPRO_ENABLED From ba67fd132e93b964b3b78131af5ed2b554729677 Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Mon, 24 Aug 2026 08:41:21 -0600 Subject: [PATCH 03/19] SITL: add SkyDroid gimbal simulator Adds SIM_SkyDroid, simulating SkyDroid's "TOP protocol" gimbal camera family for AP_Mount_SkyDroid's autotest coverage. One class simulates every model - only the "MOD" response (model name) differs between the "skydroid" (C11) and "skydroid_c13" registrations, matching the real hardware's confirmed model-independent control behaviour. Simulates only the individual-axis GSY/GSP speed commands actually moving the gimbal (matching real hardware; the combined/absolute-angle commands are absorbed silently), and the gimbal's own one-shot "center" response to the PTZ 0x05 command used for retract/neutral. Roll is left entirely to the simulated gimbal's own stabilization, matching the real hardware's roll self-stabilization with no control command at all. --- libraries/SITL/SIM_Aircraft.h | 1 + libraries/SITL/SIM_SerialDevice.cpp | 74 +++++++ libraries/SITL/SIM_SerialDevice.h | 22 +- libraries/SITL/SIM_SkyDroid.cpp | 301 ++++++++++++++++++++++++++++ libraries/SITL/SIM_SkyDroid.h | 116 +++++++++++ libraries/SITL/SIM_config.h | 6 +- 6 files changed, 516 insertions(+), 4 deletions(-) create mode 100644 libraries/SITL/SIM_SkyDroid.cpp create mode 100644 libraries/SITL/SIM_SkyDroid.h diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 9721834303ac43..6d2ec642213ba6 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -39,6 +39,7 @@ #include "SIM_GPIO_LED_RGB.h" #include "SIM_Siyi.h" #include "SIM_Topotek.h" +#include "SIM_SkyDroid.h" #include "SIM_Viewpro.h" #include "SIM_Mount.h" diff --git a/libraries/SITL/SIM_SerialDevice.cpp b/libraries/SITL/SIM_SerialDevice.cpp index 6f26c22f4cf107..65e67508909244 100644 --- a/libraries/SITL/SIM_SerialDevice.cpp +++ b/libraries/SITL/SIM_SerialDevice.cpp @@ -159,10 +159,40 @@ bool SerialDevice::listen_on_tcp_port(const uint16_t port) listener = nullptr; return false; } + listener_is_udp = false; ::printf("SIM: device listening for autopilot on TCP port %u\n", unsigned(port)); return true; } +/* + attach this device to a UDP socket. The autopilot sends datagrams to + this socket (e.g. with a NET_Pn port configured as a UDP client) + instead of talking to the device over a simulated serial port. + Unlike TCP there is no connection to accept; the autopilot's address + is simply learned from whichever packet it last sent us, and replies + are sent back to that address + */ +bool SerialDevice::listen_on_udp_port(const uint16_t port) +{ + listener = NEW_NOTHROW SocketAPM_native(true); + if (listener == nullptr) { + return false; + } + listener->reuseaddress(); + if (!listener->bind("127.0.0.1", port) || + !listener->set_blocking(false)) { + ::fprintf(stderr, "SIM: failed to bind UDP port %u: %m\n", unsigned(port)); + delete listener; + listener = nullptr; + return false; + } + listener_is_udp = true; + udp_peer_addr = 0; + udp_peer_port = 0; + ::printf("SIM: device listening for autopilot on UDP port %u\n", unsigned(port)); + return true; +} + /* move bytes between the network socket and this device. This performs the same role the SITL UART driver performs for serially-attached @@ -175,6 +205,11 @@ void SerialDevice::network_update() return; } + if (listener_is_udp) { + network_update_udp(); + return; + } + if (sock == nullptr) { sock = listener->accept(0); if (sock == nullptr) { @@ -212,6 +247,45 @@ void SerialDevice::network_update() write_to_device(buffer, nread); } } + +/* + UDP variant of network_update(). There is no connection to accept; + we simply learn the autopilot's address from whichever datagram it + last sent us and reply to that address. Until a first datagram has + arrived we have nowhere to send device->autopilot traffic, so it is + dropped (matching how a real UDP device behaves before its peer has + said anything) + */ +void SerialDevice::network_update_udp() +{ + char buffer[512]; + + // autopilot to device: + while (true) { + const ssize_t nread = listener->recv(buffer, sizeof(buffer), 0); + if (nread <= 0) { + break; + } + listener->last_recv_address(udp_peer_addr, udp_peer_port); + write_to_device(buffer, nread); + } + + // device to autopilot: + if (udp_peer_addr == 0) { + // haven't heard from the autopilot yet, nowhere to send + return; + } + while (true) { + const ssize_t nread = read_from_device(buffer, sizeof(buffer)); + if (nread <= 0) { + break; + } + if (listener->sendto(buffer, nread, udp_peer_addr, udp_peer_port) != nread) { + // if the autopilot is not keeping up we simply drop the data + break; + } + } +} #endif // AP_SIM_SERIALDEVICE_NETWORK_ENABLED /** diff --git a/libraries/SITL/SIM_SerialDevice.h b/libraries/SITL/SIM_SerialDevice.h index 8a7bc5f9c2edcc..f5cb70f43235ec 100644 --- a/libraries/SITL/SIM_SerialDevice.h +++ b/libraries/SITL/SIM_SerialDevice.h @@ -48,10 +48,18 @@ class SerialDevice { #if AP_SIM_SERIALDEVICE_NETWORK_ENABLED // attach this device to a TCP server socket rather than to a // simulated serial port. This simulates a device which the - // autopilot reaches over the network (e.g. via a NET_Pn port) - // rather than over one of its serial ports. Returns true on success + // autopilot reaches over the network (e.g. via a NET_Pn port + // configured as a TCP client) rather than over one of its serial + // ports. Returns true on success bool listen_on_tcp_port(uint16_t port) WARN_IF_UNUSED; + // attach this device to a UDP socket rather than to a simulated + // serial port. This simulates a device which the autopilot + // reaches over the network (e.g. via a NET_Pn port configured as + // a UDP client) rather than over one of its serial ports. + // Returns true on success + bool listen_on_udp_port(uint16_t port) WARN_IF_UNUSED; + // true if this device is attached to the autopilot via a network // socket rather than via a simulated serial port bool is_network_attached() const { return listener != nullptr; } @@ -76,13 +84,21 @@ class SerialDevice { bool is_match_baud(void) const; +#if AP_SIM_SERIALDEVICE_NETWORK_ENABLED + // UDP variant of network_update() + void network_update_udp(); +#endif + // baudrate the autopilot has this device open at; zero if the // device is not attached to a simulated serial port uint32_t autopilot_baud; #if AP_SIM_SERIALDEVICE_NETWORK_ENABLED SocketAPM_native *listener = nullptr; // socket the autopilot connects to, nullptr if serially attached - SocketAPM_native *sock = nullptr; // socket to the connected autopilot, nullptr if not connected + SocketAPM_native *sock = nullptr; // TCP: socket to the connected autopilot, nullptr if not connected. unused for UDP + bool listener_is_udp; // true if listener is a UDP socket rather than a TCP listening socket + uint32_t udp_peer_addr; // UDP: IP address of the autopilot, learned from the last packet received. 0 if no packet received yet + uint16_t udp_peer_port; // UDP: port of the autopilot, learned from the last packet received #endif // AP_SIM_SERIALDEVICE_NETWORK_ENABLED ssize_t corrupt_transfer(char *buffer, const ssize_t ret, const size_t size) const; diff --git a/libraries/SITL/SIM_SkyDroid.cpp b/libraries/SITL/SIM_SkyDroid.cpp new file mode 100644 index 00000000000000..4a048913423e94 --- /dev/null +++ b/libraries/SITL/SIM_SkyDroid.cpp @@ -0,0 +1,301 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + Simulator for SkyDroid gimbal +*/ + +#include "SIM_config.h" +#include + +#if AP_SIM_SKYDROID_ENABLED + +#include "SIM_SkyDroid.h" +#include "SIM_Aircraft.h" +#include +#include + +using namespace SITL; + +void SkyDroid::update(const Aircraft &aircraft) +{ + Matrix3f gimbal_dcm; + gimbal.get_dcm(gimbal_dcm); + const Vector3f vehicle_rate_gimbal = gimbal_dcm.transposed() * aircraft.get_dcm() * aircraft.get_gyro(); + + // Confirmed on real hardware: GAM/GSM/GAY/GAP are silently ignored, only the + // individual-axis GSY/GSP speed commands actually move the gimbal, and they are + // genuinely proportional (unlike PTZ's fixed-speed jog, which was tried first and + // found not to move yaw at all). Real-world calibration: see + // AP_MOUNT_SKYDROID_AXIS_DPS_PER_LSB in AP_Mount_SkyDroid.cpp for the + // 0.5deg/s-per-LSB scale this mirrors, matching the protocol doc and SkyDroid's + // own RCSDK. GSY's sign is also inverted vs the doc on real hardware; this + // simulation reproduces that same inversion so it cancels out correctly against + // the driver's compensating negation in send_target_rates(), exactly like the + // real gimbal does + const float dps_per_lsb = 0.5f; + Vector3f ja; + gimbal.get_joint_angles(ja); + float pitch_rate; + float yaw_rate; + if (_centering) { + // simulate the gimbal's own one-shot "center" response to "PTZ" 0x05 - drive + // pitch/yaw toward zero using the same simple P-controller approach the real + // driver uses for closed-loop control elsewhere, since we have no real + // hardware data on how a real gimbal's own centering actually moves + constexpr float gain = 10.0f; + pitch_rate = -ja.y * gain; + yaw_rate = -ja.z * gain; + } else { + pitch_rate = radians(_commanded_pitch_speed_lsb * dps_per_lsb); + yaw_rate = -radians(_commanded_yaw_speed_lsb * dps_per_lsb); + } + + // roll is left entirely to the simulated gimbal's own stabilization, matching the + // real hardware: SkyDroid have confirmed roll is self-stabilized with no control + // command on any model in this family + gimbal.set_demanded_rates(Vector3f( + vehicle_rate_gimbal.x, + vehicle_rate_gimbal.y + pitch_rate, + vehicle_rate_gimbal.z + yaw_rate)); + + gimbal.update(aircraft); + update_input(); + + // send attitude at 10 Hz + const uint32_t now_ms = AP_HAL::millis(); + if (now_ms - _last_attitude_ms >= 100) { + _last_attitude_ms = now_ms; + send_attitude(); + } +} + +void SkyDroid::send_attitude() +{ + // Report actual GimbalSim joint angles. + // joint_angles.y = pitch (negative = down), .z = azimuth. + // Wire encoding: pitch_cd = pitch_deg * 100, yaw_cd = yaw_deg * 100 (no sign flip) + Vector3f ja; + gimbal.get_joint_angles(ja); + const int16_t yaw_cd = (int16_t)(degrees(ja.z) * 100.0f); + const int16_t pitch_cd = (int16_t)(degrees(ja.y) * 100.0f); + const int16_t roll_cd = (int16_t)(degrees(ja.x) * 100.0f); + + uint8_t data[12]; + uint16_to_hex4((uint16_t)yaw_cd, &data[0]); + uint16_to_hex4((uint16_t)pitch_cd, &data[4]); + uint16_to_hex4((uint16_t)roll_cd, &data[8]); + + // attitude data is always sent with identifier "GAC", distinct from the + // "GAA" enable/rate-request command the driver sends to ask for it + send_packet('G', "GAC", false, data, sizeof(data)); +} + +/* + read bytes from autopilot into _buf, then scan for complete packets. + Packet format: + [0] '#' + [1] 'T' or 't' + [2] 'P' or 'p' + [3] 'U' (addr1, always UDP/external-control for SkyDroid) + [4] addr2 ('G','D','M') + [5] data_len as a single ASCII hex char + [6] 'r' or 'w' + [7..9] 3-char command ID + [10..10+data_len-1] data bytes + [10+data_len..11+data_len] 2-byte CRC + Total packet length = 12 + data_len +*/ +void SkyDroid::move_preamble_in_buffer(uint8_t search_start_pos) +{ + uint8_t i; + for (i = search_start_pos; i < _buflen; i++) { + if (_buf[i] == '#') { + break; + } + } + if (i == 0) { + return; + } + memmove(_buf, &_buf[i], _buflen - i); + _buflen -= i; +} + +void SkyDroid::update_input() +{ + const ssize_t n = read_from_autopilot((char*)&_buf[_buflen], ARRAY_SIZE(_buf) - _buflen - 1); + if (n < 0) { + if (errno != EAGAIN && errno != EWOULDBLOCK && errno != 0) { + AP_HAL::panic("Failed to read from autopilot"); + } + return; + } + _buflen += n; + + while (_buflen >= 3) { + // search for '#' at the start + if (_buf[0] != '#') { + move_preamble_in_buffer(1); + continue; + } + if (_buf[1] != 'T' && _buf[1] != 't') { + move_preamble_in_buffer(1); + continue; + } + if (_buf[2] != 'P' && _buf[2] != 'p') { + move_preamble_in_buffer(1); + continue; + } + + // need at least 6 bytes to read the data_len field + if (_buflen < 6) { + break; + } + + // parse data length from ASCII hex char at [5] + uint8_t data_len; + if (!hex_char_to_nibble(_buf[5], data_len)) { + // invalid data length — discard '#' + move_preamble_in_buffer(1); + continue; + } + + const uint8_t pkt_len = 12 + data_len; + if (pkt_len > PACKETLEN_MAX) { + move_preamble_in_buffer(1); + continue; + } + + // wait for the full packet + if (_buflen < pkt_len) { + break; + } + + // verify and dispatch the packet + handle_packet(data_len); + move_preamble_in_buffer(pkt_len); + } +} + +void SkyDroid::handle_packet(uint8_t data_len) +{ + // verify CRC + const uint8_t pkt_len = 12 + data_len; + const uint8_t crc = crc_sum_of_bytes(_buf, pkt_len - 2); + const uint8_t expected_hi = hex2char((crc >> 4) & 0x0f); + const uint8_t expected_lo = hex2char(crc & 0x0f); + if (_buf[pkt_len - 2] != expected_hi || _buf[pkt_len - 1] != expected_lo) { + return; + } + + // ID is at bytes [7..9] + const char *id = (const char*)&_buf[7]; + + if (strncmp(id, "GAA", 3) == 0) { + // attitude streaming enable/rate request + send_attitude(); + + } else if (strncmp(id, "VER", 3) == 0) { + // version response begins with a literal 'V' (see AP_Mount_SkyDroid::gimbal_version_analyse) + const uint8_t data[] { 'V', '1', '.', '0', '.', '0' }; + send_packet('D', "VER", false, data, sizeof(data)); + + } else if (strncmp(id, "SDC", 3) == 0) { + // card present: 5 hex chars remaining, 5 hex chars total, not all zero + const uint8_t data[] { '0','0','0','1','0', '0','0','0','2','0' }; + send_packet('D', "SDC", false, data, sizeof(data)); + + } else if (strncmp(id, "MOD", 3) == 0) { + // model name response is raw ASCII text, e.g. "C13" + send_packet('D', "MOD", false, (const uint8_t*)_model_name, strlen(_model_name)); + + } else if (strncmp(id, "GSY", 3) == 0 && data_len >= 2) { + // individual-axis yaw speed command - confirmed on real hardware to be the + // only thing that actually moves yaw: signed 8bit hex value, LSB units + // calibrated in update() above. A real speed command supersedes any + // in-progress centering - see _centering's comment + _centering = false; + uint32_t tmp; + if (hex_chars_to_uint32((const char*)&_buf[10], 2, tmp)) { + _commanded_yaw_speed_lsb = (int8_t)tmp; + } + + } else if (strncmp(id, "GSP", 3) == 0 && data_len >= 2) { + // individual-axis pitch speed command, same as GSY above + _centering = false; + uint32_t tmp; + if (hex_chars_to_uint32((const char*)&_buf[10], 2, tmp)) { + _commanded_pitch_speed_lsb = (int8_t)tmp; + } + + } else if (strncmp(id, "PTZ", 3) == 0 && data_len >= 2) { + // discrete gimbal control. Only 0x05 ("center") is simulated - see + // _centering's comment and update()'s use of it. Follow/lock (0x06/0x07) + // and the jog codes (0x00-0x04) are not simulated: this driver only ever + // sends follow/lock (see AP_Mount_SkyDroid::set_gimbal_lock()), which has no + // observable effect on the simulated gimbal's motion either way + uint32_t tmp; + if (hex_chars_to_uint32((const char*)&_buf[10], 2, tmp) && tmp == 0x05) { + _centering = true; + } + } + // Everything else is absorbed silently, deliberately reproducing the real + // hardware's behaviour: the combined and absolute-angle commands (GAM, GSM, GAY, + // GAP) are confirmed silently ignored on real hardware, and roll (GAR, GSR) has + // no control command at all - SkyDroid have confirmed roll is self-stabilized. + // FAE, FAI, CAP, REC, DZM and TIM are also absorbed here +} + +void SkyDroid::send_packet(char addr2, const char id[3], bool write, const uint8_t *data, uint8_t len) +{ + const uint8_t total = 12 + len; + if (total > PACKETLEN_MAX) { + return; + } + + uint8_t pkt[PACKETLEN_MAX]; + uint8_t ofs = 0; + + pkt[ofs++] = '#'; + pkt[ofs++] = 'T'; + pkt[ofs++] = 'P'; + pkt[ofs++] = 'U'; // SkyDroid always replies to the 'U' (UDP/external-control) address + pkt[ofs++] = (uint8_t)addr2; + pkt[ofs++] = hex2char(len & 0x0f); // data length as single ASCII hex char + pkt[ofs++] = write ? 'w' : 'r'; + pkt[ofs++] = (uint8_t)id[0]; + pkt[ofs++] = (uint8_t)id[1]; + pkt[ofs++] = (uint8_t)id[2]; + + for (uint8_t i = 0; i < len; i++) { + pkt[ofs++] = data[i]; + } + + // checksum: byte sum of all preceding bytes, encoded as 2 uppercase ASCII hex chars + const uint8_t crc = crc_sum_of_bytes(pkt, ofs); + pkt[ofs++] = hex2char((crc >> 4) & 0x0f); + pkt[ofs++] = hex2char(crc & 0x0f); + + write_to_autopilot((const char*)pkt, ofs); +} + +void SkyDroid::uint16_to_hex4(uint16_t val, uint8_t buf[4]) +{ + buf[0] = hex2char((val >> 12) & 0x0f); + buf[1] = hex2char((val >> 8) & 0x0f); + buf[2] = hex2char((val >> 4) & 0x0f); + buf[3] = hex2char((val ) & 0x0f); +} + +#endif // AP_SIM_SKYDROID_ENABLED diff --git a/libraries/SITL/SIM_SkyDroid.h b/libraries/SITL/SIM_SkyDroid.h new file mode 100644 index 00000000000000..fc8727073a1023 --- /dev/null +++ b/libraries/SITL/SIM_SkyDroid.h @@ -0,0 +1,116 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + Simulator for SkyDroid gimbal. This one class simulates every model in + SkyDroid's "TOP protocol" gimbal camera family: SkyDroid have confirmed the + gimbal-control commands are identical across models, so the ONLY thing the + simulated model name changes is the "MOD" response. Registered under two + device names (see SITL_State_common.cpp) purely so a test can check the + driver behaves identically regardless of which model answers. + + Roll is deliberately not controllable here, matching the real hardware: + SkyDroid have confirmed roll is self-stabilized with no control command on + any model, so GAR/GSR are absorbed and ignored like any other unimplemented + command. + +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:skydroid --speedup=1 + +param set MNT1_TYPE 15 # skydroid +param set SERIAL5_PROTOCOL 8 # gimbal +reboot + +Use --serial5=sim:skydroid_c13 to have the gimbal report itself as a "C13" +instead of a "C11". Its control behaviour is identical. + +*/ + +#pragma once + +#include "SIM_config.h" + +#if AP_SIM_SKYDROID_ENABLED + +#include "SIM_Mount.h" +#include "SIM_Gimbal.h" + +namespace SITL { + +class SkyDroid : public Mount { +public: + + // model_name is returned verbatim in response to the "MOD" command (e.g. "C11", + // "C13"). It affects nothing else - control behaviour is model-independent + SkyDroid(const char *model_name) : + _model_name(model_name) {} + + void update(const Aircraft &aircraft) override; + +private: + + // the physical gimbal: + Gimbal gimbal; + + const char *_model_name; + + // input accumulation buffer; also used as working buffer by handle_packet() + static constexpr uint8_t PACKETLEN_MAX = 28; + uint8_t _buf[PACKETLEN_MAX]; + uint8_t _buflen; + + uint32_t _last_attitude_ms; // time of last attitude packet sent + + // last GSY/GSP speed values received (signed 8bit wire units). These + // individual-axis speed commands are the only ones that actually move the real + // gimbal (GAM/GSM/GAY/GAP are all confirmed silently ignored on real hardware), + // so they're the only ones simulated here + int8_t _commanded_yaw_speed_lsb; + int8_t _commanded_pitch_speed_lsb; + + // true from the moment a "PTZ" 0x05 ("center") command is received until the + // next GSY/GSP speed command arrives - see update()'s use of this to simulate + // the gimbal's own one-shot centering behaviour (AP_Mount_SkyDroid:: + // send_target_neutral()/send_target_retracted() send this instead of driving + // GSY/GSP themselves, so without this the simulated gimbal would never move + // for RETRACT/NEUTRAL and the autotest's neutral-position check would fail) + bool _centering; + + // read and dispatch incoming packets from autopilot + void update_input(); + + // scan forward from search_start_pos for '#' and move it to _buf[0] + void move_preamble_in_buffer(uint8_t search_start_pos); + + // send gimbal attitude packet to the driver + void send_attitude(); + + // dispatch a complete packet beginning at _buf[0], data_len data bytes + void handle_packet(uint8_t data_len); + + // build and send a response packet. SkyDroid's control address is always 'U' + // (UDP-only device, no separate UART/network interface ambiguity) + void send_packet(char addr2, const char id[3], bool write, const uint8_t *data, uint8_t len); + + // encode a uint16 as 4 uppercase ASCII hex chars + static void uint16_to_hex4(uint16_t val, uint8_t buf[4]); + + // convert a nibble (0-15) to an uppercase ASCII hex character + static uint8_t hex2char(uint8_t nibble) { + return nibble < 10 ? ('0' + nibble) : ('A' + nibble - 10); + } +}; + +} // namespace SITL + +#endif // AP_SIM_SKYDROID_ENABLED diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h index f9f3c84435485c..0e5bf261a4cd70 100644 --- a/libraries/SITL/SIM_config.h +++ b/libraries/SITL/SIM_config.h @@ -227,6 +227,10 @@ #define AP_SIM_VIEWPRO_ENABLED 1 #endif +#ifndef AP_SIM_SKYDROID_ENABLED +#define AP_SIM_SKYDROID_ENABLED 1 +#endif + #ifndef AP_SIM_AVT_CM62_ENABLED #define AP_SIM_AVT_CM62_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL && HAL_MAVLINK_BINDINGS_ENABLED) #endif @@ -243,7 +247,7 @@ // base class for all simulated gimbal backends: #ifndef AP_SIM_MOUNT_ENABLED -#define AP_SIM_MOUNT_ENABLED (AP_SIM_SIYI_ENABLED || AP_SIM_TOPOTEK_ENABLED || AP_SIM_VIEWPRO_ENABLED || AP_SIM_MAVLINKGIMBALV2_ENABLED) +#define AP_SIM_MOUNT_ENABLED (AP_SIM_SIYI_ENABLED || AP_SIM_TOPOTEK_ENABLED || AP_SIM_VIEWPRO_ENABLED || AP_SIM_MAVLINKGIMBALV2_ENABLED || AP_SIM_SKYDROID_ENABLED) #endif #ifndef AP_SIM_AIRSPEED_DLVR_ENABLED From 50f25f386f33b39d2186e8f09befad44168abd63 Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Mon, 24 Aug 2026 08:41:22 -0600 Subject: [PATCH 04/19] AP_HAL_SITL: register SkyDroid gimbal simulator device names, add UDP network-attached device support Registers "skydroid" and "skydroid_c13" as SIM_SkyDroid device names for --serial and --net-device use, and extends create_net_serial_sim() to support network-attached devices connecting over UDP rather than only TCP - needed because the real C11 hardware is UDP-only. The spec format is NAME:PORT[,OPTION,...], with options comma-grouped with the port number rather than colon-chained, so future additions (e.g. a simulated firmware version) read as logically attached to the port rather than as another top-level field. --- libraries/AP_HAL_SITL/SITL_State_common.cpp | 50 +++++++++++++++++---- libraries/AP_HAL_SITL/SITL_State_common.h | 1 + libraries/AP_HAL_SITL/SITL_cmdline.cpp | 4 +- 3 files changed, 44 insertions(+), 11 deletions(-) diff --git a/libraries/AP_HAL_SITL/SITL_State_common.cpp b/libraries/AP_HAL_SITL/SITL_State_common.cpp index dca94ada05d2e5..121a465e5fed77 100644 --- a/libraries/AP_HAL_SITL/SITL_State_common.cpp +++ b/libraries/AP_HAL_SITL/SITL_State_common.cpp @@ -14,6 +14,7 @@ #include #include #include +#include #include #include @@ -243,6 +244,18 @@ SITL::SerialDevice *SITL_State_Common::create_serial_sim(const char *name, const sitl_model->add_gimbal_sim(*topotek); return topotek; #endif // AP_SIM_TOPOTEK_ENABLED +#if AP_SIM_SKYDROID_ENABLED + } else if (streq(name, "skydroid")) { + const auto skydroid = NEW_NOTHROW SITL::SkyDroid("C11"); + sitl_model->add_gimbal_sim(*skydroid); + return skydroid; + } else if (streq(name, "skydroid_c13")) { + // the model name is the ONLY difference from "skydroid" above - SkyDroid have + // confirmed the gimbal-control commands are identical across models + const auto skydroid_c13 = NEW_NOTHROW SITL::SkyDroid("C13"); + sitl_model->add_gimbal_sim(*skydroid_c13); + return skydroid_c13; +#endif // AP_SIM_SKYDROID_ENABLED #if AP_SIM_VIEWPRO_ENABLED } else if (streq(name, "viewpro")) { const auto viewpro = NEW_NOTHROW SITL::Viewpro(); @@ -359,10 +372,16 @@ SITL::SerialDevice *SITL_State_Common::create_serial_sim(const char *name, const #if AP_SIM_SERIALDEVICE_NETWORK_ENABLED /* - create a simulated device which the autopilot connects to over TCP - rather than over one of its simulated serial ports. This is used to - simulate devices attached to the autopilot's network ports (NET_Pn). - spec is of the form NAME:TCPPORT e.g. "topotek:15005" + create a simulated device which the autopilot connects to over the + network (TCP by default, or UDP) rather than over one of its + simulated serial ports. This is used to simulate devices attached + to the autopilot's network ports (NET_Pn). + spec is of the form NAME:PORT or NAME:PORT,OPTION,OPTION,..., e.g. + "topotek:15005" (TCP, the default) or "skydroid:15005,udp" - any + options beyond the port number are comma-separated from each other + (and from the port number), rather than each being tacked on with + another colon, since they're logically grouped with the port rather + than being another NAME-like top-level field */ void SITL_State_Common::create_net_serial_sim(const char *spec) { @@ -376,14 +395,27 @@ void SITL_State_Common::create_net_serial_sim(const char *spec) } char *saveptr = nullptr; const char *name = strtok_r(s, ":", &saveptr); - const char *port_str = strtok_r(nullptr, ":", &saveptr); - if (name == nullptr || port_str == nullptr) { - AP_HAL::panic("Bad network device (%s); expected NAME:TCPPORT", spec); + char *port_and_options = strtok_r(nullptr, ":", &saveptr); + if (name == nullptr || port_and_options == nullptr) { + AP_HAL::panic("Bad network device (%s); expected NAME:PORT[,PROTOCOL]", spec); + } + char *saveptr2 = nullptr; + const char *port_str = strtok_r(port_and_options, ",", &saveptr2); + const char *protocol_str = strtok_r(nullptr, ",", &saveptr2); // optional, defaults to "tcp" + if (port_str == nullptr) { + AP_HAL::panic("Bad network device (%s); expected NAME:PORT[,PROTOCOL]", spec); + } + const bool use_udp = (protocol_str != nullptr) && (strcasecmp(protocol_str, "udp") == 0); + if (protocol_str != nullptr && !use_udp && strcasecmp(protocol_str, "tcp") != 0) { + AP_HAL::panic("Bad network device protocol (%s); expected 'tcp' or 'udp'", protocol_str); } SITL::SerialDevice *device = create_serial_sim(name, nullptr, 0); - if (!device->listen_on_tcp_port(atoi(port_str))) { - AP_HAL::panic("Failed to attach %s to TCP port %s", name, port_str); + const bool ok = use_udp ? + device->listen_on_udp_port(atoi(port_str)) : + device->listen_on_tcp_port(atoi(port_str)); + if (!ok) { + AP_HAL::panic("Failed to attach %s to %s port %s", name, use_udp ? "UDP" : "TCP", port_str); } net_serial_sims[num_net_serial_sims++] = device; diff --git a/libraries/AP_HAL_SITL/SITL_State_common.h b/libraries/AP_HAL_SITL/SITL_State_common.h index 88662d4c869710..7668b4d238ee26 100644 --- a/libraries/AP_HAL_SITL/SITL_State_common.h +++ b/libraries/AP_HAL_SITL/SITL_State_common.h @@ -28,6 +28,7 @@ #include #include +#include #include #include diff --git a/libraries/AP_HAL_SITL/SITL_cmdline.cpp b/libraries/AP_HAL_SITL/SITL_cmdline.cpp index b8f3f6ce084316..fdf478490e9a40 100644 --- a/libraries/AP_HAL_SITL/SITL_cmdline.cpp +++ b/libraries/AP_HAL_SITL/SITL_cmdline.cpp @@ -109,7 +109,7 @@ void SITL_State::_usage(void) "\t--serial8 device set device string for SERIAL8\n" "\t--serial9 device set device string for SERIAL9\n" "\t--uartA device alias for --serial0 (do not use)\n" - "\t--net-device NAME:PORT attach simulated device NAME to TCP port PORT rather than to a serial port\n" + "\t--net-device NAME:PORT[,udp] attach simulated device NAME to TCP (or, with ',udp', UDP) port PORT rather than to a serial port\n" "\t--base-port PORT set port num for base port(default 5670) must be before -I option\n" "\t--rc-in-port PORT set port num for rc in\n" "\t--sim-address ADDR set address string for simulator\n" @@ -267,7 +267,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) struct AP_Param::defaults_table_struct temp_cmdline_param{}; #if AP_SIM_SERIALDEVICE_NETWORK_ENABLED - // NAME:TCPPORT strings from --net-device options: + // NAME:PORT[,udp] strings from --net-device options: const char *net_device_strings[4]; uint8_t num_net_device_strings = 0; #endif // AP_SIM_SERIALDEVICE_NETWORK_ENABLED From 80f78fd6d99c831f2bb46b66607720be80541e48 Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Mon, 24 Aug 2026 08:41:23 -0600 Subject: [PATCH 05/19] autotest: add SkyDroid gimbal driver coverage Adds three tests for the new AP_Mount_SkyDroid backend: - MountSkyDroid: C11 over a simulated serial port, exercising the full shared mount_test_body() suite. - MountSkyDroidC13: deliberately the same body as MountSkyDroid() - a regression guard that the driver stays model-independent, and that a differently-named model doesn't take a different control path. Also asserts roll never responds to RC input even with MNT1_ROLL_MIN/MAX configured, matching the confirmed roll self-stabilization. - MountSkyDroidNetwork: C11 over a UDP network port, exercising the real hardware's actual transport rather than the SITL serial-port path. Threads a pitch_tolerance parameter through test_mount_rc_targetting() and mount_test_body() (default 0.1deg, unchanged for every other backend) so MountSkyDroid()/MountSkyDroidC13() can pass a wider, measurement-backed 0.3deg tolerance specific to this backend's confirmed actuator resolution: GSY/GSP's wire value is a quantized 8-bit signed LSB (0.5deg/s per LSB, confirmed via dataflash log analysis of real hardware), which puts a genuine floor of ~0.25deg of angular error below which the closed-loop controller's commanded rate rounds to zero and it simply stops correcting - tighter than the shared test's default tolerance can ask of this kind of actuator. --- Tools/autotest/arducopter.py | 172 ++++++++++++++++++++++++++++++++++- 1 file changed, 171 insertions(+), 1 deletion(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 90ebf5087c9220..fd5a9a6c54c5f2 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -7786,7 +7786,8 @@ def test_mount_rc_targetting(self, pitch_rc_neutral=1500, do_rate_tests=True, pi pitch_tolerance defaults to the original tight 0.1deg check; backends whose actuator has a coarser confirmed physical resolution (e.g. a rate-only actuator closing an angle loop via a quantized speed command) may need to - pass a wider value''' + pass a wider value - see MountSkyDroid()'s use of this for a concrete + example with the reasoning''' if True: self.context_push() self.set_parameters({ @@ -8460,6 +8461,172 @@ def MountTopotek(self): self.mount_test_body(pitch_rc_neutral=1818, do_rate_tests=False, constrain_sysid_target=False) + def MountSkyDroid(self): + '''test SkyDroid gimbal using SIM_SkyDroid simulator''' + # pitch_rc_neutral=1818: with RC6 min=1000 max=2000 trim=1500 and + # default MNT1_PITCH_MIN=-90 / MNT1_PITCH_MAX=20, norm_input=0.636 + # maps to exactly 0 deg pitch. + pitch_rc_neutral = 1818 + # centre RC6 *before* the parameter changes below reboot the FC. MNT1's + # default mode is RC_TARGETING, so without this the mount starts driving + # toward whatever angle RC6's un-centred default value maps to the moment + # it boots, well before the test gets to explicitly select NEUTRAL mode. + # Harmless for backends with a fast control loop (they recover from that + # transient inside the neutral check's 5s budget), but avoid causing a large + # transient in the first place anyway rather than rely on recovering from it + self.set_rc(6, pitch_rc_neutral) + self.set_parameters({ + "MNT1_TYPE": 15, # SkyDroid + "CAM1_TYPE": 4, # Mount + "SERIAL5_PROTOCOL": 8, # gimbal + "RC6_OPTION": 213, # MOUNT1_PITCH + }) + self.customise_SITL_commandline(["--serial5=sim:skydroid:"]) + # version "V1.0.0" from SIM_SkyDroid: major=1 | (minor=0)<<8 | (patch=0)<<16 = 1 + # cap flags: CAPTURE_VIDEO | CAPTURE_IMAGE | HAS_BASIC_ZOOM + self.mount_check_camera_information( + "SkyDroid", "C11", + expected_fw_version=1, + expected_cap_flags=0x43, + ) + # constrain_sysid_target=True (the default): unlike Topotek/Viewpro, + # AP_Mount_SkyDroid::send_target_angles does clamp pitch/yaw to the + # configured MNT1_PITCH/YAW_MIN/MAX before sending, so the 68-deg + # sysid test (which expects that clamp) is exercised here. + # neutral_tol_deg=3.5: confirmed on real hardware that SkyDroid gimbals only + # respond to the individual-axis GSY/GSP speed commands (GAM/GSM are silently + # ignored); since there's no working absolute-angle command, the driver closes + # an angle P-controller loop on top of them, so allow a little settling + # tolerance rather than the exact positioning an absolute-angle backend gives. + # + # rc_targetting_pitch_tolerance=0.3: GSY/GSP's wire value is a quantized 8bit + # signed LSB (see AP_MOUNT_SKYDROID_AXIS_DPS_PER_LSB, confirmed on real + # hardware via dataflash log analysis to be 0.5deg/s per LSB), which puts a + # genuine, measured floor of ~0.25deg of angular error below which the + # closed-loop P-controller's commanded rate rounds to 0 LSB and it simply + # stops correcting - this is a real actuator resolution limit, not a driver + # bug, and the shared test's default 0.1deg tolerance is tighter than this + # actuator can physically deliver. 0.3 gives a little margin above the + # measured ~0.25deg floor + self.mount_test_body(pitch_rc_neutral=pitch_rc_neutral, do_rate_tests=False, neutral_tol_deg=3.5, + rc_targetting_pitch_tolerance=0.3) + + def MountSkyDroidC13(self): + '''test SkyDroid C13 gimbal using SIM_SkyDroid simulator + + SkyDroid have confirmed the gimbal-control commands are IDENTICAL across + models - the C13's extra features over the C11 are infrared thermal imaging + and laser ranging, neither of which this driver uses. So this test is + deliberately the same body as MountSkyDroid(): it is a regression guard + that the driver stays model-independent, and that a differently-named model + does not take a different control path. It also asserts roll stays + uncontrollable even when MNT1_ROLL_MIN/MAX is configured, since SkyDroid + have confirmed roll is self-stabilized with no control command at all''' + # pitch_rc_neutral=1818: with RC6 min=1000 max=2000 trim=1500 and + # default MNT1_PITCH_MIN=-90 / MNT1_PITCH_MAX=20, norm_input=0.636 + # maps to exactly 0 deg pitch. + pitch_rc_neutral = 1818 + # centre RC6 *before* the parameter changes below reboot the FC - same fix as + # MountSkyDroid() needed, and for the same reason: see the comment there + self.set_rc(6, pitch_rc_neutral) + self.set_parameters({ + "MNT1_TYPE": 15, # SkyDroid + "CAM1_TYPE": 4, # Mount + "SERIAL5_PROTOCOL": 8, # gimbal + "RC6_OPTION": 213, # MOUNT1_PITCH + # deliberately configure a roll range the gimbal cannot actually use, to + # prove the driver still refuses to drive roll - see the roll check below + "MNT1_ROLL_MIN": -45, + "MNT1_ROLL_MAX": 45, + }) + self.customise_SITL_commandline(["--serial5=sim:skydroid_c13:"]) + # version "V1.0.0" from SIM_SkyDroid: major=1 | (minor=0)<<8 | (patch=0)<<16 = 1 + # cap flags: CAPTURE_VIDEO | CAPTURE_IMAGE | HAS_BASIC_ZOOM + # model name "C13" confirms the differently-named variant was actually selected + self.mount_check_camera_information( + "SkyDroid", "C13", + expected_fw_version=1, + expected_cap_flags=0x43, + ) + # identical expectations to MountSkyDroid() - that is the point of this test, + # rc_targetting_pitch_tolerance included - see MountSkyDroid()'s comment for + # why 0.3 rather than the shared default of 0.1 + self.mount_test_body(pitch_rc_neutral=pitch_rc_neutral, do_rate_tests=False, neutral_tol_deg=3.5, + rc_targetting_pitch_tolerance=0.3) + + # roll must NOT respond: SkyDroid have confirmed roll is self-stabilized by the + # gimbal with no control command on any model, so AP_Mount_SkyDroid reports + # has_roll_control() == false and never sends a roll command. This is the + # inverse of a test that used to live here, which drove roll via "GAR" and + # expected it to move - that command turned out not to be implemented in the + # firmware at all despite being in the protocol document + self.progress("Testing mount roll stays uncommanded (roll is not controllable)") + # mount_test_body() above ends with its own RTL+landing sequence, so the + # vehicle's resting attitude here is whatever it happens to land at - NOT + # guaranteed level. So this deliberately checks for CHANGE in response to the + # RC command, not an absolute near-zero value - the claim under test is "roll + # doesn't respond to input", which holds regardless of the vehicle's own + # attitude, unlike a fixed absolute-value check + self.context_push() + self.set_parameters({ + 'RC11_OPTION': 212, # MOUNT1_ROLL + }) + self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + start_roll_deg, _, _, _ = self.get_mount_roll_pitch_yaw_deg() + self.set_rc(11, 1100) # would demand roll to the MNT1_ROLL_MIN extreme + tstart = self.get_sim_time() + max_roll_change_deg = 0 + while self.get_sim_time_cached() - tstart < 10: + mount_roll_deg, _, _, _ = self.get_mount_roll_pitch_yaw_deg() + self.progress("roll=%f (start was %f)" % (mount_roll_deg, start_roll_deg)) + max_roll_change_deg = max(max_roll_change_deg, abs(mount_roll_deg - start_roll_deg)) + self.set_rc(11, 1500) + self.context_pop() + # 15deg is well clear of the 45deg the RC input demands, while leaving room for + # whatever incidental roll change the gimbal's own stabilization shows as the + # vehicle moves - we are checking nothing *drives* roll, not that it is pinned + # at any particular value + if max_roll_change_deg > 15: + raise NotAchievedException( + "Mount roll changed %.1fdeg in response to RC input - roll should not be commandable" % + max_roll_change_deg) + + def MountSkyDroidNetwork(self): + '''test SkyDroid gimbal connected via a UDP network port rather than a serial port + + the real C11 hardware is UDP-only (no serial control interface), so this + exercises the actual transport used in the field rather than the SITL + serial-port path used by MountSkyDroid''' + self.set_parameters({ + "MNT1_TYPE": 15, # SkyDroid + "CAM1_TYPE": 4, # Mount + "NET_ENABLE": 1, + "NET_P1_TYPE": 1, # UDP client + "NET_P1_PROTOCOL": 8, # gimbal + "NET_P1_IP0": 127, + "NET_P1_IP1": 0, + "NET_P1_IP2": 0, + "NET_P1_IP3": 1, + "NET_P1_PORT": 15006, + }) + # the simulated gimbal listens on a UDP socket rather than + # being attached to one of the autopilot's serial ports: + self.customise_SITL_commandline(["--net-device=skydroid:15006,udp"]) + self.mount_check_camera_information( + "SkyDroid", "C11", + expected_fw_version=1, + expected_cap_flags=0x43, + ) + # command an angle and check the gimbal reports reaching it, + # which requires traffic in both directions: + self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING) + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL, + p1=-30, # pitch angle in degrees + p7=mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING, + ) + self.wait_mount_roll_pitch_yaw_deg(p=-30) + def MountTopotekNetwork(self): '''test Topotek gimbal connected via a network port rather than a serial port''' self.set_parameters({ @@ -20221,6 +20388,9 @@ def tests2b(self): # this block currently around 9.5mins here self.TakeoffWithLocation, self.MountTopotek, self.MountTopotekNetwork, + self.MountSkyDroid, + self.MountSkyDroidC13, + self.MountSkyDroidNetwork, self.MountViewPro, self.MountAVTCM62, self.MountAVTCM62Dual, From 912b14bc4fe67fb0b203a54f3b09d430f1cc9f0e Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Mon, 24 Aug 2026 08:41:26 -0600 Subject: [PATCH 06/19] Tools: enable SkyDroid in build_options.py Registers HAL_MOUNT_SKYDROID_ENABLED as a selectable build option for the new SkyDroid gimbal driver. --- Tools/scripts/build_options.py | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index 8518f7a2eeb112..c445584967d408 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -247,6 +247,7 @@ def config_option(self): Feature('Gimbal', 'SOLOGIMBAL', 'HAL_SOLO_GIMBAL_ENABLED', 'Enable Solo gimbal', 0, "MOUNT"), Feature('Gimbal', 'STORM32_MAVLINK', 'HAL_MOUNT_STORM32MAVLINK_ENABLED', 'Enable SToRM32 MAVLink gimbal', 0, "MOUNT"), Feature('Gimbal', 'STORM32_SERIAL', 'HAL_MOUNT_STORM32SERIAL_ENABLED', 'Enable SToRM32 Serial gimbal', 0, "MOUNT"), + Feature('Gimbal', 'SKYDROID', 'HAL_MOUNT_SKYDROID_ENABLED', 'Enable SkyDroid gimbal', 0, "MOUNT"), Feature('Gimbal', 'TOPOTEK', 'HAL_MOUNT_TOPOTEK_ENABLED', 'Enable Topotek gimbal', 0, "MOUNT"), Feature('Gimbal', 'XACTI', 'HAL_MOUNT_XACTI_ENABLED', 'Enable Xacti gimbal', 0, "MOUNT,DroneCAN"), Feature('Gimbal', 'XFROBOT', 'HAL_MOUNT_XFROBOT_ENABLED', 'Enable XFRobot gimbal', 0, "MOUNT"), From b3fa80748fc2a5c8418ef5b601fbc6230ccadb74 Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Mon, 24 Aug 2026 11:34:17 -0600 Subject: [PATCH 07/19] AP_Mount: SkyDroid keep send_target_angles() in degrees throughout send_target_angles() converted its (already-clamped, already-in-degrees) target back to radians immediately after clamping, only to convert straight back to degrees a few lines later to compute the P-controller's error term - a redundant round trip. Keeps the target and the error calculation in degrees the whole way through instead, converting only at this function's two unavoidable boundaries: angle_rad coming in (AP_Mount's own radians-based target-type convention) and _current_angle_rad (kept in radians for get_attitude_quaternion()'s benefit), read via degrees() at the point of use. Addresses Peter Barker's PR #34155 review comment asking whether centidegrees (send_attitude_to_gimbal(), the vehicle-attitude-to-gimbal command) vs radians (send_target_angles(), the target-angle-from-frontend command) in the same file was intentional - it was (each is the correct unit for what it's talking to), but this removes the specific redundant conversion inside send_target_angles() itself, converting only at the point each wire/API boundary actually requires it. --- libraries/AP_Mount/AP_Mount_SkyDroid.cpp | 24 ++++++++++++++---------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_SkyDroid.cpp b/libraries/AP_Mount/AP_Mount_SkyDroid.cpp index 9afbf3bb39c4de..2ebbb30cbd8460 100644 --- a/libraries/AP_Mount/AP_Mount_SkyDroid.cpp +++ b/libraries/AP_Mount/AP_Mount_SkyDroid.cpp @@ -509,14 +509,18 @@ void AP_Mount_SkyDroid::send_target_angles(const MountAngleTarget& angle_rad) // set gimbal's lock state (follow the body-frame target) set_gimbal_lock(false); - // clamp to the configured MNT1_YAW/PITCH_MIN/MAX range - AP_Mount's frontend does - // not clamp the target itself before calling us. Roll is deliberately absent: - // the gimbal self-stabilizes roll and offers no way to command it (see - // has_roll_control()) - const float yaw_target_rad = radians(constrain_float(degrees(angle_rad.get_bf_yaw()), - _params.yaw_angle_min, _params.yaw_angle_max)); - const float pitch_target_rad = radians(constrain_float(degrees(angle_rad.pitch), - _params.pitch_angle_min, _params.pitch_angle_max)); + // clamp to the configured MNT1_YAW/PITCH_MIN/MAX range (also in degrees) - + // AP_Mount's frontend does not clamp the target itself before calling us. + // Roll is deliberately absent: the gimbal self-stabilizes roll and offers no + // way to command it (see has_roll_control()). Everything below stays in + // degrees from here on - the only unit conversions in this function are the + // unavoidable ones at its boundaries: angle_rad (radians, AP_Mount's own + // target-type convention) coming in, and _current_angle_rad (radians, kept + // that way for get_attitude_quaternion()'s benefit) read via degrees() below + const float yaw_target_deg = constrain_float(degrees(angle_rad.get_bf_yaw()), + _params.yaw_angle_min, _params.yaw_angle_max); + const float pitch_target_deg = constrain_float(degrees(angle_rad.pitch), + _params.pitch_angle_min, _params.pitch_angle_max); // simple P-controller driving GSY/GSP as the rate actuator, using the GAC // attitude feedback already parsed by gimbal_angle_analyse(). @@ -561,8 +565,8 @@ void AP_Mount_SkyDroid::send_target_angles(const MountAngleTarget& angle_rad) // deadzone or kP tuning - re-run the autotest and see before assuming this still // passes constexpr float deadzone_deg = 0.05; // stop correcting once within this many degrees - const float yaw_error_deg = degrees(wrap_PI(yaw_target_rad - _current_angle_rad.z)); - const float pitch_error_deg = degrees(pitch_target_rad - _current_angle_rad.y); + const float yaw_error_deg = wrap_180(yaw_target_deg - degrees(_current_angle_rad.z)); + const float pitch_error_deg = pitch_target_deg - degrees(_current_angle_rad.y); const float yaw_rate_dps = (fabsf(yaw_error_deg) <= deadzone_deg) ? 0.0f : constrain_float(yaw_error_deg * kP, -AP_MOUNT_SKYDROID_AXIS_MAX_DPS, From 579e7b2a401ec3c4a7e5fe63b645683d0ff3be58 Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Tue, 25 Aug 2026 11:06:11 -0600 Subject: [PATCH 08/19] AP_Mount: extract shared #TP-frame protocol layer into AP_Mount_Backend_TPFrame AP_Mount_Topotek and AP_Mount_SkyDroid speak the same "#TP"/"#tp" wire framing (courtesy of a shared OEM hardware supplier), but each carried its own independently-duplicated implementation of the framing/CRC/packet-send layer - byte-for-byte identical in several functions. Adds AP_Mount_Backend_TPFrame, deriving from AP_Mount_Backend_Serial, which both now derive from instead, and moves the shared parts into it: - read_incoming_packets()'s parser state machine, calculate_crc(), hex2char(), send_fixedlen_packet(), send_variablelen_packet(), and the HeaderType/ControlByte/ParseState/Identifier types and _msg_buff/ _msg_buff_len/_parser members. - A handle_message(msg_id) pure-virtual hook the shared parser calls once a packet's CRC is verified, so each product's own command dispatch stays entirely its own. - packetlen_max()/is_valid_address_byte()/source_address_byte() virtuals let each product plug in its own packet-size limit, valid address-byte set, and outgoing source-address rule (Topotek's varies with whether the port is network-attached; SkyDroid's protocol doc confirms 'U' covers both UART and UDP connections, so it doesn't need to vary). Each product's own AddressByte enum, command-identifier set, and all gimbal_*_analyse()/request_*()/send_target_*() functions are untouched and stay entirely separate - only the protocol-content-free framing layer moved. A thin same-signature wrapper in each subclass keeps every existing send_fixedlen_packet()/send_variablelen_packet() call site unchanged. Named after the literal "#TP"/"#tp" marker rather than either company, since neither product's protocol document ever names or expands what "TP" stands for, and the marker is shared by both regardless of which one originated it. Addresses Peter Barker's review of the SkyDroid driver PR (ArduPilot/ardupilot#34155): "framing, parser, CRC, and packet-send functions are copy-pasted from Topotek, and both drivers being compiled in doubles that flash cost" - confirmed directly against the code before starting this (byte-for-byte identical in several functions), then measured via three independent regression passes: SkyDroid's own three autotests against the new base alone, Topotek's two existing autotests (MountTopotek, MountTopotekNetwork) with zero regressions once Topotek moved to the same base, and all five together as a final combined pass - all green throughout. --- .../AP_Mount/AP_Mount_Backend_TPFrame.cpp | 261 +++++++++++++++ libraries/AP_Mount/AP_Mount_Backend_TPFrame.h | 142 +++++++++ libraries/AP_Mount/AP_Mount_SkyDroid.cpp | 299 ++---------------- libraries/AP_Mount/AP_Mount_SkyDroid.h | 94 ++---- libraries/AP_Mount/AP_Mount_Topotek.cpp | 252 +-------------- libraries/AP_Mount/AP_Mount_Topotek.h | 88 ++---- 6 files changed, 492 insertions(+), 644 deletions(-) create mode 100644 libraries/AP_Mount/AP_Mount_Backend_TPFrame.cpp create mode 100644 libraries/AP_Mount/AP_Mount_Backend_TPFrame.h diff --git a/libraries/AP_Mount/AP_Mount_Backend_TPFrame.cpp b/libraries/AP_Mount/AP_Mount_Backend_TPFrame.cpp new file mode 100644 index 00000000000000..80f7f884907415 --- /dev/null +++ b/libraries/AP_Mount/AP_Mount_Backend_TPFrame.cpp @@ -0,0 +1,261 @@ +#include "AP_Mount_config.h" + +#if HAL_MOUNT_TOPOTEK_ENABLED || HAL_MOUNT_SKYDROID_ENABLED + +#include "AP_Mount_Backend_TPFrame.h" + +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +#define AP_MOUNT_TPFRAME_DEBUG 0 +#define debug(fmt, args ...) do { if (AP_MOUNT_TPFRAME_DEBUG) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mount: " fmt, ## args); } } while (0) + +// reading incoming packets from gimbal and confirm they are of the correct format +void AP_Mount_Backend_TPFrame::read_incoming_packets() +{ + // check for bytes on the serial port + const uint16_t nbytes = MIN(_uart->available(), 1024U); + if (nbytes == 0) { + return; + } + + // flag to allow cases below to reset parser state + bool reset_parser = false; + + // process bytes received + for (uint16_t i = 0; i < nbytes; i++) { + uint8_t b; + if (!_uart->read(b)) { + continue; + } + + // add latest byte to buffer + _msg_buff[_msg_buff_len++] = b; + + // protect against overly long messages + if (_msg_buff_len >= packetlen_max()) { + reset_parser = true; + } + + // process byte depending upon current state + switch (_parser.state) { + + case ParseState::WAITING_FOR_HEADER1: + if (b == '#') { + _parser.state = ParseState::WAITING_FOR_HEADER2; + break; + } + reset_parser = true; + break; + + case ParseState::WAITING_FOR_HEADER2: + // 't'/'T' (and 'p'/'P' below) distinguish HeaderType::VARIABLE_LEN from + // FIXED_LEN - meaningful on transmit (see send_variablelen_packet()), but + // deliberately not tracked here on receive: the packet's own Data_Len + // nibble is authoritative regardless of which header case was used to + // send it, so the parser has no need to remember which one it saw + if (b == 't' || b == 'T') { + _parser.state = ParseState::WAITING_FOR_HEADER3; + break; + } + reset_parser = true; + break; + + case ParseState::WAITING_FOR_HEADER3: + if (b == 'p' || b == 'P') { + _parser.state = ParseState::WAITING_FOR_ADDR1; + break; + } + reset_parser = true; + break; + + case ParseState::WAITING_FOR_ADDR1: + case ParseState::WAITING_FOR_ADDR2: + if (is_valid_address_byte(b)) { + // advance to next state + _parser.state = (ParseState)((uint8_t)_parser.state+1); + break; + } + reset_parser = true; + break; + + case ParseState::WAITING_FOR_DATALEN: { + // sanity check data length + uint8_t data_len; + if (hex_char_to_nibble(b, data_len) && data_len <= datalen_max()) { + _parser.data_len = data_len; + _parser.state = ParseState::WAITING_FOR_CONTROL; + break; + } + reset_parser = true; + break; + } + + case ParseState::WAITING_FOR_CONTROL: + // r or w + if (b == 'r' || b == 'w') { + _parser.state = ParseState::WAITING_FOR_ID1; + break; + } + reset_parser = true; + break; + + case ParseState::WAITING_FOR_ID1: + case ParseState::WAITING_FOR_ID2: + case ParseState::WAITING_FOR_ID3: + // 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; + } + reset_parser = true; + break; + + case ParseState::WAITING_FOR_DATA: { + // normally hex numbers in char form (e.g. '0A') + const uint8_t data_bytes_received = _msg_buff_len - (AP_MOUNT_TPFRAME_PACKETLEN_MIN - 2); + + // sanity check to protect against programming errors + if (data_bytes_received > datalen_max()) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + reset_parser = true; + break; + } + + // advance parser state once expected number of bytes have been received + if (data_bytes_received == _parser.data_len) { + _parser.state = ParseState::WAITING_FOR_CRC_LOW; + } + break; + } + + case ParseState::WAITING_FOR_CRC_LOW: + _parser.state = ParseState::WAITING_FOR_CRC_HIGH; + break; + + case ParseState::WAITING_FOR_CRC_HIGH: + // this is the last byte in the message so reset the parser + reset_parser = true; + + // sanity check to protect against programming errors + if (_msg_buff_len < AP_MOUNT_TPFRAME_PACKETLEN_MIN) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + break; + } + + // calculate and check CRC + const uint8_t crc_value = calculate_crc(_msg_buff, _msg_buff_len - 2); + const char crc_char1 = hex2char((crc_value >> 4) & 0x0f); + const char crc_char2 = hex2char((crc_value) & 0x0f); + if (crc_char1 != _msg_buff[_msg_buff_len - 2] || crc_char2 != _msg_buff[_msg_buff_len-1]) { + debug("CRC expected:%x got:%c%c", (int)crc_value, crc_char1, crc_char2); + break; + } + + // CRC is OK, dispatch on the 3-character command ID to the subclass + handle_message((const char*)&_msg_buff[AP_MOUNT_TPFRAME_MSGOFS_ID]); + } + + // handle reset of parser + if (reset_parser) { + _parser.state = ParseState::WAITING_FOR_HEADER1; + _msg_buff_len = 0; + reset_parser = false; + } + } +} + +// calculate checksum +uint8_t AP_Mount_Backend_TPFrame::calculate_crc(const uint8_t *cmd, uint8_t len) const +{ + uint8_t crc = 0; + for (uint16_t i = 0; i= data)) { + return (data + '0'); + } else { + return (data - 10 + 'A'); + } +} + +// send a fixed length packet +bool AP_Mount_Backend_TPFrame::send_fixedlen_packet(uint8_t address, const Identifier id, bool write, uint8_t value) +{ + uint8_t databuff[3]; + 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); +} + +// send variable length packet +bool AP_Mount_Backend_TPFrame::send_variablelen_packet(HeaderType header, uint8_t address, const Identifier id, bool write, const uint8_t* databuff, uint8_t databuff_len) +{ + // exit immediately if not initialised + if (!_initialised) { + return false; + } + + // calculate and sanity check packet size + const uint16_t packet_size = AP_MOUNT_TPFRAME_PACKETLEN_MIN + databuff_len; + if (packet_size > packetlen_max()) { + debug("send_packet data buff too large"); + return false; + } + + // check for sufficient space in outgoing buffer + if (_uart->txspace() < packet_size) { + debug("tx buffer full"); + return false; + } + + // create buffer for holding outgoing packet + uint8_t send_buff[packet_size]; + uint8_t send_buff_ofs = 0; + + // packet header (bytes 0 ~ 2) + send_buff[send_buff_ofs++] = '#'; + send_buff[send_buff_ofs++] = (header == HeaderType::FIXED_LEN) ? 'T' : 't'; + send_buff[send_buff_ofs++] = (header == HeaderType::FIXED_LEN) ? 'P' : 'p'; + + // address (bytes 3, 4) + send_buff[send_buff_ofs++] = source_address_byte(); + send_buff[send_buff_ofs++] = address; + + // data length (byte 5) + send_buff[send_buff_ofs++] = hex2char(databuff_len); + + // control byte (byte 6) + send_buff[send_buff_ofs++] = write ? (uint8_t)ControlByte::WRITE : (uint8_t)ControlByte::READ; + + // identifier (bytes 7 ~ 9) + send_buff[send_buff_ofs++] = id[0]; + send_buff[send_buff_ofs++] = id[1]; + send_buff[send_buff_ofs++] = id[2]; + + // data + if (databuff_len != 0) { + memcpy(&send_buff[send_buff_ofs], databuff, databuff_len); + send_buff_ofs += databuff_len; + } + + // crc + uint8_t crc = calculate_crc(send_buff, send_buff_ofs); + send_buff[send_buff_ofs++] = hex2char((crc >> 4) & 0x0f); + send_buff[send_buff_ofs++] = hex2char(crc & 0x0f); + + // send packet + _uart->write(send_buff, send_buff_ofs); + return true; +} + +#endif // HAL_MOUNT_TOPOTEK_ENABLED || HAL_MOUNT_SKYDROID_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_Backend_TPFrame.h b/libraries/AP_Mount/AP_Mount_Backend_TPFrame.h new file mode 100644 index 00000000000000..2be0c84110eb48 --- /dev/null +++ b/libraries/AP_Mount/AP_Mount_Backend_TPFrame.h @@ -0,0 +1,142 @@ +/* + Shared base for gimbal backends speaking the "#TP"/"#tp" wire framing used + by at least two independent products: Topotek's own gimbal line (see + AP_Mount_Topotek) and SkyDroid's OEM'd gimbal family (see AP_Mount_SkyDroid). + Neither product's protocol document ever names or expands what "TP" stands + for - it appears only as the literal 3-byte frame marker itself. This + class (and its name) is therefore built around that marker, not around + either company's name, since the marker is shared by multiple products + regardless of which one originated it. + + Packet format common to both products (each subclass documents its own + command-identifier set and AddressByte values, which do differ): + + ------------------------------------------------------------------------------------------- + Field Index Bytes Description + ------------------------------------------------------------------------------------------- + Frame Header 0 3 #TP (fixed length) or #tp (variable length) + Address Bit 3 2 source address first, destination address second + Data_Len 5 1 data length (hex nibble, max 0x0F) + Control Bit 6 1 r -> query w -> set/control + Identification Bit 7 3 3 character command identifier + Data 10 Data_Len + Check Bit 2 sum of all preceding bytes, output as 2 ASCII hex + characters (high nibble first) + */ + +#pragma once + +#include "AP_Mount_config.h" + +#if HAL_MOUNT_TOPOTEK_ENABLED || HAL_MOUNT_SKYDROID_ENABLED + +#include "AP_Mount_Backend_Serial.h" + +// preamble layout is fixed by the protocol and identical for every product - +// see the packet-format table above +#define AP_MOUNT_TPFRAME_PACKETLEN_MIN 12 // packet length not including the data segment +#define AP_MOUNT_TPFRAME_MSGOFS_DATALEN 5 // data length, 1 ASCII hex nibble +#define AP_MOUNT_TPFRAME_MSGOFS_ID 7 // 3-character command identifier +#define AP_MOUNT_TPFRAME_MSGOFS_DATA 10 // start of the command-specific data segment +// large enough for the bigger of the two products' own PACKETLEN_MAX (Topotek's 36); +// each subclass's packetlen_max() enforces its own, possibly smaller, real limit +#define AP_MOUNT_TPFRAME_PACKETLEN_MAX 36 + +class AP_Mount_Backend_TPFrame : public AP_Mount_Backend_Serial +{ + +public: + // inherit constructor + using AP_Mount_Backend_Serial::AP_Mount_Backend_Serial; + + // Do not allow copies + CLASS_NO_COPY(AP_Mount_Backend_TPFrame); + +protected: + + // header type (fixed or variable length) + // first three bytes of packet determined by this value + enum class HeaderType : uint8_t { + FIXED_LEN = 0x00, // #TP will be sent + VARIABLE_LEN = 0x01, // #tp will be sent + }; + + // control byte (read or write) + // sent as 7th byte of packet + enum class ControlByte : uint8_t { + READ = 114, // 'r' + WRITE = 119, // 'w' + }; + + // parsing state. Preamble states are shared by both products; a product + // needing extra states beyond WAITING_FOR_DATA would extend this, though + // neither does today + enum class ParseState : uint8_t { + WAITING_FOR_HEADER1 = 0,// # + WAITING_FOR_HEADER2, // T or t + WAITING_FOR_HEADER3, // P or p + WAITING_FOR_ADDR1, // source address + WAITING_FOR_ADDR2, // destination address + WAITING_FOR_DATALEN, + WAITING_FOR_CONTROL, // r or w + WAITING_FOR_ID1, // e.g. 'G' + WAITING_FOR_ID2, // e.g. 'A' + WAITING_FOR_ID3, // e.g. 'C' + WAITING_FOR_DATA, // normally hex numbers in char form (e.g. '0A') + WAITING_FOR_CRC_LOW, + WAITING_FOR_CRC_HIGH, + }; + + // identifier bytes + typedef char Identifier[3]; + + // reading incoming packets from gimbal and confirm they are of the correct + // format. Calls handle_message() once a packet's CRC has been verified + void read_incoming_packets(); + + // called once a complete, CRC-verified packet has been received. + // _msg_buff/_msg_buff_len describe it; msg_id points at its 3-character + // command ID (AP_MOUNT_TPFRAME_MSGOFS_ID into _msg_buff) for convenience + virtual void handle_message(const char* msg_id) = 0; + + // maximum number of bytes in a packet sent to or received from the gimbal. + // Must not exceed AP_MOUNT_TPFRAME_PACKETLEN_MAX (the size of _msg_buff) + virtual uint8_t packetlen_max() const = 0; + + // data segment length can be no more than this + uint8_t datalen_max() const { return packetlen_max() - AP_MOUNT_TPFRAME_PACKETLEN_MIN; } + + // true if b is a valid destination/source address byte for this product - + // each product's own AddressByte enum defines its actual address set + virtual bool is_valid_address_byte(uint8_t b) const = 0; + + // return the address byte to send as the source of our own outgoing + // packets. Each product's protocol document defines its own rule for + // this (e.g. whether UART vs network-attached connections use different + // source addresses), so there is no shared default + virtual uint8_t source_address_byte() const = 0; + + // calculate checksum + uint8_t calculate_crc(const uint8_t *cmd, uint8_t len) const; + + // hexadecimal to character conversion + uint8_t hex2char(uint8_t data) const; + + // send a fixed length packet to gimbal + // returns true on success, false if serial port initialization failed + bool send_fixedlen_packet(uint8_t address, const Identifier id, bool write, uint8_t value); + + // send a variable length packet to gimbal + // returns true on success, false if serial port initialization failed + bool send_variablelen_packet(HeaderType header, uint8_t address, const Identifier id, bool write, const uint8_t* databuff, uint8_t databuff_len); + + // members + uint8_t _msg_buff[AP_MOUNT_TPFRAME_PACKETLEN_MAX]; // buffer holding bytes from latest packet received. only used to calculate crc + uint8_t _msg_buff_len; // number of bytes in the msg buffer + struct { + ParseState state; // parser state + uint8_t data_len; // expected number of data bytes + } _parser; +}; + +#endif // HAL_MOUNT_TOPOTEK_ENABLED || HAL_MOUNT_SKYDROID_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_SkyDroid.cpp b/libraries/AP_Mount/AP_Mount_SkyDroid.cpp index 2ebbb30cbd8460..7e39e7a9c6835d 100644 --- a/libraries/AP_Mount/AP_Mount_SkyDroid.cpp +++ b/libraries/AP_Mount/AP_Mount_SkyDroid.cpp @@ -14,18 +14,8 @@ extern const AP_HAL::HAL& hal; #define AP_MOUNT_SKYDROID_UPDATE_INTERVAL_MS 100 // resend angle or rate targets, and push our attitude, at this interval #define AP_MOUNT_SKYDROID_HEALTH_TIMEOUT_MS 1000 // timeout for health (based on attitude reports from gimbal) -#define AP_MOUNT_SKYDROID_PACKETLEN_MIN 12 // packet length not including the data segment -#define AP_MOUNT_SKYDROID_DATALEN_MAX (AP_MOUNT_SKYDROID_PACKETLEN_MAX - AP_MOUNT_SKYDROID_PACKETLEN_MIN) // data segment len can be no more than this #define AP_MOUNT_SKYDROID_ATTITUDE_RATE_HZ 50 // rate we ask the gimbal to stream its attitude to us (matches the 50hz rate AP_Mount::update() is actually called at; doc allows up to 100hz) -// byte offsets within a received packet - see the packet-format table in this file's -// header comment. Every packet shares this same preamble layout regardless of command; -// only the data segment's own internal layout (if any) differs per command, and is -// documented at each parse function that reads one -#define AP_MOUNT_SKYDROID_MSGOFS_DATALEN 5 // data length, 1 ASCII hex nibble -#define AP_MOUNT_SKYDROID_MSGOFS_ID 7 // 3-character command identifier -#define AP_MOUNT_SKYDROID_MSGOFS_DATA 10 // start of the command-specific data segment - // 3 character identifiers #define AP_MOUNT_SKYDROID_ID3CHAR_GIMBAL_MODE "PTZ" // discrete gimbal control, data bytes: 00:stop, 01:up, 02:down, 03:left, 04:right, 05:center, 06:follow, 07:lock head. Only the follow/lock codes (06/07 - see set_gimbal_lock()) and center (05 - see send_center_command()) are used #define AP_MOUNT_SKYDROID_ID3CHAR_SPEED_YAW "GSY" // individual-axis yaw rate control, data bytes: signed 8bit hex. The only command confirmed to move yaw on real hardware (GAM/GSM/GAY all silently ignored); its sign is also inverted vs the doc - see send_target_rates() @@ -244,171 +234,21 @@ bool AP_Mount_SkyDroid::get_attitude_quaternion(Quaternion& att_quat) return true; } -// reading incoming packets from gimbal and confirm they are of the correct format -void AP_Mount_SkyDroid::read_incoming_packets() +// dispatch on the 3-character command ID to the function that consumes that +// message - called by AP_Mount_Backend_TPFrame::read_incoming_packets() once a +// packet's CRC has been verified +void AP_Mount_SkyDroid::handle_message(const char* msg_id) { - // check for bytes on the serial port - const uint16_t nbytes = MIN(_uart->available(), 1024U); - if (nbytes == 0) { - return; - } - - // flag to allow cases below to reset parser state - bool reset_parser = false; - - // process bytes received - for (uint16_t i = 0; i < nbytes; i++) { - uint8_t b; - if (!_uart->read(b)) { - continue; - } - - // add latest byte to buffer - _msg_buff[_msg_buff_len++] = b; - - // protect against overly long messages - if (_msg_buff_len >= AP_MOUNT_SKYDROID_PACKETLEN_MAX) { - reset_parser = true; - } - - // process byte depending upon current state - switch (_parser.state) { - - case ParseState::WAITING_FOR_HEADER1: - if (b == '#') { - _parser.state = ParseState::WAITING_FOR_HEADER2; - break; - } - reset_parser = true; - break; - - case ParseState::WAITING_FOR_HEADER2: - // 't'/'T' (and 'p'/'P' below) distinguish HeaderType::VARIABLE_LEN from - // FIXED_LEN - meaningful on transmit (see send_variablelen_packet()), but - // deliberately not tracked here on receive: the packet's own Data_Len - // nibble is authoritative regardless of which header case was used to - // send it, so the parser has no need to remember which one it saw - if (b == 't' || b == 'T') { - _parser.state = ParseState::WAITING_FOR_HEADER3; - break; - } - reset_parser = true; - break; - - case ParseState::WAITING_FOR_HEADER3: - if (b == 'p' || b == 'P') { - _parser.state = ParseState::WAITING_FOR_ADDR1; - break; - } - reset_parser = true; - break; - - case ParseState::WAITING_FOR_ADDR1: - case ParseState::WAITING_FOR_ADDR2: - if (b == 'U' || b == 'M' || b == 'D' || b == 'G') { - // advance to next state - _parser.state = (ParseState)((uint8_t)_parser.state+1); - break; - } - reset_parser = true; - break; - - case ParseState::WAITING_FOR_DATALEN: { - // sanity check data length - uint8_t data_len; - if (hex_char_to_nibble(b, data_len) && data_len <= AP_MOUNT_SKYDROID_DATALEN_MAX) { - _parser.data_len = data_len; - _parser.state = ParseState::WAITING_FOR_CONTROL; - break; - } - reset_parser = true; - break; - } - - case ParseState::WAITING_FOR_CONTROL: - // r or w - if (b == 'r' || b == 'w') { - _parser.state = ParseState::WAITING_FOR_ID1; - break; - } - reset_parser = true; - break; - - case ParseState::WAITING_FOR_ID1: - case ParseState::WAITING_FOR_ID2: - case ParseState::WAITING_FOR_ID3: - // 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; - } - reset_parser = true; - break; - - case ParseState::WAITING_FOR_DATA: { - // normally hex numbers in char form (e.g. '0A') - const uint8_t data_bytes_received = _msg_buff_len - (AP_MOUNT_SKYDROID_PACKETLEN_MIN - 2); - - // sanity check to protect against programming errors - if (data_bytes_received > AP_MOUNT_SKYDROID_DATALEN_MAX) { - INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); - reset_parser = true; - break; - } - - // advance parser state once expected number of bytes have been received - if (data_bytes_received == _parser.data_len) { - _parser.state = ParseState::WAITING_FOR_CRC_LOW; - } - break; - } - - case ParseState::WAITING_FOR_CRC_LOW: - _parser.state = ParseState::WAITING_FOR_CRC_HIGH; - break; - - case ParseState::WAITING_FOR_CRC_HIGH: - // this is the last byte in the message so reset the parser - reset_parser = true; - - // sanity check to protect against programming errors - if (_msg_buff_len < AP_MOUNT_SKYDROID_PACKETLEN_MIN) { - INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); - break; - } - - // calculate and check CRC - const uint8_t crc_value = calculate_crc(_msg_buff, _msg_buff_len - 2); - const char crc_char1 = hex2char((crc_value >> 4) & 0x0f); - const char crc_char2 = hex2char((crc_value) & 0x0f); - if (crc_char1 != _msg_buff[_msg_buff_len - 2] || crc_char2 != _msg_buff[_msg_buff_len-1]) { - debug("CRC expected:%x got:%c%c", (int)crc_value, crc_char1, crc_char2); - break; - } - - // CRC is OK, dispatch on the 3-character command ID to the function that - // consumes that message - const char *msg_id = (const char*)&_msg_buff[AP_MOUNT_SKYDROID_MSGOFS_ID]; - if (strncmp(msg_id, "GAC", 3) == 0) { - gimbal_angle_analyse(); - } else if (strncmp(msg_id, "REC", 3) == 0) { - gimbal_record_analyse(); - } else if (strncmp(msg_id, "SDC", 3) == 0) { - gimbal_sdcard_analyse(); - } else if (strncmp(msg_id, "VER", 3) == 0) { - gimbal_version_analyse(); - } else if (strncmp(msg_id, "MOD", 3) == 0) { - gimbal_model_analyse(); - } - } - - // handle reset of parser - if (reset_parser) { - _parser.state = ParseState::WAITING_FOR_HEADER1; - _msg_buff_len = 0; - reset_parser = false; - } + if (strncmp(msg_id, "GAC", 3) == 0) { + gimbal_angle_analyse(); + } else if (strncmp(msg_id, "REC", 3) == 0) { + gimbal_record_analyse(); + } else if (strncmp(msg_id, "SDC", 3) == 0) { + gimbal_sdcard_analyse(); + } else if (strncmp(msg_id, "VER", 3) == 0) { + gimbal_version_analyse(); + } else if (strncmp(msg_id, "MOD", 3) == 0) { + gimbal_model_analyse(); } } @@ -612,9 +452,9 @@ void AP_Mount_SkyDroid::gimbal_angle_analyse() { // consume current angles. data is yaw, pitch, roll in that order, each 4 hex chars, 0.01deg units uint32_t yaw_raw, pitch_raw, roll_raw; - if (!hex_chars_to_uint32((const char*)&_msg_buff[AP_MOUNT_SKYDROID_MSGOFS_DATA], 4, yaw_raw) || - !hex_chars_to_uint32((const char*)&_msg_buff[AP_MOUNT_SKYDROID_MSGOFS_DATA + 4], 4, pitch_raw) || - !hex_chars_to_uint32((const char*)&_msg_buff[AP_MOUNT_SKYDROID_MSGOFS_DATA + 8], 4, roll_raw)) { + if (!hex_chars_to_uint32((const char*)&_msg_buff[AP_MOUNT_TPFRAME_MSGOFS_DATA], 4, yaw_raw) || + !hex_chars_to_uint32((const char*)&_msg_buff[AP_MOUNT_TPFRAME_MSGOFS_DATA + 4], 4, pitch_raw) || + !hex_chars_to_uint32((const char*)&_msg_buff[AP_MOUNT_TPFRAME_MSGOFS_DATA + 8], 4, roll_raw)) { return; } const int16_t yaw_angle_cd = wrap_180_cd((int16_t)yaw_raw); @@ -643,7 +483,7 @@ void AP_Mount_SkyDroid::gimbal_record_analyse() { // data is 2 ASCII chars ("00" or "01") - only the low digit is ever non-zero, so // that's the only one we need to check - _recording = (_msg_buff[AP_MOUNT_SKYDROID_MSGOFS_DATA + 1] == '1'); + _recording = (_msg_buff[AP_MOUNT_TPFRAME_MSGOFS_DATA + 1] == '1'); } // information analysis of gimbal storage card @@ -654,8 +494,8 @@ void AP_Mount_SkyDroid::gimbal_sdcard_analyse() // all-zero (no card), matching this function's previous behaviour static const uint8_t all_zero_chars[10] = {'0','0','0','0','0','0','0','0','0','0'}; bool all_zero = true; - if (_msg_buff_len >= AP_MOUNT_SKYDROID_MSGOFS_DATA + ARRAY_SIZE(all_zero_chars)) { - all_zero = (memcmp(&_msg_buff[AP_MOUNT_SKYDROID_MSGOFS_DATA], all_zero_chars, ARRAY_SIZE(all_zero_chars)) == 0); + if (_msg_buff_len >= AP_MOUNT_TPFRAME_MSGOFS_DATA + ARRAY_SIZE(all_zero_chars)) { + all_zero = (memcmp(&_msg_buff[AP_MOUNT_TPFRAME_MSGOFS_DATA], all_zero_chars, ARRAY_SIZE(all_zero_chars)) == 0); } _sdcard_healthy = !all_zero; } @@ -664,8 +504,8 @@ void AP_Mount_SkyDroid::gimbal_sdcard_analyse() void AP_Mount_SkyDroid::gimbal_version_analyse() { uint8_t data_buf_len; - if (!hex_char_to_nibble(_msg_buff[AP_MOUNT_SKYDROID_MSGOFS_DATALEN], data_buf_len) || data_buf_len == 0 || - _msg_buff[AP_MOUNT_SKYDROID_MSGOFS_DATA] != 'V') { + if (!hex_char_to_nibble(_msg_buff[AP_MOUNT_TPFRAME_MSGOFS_DATALEN], data_buf_len) || data_buf_len == 0 || + _msg_buff[AP_MOUNT_TPFRAME_MSGOFS_DATA] != 'V') { return; } @@ -674,7 +514,7 @@ void AP_Mount_SkyDroid::gimbal_version_analyse() uint8_t ver_count = 0; uint32_t ver_num = 0; for (uint8_t i = 1; i < data_buf_len && ver_count < ARRAY_SIZE(version); i++) { - const uint8_t c = _msg_buff[AP_MOUNT_SKYDROID_MSGOFS_DATA + i]; + const uint8_t c = _msg_buff[AP_MOUNT_TPFRAME_MSGOFS_DATA + i]; if (c == '.') { version[ver_count++] = ver_num; ver_num = 0; @@ -709,11 +549,11 @@ void AP_Mount_SkyDroid::gimbal_version_analyse() void AP_Mount_SkyDroid::gimbal_model_analyse() { uint8_t data_buf_len; - if (!hex_char_to_nibble(_msg_buff[AP_MOUNT_SKYDROID_MSGOFS_DATALEN], data_buf_len) || data_buf_len == 0) { + if (!hex_char_to_nibble(_msg_buff[AP_MOUNT_TPFRAME_MSGOFS_DATALEN], data_buf_len) || data_buf_len == 0) { return; } memset(_model_name, 0, sizeof(_model_name)); - memcpy(_model_name, _msg_buff + AP_MOUNT_SKYDROID_MSGOFS_DATA, MIN((uint8_t)(sizeof(_model_name)-1), data_buf_len)); + memcpy(_model_name, _msg_buff + AP_MOUNT_TPFRAME_MSGOFS_DATA, MIN((uint8_t)(sizeof(_model_name)-1), data_buf_len)); // display gimbal model name to user. Informational only - no control decision // depends on it (see this driver's header comment) @@ -722,95 +562,6 @@ void AP_Mount_SkyDroid::gimbal_model_analyse() _got_model_name = true; } -// calculate checksum -uint8_t AP_Mount_SkyDroid::calculate_crc(const uint8_t *cmd, uint8_t len) const -{ - uint8_t crc = 0; - for (uint16_t i = 0; i= data)) { - return (data + '0'); - } else { - return (data - 10 + 'A'); - } -} - -// send a fixed length packet -bool AP_Mount_SkyDroid::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); - return send_variablelen_packet(HeaderType::FIXED_LEN, address, id, write, databuff, ARRAY_SIZE(databuff)-1); -} - -// send variable length packet -bool AP_Mount_SkyDroid::send_variablelen_packet(HeaderType header, AddressByte address, const Identifier id, bool write, const uint8_t* databuff, uint8_t databuff_len) -{ - // exit immediately if not initialised - if (!_initialised) { - return false; - } - - // calculate and sanity check packet size - const uint16_t packet_size = AP_MOUNT_SKYDROID_PACKETLEN_MIN + databuff_len; - if (packet_size > AP_MOUNT_SKYDROID_PACKETLEN_MAX) { - debug("send_packet data buff too large"); - return false; - } - - // check for sufficient space in outgoing buffer - if (_uart->txspace() < packet_size) { - debug("tx buffer full"); - return false; - } - - // create buffer for holding outgoing packet - uint8_t send_buff[packet_size]; - uint8_t send_buff_ofs = 0; - - // packet header (bytes 0 ~ 2) - send_buff[send_buff_ofs++] = '#'; - send_buff[send_buff_ofs++] = (header == HeaderType::FIXED_LEN) ? 'T' : 't'; - send_buff[send_buff_ofs++] = (header == HeaderType::FIXED_LEN) ? 'P' : 'p'; - - // address (bytes 3, 4). source is always the UDP/external-control address for this gimbal - send_buff[send_buff_ofs++] = (uint8_t)AddressByte::UDP; - send_buff[send_buff_ofs++] = (uint8_t)address; - - // data length (byte 5) - send_buff[send_buff_ofs++] = hex2char(databuff_len); - - // control byte (byte 6) - send_buff[send_buff_ofs++] = write ? (uint8_t)ControlByte::WRITE : (uint8_t)ControlByte::READ; - - // identifier (bytes 7 ~ 9) - send_buff[send_buff_ofs++] = id[0]; - send_buff[send_buff_ofs++] = id[1]; - send_buff[send_buff_ofs++] = id[2]; - - // data - if (databuff_len != 0) { - memcpy(&send_buff[send_buff_ofs], databuff, databuff_len); - send_buff_ofs += databuff_len; - } - - // crc - uint8_t crc = calculate_crc(send_buff, send_buff_ofs); - send_buff[send_buff_ofs++] = hex2char((crc >> 4) & 0x0f); - send_buff[send_buff_ofs++] = hex2char(crc & 0x0f); - - // send packet - _uart->write(send_buff, send_buff_ofs); - return true; -} - // set gimbal's lock vs follow mode // lock should be true if gimbal should maintain an earth-frame target // lock is false to follow / maintain a body-frame target diff --git a/libraries/AP_Mount/AP_Mount_SkyDroid.h b/libraries/AP_Mount/AP_Mount_SkyDroid.h index 0c571d2a9e8479..9c69f6d04b88ac 100644 --- a/libraries/AP_Mount/AP_Mount_SkyDroid.h +++ b/libraries/AP_Mount/AP_Mount_SkyDroid.h @@ -1,11 +1,13 @@ /* SkyDroid gimbal driver using custom serial protocol (usually run over UDP) - Packet format (courtesy of SkyDroid's "TOP" protocol document). This is - the same framing used by SkyDroid's OEM supplier for the Topotek driver - (see AP_Mount_Topotek) but the address bytes, command identifiers and - units used by SkyDroid's own firmware differ, so this is a separate, - independent implementation rather than a subclass. + This is the same "#TP"/"#tp" wire framing used by SkyDroid's OEM supplier + for the Topotek driver (see AP_Mount_Topotek) - both derive from + AP_Mount_Backend_TPFrame, which implements the shared framing/CRC/packet- + send layer. Neither protocol document ever names or expands what "TP" + stands for. The address bytes, command identifiers and units used by + SkyDroid's own firmware differ from Topotek's, so everything past that + shared layer is a separate, independent implementation. ------------------------------------------------------------------------------------------- Field Index Bytes Description @@ -70,19 +72,19 @@ #if HAL_MOUNT_SKYDROID_ENABLED -#include "AP_Mount_Backend_Serial.h" +#include "AP_Mount_Backend_TPFrame.h" #include #include #include #define AP_MOUNT_SKYDROID_PACKETLEN_MAX 28 // maximum number of bytes in a packet sent to or received from the gimbal -class AP_Mount_SkyDroid : public AP_Mount_Backend_Serial +class AP_Mount_SkyDroid : public AP_Mount_Backend_TPFrame { public: // Constructor - using AP_Mount_Backend_Serial::AP_Mount_Backend_Serial; + using AP_Mount_Backend_TPFrame::AP_Mount_Backend_TPFrame; // Do not allow copies CLASS_NO_COPY(AP_Mount_SkyDroid); @@ -171,15 +173,10 @@ class AP_Mount_SkyDroid : public AP_Mount_Backend_Serial private: - // header type (fixed or variable length) - // first three bytes of packet determined by this value - enum class HeaderType : uint8_t { - FIXED_LEN = 0x00, // #TP will be sent - VARIABLE_LEN = 0x01, // #tp will be sent - }; - // address (2nd and 3rd bytes of packet) - // first byte is always U (external/UDP controller) for our outgoing packets + // first byte is always U (external control unit, whether connected over + // UART or UDP - SkyDroid's protocol doesn't distinguish the two at the + // address-byte level) for our outgoing packets enum class AddressByte : uint8_t { SYSTEM_AND_IMAGE = 68, // 'D' GIMBAL = 71, // 'G' @@ -187,30 +184,6 @@ class AP_Mount_SkyDroid : public AP_Mount_Backend_Serial UDP = 85, // 'U' }; - // control byte (read or write) - // sent as 7th byte of packet - enum class ControlByte : uint8_t { - READ = 114, // 'r' - WRITE = 119, // 'w' - }; - - // parsing state - enum class ParseState : uint8_t { - WAITING_FOR_HEADER1 = 0,// # - WAITING_FOR_HEADER2, // T or t - WAITING_FOR_HEADER3, // P or p - WAITING_FOR_ADDR1, // normally U - WAITING_FOR_ADDR2, // M, D, G - WAITING_FOR_DATALEN, - WAITING_FOR_CONTROL, // r or w - WAITING_FOR_ID1, // e.g. 'G' - WAITING_FOR_ID2, // e.g. 'A' - WAITING_FOR_ID3, // e.g. 'C' - WAITING_FOR_DATA, // normally hex numbers in char form (e.g. '0A') - WAITING_FOR_CRC_LOW, - WAITING_FOR_CRC_HIGH, - }; - // steps of the 1hz round-robin housekeeping loop in update() - not every value is // used every cycle (some requests only fire until answered once), and a few // numbers are deliberately left spare for anything added later without needing @@ -225,14 +198,17 @@ class AP_Mount_SkyDroid : public AP_Mount_Backend_Serial NUM_STEPS = 10, // wraps back to VERSION after this - note the gap at 7-9, spare }; - // identifier bytes - typedef char Identifier[3]; - // send text prefix string static const char* send_message_prefix; - // reading incoming packets from gimbal and confirm they are of the correct format - void read_incoming_packets(); + // AP_Mount_Backend_TPFrame overrides - see that class for what each means + void handle_message(const char* msg_id) override; + uint8_t packetlen_max() const override { return AP_MOUNT_SKYDROID_PACKETLEN_MAX; } + bool is_valid_address_byte(uint8_t b) const override { + return b == (uint8_t)AddressByte::UDP || b == (uint8_t)AddressByte::LENS || + b == (uint8_t)AddressByte::SYSTEM_AND_IMAGE || b == (uint8_t)AddressByte::GIMBAL; + } + uint8_t source_address_byte() const override { return (uint8_t)AddressByte::UDP; } // request gimbal to start sending attitude at 10hz void request_gimbal_attitude(); @@ -291,19 +267,15 @@ class AP_Mount_SkyDroid : public AP_Mount_Backend_Serial // gimbal model name analysis (raw ASCII text, e.g. "C13") void gimbal_model_analyse(); - // calculate checksum - uint8_t calculate_crc(const uint8_t *cmd, uint8_t len) const; - - // hexadecimal to character conversion - uint8_t hex2char(uint8_t data) const; - - // send a fixed length packet to gimbal - // returns true on success, false if serial port initialization failed - bool send_fixedlen_packet(AddressByte address, const Identifier id, bool write, uint8_t value); - - // send a variable length packet to gimbal - // returns true on success, false if serial port initialization failed - bool send_variablelen_packet(HeaderType header, AddressByte address, const Identifier id, bool write, const uint8_t* databuff, uint8_t databuff_len); + // thin wrappers keeping call sites typed on our own AddressByte rather than + // the base class's raw uint8_t (which must accommodate every product's own, + // differently-valued AddressByte enum) + bool send_fixedlen_packet(AddressByte address, const Identifier id, bool write, uint8_t value) { + return AP_Mount_Backend_TPFrame::send_fixedlen_packet((uint8_t)address, id, write, value); + } + bool send_variablelen_packet(HeaderType header, AddressByte address, const Identifier id, bool write, const uint8_t* databuff, uint8_t databuff_len) { + return AP_Mount_Backend_TPFrame::send_variablelen_packet(header, (uint8_t)address, id, write, databuff, databuff_len); + } // set gimbal's lock vs follow mode // lock should be true if gimbal should maintain an earth-frame target @@ -327,12 +299,6 @@ class AP_Mount_SkyDroid : public AP_Mount_Backend_Serial uint32_t _last_current_angle_ms; // system time (in milliseconds) that angle information received from the gimbal uint32_t _last_req_current_info_ms; // system time that this driver last requested current gimbal information uint8_t _last_req_step; // 10hz request loop step (different requests are sent at various steps) - uint8_t _msg_buff[AP_MOUNT_SKYDROID_PACKETLEN_MAX]; // buffer holding bytes from latest packet received. only used to calculate crc - uint8_t _msg_buff_len; // number of bytes in the msg buffer - struct { - ParseState state; // parser state - uint8_t data_len; // expected number of data bytes - } _parser; }; #endif // HAL_MOUNT_SKYDROID_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_Topotek.cpp b/libraries/AP_Mount/AP_Mount_Topotek.cpp index 95b3831377c9da..12a8afcc2066a5 100644 --- a/libraries/AP_Mount/AP_Mount_Topotek.cpp +++ b/libraries/AP_Mount/AP_Mount_Topotek.cpp @@ -17,8 +17,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 AP_MOUNT_TOPOTEK_PACKETLEN_MIN 12 // packet length not including the data segment -#define AP_MOUNT_TOPOTEK_DATALEN_MAX (AP_MOUNT_TOPOTEK_PACKETLEN_MAX - AP_MOUNT_TOPOTEK_PACKETLEN_MIN) // data segment lens can be no more tha this // 3 character identifiers # define AP_MOUNT_TOPOTEK_ID3CHAR_CAPTURE "CAP" // take picture, data bytes: 01:RGB + thermal, 02:RGB, 03:thermal, 05:RGB + thermal (with temp measurement) @@ -49,9 +47,6 @@ extern const AP_HAL::HAL& hal; # define AP_MOUNT_TOPOTEK_ID3CHAR_SET_ALT "ALT" // set the gimbal's altitude # define AP_MOUNT_TOPOTEK_ID3CHAR_SET_AZIMUTH "AZI" // set the gimbal's yaw (aka azimuth) -#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) - const char* AP_Mount_Topotek::send_message_prefix = "Mount: Topotek"; // update mount position - should be called periodically @@ -495,158 +490,15 @@ bool AP_Mount_Topotek::get_attitude_quaternion(Quaternion& att_quat) return true; } -// reading incoming packets from gimbal and confirm they are of the correct format -void AP_Mount_Topotek::read_incoming_packets() +// dispatch on the 3-character command ID to the function that consumes that +// message - called by AP_Mount_Backend_TPFrame::read_incoming_packets() once a +// packet's CRC has been verified +void AP_Mount_Topotek::handle_message(const char* msg_id) { - // check for bytes on the serial port - int16_t nbytes = MIN(_uart->available(), 1024U); - if (nbytes <= 0 ) { - return; - } - - // flag to allow cases below to reset parser state - bool reset_parser = false; - - // process bytes received - for (int16_t i = 0; i < nbytes; i++) { - uint8_t b; - if (!_uart->read(b)) { - continue; - } - - // add latest byte to buffer - _msg_buff[_msg_buff_len++] = b; - - // protect against overly long messages - if (_msg_buff_len >= AP_MOUNT_TOPOTEK_PACKETLEN_MAX) { - reset_parser = true; - } - - // process byte depending upon current state - switch (_parser.state) { - - case ParseState::WAITING_FOR_HEADER1: - if (b == '#') { - _parser.state = ParseState::WAITING_FOR_HEADER2; - break; - } - reset_parser = true; - break; - - case ParseState::WAITING_FOR_HEADER2: - if (b == 't' || b == 'T') { - _parser.state = ParseState::WAITING_FOR_HEADER3; - break; - } - reset_parser = true; - break; - - case ParseState::WAITING_FOR_HEADER3: - if (b == 'p' || b == 'P') { - _parser.state = ParseState::WAITING_FOR_ADDR1; - break; - } - reset_parser = true; - break; - - case ParseState::WAITING_FOR_ADDR1: - case ParseState::WAITING_FOR_ADDR2: - if (b == 'U' || b =='M' || b == 'D' || b =='E' || b =='P' || b =='G') { - // advance to next state - _parser.state = (ParseState)((uint8_t)_parser.state+1); - break; - } - reset_parser = true; - break; - - case ParseState::WAITING_FOR_DATALEN: { - // sanity check data length - uint8_t data_len; - if (hex_char_to_nibble(b, data_len) && data_len <= AP_MOUNT_TOPOTEK_DATALEN_MAX) { - _parser.data_len = data_len; - _parser.state = ParseState::WAITING_FOR_CONTROL; - break; - } - reset_parser = true; - break; - } - - case ParseState::WAITING_FOR_CONTROL: - // r or w - if (b == 'r' || b == 'w') { - _parser.state = ParseState::WAITING_FOR_ID1; - break; - } - reset_parser = true; - break; - - case ParseState::WAITING_FOR_ID1: - case ParseState::WAITING_FOR_ID2: - case ParseState::WAITING_FOR_ID3: - // 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; - } - reset_parser = true; - break; - - case ParseState::WAITING_FOR_DATA: { - // normally hex numbers in char form (e.g. '0A') - const uint8_t data_bytes_received = _msg_buff_len - (AP_MOUNT_TOPOTEK_PACKETLEN_MIN - 2); - - // sanity check to protect against programming errors - if (data_bytes_received > AP_MOUNT_TOPOTEK_DATALEN_MAX) { - INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); - reset_parser = true; - break; - } - - // advance parser state once expected number of bytes have been received - if (data_bytes_received == _parser.data_len) { - _parser.state = ParseState::WAITING_FOR_CRC_LOW; - } - break; - } - - case ParseState::WAITING_FOR_CRC_LOW: - _parser.state = ParseState::WAITING_FOR_CRC_HIGH; + for (uint8_t count = 0; count < AP_MOUNT_RECV_GIMBAL_CMD_CATEGORIES_NUM; count++) { + if (strncmp(msg_id, (const char*)(uart_recv_cmd_compare_list[count].uart_cmd_key), 3) == 0) { + (this->*(uart_recv_cmd_compare_list[count].func))(); break; - - case ParseState::WAITING_FOR_CRC_HIGH: - // this is the last byte in the message so reset the parser - reset_parser = true; - - // sanity check to protect against programming errors - if (_msg_buff_len < AP_MOUNT_TOPOTEK_PACKETLEN_MIN) { - INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); - break; - } - - // calculate and check CRC - const uint8_t crc_value = calculate_crc(_msg_buff, _msg_buff_len - 2); - const char crc_char1 = hex2char((crc_value >> 4) & 0x0f); - const char crc_char2 = hex2char((crc_value) & 0x0f); - if (crc_char1 != _msg_buff[_msg_buff_len - 2] || crc_char2 != _msg_buff[_msg_buff_len-1]) { - debug("CRC expected:%x got:%c%c", (int)crc_value, crc_char1, crc_char2); - break; - } - - // CRC is OK, call function to process the message - for (uint8_t count = 0; count < AP_MOUNT_RECV_GIMBAL_CMD_CATEGORIES_NUM; count++) { - if (strncmp((const char*)_msg_buff + 7, (const char*)(uart_recv_cmd_compare_list[count].uart_cmd_key), 3) == 0) { - (this->*(uart_recv_cmd_compare_list[count].func))(); - break; - } - } - } - - // handle reset of parser - if (reset_parser) { - _parser.state = ParseState::WAITING_FOR_HEADER1; - _msg_buff_len = 0; - reset_parser = false; } } } @@ -1032,96 +884,6 @@ void AP_Mount_Topotek::gimbal_model_name_analyse() _got_gimbal_model_name = true; } -// calculate checksum -uint8_t AP_Mount_Topotek::calculate_crc(const uint8_t *cmd, uint8_t len) const -{ - uint8_t crc = 0; - for (uint16_t i = 0; i= data)) { - return (data + '0'); - } else { - return (data - 10 + 'A'); - } -} - - -// send a fixed length packet -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); - return send_variablelen_packet(HeaderType::FIXED_LEN, address, id, write, databuff, ARRAY_SIZE(databuff)-1); -} - -// send variable length packet -bool AP_Mount_Topotek::send_variablelen_packet(HeaderType header, AddressByte address, const Identifier id, bool write, const uint8_t* databuff, uint8_t databuff_len) -{ - // exit immediately if not initialised - if (!_initialised) { - return false; - } - - // calculate and sanity check packet size - const uint16_t packet_size = AP_MOUNT_TOPOTEK_PACKETLEN_MIN + databuff_len; - if (packet_size > AP_MOUNT_TOPOTEK_PACKETLEN_MAX) { - debug("send_packet data buff too large"); - return false; - } - - // check for sufficient space in outgoing buffer - if (_uart->txspace() < packet_size) { - debug("tx buffer full"); - return false; - } - - // create buffer for holding outgoing packet - uint8_t send_buff[packet_size]; - uint8_t send_buff_ofs = 0; - - // packet header (bytes 0 ~ 2) - send_buff[send_buff_ofs++] = '#'; - send_buff[send_buff_ofs++] = (header == HeaderType::FIXED_LEN) ? 'T' : 't'; - send_buff[send_buff_ofs++] = (header == HeaderType::FIXED_LEN) ? 'P' : 'p'; - - // address (bytes 3, 4) - send_buff[send_buff_ofs++] = (uint8_t)source_address(); - send_buff[send_buff_ofs++] = (uint8_t)address; - - // data length (byte 5) - send_buff[send_buff_ofs++] = hex2char(databuff_len); - - // control byte (byte 6) - send_buff[send_buff_ofs++] = write ? (uint8_t)ControlByte::WRITE : (uint8_t)ControlByte::READ; - - // identified (bytes 7 ~ 9) - send_buff[send_buff_ofs++] = id[0]; - send_buff[send_buff_ofs++] = id[1]; - send_buff[send_buff_ofs++] = id[2]; - - // data - if (databuff_len != 0) { - memcpy(&send_buff[send_buff_ofs], databuff, databuff_len); - send_buff_ofs += databuff_len; - } - - // crc - uint8_t crc = calculate_crc(send_buff, send_buff_ofs); - send_buff[send_buff_ofs++] = hex2char((crc >> 4) & 0x0f); - send_buff[send_buff_ofs++] = hex2char(crc & 0x0f); - - // send packet - _uart->write(send_buff, send_buff_ofs); - return true; -} - // set gimbal's lock vs follow mode // lock should be true if gimbal should maintain an earth-frame target // lock is false to follow / maintain a body-frame target diff --git a/libraries/AP_Mount/AP_Mount_Topotek.h b/libraries/AP_Mount/AP_Mount_Topotek.h index adead6799e4e6e..1ed114590710a9 100644 --- a/libraries/AP_Mount/AP_Mount_Topotek.h +++ b/libraries/AP_Mount/AP_Mount_Topotek.h @@ -1,6 +1,10 @@ /* Topotek gimbal driver using custom serial protocol + This "#TP"/"#tp" wire framing is also used by SkyDroid's OEM'd gimbal + family (see AP_Mount_SkyDroid) - both derive from AP_Mount_Backend_TPFrame, + which implements the shared framing/CRC/packet-send layer. + Packet format (courtesy of Topotek's SDK document) ------------------------------------------------------------------------------------------- @@ -22,7 +26,7 @@ #if HAL_MOUNT_TOPOTEK_ENABLED -#include "AP_Mount_Backend_Serial.h" +#include "AP_Mount_Backend_TPFrame.h" #include #include #include @@ -30,12 +34,12 @@ #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 7 // parse the number of gimbal command types -class AP_Mount_Topotek : public AP_Mount_Backend_Serial +class AP_Mount_Topotek : public AP_Mount_Backend_TPFrame { public: // Constructor - using AP_Mount_Backend_Serial::AP_Mount_Backend_Serial; + using AP_Mount_Backend_TPFrame::AP_Mount_Backend_TPFrame; // Do not allow copies CLASS_NO_COPY(AP_Mount_Topotek); @@ -132,13 +136,6 @@ class AP_Mount_Topotek : public AP_Mount_Backend_Serial private: - // header type (fixed or variable length) - // first three bytes of packet determined by this value - enum class HeaderType : uint8_t { - FIXED_LEN = 0x00, // #TP will be sent - VARIABLE_LEN = 0x01, // #tp will be sent - }; - // address (2nd and 3rd bytes of packet) // first byte is always U followed by one of the other options enum class AddressByte : uint8_t { @@ -150,30 +147,6 @@ class AP_Mount_Topotek : public AP_Mount_Backend_Serial UART = 85, // 'U' }; - // control byte (read or write) - // sent as 7th byte of packet - enum class ControlByte : uint8_t { - READ = 114, // 'r' - WRITE = 119, // 'w' - }; - - // parsing state - enum class ParseState : uint8_t { - WAITING_FOR_HEADER1 = 0,// # - WAITING_FOR_HEADER2, // T or t - WAITING_FOR_HEADER3, // P or p - WAITING_FOR_ADDR1, // normally U - WAITING_FOR_ADDR2, // M, D, E, P, G - WAITING_FOR_DATALEN, - WAITING_FOR_CONTROL, // r or w - WAITING_FOR_ID1, // e.g. 'G' - WAITING_FOR_ID2, // e.g. 'A' - WAITING_FOR_ID3, // e.g. 'C' - WAITING_FOR_DATA, // normally hex numbers in char form (e.g. '0A') - WAITING_FOR_CRC_LOW, - WAITING_FOR_CRC_HIGH, - }; - // tracking status enum class TrackingStatus : uint8_t { STOPPED_TRACKING = 0x30, // not tracking @@ -182,14 +155,17 @@ class AP_Mount_Topotek : public AP_Mount_Backend_Serial LENS_UNSUPPORT_TRACK = 0x34, // this lens does not support tracking }; - // identifier bytes - typedef char Identifier[3]; - // send text prefix string static const char* send_message_prefix; - // reading incoming packets from gimbal and confirm they are of the correct format - void read_incoming_packets(); + // AP_Mount_Backend_TPFrame overrides - see that class for what each means + void handle_message(const char* msg_id) override; + uint8_t packetlen_max() const override { return AP_MOUNT_TOPOTEK_PACKETLEN_MAX; } + bool is_valid_address_byte(uint8_t b) const override { + return b == (uint8_t)AddressByte::UART || b == (uint8_t)AddressByte::LENS || + b == (uint8_t)AddressByte::SYSTEM_AND_IMAGE || b == (uint8_t)AddressByte::AUXILIARY_EQUIPMENT || + b == (uint8_t)AddressByte::NETWORK || b == (uint8_t)AddressByte::GIMBAL; + } // request gimbal attitude void request_gimbal_attitude(); @@ -239,28 +215,24 @@ class AP_Mount_Topotek : public AP_Mount_Backend_Serial // gimbal distance information analysis void gimbal_dist_info_analyse(); - // return the address to send as the source of our packets. The + // return the address byte to send as the source of our packets. The // gimbal sends its replies out of the interface named here, so this // must be the interface we are actually connected to. This is one // of the few places it is legitimate to ask a port how it is // connected; the protocol itself carries that information - AddressByte source_address() const { - return _uart->is_network_port() ? AddressByte::NETWORK : AddressByte::UART; + uint8_t source_address_byte() const override { + return (uint8_t)(_uart->is_network_port() ? AddressByte::NETWORK : AddressByte::UART); } - // calculate checksum - uint8_t calculate_crc(const uint8_t *cmd, uint8_t len) const; - - // hexadecimal to character conversion - uint8_t hex2char(uint8_t data) const; - - // send a fixed length packet to gimbal - // returns true on success, false if serial port initialization failed - bool send_fixedlen_packet(AddressByte address, const Identifier id, bool write, uint8_t value); - - // send a variable length packet to gimbal - // returns true on success, false if serial port initialization failed - bool send_variablelen_packet(HeaderType header, AddressByte address, const Identifier id, bool write, const uint8_t* databuff, uint8_t databuff_len); + // thin wrappers keeping call sites typed on our own AddressByte rather than + // the base class's raw uint8_t (which must accommodate every product's own, + // differently-valued AddressByte enum) + bool send_fixedlen_packet(AddressByte address, const Identifier id, bool write, uint8_t value) { + return AP_Mount_Backend_TPFrame::send_fixedlen_packet((uint8_t)address, id, write, value); + } + bool send_variablelen_packet(HeaderType header, AddressByte address, const Identifier id, bool write, const uint8_t* databuff, uint8_t databuff_len) { + return AP_Mount_Backend_TPFrame::send_variablelen_packet(header, (uint8_t)address, id, write, databuff, databuff_len); + } // set gimbal's lock vs follow mode // lock should be true if gimbal should maintain an earth-frame target @@ -287,12 +259,6 @@ class AP_Mount_Topotek : public AP_Mount_Backend_Serial uint8_t _last_req_step; // 10hz request loop step (different requests are sent at various steps) uint8_t _stop_order_count; // number of stop commands sent since target rates became zero float _measure_dist_m = -1.0f; // latest rangefinder distance (in meters) - uint8_t _msg_buff[AP_MOUNT_TOPOTEK_PACKETLEN_MAX]; // buffer holding bytes from latest packet received. only used to calculate crc - uint8_t _msg_buff_len; // number of bytes in the msg buffer - struct { - ParseState state; // parser state - uint8_t data_len; // expected number of data bytes - } _parser; // mapping from received message key to member function pointer to consume the message typedef struct { From 4b32e8069a7d9af9263159c452eaa5ce5c1563c6 Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Tue, 25 Aug 2026 11:18:51 -0600 Subject: [PATCH 09/19] AP_Mount: share angle-error-to-rate P-controller between Siyi and SkyDroid AP_Mount_Siyi::send_target_angles() and AP_Mount_SkyDroid::send_target_angles() both hand-roll the same simple P-controller shape (angle error * gain, constrained to a rate limit, with SkyDroid additionally applying a deadzone) to convert an angle error into the rate command their wire protocol actually sends. Adds AP_Mount_Backend::angle_error_to_rate(error, gain, rate_max, deadzone), a small shared static helper on the common base class, and uses it from both. It is unit-agnostic (error/deadzone/rate_max just need to share one consistent unit - degrees for SkyDroid's deg/s wire value, an arbitrary -100..100 scalar for Siyi's), so no unit conversion changes at either call site - each just passes its own existing gain/limit constants. Addresses Peter Barker's review of the SkyDroid driver PR (ArduPilot/ardupilot#34155), which asked that this duplication ("Siyi backend *does* convert angles to rates, so we could potentially swipe code into here") be shared now rather than deferred to a follow-up, since a future extraction might not have gimbal hardware available to validate against, unlike now. Siyi's angle-to-rate conversion also does earth/body frame switching and an upside-down mounting transform that stay entirely its own - only the actual P-controller arithmetic is shared, not the surrounding per-product logic. Verified via the full mount regression set: MountSiyiZT30, MountTopotek, MountTopotekNetwork, and all three SkyDroid autotests - no regressions. --- libraries/AP_Mount/AP_Mount_Backend.cpp | 10 ++++++++++ libraries/AP_Mount/AP_Mount_Backend.h | 12 ++++++++++++ libraries/AP_Mount/AP_Mount_Siyi.cpp | 4 ++-- libraries/AP_Mount/AP_Mount_SkyDroid.cpp | 10 ++-------- 4 files changed, 26 insertions(+), 10 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 653981f64c7594..cc9e9524db1825 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -1119,6 +1119,16 @@ void AP_Mount_Backend::update_angle_target_from_rate(const MountRateTarget& rate } } +// simple P-controller converting an angle error to a rate command - see this +// function's declaration for the full explanation +float AP_Mount_Backend::angle_error_to_rate(float error, float gain, float rate_max, float deadzone) +{ + if (fabsf(error) <= deadzone) { + return 0.0f; + } + return constrain_float(error * gain, -rate_max, rate_max); +} + // helper function to provide GIMBAL_DEVICE_FLAGS for use in GIMBAL_DEVICE_ATTITUDE_STATUS message uint16_t AP_Mount_Backend::get_gimbal_device_flags() const { diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 2cc192fe4c684a..f1c4a693ab3efe 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -407,6 +407,18 @@ class AP_Mount_Backend // assumes a 50hz update rate void update_angle_target_from_rate(const MountRateTarget& rate_rad, MountAngleTarget& angle_rad) const; + // simple P-controller converting an angle error to a rate command: multiply + // by gain, then constrain to +/- rate_max. A non-zero deadzone forces a + // clean zero output for small errors instead of tapering to an + // ever-smaller command - useful for a rate actuator with a coarse + // resolution floor, where a command below that floor is simply rounded + // away by the wire encoding anyway, so continuing to send one just adds + // dither with no actual effect. Unit-agnostic: error, deadzone and + // rate_max must all share one consistent unit (e.g. all in deg, all in + // rad/s, or all as a normalised -100..100 scalar) - gain converts between + // error's unit and the returned rate's + static float angle_error_to_rate(float error, float gain, float rate_max, float deadzone = 0.0f); + // helper function to provide GIMBAL_DEVICE_FLAGS for use in GIMBAL_DEVICE_ATTITUDE_STATUS message uint16_t get_gimbal_device_flags() const; diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index 4def266dc727e3..9c66abb27c84ba 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.cpp +++ b/libraries/AP_Mount/AP_Mount_Siyi.cpp @@ -665,7 +665,7 @@ void AP_Mount_Siyi::send_target_angles(const MountAngleTarget &angle_rad) // use simple P controller to convert pitch angle error (in radians) to a target rate scalar (-100 to +100) const float pitch_err_rad = (pitch_rad - current_angle_transformed.y); - const float pitch_rate_scalar = constrain_float(100.0 * pitch_err_rad * AP_MOUNT_SIYI_PITCH_P / AP_MOUNT_SIYI_RATE_MAX_RADS, -100, 100); + const float pitch_rate_scalar = angle_error_to_rate(pitch_err_rad, 100.0 * AP_MOUNT_SIYI_PITCH_P / AP_MOUNT_SIYI_RATE_MAX_RADS, 100.0f); // convert yaw angle to body-frame float yaw_bf_rad = yaw_is_ef ? wrap_PI(yaw_rad - AP::ahrs().get_yaw_rad()) : yaw_rad; @@ -680,7 +680,7 @@ void AP_Mount_Siyi::send_target_angles(const MountAngleTarget &angle_rad) // use simple P controller to convert yaw angle error to a target rate scalar (-100 to +100) const float yaw_err_rad = (yaw_bf_rad - current_angle_transformed.z); - const float yaw_rate_scalar = constrain_float(100.0 * yaw_err_rad * AP_MOUNT_SIYI_YAW_P / AP_MOUNT_SIYI_RATE_MAX_RADS, -100, 100); + const float yaw_rate_scalar = angle_error_to_rate(yaw_err_rad, 100.0 * AP_MOUNT_SIYI_YAW_P / AP_MOUNT_SIYI_RATE_MAX_RADS, 100.0f); // rotate gimbal. pitch_rate and yaw_rate are scalars in the range -100 ~ +100 rotate_gimbal(pitch_rate_scalar, yaw_rate_scalar, yaw_is_ef); diff --git a/libraries/AP_Mount/AP_Mount_SkyDroid.cpp b/libraries/AP_Mount/AP_Mount_SkyDroid.cpp index 7e39e7a9c6835d..c84bad36d402fc 100644 --- a/libraries/AP_Mount/AP_Mount_SkyDroid.cpp +++ b/libraries/AP_Mount/AP_Mount_SkyDroid.cpp @@ -407,14 +407,8 @@ void AP_Mount_SkyDroid::send_target_angles(const MountAngleTarget& angle_rad) constexpr float deadzone_deg = 0.05; // stop correcting once within this many degrees const float yaw_error_deg = wrap_180(yaw_target_deg - degrees(_current_angle_rad.z)); const float pitch_error_deg = pitch_target_deg - degrees(_current_angle_rad.y); - const float yaw_rate_dps = (fabsf(yaw_error_deg) <= deadzone_deg) ? 0.0f : - constrain_float(yaw_error_deg * kP, - -AP_MOUNT_SKYDROID_AXIS_MAX_DPS, - AP_MOUNT_SKYDROID_AXIS_MAX_DPS); - const float pitch_rate_dps = (fabsf(pitch_error_deg) <= deadzone_deg) ? 0.0f : - constrain_float(pitch_error_deg * kP, - -AP_MOUNT_SKYDROID_AXIS_MAX_DPS, - AP_MOUNT_SKYDROID_AXIS_MAX_DPS); + const float yaw_rate_dps = angle_error_to_rate(yaw_error_deg, kP, AP_MOUNT_SKYDROID_AXIS_MAX_DPS, deadzone_deg); + const float pitch_rate_dps = angle_error_to_rate(pitch_error_deg, kP, AP_MOUNT_SKYDROID_AXIS_MAX_DPS, deadzone_deg); // GSY's sign is inverted vs AP_Mount's convention (see send_axis_rate below) send_axis_rate(AP_MOUNT_SKYDROID_ID3CHAR_SPEED_YAW, -yaw_rate_dps); From c5aa84527fd22651eca2a0cd29bdb612c820048a Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Thu, 27 Aug 2026 10:11:32 -0600 Subject: [PATCH 10/19] AP_Mount: SkyDroid stop commanding a stale-attitude rate send_target_angles() computed error against _current_angle_rad with no freshness check - if GAC attitude reports stopped arriving, the frozen error kept producing a nonzero rate command indefinitely instead of stopping. Gate on healthy() and command an explicit zero rate on both axes when attitude feedback is stale, rather than silently continuing to drive off old data. healthy() itself had a related gap: _last_current_angle_ms starts at 0, so (millis() - 0) < HEALTH_TIMEOUT_MS was true for the first second after boot even with no attitude ever received, masking the exact condition send_target_angles() now needs to detect. Add the same "never heard from the gimbal" guard get_attitude_quaternion() already uses. Flagged by AI-assisted review on PR #34155 (tridge/Claude, cross-checked by Codex), verified against the code before fixing. --- libraries/AP_Mount/AP_Mount_SkyDroid.cpp | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Mount/AP_Mount_SkyDroid.cpp b/libraries/AP_Mount/AP_Mount_SkyDroid.cpp index c84bad36d402fc..80323c88e135ab 100644 --- a/libraries/AP_Mount/AP_Mount_SkyDroid.cpp +++ b/libraries/AP_Mount/AP_Mount_SkyDroid.cpp @@ -128,8 +128,12 @@ bool AP_Mount_SkyDroid::healthy() const return false; } - // unhealthy if attitude information not received recently + // unhealthy until we've heard at least one "GAC" attitude report, and if + // attitude information has not been received recently since then const uint32_t last_current_angle_ms = _last_current_angle_ms; + if (last_current_angle_ms == 0) { + return false; + } return (AP_HAL::millis() - last_current_angle_ms < AP_MOUNT_SKYDROID_HEALTH_TIMEOUT_MS); } @@ -362,6 +366,18 @@ void AP_Mount_SkyDroid::send_target_angles(const MountAngleTarget& angle_rad) const float pitch_target_deg = constrain_float(degrees(angle_rad.pitch), _params.pitch_angle_min, _params.pitch_angle_max); + // if GAC attitude reports have stopped arriving, _current_angle_rad is stale - + // the error computed against it below would be wrong, and (since it's held + // fixed once the feed drops) could keep commanding a nonzero rate indefinitely + // rather than converging. Command an explicit stop instead of just skipping + // the send, since a resumed-later GAC feed is the only thing that would + // otherwise correct a stale outstanding rate command + if (!healthy()) { + send_axis_rate(AP_MOUNT_SKYDROID_ID3CHAR_SPEED_YAW, 0); + send_axis_rate(AP_MOUNT_SKYDROID_ID3CHAR_SPEED_PITCH, 0); + return; + } + // simple P-controller driving GSY/GSP as the rate actuator, using the GAC // attitude feedback already parsed by gimbal_angle_analyse(). // From 54d439ea6524f4fa4423bce7b52cc1a2b48cc90b Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Thu, 27 Aug 2026 10:11:45 -0600 Subject: [PATCH 11/19] AP_Mount: SkyDroid guard short data segments, fix stale rate comment gimbal_angle_analyse() and gimbal_record_analyse() read fixed offsets into the data segment without checking _parser.data_len first. A short-but-CRC-valid GAC/REC packet (data_len less than what these functions expect) would read past its own data into whatever bytes were left in _msg_buff from a previous, larger packet - not a memory- safety issue (the buffer is fixed-size and in bounds), but stale bytes could silently be interpreted as this packet's content. Match the length-checking style already used by gimbal_sdcard_analyse() et al. Also fix two comments claiming the GAA attitude-streaming request asks for 10hz - it actually requests AP_MOUNT_SKYDROID_ATTITUDE_RATE_HZ (50). The other "10hz" comments in this file are correct as-is (they describe the separate, genuinely-10hz request-loop throttle). Flagged by AI-assisted review on PR #34155 (tridge/Claude, cross-checked by Codex), verified against the code before fixing. --- libraries/AP_Mount/AP_Mount_SkyDroid.cpp | 8 +++++++- libraries/AP_Mount/AP_Mount_SkyDroid.h | 2 +- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_SkyDroid.cpp b/libraries/AP_Mount/AP_Mount_SkyDroid.cpp index 80323c88e135ab..14ca51304f15ea 100644 --- a/libraries/AP_Mount/AP_Mount_SkyDroid.cpp +++ b/libraries/AP_Mount/AP_Mount_SkyDroid.cpp @@ -256,7 +256,7 @@ void AP_Mount_SkyDroid::handle_message(const char* msg_id) } } -// request gimbal to (re)start sending us attitude at 10hz +// request gimbal to (re)start sending us attitude at AP_MOUNT_SKYDROID_ATTITUDE_RATE_HZ void AP_Mount_SkyDroid::request_gimbal_attitude() { // "GAA": enable/disable gimbal->us attitude streaming, data bytes: 00:off, @@ -461,6 +461,9 @@ void AP_Mount_SkyDroid::send_axis_rate(const Identifier id, float rate_dps) void AP_Mount_SkyDroid::gimbal_angle_analyse() { // consume current angles. data is yaw, pitch, roll in that order, each 4 hex chars, 0.01deg units + if (_parser.data_len < 12) { + return; + } uint32_t yaw_raw, pitch_raw, roll_raw; if (!hex_chars_to_uint32((const char*)&_msg_buff[AP_MOUNT_TPFRAME_MSGOFS_DATA], 4, yaw_raw) || !hex_chars_to_uint32((const char*)&_msg_buff[AP_MOUNT_TPFRAME_MSGOFS_DATA + 4], 4, pitch_raw) || @@ -493,6 +496,9 @@ void AP_Mount_SkyDroid::gimbal_record_analyse() { // data is 2 ASCII chars ("00" or "01") - only the low digit is ever non-zero, so // that's the only one we need to check + if (_parser.data_len < 2) { + return; + } _recording = (_msg_buff[AP_MOUNT_TPFRAME_MSGOFS_DATA + 1] == '1'); } diff --git a/libraries/AP_Mount/AP_Mount_SkyDroid.h b/libraries/AP_Mount/AP_Mount_SkyDroid.h index 9c69f6d04b88ac..e738e2596c1846 100644 --- a/libraries/AP_Mount/AP_Mount_SkyDroid.h +++ b/libraries/AP_Mount/AP_Mount_SkyDroid.h @@ -210,7 +210,7 @@ class AP_Mount_SkyDroid : public AP_Mount_Backend_TPFrame } uint8_t source_address_byte() const override { return (uint8_t)AddressByte::UDP; } - // request gimbal to start sending attitude at 10hz + // request gimbal to start sending attitude at AP_MOUNT_SKYDROID_ATTITUDE_RATE_HZ void request_gimbal_attitude(); // request gimbal memory card information From f63460a88c20e0cdf27db77d45c5d324b1ba3053 Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Fri, 28 Aug 2026 14:53:56 -0600 Subject: [PATCH 12/19] AP_Mount: SkyDroid track SD card state as unknown/present/absent take_picture()/record_video() refused every request while _sdcard_healthy was false - which was also its default value until the first "SDC" reply arrived. SDC is camera-addressed like MOD, which this driver's own header comment already documents as taking anywhere from under a second to 8+ minutes to reply on real hardware, so a mission-triggered capture could silently fail for that entire window. Replace the bool with a tri-state (UNKNOWN/PRESENT/ABSENT) and only refuse on a confirmed ABSENT reply, attempting the capture while still UNKNOWN. Also fix gimbal_sdcard_analyse()'s length gate: it checked _msg_buff_len (which includes the 2 trailing CRC characters) against the expected data length, so an SDC reply with data_len 8 or 9 would pass and 1-2 CRC characters would be read as card-capacity data - a nonzero CRC char then falsely reads as "card present". Gate on _parser.data_len instead, which is the actual number of real data bytes. Flagged by AI-assisted review on PR #34155 (tridge/Claude, cross-checked by Codex), verified against the code before fixing. --- libraries/AP_Mount/AP_Mount_SkyDroid.cpp | 27 ++++++++++++++---------- libraries/AP_Mount/AP_Mount_SkyDroid.h | 13 +++++++++++- 2 files changed, 28 insertions(+), 12 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_SkyDroid.cpp b/libraries/AP_Mount/AP_Mount_SkyDroid.cpp index 14ca51304f15ea..2dfec8ba36b46a 100644 --- a/libraries/AP_Mount/AP_Mount_SkyDroid.cpp +++ b/libraries/AP_Mount/AP_Mount_SkyDroid.cpp @@ -145,8 +145,11 @@ bool AP_Mount_SkyDroid::take_picture() return false; } - // exit immediately if the memory card is abnormal - if (!_sdcard_healthy) { + // exit immediately if the memory card is confirmed absent - UNKNOWN (no SDC + // reply yet) is allowed through, since that reply can take minutes (see + // SDCardState's comment) and we'd rather attempt the capture than silently + // refuse every request until it arrives + if (_sdcard_state == SDCardState::ABSENT) { GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "%s SD card error", send_message_prefix); return false; } @@ -164,8 +167,8 @@ bool AP_Mount_SkyDroid::record_video(bool start_recording) return false; } - // exit immediately if the memory card is abnormal - if (!_sdcard_healthy) { + // exit immediately if the memory card is confirmed absent (see take_picture()) + if (_sdcard_state == SDCardState::ABSENT) { GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "%s SD card error", send_message_prefix); return false; } @@ -506,14 +509,16 @@ void AP_Mount_SkyDroid::gimbal_record_analyse() void AP_Mount_SkyDroid::gimbal_sdcard_analyse() { // data is 10 hex chars: 5 for remaining capacity, 5 for total capacity (units MB) - // all zeros means no card inserted. A too-short buffer is treated the same as - // all-zero (no card), matching this function's previous behaviour - static const uint8_t all_zero_chars[10] = {'0','0','0','0','0','0','0','0','0','0'}; - bool all_zero = true; - if (_msg_buff_len >= AP_MOUNT_TPFRAME_MSGOFS_DATA + ARRAY_SIZE(all_zero_chars)) { - all_zero = (memcmp(&_msg_buff[AP_MOUNT_TPFRAME_MSGOFS_DATA], all_zero_chars, ARRAY_SIZE(all_zero_chars)) == 0); + // all zeros means no card inserted. Gate on _parser.data_len (the actual number + // of data bytes), not _msg_buff_len (which also counts the 2 trailing CRC chars) - + // otherwise a data_len of 8 or 9 would read 1-2 CRC characters as if they were + // card-capacity data, and a nonzero CRC char would falsely read as "card present" + if (_parser.data_len < 10) { + return; } - _sdcard_healthy = !all_zero; + static const uint8_t all_zero_chars[10] = {'0','0','0','0','0','0','0','0','0','0'}; + const bool all_zero = (memcmp(&_msg_buff[AP_MOUNT_TPFRAME_MSGOFS_DATA], all_zero_chars, ARRAY_SIZE(all_zero_chars)) == 0); + _sdcard_state = all_zero ? SDCardState::ABSENT : SDCardState::PRESENT; } // gimbal basic information analysis. response data is of the form "VX.X.X" (e.g. "V1.0.78") diff --git a/libraries/AP_Mount/AP_Mount_SkyDroid.h b/libraries/AP_Mount/AP_Mount_SkyDroid.h index e738e2596c1846..b6e13c7372f29b 100644 --- a/libraries/AP_Mount/AP_Mount_SkyDroid.h +++ b/libraries/AP_Mount/AP_Mount_SkyDroid.h @@ -198,6 +198,17 @@ class AP_Mount_SkyDroid : public AP_Mount_Backend_TPFrame NUM_STEPS = 10, // wraps back to VERSION after this - note the gap at 7-9, spare }; + // memory card state, as last reported by "SDC" (see gimbal_sdcard_analyse()). + // UNKNOWN until the first reply arrives - camera-addressed replies like this one + // can take anywhere from under a second to 8+ minutes (see this file's header + // comment), so capture attempts must not be blocked on a card that may simply not + // have reported in yet + enum class SDCardState : uint8_t { + UNKNOWN = 0, + PRESENT = 1, + ABSENT = 2, + }; + // send text prefix string static const char* send_message_prefix; @@ -288,7 +299,7 @@ class AP_Mount_SkyDroid : public AP_Mount_Backend_TPFrame // members bool _recording; // recording status, tracked locally from commands we've sent - bool _sdcard_healthy; // true if a memory card is present and OK (received from gimbal) + SDCardState _sdcard_state = SDCardState::UNKNOWN; // memory card state, as last reported by the gimbal (see SDCardState) bool _last_lock; // last lock mode sent to gimbal bool _got_gimbal_version; // true if gimbal's version has been received bool _got_model_name; // true if gimbal's model name has been received From 4e7781a171249a67d61db7d95e193be079c1e90f Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Fri, 28 Aug 2026 14:54:10 -0600 Subject: [PATCH 13/19] AP_Mount: TPFrame fix parser lockup on a zero-length data segment A received frame whose Data_Len nibble is 0 could never complete: '0' passes the WAITING_FOR_DATALEN sanity check (0 <= datalen_max()), the parser reaches WAITING_FOR_DATA after ID3 with _msg_buff_len == 10, and the "have we got all the data" check (data_bytes_received == _parser.data_len) only runs after each new byte is appended - so data_bytes_received starts at 1 on the very next byte and can never equal 0 again. The parser gets stuck consuming the real CRC bytes (and then the start of the next packet) as fake data until data_bytes_received exceeds datalen_max(), at which point it fires INTERNAL_ERROR - which fails arming checks. A single bit-flipped Data_Len nibble on the wire is enough to trigger this, and it's in the shared framing layer so it affects Topotek as well as SkyDroid. Fix by routing data_len==0 straight from ID3 to WAITING_FOR_CRC_LOW, skipping WAITING_FOR_DATA entirely - there are no data bytes to wait for, so the datalen_max() overrun this guards against really is unreachable once this case is handled explicitly. Flagged by AI-assisted review on PR #34155 (tridge/Claude, cross-checked by Codex), verified against the code before fixing. --- libraries/AP_Mount/AP_Mount_Backend_TPFrame.cpp | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Mount/AP_Mount_Backend_TPFrame.cpp b/libraries/AP_Mount/AP_Mount_Backend_TPFrame.cpp index 80f7f884907415..19e5326c72d7b1 100644 --- a/libraries/AP_Mount/AP_Mount_Backend_TPFrame.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend_TPFrame.cpp @@ -105,7 +105,6 @@ void AP_Mount_Backend_TPFrame::read_incoming_packets() case ParseState::WAITING_FOR_ID1: case ParseState::WAITING_FOR_ID2: - case ParseState::WAITING_FOR_ID3: // check all uppercase letters and numbers. eg 'GAC' if ((b >= 'A' && b <= 'Z') || (b >= '0' && b <= '9')) { // advance to next state @@ -115,6 +114,21 @@ void AP_Mount_Backend_TPFrame::read_incoming_packets() reset_parser = true; break; + case ParseState::WAITING_FOR_ID3: + if ((b >= 'A' && b <= 'Z') || (b >= '0' && b <= '9')) { + // a zero-length data segment has no data bytes to wait for - the + // WAITING_FOR_DATA case below can only advance once + // data_bytes_received (which starts at 1 on the very next byte and + // only increases) equals _parser.data_len, which can never happen + // for data_len==0, so go straight to the CRC instead of getting + // stuck consuming the CRC (and then the next packet) as fake data + // until datalen_max() overflows and fires an avoidable INTERNAL_ERROR + _parser.state = (_parser.data_len == 0) ? ParseState::WAITING_FOR_CRC_LOW : ParseState::WAITING_FOR_DATA; + break; + } + reset_parser = true; + break; + case ParseState::WAITING_FOR_DATA: { // normally hex numbers in char form (e.g. '0A') const uint8_t data_bytes_received = _msg_buff_len - (AP_MOUNT_TPFRAME_PACKETLEN_MIN - 2); From 241167dc6429f4e5d7df9d06eb3d3ae88799f27d Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Fri, 28 Aug 2026 14:54:38 -0600 Subject: [PATCH 14/19] AP_Mount: SkyDroid actually send the first follow/lock mode request set_gimbal_lock()'s dedup check (_last_lock == lock) short-circuited before ever sending anything on the very first call, because _last_lock defaults false - the same value the first call always requests (send_target_angles() calls set_gimbal_lock(false) to put the gimbal in follow mode). The driver silently assumed the gimbal had already booted in follow mode instead of confirming it. Track whether a mode has actually been sent yet with a separate _lock_sent flag so the first call always sends, regardless of which value it requests. Flagged by AI-assisted review on PR #34155 (tridge/Claude, cross-checked by Codex), verified against the code before fixing. --- libraries/AP_Mount/AP_Mount_SkyDroid.cpp | 7 ++++++- libraries/AP_Mount/AP_Mount_SkyDroid.h | 3 ++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_SkyDroid.cpp b/libraries/AP_Mount/AP_Mount_SkyDroid.cpp index 2dfec8ba36b46a..d74f70efab6eb4 100644 --- a/libraries/AP_Mount/AP_Mount_SkyDroid.cpp +++ b/libraries/AP_Mount/AP_Mount_SkyDroid.cpp @@ -588,13 +588,18 @@ void AP_Mount_SkyDroid::gimbal_model_analyse() // lock is false to follow / maintain a body-frame target bool AP_Mount_SkyDroid::set_gimbal_lock(bool lock) { - if (_last_lock == lock) { + // _last_lock defaults false, which is indistinguishable from "we've already + // confirmed the gimbal is in follow mode" unless we also track whether a mode + // has actually been sent yet - without _lock_sent, the very first call (always + // requesting follow, false) would silently no-op instead of sending anything + if (_lock_sent && _last_lock == lock) { return true; } // send message and update lock state. PTZ data: 0x06 = follow, 0x07 = lock head if (send_fixedlen_packet(AddressByte::GIMBAL, AP_MOUNT_SKYDROID_ID3CHAR_GIMBAL_MODE, true, lock ? 0x07 : 0x06)) { _last_lock = lock; + _lock_sent = true; return true; } return false; diff --git a/libraries/AP_Mount/AP_Mount_SkyDroid.h b/libraries/AP_Mount/AP_Mount_SkyDroid.h index b6e13c7372f29b..9613a2b1d2a80e 100644 --- a/libraries/AP_Mount/AP_Mount_SkyDroid.h +++ b/libraries/AP_Mount/AP_Mount_SkyDroid.h @@ -300,7 +300,8 @@ class AP_Mount_SkyDroid : public AP_Mount_Backend_TPFrame // members bool _recording; // recording status, tracked locally from commands we've sent SDCardState _sdcard_state = SDCardState::UNKNOWN; // memory card state, as last reported by the gimbal (see SDCardState) - bool _last_lock; // last lock mode sent to gimbal + bool _last_lock; // last lock mode sent to gimbal, only meaningful once _lock_sent + bool _lock_sent; // true once set_gimbal_lock() has sent a mode at least once bool _got_gimbal_version; // true if gimbal's version has been received bool _got_model_name; // true if gimbal's model name has been received bool _announced_connected; // true once we've told the user the gimbal is connected From 9b54b86c5868fb173382c4225143f5b4b74354fe Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Sat, 29 Aug 2026 10:39:18 -0600 Subject: [PATCH 15/19] SITL: cap UDP device-to-autopilot reads at 300 bytes This PR's own copy of the UDP network-attached-device transport predates the split of that work into #34159, which already carries this fix - reapplying it here directly so this PR doesn't reintroduce the truncation bug if it merges before/independently of #34159. AP_Networking_port::run() reads at most 300 bytes per recv() call. UDP is datagram-based, not a byte stream like TCP, so a datagram larger than that isn't queued for a later read - the kernel silently discards whatever didn't fit. Cap what network_update_udp() sends to what the far end can actually receive in one call, or a device that bursts more than 300 bytes at once (unlikely for SkyDroid/Topotek's own small packets, but not something this transport should silently get wrong for other devices) would have its frame truncated. Flagged by AI-assisted review on PR #34155 (tridge/Claude), verified against the code and against #34159's existing fix before applying. --- libraries/SITL/SIM_SerialDevice.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/libraries/SITL/SIM_SerialDevice.cpp b/libraries/SITL/SIM_SerialDevice.cpp index 65e67508909244..2856d7b340fd33 100644 --- a/libraries/SITL/SIM_SerialDevice.cpp +++ b/libraries/SITL/SIM_SerialDevice.cpp @@ -276,7 +276,14 @@ void SerialDevice::network_update_udp() return; } while (true) { - const ssize_t nread = read_from_device(buffer, sizeof(buffer)); + // AP_Networking_port::run() (AP_Networking_port.cpp) reads at + // most 300 bytes per recv() call. UDP is datagram-based, not + // a byte stream like TCP, so a datagram larger than that isn't + // queued for a later read -- the kernel silently discards + // whatever didn't fit. Cap what we send to what the far end + // can actually receive in one call, or a device that bursts + // more than 300 bytes at once would have its frame truncated. + const ssize_t nread = read_from_device(buffer, MIN(sizeof(buffer), (size_t)300)); if (nread <= 0) { break; } From e550c3335bcace3c221eeeffe572f60140c48df4 Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Sat, 29 Aug 2026 10:39:30 -0600 Subject: [PATCH 16/19] AP_Mount: SkyDroid fix stale UPDATE_INTERVAL_MS comment The comment claimed this constant gates resending angle/rate targets, but that hasn't been true since send_target_angles()'s P-controller was moved to run at the full 50hz update() rate - this constant only gates the attitude push and the 1hz request-loop step below it. Flagged by AI-assisted review on PR #34155 (tridge/Claude), verified against the code before fixing. --- libraries/AP_Mount/AP_Mount_SkyDroid.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Mount/AP_Mount_SkyDroid.cpp b/libraries/AP_Mount/AP_Mount_SkyDroid.cpp index d74f70efab6eb4..67954e79dd21e6 100644 --- a/libraries/AP_Mount/AP_Mount_SkyDroid.cpp +++ b/libraries/AP_Mount/AP_Mount_SkyDroid.cpp @@ -12,7 +12,7 @@ extern const AP_HAL::HAL& hal; -#define AP_MOUNT_SKYDROID_UPDATE_INTERVAL_MS 100 // resend angle or rate targets, and push our attitude, at this interval +#define AP_MOUNT_SKYDROID_UPDATE_INTERVAL_MS 100 // push our attitude to the gimbal, and step the 1hz request loop, at this interval - target angles/rates are sent every update() call instead, at the full 50hz (see update()'s comment) #define AP_MOUNT_SKYDROID_HEALTH_TIMEOUT_MS 1000 // timeout for health (based on attitude reports from gimbal) #define AP_MOUNT_SKYDROID_ATTITUDE_RATE_HZ 50 // rate we ask the gimbal to stream its attitude to us (matches the 50hz rate AP_Mount::update() is actually called at; doc allows up to 100hz) From e184fa094883fcfe3011af6bbded17bfa2cfaace Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Sun, 30 Aug 2026 15:31:50 -0600 Subject: [PATCH 17/19] AP_Mount: Topotek actually send the first follow/lock mode request Same bug as SkyDroid's set_gimbal_lock() (already fixed): _last_lock defaults false, so its dedup check (_last_lock == lock) short-circuited before ever sending anything on the first call requesting follow mode (false) - the driver silently assumed the gimbal had already booted in follow mode instead of confirming it. Track whether a mode has actually been sent yet with a separate _lock_sent flag so the first call always sends, regardless of which value it requests. Found while investigating a real KHP415 not responding to RC pitch/yaw at all despite correct RCx_OPTION/MNT1_RC_RATE config and a healthy attitude feed - consistent with the gimbal staying in whatever lock state it powered up in because it was never actually told to switch to follow mode. --- libraries/AP_Mount/AP_Mount_Topotek.cpp | 7 ++++++- libraries/AP_Mount/AP_Mount_Topotek.h | 3 ++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Topotek.cpp b/libraries/AP_Mount/AP_Mount_Topotek.cpp index 12a8afcc2066a5..1040b7776a3e11 100644 --- a/libraries/AP_Mount/AP_Mount_Topotek.cpp +++ b/libraries/AP_Mount/AP_Mount_Topotek.cpp @@ -889,13 +889,18 @@ void AP_Mount_Topotek::gimbal_model_name_analyse() // lock is false to follow / maintain a body-frame target bool AP_Mount_Topotek::set_gimbal_lock(bool lock) { - if (_last_lock == lock) { + // _last_lock defaults false, which is indistinguishable from "we've already + // confirmed the gimbal is in follow mode" unless we also track whether a mode + // has actually been sent yet - without _lock_sent, the very first call (often + // requesting follow, false) would silently no-op instead of sending anything + if (_lock_sent && _last_lock == lock) { return true; } // send message and update lock state if (send_fixedlen_packet(AddressByte::GIMBAL, AP_MOUNT_TOPOTEK_ID3CHAR_GIMBAL_MODE, true, lock ? 6 : 7)) { _last_lock = lock; + _lock_sent = true; return true; } return false; diff --git a/libraries/AP_Mount/AP_Mount_Topotek.h b/libraries/AP_Mount/AP_Mount_Topotek.h index 1ed114590710a9..b2ee60d064d332 100644 --- a/libraries/AP_Mount/AP_Mount_Topotek.h +++ b/libraries/AP_Mount/AP_Mount_Topotek.h @@ -245,7 +245,8 @@ class AP_Mount_Topotek : public AP_Mount_Backend_TPFrame TrackingStatus _last_tracking_state = TrackingStatus::STOPPED_TRACKING; // last tracking state received from gimbal uint8_t _last_mode; // mode during latest update, used to detect mode changes and cancel tracking bool _sdcard_status; // memory card status (received from gimbal) - bool _last_lock; // last lock mode sent to gimbal + bool _last_lock; // last lock mode sent to gimbal, only meaningful once _lock_sent + bool _lock_sent; // true once set_gimbal_lock() has sent a mode at least once 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) From 44b109fee1086a305fa544c459a8531757b6f3a4 Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Sun, 30 Aug 2026 20:54:10 -0600 Subject: [PATCH 18/19] AP_Mount: SkyDroid fix month sent un-converted in send_time_sync() get_date_and_time_utc() returns month 0~11, but the "TIM" wire command's MM field is 1-based (the driver's own sample comment decodes 031218 as 2018-12-03, and the Topotek equivalent already does month + 1). As written every timestamp sent to the gimbal was one month early, and any January date sent an invalid month of 00. Found by tridge's automated review of PR #34155. --- libraries/AP_Mount/AP_Mount_SkyDroid.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Mount/AP_Mount_SkyDroid.cpp b/libraries/AP_Mount/AP_Mount_SkyDroid.cpp index 67954e79dd21e6..4866c354b6abf1 100644 --- a/libraries/AP_Mount/AP_Mount_SkyDroid.cpp +++ b/libraries/AP_Mount/AP_Mount_SkyDroid.cpp @@ -311,7 +311,7 @@ bool AP_Mount_SkyDroid::send_time_sync() uint8_t databuff[16]; hal.util->snprintf((char*)databuff, ARRAY_SIZE(databuff), "%02u%02u%02u.%02u%02u%02u%02u", hour, min, sec, (unsigned)((ms / 10) % 100), - day, month, (unsigned)(year % 100)); + day, month + 1, (unsigned)(year % 100)); return send_variablelen_packet(HeaderType::VARIABLE_LEN, AddressByte::SYSTEM_AND_IMAGE, "TIM", true, databuff, ARRAY_SIZE(databuff)-1); } From ef9124d88aee12b4094600e98fed9be9b0d8e346 Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Mon, 31 Aug 2026 12:12:07 -0600 Subject: [PATCH 19/19] AP_Mount: TPFrame report actual write success from send_variablelen_packet() _uart->write()'s return value was discarded and the function always returned true once txspace() had already been checked, so a short or failed write was indistinguishable from success. Callers latch on that return value permanently - Topotek's set_gimbal_lock() sets _lock_sent true and never retries once send_fixedlen_packet() reports success, so a lost first mode packet would silently strand the gimbal in whatever mode it powered up in. Far more likely on the network-attached gimbals this driver now supports than on a serial UART. Found by tridge's automated review of PR #34155. --- libraries/AP_Mount/AP_Mount_Backend_TPFrame.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Backend_TPFrame.cpp b/libraries/AP_Mount/AP_Mount_Backend_TPFrame.cpp index 19e5326c72d7b1..a789d43aa03588 100644 --- a/libraries/AP_Mount/AP_Mount_Backend_TPFrame.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend_TPFrame.cpp @@ -267,9 +267,11 @@ bool AP_Mount_Backend_TPFrame::send_variablelen_packet(HeaderType header, uint8_ send_buff[send_buff_ofs++] = hex2char((crc >> 4) & 0x0f); send_buff[send_buff_ofs++] = hex2char(crc & 0x0f); - // send packet - _uart->write(send_buff, send_buff_ofs); - return true; + // send packet. txspace() was already confirmed sufficient above, but + // callers (e.g. set_gimbal_lock()) latch success permanently on a true + // return, so a short write must be reported as failure rather than + // silently dropping the unsent tail of the packet + return _uart->write(send_buff, send_buff_ofs) == send_buff_ofs; } #endif // HAL_MOUNT_TOPOTEK_ENABLED || HAL_MOUNT_SKYDROID_ENABLED