ardupilot/AntennaTracker/Log.cpp

105 lines
2.9 KiB
C++
Raw Normal View History

2015-12-27 03:05:14 -04:00
#include "Tracker.h"
#if LOGGING_ENABLED == ENABLED
// Code to Write and Read packets from DataFlash log memory
// Write an attitude packet
void Tracker::Log_Write_Attitude()
{
Vector3f targets;
targets.y = nav_status.pitch * 100.0f;
targets.z = wrap_360_cd(nav_status.bearing * 100.0f);
2015-12-27 03:05:14 -04:00
DataFlash.Log_Write_Attitude(ahrs, targets);
DataFlash.Log_Write_EKF(ahrs);
2015-12-27 03:05:14 -04:00
DataFlash.Log_Write_AHRS2(ahrs);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
sitl.Log_Write_SIMSTATE(&DataFlash);
#endif
DataFlash.Log_Write_POS(ahrs);
}
void Tracker::Log_Write_Baro(void)
{
2018-03-05 17:05:20 -04:00
DataFlash.Log_Write_Baro();
2015-12-27 03:05:14 -04:00
}
2016-05-12 02:17:31 -03:00
struct PACKED log_Vehicle_Baro {
LOG_PACKET_HEADER;
uint64_t time_us;
float press;
float alt_diff;
};
// Write a vehicle baro packet
void Tracker::Log_Write_Vehicle_Baro(float pressure, float altitude)
{
struct log_Vehicle_Baro pkt = {
LOG_PACKET_HEADER_INIT(LOG_V_BAR_MSG),
time_us : AP_HAL::micros64(),
press : pressure,
alt_diff : altitude
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
2016-06-09 03:33:07 -03:00
struct PACKED log_Vehicle_Pos {
LOG_PACKET_HEADER;
uint64_t time_us;
int32_t vehicle_lat;
int32_t vehicle_lng;
int32_t vehicle_alt;
float vehicle_vel_x;
float vehicle_vel_y;
float vehicle_vel_z;
2016-06-09 03:33:07 -03:00
};
// Write a vehicle pos packet
void Tracker::Log_Write_Vehicle_Pos(int32_t lat, int32_t lng, int32_t alt, const Vector3f& vel)
2016-06-09 11:13:51 -03:00
{
2016-06-09 03:33:07 -03:00
struct log_Vehicle_Pos pkt = {
LOG_PACKET_HEADER_INIT(LOG_V_POS_MSG),
time_us : AP_HAL::micros64(),
2016-06-09 11:13:51 -03:00
vehicle_lat : lat,
vehicle_lng : lng,
vehicle_alt : alt,
vehicle_vel_x : vel.x,
vehicle_vel_y : vel.y,
vehicle_vel_z : vel.z,
2016-06-09 03:33:07 -03:00
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
2016-05-12 02:17:31 -03:00
2017-09-05 21:53:00 -03:00
// type and unit information can be found in
// libraries/DataFlash/Logstructure.h; search for "log_Units" for
// units and "Format characters" for field type information
2015-12-27 03:05:14 -04:00
const struct LogStructure Tracker::log_structure[] = {
LOG_COMMON_STRUCTURES,
2016-05-12 02:17:31 -03:00
{LOG_V_BAR_MSG, sizeof(log_Vehicle_Baro),
2017-09-05 21:53:00 -03:00
"VBAR", "Qff", "TimeUS,Press,AltDiff", "sPm", "F00" },
2016-06-09 03:33:07 -03:00
{LOG_V_POS_MSG, sizeof(log_Vehicle_Pos),
2017-09-05 21:53:00 -03:00
"VPOS", "QLLefff", "TimeUS,Lat,Lng,Alt,VelX,VelY,VelZ", "sddmnnn", "FGGB000" }
2015-12-27 03:05:14 -04:00
};
void Tracker::Log_Write_Vehicle_Startup_Messages()
{
2018-02-21 19:02:00 -04:00
DataFlash.Log_Write_Mode(control_mode, MODE_REASON_INITIALISED);
gps.Write_DataFlash_Log_Startup_messages();
2015-12-27 03:05:14 -04:00
}
void Tracker::log_init(void)
{
DataFlash.Init(log_structure, ARRAY_SIZE(log_structure));
}
#else // LOGGING_ENABLED
void Tracker::Log_Write_Attitude(void) {}
void Tracker::Log_Write_Baro(void) {}
void Tracker::log_init(void) {}
void Tracker::Log_Write_Vehicle_Pos(int32_t lat, int32_t lng, int32_t alt, const Vector3f& vel) {}
void Tracker::Log_Write_Vehicle_Baro(float pressure, float altitude) {}
2015-12-27 03:05:14 -04:00
#endif // LOGGING_ENABLED