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

Dataflash: add IMT messages

Browse Source
This commit is contained in:
Randy Mackay 2015-06-13 22:06:49 +09:00 committed by Andrew Tridgell
parent ac09a61238
commit 384755e271
2 changed files with 99 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/DataFlash/DataFlash.h
View File

@ -67,6 +67,7 @@ public:
void Log_Write_Parameter(const char *name, float value);
void Log_Write_GPS(const AP_GPS &gps, uint8_t instance, int32_t relative_alt);
void Log_Write_IMU(const AP_InertialSensor &ins);
void Log_Write_IMUDT(const AP_InertialSensor &ins);
void Log_Write_Vibration(const AP_InertialSensor &ins);
void Log_Write_RCIN(void);
void Log_Write_RCOUT(void);
@ -227,6 +228,14 @@ struct PACKED log_IMU {
uint8_t gyro_health, accel_health;
};
struct PACKED log_IMUDT {
LOG_PACKET_HEADER;
uint64_t time_us;
float delta_time, delta_vel_dt;
float delta_ang_x, delta_ang_y, delta_ang_z;
float delta_vel_x, delta_vel_y, delta_vel_z;
};
struct PACKED log_Vibe {
LOG_PACKET_HEADER;
uint64_t time_us;
@ -717,8 +726,13 @@ Format characters in the format string for binary log messages
{ LOG_BAR2_MSG, sizeof(log_BARO), \
"BAR2", "Qffcf", "TimeUS,Alt,Press,Temp,CRt" }, \
{ LOG_VIBE_MSG, sizeof(log_Vibe), \
"VIBE", "QfffIII", "TimeUS,VibeX,VibeY,VibeZ,Clip0,Clip1,Clip2" }
"VIBE", "QfffIII", "TimeUS,VibeX,VibeY,VibeZ,Clip0,Clip1,Clip2" }, \
{ LOG_IMUDT_MSG, sizeof(log_IMUDT), \
"IMT","Qffffffff","TimeUS,DelT,DelvT,DelAX,DelAY,DelAZ,DelVX,DelVY,DelVZ" }, \
{ LOG_IMUDT2_MSG, sizeof(log_IMUDT), \
"IMT2","Qffffffff","TimeUS,DelT,DelvT,DelAX,DelAY,DelAZ,DelVX,DelVY,DelVZ" }, \
{ LOG_IMUDT3_MSG, sizeof(log_IMUDT), \
"IMT3","Qffffffff","TimeUS,DelT,DelvT,DelAX,DelAY,DelAZ,DelVX,DelVY,DelVZ" }
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
#define LOG_COMMON_STRUCTURES LOG_BASE_STRUCTURES, LOG_EXTRA_STRUCTURES
@ -785,6 +799,9 @@ Format characters in the format string for binary log messages
#define LOG_PIDY_MSG 181
#define LOG_PIDA_MSG 182
#define LOG_VIBE_MSG 183
#define LOG_IMUDT_MSG 184
#define LOG_IMUDT2_MSG 185
#define LOG_IMUDT3_MSG 186
// message types 200 to 210 reversed for GPS driver use
// message types 211 to 220 reversed for autotune use

80
libraries/DataFlash/LogFile.cpp
View File

@ -868,6 +868,86 @@ void DataFlash_Class::Log_Write_IMU(const AP_InertialSensor &ins)
#endif
}
// Write an accel/gyro delta time data packet
void DataFlash_Class::Log_Write_IMUDT(const AP_InertialSensor &ins)
{
float delta_t = ins.get_delta_time();
float delta_vel_t = ins.get_delta_velocity_dt(0);
Vector3f delta_angle, delta_velocity;
ins.get_delta_angle(0, delta_angle);
ins.get_delta_velocity(0, delta_velocity);
uint64_t time_us = hal.scheduler->micros64();
const Vector3f &gyro = ins.get_gyro(0);
const Vector3f &accel = ins.get_accel(0);
struct log_IMUDT pkt = {
LOG_PACKET_HEADER_INIT(LOG_IMUDT_MSG),
time_us : time_us,
delta_time : delta_t,
delta_vel_dt : delta_vel_t,
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
};
WriteBlock(&pkt, sizeof(pkt));
if (ins.get_gyro_count() < 2 && ins.get_accel_count() < 2) {
return;
}
#if INS_MAX_INSTANCES > 1
delta_vel_t = ins.get_delta_velocity_dt(1);
const Vector3f &gyro2 = ins.get_gyro(1);
const Vector3f &accel2 = ins.get_accel(1);
if (!ins.get_delta_angle(1, delta_angle)) {
delta_angle.zero();
}
if (!ins.get_delta_velocity(1, delta_velocity)) {
delta_velocity.zero();
}
struct log_IMUDT pkt2 = {
LOG_PACKET_HEADER_INIT(LOG_IMUDT2_MSG),
time_us : time_us,
delta_time : delta_t,
delta_vel_dt : delta_vel_t,
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
};
WriteBlock(&pkt2, sizeof(pkt2));
if (ins.get_gyro_count() < 3 && ins.get_accel_count() < 3) {
return;
}
delta_vel_t = ins.get_delta_velocity_dt(1);
const Vector3f &gyro3 = ins.get_gyro(2);
const Vector3f &accel3 = ins.get_accel(2);
if (!ins.get_delta_angle(2, delta_angle)) {
delta_angle.zero();
}
if (!ins.get_delta_velocity(2, delta_velocity)) {
delta_velocity.zero();
}
struct log_IMUDT pkt3 = {
LOG_PACKET_HEADER_INIT(LOG_IMUDT3_MSG),
time_us : time_us,
delta_time : delta_t,
delta_vel_dt : delta_vel_t,
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
};
WriteBlock(&pkt3, sizeof(pkt3));
#endif
}
void DataFlash_Class::Log_Write_Vibration(const AP_InertialSensor &ins)
{
#if INS_VIBRATION_CHECK