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_GPS.h>
#include <AP_InertialSensor.h> #include <AP_InertialSensor.h>
#include <AP_Baro.h> #include <AP_Baro.h>
#include <AP_AHRS.h>
#include <stdint.h> #include <stdint.h>
class DataFlash_Class class DataFlash_Class
@ -52,6 +53,7 @@ public:
void Log_Write_RCOUT(void); void Log_Write_RCOUT(void);
void Log_Write_Baro(AP_Baro &baro); void Log_Write_Baro(AP_Baro &baro);
void Log_Write_Power(void); void Log_Write_Power(void);
void Log_Write_AHRS2(AP_AHRS &ahrs);
void Log_Write_Message(const char *message); void Log_Write_Message(const char *message);
void Log_Write_Message_P(const prog_char_t *message); void Log_Write_Message_P(const prog_char_t *message);
@ -210,6 +212,16 @@ struct PACKED log_BARO {
float altitude; float altitude;
float pressure; float pressure;
int16_t temperature; 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 { struct PACKED log_POWR {
@ -240,7 +252,11 @@ struct PACKED log_POWR {
{ LOG_BARO_MSG, sizeof(log_BARO), \ { LOG_BARO_MSG, sizeof(log_BARO), \
"BARO", "Iffc", "TimeMS,Alt,Press,Temp" }, \ "BARO", "Iffc", "TimeMS,Alt,Press,Temp" }, \
{ LOG_POWR_MSG, sizeof(log_POWR), \ { 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 // message types for common messages
#define LOG_FORMAT_MSG 128 #define LOG_FORMAT_MSG 128
@ -253,6 +269,8 @@ struct PACKED log_POWR {
#define LOG_IMU2_MSG 135 #define LOG_IMU2_MSG 135
#define LOG_BARO_MSG 136 #define LOG_BARO_MSG 136
#define LOG_POWR_MSG 137 #define LOG_POWR_MSG 137
#define LOG_AHR2_MSG 138
#define LOG_SIMSTATE_MSG 139
#include "DataFlash_Block.h" #include "DataFlash_Block.h"
#include "DataFlash_File.h" #include "DataFlash_File.h"

40
libraries/DataFlash/LogFile.cpp
View File

@ -6,6 +6,7 @@
#include <AP_Param.h> #include <AP_Param.h>
#include <AP_Math.h> #include <AP_Math.h>
#include <AP_Baro.h> #include <AP_Baro.h>
#include <AP_AHRS.h>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
@ -276,7 +277,7 @@ uint16_t DataFlash_Block::find_last_page_of_log(uint16_t log_number)
/* /*
read and print a log entry using the format strings from the given structure read and print a log entry using the format strings from the given structure
*/ */
void DataFlash_Class::_print_log_entry(uint8_t msg_type, void DataFlash_Class::_print_log_entry(uint8_t msg_type,
void (*print_mode)(AP_HAL::BetterStream *port, uint8_t mode), void (*print_mode)(AP_HAL::BetterStream *port, uint8_t mode),
AP_HAL::BetterStream *port) AP_HAL::BetterStream *port)
{ {
@ -429,8 +430,8 @@ void DataFlash_Block::_print_log_formats(AP_HAL::BetterStream *port)
for (uint8_t i=0; i<_num_types; i++) { for (uint8_t i=0; i<_num_types; i++) {
const struct LogStructure *s = &_structures[i]; const struct LogStructure *s = &_structures[i];
port->printf_P(PSTR("FMT, %u, %u, %S, %S, %S\n"), port->printf_P(PSTR("FMT, %u, %u, %S, %S, %S\n"),
(unsigned)PGM_UINT8(&s->msg_type), (unsigned)PGM_UINT8(&s->msg_type),
(unsigned)PGM_UINT8(&s->msg_len), (unsigned)PGM_UINT8(&s->msg_len),
s->name, s->format, s->labels); s->name, s->format, s->labels);
} }
} }
@ -439,7 +440,7 @@ void DataFlash_Block::_print_log_formats(AP_HAL::BetterStream *port)
Read the log and print it on port Read the log and print it on port
*/ */
void DataFlash_Block::LogReadProcess(uint16_t log_num, void DataFlash_Block::LogReadProcess(uint16_t log_num,
uint16_t start_page, uint16_t end_page, uint16_t start_page, uint16_t end_page,
void (*print_mode)(AP_HAL::BetterStream *port, uint8_t mode), void (*print_mode)(AP_HAL::BetterStream *port, uint8_t mode),
AP_HAL::BetterStream *port) AP_HAL::BetterStream *port)
{ {
@ -544,9 +545,9 @@ void DataFlash_Block::ListAvailableLogs(AP_HAL::BetterStream *port)
uint16_t last_log_start = log_start, last_log_end = log_end; uint16_t last_log_start = log_start, last_log_end = log_end;
uint16_t temp = last_log_num - i + 1; uint16_t temp = last_log_num - i + 1;
get_log_boundaries(temp, log_start, log_end); get_log_boundaries(temp, log_start, log_end);
port->printf_P(PSTR("Log %u, start %u, end %u\n"), port->printf_P(PSTR("Log %u, start %u, end %u\n"),
(unsigned)temp, (unsigned)temp,
(unsigned)log_start, (unsigned)log_start,
(unsigned)log_end); (unsigned)log_end);
if (last_log_start == log_start && last_log_end == log_end) { if (last_log_start == log_start && last_log_end == log_end) {
// we are printing bogus logs // we are printing bogus logs
@ -619,8 +620,8 @@ void DataFlash_Class::Log_Write_Parameter(const char *name, float value)
/* /*
write a parameter to the log write a parameter to the log
*/ */
void DataFlash_Class::Log_Write_Parameter(const AP_Param *ap, void DataFlash_Class::Log_Write_Parameter(const AP_Param *ap,
const AP_Param::ParamToken &token, const AP_Param::ParamToken &token,
enum ap_var_type type) enum ap_var_type type)
{ {
char name[16]; char name[16];
@ -793,3 +794,24 @@ void DataFlash_Class::Log_Write_Power(void)
#endif #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));
}