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 : See AP_ExternalAHRS_Aeron_plx.h for protocol overview and usage.
21 : */
22 :
23 : #define AP_MATH_ALLOW_DOUBLE_FUNCTIONS 1
24 :
25 : #include "AP_ExternalAHRS_config.h"
26 :
27 : #if AP_EXTERNAL_AHRS_AERON_PLX_ENABLED
28 :
29 : #include "AP_ExternalAHRS_Aeron_plx.h"
30 :
31 : #include <AP_Baro/AP_Baro.h>
32 : #include <AP_Compass/AP_Compass.h>
33 : #include <AP_GPS/AP_GPS.h>
34 : #include <AP_HAL/AP_HAL.h>
35 : #include <AP_HAL/utility/sparse-endian.h>
36 : #include <AP_InertialSensor/AP_InertialSensor.h>
37 : #include <AP_Logger/AP_Logger.h>
38 : #include <AP_Math/AP_Math.h>
39 : #include <AP_Math/crc.h>
40 : #include <AP_SerialManager/AP_SerialManager.h>
41 : #include <GCS_MAVLink/GCS.h>
42 :
43 : // Local debug macro - no-op in release builds.
44 : #ifndef AERON_DEBUG
45 : #define AERON_DEBUG 0
46 : #endif // AERON_DEBUG
47 : #if AERON_DEBUG
48 : #define debug(fmt, args ...) GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "Aeron: " fmt, ##args)
49 : #else
50 : #define debug(...) do {} while (0)
51 : #endif // AERON_DEBUG
52 :
53 : extern const AP_HAL::HAL &hal;
54 :
55 11 : AP_ExternalAHRS_Aeron_plx::AP_ExternalAHRS_Aeron_plx(AP_ExternalAHRS *_frontend,
56 11 : AP_ExternalAHRS::state_t &_state)
57 11 : : AP_ExternalAHRS_backend(_frontend, _state)
58 : {
59 11 : const auto &sm = AP::serialmanager();
60 11 : uart = sm.find_serial(AP_SerialManager::SerialProtocol_AHRS, 0);
61 11 : baudrate = sm.find_baudrate(AP_SerialManager::SerialProtocol_AHRS, 0);
62 11 : port_num = sm.find_portnum(AP_SerialManager::SerialProtocol_AHRS, 0);
63 :
64 11 : if (uart == nullptr) {
65 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Aeron: Serial port not found");
66 0 : return;
67 : }
68 :
69 : // Not offering IMU by default; rates below 400 Hz are too slow for
70 : // multicopter platforms. Users running SENS_PARA at >= 400 Hz can
71 : // opt into IMU via EAHRS_SENSORS.
72 11 : set_default_sensors(uint16_t(AP_ExternalAHRS::AvailableSensor::GPS) |
73 : uint16_t(AP_ExternalAHRS::AvailableSensor::BARO) |
74 : uint16_t(AP_ExternalAHRS::AvailableSensor::COMPASS));
75 :
76 11 : if (!hal.scheduler->thread_create(
77 : FUNCTOR_BIND_MEMBER(&AP_ExternalAHRS_Aeron_plx::update_thread, void),
78 : "Aeron",
79 : 2048,
80 : AP_HAL::Scheduler::PRIORITY_SPI,
81 : 0)) {
82 0 : AP_HAL::panic("Aeron: failed to create update thread");
83 : }
84 :
85 11 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,
86 : "Aeron: Initialised on SERIAL%d @ %lu baud",
87 : port_num,
88 : (unsigned long)baudrate);
89 : }
90 :
91 11 : void AP_ExternalAHRS_Aeron_plx::update_thread()
92 : {
93 11 : uart->begin(baudrate, 2048, 256);
94 11 : setup_complete = true;
95 :
96 : while (true) {
97 43967 : check_and_decode();
98 43967 : report_hw_status();
99 43967 : hal.scheduler->delay_microseconds(1000);
100 : }
101 : }
102 :
103 : // Core pipeline: read -> parse -> publish -> deferred messages.
104 43967 : void AP_ExternalAHRS_Aeron_plx::check_and_decode()
105 : {
106 43967 : if (uart == nullptr) {
107 13794 : return;
108 : }
109 :
110 : // Drain buffer, stack-local. 256 B is ~3x the max bytes that arrive
111 : // in one 1 ms tick at 921600 baud (~92 B), and still clears a full
112 : // UART RX backlog within a few ticks if the thread is briefly
113 : // preempted.
114 : uint8_t chunk_buf[256];
115 43967 : const uint16_t avail = MIN(uart->available(), uint16_t(sizeof(chunk_buf)));
116 43967 : if (avail == 0) {
117 13794 : return;
118 : }
119 :
120 30173 : const auto n_read = uart->read(chunk_buf, avail);
121 :
122 30173 : if (n_read <= 0) {
123 0 : return;
124 : }
125 :
126 : // msgs collects GCS notifications raised mid-parse (CRC failures,
127 : // unknown packets, ext-sensor replies) so they are emitted once per
128 : // drain instead of from inside the byte loop.
129 30173 : AeronDeferredMsgs msgs{};
130 :
131 2670018 : for (auto index = 0; index < n_read; index++) {
132 2639845 : parse_byte(chunk_buf[index], msgs);
133 : }
134 :
135 30173 : send_deferred_messages(msgs);
136 :
137 30173 : check_for_query_ext_sensors();
138 : }
139 :
140 : // State-machine parser for the Aeron binary protocol. On a CRC-valid
141 : // frame, dispatches via handle_chunk_buf_packet() rather than leaving
142 : // the caller to read implicitly-set member state. CRC failures and
143 : // unknown packets are flagged in msgs for the caller to surface as
144 : // throttled GCS warnings.
145 2639845 : void AP_ExternalAHRS_Aeron_plx::parse_byte(uint8_t byte, AeronDeferredMsgs &msgs)
146 : {
147 2639845 : switch (parse_state) {
148 35055 : case ParseState::RESET:
149 35055 : write_idx = 0;
150 35055 : pkt_length = 0;
151 35055 : sync_count = 0;
152 35055 : parse_state = ParseState::SYNC;
153 : FALLTHROUGH;
154 :
155 140220 : case ParseState::SYNC:
156 140220 : if (byte == SYNC_BYTE) {
157 140220 : rx_buf[write_idx++] = byte;
158 140220 : if (++sync_count == SYNC_REQUIRED) {
159 35055 : parse_state = ParseState::LEN_HIGH;
160 : }
161 : } else {
162 0 : parse_state = ParseState::RESET;
163 : }
164 140220 : return;
165 :
166 35055 : case ParseState::LEN_HIGH:
167 35055 : pkt_length = byte;
168 35055 : rx_buf[write_idx++] = byte;
169 35055 : parse_state = ParseState::LEN_LOW;
170 35055 : return;
171 :
172 35055 : case ParseState::LEN_LOW:
173 35055 : rx_buf[write_idx++] = byte;
174 35055 : pkt_length = (pkt_length << 8) | byte;
175 35055 : if (pkt_length >= PKT_LEN_MIN && pkt_length <= PKT_LEN_MAX) {
176 35055 : parse_state = ParseState::PAYLOAD;
177 : } else {
178 0 : parse_state = ParseState::RESET;
179 : }
180 35055 : return;
181 :
182 2394460 : case ParseState::PAYLOAD:
183 2394460 : rx_buf[write_idx++] = byte;
184 2394460 : if (write_idx >= pkt_length) {
185 35055 : parse_state = ParseState::CRC_BYTE;
186 : }
187 2394460 : return;
188 :
189 35055 : case ParseState::CRC_BYTE: {
190 35055 : rx_buf[write_idx++] = byte;
191 35055 : const bool valid = (byte == crc_xor_of_bytes(rx_buf, pkt_length));
192 35055 : if (valid) {
193 35055 : const uint16_t prp_id = be16toh_ptr(&rx_buf[6]);
194 35055 : const uint16_t payload_len = (pkt_length > 8) ? (pkt_length - 8) : 0;
195 35055 : handle_chunk_buf_packet(prp_id, &rx_buf[8], payload_len, msgs);
196 : } else {
197 0 : msgs.crc_fail = true;
198 : debug("CRC-ID=0x%04X",
199 : (uint16_t(rx_buf[6]) << 8) | rx_buf[7]);
200 : }
201 35055 : parse_state = ParseState::RESET;
202 35055 : return;
203 : }
204 : }
205 :
206 : // Unreachable - all enum values handled above.
207 0 : parse_state = ParseState::RESET;
208 : }
209 :
210 : // Dispatch a CRC-valid frame. The frame sits in rx_buf[0 .. pkt_length-1];
211 : // PRP_ID is at offset 6..7 (big-endian) and the payload starts at offset 8.
212 : // Called from inside parse_byte's CRC state so the data hasn't yet been
213 : // overwritten by the next incoming byte.
214 35055 : void AP_ExternalAHRS_Aeron_plx::handle_chunk_buf_packet(uint16_t prp_id,
215 : const uint8_t *payload,
216 : uint16_t payload_len,
217 : AeronDeferredMsgs &msgs)
218 : {
219 35055 : switch (AeronPacketID(prp_id)) {
220 10294 : case AeronPacketID::NAV_PARA1:
221 10294 : if (payload_len < sizeof(NavPara1Payload)) {
222 0 : msgs.bad_length = true;
223 0 : return;
224 : }
225 10294 : publish_nav_para1(*reinterpret_cast<const NavPara1Payload *>(payload));
226 10294 : return;
227 :
228 10305 : case AeronPacketID::NAV_PARA2:
229 10305 : if (payload_len < sizeof(NavPara2Payload)) {
230 0 : msgs.bad_length = true;
231 0 : return;
232 : }
233 10305 : publish_nav_para2(*reinterpret_cast<const NavPara2Payload *>(payload));
234 10305 : return;
235 :
236 10302 : case AeronPacketID::SENS_PARA:
237 10302 : if (payload_len < sizeof(SensParaPayload)) {
238 0 : msgs.bad_length = true;
239 0 : return;
240 : }
241 10302 : publish_sens_para(*reinterpret_cast<const SensParaPayload *>(payload));
242 10302 : return;
243 :
244 2066 : case AeronPacketID::GPS_PARA:
245 2066 : if (payload_len < sizeof(GpsParaPayload)) {
246 0 : msgs.bad_length = true;
247 0 : return;
248 : }
249 2066 : publish_gps_para(*reinterpret_cast<const GpsParaPayload *>(payload), msgs);
250 2066 : return;
251 :
252 2066 : case AeronPacketID::EXTD_GNSS:
253 2066 : if (payload_len < sizeof(ExtdGnssPayload)) {
254 0 : msgs.bad_length = true;
255 0 : return;
256 : }
257 2066 : publish_extd_gnss(*reinterpret_cast<const ExtdGnssPayload *>(payload));
258 2066 : return;
259 :
260 11 : case AeronPacketID::EXT_SENSORS_MAG: {
261 11 : if (payload_len < 1) {
262 0 : msgs.bad_length = true;
263 0 : return;
264 : }
265 11 : WITH_SEMAPHORE(arn_sem);
266 11 : shared.ext_mag_present = payload[0];
267 11 : shared.ext_mag_checked = true;
268 11 : msgs.ext_mag_checked = true;
269 11 : return;
270 11 : }
271 :
272 11 : case AeronPacketID::EXT_SENSORS_ASP: {
273 11 : if (payload_len < 1) {
274 0 : msgs.bad_length = true;
275 0 : return;
276 : }
277 11 : WITH_SEMAPHORE(arn_sem);
278 11 : shared.ext_asp_present = payload[0];
279 11 : shared.ext_asp_checked = true;
280 11 : msgs.ext_asp_checked = true;
281 11 : return;
282 11 : }
283 :
284 0 : case AeronPacketID::HEADING_S:
285 : case AeronPacketID::H_SPEED_S:
286 : case AeronPacketID::V_SPEED_S:
287 : case AeronPacketID::POSITION_S:
288 : case AeronPacketID::ALTITUDE_S:
289 : case AeronPacketID::DEV_INFO:
290 : case AeronPacketID::GNSS_PKT:
291 : case AeronPacketID::GNSS_FIX_STATUS:
292 : // Recognised but unused.
293 0 : return;
294 : }
295 :
296 : // Unknown packet ID - flag as garbage.
297 0 : msgs.unknown_pkt = true;
298 0 : msgs.unknown_id = prp_id;
299 : }
300 :
301 : // NAV_PARA1: velocity, position, euler angles, course
302 10294 : void AP_ExternalAHRS_Aeron_plx::publish_nav_para1(const NavPara1Payload &data)
303 : {
304 10294 : last_nav1_ms = AP_HAL::millis();
305 :
306 : float undulation_m;
307 : bool undulation_valid;
308 : {
309 10294 : WITH_SEMAPHORE(arn_sem);
310 10294 : undulation_m = shared.gps_undulation_m;
311 : // undulation (geoid separation) only becomes valid once GPS_PARA
312 : // has been received; before that it is zero and would corrupt the
313 : // MSL altitude by the geoid height (tens of metres).
314 10294 : undulation_valid = (last_gnss_ms != 0);
315 10294 : }
316 :
317 : {
318 10294 : WITH_SEMAPHORE(state.sem);
319 20588 : state.velocity = {
320 10294 : data.velocity_ned[0],
321 10294 : data.velocity_ned[1],
322 10294 : data.velocity_ned[2]
323 : };
324 10294 : state.have_velocity = true;
325 :
326 : // Only publish an absolute location once we can compute a correct
327 : // MSL altitude (i.e. undulation is valid). Velocity above has no
328 : // such dependency and is always published.
329 10294 : if (undulation_valid) {
330 20588 : state.location = Location {
331 10294 : int32_t(data.position[0] * 1.0e7),
332 10294 : int32_t(data.position[1] * 1.0e7),
333 10294 : int32_t((data.height_abv_ellip - undulation_m) * 1.0e2),
334 : Location::AltFrame::ABSOLUTE
335 : };
336 10294 : state.have_location = true;
337 10294 : state.last_location_update_us = AP_HAL::micros();
338 : }
339 10294 : }
340 :
341 : // Logging is done outside the lock. We must not hold state.sem across that.
342 : #if HAL_LOGGING_ENABLED
343 10294 : const uint32_t now_ms = AP_HAL::millis();
344 10294 : const int16_t log_rate = AP::externalAHRS().log_rate.get();
345 10294 : if (log_rate > 0 && now_ms - last_aern_log_ms >= uint32_t(1000U / log_rate)) {
346 1873 : last_aern_log_ms = now_ms;
347 : // @LoggerMessage: AERN
348 : // @Description: Aeron PLX fused navigation solution (NAV_PARA1)
349 : // @Field: TimeUS: Time since system startup
350 : // @Field: Epoch: Unix epoch seconds at sample time
351 : // @Field: Lat: Latitude
352 : // @Field: Lng: Longitude
353 : // @Field: Alt: Height above WGS84 ellipsoid
354 : // @Field: VN: Velocity north
355 : // @Field: VE: Velocity east
356 : // @Field: VD: Velocity down
357 : // @Field: Crs: Course over ground
358 : // @Field: Roll: Roll angle
359 : // @Field: Pitch: Pitch angle
360 : // @Field: Yaw: Yaw angle
361 : // @Field: InsSt: PLX INS filter-source status word
362 : // @Field: HwSt: PLX hardware status word
363 3746 : AP::logger().WriteStreaming(
364 : "AERN",
365 : "TimeUS," "Epoch," "Lat," "Lng," "Alt," "VN," "VE," "VD," "Crs," "Roll," "Pitch," "Yaw," "InsSt," "HwSt",
366 : "s" "s" "D" "U" "m" "n" "n" "n" "h" "r" "r" "r" "-" "-",
367 : "F" "-" "-" "-" "-" "-" "-" "-" "-" "-" "-" "-" "-" "-",
368 : "Q" "I" "d" "d" "f" "f" "f" "f" "f" "f" "f" "f" "I" "I",
369 : AP_HAL::micros64(),
370 1873 : data.epoch_time,
371 1873 : data.position[0], data.position[1], double(data.height_abv_ellip),
372 1873 : data.velocity_ned[0], data.velocity_ned[1], data.velocity_ned[2],
373 1873 : data.course,
374 1873 : data.euler[0], data.euler[1], data.euler[2],
375 1873 : data.ins_status,
376 1873 : data.hw_status
377 : );
378 : }
379 : #endif // HAL_LOGGING_ENABLED
380 10294 : }
381 :
382 : // NAV_PARA2: quaternion, body velocity, ECEF, hw_status
383 10305 : void AP_ExternalAHRS_Aeron_plx::publish_nav_para2(const NavPara2Payload &data)
384 : {
385 10305 : last_nav2_ms = AP_HAL::millis();
386 :
387 : {
388 10305 : WITH_SEMAPHORE(arn_sem);
389 10305 : shared.nav2_hw_status = data.hw_status;
390 10305 : }
391 :
392 10305 : WITH_SEMAPHORE(state.sem);
393 20610 : state.quat = {
394 10305 : data.quat[0],
395 10305 : data.quat[1],
396 10305 : data.quat[2],
397 10305 : data.quat[3]
398 : };
399 10305 : state.have_quaternion = true;
400 10305 : }
401 :
402 : // SENS_PARA: gyro/accel/mag/baro
403 10302 : void AP_ExternalAHRS_Aeron_plx::publish_sens_para(const SensParaPayload &data)
404 : {
405 10302 : last_sens_ms = AP_HAL::millis();
406 :
407 : // A sensor the PLX has declared non-functional (manual 7.1.2.9 Table 15:
408 : // "non-functional" or "communication failure") must not have its samples
409 : // republished into ArduPilot's own INS, baro or compass, where they would
410 : // reach EKF3 with no health signal attached. BARO and COMPASS are
411 : // advertised by default, so this is the default path rather than an
412 : // EAHRS_SENSORS opt-in. Dropping the call lets the consuming library time
413 : // out, which is the machine-readable signal a GCS warning alone cannot be.
414 : uint32_t hw;
415 : {
416 10302 : WITH_SEMAPHORE(arn_sem);
417 10302 : hw = shared.nav2_hw_status;
418 10302 : }
419 10302 : const bool imu_ok = (hw & ((1U << uint8_t(AeronHwStatus::ACC)) |
420 : (1U << uint8_t(AeronHwStatus::GYR)))) == 0;
421 10302 : const bool baro_ok = (hw & (1U << uint8_t(AeronHwStatus::BARO))) == 0;
422 10302 : const bool mag_ok = (hw & (1U << uint8_t(AeronHwStatus::MAG))) == 0;
423 :
424 : const Vector3f accel {
425 10302 : data.accel[0],
426 10302 : data.accel[1],
427 10302 : data.accel[2]
428 10302 : };
429 : const Vector3f gyro {
430 10302 : data.gyro[0] * DEG_TO_RAD,
431 10302 : data.gyro[1] * DEG_TO_RAD,
432 10302 : data.gyro[2] * DEG_TO_RAD
433 10302 : };
434 :
435 : {
436 10302 : WITH_SEMAPHORE(state.sem);
437 10302 : state.accel = accel;
438 10302 : state.gyro = gyro;
439 10302 : }
440 :
441 10302 : if (imu_ok) {
442 : const AP_ExternalAHRS::ins_data_message_t ins {
443 : accel,
444 : gyro,
445 10302 : data.temperature
446 10302 : };
447 10302 : AP::ins().handle_external(ins);
448 : }
449 :
450 : #if AP_BARO_EXTERNALAHRS_ENABLED
451 10302 : if (baro_ok) {
452 10302 : const AP_ExternalAHRS::baro_data_message_t baro {
453 : 0, // instance
454 10302 : data.baro_pressure,
455 10302 : data.baro_temperature
456 10302 : };
457 10302 : AP::baro().handle_external(baro);
458 : }
459 : #endif // AP_BARO_EXTERNALAHRS_ENABLED
460 :
461 : #if AP_COMPASS_EXTERNALAHRS_ENABLED
462 10302 : if (mag_ok) {
463 : const AP_ExternalAHRS::mag_data_message_t mag {
464 : Vector3f {
465 10302 : data.mag[0],
466 10302 : data.mag[1],
467 10302 : data.mag[2]
468 : }
469 10302 : };
470 10302 : AP::compass().handle_external(mag);
471 : }
472 : #endif // AP_COMPASS_EXTERNALAHRS_ENABLED
473 :
474 : #if HAL_LOGGING_ENABLED
475 10302 : const uint32_t now_ms = AP_HAL::millis();
476 10302 : const int16_t log_rate = AP::externalAHRS().log_rate.get();
477 10302 : if (log_rate > 0 && now_ms - last_aers_log_ms >= uint32_t(1000U / log_rate)) {
478 1959 : last_aers_log_ms = now_ms;
479 : // @LoggerMessage: AERS
480 : // @Description: Aeron PLX raw sensor data (SENS_PARA)
481 : // @Field: TimeUS: Time since system startup
482 : // @Field: Temp: Sensor temperature
483 : // @Field: GX: Gyro X
484 : // @Field: GY: Gyro Y
485 : // @Field: GZ: Gyro Z
486 : // @Field: AX: Accel X
487 : // @Field: AY: Accel Y
488 : // @Field: AZ: Accel Z
489 : // @Field: MX: Mag X
490 : // @Field: MY: Mag Y
491 : // @Field: MZ: Mag Z
492 : // @Field: Pres: Barometric pressure
493 : // @Field: BTmp: Barometer temperature
494 : // @Field: BAlt: Barometric altitude
495 3918 : AP::logger().WriteStreaming(
496 : "AERS",
497 : "TimeUS," "Temp," "GX," "GY," "GZ," "AX," "AY," "AZ," "MX," "MY," "MZ," "Pres," "BTmp," "BAlt",
498 : "s" "O" "k" "k" "k" "o" "o" "o" "G" "G" "G" "P" "O" "m",
499 : "F" "-" "-" "-" "-" "-" "-" "-" "C" "C" "C" "-" "-" "-",
500 : "Q" "f" "f" "f" "f" "f" "f" "f" "f" "f" "f" "f" "f" "f",
501 : AP_HAL::micros64(),
502 1959 : data.temperature,
503 1959 : data.gyro[0], data.gyro[1], data.gyro[2],
504 1959 : data.accel[0], data.accel[1], data.accel[2],
505 1959 : data.mag[0], data.mag[1], data.mag[2],
506 1959 : data.baro_pressure,
507 1959 : data.baro_temperature,
508 1959 : data.baro_altitude
509 : );
510 : }
511 : #endif // HAL_LOGGING_ENABLED
512 10302 : }
513 :
514 : // GPS_PARA: GNSS status, position, DOP
515 2066 : void AP_ExternalAHRS_Aeron_plx::publish_gps_para(const GpsParaPayload &data, AeronDeferredMsgs &msgs)
516 : {
517 2066 : last_gnss_ms = AP_HAL::millis();
518 :
519 : {
520 2066 : WITH_SEMAPHORE(arn_sem);
521 2066 : shared.gps_epoch_s = data.epoch_time;
522 2066 : shared.gps_microseconds = data.microseconds;
523 2066 : shared.gps_status = data.gps_status;
524 2066 : shared.gps_undulation_m = data.gps_undulation;
525 2066 : shared.gps_hdop = data.hdop;
526 2066 : shared.gps_position[0] = data.gps_position[0];
527 2066 : shared.gps_position[1] = data.gps_position[1];
528 2066 : shared.gps_height_ellip_m = data.gps_height_abv_ellip;
529 2066 : }
530 :
531 : // Push the gps data in ardupilot gps backend.
532 2066 : format_and_push_gps(msgs);
533 2066 : }
534 :
535 : // EXTD_GNSS: accuracy metrics + GNSS NED velocity
536 2066 : void AP_ExternalAHRS_Aeron_plx::publish_extd_gnss(const ExtdGnssPayload &data)
537 : {
538 2066 : WITH_SEMAPHORE(arn_sem);
539 :
540 2066 : shared.cust_hpa_m = data.hpa;
541 2066 : shared.cust_vpa_m = data.vpa;
542 2066 : shared.cust_hva_mps = data.hva;
543 2066 : shared.cust_vdop = data.vdop;
544 2066 : memcpy(shared.cust_gnss_vel_ned_mps, data.gnss_vned, sizeof(data.gnss_vned));
545 :
546 2066 : last_extd_ms = AP_HAL::millis();
547 2066 : }
548 :
549 : /*
550 : EXTD_GNSS freshness. Caller must hold arn_sem.
551 : */
552 11825 : bool AP_ExternalAHRS_Aeron_plx::extd_fresh_locked() const
553 : {
554 23639 : return last_extd_ms != 0 &&
555 23639 : (AP_HAL::millis() - last_extd_ms) <= EXTD_VALIDITY_MS;
556 : }
557 :
558 : /*
559 : Decode the PLX GPS status word into an ArduPilot fix type.
560 :
561 : Bit 10 is the PLX's authoritative "GNSS fix present" flag (manual
562 : 7.1.2.10, Table 16); bits 0-9 are *visible* satellites, which is not by
563 : itself a fix. Both the AP_GPS publication path and get_filter_status()
564 : decode through here so they cannot reach different conclusions from the
565 : same status word.
566 : */
567 11825 : AP_GPS_FixType AP_ExternalAHRS_Aeron_plx::fix_type_from_status(uint32_t status)
568 : {
569 11825 : if ((status & GPS_STATUS_FIX_BIT) == 0 ||
570 11825 : (status & GPS_STATUS_NUMSAT_MASK) == 0) {
571 0 : return AP_GPS_FixType::NONE;
572 : }
573 :
574 11825 : switch (AeronGnssFix((status & GPS_STATUS_FIXTYPE_MASK) >> GPS_STATUS_FIXTYPE_SHIFT)) {
575 11825 : case AeronGnssFix::GNSS_FIX:
576 11825 : return AP_GPS_FixType::FIX_3D;
577 0 : case AeronGnssFix::SBAS_FIX:
578 0 : return AP_GPS_FixType::DGPS;
579 0 : case AeronGnssFix::RTK_FIX:
580 0 : return AP_GPS_FixType::RTK_FIXED;
581 0 : case AeronGnssFix::RTK_FLOAT:
582 0 : return AP_GPS_FixType::RTK_FLOAT;
583 0 : case AeronGnssFix::NO_FIX:
584 : default:
585 : // No fix, or an unrecognised PLX fix code: alive but untrusted.
586 0 : return AP_GPS_FixType::NONE;
587 : }
588 : }
589 :
590 : /*
591 : Build the ArduPilot GPS message from GPS_PARA + EXTD_GNSS fields.
592 : */
593 2066 : void AP_ExternalAHRS_Aeron_plx::format_and_push_gps(AeronDeferredMsgs &msgs)
594 : {
595 : // One coherent snapshot of the cross-thread cache, taken under
596 : // arn_sem and released immediately. Everything below works off the
597 : // local copy, so no shared.* field is read while unlocked.
598 : SharedData snap;
599 : bool extd_stale;
600 : {
601 2066 : WITH_SEMAPHORE(arn_sem);
602 2066 : snap = shared;
603 2066 : extd_stale = !extd_fresh_locked();
604 2066 : }
605 :
606 2066 : const uint32_t st = snap.gps_status;
607 2066 : const uint16_t num_sats = st & GPS_STATUS_NUMSAT_MASK;
608 2066 : const uint8_t jam = (st & 0x00030000U) >> 16;
609 2066 : const uint8_t spf = (st & 0x000C0000U) >> 18;
610 :
611 : // throttled jamming/spoofing warnings - 0.2 Hz
612 2066 : const uint32_t now = AP_HAL::millis();
613 2066 : if (now - last_jam_warn_ms > WARN_THROTTLE_MS) {
614 38 : last_jam_warn_ms = now;
615 38 : if (jam >= uint8_t(AeronJamSpoof::WARNING)) {
616 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Aeron: GNSS jamming level %u", jam);
617 : }
618 38 : if (spf >= uint8_t(AeronJamSpoof::WARNING)) {
619 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Aeron: GNSS spoofing level %u", spf);
620 : }
621 : }
622 :
623 2066 : AP_ExternalAHRS::gps_data_message_t gps_msg{};
624 :
625 : // Convert the PLX POSIX epoch to GPS week / ms-of-week using
626 : // AP_GPS.h own constants. UNIX_OFFSET_MSEC already
627 : // folds in the leap-second offset, so no separate leap term is added
628 : // here - and the leap count tracks AP_GPS.h global value rather
629 : // than a private copy.
630 2066 : const uint64_t epoch_ms = uint64_t(snap.gps_epoch_s) * 1000ULL +
631 2066 : (snap.gps_microseconds / 1000ULL);
632 2066 : if (epoch_ms >= UNIX_OFFSET_MSEC) {
633 2066 : const uint64_t gps_ms = epoch_ms - UNIX_OFFSET_MSEC;
634 2066 : gps_msg.gps_week = uint16_t(gps_ms / AP_MSEC_PER_WEEK);
635 2066 : gps_msg.ms_tow = uint32_t(gps_ms % AP_MSEC_PER_WEEK);
636 : }
637 :
638 : // satellites_in_view is uint8_t; the PLX field is 10-bit.
639 2066 : gps_msg.satellites_in_view = MIN(num_sats, uint16_t(255));
640 :
641 : // We have received a GPS_PARA packet, so the receiver is present; no
642 : // fix, no satellites, or an unrecognised fix code decodes as NONE
643 : // (receiver alive, no position fix).
644 2066 : gps_msg.fix_type = fix_type_from_status(st);
645 :
646 : // accuracy metrics from EXTD_GNSS (fresh - checked above)
647 2066 : gps_msg.horizontal_pos_accuracy = snap.cust_hpa_m;
648 2066 : gps_msg.vertical_pos_accuracy = snap.cust_vpa_m;
649 2066 : gps_msg.horizontal_vel_accuracy = snap.cust_hva_mps;
650 2066 : gps_msg.hdop = snap.gps_hdop * 1.0e2;
651 2066 : gps_msg.vdop = snap.cust_vdop * 1.0e2;
652 :
653 : // position from GPS_PARA, kept in double end-to-end
654 2066 : gps_msg.latitude = snap.gps_position[0] * 1.0e7;
655 2066 : gps_msg.longitude = snap.gps_position[1] * 1.0e7;
656 2066 : gps_msg.msl_altitude = (snap.gps_height_ellip_m - snap.gps_undulation_m) * 1.0e2;
657 :
658 : // GNSS velocity from EXTD_GNSS (fresh)
659 2066 : gps_msg.ned_vel_north = snap.cust_gnss_vel_ned_mps[0];
660 2066 : gps_msg.ned_vel_east = snap.cust_gnss_vel_ned_mps[1];
661 2066 : gps_msg.ned_vel_down = snap.cust_gnss_vel_ned_mps[2];
662 :
663 : // Set origin on the first 3D fix. The PLX only declares a 3D fix
664 : // after its own internal quality checks, so no extra gating here.
665 2066 : if (gps_msg.fix_type >= AP_GPS_FixType::FIX_3D) {
666 2066 : WITH_SEMAPHORE(state.sem);
667 2066 : if (!state.have_origin) {
668 22 : state.origin = Location {
669 11 : int32_t(snap.gps_position[0] * 1.0e7),
670 11 : int32_t(snap.gps_position[1] * 1.0e7),
671 11 : int32_t((snap.gps_height_ellip_m - snap.gps_undulation_m) * 1.0e2),
672 : Location::AltFrame::ABSOLUTE
673 : };
674 11 : state.have_origin = true;
675 : }
676 2066 : }
677 :
678 : // If EXTD_GNSS is stale we have no fresh accuracy or GNSS velocity to
679 : // attach. Rather than publish a GPS message with fabricated accuracy
680 : // and zeroed velocity - which raw-data consumers (DCM, vehicle code)
681 : // would read as real - we skip publication entirely and let AP_GPS
682 : // time out naturally, which every consumer handles. The
683 : // fused INS solution still reaches ArduPilot via state.* (NAV_PARA1).
684 : // This will happen in very rare conditions, where device itself is compromised,
685 : // as it's hard configured to send these data.
686 2066 : if (extd_stale) {
687 : // Shout on telemetry that rare condition has occurred.
688 11 : msgs.stale_data = true;
689 11 : return;
690 : }
691 :
692 : uint8_t instance;
693 2055 : if (AP::gps().get_first_external_instance(instance)) {
694 1700 : AP::gps().handle_external(gps_msg, instance);
695 : }
696 :
697 : #if HAL_LOGGING_ENABLED
698 : // AERG follows the GNSS rate (5-10 Hz) so it is logged per-packet.
699 : // @LoggerMessage: AERG
700 : // @Description: Aeron PLX GNSS status and accuracy (GPS_PARA + EXTD_GNSS)
701 : // @Field: TimeUS: Time since system startup
702 : // @Field: GMS: Time of week
703 : // @Field: GWk: GPS week
704 : // @Field: NSat: Satellites in view
705 : // @Field: FixT: GPS fix type as reported to AP_GPS
706 : // @Field: Jam: Jamming detection level (1=none,2=warn,3=critical)
707 : // @Field: Spf: Spoofing detection level (1=none,2=warn,3=critical)
708 : // @Field: HPA: Horizontal position accuracy
709 : // @Field: VPA: Vertical position accuracy
710 : // @Field: HVA: Horizontal velocity accuracy
711 : // @Field: HDOP: Horizontal dilution of precision
712 : // @Field: VDOP: Vertical dilution of precision
713 4110 : AP::logger().WriteStreaming(
714 : "AERG",
715 : "TimeUS," "GMS," "GWk," "NSat," "FixT," "Jam," "Spf," "HPA," "VPA," "HVA," "HDOP," "VDOP",
716 : "s" "-" "-" "-" "-" "-" "-" "-" "-" "-" "-" "-",
717 : "F" "-" "-" "-" "-" "-" "-" "-" "-" "-" "-" "-",
718 : "Q" "I" "H" "H" "B" "B" "B" "f" "f" "f" "f" "f",
719 : AP_HAL::micros64(),
720 : gps_msg.ms_tow,
721 2055 : gps_msg.gps_week,
722 : num_sats,
723 2055 : uint8_t(gps_msg.fix_type),
724 : jam,
725 : spf,
726 2055 : snap.cust_hpa_m,
727 2055 : snap.cust_vpa_m,
728 2055 : snap.cust_hva_mps,
729 2055 : snap.gps_hdop,
730 2055 : snap.cust_vdop
731 : );
732 : #endif // HAL_LOGGING_ENABLED
733 : }
734 :
735 30173 : void AP_ExternalAHRS_Aeron_plx::send_deferred_messages(const AeronDeferredMsgs &msgs)
736 : {
737 30173 : const uint32_t now = AP_HAL::millis();
738 :
739 30173 : if (msgs.crc_fail && (now - last_crc_warn_ms) >= WARN_THROTTLE_MS) {
740 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Aeron: CRC mismatch");
741 0 : last_crc_warn_ms = now;
742 : }
743 :
744 : // Malformed-packet warnings share one throttle. A recognised ID with
745 : // a short payload is reported distinctly from a truly unknown ID, so
746 : // the latter isn't misreported for the former.
747 30173 : if ((msgs.bad_length || msgs.unknown_pkt) &&
748 0 : (now - last_garbage_warn_ms) >= WARN_THROTTLE_MS) {
749 0 : if (msgs.unknown_pkt) {
750 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Aeron: Unknown packet 0x%04X", msgs.unknown_id);
751 : } else {
752 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Aeron: Bad packet length");
753 : }
754 0 : last_garbage_warn_ms = now;
755 : }
756 :
757 30173 : if (msgs.stale_data && (now - last_stale_data_ms) >= WARN_THROTTLE_MS) {
758 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Aeron: EXTD_GNSS Stale data, check packet!");
759 0 : last_stale_data_ms = now;
760 : }
761 :
762 30173 : if (msgs.ext_mag_checked) {
763 : uint8_t present;
764 : {
765 11 : WITH_SEMAPHORE(arn_sem);
766 11 : present = shared.ext_mag_present;
767 11 : }
768 11 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Aeron: Ext mag present=%u", present);
769 : }
770 30173 : if (msgs.ext_asp_checked) {
771 : uint8_t present;
772 : {
773 11 : WITH_SEMAPHORE(arn_sem);
774 11 : present = shared.ext_asp_present;
775 11 : }
776 11 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Aeron: Ext airspeed present=%u", present);
777 : }
778 30173 : }
779 :
780 : // Fire the external-sensor presence queries once NAV data confirms the
781 : // PLX is up and producing a fused solution. If NAV data lapses (a device
782 : // reset or link dropout) and then resumes, re-query so the presence flags
783 : // reflect the re-initialised device rather than the pre-reset state.
784 30173 : void AP_ExternalAHRS_Aeron_plx::check_for_query_ext_sensors()
785 : {
786 : // Either NAV packet proves the device is producing a solution; use
787 : // whichever arrived most recently as the "device up" timestamp.
788 30173 : const uint32_t nav_ms = MAX(last_nav1_ms, last_nav2_ms);
789 :
790 : // nav_ms == 0 means either no NAV yet, or the timestamp landed exactly
791 : // on the millis() rollover (every ~50 days). Both are benign: we
792 : // just skip this cycle and resume on the next NAV packet (~20 ms
793 : // later). The only cost is at most one deferred re-query check.
794 30173 : if (nav_ms == 0) {
795 0 : return;
796 : }
797 :
798 : // A jump in the NAV timestamp larger than the reset threshold means
799 : // NAV stopped and later resumed - treat as a fresh boot and re-query.
800 : // (A reset landing exactly on the 50-day millis() wrap could be
801 : // missed here, but that coincidence is very unlikely and
802 : // the only consequence is not re-querying optional-sensor presence.)
803 : // Presence information gathered before the reset describes the pre-reset
804 : // device, so drop it rather than carry it across. report_hw_status() then
805 : // keeps the EXT_* flags unreportable until the re-query is answered.
806 30173 : if (prev_nav_ms != 0 && (nav_ms - prev_nav_ms) > DEVICE_RESET_GAP_MS) {
807 0 : WITH_SEMAPHORE(arn_sem);
808 0 : shared.ext_mag_present = 0;
809 0 : shared.ext_asp_present = 0;
810 0 : shared.ext_mag_checked = false;
811 0 : shared.ext_asp_checked = false;
812 0 : ext_query_tries = 0;
813 0 : last_ext_query_ms = 0;
814 0 : }
815 30173 : prev_nav_ms = nav_ms;
816 :
817 : bool mag_checked;
818 : bool asp_checked;
819 : {
820 30173 : WITH_SEMAPHORE(arn_sem);
821 30173 : mag_checked = shared.ext_mag_checked;
822 30173 : asp_checked = shared.ext_asp_checked;
823 30173 : }
824 :
825 : // Nothing outstanding, or we have exhausted the retry budget.
826 30173 : if ((mag_checked && asp_checked) || ext_query_tries >= EXT_QUERY_MAX_TRIES) {
827 30162 : return;
828 : }
829 :
830 11 : const uint32_t now = AP_HAL::millis();
831 11 : if (last_ext_query_ms != 0 && (now - last_ext_query_ms) < EXT_QUERY_RETRY_MS) {
832 0 : return;
833 : }
834 :
835 : // Announce once - the retries are silent so a device that never answers
836 : // does not spam the GCS.
837 11 : if (ext_query_tries == 0) {
838 11 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Aeron: External sensor query");
839 : }
840 :
841 : // Counted even when the write is short, so a persistently backed-up port
842 : // cannot retry forever.
843 11 : last_ext_query_ms = now;
844 11 : ext_query_tries++;
845 11 : if (!query_ext_sensors(!mag_checked, !asp_checked)) {
846 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Aeron: Ext sensor query write failed");
847 : }
848 : }
849 :
850 11 : bool AP_ExternalAHRS_Aeron_plx::query_ext_sensors(bool want_mag, bool want_asp)
851 : {
852 : // External sensor presence queries - fixed packets sent once at startup
853 : static const uint8_t cmd_query_ext_mag[] = {
854 : 0x05, 0x05, 0x05, 0x05, 0x00, 0x0A, 0x00, 0xA1, 0x01, 0x17, 0xBD
855 : };
856 :
857 : static const uint8_t cmd_query_ext_asp[] = {
858 : 0x05, 0x05, 0x05, 0x05, 0x00, 0x0A, 0x00, 0xA1, 0x03, 0x62, 0xCA
859 : };
860 :
861 : // Only the outstanding queries are re-sent, so a sensor that has already
862 : // answered is not asked again.
863 11 : bool all_sent = true;
864 :
865 22 : if (want_mag &&
866 11 : uart->write(cmd_query_ext_mag, sizeof(cmd_query_ext_mag)) != sizeof(cmd_query_ext_mag)) {
867 0 : all_sent = false;
868 : }
869 22 : if (want_asp &&
870 11 : uart->write(cmd_query_ext_asp, sizeof(cmd_query_ext_asp)) != sizeof(cmd_query_ext_asp)) {
871 0 : all_sent = false;
872 : }
873 :
874 11 : return all_sent;
875 : }
876 :
877 0 : const char *AP_ExternalAHRS_Aeron_plx::get_name() const
878 : {
879 0 : return "Aeron-PLX3";
880 : }
881 :
882 33 : int8_t AP_ExternalAHRS_Aeron_plx::get_port() const
883 : {
884 33 : return uart ? port_num : -1;
885 : }
886 :
887 35894 : bool AP_ExternalAHRS_Aeron_plx::initialised() const
888 : {
889 35894 : if (!setup_complete) {
890 0 : return false;
891 : }
892 : // EXTD_GNSS is required, not optional: without it format_and_push_gps()
893 : // publishes no GPS message at all, so reporting initialised before the
894 : // first one arrives would claim readiness while AP_GPS has never had a
895 : // single sample.
896 : uint32_t extd_ms;
897 : {
898 35894 : WITH_SEMAPHORE(arn_sem);
899 35894 : extd_ms = last_extd_ms;
900 35894 : }
901 :
902 35894 : return last_sens_ms != 0
903 35894 : && last_nav1_ms != 0
904 35894 : && last_nav2_ms != 0
905 35894 : && last_gnss_ms != 0
906 71788 : && extd_ms != 0;
907 : }
908 :
909 : // hw_status reporting table. Order is not significant - report_hw_status
910 : // walks every row to compute both the effective error mask and the
911 : // fault / recovery messages.
912 : const AP_ExternalAHRS_Aeron_plx::HwStatusEntry
913 : AP_ExternalAHRS_Aeron_plx::hw_status_table[] = {
914 : { AeronHwStatus::GNSS, MAV_SEVERITY_NOTICE, "GNSS unhealthy", "GNSS okay" },
915 : { AeronHwStatus::ACC, MAV_SEVERITY_ERROR, "Accelerometer unhealthy", "Accel okay" },
916 : { AeronHwStatus::GYR, MAV_SEVERITY_ERROR, "Gyroscope unhealthy", "Gyro okay" },
917 : { AeronHwStatus::MAG, MAV_SEVERITY_ERROR, "Magnetometer unhealthy", "Mag okay" },
918 : { AeronHwStatus::BARO, MAV_SEVERITY_ERROR, "Barometer unhealthy", "Baro okay" },
919 : { AeronHwStatus::SUP_VLTG, MAV_SEVERITY_WARNING, "Supply Voltage Error", "Supply voltage okay" },
920 : { AeronHwStatus::CM_PRT, MAV_SEVERITY_WARNING, "Comport Overrun", "Comport okay" },
921 : { AeronHwStatus::RAM, MAV_SEVERITY_EMERGENCY, "RAM Failure", nullptr },
922 : { AeronHwStatus::FIRMWARE, MAV_SEVERITY_EMERGENCY, "Firmware Failure", nullptr },
923 : { AeronHwStatus::CONFIG, MAV_SEVERITY_EMERGENCY, "Config Memory Failure", nullptr },
924 : { AeronHwStatus::EXT_MAG, MAV_SEVERITY_WARNING, "Ext Mag not connected", "Ext Mag connected" },
925 : { AeronHwStatus::EXT_ASP, MAV_SEVERITY_WARNING, "Ext Airspeed not connected", "Ext Airspeed connected" },
926 : };
927 :
928 : /*
929 : Hardware-status reporting. Emits GCS warnings for any error flag in
930 : NAV_PARA2.hw_status, with rising-edge + per-flag-rate-limited
931 : semantics.
932 :
933 : Reporting strategy:
934 : - Rising edge (flag 0 -> 1): report immediately, once.
935 : - Persistent fault (flag stays 1): repeat every HEALTH_REPEAT_INTERVAL_MS.
936 : - Falling edge (flag 1 -> 0): log "okay" (if a clear_msg is set)
937 : and stop reporting.
938 :
939 : Most hw_status flags do NOT fail healthy() - the PLX continues
940 : navigation with degraded sensor inputs, so this routine only emits
941 : informational/warning GCS messages for them. There are two exceptions:
942 : the flags in HW_STATUS_FATAL_MASK (RAM, firmware, config memory, accel,
943 : gyro), which healthy() rejects outright, and MAG/BARO, which do not fail
944 : health but do stop their raw samples being forwarded to ArduPilot's own
945 : estimators (see publish_sens_para).
946 : */
947 43967 : void AP_ExternalAHRS_Aeron_plx::report_hw_status()
948 : {
949 : // last_warned_ms is sized HW_STATUS_COUNT and indexed by table row, so
950 : // the two must stay in lockstep. Adding a table entry without bumping
951 : // the constant (or vice versa) trips this at compile time.
952 : static_assert(ARRAY_SIZE(hw_status_table) == HW_STATUS_COUNT,
953 : "HW_STATUS_COUNT must equal the number of hw_status_table rows");
954 :
955 43967 : const uint32_t now = AP_HAL::millis();
956 :
957 : // Snapshot the shared status fields under the lock so a publish
958 : // from the same thread (publish_nav_para2) can't tear us. Same
959 : // thread, but the lock is conventional protection that documents
960 : // the contract.
961 : uint32_t hw;
962 : uint8_t ext_mag_present;
963 : uint8_t ext_asp_present;
964 : bool ext_mag_checked;
965 : bool ext_asp_checked;
966 : {
967 43967 : WITH_SEMAPHORE(arn_sem);
968 43967 : hw = shared.nav2_hw_status;
969 43967 : ext_mag_present = shared.ext_mag_present;
970 43967 : ext_asp_present = shared.ext_asp_present;
971 43967 : ext_mag_checked = shared.ext_mag_checked;
972 43967 : ext_asp_checked = shared.ext_asp_checked;
973 43967 : }
974 :
975 : // Build the effective error mask from the table, then drop the
976 : // EXT_* flags unless the user actually has those sensors configured.
977 43967 : uint32_t effective_mask = 0;
978 571571 : for (const auto &entry : hw_status_table) {
979 527604 : effective_mask |= 1U << uint8_t(entry.status);
980 : }
981 43967 : if (!(ext_mag_checked && ext_mag_present)) {
982 31 : effective_mask &= ~(1U << uint8_t(AeronHwStatus::EXT_MAG));
983 : }
984 43967 : if (!(ext_asp_checked && ext_asp_present)) {
985 31 : effective_mask &= ~(1U << uint8_t(AeronHwStatus::EXT_ASP));
986 : }
987 :
988 43967 : const uint32_t errors = hw & effective_mask;
989 43967 : const uint32_t newly_set = errors & ~last_warned_stat;
990 43967 : const uint32_t newly_clear = last_warned_stat & ~errors;
991 :
992 571571 : for (uint8_t row = 0; row < ARRAY_SIZE(hw_status_table); row++) {
993 527604 : const HwStatusEntry &entry = hw_status_table[row];
994 527604 : const uint32_t mask = 1U << uint8_t(entry.status);
995 :
996 : // Recovery message on falling edge.
997 527604 : if ((newly_clear & mask) != 0U && entry.clear_msg != nullptr) {
998 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Aeron: %s", entry.clear_msg);
999 : }
1000 :
1001 527604 : if ((errors & mask) == 0U) {
1002 527604 : continue;
1003 : }
1004 :
1005 : // Currently in fault: emit on rising edge or after the repeat
1006 : // interval has elapsed. last_warned_ms is indexed by table row so
1007 : // its size tracks the number of entries, not the enum bit range.
1008 0 : const bool rising = (newly_set & mask) != 0U;
1009 0 : const bool overdue = (now - last_warned_ms[row]) >= HEALTH_REPEAT_INTERVAL_MS;
1010 0 : if (!rising && !overdue) {
1011 0 : continue;
1012 : }
1013 :
1014 0 : GCS_SEND_TEXT(MAV_SEVERITY(entry.severity), "Aeron: %s", entry.fault_msg);
1015 0 : last_warned_ms[row] = now;
1016 : }
1017 :
1018 43967 : last_warned_stat = errors;
1019 43967 : }
1020 :
1021 : /*
1022 : Health check. Pure stale-timestamp test.
1023 : */
1024 17972 : bool AP_ExternalAHRS_Aeron_plx::healthy() const
1025 : {
1026 17972 : if (!initialised()) {
1027 0 : return false;
1028 : }
1029 :
1030 : // A fault in HW_STATUS_FATAL_MASK means the PLX's own solution is
1031 : // untrustworthy, so it fails health rather than only raising a GCS
1032 : // warning. See the mask definition for which flags qualify and why.
1033 : // The remaining flags stay informational because the PLX continues
1034 : // navigating with degraded sensor inputs.
1035 : uint32_t hw;
1036 : {
1037 17972 : WITH_SEMAPHORE(arn_sem);
1038 17972 : hw = shared.nav2_hw_status;
1039 17972 : }
1040 17972 : if ((hw & HW_STATUS_FATAL_MASK) != 0) {
1041 0 : return false;
1042 : }
1043 :
1044 17972 : const uint32_t now = AP_HAL::millis();
1045 : // Both NAV streams are checked independently: NAV_PARA1 carries
1046 : // position/velocity/attitude and NAV_PARA2 the quaternion + hw_status.
1047 : // Losing either must trip unhealthy, otherwise stale data from the
1048 : // lost stream would keep publishing at arbitrary age.
1049 17972 : return (now - last_sens_ms) < SENS_TIMEOUT_MS
1050 17972 : && (now - last_nav1_ms) < NAV_TIMEOUT_MS
1051 17972 : && (now - last_nav2_ms) < NAV_TIMEOUT_MS
1052 35944 : && (now - last_gnss_ms) < GNSS_TIMEOUT_MS;
1053 : }
1054 :
1055 : /*
1056 : Pre-arm check.
1057 :
1058 : GNSS-related warnings (no lock, jamming, spoofing) are surfaced via
1059 : throttled GCS_SEND_TEXT in format_and_push_gps(), because the PLX
1060 : can navigate in GNSS-denied mode and we want those warnings to be
1061 : informational rather than blocking.
1062 : */
1063 50 : bool AP_ExternalAHRS_Aeron_plx::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
1064 : {
1065 50 : if (!setup_complete) {
1066 0 : hal.util->snprintf(failure_msg, failure_msg_len, "Aeron Not Initialised");
1067 0 : return false;
1068 : }
1069 50 : if (!healthy()) {
1070 0 : hal.util->snprintf(failure_msg, failure_msg_len, "Aeron Unhealthy");
1071 0 : return false;
1072 : }
1073 50 : return true;
1074 : }
1075 :
1076 : /*
1077 : Filter status.
1078 :
1079 : Reaching the body below means healthy() returned true, which already
1080 : guarantees NAV_PARA1, NAV_PARA2, SENS and GNSS are all within their
1081 : staleness deadlines. So the PLX's fused INS solution is currently
1082 : valid, and every estimate it continuously produces - attitude, 3D
1083 : velocity, and relative position (by inertial integration and aidings) - is
1084 : available. Those flags are therefore set whenever healthy, not
1085 : unconditionally.
1086 :
1087 : horiz_pos_abs / pred_horiz_pos_abs are the fused absolute position,
1088 : which the INS holds through short GNSS outages by dead reckoning, so
1089 : they follow the (healthy) NAV solution rather than the GNSS fix.
1090 : using_gps / gps_quality_good track the raw GNSS fix bit specifically.
1091 : */
1092 9759 : void AP_ExternalAHRS_Aeron_plx::get_filter_status(nav_filter_status &status) const
1093 : {
1094 9759 : memset(&status, 0, sizeof(status));
1095 :
1096 9759 : if (!initialised()) {
1097 0 : return;
1098 : }
1099 :
1100 9759 : status.flags.initalized = true;
1101 :
1102 9759 : if (!healthy()) {
1103 0 : return;
1104 : }
1105 :
1106 : bool fix;
1107 : {
1108 9759 : WITH_SEMAPHORE(arn_sem);
1109 : // EXTD_GNSS gone stale means format_and_push_gps() publishes nothing,
1110 : // so ArduPilot is not receiving GPS regardless of what the PLX's own
1111 : // status word says. The status word itself is decoded by the same
1112 : // helper the AP_GPS path uses, so the two cannot disagree.
1113 19518 : fix = extd_fresh_locked() &&
1114 9759 : fix_type_from_status(shared.gps_status) >= AP_GPS_FixType::FIX_3D;
1115 9759 : }
1116 :
1117 9759 : status.flags.attitude = true;
1118 9759 : status.flags.horiz_vel = true;
1119 9759 : status.flags.vert_vel = true;
1120 9759 : status.flags.horiz_pos_rel = true;
1121 9759 : status.flags.vert_pos = true;
1122 9759 : status.flags.pred_horiz_pos_rel = true;
1123 9759 : status.flags.using_gps = fix;
1124 9759 : status.flags.gps_quality_good = fix;
1125 :
1126 : // healthy() guarantees NAV is fresh, so the fused absolute position
1127 : // is available now (held through GNSS outages by dead reckoning).
1128 9759 : status.flags.horiz_pos_abs = true;
1129 9759 : status.flags.pred_horiz_pos_abs = true;
1130 : }
1131 :
1132 : /*
1133 : Variance estimates from EXTD_GNSS accuracy metrics. Returns false
1134 : until the first EXTD_GNSS packet arrives so ArduPilot does not
1135 : over-trust the solution during initialisation when accuracy fields
1136 : are still zero.
1137 :
1138 : The PLX3-N INS does not expose EKF innovation variances, returning nothing
1139 : leaves the report unpopulated. We therefore map the PLX3's own GNSS accuracy
1140 : estimates onto variance fields as a best effort health signal.
1141 :
1142 : These are accuracy-derived values, not filter innovations.
1143 : */
1144 9759 : bool AP_ExternalAHRS_Aeron_plx::get_variances(float &velVar, float &posVar,
1145 : float &hgtVar, Vector3f &magVar,
1146 : float &tasVar) const
1147 : {
1148 : float hpa;
1149 : float vpa;
1150 : float hva;
1151 : {
1152 9759 : WITH_SEMAPHORE(arn_sem);
1153 :
1154 : // Report only while EXTD_GNSS is fresh: before the first packet
1155 : // (last_extd_ms == 0) or once it has gone stale, the accuracy
1156 : // values are meaningless and we return false rather than feed the
1157 : // EKF a stale variance.
1158 19518 : if (last_extd_ms == 0 ||
1159 9759 : (AP_HAL::millis() - last_extd_ms) > EXTD_VALIDITY_MS) {
1160 0 : return false;
1161 : }
1162 :
1163 9759 : hpa = shared.cust_hpa_m;
1164 9759 : vpa = shared.cust_vpa_m;
1165 9759 : hva = shared.cust_hva_mps;
1166 9759 : }
1167 :
1168 9759 : velVar = hva * vel_gate_scale;
1169 9759 : posVar = hpa * pos_gate_scale;
1170 9759 : hgtVar = vpa * hgt_gate_scale;
1171 9759 : magVar.zero();
1172 9759 : tasVar = 0;
1173 9759 : return true;
1174 : }
1175 :
1176 : #endif // AP_EXTERNAL_AHRS_AERON_PLX_ENABLED
|
