Line data Source code
1 : #include "AP_Logger_config.h"
2 :
3 : #if HAL_LOGGING_ENABLED
4 :
5 : #include <stdlib.h>
6 :
7 : #include <AP_AHRS/AP_AHRS.h>
8 : #include <AP_HAL/AP_HAL.h>
9 : #include <AP_Math/AP_Math.h>
10 : #include <AP_Param/AP_Param.h>
11 : #include <AP_RSSI/AP_RSSI.h>
12 : #include <RC_Channel/RC_Channel.h>
13 : #include <SRV_Channel/SRV_Channel.h>
14 : #include <AC_PID/AP_PIDInfo.h>
15 :
16 : #include "AP_Logger.h"
17 : #include "AP_Logger_File.h"
18 : #include "AP_Logger_MAVLink.h"
19 : #include "LoggerMessageWriter.h"
20 : #include <AP_RCProtocol/AP_RCProtocol.h>
21 :
22 : extern const AP_HAL::HAL& hal;
23 :
24 :
25 : /*
26 : write a structure format to the log - should be in frontend
27 : */
28 2091674 : void AP_Logger_Backend::Fill_Format(const struct LogStructure *s, struct log_Format &pkt)
29 : {
30 2091674 : memset(&pkt, 0, sizeof(pkt));
31 2091674 : pkt.head1 = HEAD_BYTE1;
32 2091674 : pkt.head2 = HEAD_BYTE2;
33 2091674 : pkt.msgid = LOG_FORMAT_MSG;
34 2091674 : pkt.type = s->msg_type;
35 2091674 : pkt.length = s->msg_len;
36 2091674 : strncpy_noterm(pkt.name, s->name, sizeof(pkt.name));
37 2091674 : strncpy_noterm(pkt.format, s->format, sizeof(pkt.format));
38 2091674 : strncpy_noterm(pkt.labels, s->labels, sizeof(pkt.labels));
39 2091674 : }
40 :
41 : /*
42 : Pack a LogStructure packet into a structure suitable to go to the logfile:
43 : */
44 417362 : void AP_Logger_Backend::Fill_Format_Units(const struct LogStructure *s, struct log_Format_Units &pkt)
45 : {
46 417362 : memset(&pkt, 0, sizeof(pkt));
47 417362 : pkt.head1 = HEAD_BYTE1;
48 417362 : pkt.head2 = HEAD_BYTE2;
49 417362 : pkt.msgid = LOG_FORMAT_UNITS_MSG;
50 417362 : pkt.time_us = AP_HAL::micros64();
51 417362 : pkt.format_type = s->msg_type;
52 417362 : strncpy_noterm(pkt.units, s->units, sizeof(pkt.units));
53 417362 : strncpy_noterm(pkt.multipliers, s->multipliers, sizeof(pkt.multipliers));
54 417362 : }
55 :
56 : /*
57 : write a structure format to the log
58 : */
59 2091674 : bool AP_Logger_Backend::Write_Format(const struct LogStructure *s)
60 : {
61 : struct log_Format pkt;
62 2091674 : Fill_Format(s, pkt);
63 2091674 : if (!WriteCriticalBlock(&pkt, sizeof(pkt))) {
64 1700813 : return false;
65 : }
66 390861 : _formats_written.set(s->msg_type);
67 390861 : return true;
68 : }
69 :
70 : /*
71 : write a unit definition
72 : */
73 78297 : bool AP_Logger_Backend::Write_Unit(const struct UnitStructure *s)
74 : {
75 78297 : struct log_Unit pkt{
76 : LOG_PACKET_HEADER_INIT(LOG_UNIT_MSG),
77 78297 : time_us : AP_HAL::micros64(),
78 78297 : type : s->ID,
79 : unit : { }
80 78297 : };
81 78297 : strncpy_noterm(pkt.unit, s->unit, sizeof(pkt.unit));
82 :
83 78297 : return WriteCriticalBlock(&pkt, sizeof(pkt));
84 : }
85 :
86 : /*
87 : write a unit-multiplier definition
88 : */
89 30856 : bool AP_Logger_Backend::Write_Multiplier(const struct MultiplierStructure *s)
90 : {
91 30856 : const struct log_Format_Multiplier pkt{
92 : LOG_PACKET_HEADER_INIT(LOG_MULT_MSG),
93 30856 : time_us : AP_HAL::micros64(),
94 30856 : type : s->ID,
95 30856 : multiplier : s->multiplier,
96 30856 : };
97 :
98 30856 : return WriteCriticalBlock(&pkt, sizeof(pkt));
99 : }
100 :
101 : /*
102 : write the units for a format to the log
103 : */
104 417362 : bool AP_Logger_Backend::Write_Format_Units(const struct LogStructure *s)
105 : {
106 : struct log_Format_Units pkt;
107 417362 : Fill_Format_Units(s, pkt);
108 417362 : return WriteCriticalBlock(&pkt, sizeof(pkt));
109 : }
110 :
111 : /*
112 : write a parameter to the log
113 : */
114 3006662 : bool AP_Logger_Backend::Write_Parameter(const char *name, float value, float default_val)
115 : {
116 3006662 : struct log_Parameter pkt{
117 : LOG_PACKET_HEADER_INIT(LOG_PARAMETER_MSG),
118 3006662 : time_us : AP_HAL::micros64(),
119 : name : {},
120 : value : value,
121 : default_value : default_val
122 3006662 : };
123 3006662 : strncpy_noterm(pkt.name, name, sizeof(pkt.name));
124 3006662 : return WriteCriticalBlock(&pkt, sizeof(pkt));
125 : }
126 :
127 : /*
128 : write a parameter to the log
129 : */
130 2909129 : bool AP_Logger_Backend::Write_Parameter(const AP_Param *ap,
131 : const AP_Param::ParamToken &token,
132 : enum ap_var_type type,
133 : float default_val)
134 : {
135 : char name[16];
136 2909129 : ap->copy_name_token(token, &name[0], sizeof(name), true);
137 2909129 : return Write_Parameter(name, ap->cast_to_float(type), default_val);
138 : }
139 :
140 : #if AP_RC_CHANNEL_ENABLED
141 : // Write an RCIN packet
142 2200594 : void AP_Logger::Write_RCIN(void)
143 : {
144 2200594 : uint16_t values[16] = {};
145 2200594 : rc().get_radio_in(values, ARRAY_SIZE(values));
146 2200594 : const struct log_RCIN pkt{
147 : LOG_PACKET_HEADER_INIT(LOG_RCIN_MSG),
148 2200594 : time_us : AP_HAL::micros64(),
149 2200594 : chan1 : values[0],
150 2200594 : chan2 : values[1],
151 2200594 : chan3 : values[2],
152 2200594 : chan4 : values[3],
153 2200594 : chan5 : values[4],
154 2200594 : chan6 : values[5],
155 2200594 : chan7 : values[6],
156 2200594 : chan8 : values[7],
157 2200594 : chan9 : values[8],
158 2200594 : chan10 : values[9],
159 2200594 : chan11 : values[10],
160 2200594 : chan12 : values[11],
161 2200594 : chan13 : values[12],
162 2200594 : chan14 : values[13]
163 2200594 : };
164 2200594 : WriteBlock(&pkt, sizeof(pkt));
165 :
166 2200594 : uint8_t flags = 0;
167 2200594 : if (rc().has_valid_input()) {
168 2188092 : flags |= (uint8_t)AP_Logger::RCLoggingFlags::HAS_VALID_INPUT;
169 : }
170 2200594 : if (rc().in_rc_failsafe()) {
171 10031 : flags |= (uint8_t)AP_Logger::RCLoggingFlags::IN_RC_FAILSAFE;
172 : }
173 : #if AP_RCPROTOCOL_ENABLED
174 2200594 : if (AP::RC().failsafe_active()) {
175 308 : flags |= (uint8_t)AP_Logger::RCLoggingFlags::RC_PROTOCOL_FAILSAFE;
176 : }
177 : #endif // AP_RCPROTOCOL_ENABLED
178 :
179 2200594 : const struct log_RCI2 pkt2{
180 : LOG_PACKET_HEADER_INIT(LOG_RCI2_MSG),
181 2200594 : time_us : AP_HAL::micros64(),
182 2200594 : chan15 : values[14],
183 2200594 : chan16 : values[15],
184 2200594 : override_mask : rc().get_override_mask(),
185 : flags : flags,
186 2200594 : };
187 2200594 : WriteBlock(&pkt2, sizeof(pkt2));
188 2200594 : }
189 : #endif // AP_RC_CHANNEL_ENABLED
190 :
191 : // Write an SERVO packet
192 2198823 : void AP_Logger::Write_RCOUT(void)
193 : {
194 2198823 : const uint32_t enabled_mask = ~SRV_Channels::get_output_channel_mask(SRV_Channel::k_GPIO);
195 :
196 2198823 : if ((enabled_mask & 0x3FFF) != 0) {
197 2198823 : uint16_t channels[14] {};
198 2198823 : hal.rcout->read(channels, ARRAY_SIZE(channels));
199 2198823 : const struct log_RCOUT pkt{
200 : LOG_PACKET_HEADER_INIT(LOG_RCOUT_MSG),
201 2198823 : time_us : AP_HAL::micros64(),
202 2198823 : chan1 : channels[0],
203 2198823 : chan2 : channels[1],
204 2198823 : chan3 : channels[2],
205 2198823 : chan4 : channels[3],
206 2198823 : chan5 : channels[4],
207 2198823 : chan6 : channels[5],
208 2198823 : chan7 : channels[6],
209 2198823 : chan8 : channels[7],
210 2198823 : chan9 : channels[8],
211 2198823 : chan10 : channels[9],
212 2198823 : chan11 : channels[10],
213 2198823 : chan12 : channels[11],
214 2198823 : chan13 : channels[12],
215 2198823 : chan14 : channels[13]
216 2198823 : };
217 2198823 : WriteBlock(&pkt, sizeof(pkt));
218 : }
219 :
220 : #if NUM_SERVO_CHANNELS >= 15
221 2198823 : if ((enabled_mask & 0x3C000) != 0) {
222 2198823 : const struct log_RCOUT2 pkt2{
223 : LOG_PACKET_HEADER_INIT(LOG_RCOUT2_MSG),
224 2198823 : time_us : AP_HAL::micros64(),
225 2198823 : chan15 : hal.rcout->read(14),
226 2198823 : chan16 : hal.rcout->read(15),
227 2198823 : chan17 : hal.rcout->read(16),
228 2198823 : chan18 : hal.rcout->read(17),
229 2198823 : };
230 2198823 : WriteBlock(&pkt2, sizeof(pkt2));
231 : }
232 : #endif
233 :
234 : #if NUM_SERVO_CHANNELS >= 19
235 2198823 : if ((enabled_mask & 0xFFFC0000) != 0) {
236 824 : const struct log_RCOUT pkt3{
237 : LOG_PACKET_HEADER_INIT(LOG_RCOUT3_MSG),
238 824 : time_us : AP_HAL::micros64(),
239 824 : chan1 : hal.rcout->read(18),
240 824 : chan2 : hal.rcout->read(19),
241 824 : chan3 : hal.rcout->read(20),
242 824 : chan4 : hal.rcout->read(21),
243 824 : chan5 : hal.rcout->read(22),
244 824 : chan6 : hal.rcout->read(23),
245 824 : chan7 : hal.rcout->read(24),
246 824 : chan8 : hal.rcout->read(25),
247 824 : chan9 : hal.rcout->read(26),
248 824 : chan10 : hal.rcout->read(27),
249 824 : chan11 : hal.rcout->read(28),
250 824 : chan12 : hal.rcout->read(29),
251 824 : chan13 : hal.rcout->read(30),
252 824 : chan14 : hal.rcout->read(31)
253 824 : };
254 824 : WriteBlock(&pkt3, sizeof(pkt3));
255 : }
256 : #endif
257 :
258 2198823 : }
259 :
260 : #if AP_RSSI_ENABLED
261 : // Write an RSSI packet
262 0 : void AP_Logger::Write_RSSI()
263 : {
264 0 : AP_RSSI *rssi = AP::rssi();
265 0 : if (rssi == nullptr) {
266 0 : return;
267 : }
268 :
269 0 : const struct log_RSSI pkt{
270 : LOG_PACKET_HEADER_INIT(LOG_RSSI_MSG),
271 0 : time_us : AP_HAL::micros64(),
272 0 : RXRSSI : rssi->read_receiver_rssi(),
273 0 : RXLQ : rssi->read_receiver_link_quality()
274 0 : };
275 0 : WriteBlock(&pkt, sizeof(pkt));
276 : }
277 : #endif
278 :
279 529515 : void AP_Logger::Write_Command(const mavlink_command_int_t &packet,
280 : uint8_t source_system,
281 : uint8_t source_component,
282 : const MAV_RESULT result,
283 : bool was_command_long)
284 : {
285 529515 : const struct log_MAVLink_Command pkt{
286 : LOG_PACKET_HEADER_INIT(LOG_MAVLINK_COMMAND_MSG),
287 529515 : time_us : AP_HAL::micros64(),
288 529515 : target_system : packet.target_system,
289 529515 : target_component: packet.target_component,
290 : source_system : source_system,
291 : source_component: source_component,
292 529515 : frame : packet.frame,
293 529515 : command : packet.command,
294 529515 : param1 : packet.param1,
295 529515 : param2 : packet.param2,
296 529515 : param3 : packet.param3,
297 529515 : param4 : packet.param4,
298 529515 : x : packet.x,
299 529515 : y : packet.y,
300 529515 : z : packet.z,
301 : result : (uint8_t)result,
302 : was_command_long:was_command_long,
303 529515 : };
304 529515 : return WriteBlock(&pkt, sizeof(pkt));
305 : }
306 :
307 12829 : bool AP_Logger_Backend::Write_Mission_Cmd(const AP_Mission &mission,
308 : const AP_Mission::Mission_Command &cmd,
309 : LogMessages msgid)
310 : {
311 12829 : mavlink_mission_item_int_t mav_cmd = {};
312 12829 : AP_Mission::mission_cmd_to_mavlink_int(cmd,mav_cmd);
313 12829 : const struct log_CMD pkt{
314 : LOG_PACKET_HEADER_INIT(msgid),
315 12829 : time_us : AP_HAL::micros64(),
316 12829 : command_total : mission.num_commands(),
317 12829 : sequence : mav_cmd.seq,
318 12829 : command : mav_cmd.command,
319 12829 : param1 : mav_cmd.param1,
320 12829 : param2 : mav_cmd.param2,
321 12829 : param3 : mav_cmd.param3,
322 12829 : param4 : mav_cmd.param4,
323 12829 : latitude : mav_cmd.x,
324 12829 : longitude : mav_cmd.y,
325 12829 : altitude : mav_cmd.z,
326 12829 : frame : mav_cmd.frame
327 12829 : };
328 12829 : return WriteBlock(&pkt, sizeof(pkt));
329 : }
330 :
331 : #if AP_MISSION_ENABLED
332 1287 : bool AP_Logger_Backend::Write_EntireMission()
333 : {
334 : // kick off asynchronous write:
335 1287 : return _startup_messagewriter->writeentiremission();
336 : }
337 : #endif
338 :
339 : // Write a text message to the log
340 284735 : bool AP_Logger_Backend::Write_Message(const char *message)
341 : {
342 : // i==0 here means we log an empty string if it is passed in:
343 284735 : const uint8_t id = AP::logger().get_MSG_id(); // there is a race condition on this ID; if a thread logs a message at the same time as the main thread then we can re-use this
344 :
345 284735 : uint8_t chunk_seq = 0;
346 552118 : for (uint8_t i=0; i == 0 || i<strlen(message); i += 64, chunk_seq++) {
347 284735 : const bool success = Write_MessageChunk(id, &message[i], chunk_seq);
348 284735 : if (i == 0 && !success) {
349 17352 : return false;
350 : }
351 : }
352 267383 : return true;
353 : }
354 1310203 : bool AP_Logger_Backend::Write_MessageChunk(uint8_t id, const char *messagechunk, uint8_t chunk_seq)
355 : {
356 1310203 : struct log_MSG pkt{
357 : LOG_PACKET_HEADER_INIT(LOG_MSG_MSG),
358 1310203 : time_us : AP_HAL::micros64(),
359 : id : id,
360 : chunk_seq : chunk_seq,
361 : msg : {}
362 1310203 : };
363 1310203 : const uint16_t remaining = strlen(messagechunk);
364 1310203 : strncpy_noterm(pkt.msg, messagechunk, MIN(sizeof(pkt.msg), remaining));
365 : // result comes from success writing the first chunk; this
366 : // prevents trying to write out the front of a message
367 : // repeatedly.
368 1310203 : const bool success = WriteCriticalBlock(&pkt, sizeof(pkt));
369 1310203 : if (chunk_seq == 0) {
370 737987 : return success;
371 : }
372 572216 : return true;
373 : }
374 :
375 1444293 : void AP_Logger::Write_Power(void)
376 : {
377 : #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
378 : uint8_t safety_and_armed = uint8_t(hal.util->safety_switch_state());
379 : if (hal.util->get_soft_armed()) {
380 : // encode armed state in bit 3
381 : safety_and_armed |= 1U<<2;
382 : }
383 : const uint64_t now = AP_HAL::micros64();
384 : const struct log_POWR powr_pkt{
385 : LOG_PACKET_HEADER_INIT(LOG_POWR_MSG),
386 : time_us : now,
387 : #if HAL_HAVE_BOARD_VOLTAGE
388 : Vcc : hal.analogin->board_voltage(),
389 : #else
390 : Vcc : quiet_nanf(),
391 : #endif
392 : #if HAL_HAVE_SERVO_VOLTAGE
393 : Vservo : hal.analogin->servorail_voltage(),
394 : #else
395 : Vservo : quiet_nanf(),
396 : #endif
397 : flags : hal.analogin->power_status_flags(),
398 : accumulated_flags : hal.analogin->accumulated_power_status_flags(),
399 : safety_and_arm : safety_and_armed,
400 : };
401 : WriteBlock(&powr_pkt, sizeof(powr_pkt));
402 :
403 : #if HAL_WITH_MCU_MONITORING
404 : const struct log_MCU mcu_pkt{
405 : LOG_PACKET_HEADER_INIT(LOG_MCU_MSG),
406 : time_us : now,
407 : MCU_temp : hal.analogin->mcu_temperature(),
408 : MCU_voltage : hal.analogin->mcu_voltage(),
409 : MCU_voltage_min : hal.analogin->mcu_voltage_min(),
410 : MCU_voltage_max : hal.analogin->mcu_voltage_max(),
411 : };
412 : WriteBlock(&mcu_pkt, sizeof(mcu_pkt));
413 : #endif
414 :
415 : #endif
416 1444293 : }
417 :
418 0 : void AP_Logger::Write_Radio(const mavlink_radio_t &packet)
419 : {
420 0 : const struct log_Radio pkt{
421 : LOG_PACKET_HEADER_INIT(LOG_RADIO_MSG),
422 0 : time_us : AP_HAL::micros64(),
423 0 : rssi : packet.rssi,
424 0 : remrssi : packet.remrssi,
425 0 : txbuf : packet.txbuf,
426 0 : noise : packet.noise,
427 0 : remnoise : packet.remnoise,
428 0 : rxerrors : packet.rxerrors,
429 0 : fixed : packet.fixed
430 0 : };
431 0 : WriteBlock(&pkt, sizeof(pkt));
432 0 : }
433 :
434 : // Write a mode packet.
435 9720 : bool AP_Logger_Backend::Write_Mode(uint8_t mode, const ModeReason reason)
436 : {
437 : static_assert(sizeof(ModeReason) <= sizeof(uint8_t), "Logging expects the ModeReason to fit in 8 bits");
438 9720 : const struct log_Mode pkt{
439 : LOG_PACKET_HEADER_INIT(LOG_MODE_MSG),
440 9720 : time_us : AP_HAL::micros64(),
441 : mode : mode,
442 : mode_num : mode,
443 : mode_reason : static_cast<uint8_t>(reason)
444 9720 : };
445 9720 : return WriteCriticalBlock(&pkt, sizeof(pkt));
446 : }
447 :
448 :
449 : // emit an RTC message to the onboard logs
450 : #if AP_RTC_LOGGING_ENABLED
451 9085 : bool AP_Logger_Backend::Write_RTC()
452 : {
453 9085 : uint64_t time_unix = 0;
454 9085 : AP::rtc().get_utc_usec(time_unix); // may fail, leaving time_unix at 0
455 :
456 9085 : const struct log_RTC pkt{
457 : LOG_PACKET_HEADER_INIT(LOG_RTC_MSG),
458 9085 : time_us : AP_HAL::micros64(),
459 : epoch_us : time_unix,
460 9085 : source_type: uint8_t(AP::rtc().get_source_type()),
461 9085 : };
462 9085 : return WriteCriticalBlock(&pkt, sizeof(pkt));
463 : }
464 : #endif // AP_RTC_LOGGING_ENABLED
465 :
466 : // Write a Yaw PID packet
467 11679188 : void AP_Logger::Write_PID(uint8_t msg_type, const AP_PIDInfo &info)
468 : {
469 : enum class log_PID_Flags : uint8_t {
470 : LIMIT = 1U<<0, // true if the output is saturated, I term anti windup is active
471 : PD_SUM_LIMIT = 1U<<1, // true if the PD sum limit is active
472 : RESET = 1U<<2, // true if the controller was reset
473 : I_TERM_SET = 1U<<3, // true if the I term has been set externally including reseting to 0
474 : };
475 :
476 11679188 : uint8_t flags = 0;
477 11679188 : if (info.limit) {
478 1577471 : flags |= (uint8_t)log_PID_Flags::LIMIT;
479 : }
480 11679188 : if (info.PD_limit) {
481 0 : flags |= (uint8_t)log_PID_Flags::PD_SUM_LIMIT;
482 : }
483 11679188 : if (info.reset) {
484 188892 : flags |= (uint8_t)log_PID_Flags::RESET;
485 : }
486 11679188 : if (info.I_term_set) {
487 1493873 : flags |= (uint8_t)log_PID_Flags::I_TERM_SET;
488 : }
489 :
490 11679188 : const struct log_PID pkt{
491 : LOG_PACKET_HEADER_INIT(msg_type),
492 11679188 : time_us : AP_HAL::micros64(),
493 11679188 : target : info.target,
494 11679188 : actual : info.actual,
495 11679188 : error : info.error,
496 11679188 : P : info.P,
497 11679188 : I : info.I,
498 11679188 : D : info.D,
499 11679188 : FF : info.FF,
500 11679188 : DFF : info.DFF,
501 11679188 : Dmod : info.Dmod,
502 11679188 : slew_rate : info.slew_rate,
503 : flags : flags
504 11679188 : };
505 11679188 : WriteBlock(&pkt, sizeof(pkt));
506 11679188 : }
507 :
508 53633 : void AP_Logger::Write_SRTL(bool active, uint16_t num_points, uint16_t max_points, uint8_t action, const Vector3f& breadcrumb)
509 : {
510 53633 : const struct log_SRTL pkt_srtl{
511 : LOG_PACKET_HEADER_INIT(LOG_SRTL_MSG),
512 53633 : time_us : AP_HAL::micros64(),
513 53633 : active : active,
514 : num_points : num_points,
515 : max_points : max_points,
516 : action : action,
517 53633 : N : breadcrumb.x,
518 53633 : E : breadcrumb.y,
519 53633 : D : breadcrumb.z
520 53633 : };
521 53633 : WriteBlock(&pkt_srtl, sizeof(pkt_srtl));
522 53633 : }
523 :
524 1024 : void AP_Logger::Write_Winch(bool healthy, bool thread_end, bool moving, bool clutch, uint8_t mode, float desired_length, float length, float desired_rate, uint16_t tension, float voltage, int8_t temp)
525 : {
526 1024 : struct log_Winch pkt{
527 : LOG_PACKET_HEADER_INIT(LOG_WINCH_MSG),
528 1024 : time_us : AP_HAL::micros64(),
529 1024 : healthy : healthy,
530 1024 : thread_end : thread_end,
531 1024 : moving : moving,
532 1024 : clutch : clutch,
533 : mode : mode,
534 : desired_length : desired_length,
535 : length : length,
536 : desired_rate : desired_rate,
537 : tension : tension,
538 : voltage : voltage,
539 : temp : temp
540 1024 : };
541 1024 : WriteBlock(&pkt, sizeof(pkt));
542 1024 : }
543 :
544 : #endif // HAL_LOGGING_ENABLED
|
