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>
|
2015-10-26 09:01:52 -03:00
|
|
|
#include <AP_Baro/AP_Baro.h>
|
2015-08-15 19:53:26 -03:00
|
|
|
#include <AP_BattMonitor/AP_BattMonitor.h>
|
2015-08-11 03:28:46 -03:00
|
|
|
#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>
|
2016-03-24 22:11:11 -03:00
|
|
|
#include <AP_Motors/AP_Motors.h>
|
|
|
|
#include <AC_AttitudeControl/AC_AttitudeControl.h>
|
|
|
|
#include <AC_AttitudeControl/AC_PosControl.h>
|
2017-08-08 03:15:07 -03:00
|
|
|
#include <AP_RangeFinder/RangeFinder_Backend.h>
|
2013-04-19 04:49:16 -03:00
|
|
|
|
2015-10-26 09:01:52 -03:00
|
|
|
#include "DataFlash.h"
|
2015-08-06 09:18:28 -03:00
|
|
|
#include "DataFlash_File.h"
|
2018-02-02 16:35:19 -04:00
|
|
|
#include "DataFlash_File_sd.h"
|
2015-11-10 02:34:31 -04:00
|
|
|
#include "DataFlash_MAVLink.h"
|
2018-02-02 16:35:19 -04:00
|
|
|
#include "DataFlash_Revo.h"
|
|
|
|
#include "DataFlash_File_sd.h"
|
2015-08-06 09:18:28 -03:00
|
|
|
#include "DFMessageWriter.h"
|
|
|
|
|
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
|
|
|
*/
|
2015-06-25 10:53:20 -03:00
|
|
|
void DataFlash_Backend::Log_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;
|
2015-10-25 13:23:25 -03:00
|
|
|
strncpy(pkt.name, s->name, sizeof(pkt.name));
|
|
|
|
strncpy(pkt.format, s->format, sizeof(pkt.format));
|
|
|
|
strncpy(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:
|
|
|
|
*/
|
|
|
|
void DataFlash_Backend::Log_Fill_Format_Units(const struct LogStructure *s, struct log_Format_Units &pkt)
|
|
|
|
{
|
|
|
|
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;
|
|
|
|
strncpy(pkt.units, s->units, sizeof(pkt.units));
|
|
|
|
strncpy(pkt.multipliers, s->multipliers, sizeof(pkt.multipliers));
|
|
|
|
}
|
|
|
|
|
2013-12-16 20:12:42 -04:00
|
|
|
/*
|
|
|
|
write a structure format to the log
|
|
|
|
*/
|
2015-11-09 18:14:22 -04:00
|
|
|
bool DataFlash_Backend::Log_Write_Format(const struct LogStructure *s)
|
2013-12-16 20:12:42 -04:00
|
|
|
{
|
|
|
|
struct log_Format pkt;
|
|
|
|
Log_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
|
|
|
|
*/
|
|
|
|
bool DataFlash_Backend::Log_Write_Unit(const struct UnitStructure *s)
|
|
|
|
{
|
|
|
|
struct log_Unit pkt = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_UNIT_MSG),
|
|
|
|
time_us : AP_HAL::micros64(),
|
|
|
|
type : s->ID,
|
|
|
|
unit : { }
|
|
|
|
};
|
|
|
|
strncpy(pkt.unit, s->unit, sizeof(pkt.unit));
|
|
|
|
|
|
|
|
return WriteCriticalBlock(&pkt, sizeof(pkt));
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
write a unit-multiplier definition
|
|
|
|
*/
|
|
|
|
bool DataFlash_Backend::Log_Write_Multiplier(const struct MultiplierStructure *s)
|
|
|
|
{
|
|
|
|
struct log_Format_Multiplier pkt = {
|
|
|
|
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
|
|
|
|
*/
|
|
|
|
bool DataFlash_Backend::Log_Write_Format_Units(const struct LogStructure *s)
|
|
|
|
{
|
|
|
|
struct log_Format_Units pkt;
|
|
|
|
Log_Fill_Format_Units(s, pkt);
|
|
|
|
return WriteCriticalBlock(&pkt, sizeof(pkt));
|
|
|
|
}
|
|
|
|
|
2013-04-19 04:49:16 -03:00
|
|
|
/*
|
|
|
|
write a parameter to the log
|
|
|
|
*/
|
2015-11-09 18:14:22 -04:00
|
|
|
bool DataFlash_Backend::Log_Write_Parameter(const char *name, float value)
|
2013-04-19 04:49:16 -03:00
|
|
|
{
|
|
|
|
struct log_Parameter pkt = {
|
|
|
|
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 : {},
|
|
|
|
value : value
|
|
|
|
};
|
|
|
|
strncpy(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
|
|
|
|
*/
|
2015-11-09 18:14:22 -04:00
|
|
|
bool DataFlash_Backend::Log_Write_Parameter(const AP_Param *ap,
|
|
|
|
const AP_Param::ParamToken &token,
|
|
|
|
enum ap_var_type type)
|
2013-04-19 04:49:16 -03:00
|
|
|
{
|
|
|
|
char name[16];
|
|
|
|
ap->copy_name_token(token, &name[0], sizeof(name), true);
|
2015-08-06 09:18:28 -03:00
|
|
|
return Log_Write_Parameter(name, ap->cast_to_float(type));
|
2013-04-19 04:49:16 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// Write an GPS packet
|
2018-04-11 09:17:46 -03:00
|
|
|
void DataFlash_Class::Log_Write_GPS(uint8_t i, uint64_t time_us)
|
2013-04-19 04:49:16 -03:00
|
|
|
{
|
2018-04-11 09:17:46 -03:00
|
|
|
const AP_GPS &gps = AP::gps();
|
2016-05-05 00:37:16 -03:00
|
|
|
if (time_us == 0) {
|
|
|
|
time_us = AP_HAL::micros64();
|
|
|
|
}
|
2015-08-05 03:25:48 -03:00
|
|
|
const struct Location &loc = gps.location(i);
|
|
|
|
struct log_GPS pkt = {
|
|
|
|
LOG_PACKET_HEADER_INIT((uint8_t)(LOG_GPS_MSG+i)),
|
2016-05-05 00:37:16 -03:00
|
|
|
time_us : time_us,
|
2015-08-05 03:25:48 -03:00
|
|
|
status : (uint8_t)gps.status(i),
|
|
|
|
gps_week_ms : gps.time_week_ms(i),
|
|
|
|
gps_week : gps.time_week(i),
|
|
|
|
num_sats : gps.num_sats(i),
|
|
|
|
hdop : gps.get_hdop(i),
|
|
|
|
latitude : loc.lat,
|
|
|
|
longitude : loc.lng,
|
|
|
|
altitude : loc.alt,
|
2016-05-04 21:23:34 -03:00
|
|
|
ground_speed : gps.ground_speed(i),
|
2016-05-04 22:21:34 -03:00
|
|
|
ground_course : gps.ground_course(i),
|
2015-08-05 03:25:48 -03:00
|
|
|
vel_z : gps.velocity(i).z,
|
|
|
|
used : (uint8_t)(gps.primary_sensor() == i)
|
|
|
|
};
|
|
|
|
WriteBlock(&pkt, sizeof(pkt));
|
2015-09-09 01:52:52 -03:00
|
|
|
|
2016-05-12 13:50:13 -03:00
|
|
|
/* write auxiliary accuracy information as well */
|
2015-09-09 01:52:52 -03:00
|
|
|
float hacc = 0, vacc = 0, sacc = 0;
|
|
|
|
gps.horizontal_accuracy(i, hacc);
|
|
|
|
gps.vertical_accuracy(i, vacc);
|
|
|
|
gps.speed_accuracy(i, sacc);
|
|
|
|
struct log_GPA pkt2 = {
|
|
|
|
LOG_PACKET_HEADER_INIT((uint8_t)(LOG_GPA_MSG+i)),
|
2016-05-05 00:37:16 -03:00
|
|
|
time_us : time_us,
|
2015-09-09 01:52:52 -03:00
|
|
|
vdop : gps.get_vdop(i),
|
2017-04-16 19:56:55 -03:00
|
|
|
hacc : (uint16_t)MIN((hacc*100), UINT16_MAX),
|
|
|
|
vacc : (uint16_t)MIN((vacc*100), UINT16_MAX),
|
|
|
|
sacc : (uint16_t)MIN((sacc*100), UINT16_MAX),
|
2016-05-05 03:03:59 -03:00
|
|
|
have_vv : (uint8_t)gps.have_vertical_velocity(i),
|
2017-07-08 20:06:47 -03:00
|
|
|
sample_ms : gps.last_message_time_ms(i),
|
|
|
|
delta_ms : gps.last_message_delta_time_ms(i)
|
2015-09-09 01:52:52 -03:00
|
|
|
};
|
|
|
|
WriteBlock(&pkt2, sizeof(pkt2));
|
2013-04-19 04:49:16 -03:00
|
|
|
}
|
|
|
|
|
2015-09-10 07:27:47 -03:00
|
|
|
|
|
|
|
// Write an RFND (rangefinder) packet
|
|
|
|
void DataFlash_Class::Log_Write_RFND(const RangeFinder &rangefinder)
|
|
|
|
{
|
2017-08-08 03:15:07 -03:00
|
|
|
AP_RangeFinder_Backend *s0 = rangefinder.get_backend(0);
|
|
|
|
AP_RangeFinder_Backend *s1 = rangefinder.get_backend(1);
|
|
|
|
|
2015-09-10 07:27:47 -03:00
|
|
|
struct log_RFND pkt = {
|
|
|
|
LOG_PACKET_HEADER_INIT((uint8_t)(LOG_RFND_MSG)),
|
2015-11-19 23:15:08 -04:00
|
|
|
time_us : AP_HAL::micros64(),
|
2017-08-08 03:15:07 -03:00
|
|
|
dist1 : s0 ? s0->distance_cm() : (uint16_t)0,
|
|
|
|
orient1 : s0 ? s0->orientation() : ROTATION_NONE,
|
|
|
|
dist2 : s1 ? s1->distance_cm() : (uint16_t)0,
|
|
|
|
orient2 : s1 ? s1->orientation() : ROTATION_NONE,
|
2015-09-10 07:27:47 -03:00
|
|
|
};
|
|
|
|
WriteBlock(&pkt, sizeof(pkt));
|
|
|
|
}
|
|
|
|
|
2013-11-25 17:44:00 -04:00
|
|
|
// Write an RCIN packet
|
|
|
|
void DataFlash_Class::Log_Write_RCIN(void)
|
|
|
|
{
|
|
|
|
struct log_RCIN pkt = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_RCIN_MSG),
|
2015-11-19 23:15:08 -04:00
|
|
|
time_us : AP_HAL::micros64(),
|
2018-04-03 23:19:30 -03:00
|
|
|
chan1 : RC_Channels::get_radio_in(0),
|
|
|
|
chan2 : RC_Channels::get_radio_in(1),
|
|
|
|
chan3 : RC_Channels::get_radio_in(2),
|
|
|
|
chan4 : RC_Channels::get_radio_in(3),
|
|
|
|
chan5 : RC_Channels::get_radio_in(4),
|
|
|
|
chan6 : RC_Channels::get_radio_in(5),
|
|
|
|
chan7 : RC_Channels::get_radio_in(6),
|
|
|
|
chan8 : RC_Channels::get_radio_in(7),
|
|
|
|
chan9 : RC_Channels::get_radio_in(8),
|
|
|
|
chan10 : RC_Channels::get_radio_in(9),
|
|
|
|
chan11 : RC_Channels::get_radio_in(10),
|
|
|
|
chan12 : RC_Channels::get_radio_in(11),
|
|
|
|
chan13 : RC_Channels::get_radio_in(12),
|
|
|
|
chan14 : RC_Channels::get_radio_in(13)
|
2013-11-25 17:44:00 -04:00
|
|
|
};
|
|
|
|
WriteBlock(&pkt, sizeof(pkt));
|
|
|
|
}
|
|
|
|
|
|
|
|
// Write an SERVO packet
|
2013-11-27 07:14:17 -04:00
|
|
|
void DataFlash_Class::Log_Write_RCOUT(void)
|
2013-11-25 17:44:00 -04:00
|
|
|
{
|
2013-11-27 07:14:17 -04:00
|
|
|
struct log_RCOUT pkt = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_RCOUT_MSG),
|
2015-11-19 23:15:08 -04:00
|
|
|
time_us : AP_HAL::micros64(),
|
2013-11-25 17:44:00 -04:00
|
|
|
chan1 : hal.rcout->read(0),
|
|
|
|
chan2 : hal.rcout->read(1),
|
|
|
|
chan3 : hal.rcout->read(2),
|
|
|
|
chan4 : hal.rcout->read(3),
|
|
|
|
chan5 : hal.rcout->read(4),
|
|
|
|
chan6 : hal.rcout->read(5),
|
|
|
|
chan7 : hal.rcout->read(6),
|
2014-08-12 12:08:17 -03:00
|
|
|
chan8 : hal.rcout->read(7),
|
|
|
|
chan9 : hal.rcout->read(8),
|
|
|
|
chan10 : hal.rcout->read(9),
|
|
|
|
chan11 : hal.rcout->read(10),
|
2016-06-06 00:42:03 -03:00
|
|
|
chan12 : hal.rcout->read(11),
|
|
|
|
chan13 : hal.rcout->read(12),
|
|
|
|
chan14 : hal.rcout->read(13)
|
2013-11-25 17:44:00 -04:00
|
|
|
};
|
|
|
|
WriteBlock(&pkt, sizeof(pkt));
|
2014-11-20 06:53:12 -04:00
|
|
|
Log_Write_ESC();
|
2013-11-25 17:44:00 -04:00
|
|
|
}
|
|
|
|
|
2015-09-04 12:50:23 -03:00
|
|
|
// Write an RSSI packet
|
|
|
|
void DataFlash_Class::Log_Write_RSSI(AP_RSSI &rssi)
|
|
|
|
{
|
|
|
|
struct log_RSSI pkt = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_RSSI_MSG),
|
2015-11-19 23:15:08 -04:00
|
|
|
time_us : AP_HAL::micros64(),
|
2015-09-04 12:50:23 -03:00
|
|
|
RXRSSI : rssi.read_receiver_rssi()
|
|
|
|
};
|
|
|
|
WriteBlock(&pkt, sizeof(pkt));
|
|
|
|
}
|
|
|
|
|
2018-03-05 16:36:23 -04:00
|
|
|
void DataFlash_Class::Log_Write_Baro_instance(uint64_t time_us, uint8_t baro_instance, enum LogMessages type)
|
2014-01-27 19:35:18 -04:00
|
|
|
{
|
2018-03-05 16:36:23 -04:00
|
|
|
AP_Baro &baro = AP::baro();
|
2016-05-17 22:27:39 -03:00
|
|
|
float climbrate = baro.get_climb_rate();
|
2016-05-13 17:01:43 -03:00
|
|
|
float drift_offset = baro.get_baro_drift_offset();
|
2017-03-24 02:00:22 -03:00
|
|
|
float ground_temp = baro.get_ground_temperature();
|
2014-01-27 19:35:18 -04:00
|
|
|
struct log_BARO pkt = {
|
2018-04-09 03:04:17 -03:00
|
|
|
LOG_PACKET_HEADER_INIT(type),
|
2015-05-21 22:34:05 -03:00
|
|
|
time_us : time_us,
|
2017-09-27 21:19:35 -03:00
|
|
|
altitude : baro.get_altitude(baro_instance),
|
|
|
|
pressure : baro.get_pressure(baro_instance),
|
|
|
|
temperature : (int16_t)(baro.get_temperature(baro_instance) * 100 + 0.5f),
|
2016-05-17 22:27:39 -03:00
|
|
|
climbrate : climbrate,
|
2017-09-27 21:19:35 -03:00
|
|
|
sample_time_ms: baro.get_last_update(baro_instance),
|
2016-05-13 17:01:43 -03:00
|
|
|
drift_offset : drift_offset,
|
2017-03-24 02:00:22 -03:00
|
|
|
ground_temp : ground_temp,
|
2014-01-27 19:35:18 -04:00
|
|
|
};
|
|
|
|
WriteBlock(&pkt, sizeof(pkt));
|
2017-09-27 21:19:35 -03:00
|
|
|
}
|
2015-10-14 12:53:14 -03:00
|
|
|
|
2017-09-27 21:19:35 -03:00
|
|
|
// Write a BARO packet
|
2018-03-05 16:36:23 -04:00
|
|
|
void DataFlash_Class::Log_Write_Baro(uint64_t time_us)
|
2017-09-27 21:19:35 -03:00
|
|
|
{
|
|
|
|
if (time_us == 0) {
|
|
|
|
time_us = AP_HAL::micros64();
|
|
|
|
}
|
2018-03-05 16:36:23 -04:00
|
|
|
const AP_Baro &baro = AP::baro();
|
|
|
|
Log_Write_Baro_instance(time_us, 0, LOG_BARO_MSG);
|
2015-01-05 17:37:23 -04:00
|
|
|
if (baro.num_instances() > 1 && baro.healthy(1)) {
|
2018-03-05 16:36:23 -04:00
|
|
|
Log_Write_Baro_instance(time_us, 1, LOG_BAR2_MSG);
|
2015-01-05 17:37:23 -04:00
|
|
|
}
|
2015-09-10 07:27:47 -03:00
|
|
|
if (baro.num_instances() > 2 && baro.healthy(2)) {
|
2018-03-05 16:36:23 -04:00
|
|
|
Log_Write_Baro_instance(time_us, 2, LOG_BAR3_MSG);
|
2015-09-10 07:27:47 -03:00
|
|
|
}
|
2014-01-27 19:35:18 -04:00
|
|
|
}
|
2013-04-19 04:49:16 -03:00
|
|
|
|
2018-03-10 05:36:33 -04:00
|
|
|
void DataFlash_Class::Log_Write_IMU_instance(const uint64_t time_us, const uint8_t imu_instance, const enum LogMessages type)
|
2013-04-19 04:49:16 -03:00
|
|
|
{
|
2018-03-10 05:36:33 -04:00
|
|
|
const AP_InertialSensor &ins = AP::ins();
|
2017-09-28 23:34:58 -03:00
|
|
|
const Vector3f &gyro = ins.get_gyro(imu_instance);
|
|
|
|
const Vector3f &accel = ins.get_accel(imu_instance);
|
2013-04-19 04:49:16 -03:00
|
|
|
struct log_IMU pkt = {
|
2017-09-28 23:34:58 -03:00
|
|
|
LOG_PACKET_HEADER_INIT(type),
|
2015-05-21 22:34:05 -03:00
|
|
|
time_us : time_us,
|
2013-04-19 04:49:16 -03:00
|
|
|
gyro_x : gyro.x,
|
|
|
|
gyro_y : gyro.y,
|
|
|
|
gyro_z : gyro.z,
|
|
|
|
accel_x : accel.x,
|
|
|
|
accel_y : accel.y,
|
2014-12-29 06:19:50 -04:00
|
|
|
accel_z : accel.z,
|
2017-09-28 23:34:58 -03:00
|
|
|
gyro_error : ins.get_gyro_error_count(imu_instance),
|
|
|
|
accel_error : ins.get_accel_error_count(imu_instance),
|
|
|
|
temperature : ins.get_temperature(imu_instance),
|
|
|
|
gyro_health : (uint8_t)ins.get_gyro_health(imu_instance),
|
|
|
|
accel_health : (uint8_t)ins.get_accel_health(imu_instance),
|
|
|
|
gyro_rate : ins.get_gyro_rate_hz(imu_instance),
|
|
|
|
accel_rate : ins.get_accel_rate_hz(imu_instance),
|
2013-04-19 04:49:16 -03:00
|
|
|
};
|
|
|
|
WriteBlock(&pkt, sizeof(pkt));
|
2017-09-28 23:34:58 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// Write an raw accel/gyro data packet
|
2018-03-10 05:36:33 -04:00
|
|
|
void DataFlash_Class::Log_Write_IMU()
|
2017-09-28 23:34:58 -03:00
|
|
|
{
|
|
|
|
uint64_t time_us = AP_HAL::micros64();
|
|
|
|
|
2018-03-10 05:36:33 -04:00
|
|
|
const AP_InertialSensor &ins = AP::ins();
|
|
|
|
|
|
|
|
Log_Write_IMU_instance(time_us, 0, LOG_IMU_MSG);
|
2013-12-08 05:45:04 -04:00
|
|
|
if (ins.get_gyro_count() < 2 && ins.get_accel_count() < 2) {
|
|
|
|
return;
|
|
|
|
}
|
2015-10-14 12:48:19 -03:00
|
|
|
|
2018-03-10 05:36:33 -04:00
|
|
|
Log_Write_IMU_instance(time_us, 1, LOG_IMU2_MSG);
|
2017-09-28 23:34:58 -03:00
|
|
|
|
2014-06-26 01:04:55 -03:00
|
|
|
if (ins.get_gyro_count() < 3 && ins.get_accel_count() < 3) {
|
|
|
|
return;
|
|
|
|
}
|
2017-09-28 23:34:58 -03:00
|
|
|
|
2018-03-10 05:36:33 -04:00
|
|
|
Log_Write_IMU_instance(time_us, 2, LOG_IMU3_MSG);
|
2013-12-08 05:45:04 -04:00
|
|
|
}
|
|
|
|
|
2015-06-13 10:06:49 -03:00
|
|
|
// Write an accel/gyro delta time data packet
|
2018-03-10 05:36:33 -04:00
|
|
|
void DataFlash_Class::Log_Write_IMUDT_instance(const uint64_t time_us, const uint8_t imu_instance, const enum LogMessages type)
|
2015-06-13 10:06:49 -03:00
|
|
|
{
|
2018-03-10 05:36:33 -04:00
|
|
|
const AP_InertialSensor &ins = AP::ins();
|
2015-06-13 10:06:49 -03:00
|
|
|
float delta_t = ins.get_delta_time();
|
2017-09-29 00:28:34 -03:00
|
|
|
float delta_vel_t = ins.get_delta_velocity_dt(imu_instance);
|
|
|
|
float delta_ang_t = ins.get_delta_angle_dt(imu_instance);
|
2015-06-13 10:06:49 -03:00
|
|
|
Vector3f delta_angle, delta_velocity;
|
2017-09-29 00:28:34 -03:00
|
|
|
ins.get_delta_angle(imu_instance, delta_angle);
|
|
|
|
ins.get_delta_velocity(imu_instance, delta_velocity);
|
2015-06-13 10:06:49 -03:00
|
|
|
|
|
|
|
struct log_IMUDT pkt = {
|
2017-09-29 00:28:34 -03:00
|
|
|
LOG_PACKET_HEADER_INIT(type),
|
2015-06-13 10:06:49 -03:00
|
|
|
time_us : time_us,
|
|
|
|
delta_time : delta_t,
|
|
|
|
delta_vel_dt : delta_vel_t,
|
2016-04-26 02:50:29 -03:00
|
|
|
delta_ang_dt : delta_ang_t,
|
2015-06-13 10:06:49 -03:00
|
|
|
delta_ang_x : delta_angle.x,
|
|
|
|
delta_ang_y : delta_angle.y,
|
|
|
|
delta_ang_z : delta_angle.z,
|
|
|
|
delta_vel_x : delta_velocity.x,
|
|
|
|
delta_vel_y : delta_velocity.y,
|
|
|
|
delta_vel_z : delta_velocity.z
|
|
|
|
};
|
2017-09-29 00:28:34 -03:00
|
|
|
WriteBlock(&pkt, sizeof(pkt));
|
|
|
|
}
|
|
|
|
|
2018-03-10 05:36:33 -04:00
|
|
|
void DataFlash_Class::Log_Write_IMUDT(uint64_t time_us, uint8_t imu_mask)
|
2017-09-29 00:28:34 -03:00
|
|
|
{
|
2018-03-10 05:36:33 -04:00
|
|
|
const AP_InertialSensor &ins = AP::ins();
|
2016-05-08 23:26:30 -03:00
|
|
|
if (imu_mask & 1) {
|
2018-03-10 05:36:33 -04:00
|
|
|
Log_Write_IMUDT_instance(time_us, 0, LOG_IMUDT_MSG);
|
2016-05-08 23:26:30 -03:00
|
|
|
}
|
2016-05-04 23:16:16 -03:00
|
|
|
if ((ins.get_gyro_count() < 2 && ins.get_accel_count() < 2) || !ins.use_gyro(1)) {
|
2015-06-13 10:06:49 -03:00
|
|
|
return;
|
|
|
|
}
|
2015-10-14 12:48:19 -03:00
|
|
|
|
2016-05-08 23:26:30 -03:00
|
|
|
if (imu_mask & 2) {
|
2018-03-10 05:36:33 -04:00
|
|
|
Log_Write_IMUDT_instance(time_us, 1, LOG_IMUDT2_MSG);
|
2016-05-08 23:26:30 -03:00
|
|
|
}
|
2015-06-13 10:06:49 -03:00
|
|
|
|
2016-05-04 23:16:16 -03:00
|
|
|
if ((ins.get_gyro_count() < 3 && ins.get_accel_count() < 3) || !ins.use_gyro(2)) {
|
2015-06-13 10:06:49 -03:00
|
|
|
return;
|
|
|
|
}
|
2017-09-29 00:28:34 -03:00
|
|
|
|
2016-05-08 23:26:30 -03:00
|
|
|
if (imu_mask & 4) {
|
2018-03-10 05:36:33 -04:00
|
|
|
Log_Write_IMUDT_instance(time_us, 2, LOG_IMUDT3_MSG);
|
2016-05-08 23:26:30 -03:00
|
|
|
}
|
2015-06-13 10:06:49 -03:00
|
|
|
}
|
|
|
|
|
2018-03-10 05:36:33 -04:00
|
|
|
void DataFlash_Class::Log_Write_Vibration()
|
2015-06-11 10:26:45 -03:00
|
|
|
{
|
2015-11-19 23:15:08 -04:00
|
|
|
uint64_t time_us = AP_HAL::micros64();
|
2018-03-10 05:36:33 -04:00
|
|
|
const AP_InertialSensor &ins = AP::ins();
|
|
|
|
const Vector3f vibration = ins.get_vibration_levels();
|
2015-06-11 10:26:45 -03:00
|
|
|
struct log_Vibe pkt = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_VIBE_MSG),
|
|
|
|
time_us : time_us,
|
|
|
|
vibe_x : vibration.x,
|
|
|
|
vibe_y : vibration.y,
|
|
|
|
vibe_z : vibration.z,
|
|
|
|
clipping_0 : ins.get_accel_clip_count(0),
|
|
|
|
clipping_1 : ins.get_accel_clip_count(1),
|
|
|
|
clipping_2 : ins.get_accel_clip_count(2)
|
|
|
|
};
|
|
|
|
WriteBlock(&pkt, sizeof(pkt));
|
|
|
|
}
|
|
|
|
|
2015-06-30 01:33:50 -03:00
|
|
|
// Write a mission command. Total length : 36 bytes
|
2015-11-09 18:14:22 -04:00
|
|
|
bool DataFlash_Backend::Log_Write_Mission_Cmd(const AP_Mission &mission,
|
|
|
|
const AP_Mission::Mission_Command &cmd)
|
2015-06-30 01:33:50 -03:00
|
|
|
{
|
|
|
|
mavlink_mission_item_t mav_cmd = {};
|
|
|
|
AP_Mission::mission_cmd_to_mavlink(cmd,mav_cmd);
|
2015-08-06 09:18:28 -03:00
|
|
|
return Log_Write_MavCmd(mission.num_commands(),mav_cmd);
|
2015-06-30 01:33:50 -03:00
|
|
|
}
|
|
|
|
|
2015-11-09 18:14:22 -04:00
|
|
|
void DataFlash_Backend::Log_Write_EntireMission(const AP_Mission &mission)
|
2015-06-30 01:33:50 -03:00
|
|
|
{
|
2015-11-09 18:14:22 -04:00
|
|
|
DFMessageWriter_WriteEntireMission writer;
|
|
|
|
writer.set_dataflash_backend(this);
|
2015-08-06 09:18:28 -03:00
|
|
|
writer.set_mission(&mission);
|
|
|
|
writer.process();
|
2015-06-30 01:33:50 -03:00
|
|
|
}
|
|
|
|
|
2013-05-02 20:18:14 -03:00
|
|
|
// Write a text message to the log
|
2015-11-09 18:14:22 -04:00
|
|
|
bool DataFlash_Backend::Log_Write_Message(const char *message)
|
2013-05-02 20:18:14 -03:00
|
|
|
{
|
|
|
|
struct log_Message pkt = {
|
|
|
|
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 : {}
|
|
|
|
};
|
|
|
|
strncpy(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
|
|
|
}
|
|
|
|
|
2014-02-13 07:07:32 -04:00
|
|
|
void DataFlash_Class::Log_Write_Power(void)
|
|
|
|
{
|
2018-04-08 06:48:09 -03:00
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || 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;
|
|
|
|
}
|
2014-02-13 07:07:32 -04:00
|
|
|
struct log_POWR pkt = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_POWR_MSG),
|
2015-11-19 23:15:08 -04:00
|
|
|
time_us : AP_HAL::micros64(),
|
2016-06-02 17:55:05 -03:00
|
|
|
Vcc : hal.analogin->board_voltage(),
|
|
|
|
Vservo : hal.analogin->servorail_voltage(),
|
2018-04-12 20:49:48 -03:00
|
|
|
flags : hal.analogin->power_status_flags(),
|
|
|
|
safety_and_arm : safety_and_armed
|
2014-02-13 07:07:32 -04:00
|
|
|
};
|
|
|
|
WriteBlock(&pkt, sizeof(pkt));
|
|
|
|
#endif
|
|
|
|
}
|
|
|
|
|
2014-01-03 01:01:08 -04:00
|
|
|
// Write an AHRS2 packet
|
|
|
|
void DataFlash_Class::Log_Write_AHRS2(AP_AHRS &ahrs)
|
|
|
|
{
|
|
|
|
Vector3f euler;
|
|
|
|
struct Location loc;
|
2017-04-15 08:19:36 -03:00
|
|
|
Quaternion quat;
|
2014-01-03 01:01:08 -04:00
|
|
|
if (!ahrs.get_secondary_attitude(euler) || !ahrs.get_secondary_position(loc)) {
|
|
|
|
return;
|
|
|
|
}
|
2017-04-15 08:19:36 -03:00
|
|
|
ahrs.get_secondary_quaternion(quat);
|
2014-01-03 01:01:08 -04:00
|
|
|
struct log_AHRS pkt = {
|
2014-01-30 18:33:12 -04:00
|
|
|
LOG_PACKET_HEADER_INIT(LOG_AHR2_MSG),
|
2015-11-19 23:15:08 -04:00
|
|
|
time_us : AP_HAL::micros64(),
|
2014-01-03 01:01:08 -04:00
|
|
|
roll : (int16_t)(degrees(euler.x)*100),
|
|
|
|
pitch : (int16_t)(degrees(euler.y)*100),
|
|
|
|
yaw : (uint16_t)(wrap_360_cd(degrees(euler.z)*100)),
|
|
|
|
alt : loc.alt*1.0e-2f,
|
|
|
|
lat : loc.lat,
|
2017-04-15 08:19:36 -03:00
|
|
|
lng : loc.lng,
|
|
|
|
q1 : quat.q1,
|
|
|
|
q2 : quat.q2,
|
|
|
|
q3 : quat.q3,
|
|
|
|
q4 : quat.q4,
|
2014-01-03 01:01:08 -04:00
|
|
|
};
|
|
|
|
WriteBlock(&pkt, sizeof(pkt));
|
|
|
|
}
|
|
|
|
|
2015-05-14 22:59:40 -03:00
|
|
|
// Write a POS packet
|
|
|
|
void DataFlash_Class::Log_Write_POS(AP_AHRS &ahrs)
|
|
|
|
{
|
|
|
|
Location loc;
|
|
|
|
if (!ahrs.get_position(loc)) {
|
|
|
|
return;
|
|
|
|
}
|
2017-01-30 15:09:58 -04:00
|
|
|
float home, origin;
|
|
|
|
ahrs.get_relative_position_D_home(home);
|
2015-05-14 22:59:40 -03:00
|
|
|
struct log_POS pkt = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_POS_MSG),
|
2017-01-30 15:09:58 -04:00
|
|
|
time_us : AP_HAL::micros64(),
|
|
|
|
lat : loc.lat,
|
|
|
|
lng : loc.lng,
|
|
|
|
alt : loc.alt*1.0e-2f,
|
|
|
|
rel_home_alt : -home,
|
2017-07-21 23:27:45 -03:00
|
|
|
rel_origin_alt : ahrs.get_relative_position_D_origin(origin) ? -origin : quiet_nanf(),
|
2015-05-14 22:59:40 -03:00
|
|
|
};
|
|
|
|
WriteBlock(&pkt, sizeof(pkt));
|
|
|
|
}
|
|
|
|
|
2014-01-03 20:51:22 -04:00
|
|
|
#if AP_AHRS_NAVEKF_AVAILABLE
|
2017-08-22 00:14:12 -03:00
|
|
|
void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs)
|
2016-07-14 02:08:43 -03:00
|
|
|
{
|
|
|
|
// only log EKF2 if enabled
|
|
|
|
if (ahrs.get_NavEKF2().activeCores() > 0) {
|
2017-08-22 00:14:12 -03:00
|
|
|
Log_Write_EKF2(ahrs);
|
2016-07-14 02:08:43 -03:00
|
|
|
}
|
|
|
|
// only log EKF3 if enabled
|
|
|
|
if (ahrs.get_NavEKF3().activeCores() > 0) {
|
2017-08-22 00:14:12 -03:00
|
|
|
Log_Write_EKF3(ahrs);
|
2016-07-14 02:08:43 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2017-04-27 22:34:02 -03:00
|
|
|
|
|
|
|
/*
|
|
|
|
write an EKF timing message
|
|
|
|
*/
|
|
|
|
void DataFlash_Class::Log_Write_EKF_Timing(const char *name, uint64_t time_us, const struct ekf_timing &timing)
|
|
|
|
{
|
|
|
|
Log_Write(name,
|
2018-05-23 05:56:37 -03:00
|
|
|
"TimeUS,Cnt,IMUMin,IMUMax,EKFMin,EKFMax,AngMin,AngMax,VMin,VMax",
|
|
|
|
"QIffffffff",
|
2017-04-27 22:34:02 -03:00
|
|
|
time_us,
|
|
|
|
timing.count,
|
|
|
|
(double)timing.dtIMUavg_min,
|
|
|
|
(double)timing.dtIMUavg_max,
|
2017-04-28 04:15:07 -03:00
|
|
|
(double)timing.dtEKFavg_min,
|
|
|
|
(double)timing.dtEKFavg_max,
|
2017-04-27 22:34:02 -03:00
|
|
|
(double)timing.delAngDT_min,
|
|
|
|
(double)timing.delAngDT_max,
|
|
|
|
(double)timing.delVelDT_min,
|
|
|
|
(double)timing.delVelDT_max);
|
|
|
|
}
|
|
|
|
|
2017-08-22 00:14:12 -03:00
|
|
|
void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs)
|
2015-09-22 23:09:48 -03:00
|
|
|
{
|
2016-05-05 01:07:53 -03:00
|
|
|
uint64_t time_us = AP_HAL::micros64();
|
2016-11-22 11:34:53 -04:00
|
|
|
// Write first EKF packet
|
2015-09-22 23:09:48 -03:00
|
|
|
Vector3f euler;
|
2016-07-08 23:28:40 -03:00
|
|
|
Vector2f posNE;
|
|
|
|
float posD;
|
2015-09-22 23:09:48 -03:00
|
|
|
Vector3f velNED;
|
|
|
|
Vector3f gyroBias;
|
2015-10-12 03:30:25 -03:00
|
|
|
float posDownDeriv;
|
2017-05-29 22:59:55 -03:00
|
|
|
Location originLLH;
|
2015-11-07 08:01:52 -04:00
|
|
|
ahrs.get_NavEKF2().getEulerAngles(0,euler);
|
|
|
|
ahrs.get_NavEKF2().getVelNED(0,velNED);
|
2016-07-08 23:28:40 -03:00
|
|
|
ahrs.get_NavEKF2().getPosNE(0,posNE);
|
|
|
|
ahrs.get_NavEKF2().getPosD(0,posD);
|
2015-11-07 08:01:52 -04:00
|
|
|
ahrs.get_NavEKF2().getGyroBias(0,gyroBias);
|
|
|
|
posDownDeriv = ahrs.get_NavEKF2().getPosDownDerivative(0);
|
2017-05-29 22:59:55 -03:00
|
|
|
if (!ahrs.get_NavEKF2().getOriginLLH(0,originLLH)) {
|
|
|
|
originLLH.alt = 0;
|
|
|
|
}
|
2015-09-22 23:09:48 -03:00
|
|
|
struct log_EKF1 pkt = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_NKF1_MSG),
|
2016-05-05 01:07:53 -03:00
|
|
|
time_us : time_us,
|
2015-09-22 23:09:48 -03:00
|
|
|
roll : (int16_t)(100*degrees(euler.x)), // roll angle (centi-deg, displayed as deg due to format string)
|
|
|
|
pitch : (int16_t)(100*degrees(euler.y)), // pitch angle (centi-deg, displayed as deg due to format string)
|
|
|
|
yaw : (uint16_t)wrap_360_cd(100*degrees(euler.z)), // yaw angle (centi-deg, displayed as deg due to format string)
|
|
|
|
velN : (float)(velNED.x), // velocity North (m/s)
|
|
|
|
velE : (float)(velNED.y), // velocity East (m/s)
|
|
|
|
velD : (float)(velNED.z), // velocity Down (m/s)
|
2015-10-12 03:30:25 -03:00
|
|
|
posD_dot : (float)(posDownDeriv), // first derivative of down position
|
2016-07-08 23:28:40 -03:00
|
|
|
posN : (float)(posNE.x), // metres North
|
|
|
|
posE : (float)(posNE.y), // metres East
|
|
|
|
posD : (float)(posD), // metres Down
|
2015-09-22 23:09:48 -03:00
|
|
|
gyrX : (int16_t)(100*degrees(gyroBias.x)), // cd/sec, displayed as deg/sec due to format string
|
|
|
|
gyrY : (int16_t)(100*degrees(gyroBias.y)), // cd/sec, displayed as deg/sec due to format string
|
2017-05-29 22:59:55 -03:00
|
|
|
gyrZ : (int16_t)(100*degrees(gyroBias.z)), // cd/sec, displayed as deg/sec due to format string
|
|
|
|
originHgt : originLLH.alt // WGS-84 altitude of EKF origin in cm
|
2015-09-22 23:09:48 -03:00
|
|
|
};
|
|
|
|
WriteBlock(&pkt, sizeof(pkt));
|
|
|
|
|
|
|
|
// Write second EKF packet
|
2015-09-23 22:10:34 -03:00
|
|
|
float azbias = 0;
|
2015-09-22 23:09:48 -03:00
|
|
|
Vector3f wind;
|
|
|
|
Vector3f magNED;
|
|
|
|
Vector3f magXYZ;
|
2015-09-23 22:10:34 -03:00
|
|
|
Vector3f gyroScaleFactor;
|
2015-11-08 06:28:00 -04:00
|
|
|
uint8_t magIndex = ahrs.get_NavEKF2().getActiveMag(0);
|
2015-11-07 08:01:52 -04:00
|
|
|
ahrs.get_NavEKF2().getAccelZBias(0,azbias);
|
|
|
|
ahrs.get_NavEKF2().getWind(0,wind);
|
|
|
|
ahrs.get_NavEKF2().getMagNED(0,magNED);
|
|
|
|
ahrs.get_NavEKF2().getMagXYZ(0,magXYZ);
|
|
|
|
ahrs.get_NavEKF2().getGyroScaleErrorPercentage(0,gyroScaleFactor);
|
2015-09-23 22:10:34 -03:00
|
|
|
struct log_NKF2 pkt2 = {
|
2015-09-22 23:09:48 -03:00
|
|
|
LOG_PACKET_HEADER_INIT(LOG_NKF2_MSG),
|
2016-05-05 01:07:53 -03:00
|
|
|
time_us : time_us,
|
2015-09-23 22:10:34 -03:00
|
|
|
AZbias : (int8_t)(100*azbias),
|
|
|
|
scaleX : (int16_t)(100*gyroScaleFactor.x),
|
|
|
|
scaleY : (int16_t)(100*gyroScaleFactor.y),
|
|
|
|
scaleZ : (int16_t)(100*gyroScaleFactor.z),
|
2015-09-22 23:09:48 -03:00
|
|
|
windN : (int16_t)(100*wind.x),
|
|
|
|
windE : (int16_t)(100*wind.y),
|
|
|
|
magN : (int16_t)(magNED.x),
|
|
|
|
magE : (int16_t)(magNED.y),
|
|
|
|
magD : (int16_t)(magNED.z),
|
|
|
|
magX : (int16_t)(magXYZ.x),
|
|
|
|
magY : (int16_t)(magXYZ.y),
|
2015-11-08 06:28:00 -04:00
|
|
|
magZ : (int16_t)(magXYZ.z),
|
|
|
|
index : (uint8_t)(magIndex)
|
2015-09-22 23:09:48 -03:00
|
|
|
};
|
|
|
|
WriteBlock(&pkt2, sizeof(pkt2));
|
|
|
|
|
|
|
|
// Write third EKF packet
|
|
|
|
Vector3f velInnov;
|
|
|
|
Vector3f posInnov;
|
|
|
|
Vector3f magInnov;
|
2015-09-23 05:54:46 -03:00
|
|
|
float tasInnov = 0;
|
|
|
|
float yawInnov = 0;
|
2015-11-07 08:01:52 -04:00
|
|
|
ahrs.get_NavEKF2().getInnovations(0,velInnov, posInnov, magInnov, tasInnov, yawInnov);
|
2015-09-23 22:10:34 -03:00
|
|
|
struct log_NKF3 pkt3 = {
|
2015-09-22 23:09:48 -03:00
|
|
|
LOG_PACKET_HEADER_INIT(LOG_NKF3_MSG),
|
2016-05-05 01:07:53 -03:00
|
|
|
time_us : time_us,
|
2015-09-22 23:09:48 -03:00
|
|
|
innovVN : (int16_t)(100*velInnov.x),
|
|
|
|
innovVE : (int16_t)(100*velInnov.y),
|
|
|
|
innovVD : (int16_t)(100*velInnov.z),
|
|
|
|
innovPN : (int16_t)(100*posInnov.x),
|
|
|
|
innovPE : (int16_t)(100*posInnov.y),
|
|
|
|
innovPD : (int16_t)(100*posInnov.z),
|
|
|
|
innovMX : (int16_t)(magInnov.x),
|
|
|
|
innovMY : (int16_t)(magInnov.y),
|
|
|
|
innovMZ : (int16_t)(magInnov.z),
|
2015-09-23 23:05:42 -03:00
|
|
|
innovYaw : (int16_t)(100*degrees(yawInnov)),
|
2015-09-22 23:09:48 -03:00
|
|
|
innovVT : (int16_t)(100*tasInnov)
|
|
|
|
};
|
|
|
|
WriteBlock(&pkt3, sizeof(pkt3));
|
|
|
|
|
|
|
|
// Write fourth EKF packet
|
2015-09-23 05:54:46 -03:00
|
|
|
float velVar = 0;
|
|
|
|
float posVar = 0;
|
|
|
|
float hgtVar = 0;
|
2015-09-22 23:09:48 -03:00
|
|
|
Vector3f magVar;
|
2015-09-23 05:54:46 -03:00
|
|
|
float tasVar = 0;
|
2015-09-22 23:09:48 -03:00
|
|
|
Vector2f offset;
|
2016-05-10 03:07:56 -03:00
|
|
|
uint16_t faultStatus=0;
|
|
|
|
uint8_t timeoutStatus=0;
|
2015-09-23 05:54:46 -03:00
|
|
|
nav_filter_status solutionStatus {};
|
2015-10-08 20:15:07 -03:00
|
|
|
nav_gps_status gpsStatus {};
|
2015-11-07 08:01:52 -04:00
|
|
|
ahrs.get_NavEKF2().getVariances(0,velVar, posVar, hgtVar, magVar, tasVar, offset);
|
2016-05-16 03:45:27 -03:00
|
|
|
float tempVar = fmaxf(fmaxf(magVar.x,magVar.y),magVar.z);
|
2015-11-07 08:01:52 -04:00
|
|
|
ahrs.get_NavEKF2().getFilterFaults(0,faultStatus);
|
|
|
|
ahrs.get_NavEKF2().getFilterTimeouts(0,timeoutStatus);
|
|
|
|
ahrs.get_NavEKF2().getFilterStatus(0,solutionStatus);
|
|
|
|
ahrs.get_NavEKF2().getFilterGpsStatus(0,gpsStatus);
|
2015-10-09 16:08:32 -03:00
|
|
|
float tiltError;
|
2015-11-07 08:01:52 -04:00
|
|
|
ahrs.get_NavEKF2().getTiltError(0,tiltError);
|
2016-10-13 03:22:59 -03:00
|
|
|
int8_t primaryIndex = ahrs.get_NavEKF2().getPrimaryCoreIndex();
|
2015-10-09 16:08:32 -03:00
|
|
|
struct log_NKF4 pkt4 = {
|
2015-09-22 23:09:48 -03:00
|
|
|
LOG_PACKET_HEADER_INIT(LOG_NKF4_MSG),
|
2016-05-05 01:07:53 -03:00
|
|
|
time_us : time_us,
|
2015-09-22 23:09:48 -03:00
|
|
|
sqrtvarV : (int16_t)(100*velVar),
|
|
|
|
sqrtvarP : (int16_t)(100*posVar),
|
|
|
|
sqrtvarH : (int16_t)(100*hgtVar),
|
2016-05-16 03:45:27 -03:00
|
|
|
sqrtvarM : (int16_t)(100*tempVar),
|
2015-09-22 23:09:48 -03:00
|
|
|
sqrtvarVT : (int16_t)(100*tasVar),
|
2015-10-09 16:08:32 -03:00
|
|
|
tiltErr : (float)tiltError,
|
2015-09-22 23:09:48 -03:00
|
|
|
offsetNorth : (int8_t)(offset.x),
|
|
|
|
offsetEast : (int8_t)(offset.y),
|
2016-05-10 03:07:56 -03:00
|
|
|
faults : (uint16_t)(faultStatus),
|
2015-09-22 23:09:48 -03:00
|
|
|
timeouts : (uint8_t)(timeoutStatus),
|
2015-10-08 20:15:07 -03:00
|
|
|
solution : (uint16_t)(solutionStatus.value),
|
2015-11-05 21:15:09 -04:00
|
|
|
gps : (uint16_t)(gpsStatus.value),
|
2015-11-05 23:04:53 -04:00
|
|
|
primary : (int8_t)primaryIndex
|
2015-09-22 23:09:48 -03:00
|
|
|
};
|
|
|
|
WriteBlock(&pkt4, sizeof(pkt4));
|
|
|
|
|
2015-11-07 08:01:52 -04:00
|
|
|
// Write fifth EKF packet - take data from the primary instance
|
2016-06-30 21:44:36 -03:00
|
|
|
float normInnov=0; // normalised innovation variance ratio for optical flow observations fused by the main nav filter
|
|
|
|
float gndOffset=0; // estimated vertical position of the terrain relative to the nav filter zero datum
|
|
|
|
float flowInnovX=0, flowInnovY=0; // optical flow LOS rate vector innovations from the main nav filter
|
|
|
|
float auxFlowInnov=0; // optical flow LOS rate innovation from terrain offset estimator
|
|
|
|
float HAGL=0; // height above ground level
|
|
|
|
float rngInnov=0; // range finder innovations
|
|
|
|
float range=0; // measured range
|
|
|
|
float gndOffsetErr=0; // filter ground offset state error
|
|
|
|
Vector3f predictorErrors; // output predictor angle, velocity and position tracking error
|
|
|
|
ahrs.get_NavEKF2().getFlowDebug(-1,normInnov, gndOffset, flowInnovX, flowInnovY, auxFlowInnov, HAGL, rngInnov, range, gndOffsetErr);
|
2016-07-03 05:47:30 -03:00
|
|
|
ahrs.get_NavEKF2().getOutputTrackingError(-1,predictorErrors);
|
2016-07-03 19:11:03 -03:00
|
|
|
struct log_NKF5 pkt5 = {
|
2016-06-30 21:44:36 -03:00
|
|
|
LOG_PACKET_HEADER_INIT(LOG_NKF5_MSG),
|
|
|
|
time_us : time_us,
|
|
|
|
normInnov : (uint8_t)(MIN(100*normInnov,255)),
|
|
|
|
FIX : (int16_t)(1000*flowInnovX),
|
|
|
|
FIY : (int16_t)(1000*flowInnovY),
|
|
|
|
AFI : (int16_t)(1000*auxFlowInnov),
|
|
|
|
HAGL : (int16_t)(100*HAGL),
|
|
|
|
offset : (int16_t)(100*gndOffset),
|
|
|
|
RI : (int16_t)(100*rngInnov),
|
|
|
|
meaRng : (uint16_t)(100*range),
|
|
|
|
errHAGL : (uint16_t)(100*gndOffsetErr),
|
|
|
|
angErr : (float)predictorErrors.x,
|
|
|
|
velErr : (float)predictorErrors.y,
|
|
|
|
posErr : (float)predictorErrors.z
|
|
|
|
};
|
|
|
|
WriteBlock(&pkt5, sizeof(pkt5));
|
2015-11-07 08:01:52 -04:00
|
|
|
|
2017-04-15 07:37:00 -03:00
|
|
|
// log quaternion
|
|
|
|
Quaternion quat;
|
|
|
|
ahrs.get_NavEKF2().getQuaternion(0, quat);
|
|
|
|
struct log_Quaternion pktq1 = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_NKQ1_MSG),
|
|
|
|
time_us : time_us,
|
|
|
|
q1 : quat.q1,
|
|
|
|
q2 : quat.q2,
|
|
|
|
q3 : quat.q3,
|
|
|
|
q4 : quat.q4
|
|
|
|
};
|
|
|
|
WriteBlock(&pktq1, sizeof(pktq1));
|
|
|
|
|
2015-11-07 08:01:52 -04:00
|
|
|
// log innovations for the second IMU if enabled
|
|
|
|
if (ahrs.get_NavEKF2().activeCores() >= 2) {
|
|
|
|
// Write 6th EKF packet
|
|
|
|
ahrs.get_NavEKF2().getEulerAngles(1,euler);
|
|
|
|
ahrs.get_NavEKF2().getVelNED(1,velNED);
|
2016-07-08 23:28:40 -03:00
|
|
|
ahrs.get_NavEKF2().getPosNE(1,posNE);
|
|
|
|
ahrs.get_NavEKF2().getPosD(1,posD);
|
2015-11-07 08:01:52 -04:00
|
|
|
ahrs.get_NavEKF2().getGyroBias(1,gyroBias);
|
|
|
|
posDownDeriv = ahrs.get_NavEKF2().getPosDownDerivative(1);
|
2017-05-29 22:59:55 -03:00
|
|
|
if (!ahrs.get_NavEKF2().getOriginLLH(1,originLLH)) {
|
|
|
|
originLLH.alt = 0;
|
|
|
|
}
|
2015-11-07 08:01:52 -04:00
|
|
|
struct log_EKF1 pkt6 = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_NKF6_MSG),
|
2016-05-05 01:07:53 -03:00
|
|
|
time_us : time_us,
|
2015-11-07 08:01:52 -04:00
|
|
|
roll : (int16_t)(100*degrees(euler.x)), // roll angle (centi-deg, displayed as deg due to format string)
|
|
|
|
pitch : (int16_t)(100*degrees(euler.y)), // pitch angle (centi-deg, displayed as deg due to format string)
|
|
|
|
yaw : (uint16_t)wrap_360_cd(100*degrees(euler.z)), // yaw angle (centi-deg, displayed as deg due to format string)
|
|
|
|
velN : (float)(velNED.x), // velocity North (m/s)
|
|
|
|
velE : (float)(velNED.y), // velocity East (m/s)
|
|
|
|
velD : (float)(velNED.z), // velocity Down (m/s)
|
|
|
|
posD_dot : (float)(posDownDeriv), // first derivative of down position
|
2016-07-08 23:28:40 -03:00
|
|
|
posN : (float)(posNE.x), // metres North
|
|
|
|
posE : (float)(posNE.y), // metres East
|
|
|
|
posD : (float)(posD), // metres Down
|
2015-11-07 08:01:52 -04:00
|
|
|
gyrX : (int16_t)(100*degrees(gyroBias.x)), // cd/sec, displayed as deg/sec due to format string
|
|
|
|
gyrY : (int16_t)(100*degrees(gyroBias.y)), // cd/sec, displayed as deg/sec due to format string
|
2017-05-29 22:59:55 -03:00
|
|
|
gyrZ : (int16_t)(100*degrees(gyroBias.z)), // cd/sec, displayed as deg/sec due to format string
|
|
|
|
originHgt : originLLH.alt // WGS-84 altitude of EKF origin in cm
|
2015-11-07 08:01:52 -04:00
|
|
|
};
|
|
|
|
WriteBlock(&pkt6, sizeof(pkt6));
|
|
|
|
|
|
|
|
// Write 7th EKF packet
|
|
|
|
ahrs.get_NavEKF2().getAccelZBias(1,azbias);
|
|
|
|
ahrs.get_NavEKF2().getWind(1,wind);
|
|
|
|
ahrs.get_NavEKF2().getMagNED(1,magNED);
|
|
|
|
ahrs.get_NavEKF2().getMagXYZ(1,magXYZ);
|
|
|
|
ahrs.get_NavEKF2().getGyroScaleErrorPercentage(1,gyroScaleFactor);
|
2015-11-08 06:28:00 -04:00
|
|
|
magIndex = ahrs.get_NavEKF2().getActiveMag(1);
|
2015-11-07 08:01:52 -04:00
|
|
|
struct log_NKF2 pkt7 = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_NKF7_MSG),
|
2016-05-05 01:07:53 -03:00
|
|
|
time_us : time_us,
|
2015-11-07 08:01:52 -04:00
|
|
|
AZbias : (int8_t)(100*azbias),
|
|
|
|
scaleX : (int16_t)(100*gyroScaleFactor.x),
|
|
|
|
scaleY : (int16_t)(100*gyroScaleFactor.y),
|
|
|
|
scaleZ : (int16_t)(100*gyroScaleFactor.z),
|
|
|
|
windN : (int16_t)(100*wind.x),
|
|
|
|
windE : (int16_t)(100*wind.y),
|
|
|
|
magN : (int16_t)(magNED.x),
|
|
|
|
magE : (int16_t)(magNED.y),
|
|
|
|
magD : (int16_t)(magNED.z),
|
|
|
|
magX : (int16_t)(magXYZ.x),
|
|
|
|
magY : (int16_t)(magXYZ.y),
|
2015-11-08 06:28:00 -04:00
|
|
|
magZ : (int16_t)(magXYZ.z),
|
|
|
|
index : (uint8_t)(magIndex)
|
2015-11-07 08:01:52 -04:00
|
|
|
};
|
|
|
|
WriteBlock(&pkt7, sizeof(pkt7));
|
|
|
|
|
|
|
|
// Write 8th EKF packet
|
|
|
|
ahrs.get_NavEKF2().getInnovations(1,velInnov, posInnov, magInnov, tasInnov, yawInnov);
|
|
|
|
struct log_NKF3 pkt8 = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_NKF8_MSG),
|
2016-05-05 01:07:53 -03:00
|
|
|
time_us : time_us,
|
2015-11-07 08:01:52 -04:00
|
|
|
innovVN : (int16_t)(100*velInnov.x),
|
|
|
|
innovVE : (int16_t)(100*velInnov.y),
|
|
|
|
innovVD : (int16_t)(100*velInnov.z),
|
|
|
|
innovPN : (int16_t)(100*posInnov.x),
|
|
|
|
innovPE : (int16_t)(100*posInnov.y),
|
|
|
|
innovPD : (int16_t)(100*posInnov.z),
|
|
|
|
innovMX : (int16_t)(magInnov.x),
|
|
|
|
innovMY : (int16_t)(magInnov.y),
|
|
|
|
innovMZ : (int16_t)(magInnov.z),
|
|
|
|
innovYaw : (int16_t)(100*degrees(yawInnov)),
|
|
|
|
innovVT : (int16_t)(100*tasInnov)
|
|
|
|
};
|
|
|
|
WriteBlock(&pkt8, sizeof(pkt8));
|
|
|
|
|
|
|
|
// Write 9th EKF packet
|
|
|
|
ahrs.get_NavEKF2().getVariances(1,velVar, posVar, hgtVar, magVar, tasVar, offset);
|
2016-05-16 03:45:27 -03:00
|
|
|
tempVar = fmaxf(fmaxf(magVar.x,magVar.y),magVar.z);
|
2015-11-07 08:01:52 -04:00
|
|
|
ahrs.get_NavEKF2().getFilterFaults(1,faultStatus);
|
|
|
|
ahrs.get_NavEKF2().getFilterTimeouts(1,timeoutStatus);
|
|
|
|
ahrs.get_NavEKF2().getFilterStatus(1,solutionStatus);
|
|
|
|
ahrs.get_NavEKF2().getFilterGpsStatus(1,gpsStatus);
|
|
|
|
ahrs.get_NavEKF2().getTiltError(1,tiltError);
|
|
|
|
struct log_NKF4 pkt9 = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_NKF9_MSG),
|
2016-05-05 01:07:53 -03:00
|
|
|
time_us : time_us,
|
2015-11-07 08:01:52 -04:00
|
|
|
sqrtvarV : (int16_t)(100*velVar),
|
|
|
|
sqrtvarP : (int16_t)(100*posVar),
|
|
|
|
sqrtvarH : (int16_t)(100*hgtVar),
|
2016-05-16 03:45:27 -03:00
|
|
|
sqrtvarM : (int16_t)(100*tempVar),
|
2015-11-07 08:01:52 -04:00
|
|
|
sqrtvarVT : (int16_t)(100*tasVar),
|
|
|
|
tiltErr : (float)tiltError,
|
|
|
|
offsetNorth : (int8_t)(offset.x),
|
|
|
|
offsetEast : (int8_t)(offset.y),
|
2016-05-10 03:07:56 -03:00
|
|
|
faults : (uint16_t)(faultStatus),
|
2015-11-07 08:01:52 -04:00
|
|
|
timeouts : (uint8_t)(timeoutStatus),
|
|
|
|
solution : (uint16_t)(solutionStatus.value),
|
|
|
|
gps : (uint16_t)(gpsStatus.value),
|
|
|
|
primary : (int8_t)primaryIndex
|
|
|
|
};
|
|
|
|
WriteBlock(&pkt9, sizeof(pkt9));
|
2017-04-15 07:37:00 -03:00
|
|
|
|
|
|
|
ahrs.get_NavEKF2().getQuaternion(1, quat);
|
|
|
|
struct log_Quaternion pktq2 = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_NKQ2_MSG),
|
|
|
|
time_us : time_us,
|
|
|
|
q1 : quat.q1,
|
|
|
|
q2 : quat.q2,
|
|
|
|
q3 : quat.q3,
|
|
|
|
q4 : quat.q4
|
|
|
|
};
|
|
|
|
WriteBlock(&pktq2, sizeof(pktq2));
|
2015-11-07 08:01:52 -04:00
|
|
|
}
|
2016-11-23 08:04:51 -04:00
|
|
|
|
|
|
|
// write range beacon fusion debug packet if the range value is non-zero
|
|
|
|
if (ahrs.get_beacon() != nullptr) {
|
|
|
|
uint8_t ID;
|
|
|
|
float rng;
|
|
|
|
float innovVar;
|
|
|
|
float innov;
|
|
|
|
float testRatio;
|
|
|
|
Vector3f beaconPosNED;
|
|
|
|
float bcnPosOffsetHigh;
|
|
|
|
float bcnPosOffsetLow;
|
|
|
|
if (ahrs.get_NavEKF2().getRangeBeaconDebug(-1, ID, rng, innov, innovVar, testRatio, beaconPosNED, bcnPosOffsetHigh, bcnPosOffsetLow)) {
|
|
|
|
if (rng > 0.0f) {
|
|
|
|
struct log_RngBcnDebug pkt10 = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_NKF10_MSG),
|
|
|
|
time_us : time_us,
|
|
|
|
ID : (uint8_t)ID,
|
|
|
|
rng : (int16_t)(100*rng),
|
|
|
|
innov : (int16_t)(100*innov),
|
|
|
|
sqrtInnovVar : (uint16_t)(100*safe_sqrt(innovVar)),
|
2017-06-08 08:49:20 -03:00
|
|
|
testRatio : (uint16_t)(100*constrain_float(testRatio,0.0f,650.0f)),
|
2016-11-23 08:04:51 -04:00
|
|
|
beaconPosN : (int16_t)(100*beaconPosNED.x),
|
|
|
|
beaconPosE : (int16_t)(100*beaconPosNED.y),
|
|
|
|
beaconPosD : (int16_t)(100*beaconPosNED.z),
|
|
|
|
offsetHigh : (int16_t)(100*bcnPosOffsetHigh),
|
2016-12-17 06:00:57 -04:00
|
|
|
offsetLow : (int16_t)(100*bcnPosOffsetLow),
|
|
|
|
posN : 0,
|
|
|
|
posE : 0,
|
|
|
|
posD : 0
|
2016-11-23 08:04:51 -04:00
|
|
|
};
|
|
|
|
WriteBlock(&pkt10, sizeof(pkt10));
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2017-04-27 22:34:02 -03:00
|
|
|
|
|
|
|
// log EKF timing statistics every 5s
|
|
|
|
static uint32_t lastTimingLogTime_ms = 0;
|
|
|
|
if (AP_HAL::millis() - lastTimingLogTime_ms > 5000) {
|
|
|
|
lastTimingLogTime_ms = AP_HAL::millis();
|
|
|
|
struct ekf_timing timing;
|
|
|
|
for (uint8_t i=0; i<ahrs.get_NavEKF2().activeCores(); i++) {
|
|
|
|
ahrs.get_NavEKF2().getTimingStatistics(i, timing);
|
|
|
|
Log_Write_EKF_Timing(i==0?"NKT1":"NKT2", time_us, timing);
|
|
|
|
}
|
|
|
|
}
|
2014-01-03 20:51:22 -04:00
|
|
|
}
|
2016-07-14 02:08:43 -03:00
|
|
|
|
|
|
|
|
2017-08-22 00:14:12 -03:00
|
|
|
void DataFlash_Class::Log_Write_EKF3(AP_AHRS_NavEKF &ahrs)
|
2016-07-14 02:08:43 -03:00
|
|
|
{
|
|
|
|
uint64_t time_us = AP_HAL::micros64();
|
|
|
|
// Write first EKF packet
|
|
|
|
Vector3f euler;
|
|
|
|
Vector2f posNE;
|
|
|
|
float posD;
|
|
|
|
Vector3f velNED;
|
|
|
|
Vector3f gyroBias;
|
|
|
|
float posDownDeriv;
|
2017-05-29 22:59:55 -03:00
|
|
|
Location originLLH;
|
2016-07-14 02:08:43 -03:00
|
|
|
ahrs.get_NavEKF3().getEulerAngles(0,euler);
|
|
|
|
ahrs.get_NavEKF3().getVelNED(0,velNED);
|
|
|
|
ahrs.get_NavEKF3().getPosNE(0,posNE);
|
|
|
|
ahrs.get_NavEKF3().getPosD(0,posD);
|
|
|
|
ahrs.get_NavEKF3().getGyroBias(0,gyroBias);
|
|
|
|
posDownDeriv = ahrs.get_NavEKF3().getPosDownDerivative(0);
|
2017-05-29 22:59:55 -03:00
|
|
|
if (!ahrs.get_NavEKF3().getOriginLLH(0,originLLH)) {
|
|
|
|
originLLH.alt = 0;
|
|
|
|
}
|
2016-07-14 02:08:43 -03:00
|
|
|
struct log_EKF1 pkt = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_XKF1_MSG),
|
|
|
|
time_us : time_us,
|
|
|
|
roll : (int16_t)(100*degrees(euler.x)), // roll angle (centi-deg, displayed as deg due to format string)
|
|
|
|
pitch : (int16_t)(100*degrees(euler.y)), // pitch angle (centi-deg, displayed as deg due to format string)
|
|
|
|
yaw : (uint16_t)wrap_360_cd(100*degrees(euler.z)), // yaw angle (centi-deg, displayed as deg due to format string)
|
|
|
|
velN : (float)(velNED.x), // velocity North (m/s)
|
|
|
|
velE : (float)(velNED.y), // velocity East (m/s)
|
|
|
|
velD : (float)(velNED.z), // velocity Down (m/s)
|
|
|
|
posD_dot : (float)(posDownDeriv), // first derivative of down position
|
|
|
|
posN : (float)(posNE.x), // metres North
|
|
|
|
posE : (float)(posNE.y), // metres East
|
|
|
|
posD : (float)(posD), // metres Down
|
|
|
|
gyrX : (int16_t)(100*degrees(gyroBias.x)), // cd/sec, displayed as deg/sec due to format string
|
|
|
|
gyrY : (int16_t)(100*degrees(gyroBias.y)), // cd/sec, displayed as deg/sec due to format string
|
2017-05-29 22:59:55 -03:00
|
|
|
gyrZ : (int16_t)(100*degrees(gyroBias.z)), // cd/sec, displayed as deg/sec due to format string
|
|
|
|
originHgt : originLLH.alt // WGS-84 altitude of EKF origin in cm
|
2016-07-14 02:08:43 -03:00
|
|
|
};
|
|
|
|
WriteBlock(&pkt, sizeof(pkt));
|
|
|
|
|
|
|
|
// Write second EKF packet
|
|
|
|
Vector3f accelBias;
|
|
|
|
Vector3f wind;
|
|
|
|
Vector3f magNED;
|
|
|
|
Vector3f magXYZ;
|
|
|
|
uint8_t magIndex = ahrs.get_NavEKF3().getActiveMag(0);
|
|
|
|
ahrs.get_NavEKF3().getAccelBias(0,accelBias);
|
|
|
|
ahrs.get_NavEKF3().getWind(0,wind);
|
|
|
|
ahrs.get_NavEKF3().getMagNED(0,magNED);
|
|
|
|
ahrs.get_NavEKF3().getMagXYZ(0,magXYZ);
|
|
|
|
struct log_NKF2a pkt2 = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_XKF2_MSG),
|
|
|
|
time_us : time_us,
|
|
|
|
accBiasX : (int16_t)(100*accelBias.x),
|
|
|
|
accBiasY : (int16_t)(100*accelBias.y),
|
|
|
|
accBiasZ : (int16_t)(100*accelBias.z),
|
|
|
|
windN : (int16_t)(100*wind.x),
|
|
|
|
windE : (int16_t)(100*wind.y),
|
|
|
|
magN : (int16_t)(magNED.x),
|
|
|
|
magE : (int16_t)(magNED.y),
|
|
|
|
magD : (int16_t)(magNED.z),
|
|
|
|
magX : (int16_t)(magXYZ.x),
|
|
|
|
magY : (int16_t)(magXYZ.y),
|
|
|
|
magZ : (int16_t)(magXYZ.z),
|
|
|
|
index : (uint8_t)(magIndex)
|
|
|
|
};
|
|
|
|
WriteBlock(&pkt2, sizeof(pkt2));
|
|
|
|
|
|
|
|
// Write third EKF packet
|
|
|
|
Vector3f velInnov;
|
|
|
|
Vector3f posInnov;
|
|
|
|
Vector3f magInnov;
|
|
|
|
float tasInnov = 0;
|
|
|
|
float yawInnov = 0;
|
|
|
|
ahrs.get_NavEKF3().getInnovations(0,velInnov, posInnov, magInnov, tasInnov, yawInnov);
|
|
|
|
struct log_NKF3 pkt3 = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_XKF3_MSG),
|
|
|
|
time_us : time_us,
|
|
|
|
innovVN : (int16_t)(100*velInnov.x),
|
|
|
|
innovVE : (int16_t)(100*velInnov.y),
|
|
|
|
innovVD : (int16_t)(100*velInnov.z),
|
|
|
|
innovPN : (int16_t)(100*posInnov.x),
|
|
|
|
innovPE : (int16_t)(100*posInnov.y),
|
|
|
|
innovPD : (int16_t)(100*posInnov.z),
|
|
|
|
innovMX : (int16_t)(magInnov.x),
|
|
|
|
innovMY : (int16_t)(magInnov.y),
|
|
|
|
innovMZ : (int16_t)(magInnov.z),
|
|
|
|
innovYaw : (int16_t)(100*degrees(yawInnov)),
|
|
|
|
innovVT : (int16_t)(100*tasInnov)
|
|
|
|
};
|
|
|
|
WriteBlock(&pkt3, sizeof(pkt3));
|
|
|
|
|
|
|
|
// Write fourth EKF packet
|
|
|
|
float velVar = 0;
|
|
|
|
float posVar = 0;
|
|
|
|
float hgtVar = 0;
|
|
|
|
Vector3f magVar;
|
|
|
|
float tasVar = 0;
|
|
|
|
Vector2f offset;
|
|
|
|
uint16_t faultStatus=0;
|
|
|
|
uint8_t timeoutStatus=0;
|
|
|
|
nav_filter_status solutionStatus {};
|
|
|
|
nav_gps_status gpsStatus {};
|
|
|
|
ahrs.get_NavEKF3().getVariances(0,velVar, posVar, hgtVar, magVar, tasVar, offset);
|
|
|
|
float tempVar = fmaxf(fmaxf(magVar.x,magVar.y),magVar.z);
|
|
|
|
ahrs.get_NavEKF3().getFilterFaults(0,faultStatus);
|
|
|
|
ahrs.get_NavEKF3().getFilterTimeouts(0,timeoutStatus);
|
|
|
|
ahrs.get_NavEKF3().getFilterStatus(0,solutionStatus);
|
|
|
|
ahrs.get_NavEKF3().getFilterGpsStatus(0,gpsStatus);
|
|
|
|
float tiltError;
|
|
|
|
ahrs.get_NavEKF3().getTiltError(0,tiltError);
|
|
|
|
uint8_t primaryIndex = ahrs.get_NavEKF3().getPrimaryCoreIndex();
|
|
|
|
struct log_NKF4 pkt4 = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_XKF4_MSG),
|
|
|
|
time_us : time_us,
|
|
|
|
sqrtvarV : (int16_t)(100*velVar),
|
|
|
|
sqrtvarP : (int16_t)(100*posVar),
|
|
|
|
sqrtvarH : (int16_t)(100*hgtVar),
|
|
|
|
sqrtvarM : (int16_t)(100*tempVar),
|
|
|
|
sqrtvarVT : (int16_t)(100*tasVar),
|
|
|
|
tiltErr : (float)tiltError,
|
|
|
|
offsetNorth : (int8_t)(offset.x),
|
|
|
|
offsetEast : (int8_t)(offset.y),
|
|
|
|
faults : (uint16_t)(faultStatus),
|
|
|
|
timeouts : (uint8_t)(timeoutStatus),
|
|
|
|
solution : (uint16_t)(solutionStatus.value),
|
|
|
|
gps : (uint16_t)(gpsStatus.value),
|
|
|
|
primary : (int8_t)primaryIndex
|
|
|
|
};
|
|
|
|
WriteBlock(&pkt4, sizeof(pkt4));
|
|
|
|
|
|
|
|
// Write fifth EKF packet - take data from the primary instance
|
|
|
|
float normInnov=0; // normalised innovation variance ratio for optical flow observations fused by the main nav filter
|
|
|
|
float gndOffset=0; // estimated vertical position of the terrain relative to the nav filter zero datum
|
|
|
|
float flowInnovX=0, flowInnovY=0; // optical flow LOS rate vector innovations from the main nav filter
|
|
|
|
float auxFlowInnov=0; // optical flow LOS rate innovation from terrain offset estimator
|
|
|
|
float HAGL=0; // height above ground level
|
|
|
|
float rngInnov=0; // range finder innovations
|
|
|
|
float range=0; // measured range
|
|
|
|
float gndOffsetErr=0; // filter ground offset state error
|
|
|
|
Vector3f predictorErrors; // output predictor angle, velocity and position tracking error
|
|
|
|
ahrs.get_NavEKF3().getFlowDebug(-1,normInnov, gndOffset, flowInnovX, flowInnovY, auxFlowInnov, HAGL, rngInnov, range, gndOffsetErr);
|
|
|
|
ahrs.get_NavEKF3().getOutputTrackingError(-1,predictorErrors);
|
|
|
|
struct log_NKF5 pkt5 = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_XKF5_MSG),
|
|
|
|
time_us : time_us,
|
|
|
|
normInnov : (uint8_t)(MIN(100*normInnov,255)),
|
|
|
|
FIX : (int16_t)(1000*flowInnovX),
|
|
|
|
FIY : (int16_t)(1000*flowInnovY),
|
|
|
|
AFI : (int16_t)(1000*auxFlowInnov),
|
|
|
|
HAGL : (int16_t)(100*HAGL),
|
|
|
|
offset : (int16_t)(100*gndOffset),
|
|
|
|
RI : (int16_t)(100*rngInnov),
|
|
|
|
meaRng : (uint16_t)(100*range),
|
|
|
|
errHAGL : (uint16_t)(100*gndOffsetErr),
|
|
|
|
angErr : (float)predictorErrors.x,
|
|
|
|
velErr : (float)predictorErrors.y,
|
|
|
|
posErr : (float)predictorErrors.z
|
|
|
|
};
|
|
|
|
WriteBlock(&pkt5, sizeof(pkt5));
|
|
|
|
|
2017-04-15 07:37:00 -03:00
|
|
|
// log quaternion
|
|
|
|
Quaternion quat;
|
|
|
|
ahrs.get_NavEKF3().getQuaternion(0, quat);
|
|
|
|
struct log_Quaternion pktq1 = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_XKQ1_MSG),
|
|
|
|
time_us : time_us,
|
|
|
|
q1 : quat.q1,
|
|
|
|
q2 : quat.q2,
|
|
|
|
q3 : quat.q3,
|
|
|
|
q4 : quat.q4
|
|
|
|
};
|
|
|
|
WriteBlock(&pktq1, sizeof(pktq1));
|
|
|
|
|
2016-07-14 02:08:43 -03:00
|
|
|
// log innovations for the second IMU if enabled
|
|
|
|
if (ahrs.get_NavEKF3().activeCores() >= 2) {
|
|
|
|
// Write 6th EKF packet
|
|
|
|
ahrs.get_NavEKF3().getEulerAngles(1,euler);
|
|
|
|
ahrs.get_NavEKF3().getVelNED(1,velNED);
|
|
|
|
ahrs.get_NavEKF3().getPosNE(1,posNE);
|
|
|
|
ahrs.get_NavEKF3().getPosD(1,posD);
|
|
|
|
ahrs.get_NavEKF3().getGyroBias(1,gyroBias);
|
|
|
|
posDownDeriv = ahrs.get_NavEKF3().getPosDownDerivative(1);
|
2017-05-29 22:59:55 -03:00
|
|
|
if (!ahrs.get_NavEKF3().getOriginLLH(1,originLLH)) {
|
|
|
|
originLLH.alt = 0;
|
|
|
|
}
|
2016-07-14 02:08:43 -03:00
|
|
|
struct log_EKF1 pkt6 = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_XKF6_MSG),
|
|
|
|
time_us : time_us,
|
|
|
|
roll : (int16_t)(100*degrees(euler.x)), // roll angle (centi-deg, displayed as deg due to format string)
|
|
|
|
pitch : (int16_t)(100*degrees(euler.y)), // pitch angle (centi-deg, displayed as deg due to format string)
|
|
|
|
yaw : (uint16_t)wrap_360_cd(100*degrees(euler.z)), // yaw angle (centi-deg, displayed as deg due to format string)
|
|
|
|
velN : (float)(velNED.x), // velocity North (m/s)
|
|
|
|
velE : (float)(velNED.y), // velocity East (m/s)
|
|
|
|
velD : (float)(velNED.z), // velocity Down (m/s)
|
|
|
|
posD_dot : (float)(posDownDeriv), // first derivative of down position
|
|
|
|
posN : (float)(posNE.x), // metres North
|
|
|
|
posE : (float)(posNE.y), // metres East
|
|
|
|
posD : (float)(posD), // metres Down
|
|
|
|
gyrX : (int16_t)(100*degrees(gyroBias.x)), // cd/sec, displayed as deg/sec due to format string
|
|
|
|
gyrY : (int16_t)(100*degrees(gyroBias.y)), // cd/sec, displayed as deg/sec due to format string
|
2017-05-29 22:59:55 -03:00
|
|
|
gyrZ : (int16_t)(100*degrees(gyroBias.z)), // cd/sec, displayed as deg/sec due to format string
|
|
|
|
originHgt : originLLH.alt // WGS-84 altitude of EKF origin in cm
|
2016-07-14 02:08:43 -03:00
|
|
|
};
|
|
|
|
WriteBlock(&pkt6, sizeof(pkt6));
|
|
|
|
|
|
|
|
// Write 7th EKF packet
|
|
|
|
ahrs.get_NavEKF3().getAccelBias(1,accelBias);
|
|
|
|
ahrs.get_NavEKF3().getWind(1,wind);
|
|
|
|
ahrs.get_NavEKF3().getMagNED(1,magNED);
|
|
|
|
ahrs.get_NavEKF3().getMagXYZ(1,magXYZ);
|
|
|
|
magIndex = ahrs.get_NavEKF3().getActiveMag(1);
|
|
|
|
struct log_NKF2a pkt7 = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_XKF7_MSG),
|
|
|
|
time_us : time_us,
|
|
|
|
accBiasX : (int16_t)(100*accelBias.x),
|
|
|
|
accBiasY : (int16_t)(100*accelBias.y),
|
|
|
|
accBiasZ : (int16_t)(100*accelBias.z),
|
|
|
|
windN : (int16_t)(100*wind.x),
|
|
|
|
windE : (int16_t)(100*wind.y),
|
|
|
|
magN : (int16_t)(magNED.x),
|
|
|
|
magE : (int16_t)(magNED.y),
|
|
|
|
magD : (int16_t)(magNED.z),
|
|
|
|
magX : (int16_t)(magXYZ.x),
|
|
|
|
magY : (int16_t)(magXYZ.y),
|
|
|
|
magZ : (int16_t)(magXYZ.z),
|
|
|
|
index : (uint8_t)(magIndex)
|
|
|
|
};
|
|
|
|
WriteBlock(&pkt7, sizeof(pkt7));
|
|
|
|
|
|
|
|
// Write 8th EKF packet
|
|
|
|
ahrs.get_NavEKF3().getInnovations(1,velInnov, posInnov, magInnov, tasInnov, yawInnov);
|
|
|
|
struct log_NKF3 pkt8 = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_XKF8_MSG),
|
|
|
|
time_us : time_us,
|
|
|
|
innovVN : (int16_t)(100*velInnov.x),
|
|
|
|
innovVE : (int16_t)(100*velInnov.y),
|
|
|
|
innovVD : (int16_t)(100*velInnov.z),
|
|
|
|
innovPN : (int16_t)(100*posInnov.x),
|
|
|
|
innovPE : (int16_t)(100*posInnov.y),
|
|
|
|
innovPD : (int16_t)(100*posInnov.z),
|
|
|
|
innovMX : (int16_t)(magInnov.x),
|
|
|
|
innovMY : (int16_t)(magInnov.y),
|
|
|
|
innovMZ : (int16_t)(magInnov.z),
|
|
|
|
innovYaw : (int16_t)(100*degrees(yawInnov)),
|
|
|
|
innovVT : (int16_t)(100*tasInnov)
|
|
|
|
};
|
|
|
|
WriteBlock(&pkt8, sizeof(pkt8));
|
|
|
|
|
|
|
|
// Write 9th EKF packet
|
|
|
|
ahrs.get_NavEKF3().getVariances(1,velVar, posVar, hgtVar, magVar, tasVar, offset);
|
|
|
|
tempVar = fmaxf(fmaxf(magVar.x,magVar.y),magVar.z);
|
|
|
|
ahrs.get_NavEKF3().getFilterFaults(1,faultStatus);
|
|
|
|
ahrs.get_NavEKF3().getFilterTimeouts(1,timeoutStatus);
|
|
|
|
ahrs.get_NavEKF3().getFilterStatus(1,solutionStatus);
|
|
|
|
ahrs.get_NavEKF3().getFilterGpsStatus(1,gpsStatus);
|
|
|
|
ahrs.get_NavEKF3().getTiltError(1,tiltError);
|
|
|
|
struct log_NKF4 pkt9 = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_XKF9_MSG),
|
|
|
|
time_us : time_us,
|
|
|
|
sqrtvarV : (int16_t)(100*velVar),
|
|
|
|
sqrtvarP : (int16_t)(100*posVar),
|
|
|
|
sqrtvarH : (int16_t)(100*hgtVar),
|
|
|
|
sqrtvarM : (int16_t)(100*tempVar),
|
|
|
|
sqrtvarVT : (int16_t)(100*tasVar),
|
|
|
|
tiltErr : (float)tiltError,
|
|
|
|
offsetNorth : (int8_t)(offset.x),
|
|
|
|
offsetEast : (int8_t)(offset.y),
|
|
|
|
faults : (uint16_t)(faultStatus),
|
|
|
|
timeouts : (uint8_t)(timeoutStatus),
|
|
|
|
solution : (uint16_t)(solutionStatus.value),
|
|
|
|
gps : (uint16_t)(gpsStatus.value),
|
|
|
|
primary : (int8_t)primaryIndex
|
|
|
|
};
|
|
|
|
WriteBlock(&pkt9, sizeof(pkt9));
|
|
|
|
|
2017-04-15 07:37:00 -03:00
|
|
|
// log quaternion
|
|
|
|
ahrs.get_NavEKF3().getQuaternion(1, quat);
|
|
|
|
struct log_Quaternion pktq2 = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_XKQ2_MSG),
|
|
|
|
time_us : time_us,
|
|
|
|
q1 : quat.q1,
|
|
|
|
q2 : quat.q2,
|
|
|
|
q3 : quat.q3,
|
|
|
|
q4 : quat.q4
|
|
|
|
};
|
|
|
|
WriteBlock(&pktq2, sizeof(pktq2));
|
2016-07-14 02:08:43 -03:00
|
|
|
}
|
|
|
|
// write range beacon fusion debug packet if the range value is non-zero
|
|
|
|
uint8_t ID;
|
|
|
|
float rng;
|
|
|
|
float innovVar;
|
|
|
|
float innov;
|
|
|
|
float testRatio;
|
|
|
|
Vector3f beaconPosNED;
|
|
|
|
float bcnPosOffsetHigh;
|
|
|
|
float bcnPosOffsetLow;
|
2016-12-17 06:00:57 -04:00
|
|
|
Vector3f posNED;
|
|
|
|
if (ahrs.get_NavEKF3().getRangeBeaconDebug(-1, ID, rng, innov, innovVar, testRatio, beaconPosNED, bcnPosOffsetHigh, bcnPosOffsetLow, posNED)) {
|
2016-07-14 02:08:43 -03:00
|
|
|
if (rng > 0.0f) {
|
|
|
|
struct log_RngBcnDebug pkt10 = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_XKF10_MSG),
|
|
|
|
time_us : time_us,
|
|
|
|
ID : (uint8_t)ID,
|
|
|
|
rng : (int16_t)(100*rng),
|
|
|
|
innov : (int16_t)(100*innov),
|
|
|
|
sqrtInnovVar : (uint16_t)(100*sqrtf(innovVar)),
|
2017-06-08 08:49:20 -03:00
|
|
|
testRatio : (uint16_t)(100*constrain_float(testRatio,0.0f,650.0f)),
|
2016-07-14 02:08:43 -03:00
|
|
|
beaconPosN : (int16_t)(100*beaconPosNED.x),
|
|
|
|
beaconPosE : (int16_t)(100*beaconPosNED.y),
|
|
|
|
beaconPosD : (int16_t)(100*beaconPosNED.z),
|
|
|
|
offsetHigh : (int16_t)(100*bcnPosOffsetHigh),
|
2016-12-17 06:00:57 -04:00
|
|
|
offsetLow : (int16_t)(100*bcnPosOffsetLow),
|
|
|
|
posN : (int16_t)(100*posNED.x),
|
|
|
|
posE : (int16_t)(100*posNED.y),
|
|
|
|
posD : (int16_t)(100*posNED.z)
|
|
|
|
|
2016-07-14 02:08:43 -03:00
|
|
|
};
|
|
|
|
WriteBlock(&pkt10, sizeof(pkt10));
|
|
|
|
}
|
|
|
|
}
|
2017-03-22 00:46:33 -03:00
|
|
|
// write debug data for body frame odometry fusion
|
|
|
|
Vector3f velBodyInnov,velBodyInnovVar;
|
|
|
|
static uint32_t lastUpdateTime_ms = 0;
|
|
|
|
uint32_t updateTime_ms = ahrs.get_NavEKF3().getBodyFrameOdomDebug(-1, velBodyInnov, velBodyInnovVar);
|
|
|
|
if (updateTime_ms > lastUpdateTime_ms) {
|
|
|
|
struct log_ekfBodyOdomDebug pkt11 = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_XKFD_MSG),
|
|
|
|
time_us : time_us,
|
|
|
|
velInnovX : velBodyInnov.x,
|
|
|
|
velInnovY : velBodyInnov.y,
|
|
|
|
velInnovZ : velBodyInnov.z,
|
|
|
|
velInnovVarX : velBodyInnovVar.x,
|
|
|
|
velInnovVarY : velBodyInnovVar.y,
|
|
|
|
velInnovVarZ : velBodyInnovVar.z
|
|
|
|
};
|
|
|
|
WriteBlock(&pkt11, sizeof(pkt11));
|
2018-03-31 11:41:26 -03:00
|
|
|
lastUpdateTime_ms = updateTime_ms;
|
2017-03-22 00:46:33 -03:00
|
|
|
}
|
2017-04-27 22:34:02 -03:00
|
|
|
|
2017-05-20 23:13:08 -03:00
|
|
|
// log state variances every 0.49s
|
|
|
|
static uint32_t lastEkfStateVarLogTime_ms = 0;
|
|
|
|
if (AP_HAL::millis() - lastEkfStateVarLogTime_ms > 490) {
|
|
|
|
lastEkfStateVarLogTime_ms = AP_HAL::millis();
|
|
|
|
float stateVar[24];
|
|
|
|
ahrs.get_NavEKF3().getStateVariances(-1, stateVar);
|
|
|
|
struct log_ekfStateVar pktv1 = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_XKV1_MSG),
|
|
|
|
time_us : time_us,
|
|
|
|
v00 : stateVar[0],
|
|
|
|
v01 : stateVar[1],
|
|
|
|
v02 : stateVar[2],
|
|
|
|
v03 : stateVar[3],
|
|
|
|
v04 : stateVar[4],
|
|
|
|
v05 : stateVar[5],
|
|
|
|
v06 : stateVar[6],
|
|
|
|
v07 : stateVar[7],
|
|
|
|
v08 : stateVar[8],
|
|
|
|
v09 : stateVar[9],
|
|
|
|
v10 : stateVar[10],
|
|
|
|
v11 : stateVar[11]
|
|
|
|
};
|
|
|
|
WriteBlock(&pktv1, sizeof(pktv1));
|
|
|
|
struct log_ekfStateVar pktv2 = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_XKV2_MSG),
|
|
|
|
time_us : time_us,
|
|
|
|
v00 : stateVar[12],
|
|
|
|
v01 : stateVar[13],
|
|
|
|
v02 : stateVar[14],
|
|
|
|
v03 : stateVar[15],
|
|
|
|
v04 : stateVar[16],
|
|
|
|
v05 : stateVar[17],
|
|
|
|
v06 : stateVar[18],
|
|
|
|
v07 : stateVar[19],
|
|
|
|
v08 : stateVar[20],
|
|
|
|
v09 : stateVar[21],
|
|
|
|
v10 : stateVar[22],
|
|
|
|
v11 : stateVar[23]
|
|
|
|
};
|
|
|
|
WriteBlock(&pktv2, sizeof(pktv2));
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2017-04-27 22:34:02 -03:00
|
|
|
// log EKF timing statistics every 5s
|
|
|
|
static uint32_t lastTimingLogTime_ms = 0;
|
|
|
|
if (AP_HAL::millis() - lastTimingLogTime_ms > 5000) {
|
|
|
|
lastTimingLogTime_ms = AP_HAL::millis();
|
|
|
|
struct ekf_timing timing;
|
|
|
|
for (uint8_t i=0; i<ahrs.get_NavEKF3().activeCores(); i++) {
|
|
|
|
ahrs.get_NavEKF3().getTimingStatistics(i, timing);
|
|
|
|
Log_Write_EKF_Timing(i==0?"XKT1":"XKT2", time_us, timing);
|
|
|
|
}
|
|
|
|
}
|
2016-07-14 02:08:43 -03:00
|
|
|
}
|
2014-01-30 18:33:12 -04:00
|
|
|
#endif
|
2014-01-03 20:51:22 -04:00
|
|
|
|
2014-03-12 05:17:13 -03:00
|
|
|
// Write a command processing packet
|
2015-11-09 18:14:22 -04:00
|
|
|
bool DataFlash_Backend::Log_Write_MavCmd(uint16_t cmd_total, const mavlink_mission_item_t& mav_cmd)
|
2014-03-16 05:40:30 -03:00
|
|
|
{
|
|
|
|
struct log_Cmd pkt = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_CMD_MSG),
|
2015-11-19 23:15:08 -04:00
|
|
|
time_us : AP_HAL::micros64(),
|
2014-03-16 05:40:30 -03:00
|
|
|
command_total : (uint16_t)cmd_total,
|
|
|
|
sequence : (uint16_t)mav_cmd.seq,
|
|
|
|
command : (uint16_t)mav_cmd.command,
|
|
|
|
param1 : (float)mav_cmd.param1,
|
|
|
|
param2 : (float)mav_cmd.param2,
|
|
|
|
param3 : (float)mav_cmd.param3,
|
|
|
|
param4 : (float)mav_cmd.param4,
|
|
|
|
latitude : (float)mav_cmd.x,
|
|
|
|
longitude : (float)mav_cmd.y,
|
2018-03-15 15:39:51 -03:00
|
|
|
altitude : (float)mav_cmd.z,
|
|
|
|
frame : (uint8_t)mav_cmd.frame
|
2014-03-16 05:40:30 -03:00
|
|
|
};
|
2015-08-06 09:18:28 -03:00
|
|
|
return WriteBlock(&pkt, sizeof(pkt));
|
2014-03-12 05:17:13 -03:00
|
|
|
}
|
2014-03-11 14:05:02 -03:00
|
|
|
|
2016-11-22 11:34:53 -04:00
|
|
|
void DataFlash_Class::Log_Write_Radio(const mavlink_radio_t &packet)
|
2014-03-11 14:05:02 -03:00
|
|
|
{
|
|
|
|
struct log_Radio pkt = {
|
|
|
|
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
|
|
|
}
|
|
|
|
|
2014-06-10 23:55:04 -03:00
|
|
|
// Write a Camera packet
|
2017-11-12 20:31:31 -04:00
|
|
|
void DataFlash_Class::Log_Write_CameraInfo(enum LogMessages msg, const AP_AHRS &ahrs, const Location ¤t_loc)
|
2014-06-10 23:55:04 -03:00
|
|
|
{
|
2016-01-28 18:51:52 -04:00
|
|
|
int32_t altitude, altitude_rel, altitude_gps;
|
2014-06-10 23:55:04 -03:00
|
|
|
if (current_loc.flags.relative_alt) {
|
|
|
|
altitude = current_loc.alt+ahrs.get_home().alt;
|
|
|
|
altitude_rel = current_loc.alt;
|
|
|
|
} else {
|
|
|
|
altitude = current_loc.alt;
|
|
|
|
altitude_rel = current_loc.alt - ahrs.get_home().alt;
|
|
|
|
}
|
2017-11-12 20:31:31 -04:00
|
|
|
const AP_GPS &gps = AP::gps();
|
2016-01-28 18:51:52 -04:00
|
|
|
if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
|
|
|
|
altitude_gps = gps.location().alt;
|
|
|
|
} else {
|
|
|
|
altitude_gps = 0;
|
|
|
|
}
|
2014-06-10 23:55:04 -03:00
|
|
|
|
|
|
|
struct log_Camera pkt = {
|
2016-02-26 01:16:04 -04:00
|
|
|
LOG_PACKET_HEADER_INIT(static_cast<uint8_t>(msg)),
|
2015-11-19 23:15:08 -04:00
|
|
|
time_us : AP_HAL::micros64(),
|
2014-06-10 23:55:04 -03:00
|
|
|
gps_time : gps.time_week_ms(),
|
|
|
|
gps_week : gps.time_week(),
|
|
|
|
latitude : current_loc.lat,
|
|
|
|
longitude : current_loc.lng,
|
|
|
|
altitude : altitude,
|
|
|
|
altitude_rel: altitude_rel,
|
2016-01-28 18:51:52 -04:00
|
|
|
altitude_gps: altitude_gps,
|
2014-06-10 23:55:04 -03:00
|
|
|
roll : (int16_t)ahrs.roll_sensor,
|
|
|
|
pitch : (int16_t)ahrs.pitch_sensor,
|
|
|
|
yaw : (uint16_t)ahrs.yaw_sensor
|
|
|
|
};
|
2015-10-06 01:13:40 -03:00
|
|
|
WriteCriticalBlock(&pkt, sizeof(pkt));
|
2014-06-10 23:55:04 -03:00
|
|
|
}
|
2014-11-13 06:38:33 -04:00
|
|
|
|
2016-01-28 18:51:52 -04:00
|
|
|
// Write a Camera packet
|
2017-11-12 20:31:31 -04:00
|
|
|
void DataFlash_Class::Log_Write_Camera(const AP_AHRS &ahrs, const Location ¤t_loc)
|
2016-01-28 18:51:52 -04:00
|
|
|
{
|
2017-11-12 20:31:31 -04:00
|
|
|
Log_Write_CameraInfo(LOG_CAMERA_MSG, ahrs, current_loc);
|
2016-01-28 18:51:52 -04:00
|
|
|
}
|
|
|
|
|
2016-01-06 20:29:52 -04:00
|
|
|
// Write a Trigger packet
|
2017-11-12 20:31:31 -04:00
|
|
|
void DataFlash_Class::Log_Write_Trigger(const AP_AHRS &ahrs, const Location ¤t_loc)
|
2016-01-06 20:29:52 -04:00
|
|
|
{
|
2017-11-12 20:31:31 -04:00
|
|
|
Log_Write_CameraInfo(LOG_TRIGGER_MSG, ahrs, current_loc);
|
2016-01-06 20:29:52 -04:00
|
|
|
}
|
|
|
|
|
2014-12-18 12:55:11 -04:00
|
|
|
// Write an attitude packet
|
2015-01-20 15:44:39 -04:00
|
|
|
void DataFlash_Class::Log_Write_Attitude(AP_AHRS &ahrs, const Vector3f &targets)
|
2017-03-30 20:37:35 -03:00
|
|
|
{
|
|
|
|
struct log_Attitude pkt = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG),
|
|
|
|
time_us : AP_HAL::micros64(),
|
|
|
|
control_roll : (int16_t)targets.x,
|
|
|
|
roll : (int16_t)ahrs.roll_sensor,
|
|
|
|
control_pitch : (int16_t)targets.y,
|
|
|
|
pitch : (int16_t)ahrs.pitch_sensor,
|
|
|
|
control_yaw : (uint16_t)targets.z,
|
|
|
|
yaw : (uint16_t)ahrs.yaw_sensor,
|
|
|
|
error_rp : (uint16_t)(ahrs.get_error_rp() * 100),
|
|
|
|
error_yaw : (uint16_t)(ahrs.get_error_yaw() * 100)
|
|
|
|
};
|
|
|
|
WriteBlock(&pkt, sizeof(pkt));
|
|
|
|
}
|
|
|
|
|
|
|
|
// Write an attitude packet
|
|
|
|
void DataFlash_Class::Log_Write_AttitudeView(AP_AHRS_View &ahrs, const Vector3f &targets)
|
2014-12-18 12:55:11 -04:00
|
|
|
{
|
|
|
|
struct log_Attitude pkt = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG),
|
2015-11-19 23:15:08 -04:00
|
|
|
time_us : AP_HAL::micros64(),
|
2014-12-18 12:55:11 -04:00
|
|
|
control_roll : (int16_t)targets.x,
|
|
|
|
roll : (int16_t)ahrs.roll_sensor,
|
|
|
|
control_pitch : (int16_t)targets.y,
|
|
|
|
pitch : (int16_t)ahrs.pitch_sensor,
|
|
|
|
control_yaw : (uint16_t)targets.z,
|
|
|
|
yaw : (uint16_t)ahrs.yaw_sensor,
|
|
|
|
error_rp : (uint16_t)(ahrs.get_error_rp() * 100),
|
|
|
|
error_yaw : (uint16_t)(ahrs.get_error_yaw() * 100)
|
|
|
|
};
|
|
|
|
WriteBlock(&pkt, sizeof(pkt));
|
|
|
|
}
|
|
|
|
|
2018-01-16 15:09:47 -04:00
|
|
|
void DataFlash_Class::Log_Write_Current_instance(const uint64_t time_us,
|
2017-09-29 00:09:59 -03:00
|
|
|
const uint8_t battery_instance,
|
2017-09-29 00:15:20 -03:00
|
|
|
const enum LogMessages type,
|
|
|
|
const enum LogMessages celltype)
|
2014-12-18 17:34:29 -04:00
|
|
|
{
|
2018-01-16 15:09:47 -04:00
|
|
|
AP_BattMonitor &battery = AP::battery();
|
2017-09-29 00:09:59 -03:00
|
|
|
float temp;
|
|
|
|
bool has_temp = battery.get_temperature(temp, battery_instance);
|
|
|
|
struct log_Current pkt = {
|
|
|
|
LOG_PACKET_HEADER_INIT(type),
|
|
|
|
time_us : time_us,
|
|
|
|
voltage : battery.voltage(battery_instance),
|
|
|
|
voltage_resting : battery.voltage_resting_estimate(battery_instance),
|
|
|
|
current_amps : battery.current_amps(battery_instance),
|
2018-02-15 12:10:37 -04:00
|
|
|
current_total : battery.consumed_mah(battery_instance),
|
2017-12-05 05:54:11 -04:00
|
|
|
consumed_wh : battery.consumed_wh(battery_instance),
|
2017-09-29 00:09:59 -03:00
|
|
|
temperature : (int16_t)(has_temp ? (temp * 100) : 0),
|
|
|
|
resistance : battery.get_resistance(battery_instance)
|
|
|
|
};
|
|
|
|
WriteBlock(&pkt, sizeof(pkt));
|
|
|
|
|
|
|
|
// individual cell voltages
|
|
|
|
if (battery.has_cell_voltages(battery_instance)) {
|
|
|
|
const AP_BattMonitor::cells &cells = battery.get_cell_voltages(battery_instance);
|
|
|
|
struct log_Current_Cells cell_pkt = {
|
2017-09-29 00:15:20 -03:00
|
|
|
LOG_PACKET_HEADER_INIT(celltype),
|
2017-09-29 00:09:59 -03:00
|
|
|
time_us : time_us,
|
|
|
|
voltage : battery.voltage(battery_instance)
|
2016-06-02 18:00:57 -03:00
|
|
|
};
|
2017-09-29 00:09:59 -03:00
|
|
|
for (uint8_t i = 0; i < ARRAY_SIZE(cells.cells); i++) {
|
|
|
|
cell_pkt.cell_voltages[i] = cells.cells[i] + 1;
|
|
|
|
}
|
|
|
|
WriteBlock(&cell_pkt, sizeof(cell_pkt));
|
2017-04-08 16:04:12 -03:00
|
|
|
|
2017-09-29 00:09:59 -03:00
|
|
|
// check battery structure can hold all cells
|
|
|
|
static_assert(ARRAY_SIZE(cells.cells) == (sizeof(cell_pkt.cell_voltages) / sizeof(cell_pkt.cell_voltages[0])),
|
|
|
|
"Battery cell number doesn't match in library and log structure");
|
|
|
|
}
|
|
|
|
}
|
2017-04-08 16:04:12 -03:00
|
|
|
|
2017-09-29 00:09:59 -03:00
|
|
|
// Write an Current data packet
|
2018-01-16 15:09:47 -04:00
|
|
|
void DataFlash_Class::Log_Write_Current()
|
2017-09-29 00:09:59 -03:00
|
|
|
{
|
|
|
|
const uint64_t time_us = AP_HAL::micros64();
|
2018-01-16 15:09:47 -04:00
|
|
|
const uint8_t num_instances = AP::battery().num_instances();
|
|
|
|
if (num_instances >= 1) {
|
|
|
|
Log_Write_Current_instance(time_us,
|
2017-09-29 00:15:20 -03:00
|
|
|
0,
|
|
|
|
LOG_CURRENT_MSG,
|
|
|
|
LOG_CURRENT_CELLS_MSG);
|
2016-06-02 18:00:57 -03:00
|
|
|
}
|
|
|
|
|
2018-01-16 15:09:47 -04:00
|
|
|
if (num_instances >= 2) {
|
|
|
|
Log_Write_Current_instance(time_us,
|
2017-09-29 00:15:20 -03:00
|
|
|
1,
|
|
|
|
LOG_CURRENT2_MSG,
|
|
|
|
LOG_CURRENT_CELLS2_MSG);
|
2016-06-02 18:00:57 -03:00
|
|
|
}
|
2014-12-18 17:34:29 -04:00
|
|
|
}
|
|
|
|
|
2018-06-25 08:57:09 -03:00
|
|
|
void DataFlash_Class::Log_Write_Compass_instance(const uint64_t time_us, const uint8_t mag_instance, const enum LogMessages type)
|
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);
|
2014-12-20 15:08:13 -04:00
|
|
|
struct log_Compass pkt = {
|
2017-09-28 23:53:31 -03:00
|
|
|
LOG_PACKET_HEADER_INIT(type),
|
2016-05-05 00:37:16 -03:00
|
|
|
time_us : time_us,
|
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
|
2018-06-25 08:57:09 -03:00
|
|
|
void DataFlash_Class::Log_Write_Compass(uint64_t time_us)
|
2017-09-28 23:53:31 -03:00
|
|
|
{
|
|
|
|
if (time_us == 0) {
|
|
|
|
time_us = AP_HAL::micros64();
|
|
|
|
}
|
2018-06-25 08:57:09 -03:00
|
|
|
const Compass &compass = AP::compass();
|
|
|
|
if (compass.get_count() > 0) {
|
|
|
|
Log_Write_Compass_instance(time_us, 0, LOG_COMPASS_MSG);
|
|
|
|
}
|
2015-10-14 13:05:36 -03:00
|
|
|
|
2015-01-16 18:27:01 -04:00
|
|
|
if (compass.get_count() > 1) {
|
2018-06-25 08:57:09 -03:00
|
|
|
Log_Write_Compass_instance(time_us, 1, LOG_COMPASS2_MSG);
|
2015-01-16 18:27:01 -04:00
|
|
|
}
|
2015-10-14 13:05:36 -03:00
|
|
|
|
2015-01-16 18:27:01 -04:00
|
|
|
if (compass.get_count() > 2) {
|
2018-06-25 08:57:09 -03:00
|
|
|
Log_Write_Compass_instance(time_us, 2, LOG_COMPASS3_MSG);
|
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.
|
2016-01-25 19:45:27 -04:00
|
|
|
bool DataFlash_Backend::Log_Write_Mode(uint8_t mode, uint8_t reason)
|
2014-12-22 16:10:02 -04:00
|
|
|
{
|
|
|
|
struct log_Mode pkt = {
|
|
|
|
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,
|
|
|
|
mode_reason : 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
|
|
|
}
|
|
|
|
|
2014-11-20 06:53:12 -04:00
|
|
|
// Write ESC status messages
|
2014-11-13 06:38:33 -04:00
|
|
|
void DataFlash_Class::Log_Write_ESC(void)
|
|
|
|
{
|
2014-11-20 23:48:44 -04:00
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
2014-11-13 06:38:33 -04:00
|
|
|
static int _esc_status_sub = -1;
|
2014-11-20 06:53:12 -04:00
|
|
|
struct esc_status_s esc_status;
|
2014-11-13 06:38:33 -04:00
|
|
|
|
2014-11-20 06:53:12 -04:00
|
|
|
if (_esc_status_sub == -1) {
|
2014-11-13 06:38:33 -04:00
|
|
|
// subscribe to ORB topic on first call
|
2016-11-22 11:34:53 -04:00
|
|
|
_esc_status_sub = orb_subscribe(ORB_ID(esc_status));
|
|
|
|
}
|
2014-11-13 06:38:33 -04:00
|
|
|
|
|
|
|
// check for new ESC status data
|
|
|
|
bool esc_updated = false;
|
|
|
|
orb_check(_esc_status_sub, &esc_updated);
|
|
|
|
if (esc_updated && (OK == orb_copy(ORB_ID(esc_status), _esc_status_sub, &esc_status))) {
|
2014-11-20 06:53:12 -04:00
|
|
|
if (esc_status.esc_count > 8) {
|
|
|
|
esc_status.esc_count = 8;
|
|
|
|
}
|
2015-11-19 23:15:08 -04:00
|
|
|
uint64_t time_us = AP_HAL::micros64();
|
2014-11-20 06:53:12 -04:00
|
|
|
for (uint8_t i = 0; i < esc_status.esc_count; i++) {
|
|
|
|
// skip logging ESCs with a esc_address of zero, and this
|
|
|
|
// are probably not populated. The Pixhawk itself should
|
|
|
|
// be address zero
|
|
|
|
if (esc_status.esc[i].esc_address != 0) {
|
|
|
|
struct log_Esc pkt = {
|
|
|
|
LOG_PACKET_HEADER_INIT((uint8_t)(LOG_ESC1_MSG + i)),
|
2015-05-21 22:34:05 -03:00
|
|
|
time_us : time_us,
|
2018-04-02 01:21:56 -03:00
|
|
|
rpm : (int32_t)(esc_status.esc[i].esc_rpm/10),
|
|
|
|
voltage : (uint16_t)(esc_status.esc[i].esc_voltage*100.0f + .5f),
|
|
|
|
current : (uint16_t)(esc_status.esc[i].esc_current*100.0f + .5f),
|
|
|
|
temperature : (int16_t)(esc_status.esc[i].esc_temperature*100.0f + .5f),
|
|
|
|
current_tot : 0
|
2014-11-20 06:53:12 -04:00
|
|
|
};
|
|
|
|
|
|
|
|
WriteBlock(&pkt, sizeof(pkt));
|
2014-11-13 06:38:33 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2014-11-20 23:48:44 -04:00
|
|
|
#endif // CONFIG_HAL_BOARD
|
2014-11-13 06:38:33 -04:00
|
|
|
}
|
2015-01-19 18:10:33 -04:00
|
|
|
|
|
|
|
// Write a AIRSPEED packet
|
|
|
|
void DataFlash_Class::Log_Write_Airspeed(AP_Airspeed &airspeed)
|
|
|
|
{
|
2017-11-03 03:37:40 -03:00
|
|
|
uint64_t now = AP_HAL::micros64();
|
|
|
|
for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
|
|
|
|
if (!airspeed.enabled(i)) {
|
|
|
|
continue;
|
|
|
|
}
|
|
|
|
float temperature;
|
|
|
|
if (!airspeed.get_temperature(i, temperature)) {
|
|
|
|
temperature = 0;
|
|
|
|
}
|
|
|
|
struct log_AIRSPEED pkt = {
|
|
|
|
LOG_PACKET_HEADER_INIT(i==0?LOG_ARSP_MSG:LOG_ASP2_MSG),
|
|
|
|
time_us : now,
|
|
|
|
airspeed : airspeed.get_raw_airspeed(i),
|
|
|
|
diffpressure : airspeed.get_differential_pressure(i),
|
|
|
|
temperature : (int16_t)(temperature * 100.0f),
|
|
|
|
rawpressure : airspeed.get_corrected_pressure(i),
|
|
|
|
offset : airspeed.get_offset(i),
|
2017-11-03 07:28:20 -03:00
|
|
|
use : airspeed.use(i),
|
|
|
|
healthy : airspeed.healthy(i),
|
|
|
|
primary : airspeed.get_primary()
|
2017-11-03 03:37:40 -03:00
|
|
|
};
|
|
|
|
WriteBlock(&pkt, sizeof(pkt));
|
2015-01-19 18:10:33 -04:00
|
|
|
}
|
|
|
|
}
|
2015-05-21 22:42:08 -03:00
|
|
|
|
|
|
|
// Write a Yaw PID packet
|
|
|
|
void DataFlash_Class::Log_Write_PID(uint8_t msg_type, const PID_Info &info)
|
|
|
|
{
|
|
|
|
struct log_PID pkt = {
|
|
|
|
LOG_PACKET_HEADER_INIT(msg_type),
|
2015-11-19 23:15:08 -04:00
|
|
|
time_us : AP_HAL::micros64(),
|
2015-05-23 18:50:32 -03:00
|
|
|
desired : info.desired,
|
2015-05-21 22:42:08 -03:00
|
|
|
P : info.P,
|
|
|
|
I : info.I,
|
|
|
|
D : info.D,
|
2015-05-22 20:57:49 -03:00
|
|
|
FF : info.FF,
|
|
|
|
AFF : info.AFF
|
2015-05-21 22:42:08 -03:00
|
|
|
};
|
|
|
|
WriteBlock(&pkt, sizeof(pkt));
|
|
|
|
}
|
2015-07-03 08:49:45 -03:00
|
|
|
|
|
|
|
void DataFlash_Class::Log_Write_Origin(uint8_t origin_type, const Location &loc)
|
|
|
|
{
|
2015-11-19 23:15:08 -04:00
|
|
|
uint64_t time_us = AP_HAL::micros64();
|
2015-07-03 08:49:45 -03:00
|
|
|
struct log_ORGN pkt = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_ORGN_MSG),
|
|
|
|
time_us : time_us,
|
|
|
|
origin_type : origin_type,
|
|
|
|
latitude : loc.lat,
|
|
|
|
longitude : loc.lng,
|
|
|
|
altitude : loc.alt
|
|
|
|
};
|
|
|
|
WriteBlock(&pkt, sizeof(pkt));
|
|
|
|
}
|
2015-08-07 07:34:14 -03:00
|
|
|
|
|
|
|
void DataFlash_Class::Log_Write_RPM(const AP_RPM &rpm_sensor)
|
|
|
|
{
|
|
|
|
struct log_RPM pkt = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_RPM_MSG),
|
2015-11-19 23:15:08 -04:00
|
|
|
time_us : AP_HAL::micros64(),
|
2015-08-07 07:34:14 -03:00
|
|
|
rpm1 : rpm_sensor.get_rpm(0),
|
|
|
|
rpm2 : rpm_sensor.get_rpm(1)
|
|
|
|
};
|
|
|
|
WriteBlock(&pkt, sizeof(pkt));
|
2016-01-28 18:51:52 -04:00
|
|
|
}
|
2016-03-24 22:11:11 -03:00
|
|
|
|
2017-04-18 11:43:03 -03:00
|
|
|
// Write a rate packet
|
2016-03-24 22:11:11 -03:00
|
|
|
void DataFlash_Class::Log_Write_Rate(const AP_AHRS &ahrs,
|
|
|
|
const AP_Motors &motors,
|
|
|
|
const AC_AttitudeControl &attitude_control,
|
|
|
|
const AC_PosControl &pos_control)
|
|
|
|
{
|
|
|
|
const Vector3f &rate_targets = attitude_control.rate_bf_targets();
|
|
|
|
const Vector3f &accel_target = pos_control.get_accel_target();
|
|
|
|
struct log_Rate pkt_rate = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_RATE_MSG),
|
|
|
|
time_us : AP_HAL::micros64(),
|
2016-06-18 07:47:45 -03:00
|
|
|
control_roll : degrees(rate_targets.x),
|
|
|
|
roll : degrees(ahrs.get_gyro().x),
|
2016-03-24 22:11:11 -03:00
|
|
|
roll_out : motors.get_roll(),
|
2016-06-18 07:47:45 -03:00
|
|
|
control_pitch : degrees(rate_targets.y),
|
|
|
|
pitch : degrees(ahrs.get_gyro().y),
|
2016-03-24 22:11:11 -03:00
|
|
|
pitch_out : motors.get_pitch(),
|
2016-06-18 07:47:45 -03:00
|
|
|
control_yaw : degrees(rate_targets.z),
|
|
|
|
yaw : degrees(ahrs.get_gyro().z),
|
2016-03-24 22:11:11 -03:00
|
|
|
yaw_out : motors.get_yaw(),
|
|
|
|
control_accel : (float)accel_target.z,
|
|
|
|
accel : (float)(-(ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f),
|
|
|
|
accel_out : motors.get_throttle()
|
|
|
|
};
|
|
|
|
WriteBlock(&pkt_rate, sizeof(pkt_rate));
|
|
|
|
}
|
2016-07-03 23:14:26 -03:00
|
|
|
|
|
|
|
// Write rally points
|
|
|
|
void DataFlash_Class::Log_Write_Rally(const AP_Rally &rally)
|
|
|
|
{
|
|
|
|
RallyLocation rally_point;
|
|
|
|
for (uint8_t i=0; i<rally.get_rally_total(); i++) {
|
|
|
|
if (rally.get_rally_point_with_index(i, rally_point)) {
|
|
|
|
struct log_Rally pkt_rally = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_RALLY_MSG),
|
|
|
|
time_us : AP_HAL::micros64(),
|
|
|
|
total : rally.get_rally_total(),
|
|
|
|
sequence : i,
|
|
|
|
latitude : rally_point.lat,
|
|
|
|
longitude : rally_point.lng,
|
|
|
|
altitude : rally_point.alt
|
|
|
|
};
|
|
|
|
WriteBlock(&pkt_rally, sizeof(pkt_rally));
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2017-04-03 23:47:04 -03:00
|
|
|
|
|
|
|
// Write visual odometry sensor data
|
|
|
|
void DataFlash_Class::Log_Write_VisualOdom(float time_delta, const Vector3f &angle_delta, const Vector3f &position_delta, float confidence)
|
|
|
|
{
|
|
|
|
struct log_VisualOdom pkt_visualodom = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_VISUALODOM_MSG),
|
|
|
|
time_us : AP_HAL::micros64(),
|
|
|
|
time_delta : time_delta,
|
|
|
|
angle_delta_x : angle_delta.x,
|
|
|
|
angle_delta_y : angle_delta.y,
|
|
|
|
angle_delta_z : angle_delta.z,
|
|
|
|
position_delta_x : position_delta.x,
|
|
|
|
position_delta_y : position_delta.y,
|
|
|
|
position_delta_z : position_delta.z,
|
|
|
|
confidence : confidence
|
|
|
|
};
|
|
|
|
WriteBlock(&pkt_visualodom, sizeof(log_VisualOdom));
|
|
|
|
}
|
2017-04-09 08:17:17 -03:00
|
|
|
|
|
|
|
// Write AOA and SSA
|
|
|
|
void DataFlash_Class::Log_Write_AOA_SSA(AP_AHRS &ahrs)
|
|
|
|
{
|
|
|
|
struct log_AOA_SSA aoa_ssa = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_AOA_SSA_MSG),
|
|
|
|
time_us : AP_HAL::micros64(),
|
|
|
|
AOA : ahrs.getAOA(),
|
|
|
|
SSA : ahrs.getSSA()
|
|
|
|
};
|
|
|
|
|
|
|
|
WriteBlock(&aoa_ssa, sizeof(aoa_ssa));
|
|
|
|
}
|
2017-04-18 11:43:03 -03:00
|
|
|
|
|
|
|
// Write beacon sensor (position) data
|
|
|
|
void DataFlash_Class::Log_Write_Beacon(AP_Beacon &beacon)
|
|
|
|
{
|
2018-02-22 00:53:55 -04:00
|
|
|
if (!beacon.enabled()) {
|
|
|
|
return;
|
|
|
|
}
|
2017-04-18 11:43:03 -03:00
|
|
|
// position
|
|
|
|
Vector3f pos;
|
|
|
|
float accuracy = 0.0f;
|
|
|
|
beacon.get_vehicle_position_ned(pos, accuracy);
|
|
|
|
|
|
|
|
struct log_Beacon pkt_beacon = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_BEACON_MSG),
|
|
|
|
time_us : AP_HAL::micros64(),
|
|
|
|
health : (uint8_t)beacon.healthy(),
|
|
|
|
count : (uint8_t)beacon.count(),
|
|
|
|
dist0 : beacon.beacon_distance(0),
|
|
|
|
dist1 : beacon.beacon_distance(1),
|
|
|
|
dist2 : beacon.beacon_distance(2),
|
|
|
|
dist3 : beacon.beacon_distance(3),
|
|
|
|
posx : pos.x,
|
|
|
|
posy : pos.y,
|
|
|
|
posz : pos.z
|
|
|
|
};
|
|
|
|
WriteBlock(&pkt_beacon, sizeof(pkt_beacon));
|
|
|
|
}
|
2017-07-14 14:00:13 -03:00
|
|
|
|
|
|
|
// Write proximity sensor distances
|
|
|
|
void DataFlash_Class::Log_Write_Proximity(AP_Proximity &proximity)
|
|
|
|
{
|
|
|
|
// exit immediately if not enabled
|
|
|
|
if (proximity.get_status() == AP_Proximity::Proximity_NotConnected) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2017-07-14 14:00:21 -03:00
|
|
|
AP_Proximity::Proximity_Distance_Array dist_array {};
|
|
|
|
proximity.get_horizontal_distances(dist_array);
|
2017-07-14 14:00:13 -03:00
|
|
|
|
|
|
|
float dist_up;
|
|
|
|
if (!proximity.get_upward_distance(dist_up)) {
|
|
|
|
dist_up = 0.0f;
|
|
|
|
}
|
|
|
|
|
|
|
|
float close_ang = 0.0f, close_dist = 0.0f;
|
|
|
|
proximity.get_closest_object(close_ang, close_dist);
|
|
|
|
|
|
|
|
struct log_Proximity pkt_proximity = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_PROXIMITY_MSG),
|
|
|
|
time_us : AP_HAL::micros64(),
|
|
|
|
health : (uint8_t)proximity.get_status(),
|
2017-07-14 14:00:21 -03:00
|
|
|
dist0 : dist_array.distance[0],
|
|
|
|
dist45 : dist_array.distance[1],
|
|
|
|
dist90 : dist_array.distance[2],
|
|
|
|
dist135 : dist_array.distance[3],
|
|
|
|
dist180 : dist_array.distance[4],
|
|
|
|
dist225 : dist_array.distance[5],
|
|
|
|
dist270 : dist_array.distance[6],
|
|
|
|
dist315 : dist_array.distance[7],
|
2017-07-14 14:00:13 -03:00
|
|
|
distup : dist_up,
|
|
|
|
closest_angle : close_ang,
|
|
|
|
closest_dist : close_dist
|
|
|
|
};
|
|
|
|
WriteBlock(&pkt_proximity, sizeof(pkt_proximity));
|
|
|
|
}
|
2017-07-26 14:09:33 -03:00
|
|
|
|
|
|
|
void DataFlash_Class::Log_Write_SRTL(bool active, uint16_t num_points, uint16_t max_points, uint8_t action, const Vector3f& breadcrumb)
|
|
|
|
{
|
|
|
|
struct log_SRTL pkt_srtl = {
|
|
|
|
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));
|
|
|
|
}
|