2 Commits

Author	SHA1	Message	Date
Alex Klimaj	90913df7de	feat(gps): inject SPARTN corrections alongside RTCM (#27919) ... * feat(gps): inject SPARTN corrections alongside RTCM Add a SPARTN transport-layer framer and feed gps_inject_data through both RTCM3 and SPARTN parsers so PointPerfect-style SPARTN streams can be reassembled and written to the receiver the same way RTCM already is. Gated by CONFIG_GPS_SPARTN (default on). Disabled on px4_fmu-v6x where flash is already at the limit; enabled on ark_can-rtk-gps. Depends on PX4-GPSDrivers for automatic u-blox SPARTN input enable. Signed-off-by: alexklimaj <alex@arkelectron.com> * fix(gnss): avoid undefined shift in SPARTN CRC-32 Use uint64_t for the CRC working register so n==32 does not perform 1u << 32 (clang-analyzer BitwiseShift). Signed-off-by: alexklimaj <alex@arkelectron.com> * make format * feat(gps): enable SPARTN support in board configurations * feat(gps): enhance SPARTN support with additional frame tracking and status reporting * fix(gps): frame RTCM3 and SPARTN from a single buffer Feeding every inject chunk to an independent framer per protocol let each one resync inside the other's payloads. That is not symmetric: RTCM3 is covered by CRC-24Q, but SPARTN's header carries no usable integrity check (TF006 is 4 bits over a non-byte-aligned field) and TF005 permits an 8-bit message CRC, so a stray 0x73 in an RTCM3 payload is framed as SPARTN at roughly 1 in 1024. RTCM3-only is what every board actually runs, and over 50 MB of it the two framers produced 211 bogus SPARTN frames (210 declaring CRC-8), each re-injecting up to 1 kB of the stream back into the receiver. The reverse direction produced none. Frame both protocols from one buffer instead: whichever preamble comes first is framed, and a valid frame consumes its own payload, so bytes inside one protocol's frame never start the other's. The same 50 MB now yields zero. Frames are also injected in arrival order rather than all RTCM3 then all SPARTN, and one buffer replaces two (2248 B/instance, down from ~4350 B). Also reject TF002 message types 5-119, which SPARTN reserves, as the one header field with a checkable range. CONFIG_GPS_SPARTN was default y, so it built into every target with a GPS including px4_fmu-v6x, which the flash report showed gaining the framer despite the intent to keep it off. Default it to n and enable it explicitly where it is wanted; the ark GPS boards already opt in, and SITL opts in so the framing tests keep running in CI. Rtcm3Parser and SpartnParser are replaced by CorrectionFramer; their tests carry over to it. RtcmStress fed "garbage" drawn from 0x01-0xD2 to avoid a preamble, which includes 0x73, and RtcmBustedSender ended its stream on a candidate the framer was still waiting to complete; both now avoid every preamble and flush respectively. * feat(gps): enable SPARTN on ARK flight controllers Covers receivers attached over UART rather than CAN. All four targets link with margin; fmu-v6xrt has no px4board on this branch yet. Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com> --------- Signed-off-by: alexklimaj <alex@arkelectron.com> Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com> Co-authored-by: Jacob Dahl <dahl.jakejacob@gmail.com>	2026-07-16 14:24:07 -06:00
Jacob Dahl	5d8874cd2b	fix(gps): split RTCM corrections and moving-baseline uORB topics (#27097) ... * fix(gps): split RTCM corrections and moving-baseline uORB topics The single gps_inject_data topic served two unrelated purposes: external fixed-base RTCM corrections (from MAVLink GPS_RTCM_DATA or UAVCAN RTCMStream) and moving-base-to-rover RTCM 4072. In a dual-GPS-with-moving-base plus fixed-base setup, the two streams collided on the same queue and the FMU UAVCAN bridge mirrored fixed-base RTCM onto the MovingBaselineData CAN message, breaking rover heading or RTK fix (see PX4/PX4-Autopilot#27088). Split by role: - rtcm_corrections (renamed from gps_inject_data): external RTCM flowing into the vehicle; producers are MAVLink, UAVCAN RTCMStream, and GPS drivers in dump mode. - rtcm_moving_baseline (new): moving-base GPS output intended for a rover; single producer per vehicle (MAX_INSTANCES = 1). The GPS driver routes its own RTCM output to the right topic via GPSHelper::isMovingBase(), and gates the two inbound streams per role using new GPSHelper virtuals (PX4-GPSDrivers#212): shouldInjectRTCMCorrections() is true for any configured receiver, so a UART2 moving-base rover still accepts fixed-base corrections over its main link; shouldInjectMovingBaseline() is true only for a UART1/CAN heading rover, since a UART2 rover gets the baseline directly in hardware and a moving base produces rather than consumes it. That submodule PR also renames the ambiguous UBXMode fields to name their UART explicitly (RoverWithMovingBase -> RoverWithMovingBaseUART2, MovingBase -> MovingBaseUART2). Septentrio's publish_rtcm_corrections() always publishes to rtcm_moving_baseline (only the Secondary moving base calls it). Rover-side consumers (gps, septentrio, uavcan bridge) drain both topics independently; each topic gets its own stale-link switchover timer so corrections failover is not suppressed by moving-baseline traffic, or vice versa. FMU UAVCAN bridge: two independent drain loops, one per topic. No more dual-publish of a single uORB message onto both RTCMStream and MovingBaselineData CAN streams. CANnode MovingBaselineDataPub subscribes to rtcm_moving_baseline. The bus_type == UAVCAN check is kept, now purely as a loop guard so a node with both CANNODE_PUB_MBD and CANNODE_SUB_MBD does not rebroadcast a peer's moving-baseline data back onto the bus. CANnode RTCMStream subscriber maps each CAN source node ID to its own rtcm_corrections instance (one PublicationMulti per source, capped at MAX_INSTANCES) so multiple CAN RTCM sources (e.g. dual rovers outputting MSM7 for logging plus a fixed-base feed) land on independent uORB instances instead of interleaving on one, which would otherwise defeat the consumer's per-instance stale-link selection. Depends on PX4-GPSDrivers#212 (submodule bump included). Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com> * fix(gps): use separate RTCM parsers for corrections and moving baseline On a rover injecting both fixed-base corrections and moving-baseline RTCM, feeding both streams through a single parser allowed a fragmented frame from one source to be corrupted by bytes interleaved from the other. Reassemble each stream in its own Rtcm3Parser so frames are recovered independently. Also collapse the two near-identical topics into a single RtcmData.msg published under both rtcm_corrections and rtcm_moving_baseline (the SensorGps pattern), track corrections and moving-baseline injection on separate perf counters so the reported corrections rate is no longer inflated by moving-baseline traffic, and zero-initialize the CAN DeviceId unions before populating their fields. Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com> * fix(septentrio): avoid bugprone sizeof division on RTCM byte buffer moving_baseline.data is a uint8_t array, so sizeof(data)/sizeof(data[0]) divides by 1; clang-tidy's bugprone-sizeof-expression flags this as a suspicious sizeof(T)/sizeof(T). Use sizeof(data) directly - the capacity value is unchanged. Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com> * refactor(gps): use dedicated per-stream RTCM drain functions rtcm_moving_baseline has a single publisher (instance 0), so its consumers are now a plain uORB::Subscription instead of a 4-instance SubscriptionMultiArray, and the per-stream selected-instance and stale-link timer members it no longer needs are removed. With each RTCM stream now a fixed type with a single caller, the templated drain helpers (drain_rtcm_subscriptions, the overloaded drain_rtcm_to_can) bought nothing, so replace them with dedicated functions: drainRtcmCorrections()/drainMovingBaseline() in the GPS driver and the UAVCAN bridge, drain_rtcm_corrections()/ drain_moving_baseline() in Septentrio. The UAVCAN bridge calls PublishRTCMStream/PublishMovingBaselineData directly instead of through Forward lambdas. Rename SeptentrioDriver::publish_rtcm_corrections() to publish_moving_baseline(): it is only reached from the Secondary moving-base decode path and only ever emits moving-baseline RTCM. Corrections-path behavior (instance selection, generation-gap warning, burst cap, self-injection filter) is unchanged. Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com> * refactor(gps): rename RTCM inject gate to receiverReady and simplify chunk helper Rename GPSHelper::shouldInjectRTCMCorrections() to receiverReady(). The virtual gates injection of both RTCM corrections and moving-baseline, and for UBX it simply reports whether the receiver is configured, so the name now describes what it actually gates rather than implying it only concerns corrections. Bumps the GPS-drivers submodule to the matching rename. Drop the vestigial message-type template parameter from publish_rtcm_chunks: both topics share rtcm_data_s, so only the publication type needs templating. * fix(septentrio): log dropped RTCM uORB generations Match the gps driver and warn when the RTCM corrections or moving-baseline subscription skips a uORB generation, so dropped injection data is visible. * docs(docs): Docs only update to the RtcmData msg * chore(gps): pin GPSDrivers to merged main Contains #212 (RTCM/moving-baseline gating virtuals), #213 (X20 CFG-ODO NAK tolerance), and #215 (SPARTN input enable, best-effort VALSET). Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com> * fix(septentrio): reassemble RTCM frames per stream before injecting Both drains wrote raw uORB chunks to the receiver, so a fragmented frame on one stream could get the other stream's bytes spliced in mid-frame and corrupt both. Reassemble each stream in its own parser and only write complete frames, mirroring the gps driver. Injection stats now count frames instead of uORB chunks. Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com> * refactor(mavlink)!: remove GPS_RTCM_DATA output stream GPS_RTCM_DATA is a GCS-to-vehicle correction transport; echoing rtcm_corrections back out over MAVLink had no consumer and the echo was lossy anyway (uint8 len and 180-byte payload truncate 300-byte uORB chunks). Receiving GPS_RTCM_DATA is unchanged. Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com> --------- Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com> Co-authored-by: Hamish Willee <hamishwillee@gmail.com>	2026-07-16 13:23:26 -06:00