Line data Source code
1 : #include <AP_Logger/AP_Logger_config.h>
2 :
3 : #if HAL_LOGGING_ENABLED
4 :
5 : #include "AP_NavEKF2.h"
6 : #include "AP_NavEKF2_core.h"
7 :
8 : #include <AP_Logger/AP_Logger.h>
9 : #include <AP_DAL/AP_DAL.h>
10 :
11 1567096 : void NavEKF2_core::Log_Write_NKF1(uint64_t time_us) const
12 : {
13 : // Write first EKF packet
14 1567096 : Vector3f euler;
15 1567096 : Vector2p posNE;
16 : postype_t posD;
17 1567096 : Vector3f velNED;
18 1567096 : Vector3f gyroBias;
19 : float posDownDeriv;
20 1567096 : Location originLLH;
21 1567096 : getEulerAngles(euler);
22 1567096 : getVelNED(velNED);
23 1567096 : getPosNE(posNE);
24 1567096 : getPosD(posD);
25 1567096 : getGyroBias(gyroBias);
26 1567096 : posDownDeriv = getPosDownDerivative();
27 1567096 : if (!getOriginLLH(originLLH)) {
28 179146 : originLLH.alt = 0;
29 : }
30 1567096 : const struct log_NKF1 pkt{
31 : LOG_PACKET_HEADER_INIT(LOG_NKF1_MSG),
32 : time_us : time_us,
33 1567096 : core : DAL_CORE(core_index),
34 3134192 : roll : (int16_t)(100*degrees(euler.x)), // roll angle (centi-deg, displayed as deg due to format string)
35 3134192 : pitch : (int16_t)(100*degrees(euler.y)), // pitch angle (centi-deg, displayed as deg due to format string)
36 1567096 : yaw : (uint16_t)wrap_360_cd(100*degrees(euler.z)), // yaw angle (centi-deg, displayed as deg due to format string)
37 1567096 : velN : (float)(velNED.x), // velocity North (m/s)
38 1567096 : velE : (float)(velNED.y), // velocity East (m/s)
39 1567096 : velD : (float)(velNED.z), // velocity Down (m/s)
40 : posD_dot : (float)(posDownDeriv), // first derivative of down position
41 1567096 : posN : (float)(posNE.x), // metres North
42 1567096 : posE : (float)(posNE.y), // metres East
43 1567096 : posD : (float)(posD), // metres Down
44 3134192 : gyrX : (int16_t)(100*degrees(gyroBias.x)), // cd/sec, displayed as deg/sec due to format string
45 3134192 : gyrY : (int16_t)(100*degrees(gyroBias.y)), // cd/sec, displayed as deg/sec due to format string
46 3134192 : gyrZ : (int16_t)(100*degrees(gyroBias.z)), // cd/sec, displayed as deg/sec due to format string
47 1567096 : originHgt : originLLH.alt // WGS-84 altitude of EKF origin in cm
48 4701288 : };
49 1567096 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
50 1567096 : }
51 :
52 1567096 : void NavEKF2_core::Log_Write_NKF2(uint64_t time_us) const
53 : {
54 : // Write second EKF packet
55 1567096 : float azbias = 0;
56 1567096 : Vector3f wind;
57 1567096 : Vector3f magNED;
58 1567096 : Vector3f magXYZ;
59 1567096 : Vector3f gyroScaleFactor;
60 1567096 : getAccelZBias(azbias);
61 1567096 : getWind(wind);
62 1567096 : getMagNED(magNED);
63 1567096 : getMagXYZ(magXYZ);
64 1567096 : getGyroScaleErrorPercentage(gyroScaleFactor);
65 1567096 : const struct log_NKF2 pkt2{
66 : LOG_PACKET_HEADER_INIT(LOG_NKF2_MSG),
67 : time_us : time_us,
68 1567096 : core : DAL_CORE(core_index),
69 1567096 : AZbias : (int8_t)(100*azbias),
70 1567096 : scaleX : (int16_t)(100*gyroScaleFactor.x),
71 1567096 : scaleY : (int16_t)(100*gyroScaleFactor.y),
72 1567096 : scaleZ : (int16_t)(100*gyroScaleFactor.z),
73 1567096 : windN : (int16_t)(100*wind.x),
74 1567096 : windE : (int16_t)(100*wind.y),
75 1567096 : magN : (int16_t)(magNED.x),
76 1567096 : magE : (int16_t)(magNED.y),
77 1567096 : magD : (int16_t)(magNED.z),
78 1567096 : magX : (int16_t)(magXYZ.x),
79 1567096 : magY : (int16_t)(magXYZ.y),
80 1567096 : magZ : (int16_t)(magXYZ.z),
81 1567096 : index : magSelectIndex
82 1567096 : };
83 1567096 : AP::logger().WriteBlock(&pkt2, sizeof(pkt2));
84 1567096 : }
85 :
86 1567096 : void NavEKF2_core::Log_Write_NKF3(uint64_t time_us) const
87 : {
88 : // Write third EKF packet
89 1567096 : Vector3f velInnov;
90 1567096 : Vector3f posInnov;
91 1567096 : Vector3f magInnov;
92 1567096 : float tasInnov = 0;
93 1567096 : float yawInnov = 0;
94 1567096 : getInnovations(velInnov, posInnov, magInnov, tasInnov, yawInnov);
95 1567096 : const struct log_NKF3 pkt3{
96 : LOG_PACKET_HEADER_INIT(LOG_NKF3_MSG),
97 : time_us : time_us,
98 1567096 : core : DAL_CORE(core_index),
99 1567096 : innovVN : (int16_t)(100*velInnov.x),
100 1567096 : innovVE : (int16_t)(100*velInnov.y),
101 1567096 : innovVD : (int16_t)(100*velInnov.z),
102 1567096 : innovPN : (int16_t)(100*posInnov.x),
103 1567096 : innovPE : (int16_t)(100*posInnov.y),
104 1567096 : innovPD : (int16_t)(100*posInnov.z),
105 1567096 : innovMX : (int16_t)(magInnov.x),
106 1567096 : innovMY : (int16_t)(magInnov.y),
107 1567096 : innovMZ : (int16_t)(magInnov.z),
108 3134192 : innovYaw : (int16_t)(100*degrees(yawInnov)),
109 1567096 : innovVT : (int16_t)(100*tasInnov),
110 : rerr : 0, // TODO : Relative Error based Lane-Switching like EK3
111 : errorScore : 0 // TODO : Relative Error based Lane-Switching like EK3
112 3134192 : };
113 1567096 : AP::logger().WriteBlock(&pkt3, sizeof(pkt3));
114 1567096 : }
115 :
116 1567096 : void NavEKF2_core::Log_Write_NKF4(uint64_t time_us) const
117 : {
118 : // Write fourth EKF packet
119 1567096 : float velVar = 0;
120 1567096 : float posVar = 0;
121 1567096 : float hgtVar = 0;
122 1567096 : Vector3f magVar;
123 1567096 : float tasVar = 0;
124 1567096 : Vector2f offset;
125 1567096 : uint16_t _faultStatus=0;
126 1567096 : const uint8_t timeoutStatus =
127 1567096 : posTimeout<<0 |
128 1567096 : velTimeout<<1 |
129 1567096 : hgtTimeout<<2 |
130 1567096 : magTimeout<<3 |
131 1567096 : tasTimeout<<4;
132 :
133 1567096 : nav_filter_status solutionStatus {};
134 1567096 : nav_gps_status gpsStatus {};
135 1567096 : getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
136 1567096 : ftype tempVar = fmaxF(fmaxF(magVar.x,magVar.y),magVar.z);
137 1567096 : getFilterFaults(_faultStatus);
138 1567096 : getFilterStatus(solutionStatus);
139 1567096 : getFilterGpsStatus(gpsStatus);
140 1567096 : const struct log_NKF4 pkt4{
141 : LOG_PACKET_HEADER_INIT(LOG_NKF4_MSG),
142 : time_us : time_us,
143 1567096 : core : DAL_CORE(core_index),
144 1567096 : sqrtvarV : (int16_t)(100*velVar),
145 1567096 : sqrtvarP : (int16_t)(100*posVar),
146 1567096 : sqrtvarH : (int16_t)(100*hgtVar),
147 1567096 : sqrtvarM : (int16_t)(100*tempVar),
148 1567096 : sqrtvarVT : (int16_t)(100*tasVar),
149 1567096 : tiltErr : float(tiltErrFilt), // tilt error convergence metric
150 1567096 : offsetNorth : offset.x,
151 1567096 : offsetEast : offset.y,
152 : faults : _faultStatus,
153 : timeouts : (uint8_t)(timeoutStatus),
154 1567096 : solution : (uint32_t)(solutionStatus.value),
155 1567096 : gps : (uint16_t)(gpsStatus.value),
156 1567096 : primary : frontend->getPrimaryCoreIndex()
157 3134192 : };
158 1567096 : AP::logger().WriteBlock(&pkt4, sizeof(pkt4));
159 1567096 : }
160 :
161 1567096 : void NavEKF2_core::Log_Write_NKF5(uint64_t time_us) const
162 : {
163 1567096 : if (core_index != frontend->primary) {
164 : // log only primary instance for now
165 783548 : return;
166 : }
167 :
168 : // Write fifth EKF packet
169 783548 : const struct log_NKF5 pkt5{
170 : LOG_PACKET_HEADER_INIT(LOG_NKF5_MSG),
171 : time_us : time_us,
172 783548 : core : DAL_CORE(core_index),
173 783548 : 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
174 783548 : FIX : (int16_t)(1000*innovOptFlow[0]), // optical flow LOS rate vector innovations from the main nav filter
175 783548 : FIY : (int16_t)(1000*innovOptFlow[1]), // optical flow LOS rate vector innovations from the main nav filter
176 1567096 : AFI : (int16_t)(1000 * auxFlowObsInnov.length()), // optical flow LOS rate innovation from terrain offset estimator
177 783548 : HAGL : float_to_int16(100*(terrainState - stateStruct.position.z)), // height above ground level
178 783548 : offset : (int16_t)(100*terrainState), // // estimated vertical position of the terrain relative to the nav filter zero datum
179 783548 : RI : (int16_t)(100*innovRng), // range finder innovations
180 783548 : meaRng : (uint16_t)(100*rangeDataDelayed.rng), // measured range
181 783548 : errHAGL : (uint16_t)(100*sqrtF(Popt)), // filter ground offset state error
182 783548 : angErr : float(outputTrackError.x),
183 783548 : velErr : float(outputTrackError.y),
184 783548 : posErr : float(outputTrackError.z)
185 2350644 : };
186 783548 : AP::logger().WriteBlock(&pkt5, sizeof(pkt5));
187 : }
188 :
189 1567096 : void NavEKF2_core::Log_Write_Quaternion(uint64_t time_us) const
190 : {
191 : // log quaternion
192 1567096 : Quaternion quat;
193 1567096 : getQuaternion(quat);
194 1567096 : const struct log_NKQ pktq1{
195 : LOG_PACKET_HEADER_INIT(LOG_NKQ_MSG),
196 : time_us : time_us,
197 1567096 : core : DAL_CORE(core_index),
198 1567096 : q1 : quat.q1,
199 1567096 : q2 : quat.q2,
200 1567096 : q3 : quat.q3,
201 1567096 : q4 : quat.q4
202 1567096 : };
203 1567096 : AP::logger().WriteBlock(&pktq1, sizeof(pktq1));
204 1567096 : }
205 :
206 : #if AP_BEACON_ENABLED
207 1567096 : void NavEKF2_core::Log_Write_Beacon(uint64_t time_us)
208 : {
209 1567096 : if (core_index != frontend->primary) {
210 : // log only primary instance for now
211 1567096 : return;
212 : }
213 :
214 783548 : if (AP::beacon() == nullptr) {
215 : return;
216 : }
217 :
218 783548 : if (!statesInitialised || N_beacons == 0) {
219 : return;
220 : }
221 :
222 : // Ensure that beacons are not skipped due to calling this
223 : // function at a rate lower than the updates
224 0 : if (rngBcnFuseDataReportIndex >= N_beacons) {
225 0 : rngBcnFuseDataReportIndex = 0;
226 : }
227 :
228 0 : const rngBcnFusionReport_t &report = rngBcnFusionReport[rngBcnFuseDataReportIndex];
229 :
230 0 : if (report.rng <= 0.0f) {
231 0 : rngBcnFuseDataReportIndex++;
232 0 : return;
233 : }
234 :
235 0 : struct log_NKF0 pkt0 = {
236 : LOG_PACKET_HEADER_INIT(LOG_NKF0_MSG),
237 : time_us : time_us,
238 0 : core : DAL_CORE(core_index),
239 0 : ID : rngBcnFuseDataReportIndex,
240 0 : rng : (int16_t)(100*report.rng),
241 0 : innov : (int16_t)(100*report.innov),
242 0 : sqrtInnovVar : (uint16_t)(100*safe_sqrt(report.innovVar)),
243 0 : testRatio : (uint16_t)(100*constrain_ftype(report.testRatio,0.0f,650.0f)),
244 0 : beaconPosN : (int16_t)(100*report.beaconPosNED.x),
245 0 : beaconPosE : (int16_t)(100*report.beaconPosNED.y),
246 0 : beaconPosD : (int16_t)(100*report.beaconPosNED.z),
247 0 : offsetHigh : (int16_t)(100*bcnPosOffsetMax),
248 0 : offsetLow : (int16_t)(100*bcnPosOffsetMin),
249 : posN : 0,
250 : posE : 0,
251 : posD : 0
252 0 : };
253 0 : AP::logger().WriteBlock(&pkt0, sizeof(pkt0));
254 0 : rngBcnFuseDataReportIndex++;
255 : }
256 : #endif // AP_BEACON_ENABLED
257 :
258 1567096 : void NavEKF2_core::Log_Write_Timing(uint64_t time_us)
259 : {
260 : // log EKF timing statistics every 5s
261 1567096 : if (AP::dal().millis() - lastTimingLogTime_ms <= 5000) {
262 1554166 : return;
263 : }
264 12930 : lastTimingLogTime_ms = AP::dal().millis();
265 :
266 12930 : const struct log_NKT nkt{
267 : LOG_PACKET_HEADER_INIT(LOG_NKT_MSG),
268 : time_us : time_us,
269 12930 : core : core_index,
270 12930 : timing_count : timing.count,
271 12930 : dtIMUavg_min : timing.dtIMUavg_min,
272 12930 : dtIMUavg_max : timing.dtIMUavg_max,
273 12930 : dtEKFavg_min : timing.dtEKFavg_min,
274 12930 : dtEKFavg_max : timing.dtEKFavg_max,
275 12930 : delAngDT_min : timing.delAngDT_min,
276 12930 : delAngDT_max : timing.delAngDT_max,
277 12930 : delVelDT_min : timing.delVelDT_min,
278 12930 : delVelDT_max : timing.delVelDT_max,
279 12930 : };
280 12930 : memset(&timing, 0, sizeof(timing));
281 :
282 12930 : AP::logger().WriteBlock(&nkt, sizeof(nkt));
283 : }
284 :
285 :
286 2461314 : void NavEKF2::Log_Write()
287 : {
288 : // only log if enabled
289 2461314 : if (activeCores() <= 0) {
290 : return;
291 : }
292 :
293 783577 : if (lastLogWrite_us == imuSampleTime_us) {
294 : // vehicle is doubling up on logging
295 : return;
296 : }
297 783548 : lastLogWrite_us = imuSampleTime_us;
298 :
299 783548 : const uint64_t time_us = AP::dal().micros64();
300 :
301 : // note that several of these functions exit-early if they're not
302 : // attempting to log the primary core.
303 2350644 : for (uint8_t i=0; i<activeCores(); i++) {
304 1567096 : core[i].Log_Write(time_us);
305 : }
306 :
307 783548 : AP::dal().start_frame(AP_DAL::FrameType::LogWriteEKF2);
308 : }
309 :
310 1567096 : void NavEKF2_core::Log_Write(uint64_t time_us)
311 : {
312 : // note that several of these functions exit-early if they're not
313 : // attempting to log the primary core.
314 1567096 : Log_Write_NKF1(time_us);
315 1567096 : Log_Write_NKF2(time_us);
316 1567096 : Log_Write_NKF3(time_us);
317 1567096 : Log_Write_NKF4(time_us);
318 1567096 : Log_Write_NKF5(time_us);
319 :
320 1567096 : Log_Write_Quaternion(time_us);
321 1567096 : Log_Write_GSF(time_us);
322 :
323 : #if AP_BEACON_ENABLED
324 : // write range beacon fusion debug packet if the range value is non-zero
325 1567096 : Log_Write_Beacon(time_us);
326 : #endif
327 :
328 1567096 : Log_Write_Timing(time_us);
329 1567096 : }
330 :
331 1567096 : void NavEKF2_core::Log_Write_GSF(uint64_t time_us) const
332 : {
333 1567096 : if (yawEstimator == nullptr) {
334 : return;
335 : }
336 1567096 : yawEstimator->Log_Write(time_us, LOG_NKY0_MSG, LOG_NKY1_MSG, DAL_CORE(core_index));
337 : }
338 :
339 : #endif // HAL_LOGGING_ENABLED
|
