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

Copter: added dual compass logging when available

Browse Source
This commit is contained in:
Andrew Tridgell 2013-12-10 13:31:42 +11:00
parent cba0cb963a
commit cc3ee2ec0c
2 changed files with 24 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
ArduCopter/Log.pde
View File

@ -351,9 +351,9 @@ struct PACKED log_Compass {
// Write a Compass packet
static void Log_Write_Compass()
{
const Vector3f &mag_offsets = compass.get_offsets();
const Vector3f &mag_motor_offsets = compass.get_motor_offsets();
const Vector3f &mag = compass.get_field();
const Vector3f &mag_offsets = compass.get_offsets(0);
const Vector3f &mag_motor_offsets = compass.get_motor_offsets(0);
const Vector3f &mag = compass.get_field(0);
struct log_Compass pkt = {
LOG_PACKET_HEADER_INIT(LOG_COMPASS_MSG),
mag_x : (int16_t)mag.x,
@ -367,6 +367,24 @@ static void Log_Write_Compass()
motor_offset_z : (int16_t)mag_motor_offsets.z
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
#if COMPASS_MAX_INSTANCES > 1
const Vector3f &mag2_offsets = compass.get_offsets(1);
const Vector3f &mag2_motor_offsets = compass.get_motor_offsets(1);
const Vector3f &mag2 = compass.get_field(1);
struct log_Compass pkt2 = {
LOG_PACKET_HEADER_INIT(LOG_COMPASS2_MSG),
mag_x : (int16_t)mag2.x,
mag_y : (int16_t)mag2.y,
mag_z : (int16_t)mag2.z,
offset_x : (int16_t)mag2_offsets.x,
offset_y : (int16_t)mag2_offsets.y,
offset_z : (int16_t)mag2_offsets.z,
motor_offset_x : (int16_t)mag2_motor_offsets.x,
motor_offset_y : (int16_t)mag2_motor_offsets.y,
motor_offset_z : (int16_t)mag2_motor_offsets.z
};
DataFlash.WriteBlock(&pkt2, sizeof(pkt2));
#endif
}
struct PACKED log_Performance {
@ -724,6 +742,8 @@ static const struct LogStructure log_structure[] PROGMEM = {
"CTUN", "hcefchhhh", "ThrIn,SonAlt,BarAlt,WPAlt,DesSonAlt,AngBst,CRate,ThrOut,DCRate" },
{ LOG_COMPASS_MSG, sizeof(log_Compass),
"MAG", "hhhhhhhhh", "MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ" },
{ LOG_COMPASS2_MSG, sizeof(log_Compass),
"MAG2", "hhhhhhhhh", "MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ" },
{ LOG_PERFORMANCE_MSG, sizeof(log_Performance),
"PM", "BBHHIhBHB", "RenCnt,RenBlw,NLon,NLoop,MaxT,PMT,I2CErr,INSErr,INAVErr" },
{ LOG_CMD_MSG, sizeof(log_Cmd),

1
ArduCopter/defines.h
View File

@ -293,6 +293,7 @@ enum ap_message {
#define LOG_DATA_FLOAT_MSG 0x18
#define LOG_AUTOTUNE_MSG 0x19
#define LOG_AUTOTUNEDETAILS_MSG 0x1A
#define LOG_COMPASS2_MSG 0x1B
#define LOG_INDEX_MSG 0xF0
#define MAX_NUM_LOGS 50