aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

DataFlash: factor out a Log_Write_IMU_instance

Browse Source
This commit is contained in:
Peter Barker 2017-09-29 12:34:58 +10:00 committed by Francisco Ferreira
parent f5b4d8d99c
commit a7063393b4
2 changed files with 27 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/DataFlash/DataFlash.h
View File

@ -274,6 +274,10 @@ private:
#endif
void Log_Write_Baro_instance(AP_Baro &baro, uint64_t time_us, uint8_t baro_instance, enum LogMessages type);
void Log_Write_IMU_instance(const AP_InertialSensor &ins,
uint64_t time_us,
uint8_t imu_instance,
enum LogMessages type);
void backend_starting_new_log(const DataFlash_Backend *backend);

76
libraries/DataFlash/LogFile.cpp
View File

@ -400,14 +400,12 @@ void DataFlash_Class::Log_Write_Baro(AP_Baro &baro, uint64_t time_us)
}
}
// Write an raw accel/gyro data packet
void DataFlash_Class::Log_Write_IMU(const AP_InertialSensor &ins)
void DataFlash_Class::Log_Write_IMU_instance(const AP_InertialSensor &ins, const uint64_t time_us, const uint8_t imu_instance, const enum LogMessages type)
{
uint64_t time_us = AP_HAL::micros64();
const Vector3f &gyro = ins.get_gyro(0);
const Vector3f &accel = ins.get_accel(0);
const Vector3f &gyro = ins.get_gyro(imu_instance);
const Vector3f &accel = ins.get_accel(imu_instance);
struct log_IMU pkt = {
LOG_PACKET_HEADER_INIT(LOG_IMU_MSG),
LOG_PACKET_HEADER_INIT(type),
time_us : time_us,
gyro_x : gyro.x,
gyro_y : gyro.y,
@ -415,62 +413,34 @@ void DataFlash_Class::Log_Write_IMU(const AP_InertialSensor &ins)
accel_x : accel.x,
accel_y : accel.y,
accel_z : accel.z,
gyro_error : ins.get_gyro_error_count(0),
accel_error : ins.get_accel_error_count(0),
temperature : ins.get_temperature(0),
gyro_health : (uint8_t)ins.get_gyro_health(0),
accel_health : (uint8_t)ins.get_accel_health(0),
gyro_rate : ins.get_gyro_rate_hz(0),
accel_rate : ins.get_accel_rate_hz(0),
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),
};
WriteBlock(&pkt, sizeof(pkt));
}
// Write an raw accel/gyro data packet
void DataFlash_Class::Log_Write_IMU(const AP_InertialSensor &ins)
{
uint64_t time_us = AP_HAL::micros64();
Log_Write_IMU_instance(ins, time_us, 0, LOG_IMU_MSG);
if (ins.get_gyro_count() < 2 && ins.get_accel_count() < 2) {
return;
}
const Vector3f &gyro2 = ins.get_gyro(1);
const Vector3f &accel2 = ins.get_accel(1);
struct log_IMU pkt2 = {
LOG_PACKET_HEADER_INIT(LOG_IMU2_MSG),
time_us : time_us,
gyro_x : gyro2.x,
gyro_y : gyro2.y,
gyro_z : gyro2.z,
accel_x : accel2.x,
accel_y : accel2.y,
accel_z : accel2.z,
gyro_error : ins.get_gyro_error_count(1),
accel_error : ins.get_accel_error_count(1),
temperature : ins.get_temperature(1),
gyro_health : (uint8_t)ins.get_gyro_health(1),
accel_health : (uint8_t)ins.get_accel_health(1),
gyro_rate : ins.get_gyro_rate_hz(1),
accel_rate : ins.get_accel_rate_hz(1),
};
WriteBlock(&pkt2, sizeof(pkt2));
Log_Write_IMU_instance(ins, time_us, 1, LOG_IMU2_MSG);
if (ins.get_gyro_count() < 3 && ins.get_accel_count() < 3) {
return;
}
const Vector3f &gyro3 = ins.get_gyro(2);
const Vector3f &accel3 = ins.get_accel(2);
struct log_IMU pkt3 = {
LOG_PACKET_HEADER_INIT(LOG_IMU3_MSG),
time_us : time_us,
gyro_x : gyro3.x,
gyro_y : gyro3.y,
gyro_z : gyro3.z,
accel_x : accel3.x,
accel_y : accel3.y,
accel_z : accel3.z,
gyro_error : ins.get_gyro_error_count(2),
accel_error : ins.get_accel_error_count(2),
temperature : ins.get_temperature(2),
gyro_health : (uint8_t)ins.get_gyro_health(2),
accel_health : (uint8_t)ins.get_accel_health(2),
gyro_rate : ins.get_gyro_rate_hz(2),
accel_rate : ins.get_accel_rate_hz(2),
};
WriteBlock(&pkt3, sizeof(pkt3));
Log_Write_IMU_instance(ins, time_us, 2, LOG_IMU3_MSG);
}
// Write an accel/gyro delta time data packet