Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 09:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

DataFlash: added Log_Write_IMU2() for logging 2nd INS sensors

Browse Source
This commit is contained in:
Andrew Tridgell 2013-12-08 20:45:04 +11:00
parent d9b6f7f0f7
commit eb883fbb0c
2 changed files with 27 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/DataFlash/DataFlash.h
View File

@ -47,6 +47,7 @@ public:
void Log_Write_Parameter(const char *name, float value);
void Log_Write_GPS(const GPS *gps, int32_t relative_alt);
void Log_Write_IMU(const AP_InertialSensor &ins);
void Log_Write_IMU2(const AP_InertialSensor &ins);
void Log_Write_RCIN(void);
void Log_Write_RCOUT(void);
void Log_Write_Message(const char *message);
@ -204,6 +205,8 @@ struct PACKED log_RCOUT {
"GPS", "BIHBcLLeeEefI", "Status,TimeMS,Week,NSats,HDop,Lat,Lng,RelAlt,Alt,Spd,GCrs,VZ,T" }, \
{ LOG_IMU_MSG, sizeof(log_IMU), \
"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_MESSAGE_MSG, sizeof(log_Message), \
"MSG", "Z", "Message"}, \
{ LOG_RCIN_MSG, sizeof(log_RCIN), \
@ -219,6 +222,7 @@ struct PACKED log_RCOUT {
#define LOG_MESSAGE_MSG 132
#define LOG_RCIN_MSG 133
#define LOG_RCOUT_MSG 134
#define LOG_IMU2_MSG 135
#include "DataFlash_Block.h"
#include "DataFlash_File.h"

23
libraries/DataFlash/LogFile.cpp
View File

@ -702,6 +702,29 @@ void DataFlash_Class::Log_Write_IMU(const AP_InertialSensor &ins)
WriteBlock(&pkt, sizeof(pkt));
}
// Write an raw accel/gyro data packet
void DataFlash_Class::Log_Write_IMU2(const AP_InertialSensor &ins)
{
if (ins.get_gyro_count() < 2 && ins.get_accel_count() < 2) {
return;
}
Vector3f gyro;
Vector3f accel;
ins.get_gyro_instance(1, gyro);
ins.get_accel_instance(1, accel);
struct log_IMU pkt = {
LOG_PACKET_HEADER_INIT(LOG_IMU2_MSG),
timestamp : hal.scheduler->millis(),
gyro_x : gyro.x,
gyro_y : gyro.y,
gyro_z : gyro.z,
accel_x : accel.x,
accel_y : accel.y,
accel_z : accel.z
};
WriteBlock(&pkt, sizeof(pkt));
}
// Write a text message to the log
void DataFlash_Class::Log_Write_Message(const char *message)
{