Line data Source code
1 : #pragma once
2 :
3 : /*
4 : The DAL (Data Abstraction Layer) acts as an intermediary between ArduPilot's AHRS state estimators and its data sources. Currently the only in-tree estimators using the DAL are the EKF2 and EKF3 estimators.
5 :
6 : When LOG_REPLAY is set to 1 all data entering the DAL is logged into ArduPilot's onboard ("dataflash") log. LOG_DISARMED should be set when LOG_REPLAY is set. Data is recorded in "frames" corresponding to a run of the EKF update functions.
7 :
8 : The logs produced in this manner can be "replayed" via a "replay" tool. Data frames are read from the log and the estimator's update methods called to run an update cycle based on that data.
9 :
10 : In this way estimator problems can be reproduced from logs from real vehicles.
11 :
12 : Replay also allows for adjustment of the EKF's tunables to produce better estimates for given logs (and hopefully the vehicle the logs were produced from.
13 :
14 : See also https://ardupilot.club/dev/docs/testing-with-replay.html
15 : */
16 :
17 : #include "AP_DAL_InertialSensor.h"
18 : #include "AP_DAL_Baro.h"
19 : #include "AP_DAL_GPS.h"
20 : #include "AP_DAL_RangeFinder.h"
21 : #include "AP_DAL_Compass.h"
22 : #include "AP_DAL_Airspeed.h"
23 : #include "AP_DAL_Beacon.h"
24 : #include "AP_DAL_VisualOdom.h"
25 :
26 : #include "LogStructure.h"
27 :
28 : #include <stdio.h>
29 : #include <stdint.h>
30 : #include <cstddef>
31 :
32 :
33 : #define DAL_CORE(c) AP::dal().logging_core(c)
34 :
35 : class NavEKF2;
36 : class NavEKF3;
37 :
38 : class AP_DAL {
39 : public:
40 :
41 : enum class FrameType : uint8_t {
42 : InitialiseFilterEKF2 = 1U<<0,
43 : UpdateFilterEKF2 = 1U<<1,
44 : InitialiseFilterEKF3 = 1<<2,
45 : UpdateFilterEKF3 = 1<<3,
46 : LogWriteEKF2 = 1<<4,
47 : LogWriteEKF3 = 1<<5,
48 : };
49 :
50 : enum class Event : uint8_t {
51 : resetGyroBias = 0,
52 : resetHeightDatum = 1,
53 : //setInhibitGPS = 2, // removed
54 : //setTakeoffExpected = 3, // removed
55 : //unsetTakeoffExpected = 4, // removed
56 : //setTouchdownExpected = 5, // removed
57 : //unsetTouchdownExpected = 6, // removed
58 : //setInhibitGpsVertVelUse = 7, // removed
59 : //unsetInhibitGpsVertVelUse = 8, // removed
60 : setTerrainHgtStable = 9,
61 : unsetTerrainHgtStable = 10,
62 : requestYawReset = 11,
63 : checkLaneSwitch = 12,
64 : setSourceSet0 = 13,
65 : setSourceSet1 = 14,
66 : setSourceSet2 = 15,
67 : };
68 :
69 : // must remain the same as AP_AHRS_VehicleClass numbers-wise
70 : enum class VehicleClass : uint8_t {
71 : UNKNOWN,
72 : GROUND,
73 : COPTER,
74 : FIXED_WING,
75 : SUBMARINE,
76 : };
77 :
78 2193 : AP_DAL() {}
79 :
80 265897692 : static AP_DAL *get_singleton() {
81 265897692 : if (!_singleton) {
82 2193 : _singleton = NEW_NOTHROW AP_DAL();
83 : }
84 265897692 : return _singleton;
85 : }
86 :
87 : void start_frame(FrameType frametype);
88 : void end_frame(void);
89 46415382 : uint64_t micros64() const { return _RFRH.time_us; }
90 : uint32_t micros() const { return _micros; }
91 92044889 : uint32_t millis() const { return _millis; }
92 :
93 : void log_event2(Event event);
94 : void log_SetOriginLLH2(const Location &loc);
95 : void log_writeDefaultAirSpeed2(const float aspeed, const float uncertainty);
96 :
97 : void log_event3(Event event);
98 : void log_SetOriginLLH3(const Location &loc);
99 : void log_SetLatLng(const Location &loc, float posAccuracy, uint32_t timestamp_ms);
100 :
101 : void log_writeDefaultAirSpeed3(const float aspeed, const float uncertainty);
102 : void log_writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t timeStamp_ms, uint8_t type);
103 :
104 : enum class RFRNFlags {
105 : ARMED = (1U<<0),
106 : UNUSED = (1U<<1),
107 : FLY_FORWARD = (1U<<2),
108 : AHRS_AIRSPEED_SENSOR_ENABLED = (1U<<3),
109 : OPTICALFLOW_ENABLED = (1U<<4),
110 : WHEELENCODER_ENABLED = (1U<<5),
111 : TAKEOFF_EXPECTED = (1U<<6),
112 : TOUCHDOWN_EXPECTED = (1U<<7),
113 : };
114 :
115 : // EKF ID for timing checks
116 : enum class EKFType : uint8_t {
117 : EKF2 = 0,
118 : EKF3 = 1,
119 : };
120 :
121 : // check if we are low on CPU for this core
122 : bool ekf_low_time_remaining(EKFType etype, uint8_t core);
123 :
124 : // returns armed state for the current frame
125 200873827 : bool get_armed() const { return _RFRN.armed; }
126 :
127 : // memory available at start of current frame. While this could
128 : // potentially change as we go through the frame, the
129 : // ramifications of being out of memory are that you don't start
130 : // the EKF, so the simplicity of having one value for the entire
131 : // frame is worthwhile.
132 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
133 7482 : uint32_t available_memory() const { return _RFRN.available_memory + 10240; }
134 : #else
135 0 : uint32_t available_memory() const { return _RFRN.available_memory; }
136 : #endif
137 :
138 38945869 : int8_t configured_ekf_type(void) const {
139 38945869 : return _RFRN.ekf_type;
140 : }
141 :
142 : int snprintf(char* str, size_t size, const char *format, ...) const;
143 :
144 : // copied in AP_HAL/Util.h
145 : enum class MemoryType : uint8_t {
146 : DMA_SAFE = 0,
147 : FAST = 1,
148 : };
149 : void *malloc_type(size_t size, MemoryType mem_type) const;
150 : void free_type(void *ptr, size_t size, MemoryType memtype) const;
151 :
152 379652469 : AP_DAL_InertialSensor &ins() { return _ins; }
153 102726639 : AP_DAL_Baro &baro() { return _baro; }
154 145962459 : AP_DAL_GPS &gps() { return _gps; }
155 :
156 : #if AP_RANGEFINDER_ENABLED
157 44136360 : AP_DAL_RangeFinder *rangefinder() {
158 44136360 : return _rangefinder;
159 : }
160 : #endif
161 :
162 107475394 : AP_DAL_Airspeed *airspeed() {
163 107475394 : return _airspeed;
164 : }
165 : #if AP_BEACON_ENABLED
166 23220913 : AP_DAL_Beacon *beacon() {
167 23220913 : return _beacon;
168 : }
169 : #endif
170 : #if HAL_VISUALODOM_ENABLED
171 39328608 : AP_DAL_VisualOdom *visualodom() {
172 39328608 : return _visualodom;
173 : }
174 : #endif
175 :
176 454732030 : AP_DAL_Compass &compass() { return _compass; }
177 :
178 : // random methods that AP_NavEKF3 wants to call on AHRS:
179 29152712 : bool airspeed_sensor_enabled(void) const {
180 29152712 : return _RFRN.ahrs_airspeed_sensor_enabled;
181 : }
182 :
183 : // get apparent to true airspeed ratio; this is a sensor input
184 : // to the EKFs, captured from the barometer's atmosphere model
185 : // in start_frame():
186 31333561 : float get_EAS2TAS(void) const {
187 31333561 : return _RFRN.EAS2TAS;
188 : }
189 :
190 51851028 : VehicleClass get_vehicle_class(void) const {
191 51851028 : return (VehicleClass)_RFRN.vehicle_class;
192 : }
193 :
194 302870815 : bool get_fly_forward(void) const {
195 302870815 : return _RFRN.fly_forward;
196 : }
197 :
198 91771621 : bool get_takeoff_expected(void) const {
199 91771621 : return _RFRN.takeoff_expected;
200 : }
201 :
202 29953912 : bool get_touchdown_expected(void) const {
203 29953912 : return _RFRN.touchdown_expected;
204 : }
205 :
206 : // for EKF usage to enable takeoff expected to true
207 : void set_takeoff_expected();
208 :
209 : // get ahrs trim
210 48579808 : const Vector3f &get_trim() const {
211 48579808 : return _RFRN.ahrs_trim;
212 : }
213 :
214 42337748 : const Matrix3f &get_rotation_vehicle_body_to_autopilot_body(void) const {
215 42337748 : return _rotation_vehicle_body_to_autopilot_body;
216 : }
217 :
218 : // get the home location. This is const to prevent any changes to
219 : // home without telling AHRS about the change
220 3702 : const class Location &get_home(void) const {
221 3702 : return _home;
222 : }
223 :
224 47215099 : uint32_t get_time_flying_ms(void) const {
225 47215099 : return _RFRH.time_flying_ms;
226 : }
227 :
228 3996 : bool opticalflow_enabled(void) const {
229 3996 : return _RFRN.opticalflow_enabled;
230 : }
231 :
232 0 : bool wheelencoder_enabled(void) const {
233 0 : return _RFRN.wheelencoder_enabled;
234 : }
235 :
236 : // log optical flow data
237 : void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride);
238 :
239 : // log external nav data
240 : void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms);
241 : void writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms);
242 :
243 : // log wheel odometry data
244 : void writeWheelOdom(float delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset, float radius);
245 : void writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset);
246 :
247 : // Write terrain altitude (derived from SRTM) in meters above the origin
248 : void writeTerrainData(float alt_m);
249 :
250 : // Replay support:
251 : void handle_message(const log_RFRH &msg) {
252 : _RFRH = msg;
253 : _micros = _RFRH.time_us;
254 : _millis = _RFRH.time_us / 1000UL;
255 : }
256 : void handle_message(const log_RFRN &msg) {
257 : _RFRN = msg;
258 : _home = {
259 : msg.lat,
260 : msg.lng,
261 : msg.alt,
262 : Location::AltFrame::ABSOLUTE
263 : };
264 : }
265 : void handle_message(const log_RFRF &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
266 :
267 : void handle_message(const log_RISH &msg) {
268 : _ins.handle_message(msg);
269 : }
270 : void handle_message(const log_RISI &msg) {
271 : _ins.handle_message(msg);
272 : }
273 : void handle_message(const log_RISJ &msg) {
274 : _ins.handle_message(msg);
275 : }
276 :
277 : void handle_message(const log_RASH &msg) {
278 : if (_airspeed == nullptr) {
279 : _airspeed = NEW_NOTHROW AP_DAL_Airspeed;
280 : }
281 : _airspeed->handle_message(msg);
282 : }
283 : void handle_message(const log_RASI &msg) {
284 : if (_airspeed == nullptr) {
285 : _airspeed = NEW_NOTHROW AP_DAL_Airspeed;
286 : }
287 : _airspeed->handle_message(msg);
288 : }
289 :
290 : void handle_message(const log_RBRH &msg) {
291 : _baro.handle_message(msg);
292 : }
293 : void handle_message(const log_RBRI &msg) {
294 : _baro.handle_message(msg);
295 : }
296 :
297 : void handle_message(const log_RRNH &msg) {
298 : #if AP_RANGEFINDER_ENABLED
299 : if (_rangefinder == nullptr) {
300 : _rangefinder = NEW_NOTHROW AP_DAL_RangeFinder;
301 : }
302 : _rangefinder->handle_message(msg);
303 : #endif
304 : }
305 : void handle_message(const log_RRNI &msg) {
306 : #if AP_RANGEFINDER_ENABLED
307 : if (_rangefinder == nullptr) {
308 : _rangefinder = NEW_NOTHROW AP_DAL_RangeFinder;
309 : }
310 : _rangefinder->handle_message(msg);
311 : #endif
312 : }
313 :
314 : void handle_message(const log_RGPH &msg) {
315 : _gps.handle_message(msg);
316 : }
317 : void handle_message(const log_RGPI &msg) {
318 : _gps.handle_message(msg);
319 : }
320 : void handle_message(const log_RGPJ &msg) {
321 : _gps.handle_message(msg);
322 : }
323 : #if AP_DAL_RGPK_LOGGING_ENABLED
324 : void handle_message(const log_RGPK &msg) {
325 : _gps.handle_message(msg);
326 : }
327 : #endif
328 :
329 : void handle_message(const log_RMGH &msg) {
330 : _compass.handle_message(msg);
331 : }
332 : void handle_message(const log_RMGI &msg) {
333 : _compass.handle_message(msg);
334 : }
335 :
336 : void handle_message(const log_RBCH &msg) {
337 : #if AP_BEACON_ENABLED
338 : if (_beacon == nullptr) {
339 : _beacon = NEW_NOTHROW AP_DAL_Beacon;
340 : }
341 : _beacon->handle_message(msg);
342 : #endif
343 : }
344 : void handle_message(const log_RBCI &msg) {
345 : #if AP_BEACON_ENABLED
346 : if (_beacon == nullptr) {
347 : _beacon = NEW_NOTHROW AP_DAL_Beacon;
348 : }
349 : _beacon->handle_message(msg);
350 : #endif
351 : }
352 : void handle_message(const log_RVOH &msg) {
353 : #if HAL_VISUALODOM_ENABLED
354 : if (_visualodom == nullptr) {
355 : _visualodom = NEW_NOTHROW AP_DAL_VisualOdom;
356 : }
357 : _visualodom->handle_message(msg);
358 : #endif
359 : }
360 : void handle_message(const log_ROFH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
361 : void handle_message(const log_REPH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
362 : void handle_message(const log_REVH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
363 : void handle_message(const log_RWOH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
364 : void handle_message(const log_RBOH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
365 : void handle_message(const log_RSLL &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
366 : void handle_message(const log_RTER &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
367 :
368 : // map core number for replay
369 : uint8_t logging_core(uint8_t c) const;
370 :
371 : #if HAL_LOGGING_ENABLED
372 : // write out a DAL log message. If old_msg is non-null, then
373 : // only write if the content has changed
374 : static void WriteLogMessage(enum LogMessages msg_type, void *msg, const void *old_msg, uint8_t msg_size);
375 : #endif
376 :
377 : private:
378 :
379 : static AP_DAL *_singleton;
380 :
381 : // framing structures
382 : struct log_RFRH _RFRH;
383 : struct log_RFRF _RFRF;
384 : struct log_RFRN _RFRN;
385 :
386 : // push-based sensor structures
387 : struct log_ROFH _ROFH;
388 : struct log_REPH _REPH;
389 : struct log_REVH _REVH;
390 : struct log_RWOH _RWOH;
391 : struct log_RBOH _RBOH;
392 : struct log_RSLL _RSLL;
393 : struct log_RTER _RTER;
394 :
395 : // cached variables for speed:
396 : uint32_t _micros;
397 : uint32_t _millis;
398 :
399 : Matrix3f _rotation_vehicle_body_to_autopilot_body;
400 : Location _home;
401 : uint32_t _last_imu_time_us;
402 :
403 : AP_DAL_InertialSensor _ins;
404 : AP_DAL_Baro _baro;
405 : AP_DAL_GPS _gps;
406 : #if AP_RANGEFINDER_ENABLED
407 : AP_DAL_RangeFinder *_rangefinder;
408 : #endif
409 : AP_DAL_Compass _compass;
410 : AP_DAL_Airspeed *_airspeed;
411 : #if AP_BEACON_ENABLED
412 : AP_DAL_Beacon *_beacon;
413 : #endif
414 : #if HAL_VISUALODOM_ENABLED
415 : AP_DAL_VisualOdom *_visualodom;
416 : #endif
417 :
418 : static bool logging_started;
419 : static bool force_write;
420 :
421 : bool ekf2_init_done;
422 : bool ekf3_init_done;
423 :
424 : void init_sensors(void);
425 : bool init_done;
426 : };
427 :
428 : #if HAL_LOGGING_ENABLED
429 : #define WRITE_REPLAY_BLOCK(sname,v) AP_DAL::WriteLogMessage(LOG_## sname ##_MSG, &v, nullptr, offsetof(log_ ##sname, _end))
430 : #define WRITE_REPLAY_BLOCK_IFCHANGED(sname,v,old) do { static_assert(sizeof(v) == sizeof(old), "types must match"); \
431 : AP_DAL::WriteLogMessage(LOG_## sname ##_MSG, &v, &old, offsetof(log_ ##sname, _end)); } \
432 : while (0)
433 : #else
434 : #define WRITE_REPLAY_BLOCK(sname,v) do { (void)v; } while (false)
435 : #define WRITE_REPLAY_BLOCK_IFCHANGED(sname,v,old) do { (void)old; } while (false)
436 : #endif
437 :
438 : namespace AP {
439 : AP_DAL &dal();
440 : };
441 :
442 : // replay printf for debugging
443 : void rprintf(const char *format, ...);
444 :
|
