mirror of https://github.com/ArduPilot/ardupilot
Rover: Change Compass logging to use DataFlash library method.
This commit is contained in:
parent
89fd543f4d
commit
b3aab67239
|
@ -358,80 +358,19 @@ static void Log_Write_Current()
|
||||||
DataFlash.Log_Write_Power();
|
DataFlash.Log_Write_Power();
|
||||||
}
|
}
|
||||||
|
|
||||||
struct PACKED log_Compass {
|
// Write a Compass packet
|
||||||
LOG_PACKET_HEADER;
|
|
||||||
uint32_t time_ms;
|
|
||||||
int16_t mag_x;
|
|
||||||
int16_t mag_y;
|
|
||||||
int16_t mag_z;
|
|
||||||
int16_t offset_x;
|
|
||||||
int16_t offset_y;
|
|
||||||
int16_t offset_z;
|
|
||||||
int16_t motor_offset_x;
|
|
||||||
int16_t motor_offset_y;
|
|
||||||
int16_t motor_offset_z;
|
|
||||||
};
|
|
||||||
|
|
||||||
// Write a Compass packet. Total length : 15 bytes
|
|
||||||
static void Log_Write_Compass()
|
static void Log_Write_Compass()
|
||||||
{
|
{
|
||||||
const Vector3f &mag_offsets = compass.get_offsets();
|
DataFlash.Log_Write_Compass(compass, 0);
|
||||||
const Vector3f &mag_motor_offsets = compass.get_motor_offsets();
|
|
||||||
const Vector3f &mag = compass.get_field();
|
|
||||||
struct log_Compass pkt = {
|
|
||||||
LOG_PACKET_HEADER_INIT(LOG_COMPASS_MSG),
|
|
||||||
time_ms : millis(),
|
|
||||||
mag_x : (int16_t)mag.x,
|
|
||||||
mag_y : (int16_t)mag.y,
|
|
||||||
mag_z : (int16_t)mag.z,
|
|
||||||
offset_x : (int16_t)mag_offsets.x,
|
|
||||||
offset_y : (int16_t)mag_offsets.y,
|
|
||||||
offset_z : (int16_t)mag_offsets.z,
|
|
||||||
motor_offset_x : (int16_t)mag_motor_offsets.x,
|
|
||||||
motor_offset_y : (int16_t)mag_motor_offsets.y,
|
|
||||||
motor_offset_z : (int16_t)mag_motor_offsets.z
|
|
||||||
};
|
|
||||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
|
||||||
#if COMPASS_MAX_INSTANCES > 1
|
#if COMPASS_MAX_INSTANCES > 1
|
||||||
if (compass.get_count() > 1) {
|
if (compass.get_count() > 1) {
|
||||||
const Vector3f &mag2_offsets = compass.get_offsets(1);
|
DataFlash.Log_Write_Compass(compass, 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),
|
|
||||||
time_ms : millis(),
|
|
||||||
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
|
#endif
|
||||||
#if COMPASS_MAX_INSTANCES > 2
|
#if COMPASS_MAX_INSTANCES > 2
|
||||||
if (compass.get_count() > 2) {
|
if (compass.get_count() > 2) {
|
||||||
const Vector3f &mag3_offsets = compass.get_offsets(2);
|
DataFlash.Log_Write_Compass(compass, 2);
|
||||||
const Vector3f &mag3_motor_offsets = compass.get_motor_offsets(2);
|
|
||||||
const Vector3f &mag3 = compass.get_field(2);
|
|
||||||
struct log_Compass pkt3 = {
|
|
||||||
LOG_PACKET_HEADER_INIT(LOG_COMPASS3_MSG),
|
|
||||||
time_ms : millis(),
|
|
||||||
mag_x : (int16_t)mag3.x,
|
|
||||||
mag_y : (int16_t)mag3.y,
|
|
||||||
mag_z : (int16_t)mag3.z,
|
|
||||||
offset_x : (int16_t)mag3_offsets.x,
|
|
||||||
offset_y : (int16_t)mag3_offsets.y,
|
|
||||||
offset_z : (int16_t)mag3_offsets.z,
|
|
||||||
motor_offset_x : (int16_t)mag3_motor_offsets.x,
|
|
||||||
motor_offset_y : (int16_t)mag3_motor_offsets.y,
|
|
||||||
motor_offset_z : (int16_t)mag3_motor_offsets.z
|
|
||||||
};
|
|
||||||
DataFlash.WriteBlock(&pkt3, sizeof(pkt3));
|
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
@ -460,10 +399,6 @@ static const struct LogStructure log_structure[] PROGMEM = {
|
||||||
"NTUN", "IHfHHb", "TimeMS,Yaw,WpDist,TargBrg,NavBrg,Thr" },
|
"NTUN", "IHfHHb", "TimeMS,Yaw,WpDist,TargBrg,NavBrg,Thr" },
|
||||||
{ LOG_SONAR_MSG, sizeof(log_Sonar),
|
{ LOG_SONAR_MSG, sizeof(log_Sonar),
|
||||||
"SONR", "IfHHHbHCb", "TimeMS,LatAcc,S1Dist,S2Dist,DCnt,TAng,TTim,Spd,Thr" },
|
"SONR", "IfHHHbHCb", "TimeMS,LatAcc,S1Dist,S2Dist,DCnt,TAng,TTim,Spd,Thr" },
|
||||||
{ LOG_COMPASS_MSG, sizeof(log_Compass),
|
|
||||||
"MAG", "Ihhhhhhhhh", "TimeMS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ" },
|
|
||||||
{ LOG_COMPASS2_MSG, sizeof(log_Compass),
|
|
||||||
"MAG2", "Ihhhhhhhhh", "TimeMS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ" },
|
|
||||||
{ LOG_STEERING_MSG, sizeof(log_Steering),
|
{ LOG_STEERING_MSG, sizeof(log_Steering),
|
||||||
"STER", "Iff", "TimeMS,Demanded,Achieved" },
|
"STER", "Iff", "TimeMS,Demanded,Achieved" },
|
||||||
};
|
};
|
||||||
|
|
|
@ -67,10 +67,7 @@ enum mode {
|
||||||
#define LOG_PERFORMANCE_MSG 0x03
|
#define LOG_PERFORMANCE_MSG 0x03
|
||||||
#define LOG_STARTUP_MSG 0x06
|
#define LOG_STARTUP_MSG 0x06
|
||||||
#define LOG_SONAR_MSG 0x07
|
#define LOG_SONAR_MSG 0x07
|
||||||
#define LOG_COMPASS_MSG 0x0A
|
|
||||||
#define LOG_COMPASS2_MSG 0x0C
|
|
||||||
#define LOG_STEERING_MSG 0x0D
|
#define LOG_STEERING_MSG 0x0D
|
||||||
#define LOG_COMPASS3_MSG 0x0E
|
|
||||||
|
|
||||||
#define TYPE_AIRSTART_MSG 0x00
|
#define TYPE_AIRSTART_MSG 0x00
|
||||||
#define TYPE_GROUNDSTART_MSG 0x01
|
#define TYPE_GROUNDSTART_MSG 0x01
|
||||||
|
|
Loading…
Reference in New Issue