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

Plane: added dataflash logging of EKF data

Browse Source
This commit is contained in:
Andrew Tridgell 2013-12-30 22:24:36 +11:00
parent df42dd691c
commit ce9047cec5
3 changed files with 42 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduPlane/ArduPlane.pde
View File

@ -807,8 +807,10 @@ static void ahrs_update()
ahrs.update();
NavEKF.UpdateFilter();
if (should_log(MASK_LOG_ATTITUDE_FAST))
if (should_log(MASK_LOG_ATTITUDE_FAST)) {
Log_Write_Attitude();
Log_Write_EKF();
}
if (should_log(MASK_LOG_IMU))
Log_Write_IMU();
@ -889,8 +891,10 @@ static void barometer_accumulate(void)
*/
static void update_logging1(void)
{
if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST))
if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) {
Log_Write_Attitude();
Log_Write_EKF();
}
if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_IMU))
Log_Write_IMU();

36
ArduPlane/Log.pde
View File

@ -174,7 +174,7 @@ struct PACKED log_Attitude {
uint16_t error_yaw;
};
// Write an attitude packet. Total length : 10 bytes
// Write an attitude packet
static void Log_Write_Attitude(void)
{
struct log_Attitude pkt = {
@ -189,6 +189,37 @@ static void Log_Write_Attitude(void)
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
struct PACKED log_EKF {
LOG_PACKET_HEADER;
uint32_t time_ms;
int16_t roll;
int16_t pitch;
uint16_t yaw;
float alt;
int32_t lat;
int32_t lon;
};
// Write an EKF packet
static void Log_Write_EKF(void)
{
Vector3f euler;
struct Location loc;
NavEKF.getEulerAngles(euler);
NavEKF.getLLH(loc);
struct log_EKF pkt = {
LOG_PACKET_HEADER_INIT(LOG_EKF_MSG),
time_ms : hal.scheduler->millis(),
roll : (int16_t)(100*degrees(euler.x)),
pitch : (int16_t)(100*degrees(euler.y)),
yaw : (uint16_t)wrap_360_cd(100*degrees(euler.z)),
alt : loc.alt*1.0e-2f,
lat : loc.lat,
lon : loc.lng
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
struct PACKED log_Performance {
LOG_PACKET_HEADER;
uint32_t loop_time;
@ -582,6 +613,8 @@ static const struct LogStructure log_structure[] PROGMEM = {
"ARM", "IHB", "TimeMS,ArmState,ArmChecks" },
{ LOG_AIRSPEED_MSG, sizeof(log_AIRSPEED),
"ARSP", "Iffc", "TimeMS,Airspeed,DiffPress,Temp" },
{ LOG_EKF_MSG, sizeof(log_EKF),
"EKF", "IccCfLL", "TimeMS,Roll,Pitch,Yaw,Alt,Lat,Lng" },
TECS_LOG_FORMAT(LOG_TECS_MSG),
};
@ -625,6 +658,7 @@ static void Log_Write_Nav_Tuning() {}
static void Log_Write_TECS_Tuning() {}
static void Log_Write_Performance() {}
static void Log_Write_Attitude() {}
static void Log_Write_EKF() {}
static void Log_Write_Control_Tuning() {}
static void Log_Write_Camera() {}
static void Log_Write_Mode(uint8_t mode) {}

1
ArduPlane/defines.h
View File

@ -144,6 +144,7 @@ enum log_messages {
LOG_COMPASS2_MSG,
LOG_ARM_DISARM_MSG,
LOG_AIRSPEED_MSG,
LOG_EKF_MSG,
MAX_NUM_LOGS // always at the end
};