2023-09-05 10:08:30 -03:00
|
|
|
#include "AP_Logger_config.h"
|
|
|
|
|
|
|
|
#if HAL_LOGGING_ENABLED
|
|
|
|
|
2013-04-19 04:49:16 -03:00
|
|
|
#include <stdlib.h>
|
2015-10-26 09:01:52 -03:00
|
|
|
|
2015-08-11 03:28:46 -03:00
|
|
|
#include <AP_AHRS/AP_AHRS.h>
|
|
|
|
#include <AP_Compass/AP_Compass.h>
|
2015-10-26 09:01:52 -03:00
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
#include <AP_Math/AP_Math.h>
|
|
|
|
#include <AP_Param/AP_Param.h>
|
2019-04-05 19:48:11 -03:00
|
|
|
#include <AP_RSSI/AP_RSSI.h>
|
2022-07-14 02:48:35 -03:00
|
|
|
#include <RC_Channel/RC_Channel.h>
|
|
|
|
#include <SRV_Channel/SRV_Channel.h>
|
2022-07-14 02:48:35 -03:00
|
|
|
#include <AC_PID/AP_PIDInfo.h>
|
2013-04-19 04:49:16 -03:00
|
|
|
|
2019-01-18 00:23:42 -04:00
|
|
|
#include "AP_Logger.h"
|
|
|
|
#include "AP_Logger_File.h"
|
|
|
|
#include "AP_Logger_MAVLink.h"
|
|
|
|
#include "LoggerMessageWriter.h"
|
2015-08-06 09:18:28 -03:00
|
|
|
|
2013-04-19 04:49:16 -03:00
|
|
|
extern const AP_HAL::HAL& hal;
|
2013-01-14 23:02:08 -04:00
|
|
|
|
|
|
|
|
2013-04-19 10:19:16 -03:00
|
|
|
/*
|
2015-06-25 10:53:20 -03:00
|
|
|
write a structure format to the log - should be in frontend
|
2013-04-19 10:19:16 -03:00
|
|
|
*/
|
2019-01-18 00:24:08 -04:00
|
|
|
void AP_Logger_Backend::Fill_Format(const struct LogStructure *s, struct log_Format &pkt)
|
2013-04-19 10:19:16 -03:00
|
|
|
{
|
2013-12-20 22:56:54 -04:00
|
|
|
memset(&pkt, 0, sizeof(pkt));
|
2013-12-16 20:12:42 -04:00
|
|
|
pkt.head1 = HEAD_BYTE1;
|
|
|
|
pkt.head2 = HEAD_BYTE2;
|
|
|
|
pkt.msgid = LOG_FORMAT_MSG;
|
2015-12-23 12:06:58 -04:00
|
|
|
pkt.type = s->msg_type;
|
|
|
|
pkt.length = s->msg_len;
|
2020-07-07 00:14:35 -03:00
|
|
|
strncpy_noterm(pkt.name, s->name, sizeof(pkt.name));
|
|
|
|
strncpy_noterm(pkt.format, s->format, sizeof(pkt.format));
|
|
|
|
strncpy_noterm(pkt.labels, s->labels, sizeof(pkt.labels));
|
2013-12-16 20:12:42 -04:00
|
|
|
}
|
|
|
|
|
2015-12-07 20:51:46 -04:00
|
|
|
/*
|
|
|
|
Pack a LogStructure packet into a structure suitable to go to the logfile:
|
|
|
|
*/
|
2019-01-18 00:24:08 -04:00
|
|
|
void AP_Logger_Backend::Fill_Format_Units(const struct LogStructure *s, struct log_Format_Units &pkt)
|
2015-12-07 20:51:46 -04:00
|
|
|
{
|
|
|
|
memset(&pkt, 0, sizeof(pkt));
|
|
|
|
pkt.head1 = HEAD_BYTE1;
|
|
|
|
pkt.head2 = HEAD_BYTE2;
|
|
|
|
pkt.msgid = LOG_FORMAT_UNITS_MSG;
|
|
|
|
pkt.time_us = AP_HAL::micros64();
|
|
|
|
pkt.format_type = s->msg_type;
|
2020-07-07 00:14:35 -03:00
|
|
|
strncpy_noterm(pkt.units, s->units, sizeof(pkt.units));
|
|
|
|
strncpy_noterm(pkt.multipliers, s->multipliers, sizeof(pkt.multipliers));
|
2015-12-07 20:51:46 -04:00
|
|
|
}
|
|
|
|
|
2013-12-16 20:12:42 -04:00
|
|
|
/*
|
|
|
|
write a structure format to the log
|
|
|
|
*/
|
2019-01-18 00:24:08 -04:00
|
|
|
bool AP_Logger_Backend::Write_Format(const struct LogStructure *s)
|
2013-12-16 20:12:42 -04:00
|
|
|
{
|
|
|
|
struct log_Format pkt;
|
2019-01-18 00:24:08 -04:00
|
|
|
Fill_Format(s, pkt);
|
2015-08-06 09:18:28 -03:00
|
|
|
return WriteCriticalBlock(&pkt, sizeof(pkt));
|
2013-04-19 10:19:16 -03:00
|
|
|
}
|
|
|
|
|
2015-12-07 20:51:46 -04:00
|
|
|
/*
|
|
|
|
write a unit definition
|
|
|
|
*/
|
2019-01-18 00:24:08 -04:00
|
|
|
bool AP_Logger_Backend::Write_Unit(const struct UnitStructure *s)
|
2015-12-07 20:51:46 -04:00
|
|
|
{
|
2019-07-02 06:31:02 -03:00
|
|
|
struct log_Unit pkt{
|
2015-12-07 20:51:46 -04:00
|
|
|
LOG_PACKET_HEADER_INIT(LOG_UNIT_MSG),
|
|
|
|
time_us : AP_HAL::micros64(),
|
|
|
|
type : s->ID,
|
|
|
|
unit : { }
|
|
|
|
};
|
2020-07-07 00:14:35 -03:00
|
|
|
strncpy_noterm(pkt.unit, s->unit, sizeof(pkt.unit));
|
2015-12-07 20:51:46 -04:00
|
|
|
|
|
|
|
return WriteCriticalBlock(&pkt, sizeof(pkt));
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
write a unit-multiplier definition
|
|
|
|
*/
|
2019-01-18 00:24:08 -04:00
|
|
|
bool AP_Logger_Backend::Write_Multiplier(const struct MultiplierStructure *s)
|
2015-12-07 20:51:46 -04:00
|
|
|
{
|
2019-07-02 06:31:02 -03:00
|
|
|
const struct log_Format_Multiplier pkt{
|
2015-12-07 20:51:46 -04:00
|
|
|
LOG_PACKET_HEADER_INIT(LOG_MULT_MSG),
|
|
|
|
time_us : AP_HAL::micros64(),
|
|
|
|
type : s->ID,
|
|
|
|
multiplier : s->multiplier,
|
|
|
|
};
|
|
|
|
|
|
|
|
return WriteCriticalBlock(&pkt, sizeof(pkt));
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
write the units for a format to the log
|
|
|
|
*/
|
2019-01-18 00:24:08 -04:00
|
|
|
bool AP_Logger_Backend::Write_Format_Units(const struct LogStructure *s)
|
2015-12-07 20:51:46 -04:00
|
|
|
{
|
|
|
|
struct log_Format_Units pkt;
|
2019-01-18 00:24:08 -04:00
|
|
|
Fill_Format_Units(s, pkt);
|
2015-12-07 20:51:46 -04:00
|
|
|
return WriteCriticalBlock(&pkt, sizeof(pkt));
|
|
|
|
}
|
|
|
|
|
2013-04-19 04:49:16 -03:00
|
|
|
/*
|
|
|
|
write a parameter to the log
|
|
|
|
*/
|
2022-06-13 16:02:15 -03:00
|
|
|
bool AP_Logger_Backend::Write_Parameter(const char *name, float value, float default_val)
|
2013-04-19 04:49:16 -03:00
|
|
|
{
|
2019-07-02 06:31:02 -03:00
|
|
|
struct log_Parameter pkt{
|
2013-04-19 04:49:16 -03:00
|
|
|
LOG_PACKET_HEADER_INIT(LOG_PARAMETER_MSG),
|
2015-11-19 23:15:08 -04:00
|
|
|
time_us : AP_HAL::micros64(),
|
2013-04-19 04:49:16 -03:00
|
|
|
name : {},
|
2022-06-13 16:02:15 -03:00
|
|
|
value : value,
|
|
|
|
default_value : default_val
|
2013-04-19 04:49:16 -03:00
|
|
|
};
|
2020-07-07 00:14:35 -03:00
|
|
|
strncpy_noterm(pkt.name, name, sizeof(pkt.name));
|
2015-08-06 09:18:28 -03:00
|
|
|
return WriteCriticalBlock(&pkt, sizeof(pkt));
|
2013-04-19 04:49:16 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
write a parameter to the log
|
|
|
|
*/
|
2019-01-18 00:24:08 -04:00
|
|
|
bool AP_Logger_Backend::Write_Parameter(const AP_Param *ap,
|
2015-11-09 18:14:22 -04:00
|
|
|
const AP_Param::ParamToken &token,
|
2022-06-13 16:02:15 -03:00
|
|
|
enum ap_var_type type,
|
|
|
|
float default_val)
|
2013-04-19 04:49:16 -03:00
|
|
|
{
|
|
|
|
char name[16];
|
|
|
|
ap->copy_name_token(token, &name[0], sizeof(name), true);
|
2022-06-13 16:02:15 -03:00
|
|
|
return Write_Parameter(name, ap->cast_to_float(type), default_val);
|
2013-04-19 04:49:16 -03:00
|
|
|
}
|
|
|
|
|
2023-03-19 07:02:24 -03:00
|
|
|
#if AP_RC_CHANNEL_ENABLED
|
2013-11-25 17:44:00 -04:00
|
|
|
// Write an RCIN packet
|
2019-01-18 00:24:08 -04:00
|
|
|
void AP_Logger::Write_RCIN(void)
|
2013-11-25 17:44:00 -04:00
|
|
|
{
|
2020-08-19 05:56:50 -03:00
|
|
|
uint16_t values[16] = {};
|
2018-08-08 21:34:15 -03:00
|
|
|
rc().get_radio_in(values, ARRAY_SIZE(values));
|
2019-07-02 06:31:02 -03:00
|
|
|
const struct log_RCIN pkt{
|
2013-11-25 17:44:00 -04:00
|
|
|
LOG_PACKET_HEADER_INIT(LOG_RCIN_MSG),
|
2015-11-19 23:15:08 -04:00
|
|
|
time_us : AP_HAL::micros64(),
|
2018-04-26 09:00:30 -03:00
|
|
|
chan1 : values[0],
|
|
|
|
chan2 : values[1],
|
|
|
|
chan3 : values[2],
|
|
|
|
chan4 : values[3],
|
|
|
|
chan5 : values[4],
|
|
|
|
chan6 : values[5],
|
|
|
|
chan7 : values[6],
|
|
|
|
chan8 : values[7],
|
|
|
|
chan9 : values[8],
|
|
|
|
chan10 : values[9],
|
|
|
|
chan11 : values[10],
|
|
|
|
chan12 : values[11],
|
|
|
|
chan13 : values[12],
|
|
|
|
chan14 : values[13]
|
2013-11-25 17:44:00 -04:00
|
|
|
};
|
|
|
|
WriteBlock(&pkt, sizeof(pkt));
|
2020-08-19 05:56:50 -03:00
|
|
|
|
2023-09-11 19:53:38 -03:00
|
|
|
uint8_t flags = 0;
|
|
|
|
if (rc().has_valid_input()) {
|
|
|
|
flags |= (uint8_t)AP_Logger::RCLoggingFlags::HAS_VALID_INPUT;
|
2021-05-15 00:45:43 -03:00
|
|
|
}
|
2023-09-11 19:53:38 -03:00
|
|
|
if (rc().in_rc_failsafe()) {
|
|
|
|
flags |= (uint8_t)AP_Logger::RCLoggingFlags::IN_RC_FAILSAFE;
|
2020-08-19 05:56:50 -03:00
|
|
|
}
|
|
|
|
|
2023-09-11 19:53:38 -03:00
|
|
|
const struct log_RCI2 pkt2{
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_RCI2_MSG),
|
2020-08-19 05:56:50 -03:00
|
|
|
time_us : AP_HAL::micros64(),
|
|
|
|
chan15 : values[14],
|
2021-05-15 00:45:43 -03:00
|
|
|
chan16 : values[15],
|
2023-09-11 19:53:38 -03:00
|
|
|
override_mask : rc().get_override_mask(),
|
|
|
|
flags : flags,
|
2020-08-19 05:56:50 -03:00
|
|
|
};
|
|
|
|
WriteBlock(&pkt2, sizeof(pkt2));
|
2013-11-25 17:44:00 -04:00
|
|
|
}
|
2023-03-19 07:02:24 -03:00
|
|
|
#endif // AP_RC_CHANNEL_ENABLED
|
2013-11-25 17:44:00 -04:00
|
|
|
|
|
|
|
// Write an SERVO packet
|
2019-01-18 00:24:08 -04:00
|
|
|
void AP_Logger::Write_RCOUT(void)
|
2013-11-25 17:44:00 -04:00
|
|
|
{
|
2022-01-02 13:40:51 -04:00
|
|
|
const uint32_t enabled_mask = ~SRV_Channels::get_output_channel_mask(SRV_Channel::k_GPIO);
|
|
|
|
|
|
|
|
if ((enabled_mask & 0x3FFF) != 0) {
|
2023-04-14 21:19:57 -03:00
|
|
|
uint16_t channels[14] {};
|
|
|
|
hal.rcout->read(channels, ARRAY_SIZE(channels));
|
2022-01-02 13:40:51 -04:00
|
|
|
const struct log_RCOUT pkt{
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_RCOUT_MSG),
|
|
|
|
time_us : AP_HAL::micros64(),
|
2023-04-14 21:19:57 -03:00
|
|
|
chan1 : channels[0],
|
|
|
|
chan2 : channels[1],
|
|
|
|
chan3 : channels[2],
|
|
|
|
chan4 : channels[3],
|
|
|
|
chan5 : channels[4],
|
|
|
|
chan6 : channels[5],
|
|
|
|
chan7 : channels[6],
|
|
|
|
chan8 : channels[7],
|
|
|
|
chan9 : channels[8],
|
|
|
|
chan10 : channels[9],
|
|
|
|
chan11 : channels[10],
|
|
|
|
chan12 : channels[11],
|
|
|
|
chan13 : channels[12],
|
|
|
|
chan14 : channels[13]
|
2022-01-02 13:40:51 -04:00
|
|
|
};
|
|
|
|
WriteBlock(&pkt, sizeof(pkt));
|
|
|
|
}
|
2021-01-11 21:44:07 -04:00
|
|
|
|
|
|
|
#if NUM_SERVO_CHANNELS >= 15
|
2022-01-02 13:40:51 -04:00
|
|
|
if ((enabled_mask & 0x3C000) != 0) {
|
|
|
|
const struct log_RCOUT2 pkt2{
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_RCOUT2_MSG),
|
|
|
|
time_us : AP_HAL::micros64(),
|
|
|
|
chan15 : hal.rcout->read(14),
|
|
|
|
chan16 : hal.rcout->read(15),
|
|
|
|
chan17 : hal.rcout->read(16),
|
|
|
|
chan18 : hal.rcout->read(17),
|
|
|
|
};
|
|
|
|
WriteBlock(&pkt2, sizeof(pkt2));
|
|
|
|
}
|
2021-01-11 21:44:07 -04:00
|
|
|
#endif
|
|
|
|
|
|
|
|
#if NUM_SERVO_CHANNELS >= 19
|
2022-01-02 13:40:51 -04:00
|
|
|
if ((enabled_mask & 0xFFFC0000) != 0) {
|
|
|
|
const struct log_RCOUT pkt3{
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_RCOUT3_MSG),
|
|
|
|
time_us : AP_HAL::micros64(),
|
|
|
|
chan1 : hal.rcout->read(18),
|
|
|
|
chan2 : hal.rcout->read(19),
|
|
|
|
chan3 : hal.rcout->read(20),
|
|
|
|
chan4 : hal.rcout->read(21),
|
|
|
|
chan5 : hal.rcout->read(22),
|
|
|
|
chan6 : hal.rcout->read(23),
|
|
|
|
chan7 : hal.rcout->read(24),
|
|
|
|
chan8 : hal.rcout->read(25),
|
|
|
|
chan9 : hal.rcout->read(26),
|
|
|
|
chan10 : hal.rcout->read(27),
|
|
|
|
chan11 : hal.rcout->read(28),
|
|
|
|
chan12 : hal.rcout->read(29),
|
|
|
|
chan13 : hal.rcout->read(30),
|
|
|
|
chan14 : hal.rcout->read(31)
|
|
|
|
};
|
|
|
|
WriteBlock(&pkt3, sizeof(pkt3));
|
|
|
|
}
|
2021-01-11 21:44:07 -04:00
|
|
|
#endif
|
|
|
|
|
2013-11-25 17:44:00 -04:00
|
|
|
}
|
|
|
|
|
2015-09-04 12:50:23 -03:00
|
|
|
// Write an RSSI packet
|
2019-04-05 19:48:11 -03:00
|
|
|
void AP_Logger::Write_RSSI()
|
2015-09-04 12:50:23 -03:00
|
|
|
{
|
2019-04-05 19:48:11 -03:00
|
|
|
AP_RSSI *rssi = AP::rssi();
|
|
|
|
if (rssi == nullptr) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2019-07-02 06:31:02 -03:00
|
|
|
const struct log_RSSI pkt{
|
2015-09-04 12:50:23 -03:00
|
|
|
LOG_PACKET_HEADER_INIT(LOG_RSSI_MSG),
|
2015-11-19 23:15:08 -04:00
|
|
|
time_us : AP_HAL::micros64(),
|
2021-07-13 13:03:00 -03:00
|
|
|
RXRSSI : rssi->read_receiver_rssi(),
|
|
|
|
RXLQ : rssi->read_receiver_link_quality()
|
2015-09-04 12:50:23 -03:00
|
|
|
};
|
|
|
|
WriteBlock(&pkt, sizeof(pkt));
|
|
|
|
}
|
|
|
|
|
2019-08-22 22:21:39 -03:00
|
|
|
void AP_Logger::Write_Command(const mavlink_command_int_t &packet,
|
2021-09-04 03:40:38 -03:00
|
|
|
uint8_t source_system,
|
|
|
|
uint8_t source_component,
|
2019-08-22 22:21:39 -03:00
|
|
|
const MAV_RESULT result,
|
|
|
|
bool was_command_long)
|
|
|
|
{
|
|
|
|
const struct log_MAVLink_Command pkt{
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_MAVLINK_COMMAND_MSG),
|
|
|
|
time_us : AP_HAL::micros64(),
|
|
|
|
target_system : packet.target_system,
|
|
|
|
target_component: packet.target_component,
|
2021-09-04 03:40:38 -03:00
|
|
|
source_system : source_system,
|
|
|
|
source_component: source_component,
|
2019-08-22 22:21:39 -03:00
|
|
|
frame : packet.frame,
|
|
|
|
command : packet.command,
|
|
|
|
param1 : packet.param1,
|
|
|
|
param2 : packet.param2,
|
|
|
|
param3 : packet.param3,
|
|
|
|
param4 : packet.param4,
|
|
|
|
x : packet.x,
|
|
|
|
y : packet.y,
|
|
|
|
z : packet.z,
|
|
|
|
result : (uint8_t)result,
|
|
|
|
was_command_long:was_command_long,
|
|
|
|
};
|
|
|
|
return WriteBlock(&pkt, sizeof(pkt));
|
|
|
|
}
|
|
|
|
|
2019-01-18 00:24:08 -04:00
|
|
|
bool AP_Logger_Backend::Write_Mission_Cmd(const AP_Mission &mission,
|
2015-11-09 18:14:22 -04:00
|
|
|
const AP_Mission::Mission_Command &cmd)
|
2015-06-30 01:33:50 -03:00
|
|
|
{
|
2018-11-24 23:10:07 -04:00
|
|
|
mavlink_mission_item_int_t mav_cmd = {};
|
|
|
|
AP_Mission::mission_cmd_to_mavlink_int(cmd,mav_cmd);
|
2019-07-02 06:31:02 -03:00
|
|
|
const struct log_Cmd pkt{
|
2018-11-24 23:10:07 -04:00
|
|
|
LOG_PACKET_HEADER_INIT(LOG_CMD_MSG),
|
|
|
|
time_us : AP_HAL::micros64(),
|
|
|
|
command_total : mission.num_commands(),
|
|
|
|
sequence : mav_cmd.seq,
|
|
|
|
command : mav_cmd.command,
|
|
|
|
param1 : mav_cmd.param1,
|
|
|
|
param2 : mav_cmd.param2,
|
|
|
|
param3 : mav_cmd.param3,
|
|
|
|
param4 : mav_cmd.param4,
|
|
|
|
latitude : mav_cmd.x,
|
|
|
|
longitude : mav_cmd.y,
|
|
|
|
altitude : mav_cmd.z,
|
|
|
|
frame : mav_cmd.frame
|
|
|
|
};
|
|
|
|
return WriteBlock(&pkt, sizeof(pkt));
|
2015-06-30 01:33:50 -03:00
|
|
|
}
|
|
|
|
|
2022-08-14 20:08:16 -03:00
|
|
|
#if AP_MISSION_ENABLED
|
2020-04-13 01:20:30 -03:00
|
|
|
bool AP_Logger_Backend::Write_EntireMission()
|
2015-06-30 01:33:50 -03:00
|
|
|
{
|
2020-04-13 01:20:30 -03:00
|
|
|
// kick off asynchronous write:
|
|
|
|
return _startup_messagewriter->writeentiremission();
|
2015-06-30 01:33:50 -03:00
|
|
|
}
|
2021-05-19 23:34:13 -03:00
|
|
|
#endif
|
2015-06-30 01:33:50 -03:00
|
|
|
|
2013-05-02 20:18:14 -03:00
|
|
|
// Write a text message to the log
|
2019-01-18 00:24:08 -04:00
|
|
|
bool AP_Logger_Backend::Write_Message(const char *message)
|
2013-05-02 20:18:14 -03:00
|
|
|
{
|
2019-07-02 06:31:02 -03:00
|
|
|
struct log_Message pkt{
|
2013-05-02 20:18:14 -03:00
|
|
|
LOG_PACKET_HEADER_INIT(LOG_MESSAGE_MSG),
|
2015-11-19 23:15:08 -04:00
|
|
|
time_us : AP_HAL::micros64(),
|
2013-05-02 20:18:14 -03:00
|
|
|
msg : {}
|
|
|
|
};
|
2020-07-07 00:14:35 -03:00
|
|
|
strncpy_noterm(pkt.msg, message, sizeof(pkt.msg));
|
2015-08-27 23:47:16 -03:00
|
|
|
return WriteCriticalBlock(&pkt, sizeof(pkt));
|
2013-05-02 20:18:14 -03:00
|
|
|
}
|
|
|
|
|
2019-01-18 00:24:08 -04:00
|
|
|
void AP_Logger::Write_Power(void)
|
2014-02-13 07:07:32 -04:00
|
|
|
{
|
2019-01-15 21:15:24 -04:00
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
|
2018-04-12 20:49:48 -03:00
|
|
|
uint8_t safety_and_armed = uint8_t(hal.util->safety_switch_state());
|
|
|
|
if (hal.util->get_soft_armed()) {
|
|
|
|
// encode armed state in bit 3
|
|
|
|
safety_and_armed |= 1U<<2;
|
|
|
|
}
|
2023-01-25 15:12:21 -04:00
|
|
|
const uint64_t now = AP_HAL::micros64();
|
|
|
|
const struct log_POWR powr_pkt{
|
2014-02-13 07:07:32 -04:00
|
|
|
LOG_PACKET_HEADER_INIT(LOG_POWR_MSG),
|
2023-01-25 15:12:21 -04:00
|
|
|
time_us : now,
|
|
|
|
#if HAL_HAVE_BOARD_VOLTAGE
|
2016-06-02 17:55:05 -03:00
|
|
|
Vcc : hal.analogin->board_voltage(),
|
2023-01-25 15:12:21 -04:00
|
|
|
#else
|
|
|
|
Vcc : quiet_nanf(),
|
|
|
|
#endif
|
|
|
|
#if HAL_HAVE_SERVO_VOLTAGE
|
2016-06-02 17:55:05 -03:00
|
|
|
Vservo : hal.analogin->servorail_voltage(),
|
2023-01-25 15:12:21 -04:00
|
|
|
#else
|
|
|
|
Vservo : quiet_nanf(),
|
|
|
|
#endif
|
2018-04-12 20:49:48 -03:00
|
|
|
flags : hal.analogin->power_status_flags(),
|
2020-07-19 21:34:43 -03:00
|
|
|
accumulated_flags : hal.analogin->accumulated_power_status_flags(),
|
2021-08-22 22:39:48 -03:00
|
|
|
safety_and_arm : safety_and_armed,
|
2014-02-13 07:07:32 -04:00
|
|
|
};
|
2023-01-25 15:12:21 -04:00
|
|
|
WriteBlock(&powr_pkt, sizeof(powr_pkt));
|
|
|
|
|
|
|
|
#if HAL_WITH_MCU_MONITORING
|
|
|
|
const struct log_MCU mcu_pkt{
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_MCU_MSG),
|
|
|
|
time_us : now,
|
|
|
|
MCU_temp : hal.analogin->mcu_temperature(),
|
|
|
|
MCU_voltage : hal.analogin->mcu_voltage(),
|
|
|
|
MCU_voltage_min : hal.analogin->mcu_voltage_min(),
|
|
|
|
MCU_voltage_max : hal.analogin->mcu_voltage_max(),
|
|
|
|
};
|
|
|
|
WriteBlock(&mcu_pkt, sizeof(mcu_pkt));
|
|
|
|
#endif
|
|
|
|
|
2014-02-13 07:07:32 -04:00
|
|
|
#endif
|
|
|
|
}
|
|
|
|
|
2019-01-18 00:24:08 -04:00
|
|
|
void AP_Logger::Write_Radio(const mavlink_radio_t &packet)
|
2014-03-11 14:05:02 -03:00
|
|
|
{
|
2019-07-02 06:31:02 -03:00
|
|
|
const struct log_Radio pkt{
|
2014-03-11 14:05:02 -03:00
|
|
|
LOG_PACKET_HEADER_INIT(LOG_RADIO_MSG),
|
2015-11-19 23:15:08 -04:00
|
|
|
time_us : AP_HAL::micros64(),
|
2014-03-11 14:05:02 -03:00
|
|
|
rssi : packet.rssi,
|
|
|
|
remrssi : packet.remrssi,
|
|
|
|
txbuf : packet.txbuf,
|
|
|
|
noise : packet.noise,
|
|
|
|
remnoise : packet.remnoise,
|
|
|
|
rxerrors : packet.rxerrors,
|
|
|
|
fixed : packet.fixed
|
|
|
|
};
|
2016-11-22 11:34:53 -04:00
|
|
|
WriteBlock(&pkt, sizeof(pkt));
|
2014-03-11 14:05:02 -03:00
|
|
|
}
|
|
|
|
|
2020-11-01 21:46:43 -04:00
|
|
|
void AP_Logger::Write_Compass_instance(const uint64_t time_us, const uint8_t mag_instance)
|
2014-12-20 15:08:13 -04:00
|
|
|
{
|
2018-06-25 08:57:09 -03:00
|
|
|
const Compass &compass = AP::compass();
|
|
|
|
|
2017-09-28 23:53:31 -03:00
|
|
|
const Vector3f &mag_field = compass.get_field(mag_instance);
|
|
|
|
const Vector3f &mag_offsets = compass.get_offsets(mag_instance);
|
|
|
|
const Vector3f &mag_motor_offsets = compass.get_motor_offsets(mag_instance);
|
2020-11-01 21:46:43 -04:00
|
|
|
const struct log_MAG pkt{
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_MAG_MSG),
|
2016-05-05 00:37:16 -03:00
|
|
|
time_us : time_us,
|
2020-11-01 21:46:43 -04:00
|
|
|
instance : mag_instance,
|
2014-12-20 15:08:13 -04:00
|
|
|
mag_x : (int16_t)mag_field.x,
|
|
|
|
mag_y : (int16_t)mag_field.y,
|
|
|
|
mag_z : (int16_t)mag_field.z,
|
|
|
|
offset_x : (int16_t)mag_offsets.x,
|
|
|
|
offset_y : (int16_t)mag_offsets.y,
|
|
|
|
offset_z : (int16_t)mag_offsets.z,
|
|
|
|
motor_offset_x : (int16_t)mag_motor_offsets.x,
|
|
|
|
motor_offset_y : (int16_t)mag_motor_offsets.y,
|
2015-04-18 09:31:48 -03:00
|
|
|
motor_offset_z : (int16_t)mag_motor_offsets.z,
|
2017-09-28 23:53:31 -03:00
|
|
|
health : (uint8_t)compass.healthy(mag_instance),
|
|
|
|
SUS : compass.last_update_usec(mag_instance)
|
2014-12-20 15:08:13 -04:00
|
|
|
};
|
|
|
|
WriteBlock(&pkt, sizeof(pkt));
|
2017-09-28 23:53:31 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// Write a Compass packet
|
2020-11-19 18:25:51 -04:00
|
|
|
void AP_Logger::Write_Compass()
|
2017-09-28 23:53:31 -03:00
|
|
|
{
|
2020-11-19 18:25:51 -04:00
|
|
|
const uint64_t time_us = AP_HAL::micros64();
|
2018-06-25 08:57:09 -03:00
|
|
|
const Compass &compass = AP::compass();
|
2020-11-01 21:46:43 -04:00
|
|
|
for (uint8_t i=0; i<compass.get_count(); i++) {
|
|
|
|
Write_Compass_instance(time_us, i);
|
2015-01-16 18:27:01 -04:00
|
|
|
}
|
2014-12-20 15:08:13 -04:00
|
|
|
}
|
|
|
|
|
2014-12-22 16:10:02 -04:00
|
|
|
// Write a mode packet.
|
2019-10-17 00:48:00 -03:00
|
|
|
bool AP_Logger_Backend::Write_Mode(uint8_t mode, const ModeReason reason)
|
2014-12-22 16:10:02 -04:00
|
|
|
{
|
2019-10-17 00:48:00 -03:00
|
|
|
static_assert(sizeof(ModeReason) <= sizeof(uint8_t), "Logging expects the ModeReason to fit in 8 bits");
|
2019-07-02 06:31:02 -03:00
|
|
|
const struct log_Mode pkt{
|
2014-12-22 16:10:02 -04:00
|
|
|
LOG_PACKET_HEADER_INIT(LOG_MODE_MSG),
|
2015-11-19 23:15:08 -04:00
|
|
|
time_us : AP_HAL::micros64(),
|
2014-12-22 16:10:02 -04:00
|
|
|
mode : mode,
|
2016-01-25 19:45:27 -04:00
|
|
|
mode_num : mode,
|
2019-10-17 00:48:00 -03:00
|
|
|
mode_reason : static_cast<uint8_t>(reason)
|
2014-12-22 16:10:02 -04:00
|
|
|
};
|
2015-08-27 23:47:16 -03:00
|
|
|
return WriteCriticalBlock(&pkt, sizeof(pkt));
|
2014-12-22 16:10:02 -04:00
|
|
|
}
|
|
|
|
|
2020-01-04 00:25:55 -04:00
|
|
|
/*
|
|
|
|
write servo status from CAN servo
|
|
|
|
*/
|
2022-12-01 23:17:14 -04:00
|
|
|
void AP_Logger::Write_ServoStatus(uint64_t time_us, uint8_t id, float position, float force, float speed, uint8_t power_pct,
|
|
|
|
float pos_cmd, float voltage, float current, float mot_temp, float pcb_temp, uint8_t error)
|
2020-01-04 00:25:55 -04:00
|
|
|
{
|
|
|
|
const struct log_CSRV pkt {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_CSRV_MSG),
|
|
|
|
time_us : time_us,
|
|
|
|
id : id,
|
|
|
|
position : position,
|
|
|
|
force : force,
|
|
|
|
speed : speed,
|
2022-12-01 23:17:14 -04:00
|
|
|
power_pct : power_pct,
|
|
|
|
pos_cmd : pos_cmd,
|
|
|
|
voltage : voltage,
|
|
|
|
current : current,
|
|
|
|
mot_temp : mot_temp,
|
|
|
|
pcb_temp : pcb_temp,
|
|
|
|
error : error,
|
2020-01-04 00:25:55 -04:00
|
|
|
};
|
|
|
|
WriteBlock(&pkt, sizeof(pkt));
|
|
|
|
}
|
|
|
|
|
2020-01-04 00:56:44 -04:00
|
|
|
|
2015-05-21 22:42:08 -03:00
|
|
|
// Write a Yaw PID packet
|
2022-03-03 23:29:47 -04:00
|
|
|
void AP_Logger::Write_PID(uint8_t msg_type, const AP_PIDInfo &info)
|
2015-05-21 22:42:08 -03:00
|
|
|
{
|
2019-07-02 06:31:02 -03:00
|
|
|
const struct log_PID pkt{
|
2015-05-21 22:42:08 -03:00
|
|
|
LOG_PACKET_HEADER_INIT(msg_type),
|
2015-11-19 23:15:08 -04:00
|
|
|
time_us : AP_HAL::micros64(),
|
2019-06-27 06:31:52 -03:00
|
|
|
target : info.target,
|
2018-08-08 01:11:43 -03:00
|
|
|
actual : info.actual,
|
2019-06-27 06:31:52 -03:00
|
|
|
error : info.error,
|
2015-05-21 22:42:08 -03:00
|
|
|
P : info.P,
|
|
|
|
I : info.I,
|
|
|
|
D : info.D,
|
2020-05-10 04:16:23 -03:00
|
|
|
FF : info.FF,
|
2021-01-02 14:02:46 -04:00
|
|
|
Dmod : info.Dmod,
|
2021-04-04 06:49:09 -03:00
|
|
|
slew_rate : info.slew_rate,
|
2021-01-02 14:02:46 -04:00
|
|
|
limit : info.limit
|
2015-05-21 22:42:08 -03:00
|
|
|
};
|
|
|
|
WriteBlock(&pkt, sizeof(pkt));
|
|
|
|
}
|
2015-07-03 08:49:45 -03:00
|
|
|
|
2019-01-18 00:24:08 -04:00
|
|
|
void AP_Logger::Write_SRTL(bool active, uint16_t num_points, uint16_t max_points, uint8_t action, const Vector3f& breadcrumb)
|
2017-07-26 14:09:33 -03:00
|
|
|
{
|
2019-07-02 06:31:02 -03:00
|
|
|
const struct log_SRTL pkt_srtl{
|
2017-07-26 14:09:33 -03:00
|
|
|
LOG_PACKET_HEADER_INIT(LOG_SRTL_MSG),
|
|
|
|
time_us : AP_HAL::micros64(),
|
|
|
|
active : active,
|
|
|
|
num_points : num_points,
|
|
|
|
max_points : max_points,
|
|
|
|
action : action,
|
|
|
|
N : breadcrumb.x,
|
|
|
|
E : breadcrumb.y,
|
|
|
|
D : breadcrumb.z
|
|
|
|
};
|
|
|
|
WriteBlock(&pkt_srtl, sizeof(pkt_srtl));
|
|
|
|
}
|
2019-06-08 01:59:04 -03:00
|
|
|
|
2020-07-24 02:59:41 -03:00
|
|
|
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)
|
|
|
|
{
|
|
|
|
struct log_Winch pkt{
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_WINCH_MSG),
|
|
|
|
time_us : AP_HAL::micros64(),
|
|
|
|
healthy : healthy,
|
|
|
|
thread_end : thread_end,
|
|
|
|
moving : moving,
|
|
|
|
clutch : clutch,
|
|
|
|
mode : mode,
|
|
|
|
desired_length : desired_length,
|
|
|
|
length : length,
|
|
|
|
desired_rate : desired_rate,
|
|
|
|
tension : tension,
|
|
|
|
voltage : voltage,
|
|
|
|
temp : temp
|
|
|
|
};
|
|
|
|
WriteBlock(&pkt, sizeof(pkt));
|
|
|
|
}
|
2020-09-09 04:16:39 -03:00
|
|
|
|
2023-03-08 19:19:18 -04:00
|
|
|
// a convenience function for writing out the position controller PIDs
|
|
|
|
void AP_Logger::Write_PSCx(LogMessages id, float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel)
|
2021-01-29 00:42:26 -04:00
|
|
|
{
|
2023-03-08 19:19:18 -04:00
|
|
|
const struct log_PSCx pkt{
|
|
|
|
LOG_PACKET_HEADER_INIT(id),
|
2021-09-21 23:41:15 -03:00
|
|
|
time_us : AP_HAL::micros64(),
|
|
|
|
pos_target : pos_target * 0.01f,
|
|
|
|
pos : pos * 0.01f,
|
|
|
|
vel_desired : vel_desired * 0.01f,
|
|
|
|
vel_target : vel_target * 0.01f,
|
|
|
|
vel : vel * 0.01f,
|
|
|
|
accel_desired : accel_desired * 0.01f,
|
|
|
|
accel_target : accel_target * 0.01f,
|
|
|
|
accel : accel * 0.01f
|
|
|
|
};
|
|
|
|
WriteBlock(&pkt, sizeof(pkt));
|
|
|
|
}
|
|
|
|
|
2023-03-08 19:19:18 -04:00
|
|
|
void AP_Logger::Write_PSCN(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel)
|
|
|
|
{
|
|
|
|
Write_PSCx(LOG_PSCN_MSG, pos_target, pos, vel_desired, vel_target, vel, accel_desired, accel_target, accel);
|
|
|
|
}
|
|
|
|
|
|
|
|
void AP_Logger::Write_PSCE(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel)
|
|
|
|
{
|
|
|
|
Write_PSCx(LOG_PSCE_MSG, pos_target, pos, vel_desired, vel_target, vel, accel_desired, accel_target, accel);
|
|
|
|
}
|
|
|
|
|
2021-09-21 23:41:15 -03:00
|
|
|
void AP_Logger::Write_PSCD(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel)
|
|
|
|
{
|
2023-03-08 19:19:18 -04:00
|
|
|
Write_PSCx(LOG_PSCD_MSG, pos_target, pos, vel_desired, vel_target, vel, accel_desired, accel_target, accel);
|
2021-01-29 00:42:26 -04:00
|
|
|
}
|
2023-09-05 10:08:30 -03:00
|
|
|
|
|
|
|
#endif // HAL_LOGGING_ENABLED
|