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

DataFlash: added logging of AHRS2

This commit is contained in:
Andrew Tridgell 2014-01-03 16:01:08 +11:00
parent 5578552574
commit d0a25b53f2
2 changed files with 50 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/DataFlash/DataFlash.h
View File

@ -11,6 +11,7 @@
#include <AP_GPS.h>
#include <AP_InertialSensor.h>
#include <AP_Baro.h>
#include <AP_AHRS.h>
#include <stdint.h>
class DataFlash_Class
@ -52,6 +53,7 @@ public:
void Log_Write_RCOUT(void);
void Log_Write_Baro(AP_Baro &baro);
void Log_Write_Power(void);
void Log_Write_AHRS2(AP_AHRS &ahrs);
void Log_Write_Message(const char *message);
void Log_Write_Message_P(const prog_char_t *message);
@ -210,6 +212,16 @@ struct PACKED log_BARO {
float altitude;
float pressure;
int16_t temperature;
struct PACKED log_AHRS {
LOG_PACKET_HEADER;
uint32_t time_ms;
int16_t roll;
int16_t pitch;
uint16_t yaw;
float alt;
int32_t lat;
int32_t lng;
};
struct PACKED log_POWR {
@ -240,7 +252,11 @@ struct PACKED log_POWR {
{ LOG_BARO_MSG, sizeof(log_BARO), \
"BARO", "Iffc", "TimeMS,Alt,Press,Temp" }, \
{ LOG_POWR_MSG, sizeof(log_POWR), \
"POWR","ICCH","TimeMS,Vcc,VServo,Flags" }
"POWR","ICCH","TimeMS,Vcc,VServo,Flags" } \
{ LOG_AHR2_MSG, sizeof(log_AHRS), \
"AHR2","IccCfLL","TimeMS,Roll,Pitch,Yaw,Alt,Lat,Lng" }, \
{ LOG_SIMSTATE_MSG, sizeof(log_AHRS), \
"SIM","IccCfLL","TimeMS,Roll,Pitch,Yaw,Alt,Lat,Lng" }
// message types for common messages
#define LOG_FORMAT_MSG 128
@ -253,6 +269,8 @@ struct PACKED log_POWR {
#define LOG_IMU2_MSG 135
#define LOG_BARO_MSG 136
#define LOG_POWR_MSG 137
#define LOG_AHR2_MSG 138
#define LOG_SIMSTATE_MSG 139
#include "DataFlash_Block.h"
#include "DataFlash_File.h"

22
libraries/DataFlash/LogFile.cpp
View File

@ -6,6 +6,7 @@
#include <AP_Param.h>
#include <AP_Math.h>
#include <AP_Baro.h>
#include <AP_AHRS.h>
extern const AP_HAL::HAL& hal;
@ -793,3 +794,24 @@ void DataFlash_Class::Log_Write_Power(void)
#endif
}
// Write an AHRS2 packet
void DataFlash_Class::Log_Write_AHRS2(AP_AHRS &ahrs)
{
Vector3f euler;
struct Location loc;
if (!ahrs.get_secondary_attitude(euler) || !ahrs.get_secondary_position(loc)) {
return;
}
struct log_AHRS pkt = {
LOG_PACKET_HEADER_INIT(LOG_AHR2_MSG),
time_ms : hal.scheduler->millis(),
roll : (int16_t)(degrees(euler.x)*100),
pitch : (int16_t)(degrees(euler.y)*100),
yaw : (uint16_t)(wrap_360_cd(degrees(euler.z)*100)),
alt : loc.alt*1.0e-2f,
lat : loc.lat,
lng : loc.lng
};
WriteBlock(&pkt, sizeof(pkt));
}