mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-10-06 09:02:52 +08:00
Jacob Dahl
eb8f2f9b42
* feat(sensors/vehicle_gps_position): publish GNSS heading on its own topic Dual-antenna heading was only reachable through the blended vehicle_gps_position, so it carried the position timestamp, followed the position blending weights and was published at the position rate. vehicle_gnss_heading carries one selected source at a time, preferring sensor_gnss_relative (callback driven) and falling back to sensor_gps.heading for receivers that do not publish it. Per-receiver delay and the driver heading offset are resolved by device_id; a sample timestamp within 10 ms of the publish timestamp is treated as unset so the configured delay applies. * refactor(ekf2): fuse GNSS heading from its own sample buffer Heading rode on gnssSample and was only fused when a position sample was popped: faster heading was dropped by the observation rate limit, its timestamp was the position timestamp and a position outage blocked the start. gnssYawSample has its own TimestampedRingBuffer and pop. Non-finite yaw is rejected at the interface and a NaN heading_offset is treated as zero instead of reaching the fusion Jacobian. The test simulator feeds heading as an independent sensor. * feat(sensors/vehicle_gps_position): per-receiver baseline rotation SENS_GPSn_ROT The antenna baseline orientation lived in three places depending on the transport: the serial driver (GPS_YAW_OFFSET), the Septentrio receiver (SEP_YAW_OFFS, SEP_PITCH_OFFS) and EKF2 for anything arriving with a NaN offset, in practice CAN receivers (EKF2_GPS_YAW_OFF). A second receiver with a different baseline had no parameter at all. Drivers now publish the raw baseline heading and vehicle_gps_position rotates it into the body frame from the receiver's SENS_GPSn_* slot, the same Rotation enum the other sensors use, before blending and before publishing vehicle_gnss_heading. Bumps PX4-GPSDrivers so sensor_gnss_relative carries a real UTC time and no parse-time sample timestamp, which lets the relative-position heading be delay-compensated and PPS-aligned. * refactor(drivers/gps): drop the UBX heading_offset setting Bumps PX4-GPSDrivers to PX4/PX4-GPSDrivers#241, which removes the driver-side heading offset now that SENS_GPSn_ROT is the only place the baseline is rotated. Assisted-by: Claude:claude-opus-5-5 * refactor(sensors/vehicle_gps_position): remove sensor_gps.heading_offset With the baseline rotation in SENS_GPSn_ROT, a finite offset only meant "the driver already rotated this heading, skip SENS_GPSn_ROT". The only remaining writer was older DroneCAN node firmware through the Fix2 ECEF hack, which made a CAN node's heading switch between two conventions depending on whether a RelPosHeading arrived between Fix2 messages: with a non-zero SENS_GPSn_ROT yaw the heading jumped by that angle. Every receiver's heading is now the measured baseline and SENS_GPSn_ROT is the only rotation. MicroStrain external heading aiding, which recovered the baseline heading from vehicle_gps_position, reads vehicle_gnss_heading. Assisted-by: Claude:claude-opus-5-5 * fix(mavlink): report the body-frame GNSS heading in GPS_RAW_INT and GPS2_RAW sensor_gps now carries the measured baseline heading, so for a baseline not along the body x axis the yaw field was off by the mounting angle. The body-frame heading is taken from vehicle_gnss_heading when the streamed receiver is the heading source. Assisted-by: Claude:claude-opus-5-5 * fix(replay): replay vehicle_gnss_heading to EKF2 ReplayEkf2 publishes only the topics it lists from inside the lockstep barrier, so EKF2 replay never received the heading. Assisted-by: Claude:claude-opus-5-5 * feat(parameters): migrate GNSS heading offsets to SENS_GPSn_YAW GPS_YAW_OFFSET, SEP_YAW_OFFS and EKF2_GPS_YAW_OFF were removed without a migration, so an upgraded vehicle silently lost its antenna mounting offset and its GNSS yaw was off by that angle. Assisted-by: Claude:claude-opus-5-5 * fix(sensors/vehicle_gps_position): build GNSS heading only where it is consumed The heading selection, baseline rotation and SENS_GPSn_ROT parameters overflowed diatone_mamba-f405-mk2 flash by 657 bytes. Boards built without EKF2 GNSS yaw fusion or MicroStrain have no consumer for vehicle_gnss_heading, so SENSORS_VEHICLE_GNSS_HEADING defaults on only with one of them. The parameter migration also drops fmodf, since the old parameters never exceeded one wrap. Assisted-by: Claude:claude-opus-5-5 * feat(sensors/vehicle_gps_position): carry the heading receiver's jamming and spoofing state So that EKF2 can gate GNSS yaw on the receiver providing the heading, which is not necessarily the position receiver. Assisted-by: Claude:claude-fable-5-1 * fix(ekf2): gate GNSS yaw on the heading, not the position checks Yaw fusion could only start once the position checks passed, and stopped with them. On an ARK G5 outdoors the heading was good from 36 s after power-up and was first fused at 104 s, held back by EPV while Galileo HAS converged; a 0.45 s PDOP failure while the vehicle was handled on the ground stopped yaw for 10 s with its innovations under 5.4 deg. The heading is its own observation, often from its own receiver: yaw now starts on fresh data, tilt alignment and the heading receiver's spoofing and jamming state under the EKF2_GPS_CHECK bits, and keeps running when position and velocity fusion stop. The hold-off after a yaw fusion failure was a side effect of the position checks resetting; it is now a yaw timer that withholds the heading from a reset for the GNSS health time. Assisted-by: Claude:claude-fable-5-1 * fix(sensors/vehicle_gps_position): carry an unknown GNSS sample time as 0 A GNSS sample time within 10 ms of the publish time was treated as unset and replaced by SENS_GPSn_DELAY. An ARK G5 over DroneCAN measures 8.3 to 21.5 ms from epoch to receipt, so 0.4% of its headings were re-stamped 110 ms early; at 20 Hz EKF2 dropped them as out of order, at a lower rate they would have been fused 100 ms late. A stamp meaning unset that crossed a busy bus in more than 10 ms would have been trusted instead. Unknown is now 0 end to end: the serial driver leaves timestamp_sample at 0, the CAN node sends RelPosHeading's timestamp as UNKNOWN for it, and the autopilot's bridge publishes 0 when the node timestamp can't be used. Any other sample time before the publish time is trusted. Assisted-by: Claude:claude-opus-5-5 * feat(sensors/vehicle_gps_position): derive the GNSS heading baseline from antenna positions SENS_GPSn_ROT and its Euler angles carried one number: roll never moves a baseline along x, pitch only flagged a vertical one, and the heading model uses the baseline yaw alone. Nothing checked the heading against the installation either, so a receiver that flagged float solutions as valid reset yaw 87 deg wrong at first fix on an ARK G5 moving-base pair. As in ArduPilot, the baseline now comes from the antenna positions: SENS_GPSn_HDG selects the other receiver's antenna (moving base, from both slots' SENS_GPSn_OFF) or a custom SENS_GPSn_BLX/Y/Z (single dual-antenna receiver), and a heading is used only when the receiver reports a baseline within 20% of it in length and in its vertical component over the sample's attitude. Those float solutions reported 10.9 m and 1.04 m for a 0.347 m baseline. A source is published once it has passed for 1 s: the headings right after the base antenna was re-plugged passed for 0.9 s while up to 25 deg wrong. vehicle_gnss_heading logs the reported baseline length. A yaw offset can't be turned into a baseline, so GPS_YAW_OFFSET, SEP_YAW_OFFS and EKF2_GPS_YAW_OFF are not migrated; GNSS yaw stays off, with a warning, until SENS_GPSn_HDG is set. Assisted-by: Claude:claude-opus-5-5 * fix(ekf2): stop GNSS yaw on a spoofed or jammed heading receiver The heading receiver's spoofing and jamming state only kept yaw fusion from starting; a receiver that reported spoofing in flight kept feeding yaw, where position fusion stops. Its samples are now not fused, the fusion stops once none has been for the reset timeout, and a reset to its heading waits for the GNSS health time, as for position. Assisted-by: Claude:claude-opus-5-5 * docs(dronecan): configure the ARK G5 heading baseline on the autopilot The page still configured the heading offset with the module's SEP_OFFS_YAW, and required GPS blending for the heading to be published, which no longer carries it. Assisted-by: Claude:claude-opus-5-5 * fix(sensors/vehicle_gps_position): drop a mismatched GNSS heading without restarting the settle Outdoors on an ARK G5 moving-base pair 0.35 m apart, the reported down component of the baseline scattered by up to 0.16 m while the airframe was turned, and 9 cm at rest, against the 7 cm the vertical check allows. The rejected headings were as good as the accepted ones, but every rejection restarted the 1 s settle, so during 720 deg turns a heading was published for 4 to 62% of the time. A sample that fails the baseline checks is now only dropped; the settle restarts when the receiver reports no heading. Replayed on that log, 85 to 99% of the turns get a heading. Assisted-by: Claude:claude-opus-5-5 * fix(sensors/vehicle_gps_position): drop the GNSS heading baseline's vertical check The check compared the reported down component of the baseline with the configured baseline rotated by the vehicle attitude, within 20% of the baseline length. On an ARK G5 moving-base pair 0.35 m apart the down component scattered by up to 9 cm at rest and 0.16 m while the airframe was turned, so it rejected good headings: across seven outdoor logs it rejected 184 samples under 5 deg of error and 6 over 10 deg, all of which the EKF innovation gate would handle, and nothing the length check missed on a receiver that withholds float headings. The length check and the near-vertical check on the reported baseline stay. The sensors module no longer reads the vehicle attitude to check the heading. * feat(sensors/vehicle_gps_position): SENS_GNSSn_HDG with the auxiliary antenna position The heading parameters are new, so they take the GNSS name the rest of the SENS_GPS family moves to rather than being renamed again shortly. The setup names what the receiver is: the rover of a moving base pair, or a receiver with two antennas. For the latter the second antenna is given by its position, like the main one's SENS_GPSn_OFF, so every setup derives the baseline from antenna positions. Assisted-by: Claude:claude-opus-5-5 * feat(drivers/septentrio): publish the heading baseline on sensor_gnss_relative The heading only reached PX4 through sensor_gps, without the baseline the receiver measured, so the sensors module could not check it against the configured one and a heading from float ambiguities, confident on a baseline metres off, was used. AuxAntPositions (dual antenna) or BaseVectorGeod (moving base rover) now go out with the heading of the same epoch on sensor_gnss_relative at EndOfAtt, the heading valid only with fixed ambiguities. Ported from ARK's driver, where it was tested outdoors on mosaic-G5 P3H, P6 and P8. A receiver that rejects the extra blocks falls back to the previous output without a heading. The heading accuracy is the square root of the reported variance; it was the deg^2 variance scaled as if it were degrees. Assisted-by: Claude:claude-opus-5-5 * refactor(sensors/vehicle_gps_position): take GNSS heading only from sensor_gnss_relative The sensor_gps fallback carried no reported baseline, so its headings skipped the length check that rejects a wrong ambiguity fix. Every receiver that matters now reports on sensor_gnss_relative: u-blox, Septentrio, the Unicore UM982 (PX4/PX4-GPSDrivers bump) and DroneCAN RelPosHeading. The heading fields of sensor_gps go, with the paths that filled them: NMEA HDT, Trimble MB-Two, Femtomes, SBG and MicroStrain, the heading in DroneCAN Fix2 on both the node and the autopilot, and the RelPosHeading copy into sensor_gps. Assisted-by: Claude:claude-opus-5-5 * docs(gps_compass): configure GNSS heading with SENS_GNSSn_HDG The heading parameters were renamed and the heading now only comes from receivers that report their baseline, so the Trimble MB-Two and Femtomes pages no longer describe a heading setup. The antenna spacing is the recommended 30 cm rather than the 5 cm the code rejects below. Assisted-by: Claude:claude-opus-5-5 * chore(drivers/gps): point PX4-GPSDrivers at the merged PX4/PX4-GPSDrivers#242 The submodule tracked the PR's branch commit; the squash-merged commit has the same tree. Assisted-by: Claude:claude-opus-5-5