Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

DataFlash: allow logging of 3 accels/gyros

This commit is contained in:
Andrew Tridgell 2014-06-26 14:04:55 +10:00
parent ca12592448
commit e6d8e329d5
2 changed files with 19 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/DataFlash/DataFlash.h
View File

@ -381,6 +381,8 @@ struct PACKED log_Camera {
"IMU", "Iffffff", "TimeMS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ" }, \
{ LOG_IMU2_MSG, sizeof(log_IMU), \
"IMU2", "Iffffff", "TimeMS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ" }, \
{ LOG_IMU3_MSG, sizeof(log_IMU), \
"IMU3", "Iffffff", "TimeMS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ" }, \
{ LOG_MESSAGE_MSG, sizeof(log_Message), \
"MSG", "Z", "Message"}, \
{ LOG_RCIN_MSG, sizeof(log_RCIN), \
@ -434,6 +436,7 @@ struct PACKED log_Camera {
#define LOG_RADIO_MSG 146
#define LOG_ATRP_MSG 147
#define LOG_CAMERA_MSG 148
#define LOG_IMU3_MSG 149
// message types 200 to 210 reversed for GPS driver use
// message types 211 to 220 reversed for autotune use

16
libraries/DataFlash/LogFile.cpp
View File

@ -798,6 +798,22 @@ void DataFlash_Class::Log_Write_IMU(const AP_InertialSensor &ins)
accel_z : accel2.z
};
WriteBlock(&pkt2, sizeof(pkt2));
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),
timestamp : tstamp,
gyro_x : gyro3.x,
gyro_y : gyro3.y,
gyro_z : gyro3.z,
accel_x : accel3.x,
accel_y : accel3.y,
accel_z : accel3.z
};
WriteBlock(&pkt3, sizeof(pkt3));
}
// Write a text message to the log