Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 :
15 : ATH- Vinayak Pundhir (VP)
16 : */
17 :
18 : /*
19 : Aeron Systems PLX series INS — External AHRS backend.
20 :
21 : Supports NAV_PARA1 (0xAAA1), NAV_PARA2 (0xA2A2), SENS_PARA (0xA2A3),
22 : GPS_PARA (0xA2A4), DEV_INFO (0xA2A6), EXTD_GNSS (0xA2A8)
23 :
24 : The driver runs a dedicated update thread that reads bytes from the
25 : UART, runs them through a state-machine parser, then dispatches each
26 : CRC-valid frame to a publisher that re-casts the payload as a packed
27 : on-wire struct and pushes the values into the AP_ExternalAHRS state
28 : plus the INS / Baro / Compass / GPS frontends.
29 :
30 : Recommended usage (Plane / Copter / Quadplane):
31 :
32 : param set AHRS_EKF_TYPE 11
33 : param set EAHRS_TYPE 10
34 : param set EAHRS_RATE <SENS_PARA rate, e.g. 200>
35 : param set SERIAL*_PROTOCOL 36
36 : param set SERIAL*_BAUD 921
37 : param set GPS1_TYPE 21
38 :
39 : EAHRS_RATE must match the actual SENS_PARA rate, and SENS_PARA rate
40 : must be at least the vehicle's SCHED_LOOP_RATE (400 Hz copter,
41 : 300 Hz VTOL, 50 Hz plane) when using IMU data.
42 : */
43 :
44 : #pragma once
45 :
46 : #include "AP_ExternalAHRS_config.h"
47 :
48 : #if AP_EXTERNAL_AHRS_AERON_PLX_ENABLED
49 :
50 : #include "AP_ExternalAHRS_backend.h"
51 :
52 : class AP_ExternalAHRS_Aeron_plx : public AP_ExternalAHRS_backend
53 : {
54 : public:
55 : AP_ExternalAHRS_Aeron_plx(AP_ExternalAHRS *frontend, AP_ExternalAHRS::state_t &state);
56 :
57 : // AP_ExternalAHRS_backend interface
58 : int8_t get_port() const override;
59 : const char *get_name() const override;
60 : bool healthy() const override;
61 : bool initialised() const override;
62 : bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override;
63 : void get_filter_status(nav_filter_status &status) const override;
64 : bool get_variances(float &velVar, float &posVar, float &hgtVar,
65 : Vector3f &magVar, float &tasVar) const override;
66 8163 : void update() override {} // all work happens in update_thread()
67 :
68 : protected:
69 50 : uint8_t num_gps_sensors() const override
70 : {
71 50 : return 1;
72 : }
73 :
74 : private:
75 :
76 : // packet identifiers (PRP_ID field, big-endian on the wire)
77 : enum class AeronPacketID : uint16_t {
78 : NAV_PARA1 = 0xAAA1,
79 : NAV_PARA2 = 0xA2A2,
80 : SENS_PARA = 0xA2A3,
81 : GPS_PARA = 0xA2A4,
82 : DEV_INFO = 0xA2A6,
83 : EXTD_GNSS = 0xA2A8,
84 : HEADING_S = 0x0165,
85 : H_SPEED_S = 0x0166,
86 : V_SPEED_S = 0x0167,
87 : POSITION_S = 0x0168,
88 : ALTITUDE_S = 0x0169,
89 : GNSS_PKT = 0x0212,
90 : GNSS_FIX_STATUS = 0x0067,
91 : EXT_SENSORS_MAG = 0x0117,
92 : EXT_SENSORS_ASP = 0x0362,
93 : };
94 :
95 : // GNSS fix types reported by PLX (raw value field inside gps_status)
96 : enum class AeronGnssFix : uint8_t {
97 : NO_FIX = 0,
98 : GNSS_FIX = 1,
99 : SBAS_FIX = 2,
100 : RTK_FIX = 3,
101 : RTK_FLOAT = 4,
102 : };
103 :
104 : // jamming/spoofing detection levels
105 : enum class AeronJamSpoof : uint8_t {
106 : NOT_DETECTED = 1,
107 : WARNING = 2,
108 : CRITICAL = 3,
109 : };
110 :
111 : // hardware status flag positions inside NAV_PARA2.hw_status
112 : enum class AeronHwStatus : uint8_t {
113 : GNSS = 0,
114 : ACC = 1,
115 : GYR = 2,
116 : MAG = 3,
117 : BARO = 4,
118 : SUP_VLTG = 5,
119 : CM_PRT = 10,
120 : RAM = 11,
121 : FIRMWARE = 12,
122 : CONFIG = 13,
123 : EXT_MAG = 22,
124 : EXT_ASP = 23,
125 : };
126 :
127 : // Deferred GCS messages - collected during one drain pass and emitted
128 : // once at the end, so GCS_SEND_TEXT() calls don't get sprinkled through
129 : // the byte-by-byte parser loop.
130 : struct AeronDeferredMsgs {
131 : bool crc_fail;
132 : bool bad_length; // recognised ID but payload too short for it
133 : bool unknown_pkt; // unrecognised packet ID (unknown_id is valid)
134 : bool ext_mag_checked;
135 : bool ext_asp_checked;
136 : bool stale_data;
137 : uint16_t unknown_id; // only meaningful when unknown_pkt is set
138 : };
139 :
140 : // ---- packed on-wire payload layouts ----
141 : // These are cast directly over rx_buf[8 .. payload_end] once the CRC
142 : // has validated the frame, so the layout must exactly match the
143 : // PLX3 wire format. The matching SIM_Aeron structs carry the same
144 : // static_asserts.
145 :
146 : struct PACKED NavPara1Payload {
147 : uint32_t epoch_time;
148 : uint32_t microseconds;
149 : uint32_t ins_status;
150 : uint32_t hw_status;
151 : float euler[3];
152 : float course;
153 : float velocity_ned[3];
154 : double position[2];
155 : float height_abv_ellip;
156 : };
157 : static_assert(sizeof(NavPara1Payload) == 64, "NavPara1Payload size mismatch");
158 :
159 : struct PACKED NavPara2Payload {
160 : uint32_t epoch_time;
161 : uint32_t microseconds;
162 : uint32_t ins_status;
163 : uint32_t hw_status;
164 : float quat[4];
165 : float body_vel[3];
166 : double ecef_pos[3];
167 : float altitude;
168 : };
169 : static_assert(sizeof(NavPara2Payload) == 72, "NavPara2Payload size mismatch");
170 :
171 : struct PACKED SensParaPayload {
172 : uint32_t epoch_time;
173 : uint32_t microseconds;
174 : float temperature;
175 : float gyro[3];
176 : float accel[3];
177 : float mag[3];
178 : float euler_rates[3];
179 : float baro_pressure;
180 : float baro_temperature;
181 : float baro_altitude;
182 : };
183 : static_assert(sizeof(SensParaPayload) == 72, "SensParaPayload size mismatch");
184 :
185 : struct PACKED GpsParaPayload {
186 : uint32_t epoch_time;
187 : uint32_t microseconds;
188 : uint32_t gps_status;
189 : double gps_position[2];
190 : float gps_height_abv_ellip;
191 : float gps_undulation;
192 : float pdop;
193 : float hdop;
194 : float gdop;
195 : };
196 : static_assert(sizeof(GpsParaPayload) == 48, "GpsParaPayload size mismatch");
197 :
198 : struct PACKED ExtdGnssPayload {
199 : float hpa;
200 : float vpa;
201 : float hva;
202 : float vdop;
203 : double gnss_vned[3];
204 : };
205 : static_assert(sizeof(ExtdGnssPayload) == 40, "ExtdGnssPayload size mismatch");
206 :
207 : // ---- hw_status reporting table ----
208 : // One row per status flag we track. Used by report_hw_status() to
209 : // emit both the fault and the matching "okay" message, so the value
210 : // -> name mapping lives in exactly one place.
211 : struct HwStatusEntry {
212 : AeronHwStatus status;
213 : uint8_t severity; // a MAV_SEVERITY_* value
214 : const char *fault_msg;
215 : const char *clear_msg; // nullptr -> never emit a recovery message
216 : };
217 : static const HwStatusEntry hw_status_table[];
218 :
219 : // Protects every field of `shared` (the cross-thread cache written by
220 : // the parser thread's publish_* handlers and read by the const
221 : // accessors get_filter_status()/report_hw_status()/get_variances() on
222 : // other threads).
223 : //
224 : // Lock ordering: the frontend `state` struct has its own lock
225 : // (state.sem, owned by the base class). These two locks CAN nest - the
226 : // frontend holds state.sem while calling get_filter_status(), which
227 : // then takes arn_sem. To avoid an AB-BA deadlock the order is fixed:
228 : // state.sem is the OUTER lock, arn_sem is the INNER lock.
229 : // i.e. code may take arn_sem while holding state.sem, but must never
230 : // take state.sem while holding arn_sem. The publish_* handlers respect
231 : // this by snapshotting `shared` under arn_sem and RELEASING it before
232 : // acquiring state.sem to write `state`.
233 : mutable HAL_Semaphore arn_sem;
234 :
235 : void update_thread();
236 : void check_and_decode();
237 : void report_hw_status();
238 : void parse_byte(uint8_t byte, AeronDeferredMsgs &msgs);
239 : void handle_chunk_buf_packet(uint16_t prp_id, const uint8_t *payload,
240 : uint16_t payload_len, AeronDeferredMsgs &msgs);
241 :
242 : void publish_nav_para1(const NavPara1Payload &p);
243 : void publish_nav_para2(const NavPara2Payload &p);
244 : void publish_sens_para(const SensParaPayload &p);
245 : void publish_gps_para(const GpsParaPayload &p, AeronDeferredMsgs &msgs);
246 : void publish_extd_gnss(const ExtdGnssPayload &p);
247 :
248 : // GPS formatting - combines GPS_PARA + EXTD_GNSS + NAV_PARA1 fields
249 : void format_and_push_gps(AeronDeferredMsgs &msgs);
250 :
251 : // Single source of truth for decoding the PLX GPS status word into an
252 : // ArduPilot fix type, so the AP_GPS publication path and
253 : // get_filter_status() cannot reach different conclusions from the same
254 : // status word.
255 : static AP_GPS_FixType fix_type_from_status(uint32_t status);
256 :
257 : // EXTD_GNSS freshness. Caller must hold arn_sem.
258 : bool extd_fresh_locked() const;
259 :
260 : // utility
261 : void send_deferred_messages(const AeronDeferredMsgs &msgs);
262 : // Sends only the requested queries. Returns false if a request could
263 : // not be written to the UART in full.
264 : bool query_ext_sensors(bool want_mag, bool want_asp);
265 : void check_for_query_ext_sensors();
266 :
267 : // UART interface (read-only after construction)
268 : AP_HAL::UARTDriver *uart;
269 : uint32_t baudrate;
270 : int8_t port_num;
271 : bool setup_complete;
272 :
273 : // Shared sensor data
274 : struct SharedData {
275 : // NAV_PARA2
276 : uint32_t nav2_hw_status;
277 : // GPS_PARA
278 : uint32_t gps_epoch_s;
279 : uint32_t gps_status;
280 : uint32_t gps_microseconds;
281 : float gps_undulation_m;
282 : float gps_hdop;
283 : double gps_position[2];
284 : float gps_height_ellip_m;
285 : // EXTD_GNSS
286 : float cust_hpa_m;
287 : float cust_vpa_m;
288 : float cust_hva_mps;
289 : float cust_vdop;
290 : double cust_gnss_vel_ned_mps[3];
291 : // external sensor presence / response-received flags
292 : uint8_t ext_mag_present;
293 : uint8_t ext_asp_present;
294 : bool ext_mag_checked;
295 : bool ext_asp_checked;
296 : };
297 :
298 : SharedData shared;
299 :
300 : // parser state
301 : enum class ParseState : uint8_t {
302 : RESET = 0,
303 : SYNC,
304 : LEN_HIGH,
305 : LEN_LOW,
306 : PAYLOAD,
307 : CRC_BYTE,
308 : };
309 :
310 : ParseState parse_state;
311 : uint8_t sync_count;
312 : uint16_t pkt_length;
313 : uint16_t write_idx;
314 : // A frame is bounded by the LEN sanity check to pkt_length <=
315 : // PKT_LEN_MAX (255) plus one CRC byte, so the largest index written
316 : // is 255 - rx_buf[256] holds any valid frame exactly.
317 : uint8_t rx_buf[256];
318 :
319 : // Per-packet-group staleness timestamps.
320 : volatile uint32_t last_sens_ms;
321 : volatile uint32_t last_nav1_ms;
322 : volatile uint32_t last_nav2_ms;
323 : volatile uint32_t last_gnss_ms;
324 : volatile uint32_t last_extd_ms;
325 :
326 : // GNSS state derived on demand from shared.gps_status (fix bit),
327 : // not stored - see get_filter_status().
328 : uint32_t last_jam_warn_ms;
329 :
330 : // init / gating flags
331 : uint32_t prev_nav_ms; // last NAV timestamp seen by check_for_query_ext_sensors
332 :
333 : // Bounded retry for the external-sensor presence queries. The PLX answers
334 : // whether or not the sensor is fitted, so an unanswered query means the
335 : // request or its reply was lost rather than that the sensor is absent.
336 : // Retry the outstanding ones a few times, then stop: firmware that does
337 : // not implement the query would otherwise retry forever. Giving up leaves
338 : // the *_checked flags false, which keeps EXT_MAG/EXT_ASP unreportable in
339 : // report_hw_status() - the safe default.
340 : static constexpr uint32_t EXT_QUERY_RETRY_MS = 2000;
341 : static constexpr uint8_t EXT_QUERY_MAX_TRIES = 5;
342 : uint32_t last_ext_query_ms;
343 : uint8_t ext_query_tries;
344 :
345 : // throttled-warning state for hw_status flags. Rising edges fire
346 : // immediately; persistent faults re-warn every HEALTH_REPEAT_INTERVAL_MS.
347 : // last_warned_ms is indexed by table ROW (0..HW_STATUS_COUNT-1), so its
348 : // size tracks the number of table entries and does not depend on the
349 : // enum bit values. A static_assert in the .cpp keeps this in sync with
350 : // ARRAY_SIZE(hw_status_table).
351 : static constexpr uint8_t HW_STATUS_COUNT = 12;
352 : uint32_t last_warned_stat;
353 : uint32_t last_warned_ms[HW_STATUS_COUNT];
354 :
355 : // hw_status bits that mean the device cannot be trusted to navigate.
356 : // Manual 7.1.2.9 Table 15 rates all of these High severity and PBIT:
357 : // they are power-on self-test failures with no recovery path, which is
358 : // why hw_status_table gives the first three no clear_msg. Config-memory
359 : // failure in particular leaves the device streaming fresh packets from a
360 : // wrong configuration, which no staleness check can catch.
361 : //
362 : // ACC and GYR are included because the PLX's own fused solution is built
363 : // on them: a unit reporting a non-functional accelerometer or gyro is no
364 : // more trustworthy fused than raw. Being PBIT, they are set at power-on
365 : // rather than mid-flight, so this fails pre-arm rather than dropping the
366 : // AHRS in the air. MAG and BARO are deliberately absent - the PLX
367 : // navigates without them, so those are gated at the publication site
368 : // instead (see publish_sens_para).
369 : static constexpr uint32_t HW_STATUS_FATAL_MASK =
370 : (1U << uint8_t(AeronHwStatus::RAM))
371 : | (1U << uint8_t(AeronHwStatus::FIRMWARE))
372 : | (1U << uint8_t(AeronHwStatus::CONFIG))
373 : | (1U << uint8_t(AeronHwStatus::ACC))
374 : | (1U << uint8_t(AeronHwStatus::GYR));
375 :
376 : // Throttle state for the noisy parser-side GCS messages.
377 : uint32_t last_crc_warn_ms;
378 : uint32_t last_garbage_warn_ms;
379 : uint32_t last_stale_data_ms;
380 :
381 : // Per-message log decimation timestamps (high-rate messages only).
382 : uint32_t last_aern_log_ms;
383 : uint32_t last_aers_log_ms;
384 :
385 : // Per-group staleness deadlines for healthy().
386 : static constexpr uint32_t SENS_TIMEOUT_MS = 40;
387 : static constexpr uint32_t NAV_TIMEOUT_MS = 200;
388 : static constexpr uint32_t GNSS_TIMEOUT_MS = 500;
389 :
390 : // EXTD_GNSS validity window. Past this, the GPS message is not
391 : // published at all (see format_and_push_gps) so no stale accuracy or
392 : // velocity reaches AP_GPS.
393 : static constexpr uint32_t EXTD_VALIDITY_MS = 500;
394 :
395 : // NAV gap beyond which the PLX is assumed to have reset (re-query
396 : // external sensors on resumption).
397 : static constexpr uint32_t DEVICE_RESET_GAP_MS = 500;
398 :
399 : // At how much interval in ms should we shout health messages?
400 : static constexpr uint32_t HEALTH_REPEAT_INTERVAL_MS = 10000;
401 :
402 : // Minimum spacing between repeated parser-side telemetry warnings.
403 : static constexpr uint32_t WARN_THROTTLE_MS = 5000;
404 :
405 : // State Machine gears
406 : static constexpr uint8_t SYNC_BYTE = 0x05;
407 : static constexpr uint8_t SYNC_REQUIRED = 4;
408 : static constexpr uint16_t PKT_LEN_MIN = 8;
409 : static constexpr uint16_t PKT_LEN_MAX = 255;
410 :
411 : // GPS_PARA status word: bit 10 is the "GNSS fix present" flag,
412 : // bits 0-9 the visible-satellite count and bits 11-14 the fix type
413 : // code (manual 7.1.2.10, Table 16).
414 : static constexpr uint32_t GPS_STATUS_FIX_BIT = 0x00000400U;
415 : static constexpr uint32_t GPS_STATUS_NUMSAT_MASK = 0x000003FFU;
416 : static constexpr uint32_t GPS_STATUS_FIXTYPE_MASK = 0x00007800U;
417 : static constexpr uint8_t GPS_STATUS_FIXTYPE_SHIFT = 11;
418 : };
419 :
420 : #endif // AP_EXTERNAL_AHRS_AERON_PLX_ENABLED
|
