libraries/AP_Logger/LogFile.cpp

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