Line data Source code
1 : #include "AP_DAL.h"
2 :
3 : #include <AP_HAL/AP_HAL.h>
4 : #include <AP_Logger/AP_Logger.h>
5 : #include <AP_AHRS/AP_AHRS.h>
6 : #include <AP_Vehicle/AP_Vehicle.h>
7 : #include <AP_OpticalFlow/AP_OpticalFlow.h>
8 : #include <AP_WheelEncoder/AP_WheelEncoder.h>
9 : #include <AP_Vehicle/AP_Vehicle_Type.h>
10 : #include <AP_NavEKF3/AP_NavEKF3_feature.h>
11 : #include <AP_NavEKF/AP_Nav_Common.h>
12 :
13 : #if APM_BUILD_TYPE(APM_BUILD_Replay)
14 : #include <AP_NavEKF2/AP_NavEKF2.h>
15 : #include <AP_NavEKF3/AP_NavEKF3.h>
16 : #endif
17 :
18 : extern const AP_HAL::HAL& hal;
19 :
20 : AP_DAL *AP_DAL::_singleton = nullptr;
21 :
22 : bool AP_DAL::force_write;
23 : bool AP_DAL::logging_started;
24 :
25 84387106 : void AP_DAL::start_frame(AP_DAL::FrameType frametype)
26 : {
27 : #if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)
28 :
29 84387106 : if (!init_done) {
30 2006 : init_sensors();
31 : }
32 :
33 84387106 : const AP_AHRS &ahrs = AP::ahrs();
34 :
35 84387106 : const uint32_t imu_us = AP::ins().get_last_update_usec();
36 84387106 : if (_last_imu_time_us == imu_us) {
37 43230821 : _RFRF.frame_types |= uint8_t(frametype);
38 43230821 : return;
39 : }
40 41156285 : _last_imu_time_us = imu_us;
41 :
42 : // we force write all msgs when logging starts
43 : #if HAL_LOGGING_ENABLED
44 41156285 : bool logging = AP::logger().logging_started() && AP::logger().allow_start_ekf();
45 41156285 : if (logging && !logging_started) {
46 2032 : force_write = true;
47 : }
48 41156285 : logging_started = logging;
49 : #endif
50 :
51 41156285 : end_frame();
52 :
53 41156285 : _RFRF.frame_types = uint8_t(frametype);
54 :
55 : #if AP_VEHICLE_ENABLED
56 41156285 : _RFRH.time_flying_ms = AP::vehicle()->get_time_flying_ms();
57 : #else
58 : _RFRH.time_flying_ms = 0;
59 : #endif
60 41156285 : _RFRH.time_us = AP_HAL::micros64();
61 41156285 : WRITE_REPLAY_BLOCK(RFRH, _RFRH);
62 :
63 : // update RFRN data
64 41156285 : const log_RFRN old = _RFRN;
65 41156285 : _RFRN.armed = hal.util->get_soft_armed();
66 41156285 : _home = ahrs.get_home();
67 41156285 : _RFRN.lat = _home.lat;
68 41156285 : _RFRN.lng = _home.lng;
69 41156285 : _RFRN.alt = _home.alt;
70 : // EAS2TAS is essentially a sensor input to the EKFs, so it is
71 : // deliberately taken from the barometer's atmosphere model
72 : // rather than the AHRS, lest a state estimator's output be fed
73 : // back into the EKFs as an input:
74 41156285 : _RFRN.EAS2TAS = AP::baro().get_EAS2TAS();
75 41156285 : _RFRN.vehicle_class = (uint8_t)ahrs.get_vehicle_class();
76 41156285 : _RFRN.fly_forward = ahrs.get_fly_forward();
77 41156285 : _RFRN.takeoff_expected = ahrs.get_takeoff_expected();
78 41156285 : _RFRN.touchdown_expected = ahrs.get_touchdown_expected();
79 41156285 : _RFRN.ahrs_airspeed_sensor_enabled = ahrs.airspeed_sensor_enabled(ahrs.get_active_airspeed_index());
80 41156285 : _RFRN.available_memory = hal.util->available_memory();
81 41156285 : _RFRN.ahrs_trim = ahrs.get_trim();
82 : #if AP_OPTICALFLOW_ENABLED
83 41156285 : _RFRN.opticalflow_enabled = AP::opticalflow() && AP::opticalflow()->enabled();
84 : #endif
85 41156285 : _RFRN.wheelencoder_enabled = AP::wheelencoder() && (AP::wheelencoder()->num_sensors() > 0);
86 41156285 : _RFRN.ekf_type = int8_t(ahrs.configured_ekf_type());
87 41156285 : WRITE_REPLAY_BLOCK_IFCHANGED(RFRN, _RFRN, old);
88 :
89 : // update body conversion
90 41156285 : _rotation_vehicle_body_to_autopilot_body = ahrs.get_rotation_vehicle_body_to_autopilot_body();
91 :
92 41156285 : _ins.start_frame();
93 41156285 : _baro.start_frame();
94 41156285 : _gps.start_frame();
95 41156285 : _compass.start_frame();
96 41156285 : if (_airspeed) {
97 8609553 : _airspeed->start_frame();
98 : }
99 : #if AP_RANGEFINDER_ENABLED
100 41156285 : if (_rangefinder) {
101 4663689 : _rangefinder->start_frame();
102 : }
103 : #endif
104 : #if AP_BEACON_ENABLED
105 41156285 : if (_beacon) {
106 153757 : _beacon->start_frame();
107 : }
108 : #endif
109 : #if HAL_VISUALODOM_ENABLED
110 41156285 : if (_visualodom) {
111 284793 : _visualodom->start_frame();
112 : }
113 : #endif
114 :
115 : // populate some derivative values:
116 41156285 : _micros = _RFRH.time_us;
117 41156285 : _millis = _RFRH.time_us / 1000UL;
118 :
119 41156285 : force_write = false;
120 : #endif
121 : }
122 :
123 : // for EKF usage to enable takeoff expected to true
124 19 : void AP_DAL::set_takeoff_expected()
125 : {
126 : #if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)
127 19 : AP_AHRS &ahrs = AP::ahrs();
128 19 : ahrs.set_takeoff_expected(true);
129 : #endif
130 19 : }
131 :
132 : /*
133 : setup optional sensor backends
134 : */
135 2006 : void AP_DAL::init_sensors(void)
136 : {
137 2006 : init_done = true;
138 2006 : bool alloc_failed = false;
139 :
140 : /*
141 : we only allocate the DAL backends if we had at least one sensor
142 : at the time we startup the EKF
143 : */
144 :
145 : #if AP_RANGEFINDER_ENABLED
146 2006 : auto *rng = AP::rangefinder();
147 2006 : if (rng && rng->num_sensors() > 0) {
148 172 : alloc_failed |= (_rangefinder = NEW_NOTHROW AP_DAL_RangeFinder) == nullptr;
149 : }
150 : #endif
151 :
152 : #if AP_AIRSPEED_ENABLED
153 2006 : auto *aspeed = AP::airspeed();
154 2006 : if (aspeed != nullptr && aspeed->get_num_sensors() > 0) {
155 691 : alloc_failed |= (_airspeed = NEW_NOTHROW AP_DAL_Airspeed) == nullptr;
156 : }
157 : #endif
158 :
159 : #if AP_BEACON_ENABLED
160 2006 : auto *bcn = AP::beacon();
161 2006 : if (bcn != nullptr && bcn->enabled()) {
162 5 : alloc_failed |= (_beacon = NEW_NOTHROW AP_DAL_Beacon) == nullptr;
163 : }
164 : #endif
165 :
166 : #if HAL_VISUALODOM_ENABLED
167 2006 : auto *vodom = AP::visualodom();
168 2006 : if (vodom != nullptr && vodom->enabled()) {
169 10 : alloc_failed |= (_visualodom = NEW_NOTHROW AP_DAL_VisualOdom) == nullptr;
170 : }
171 : #endif
172 :
173 2006 : if (alloc_failed) {
174 0 : AP_BoardConfig::allocation_error("DAL backends");
175 : }
176 2006 : }
177 :
178 : /*
179 : end a frame. Must be called on all events and injections of data (eg
180 : flow) and before starting a new frame
181 : */
182 42289200 : void AP_DAL::end_frame(void)
183 : {
184 42289200 : if (_RFRF.frame_types != 0) {
185 41157081 : WRITE_REPLAY_BLOCK(RFRF, _RFRF);
186 41157081 : _RFRF.frame_types = 0;
187 : }
188 42289200 : }
189 :
190 5470 : void AP_DAL::log_event2(AP_DAL::Event event)
191 : {
192 : #if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)
193 5470 : end_frame();
194 5470 : struct log_REV2 pkt{
195 : event : uint8_t(event),
196 5470 : };
197 5470 : WRITE_REPLAY_BLOCK(REV2, pkt);
198 : #endif
199 5470 : }
200 :
201 750 : void AP_DAL::log_SetOriginLLH2(const Location &loc)
202 : {
203 : #if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)
204 750 : struct log_RSO2 pkt{
205 750 : lat : loc.lat,
206 750 : lng : loc.lng,
207 750 : alt : loc.alt,
208 750 : };
209 750 : WRITE_REPLAY_BLOCK(RSO2, pkt);
210 : #endif
211 750 : }
212 :
213 0 : void AP_DAL::log_writeDefaultAirSpeed2(const float aspeed, const float uncertainty)
214 : {
215 : #if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)
216 0 : struct log_RWA2 pkt{
217 : airspeed: aspeed,
218 : uncertainty: uncertainty,
219 0 : };
220 0 : WRITE_REPLAY_BLOCK(RWA2, pkt);
221 : #endif
222 0 : }
223 :
224 5518 : void AP_DAL::log_event3(AP_DAL::Event event)
225 : {
226 : #if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)
227 5518 : end_frame();
228 5518 : struct log_REV3 pkt{
229 : event : uint8_t(event),
230 5518 : };
231 5518 : WRITE_REPLAY_BLOCK(REV3, pkt);
232 : #endif
233 5518 : }
234 :
235 219 : void AP_DAL::log_SetOriginLLH3(const Location &loc)
236 : {
237 : #if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)
238 219 : struct log_RSO3 pkt{
239 219 : lat : loc.lat,
240 219 : lng : loc.lng,
241 219 : alt : loc.alt,
242 219 : };
243 219 : WRITE_REPLAY_BLOCK(RSO3, pkt);
244 : #endif
245 219 : }
246 :
247 0 : void AP_DAL::log_writeDefaultAirSpeed3(const float aspeed, const float uncertainty)
248 : {
249 : #if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)
250 0 : struct log_RWA3 pkt{
251 : airspeed: aspeed,
252 : uncertainty: uncertainty
253 0 : };
254 0 : WRITE_REPLAY_BLOCK(RWA3, pkt);
255 : #endif
256 0 : }
257 :
258 0 : void AP_DAL::log_writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t timeStamp_ms, uint8_t type)
259 : {
260 : #if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)
261 0 : struct log_REY3 pkt{
262 : yawangle : yawAngle,
263 : yawangleerr : yawAngleErr,
264 : timestamp_ms : timeStamp_ms,
265 : type : type,
266 0 : };
267 0 : WRITE_REPLAY_BLOCK(REY3, pkt);
268 : #endif
269 0 : }
270 :
271 1013350 : int AP_DAL::snprintf(char* str, size_t size, const char *format, ...) const
272 : {
273 : va_list ap;
274 1013350 : va_start(ap, format);
275 1013350 : int res = hal.util->vsnprintf(str, size, format, ap);
276 1013350 : va_end(ap);
277 1013350 : return res;
278 : }
279 :
280 2506 : void *AP_DAL::malloc_type(size_t size, MemoryType mem_type) const
281 : {
282 2506 : return hal.util->malloc_type(size, AP_HAL::Util::Memory_Type(mem_type));
283 : }
284 0 : void AP_DAL::free_type(void *ptr, size_t size, MemoryType mem_type) const
285 : {
286 0 : return hal.util->free_type(ptr, size, AP_HAL::Util::Memory_Type(mem_type));
287 : }
288 :
289 : // map core number for replay
290 46396036 : uint8_t AP_DAL::logging_core(uint8_t c) const
291 : {
292 : #if APM_BUILD_TYPE(APM_BUILD_Replay)
293 : return c+100U;
294 : #else
295 46396036 : return c;
296 : #endif
297 : }
298 :
299 : #if HAL_LOGGING_ENABLED
300 : // write out a DAL log message. If old_msg is non-null, then
301 : // only write if the content has changed
302 1149211250 : void AP_DAL::WriteLogMessage(enum LogMessages msg_type, void *msg, const void *old_msg, uint8_t msg_size)
303 : {
304 1149211250 : if (!logging_started) {
305 : // we're not logging
306 50454505 : return;
307 : }
308 : // we use the _end byte to hold a flag for forcing output
309 1098756745 : uint8_t &_end = ((uint8_t *)msg)[msg_size];
310 1098756745 : if (old_msg && !force_write && _end == 0 && memcmp(msg, old_msg, msg_size) == 0) {
311 : // no change, skip this block write
312 931046679 : return;
313 : }
314 167710066 : if (!AP::logger().WriteReplayBlock(msg_type, msg, msg_size)) {
315 : // mark for forced write next time
316 0 : _end = 1;
317 : } else {
318 167710066 : _end = 0;
319 : }
320 : }
321 : #endif
322 :
323 : /*
324 : check if we are low on CPU for this core. This needs to capture the
325 : timing of running the cores
326 : */
327 82531734 : bool AP_DAL::ekf_low_time_remaining(EKFType etype, uint8_t core)
328 : {
329 : static_assert(MAX_EKF_CORES <= 4, "max 4 EKF cores supported");
330 82531734 : const uint8_t mask = (1U<<(core+(uint8_t(etype)*4)));
331 : #if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)
332 : /*
333 : if we have used more than 1/3 of the time for a loop then we
334 : return true, indicating that we are low on CPU. This changes the
335 : scheduling of fusion between lanes
336 : */
337 82531734 : const auto &imu = AP::ins();
338 82531734 : if ((AP_HAL::micros() - imu.get_last_update_usec())*1.0e-6 > imu.get_loop_delta_t()*0.33) {
339 0 : _RFRF.core_slow |= mask;
340 : } else {
341 82531734 : _RFRF.core_slow &= ~mask;
342 : }
343 : #endif
344 82531734 : return (_RFRF.core_slow & mask) != 0;
345 : }
346 :
347 : // log optical flow data
348 20256 : void AP_DAL::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
349 : {
350 20256 : end_frame();
351 :
352 20256 : const log_ROFH old = _ROFH;
353 20256 : _ROFH.rawFlowQuality = rawFlowQuality;
354 20256 : _ROFH.rawFlowRates = rawFlowRates;
355 20256 : _ROFH.rawGyroRates = rawGyroRates;
356 20256 : _ROFH.msecFlowMeas = msecFlowMeas;
357 20256 : _ROFH.posOffset = posOffset;
358 20256 : _ROFH.heightOverride = heightOverride;
359 20256 : WRITE_REPLAY_BLOCK_IFCHANGED(ROFH, _ROFH, old);
360 20256 : }
361 :
362 : // log external navigation data
363 20138 : void AP_DAL::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms)
364 : {
365 20138 : end_frame();
366 :
367 20138 : const log_REPH old = _REPH;
368 20138 : _REPH.pos = pos;
369 20138 : _REPH.quat = quat;
370 20138 : _REPH.posErr = posErr;
371 20138 : _REPH.angErr = angErr;
372 20138 : _REPH.timeStamp_ms = timeStamp_ms;
373 20138 : _REPH.delay_ms = delay_ms;
374 20138 : _REPH.resetTime_ms = resetTime_ms;
375 20138 : WRITE_REPLAY_BLOCK_IFCHANGED(REPH, _REPH, old);
376 20138 : }
377 :
378 2 : void AP_DAL::log_SetLatLng(const Location &loc, float posAccuracy, uint32_t timestamp_ms)
379 : {
380 2 : end_frame();
381 2 : const log_RSLL old = _RSLL;
382 2 : _RSLL.lat = loc.lat;
383 2 : _RSLL.lng = loc.lng;
384 2 : _RSLL.posAccSD = posAccuracy;
385 2 : _RSLL.timestamp_ms = timestamp_ms;
386 2 : WRITE_REPLAY_BLOCK_IFCHANGED(RSLL, _RSLL, old);
387 2 : }
388 :
389 : // log external velocity data
390 8556 : void AP_DAL::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms)
391 : {
392 8556 : end_frame();
393 :
394 8556 : const log_REVH old = _REVH;
395 8556 : _REVH.vel = vel;
396 8556 : _REVH.err = err;
397 8556 : _REVH.timeStamp_ms = timeStamp_ms;
398 8556 : _REVH.delay_ms = delay_ms;
399 8556 : WRITE_REPLAY_BLOCK_IFCHANGED(REVH, _REVH, old);
400 :
401 8556 : }
402 :
403 : // log wheel odometry data
404 4353 : void AP_DAL::writeWheelOdom(float delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset, float radius)
405 : {
406 4353 : end_frame();
407 :
408 4353 : const log_RWOH old = _RWOH;
409 4353 : _RWOH.delAng = delAng;
410 4353 : _RWOH.delTime = delTime;
411 4353 : _RWOH.timeStamp_ms = timeStamp_ms;
412 4353 : _RWOH.posOffset = posOffset;
413 4353 : _RWOH.radius = radius;
414 4353 : WRITE_REPLAY_BLOCK_IFCHANGED(RWOH, _RWOH, old);
415 4353 : }
416 :
417 3633 : void AP_DAL::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset)
418 : {
419 3633 : end_frame();
420 :
421 3633 : const log_RBOH old = _RBOH;
422 3633 : _RBOH.quality = quality;
423 3633 : _RBOH.delPos = delPos;
424 3633 : _RBOH.delAng = delAng;
425 3633 : _RBOH.delTime = delTime;
426 3633 : _RBOH.timeStamp_ms = timeStamp_ms;
427 3633 : _RBOH.posOffset = posOffset;
428 3633 : _RBOH.delay_ms = delay_ms;
429 3633 : WRITE_REPLAY_BLOCK_IFCHANGED(RBOH, _RBOH, old);
430 3633 : }
431 :
432 : // Write terrain altitude (derived from SRTM) in meters above the origin
433 1064989 : void AP_DAL::writeTerrainData(float alt_m)
434 : {
435 : #if EK3_FEATURE_OPTFLOW_SRTM
436 1064989 : end_frame();
437 :
438 1064989 : const log_RTER old = _RTER;
439 1064989 : _RTER.alt_m = alt_m;
440 1064989 : WRITE_REPLAY_BLOCK_IFCHANGED(RTER, _RTER, old);
441 : #endif
442 1064989 : }
443 :
444 : #if APM_BUILD_TYPE(APM_BUILD_Replay)
445 : /*
446 : handle frame message. This message triggers the EKF2/EKF3 updates and logging
447 : */
448 : void AP_DAL::handle_message(const log_RFRF &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
449 : {
450 : _RFRF.core_slow = msg.core_slow;
451 :
452 : /*
453 : note that we need to handle the case of LOG_REPLAY=1 with
454 : LOG_DISARMED=0. To handle this we need to record the start of the filter
455 : */
456 : const uint8_t frame_types = msg.frame_types;
457 : if (frame_types & uint8_t(AP_DAL::FrameType::InitialiseFilterEKF2)) {
458 : ekf2_init_done = ekf2.InitialiseFilter();
459 : }
460 : if (frame_types & uint8_t(AP_DAL::FrameType::UpdateFilterEKF2)) {
461 : if (!ekf2_init_done) {
462 : ekf2_init_done = ekf2.InitialiseFilter();
463 : }
464 : if (ekf2_init_done) {
465 : ekf2.UpdateFilter();
466 : }
467 : }
468 : if (frame_types & uint8_t(AP_DAL::FrameType::InitialiseFilterEKF3)) {
469 : ekf3_init_done = ekf3.InitialiseFilter();
470 : }
471 : if (frame_types & uint8_t(AP_DAL::FrameType::UpdateFilterEKF3)) {
472 : if (!ekf3_init_done) {
473 : ekf3_init_done = ekf3.InitialiseFilter();
474 : }
475 : if (ekf3_init_done) {
476 : ekf3.UpdateFilter();
477 : }
478 : }
479 : if (frame_types & uint8_t(AP_DAL::FrameType::LogWriteEKF2)) {
480 : ekf2.Log_Write();
481 : }
482 : if (frame_types & uint8_t(AP_DAL::FrameType::LogWriteEKF3)) {
483 : ekf3.Log_Write();
484 : }
485 : }
486 :
487 : /*
488 : handle optical flow message
489 : */
490 : void AP_DAL::handle_message(const log_ROFH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
491 : {
492 : _ROFH = msg;
493 : ekf2.writeOptFlowMeas(msg.rawFlowQuality, msg.rawFlowRates, msg.rawGyroRates, msg.msecFlowMeas, msg.posOffset, msg.heightOverride);
494 : #if EK3_FEATURE_OPTFLOW_FUSION
495 : ekf3.writeOptFlowMeas(msg.rawFlowQuality, msg.rawFlowRates, msg.rawGyroRates, msg.msecFlowMeas, msg.posOffset, msg.heightOverride);
496 : #endif
497 : }
498 :
499 : /*
500 : handle external position data
501 : */
502 : void AP_DAL::handle_message(const log_REPH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
503 : {
504 : _REPH = msg;
505 : ekf2.writeExtNavData(msg.pos, msg.quat, msg.posErr, msg.angErr, msg.timeStamp_ms, msg.delay_ms, msg.resetTime_ms);
506 : ekf3.writeExtNavData(msg.pos, msg.quat, msg.posErr, msg.angErr, msg.timeStamp_ms, msg.delay_ms, msg.resetTime_ms);
507 : }
508 :
509 : /*
510 : handle external velocity data
511 : */
512 : void AP_DAL::handle_message(const log_REVH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
513 : {
514 : _REVH = msg;
515 : ekf2.writeExtNavVelData(msg.vel, msg.err, msg.timeStamp_ms, msg.delay_ms);
516 : ekf3.writeExtNavVelData(msg.vel, msg.err, msg.timeStamp_ms, msg.delay_ms);
517 : }
518 :
519 : /*
520 : handle wheel odometry data
521 : */
522 : void AP_DAL::handle_message(const log_RWOH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
523 : {
524 : _RWOH = msg;
525 : // note that EKF2 does not support wheel odometry
526 : ekf3.writeWheelOdom(msg.delAng, msg.delTime, msg.timeStamp_ms, msg.posOffset, msg.radius);
527 : }
528 :
529 : /*
530 : handle body frame odometry
531 : */
532 : void AP_DAL::handle_message(const log_RBOH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
533 : {
534 : _RBOH = msg;
535 : // note that EKF2 does not support body frame odometry
536 : ekf3.writeBodyFrameOdom(msg.quality, msg.delPos, msg.delAng, msg.delTime, msg.timeStamp_ms, msg.delay_ms, msg.posOffset);
537 : }
538 :
539 : /*
540 : * handle terrain altitude data message
541 : */
542 : void AP_DAL::handle_message(const log_RTER &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
543 : {
544 : #if EK3_FEATURE_OPTFLOW_SRTM
545 : _RTER = msg;
546 : // note that EKF2 does not accept the terrain altitude
547 : ekf3.writeTerrainData(msg.alt_m);
548 : #endif
549 : }
550 :
551 : /*
552 : handle position reset
553 : */
554 : void AP_DAL::handle_message(const log_RSLL &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
555 : {
556 : _RSLL = msg;
557 : // note that EKF2 does not support body frame odometry
558 : const Location loc {msg.lat, msg.lng, 0, Location::AltFrame::ABSOLUTE };
559 : ekf3.setLatLng(loc, msg.posAccSD, msg.timestamp_ms);
560 : }
561 : #endif // APM_BUILD_Replay
562 :
563 : namespace AP {
564 :
565 265897692 : AP_DAL &dal()
566 : {
567 265897692 : return *AP_DAL::get_singleton();
568 : }
569 :
570 : };
571 :
572 : /*
573 : replay printf. To debug replay failures add rprintf() calls into
574 : EKF2/EKF3 and compare /tmp/replay.log to /tmp/real.log
575 : */
576 0 : void rprintf(const char *format, ...)
577 : {
578 :
579 : #if (APM_BUILD_TYPE(APM_BUILD_Replay) || CONFIG_HAL_BOARD == HAL_BOARD_SITL) && CONFIG_HAL_BOARD != HAL_BOARD_CHIBIOS
580 : #if APM_BUILD_TYPE(APM_BUILD_Replay)
581 : const char *fname = "/tmp/replay.log";
582 : #elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
583 0 : const char *fname = "/tmp/real.log";
584 : #endif
585 : static FILE *f;
586 0 : if (!f) {
587 0 : f = ::fopen(fname, "w");
588 : }
589 0 : if (!f) {
590 0 : return;
591 : }
592 : va_list ap;
593 0 : va_start(ap, format);
594 0 : vfprintf(f, format, ap);
595 0 : fflush(f);
596 0 : va_end(ap);
597 : #endif
598 0 : }
599 :
|
