Skydroid driver for C11 and C13 gimbals - #34155
Conversation
peterbarker
left a comment
There was a problem hiding this comment.
Partial review only.
| // 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) |
There was a problem hiding this comment.
We need to move the same code up from AP_Mount_Siyi rather than doing the same thing again.
See this comment:
// we don't know how to convert ANGLE to anything else. Note
// that the Siyi backend *does* convert angles to rates, so we
// could potentially swipe code into here.
There was a problem hiding this comment.
Sounds like a good idea of a follow up PR rather than increasing the scope here.
There was a problem hiding this comment.
Sounds like a good idea of a follow up PR rather than increasing the scope here.
No, we didn't move the code up from Siyi because there's only one consumer of it at the moment.
At least you have access to the camera to test the code that's moved up. Someone wanting to do the cleanup later may not. We could rely on the simulators for all of the testing, but better to not need to.
There was a problem hiding this comment.
Sounds like a good idea of a follow up PR rather than increasing the scope here.
No, we didn't move the code up from Siyi because there's only one consumer of it at the moment.
At least you have access to the camera to test the code that's moved up. Someone wanting to do the cleanup later may not. We could rely on the simulators for all of the testing, but better to not need to.
Ok - yes I do have a SiYI and Topotek gimbal, so I'm going to try to do this. Stay tuned. I have to dig one gimbal out of a crashed airframe and set it up to test.
|
Assuming the sim udp stuff is well-tested I'd be happy to merge that as a separate PR. Same with taking the pitch tolerance as a parameter. |
Peter Barker's review of PR ArduPilot#34155 (the SkyDroid gimbal driver) pointed out that NAME:PORT:PROTOCOL colon-chains an arbitrary sequence of unrelated fields as if each were another top-level thing like NAME - a future option (e.g. a simulated firmware version) would read as a sibling of PROTOCOL rather than being logically grouped with the port it actually modifies. Changes the spec format to NAME:PORT[,OPTION,OPTION,...], so any options beyond the base NAME:PORT are comma-separated among themselves and read as attached to the port, not tacked on as more colon-delimited fields. Updates the one existing caller (SkyDroid's UDP network-device autotest) and the --net-device help text to match.
Responds to review feedback on PR ArduPilot#34155: - get_attitude_quaternion() now returns false until a real "GAC" packet has actually been received, instead of always reporting a fabricated (0,0,0) attitude - a real bug, not just style, since AP_Mount_Backend checks this return value in several places. - read_incoming_packets()'s byte count is unsigned (there was no legitimate way for it to go negative). - Named the 1hz request-loop's steps via a ReqStep enum instead of magic case numbers. - Renamed _sdcard_status to _sdcard_healthy (that's what it represents). - Folded the ten 3-character command-ID macros that were each used in exactly one place into their call sites (keeping their documentation as a regular comment there), and deleted one that turned out to be entirely unused (the incoming-message dispatch used a raw string literal instead of the macro that supposedly existed for it). - Replaced the manual index loop + function-pointer lookup table dispatching incoming messages with a plain if/else chain on the 3-character command ID, since the table had no other use. - Introduced named byte-offset constants for the packet preamble (data-length, command ID, start of data) shared by every message type, and used them everywhere instead of bare magic indices into _msg_buff - including turning the SD-card-info all-zero check into a length-guarded memcmp against a static all-zero buffer. - Added a comment explaining why the parser deliberately accepts both cases of the #TP/#tp header without recording which one it saw: the packet's own explicit data-length field is authoritative regardless, so the distinction only matters when sending, not receiving. - Added a comment being explicit that the FAI command's fixed-wing/ hover byte's real effect on the gimbal's own firmware is unknown - raised with SkyDroid rather than assuming a "more correct" runtime proxy would actually be better than the current compile-time one.
8b62c1c to
5f7d697
Compare
Broken out into a new PR: #34159 |
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 ArduPilot#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.
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 ArduPilot#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.
|
Please take a look @UAVcihang - any corrections or suggestions very much appreciated. |
|
Any time I see the words "copy/paste" I start to be concerned. |
|
OK, so this was a sideways copy of the driver. Not the right way to do things. It increases flash size and maintenance burden and the likelihood of found-bugs remaining unfixed. Rough calculation is that about twice as many LOCs introduced in this PR as required to add support. There's a question as to why this protocol needs to differ to the one that was copied sideways. |
…nd_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#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.
…Droid 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#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.
…nd_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#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.
…Droid 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#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.
Good point. This seems to mean that the SkyDroid gimbal is built on the same internal components as the Topotek gimbals. So based on the fact that both drones preface messages with #tp or #TP, I'm calling it "TPFrame" as a backend and making it common between both. |
Now there is a common backend. The vendor specifically contents specific to each vendor remain in the respective drivers. |
test_mount_rc_targetting() and mount_test_body() hardcoded a 0.1deg pitch tolerance for the RC-targeting checks. Threads a pitch_tolerance / rc_targetting_pitch_tolerance parameter through both instead (default 0.1deg, unchanged for every existing caller), so a backend whose actuator has a coarser confirmed physical resolution - e.g. a rate-only actuator closing an angle loop via a quantized speed command - can pass a wider, measurement-backed value instead of failing a tolerance the hardware cannot physically meet. Split out of the SkyDroid gimbal driver PR (#34155) at Peter Barker's review suggestion - this parameter is generically useful and doesn't depend on anything SkyDroid-specific.
Previous review (2026-08-27)Automated review note — AI-generated (Claude), validated against the live diff. Please sanity-check before acting. Full report: https://uav.tridgell.net/DevCallReviews/2026_08_27/devcall_pr_reviews.html Reviewed at head Verdict: REQUEST CHANGES — the architecture concerns from earlier review are resolved (shared
|
Peter Barker's review of PR ArduPilot#34155 (the SkyDroid gimbal driver) pointed out that NAME:PORT:PROTOCOL colon-chains an arbitrary sequence of unrelated fields as if each were another top-level thing like NAME - a future option (e.g. a simulated firmware version) would read as a sibling of PROTOCOL rather than being logically grouped with the port it actually modifies. Changes the spec format to NAME:PORT[,OPTION,OPTION,...], so any options beyond the base NAME:PORT are comma-separated among themselves and read as attached to the port, not tacked on as more colon-delimited fields. Updates the one existing caller (SkyDroid's UDP network-device autotest) and the --net-device help text to match.
Responds to review feedback on PR ArduPilot#34155: - get_attitude_quaternion() now returns false until a real "GAC" packet has actually been received, instead of always reporting a fabricated (0,0,0) attitude - a real bug, not just style, since AP_Mount_Backend checks this return value in several places. - read_incoming_packets()'s byte count is unsigned (there was no legitimate way for it to go negative). - Named the 1hz request-loop's steps via a ReqStep enum instead of magic case numbers. - Renamed _sdcard_status to _sdcard_healthy (that's what it represents). - Folded the ten 3-character command-ID macros that were each used in exactly one place into their call sites (keeping their documentation as a regular comment there), and deleted one that turned out to be entirely unused (the incoming-message dispatch used a raw string literal instead of the macro that supposedly existed for it). - Replaced the manual index loop + function-pointer lookup table dispatching incoming messages with a plain if/else chain on the 3-character command ID, since the table had no other use. - Introduced named byte-offset constants for the packet preamble (data-length, command ID, start of data) shared by every message type, and used them everywhere instead of bare magic indices into _msg_buff - including turning the SD-card-info all-zero check into a length-guarded memcmp against a static all-zero buffer. - Added a comment explaining why the parser deliberately accepts both cases of the #TP/#tp header without recording which one it saw: the packet's own explicit data-length field is authoritative regardless, so the distinction only matters when sending, not receiving. - Added a comment being explicit that the FAI command's fixed-wing/ hover byte's real effect on the gimbal's own firmware is unknown - raised with SkyDroid rather than assuming a "more correct" runtime proxy would actually be better than the current compile-time one.
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 ArduPilot#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.
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 ArduPilot#34155 (tridge/Claude, cross-checked by Codex), verified against the code before fixing.
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 ArduPilot#34155 (tridge/Claude, cross-checked by Codex), verified against the code before fixing.
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 ArduPilot#34155 (tridge/Claude, cross-checked by Codex), verified against the code before fixing.
Responses: Zero-length-data-segment INTERNAL_ERROR: confirmed by tracing the state machine by hand - you're right that SD card gating: switched
Retract/neutral 50Hz resend: leaving as-is per the earlier discussion, noted as non-blocking. Regression-tested against |
|
Automated review note — AI-generated (Claude), validated against the live diff. Please sanity-check before acting. Re-reviewed at head
Non-blocking notes:
Parser memory-safety re-confirmed at head by both passes (max write index 35 into the 36-byte buffer, CRC before dispatch, resync works). Once the month fix lands, this returns to "clean code, gated on the C11 hardware re-validation, the Topotek/Siyi regression test, and @peterbarker's re-review" — those process items are unchanged from my previous comment. |
This PR's own copy of the UDP network-attached-device transport predates the split of that work into ArduPilot#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 ArduPilot#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 ArduPilot#34155 (tridge/Claude), verified against the code and against ArduPilot#34159's existing fix before applying.
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 ArduPilot#34155 (tridge/Claude), verified against the code before fixing.
This PR's own copy of the UDP network-attached-device transport predates the split of that work into ArduPilot#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 ArduPilot#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 ArduPilot#34155 (tridge/Claude), verified against the code and against ArduPilot#34159's existing fix before applying.
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 ArduPilot#34155 (tridge/Claude), verified against the code before fixing.
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.
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.
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.
… 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.
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.
Registers HAL_MOUNT_SKYDROID_ENABLED as a selectable build option for the new SkyDroid gimbal driver.
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 ArduPilot#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.
…nd_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#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.
…Droid 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#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.
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 ArduPilot#34155 (tridge/Claude, cross-checked by Codex), verified against the code before fixing.
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 ArduPilot#34155 (tridge/Claude, cross-checked by Codex), verified against the code before fixing.
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 ArduPilot#34155 (tridge/Claude, cross-checked by Codex), verified against the code before fixing.
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 ArduPilot#34155 (tridge/Claude, cross-checked by Codex), verified against the code before fixing.
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 ArduPilot#34155 (tridge/Claude, cross-checked by Codex), verified against the code before fixing.
This PR's own copy of the UDP network-attached-device transport predates the split of that work into ArduPilot#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 ArduPilot#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 ArduPilot#34155 (tridge/Claude), verified against the code and against ArduPilot#34159's existing fix before applying.
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 ArduPilot#34155 (tridge/Claude), verified against the code before fixing.
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.
Same bug as SkyDroid's SD-card gating (already fixed): take_picture()/ record_video() refused every request while _sdcard_status was false - which was also its default value until the first "SDC" reply arrived. Camera-addressed replies can take a while, so a capture attempt right after boot/arming could be refused for that entire window with a misleading "SD card error" message. Replace the bool with a tri-state (UNKNOWN/PRESENT/ABSENT) and only refuse on a confirmed ABSENT reply, attempting the capture while still UNKNOWN. Found investigating a real KHP415 throwing an error on Camera Trigger/ Record sent shortly after boot via Mission Planner's Aux Function panel.
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 ArduPilot#34155.
a60aa18 to
628e88b
Compare
Confirmed and fixed 1. Month bug: fixed in Non-blocking notes 2-4 (SIM_SkyDroid GAC-rate comment, This reply and the fix were prepared with AI assistance (Claude). |
Summary
Adds
AP_Mount_SkyDroid, a new mount backend for SkyDroid's "TOP protocol" gimbal camera family (MNT1_TYPE=15), covering both the C11 and C13.Classification & Testing (check all that apply and add your own)
Tested in SITL via three new autotests:
MountSkyDroid(C11 over serial),MountSkyDroidC13(identical body, proving the driver behaves the same regardless of which model answersMOD, plus that roll never responds to RC input), andMountSkyDroidNetwork(C11 over a UDP network port — the real hardware's actual transport). All three pass.Tested extensively on real C11 hardware over multiple sessions, including: RC and MAVLink pitch/yaw control, picture/video capture, digital zoom, the retract/neutral "center" command, and a dataflash-log-based calibration measurement of the individual-axis rate commands' real-world speed. Roll self-stabilization and the model-independence of gimbal control were both directly confirmed with SkyDroid.
Some limited bench testing with the C13 gimbal was also done.
Description
What: A new
AP_Mountbackend for SkyDroid's gimbal camera family, communicating over UDP using SkyDroid's own#TP/#tppacket protocol. 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.Why: SkyDroid gimbals have no existing ArduPilot support; this adds it.
Key protocol findings, confirmed on real hardware:
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 onGSY/GSPalone.GAR/GSR) but they aren't implemented in firmware. The driver reportshas_roll_control() == falseand never sends them, while still parsing roll from the gimbal's own attitude reports for telemetry.Parameter changes: adds
MNT1_TYPE=15(SkyDroid) as a new enum value — purely additive, no migration considerations for existing configurations.** dependancies **
#34159
#34160 (merged)
Also the branch is now rebased on top of #34227 (an
AP_Networkingfix forUDP_CLIENTsockets dropping replies from a source port different than the connect()-target port, found while testing this driver's network-mount config on real hardware). That commit will show in this PR's diff until #34227 merges — not part of this PR's own scope.AI-assisted contribution: this PR was developed with substantial AI assistance (Claude Code), across protocol reverse-engineering, driver/simulator/autotest implementation, and analysis of real-hardware dataflash logs used to validate and correct the calibration constant referenced above. All real-hardware testing, and review and validation of the AI's work, was done directly by the human author.