Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-01 13:38:38 -04:00
Code Issues Packages Projects Releases Wiki Activity

DataFlash: use caller supplied timestamps for sensor logging

Browse Source 
used to support EK2_LOGGING=1
This commit is contained in:
Andrew Tridgell 2016-05-05 13:37:16 +10:00
parent 6aa02f06a9
commit 35c3adb1ad
2 changed files with 22 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/DataFlash/DataFlash.h
View File

@ -105,15 +105,15 @@ public:
void StopLogging();
void Log_Write_Parameter(const char *name, float value);
void Log_Write_GPS(const AP_GPS &gps, uint8_t instance);
void Log_Write_GPS(const AP_GPS &gps, uint8_t instance, uint64_t time_us=0);
void Log_Write_RFND(const RangeFinder &rangefinder);
void Log_Write_IMU(const AP_InertialSensor &ins);
void Log_Write_IMUDT(const AP_InertialSensor &ins);
void Log_Write_IMUDT(const AP_InertialSensor &ins, uint64_t time_us);
void Log_Write_Vibration(const AP_InertialSensor &ins);
void Log_Write_RCIN(void);
void Log_Write_RCOUT(void);
void Log_Write_RSSI(AP_RSSI &rssi);
void Log_Write_Baro(AP_Baro &baro);
void Log_Write_Baro(AP_Baro &baro, uint64_t time_us=0);
void Log_Write_Power(void);
void Log_Write_AHRS2(AP_AHRS &ahrs);
void Log_Write_POS(AP_AHRS &ahrs);
@ -131,7 +131,7 @@ public:
void Log_Write_Airspeed(AP_Airspeed &airspeed);
void Log_Write_Attitude(AP_AHRS &ahrs, const Vector3f &targets);
void Log_Write_Current(const AP_BattMonitor &battery, int16_t throttle);
void Log_Write_Compass(const Compass &compass);
void Log_Write_Compass(const Compass &compass, uint64_t time_us=0);
void Log_Write_Mode(uint8_t mode, uint8_t reason = 0);
void Log_Write_EntireMission(const AP_Mission &mission);

29
libraries/DataFlash/LogFile.cpp
View File

@ -707,12 +707,15 @@ bool DataFlash_Backend::Log_Write_Parameter(const AP_Param *ap,
}
// Write an GPS packet
void DataFlash_Class::Log_Write_GPS(const AP_GPS &gps, uint8_t i)
void DataFlash_Class::Log_Write_GPS(const AP_GPS &gps, uint8_t i, uint64_t time_us)
{
if (time_us == 0) {
time_us = AP_HAL::micros64();
}
const struct Location &loc = gps.location(i);
struct log_GPS pkt = {
LOG_PACKET_HEADER_INIT((uint8_t)(LOG_GPS_MSG+i)),
time_us : AP_HAL::micros64(),
time_us : time_us,
status : (uint8_t)gps.status(i),
gps_week_ms : gps.time_week_ms(i),
gps_week : gps.time_week(i),
@ -735,7 +738,7 @@ void DataFlash_Class::Log_Write_GPS(const AP_GPS &gps, uint8_t i)
gps.speed_accuracy(i, sacc);
struct log_GPA pkt2 = {
LOG_PACKET_HEADER_INIT((uint8_t)(LOG_GPA_MSG+i)),
time_us : AP_HAL::micros64(),
time_us : time_us,
vdop : gps.get_vdop(i),
hacc : (uint16_t)(hacc*100),
vacc : (uint16_t)(vacc*100),
@ -817,9 +820,11 @@ void DataFlash_Class::Log_Write_RSSI(AP_RSSI &rssi)
}
// Write a BARO packet
void DataFlash_Class::Log_Write_Baro(AP_Baro &baro)
void DataFlash_Class::Log_Write_Baro(AP_Baro &baro, uint64_t time_us)
{
uint64_t time_us = AP_HAL::micros64();
if (time_us == 0) {
time_us = AP_HAL::micros64();
}
struct log_BARO pkt = {
LOG_PACKET_HEADER_INIT(LOG_BARO_MSG),
time_us : time_us,
@ -926,7 +931,7 @@ void DataFlash_Class::Log_Write_IMU(const AP_InertialSensor &ins)
}
// Write an accel/gyro delta time data packet
void DataFlash_Class::Log_Write_IMUDT(const AP_InertialSensor &ins)
void DataFlash_Class::Log_Write_IMUDT(const AP_InertialSensor &ins, uint64_t time_us)
{
float delta_t = ins.get_delta_time();
float delta_vel_t = ins.get_delta_velocity_dt(0);
@ -935,7 +940,6 @@ void DataFlash_Class::Log_Write_IMUDT(const AP_InertialSensor &ins)
ins.get_delta_angle(0, delta_angle);
ins.get_delta_velocity(0, delta_velocity);
uint64_t time_us = AP_HAL::micros64();
struct log_IMUDT pkt = {
LOG_PACKET_HEADER_INIT(LOG_IMUDT_MSG),
time_us : time_us,
@ -1635,14 +1639,17 @@ void DataFlash_Class::Log_Write_Current(const AP_BattMonitor &battery, int16_t t
}
// Write a Compass packet
void DataFlash_Class::Log_Write_Compass(const Compass &compass)
void DataFlash_Class::Log_Write_Compass(const Compass &compass, uint64_t time_us)
{
if (time_us == 0) {
time_us = AP_HAL::micros64();
}
const Vector3f &mag_field = compass.get_field(0);
const Vector3f &mag_offsets = compass.get_offsets(0);
const Vector3f &mag_motor_offsets = compass.get_motor_offsets(0);
struct log_Compass pkt = {
LOG_PACKET_HEADER_INIT(LOG_COMPASS_MSG),
time_us : AP_HAL::micros64(),
time_us : time_us,
mag_x : (int16_t)mag_field.x,
mag_y : (int16_t)mag_field.y,
mag_z : (int16_t)mag_field.z,
@ -1663,7 +1670,7 @@ void DataFlash_Class::Log_Write_Compass(const Compass &compass)
const Vector3f &mag_motor_offsets2 = compass.get_motor_offsets(1);
struct log_Compass pkt2 = {
LOG_PACKET_HEADER_INIT(LOG_COMPASS2_MSG),
time_us : AP_HAL::micros64(),
time_us : time_us,
mag_x : (int16_t)mag_field2.x,
mag_y : (int16_t)mag_field2.y,
mag_z : (int16_t)mag_field2.z,
@ -1685,7 +1692,7 @@ void DataFlash_Class::Log_Write_Compass(const Compass &compass)
const Vector3f &mag_motor_offsets3 = compass.get_motor_offsets(2);
struct log_Compass pkt3 = {
LOG_PACKET_HEADER_INIT(LOG_COMPASS3_MSG),
time_us : AP_HAL::micros64(),
time_us : time_us,
mag_x : (int16_t)mag_field3.x,
mag_y : (int16_t)mag_field3.y,
mag_z : (int16_t)mag_field3.z,