Line data Source code
1 : #include <AP_Logger/AP_Logger_config.h>
2 :
3 : #if HAL_LOGGING_ENABLED
4 :
5 : #include "AP_NavEKF3.h"
6 : #include "AP_NavEKF3_core.h"
7 :
8 : #include <AP_HAL/HAL.h>
9 : #include <AP_Logger/AP_Logger.h>
10 :
11 : #include <AP_DAL/AP_DAL.h>
12 :
13 : #pragma GCC diagnostic ignored "-Wnarrowing"
14 :
15 4749244 : void NavEKF3_core::Log_Write_XKF1(uint64_t time_us) const
16 : {
17 : // Write first EKF packet
18 4749244 : Vector3f euler;
19 4749244 : Vector2p posNE;
20 : postype_t posD;
21 4749244 : Vector3f velNED;
22 4749244 : Vector3f gyroBias;
23 : float posDownDeriv;
24 4749244 : Location originLLH;
25 4749244 : getEulerAngles(euler);
26 4749244 : getVelNED(velNED);
27 4749244 : getPosNE(posNE);
28 4749244 : getPosD(posD);
29 4749244 : getGyroBias(gyroBias);
30 4749244 : posDownDeriv = getPosDownDerivative();
31 4749244 : if (!getOriginLLH(originLLH)) {
32 623232 : originLLH.alt = 0;
33 : }
34 4749244 : const struct log_XKF1 pkt{
35 : LOG_PACKET_HEADER_INIT(LOG_XKF1_MSG),
36 : time_us : time_us,
37 4749244 : core : DAL_CORE(core_index),
38 9498488 : roll : (int16_t)(100*degrees(euler.x)), // roll angle (centi-deg, displayed as deg due to format string)
39 9498488 : pitch : (int16_t)(100*degrees(euler.y)), // pitch angle (centi-deg, displayed as deg due to format string)
40 4749244 : yaw : (uint16_t)wrap_360_cd(100*degrees(euler.z)), // yaw angle (centi-deg, displayed as deg due to format string)
41 4749244 : velN : (float)(velNED.x), // velocity North (m/s)
42 4749244 : velE : (float)(velNED.y), // velocity East (m/s)
43 4749244 : velD : (float)(velNED.z), // velocity Down (m/s)
44 : posD_dot : (float)(posDownDeriv), // first derivative of down position
45 4749244 : posN : (float)(posNE.x), // metres North
46 4749244 : posE : (float)(posNE.y), // metres East
47 4749244 : posD : (float)(posD), // metres Down
48 9498488 : gyrX : (int16_t)(100*degrees(gyroBias.x)), // cd/sec, displayed as deg/sec due to format string
49 9498488 : gyrY : (int16_t)(100*degrees(gyroBias.y)), // cd/sec, displayed as deg/sec due to format string
50 9498488 : gyrZ : (int16_t)(100*degrees(gyroBias.z)), // cd/sec, displayed as deg/sec due to format string
51 4749244 : originHgt : originLLH.alt // WGS-84 altitude of EKF origin in cm
52 14247732 : };
53 4749244 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
54 4749244 : }
55 :
56 4749244 : void NavEKF3_core::Log_Write_XKF2(uint64_t time_us) const
57 : {
58 : // Write second EKF packet
59 4749244 : Vector3f accelBias;
60 4749244 : Vector3f wind;
61 4749244 : Vector3f magNED;
62 4749244 : Vector3f magXYZ;
63 4749244 : getAccelBias(accelBias);
64 4749244 : getWind(wind);
65 4749244 : getMagNED(magNED);
66 4749244 : getMagXYZ(magXYZ);
67 4749244 : Vector2f dragInnov;
68 4749244 : float betaInnov = 0;
69 4749244 : getSynthAirDataInnovations(dragInnov, betaInnov);
70 4749244 : const struct log_XKF2 pkt2{
71 : LOG_PACKET_HEADER_INIT(LOG_XKF2_MSG),
72 : time_us : time_us,
73 4749244 : core : DAL_CORE(core_index),
74 4749244 : accBiasX : (int16_t)(100*accelBias.x),
75 4749244 : accBiasY : (int16_t)(100*accelBias.y),
76 4749244 : accBiasZ : (int16_t)(100*accelBias.z),
77 4749244 : windN : (int16_t)(100*wind.x),
78 4749244 : windE : (int16_t)(100*wind.y),
79 4749244 : magN : (int16_t)(magNED.x),
80 4749244 : magE : (int16_t)(magNED.y),
81 4749244 : magD : (int16_t)(magNED.z),
82 4749244 : magX : (int16_t)(magXYZ.x),
83 4749244 : magY : (int16_t)(magXYZ.y),
84 4749244 : magZ : (int16_t)(magXYZ.z),
85 4749244 : innovDragX : dragInnov.x,
86 4749244 : innovDragY : dragInnov.y,
87 : innovSideslip : betaInnov
88 4749244 : };
89 4749244 : AP::logger().WriteBlock(&pkt2, sizeof(pkt2));
90 4749244 : }
91 :
92 4749244 : void NavEKF3_core::Log_Write_XKFS(uint64_t time_us) const
93 : {
94 : // Write sensor selection EKF packet
95 4749244 : const struct log_XKFS pkt {
96 : LOG_PACKET_HEADER_INIT(LOG_XKFS_MSG),
97 : time_us : time_us,
98 4749244 : core : DAL_CORE(core_index),
99 4749244 : mag_index : magSelectIndex,
100 4749244 : baro_index : selected_baro,
101 4749244 : gps_index : selected_gps,
102 4749244 : airspeed_index : getActiveAirspeed(),
103 4749244 : source_set : frontend->sources.getActiveSourceSet(core_index),
104 4749244 : gps_good_to_align : gpsGoodToAlign,
105 4749244 : wait_for_gps_checks : waitingForGpsChecks,
106 4749244 : mag_fusion: (uint8_t) magFusionSel
107 9498488 : };
108 4749244 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
109 4749244 : }
110 :
111 4749244 : void NavEKF3_core::Log_Write_XKF3(uint64_t time_us) const
112 : {
113 : // Write third EKF packet
114 4749244 : Vector3f velInnov;
115 4749244 : Vector3f posInnov;
116 4749244 : Vector3f magInnov;
117 4749244 : float tasInnov = 0;
118 4749244 : float yawInnov = 0;
119 4749244 : getInnovations(velInnov, posInnov, magInnov, tasInnov, yawInnov);
120 4749244 : const struct log_XKF3 pkt3{
121 : LOG_PACKET_HEADER_INIT(LOG_XKF3_MSG),
122 : time_us : time_us,
123 4749244 : core : DAL_CORE(core_index),
124 4749244 : innovVN : (int16_t)(100*velInnov.x),
125 4749244 : innovVE : (int16_t)(100*velInnov.y),
126 4749244 : innovVD : (int16_t)(100*velInnov.z),
127 4749244 : innovPN : (int16_t)(100*posInnov.x),
128 4749244 : innovPE : (int16_t)(100*posInnov.y),
129 4749244 : innovPD : (int16_t)(100*posInnov.z),
130 4749244 : innovMX : (int16_t)(magInnov.x),
131 4749244 : innovMY : (int16_t)(magInnov.y),
132 4749244 : innovMZ : (int16_t)(magInnov.z),
133 9498488 : innovYaw : (int16_t)(100*degrees(yawInnov)),
134 4749244 : innovVT : (int16_t)(100*tasInnov),
135 4749244 : rerr : frontend->coreRelativeErrors[core_index],
136 4749244 : errorScore : frontend->coreErrorScores[core_index]
137 9498488 : };
138 4749244 : AP::logger().WriteBlock(&pkt3, sizeof(pkt3));
139 4749244 : }
140 :
141 4749244 : void NavEKF3_core::Log_Write_XKF4(uint64_t time_us) const
142 : {
143 : // Write fourth EKF packet
144 4749244 : float velVar = 0;
145 4749244 : float posVar = 0;
146 4749244 : float hgtVar = 0;
147 4749244 : Vector3f magVar;
148 4749244 : float tasVar = 0;
149 4749244 : uint16_t _faultStatus=0;
150 4749244 : Vector2f offset;
151 4749244 : const uint8_t timeoutStatus =
152 4749244 : posTimeout<<0 |
153 4749244 : velTimeout<<1 |
154 4749244 : hgtTimeout<<2 |
155 4749244 : magTimeout<<3 |
156 4749244 : tasTimeout<<4 |
157 4749244 : dragTimeout<<5;
158 :
159 4749244 : nav_filter_status solutionStatus {};
160 4749244 : getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
161 4749244 : float tempVar = fmaxF(fmaxF(magVar.x,magVar.y),magVar.z);
162 4749244 : getFilterFaults(_faultStatus);
163 4749244 : getFilterStatus(solutionStatus);
164 4749244 : const struct log_XKF4 pkt4{
165 : LOG_PACKET_HEADER_INIT(LOG_XKF4_MSG),
166 : time_us : time_us,
167 4749244 : core : DAL_CORE(core_index),
168 4749244 : sqrtvarV : (int16_t)(100*velVar),
169 4749244 : sqrtvarP : (int16_t)(100*posVar),
170 4749244 : sqrtvarH : (int16_t)(100*hgtVar),
171 4749244 : sqrtvarM : (int16_t)(100*tempVar),
172 4749244 : sqrtvarVT : (int16_t)(100*tasVar),
173 9498488 : tiltErr : sqrtF(MAX(tiltErrorVariance,0.0f)), // estimated 1-sigma tilt error in radians
174 4749244 : offsetNorth : offset.x,
175 4749244 : offsetEast : offset.y,
176 : faults : _faultStatus,
177 : timeouts : timeoutStatus,
178 4749244 : solution : solutionStatus.value,
179 4749244 : gps : gpsCheckStatus.value,
180 4749244 : primary : frontend->getPrimaryCoreIndex()
181 9498488 : };
182 4749244 : AP::logger().WriteBlock(&pkt4, sizeof(pkt4));
183 4749244 : }
184 :
185 :
186 4749244 : void NavEKF3_core::Log_Write_XKF5(uint64_t time_us) const
187 : {
188 4749244 : if (core_index != frontend->primary) {
189 : // log only primary instance for now
190 2356435 : return;
191 : }
192 :
193 2392809 : const struct log_XKF5 pkt5{
194 : LOG_PACKET_HEADER_INIT(LOG_XKF5_MSG),
195 : time_us : time_us,
196 2392809 : core : DAL_CORE(core_index),
197 2392809 : normInnov : (uint8_t)(MIN(100*MAX(flowTestRatio[0],flowTestRatio[1]),255)), // normalised innovation variance ratio for optical flow observations fused by the main nav filter
198 2392809 : FIX : (int16_t)(1000*flowInnov[0]), // optical flow LOS rate vector innovations from the main nav filter
199 2392809 : FIY : (int16_t)(1000*flowInnov[1]), // optical flow LOS rate vector innovations from the main nav filter
200 4785618 : AFI : (int16_t)(1000 * auxFlowObsInnov.length()), // optical flow LOS rate innovation from terrain offset estimator
201 2392809 : HAGL : float_to_int16(100*(terrainState - stateStruct.position.z)), // height above ground level
202 2392809 : terrOffset : (int16_t)(100*terrainState), // filter ground offset state error
203 2392809 : RI : (int16_t)(100*innovRng), // range finder innovations
204 2392809 : meaRng : (uint16_t)(100*rangeDataDelayed.rng), // measured range
205 : #if EK3_FEATURE_OPTFLOW_FUSION
206 2392809 : errHAGL : (uint16_t)(100*sqrtF(Popt)), // note Popt is constrained to be non-negative in EstimateTerrainOffset()
207 : #else
208 : errHAGL : 0, // note Popt is constrained to be non-negative in EstimateTerrainOffset()
209 : #endif
210 2392809 : angErr : (float)outputTrackError.x, // output predictor angle error
211 2392809 : velErr : (float)outputTrackError.y, // output predictor velocity error
212 2392809 : posErr : (float)outputTrackError.z, // output predictor position tracking error
213 2392809 : baroOffset : (float)baroHgtOffset // barometer height offset subtracted from raw baro measurement when fusing baro height
214 7178427 : };
215 2392809 : AP::logger().WriteBlock(&pkt5, sizeof(pkt5));
216 :
217 : #if EK3_FEATURE_OPTFLOW_AGL_KF
218 : // log the optional "special" KF for HAGL estimation directly via RangeFinder + IMU
219 2392809 : if (frontend->option_is_enabled(NavEKF3::Option::AglKfForOptflow)) {
220 0 : const struct log_XKFA pktfA{
221 : LOG_PACKET_HEADER_INIT(LOG_XKFA_MSG),
222 : time_us : time_us,
223 0 : core : DAL_CORE(core_index),
224 0 : hAgl : (float)aglKfH, // AGL height estimate (m)
225 0 : vAgl : (float)aglKfV, // AGL velocity estimate (m/s, +ve = climbing)
226 0 : hAglStd : (float)sqrtF(aglKfP[0][0]), // std-dev of h_agl (m)
227 0 : vAglStd : (float)sqrtF(aglKfP[1][1]), // std-dev of v_agl (m/s)
228 0 : valid : (uint8_t)aglKfValid // 1 when RF fused within last 5 s
229 0 : };
230 0 : AP::logger().WriteBlock(&pktfA, sizeof(pktfA));
231 : }
232 : #endif
233 : }
234 :
235 4749244 : void NavEKF3_core::Log_Write_Quaternion(uint64_t time_us) const
236 : {
237 : // log quaternion
238 4749244 : Quaternion quat;
239 4749244 : getQuaternion( quat);
240 4749244 : const struct log_XKQ pktq1{
241 : LOG_PACKET_HEADER_INIT(LOG_XKQ_MSG),
242 : time_us : time_us,
243 4749244 : core : DAL_CORE(core_index),
244 4749244 : q1 : quat.q1,
245 4749244 : q2 : quat.q2,
246 4749244 : q3 : quat.q3,
247 4749244 : q4 : quat.q4
248 4749244 : };
249 4749244 : AP::logger().WriteBlock(&pktq1, sizeof(pktq1));
250 4749244 : }
251 :
252 : #if EK3_FEATURE_BEACON_FUSION
253 : // logs beacon information, one beacon per call
254 4749244 : void NavEKF3_core::Log_Write_Beacon(uint64_t time_us)
255 : {
256 4749244 : if (core_index != frontend->primary) {
257 : // log only primary instance for now
258 4741738 : return;
259 : }
260 :
261 2392809 : if (!statesInitialised || rngBcn.N == 0 || rngBcn.fusionReport == nullptr) {
262 : return;
263 : }
264 :
265 : // Ensure that beacons are not skipped due to calling this function at a rate lower than the updates
266 7701 : if (rngBcn.fuseDataReportIndex >= rngBcn.N ||
267 5778 : rngBcn.fuseDataReportIndex > rngBcn.numFusionReports) {
268 1923 : rngBcn.fuseDataReportIndex = 0;
269 : }
270 :
271 7701 : const auto &report = rngBcn.fusionReport[rngBcn.fuseDataReportIndex];
272 :
273 : // write range beacon fusion debug packet if the range value is non-zero
274 7701 : if (report.rng <= 0.0f) {
275 195 : rngBcn.fuseDataReportIndex++;
276 195 : return;
277 : }
278 :
279 7506 : const struct log_XKF0 pkt10{
280 : LOG_PACKET_HEADER_INIT(LOG_XKF0_MSG),
281 : time_us : time_us,
282 7506 : core : DAL_CORE(core_index),
283 7506 : ID : rngBcn.fuseDataReportIndex,
284 7506 : rng : (int16_t)(100*report.rng),
285 7506 : innov : (int16_t)(100*report.innov),
286 7506 : sqrtInnovVar : (uint16_t)(100*sqrtF(report.innovVar)),
287 15012 : testRatio : (uint16_t)(100*constrain_ftype(report.testRatio,0.0f,650.0f)),
288 7506 : beaconPosN : (int16_t)(100*report.beaconPosNED.x),
289 7506 : beaconPosE : (int16_t)(100*report.beaconPosNED.y),
290 7506 : beaconPosD : (int16_t)(100*report.beaconPosNED.z),
291 7506 : offsetHigh : (int16_t)(100*rngBcn.posDownOffsetMax),
292 7506 : offsetLow : (int16_t)(100*rngBcn.posDownOffsetMin),
293 7506 : posN : (int16_t)(100*rngBcn.receiverPos.x),
294 7506 : posE : (int16_t)(100*rngBcn.receiverPos.y),
295 7506 : posD : (int16_t)(100*rngBcn.receiverPos.z)
296 15012 : };
297 7506 : AP::logger().WriteBlock(&pkt10, sizeof(pkt10));
298 7506 : rngBcn.fuseDataReportIndex++;
299 : }
300 : #endif // EK3_FEATURE_BEACON_FUSION
301 :
302 : #if EK3_FEATURE_BODY_ODOM
303 4749244 : void NavEKF3_core::Log_Write_BodyOdom(uint64_t time_us)
304 : {
305 4749244 : if (core_index != frontend->primary) {
306 : // log only primary instance for now
307 : return;
308 : }
309 :
310 2392809 : const uint32_t updateTime_ms = MAX(bodyOdmDataDelayed.time_ms,wheelOdmDataDelayed.time_ms);
311 2392809 : if (updateTime_ms > lastUpdateTime_ms) {
312 1459 : const struct log_XKFD pkt11{
313 : LOG_PACKET_HEADER_INIT(LOG_XKFD_MSG),
314 : time_us : time_us,
315 1459 : core : DAL_CORE(core_index),
316 1459 : velInnovX : innovBodyVel[0],
317 1459 : velInnovY : innovBodyVel[1],
318 1459 : velInnovZ : innovBodyVel[2],
319 1459 : velInnovVarX : varInnovBodyVel[0],
320 1459 : velInnovVarY : varInnovBodyVel[1],
321 1459 : velInnovVarZ : varInnovBodyVel[2]
322 1459 : };
323 1459 : AP::logger().WriteBlock(&pkt11, sizeof(pkt11));
324 1459 : lastUpdateTime_ms = updateTime_ms;
325 : }
326 : }
327 : #endif
328 :
329 4749244 : void NavEKF3_core::Log_Write_State_Variances(uint64_t time_us)
330 : {
331 4749244 : if (core_index != frontend->primary) {
332 : // log only primary instance for now
333 : return;
334 : }
335 :
336 2392809 : if (AP::dal().millis() - lastEkfStateVarLogTime_ms > 490) {
337 283088 : lastEkfStateVarLogTime_ms = AP::dal().millis();
338 283088 : const struct log_XKV pktv1{
339 : LOG_PACKET_HEADER_INIT(LOG_XKV1_MSG),
340 : time_us : time_us,
341 283088 : core : DAL_CORE(core_index),
342 283088 : v00 : P[0][0],
343 283088 : v01 : P[1][1],
344 283088 : v02 : P[2][2],
345 283088 : v03 : P[3][3],
346 283088 : v04 : P[4][4],
347 283088 : v05 : P[5][5],
348 283088 : v06 : P[6][6],
349 283088 : v07 : P[7][7],
350 283088 : v08 : P[8][8],
351 283088 : v09 : P[9][9],
352 283088 : v10 : P[10][10],
353 283088 : v11 : P[11][11]
354 283088 : };
355 283088 : AP::logger().WriteBlock(&pktv1, sizeof(pktv1));
356 283088 : const struct log_XKV pktv2{
357 : LOG_PACKET_HEADER_INIT(LOG_XKV2_MSG),
358 : time_us : time_us,
359 283088 : core : DAL_CORE(core_index),
360 283088 : v00 : P[12][12],
361 283088 : v01 : P[13][13],
362 283088 : v02 : P[14][14],
363 283088 : v03 : P[15][15],
364 283088 : v04 : P[16][16],
365 283088 : v05 : P[17][17],
366 283088 : v06 : P[18][18],
367 283088 : v07 : P[19][19],
368 283088 : v08 : P[20][20],
369 283088 : v09 : P[21][21],
370 283088 : v10 : P[22][22],
371 283088 : v11 : P[23][23]
372 283088 : };
373 283088 : AP::logger().WriteBlock(&pktv2, sizeof(pktv2));
374 : }
375 : }
376 :
377 2461314 : void NavEKF3::Log_Write()
378 : {
379 : // only log if enabled
380 2461314 : if (activeCores() <= 0) {
381 : return;
382 : }
383 2392822 : if (lastLogWrite_us == imuSampleTime_us) {
384 : // vehicle is doubling up on logging
385 : return;
386 : }
387 2392809 : lastLogWrite_us = imuSampleTime_us;
388 :
389 2392809 : uint64_t time_us = AP::dal().micros64();
390 :
391 7142053 : for (uint8_t i=0; i<activeCores(); i++) {
392 4749244 : core[i].Log_Write(time_us);
393 : }
394 :
395 2392809 : AP::dal().start_frame(AP_DAL::FrameType::LogWriteEKF3);
396 : }
397 :
398 4749244 : void NavEKF3_core::Log_Write(uint64_t time_us)
399 : {
400 4749244 : const auto level = frontend->_log_level;
401 4749244 : if (level == NavEKF3::LogLevel::NONE) { // no logging from EK3_LOG_LEVEL param
402 0 : return;
403 : }
404 4749244 : Log_Write_XKF4(time_us);
405 4749244 : if (level == NavEKF3::LogLevel::XKF4) { // only log XKF4 scaled innovations
406 : return;
407 : }
408 4749244 : Log_Write_GSF(time_us);
409 4749244 : if (level == NavEKF3::LogLevel::XKF4_GSF) { // only log XKF4 scaled innovations and GSF, otherwise log everything
410 : return;
411 : }
412 : // note that several of these functions exit-early if they're not
413 : // attempting to log the primary core.
414 4749244 : Log_Write_XKF1(time_us);
415 4749244 : Log_Write_XKF2(time_us);
416 4749244 : Log_Write_XKF3(time_us);
417 4749244 : Log_Write_XKF5(time_us);
418 :
419 4749244 : Log_Write_XKFS(time_us);
420 4749244 : Log_Write_Quaternion(time_us);
421 :
422 :
423 : #if EK3_FEATURE_BEACON_FUSION
424 : // write range beacon fusion debug packet if the range value is non-zero
425 4749244 : Log_Write_Beacon(time_us);
426 : #endif
427 :
428 : #if EK3_FEATURE_BODY_ODOM
429 : // write debug data for body frame odometry fusion
430 4749244 : Log_Write_BodyOdom(time_us);
431 : #endif
432 :
433 : // log state variances every 0.49s
434 4749244 : Log_Write_State_Variances(time_us);
435 :
436 4749244 : Log_Write_Timing(time_us);
437 : }
438 :
439 4749244 : void NavEKF3_core::Log_Write_Timing(uint64_t time_us)
440 : {
441 : // log EKF timing statistics every 5s
442 4749244 : if (AP::dal().millis() - lastTimingLogTime_ms <= 5000) {
443 4692646 : return;
444 : }
445 56598 : lastTimingLogTime_ms = AP::dal().millis();
446 :
447 56598 : const struct log_XKT xkt{
448 : LOG_PACKET_HEADER_INIT(LOG_XKT_MSG),
449 : time_us : time_us,
450 56598 : core : core_index,
451 56598 : timing_count : timing.count,
452 56598 : dtIMUavg_min : timing.dtIMUavg_min,
453 56598 : dtIMUavg_max : timing.dtIMUavg_max,
454 56598 : dtEKFavg_min : timing.dtEKFavg_min,
455 56598 : dtEKFavg_max : timing.dtEKFavg_max,
456 56598 : delAngDT_min : timing.delAngDT_min,
457 56598 : delAngDT_max : timing.delAngDT_max,
458 56598 : delVelDT_min : timing.delVelDT_min,
459 56598 : delVelDT_max : timing.delVelDT_max,
460 56598 : };
461 56598 : memset(&timing, 0, sizeof(timing));
462 :
463 56598 : AP::logger().WriteBlock(&xkt, sizeof(xkt));
464 : }
465 :
466 4749244 : void NavEKF3_core::Log_Write_GSF(uint64_t time_us)
467 : {
468 4749244 : if (yawEstimator == nullptr) {
469 : return;
470 : }
471 4746498 : yawEstimator->Log_Write(time_us, LOG_XKY0_MSG, LOG_XKY1_MSG, DAL_CORE(core_index));
472 : }
473 :
474 : #endif // HAL_LOGGING_ENABLED
|
