mirror of https://github.com/ArduPilot/ardupilot
Plane : Updated NavEKF flash data logging
This commit is contained in:
parent
3ee5ef852b
commit
e0e4b1aefa
|
@ -195,12 +195,12 @@ struct PACKED log_EKF1 {
|
||||||
int16_t roll;
|
int16_t roll;
|
||||||
int16_t pitch;
|
int16_t pitch;
|
||||||
uint16_t yaw;
|
uint16_t yaw;
|
||||||
int16_t velN;
|
float velN;
|
||||||
int16_t velE;
|
float velE;
|
||||||
int16_t velD;
|
float velD;
|
||||||
int16_t posN;
|
float posN;
|
||||||
int16_t posE;
|
float posE;
|
||||||
int16_t posD;
|
float posD;
|
||||||
int8_t gyrX;
|
int8_t gyrX;
|
||||||
int8_t gyrY;
|
int8_t gyrY;
|
||||||
int8_t gyrZ;
|
int8_t gyrZ;
|
||||||
|
@ -223,18 +223,18 @@ static void Log_Write_EKF1(void)
|
||||||
struct log_EKF1 pkt = {
|
struct log_EKF1 pkt = {
|
||||||
LOG_PACKET_HEADER_INIT(LOG_EKF1_MSG),
|
LOG_PACKET_HEADER_INIT(LOG_EKF1_MSG),
|
||||||
time_ms : hal.scheduler->millis(),
|
time_ms : hal.scheduler->millis(),
|
||||||
roll : (int16_t)(100*degrees(euler.x)),
|
roll : (int16_t)(100*degrees(euler.x)), // roll angle (centi-deg)
|
||||||
pitch : (int16_t)(100*degrees(euler.y)),
|
pitch : (int16_t)(100*degrees(euler.y)), // pitch angle (centi-deg)
|
||||||
yaw : (uint16_t)wrap_360_cd(100*degrees(euler.z)),
|
yaw : (uint16_t)wrap_360_cd(100*degrees(euler.z)), // yaw angle (centi-deg)
|
||||||
velN : (int16_t)(100*velNED.x),
|
velN : (float)(velNED.x), // velocity North (m/s)
|
||||||
velE : (int16_t)(100*velNED.y),
|
velE : (float)(velNED.y), // velocity East (m/s)
|
||||||
velD : (int16_t)(100*velNED.z),
|
velD : (float)(velNED.z), // velocity Down (m/s)
|
||||||
posN : (int16_t)(100*posNED.x),
|
posN : (float)(posNED.x), // metres North
|
||||||
posE : (int16_t)(100*posNED.y),
|
posE : (float)(posNED.y), // metres East
|
||||||
posD : (int16_t)(100*posNED.z),
|
posD : (float)(posNED.z), // metres Down
|
||||||
gyrX : (int8_t)(gyroBias.x),
|
gyrX : (int8_t)(gyroBias.x), // deg/min
|
||||||
gyrY : (int8_t)(gyroBias.y),
|
gyrY : (int8_t)(gyroBias.y), // deg/min
|
||||||
gyrZ : (int8_t)(gyroBias.z)
|
gyrZ : (int8_t)(gyroBias.z) // deg/min
|
||||||
};
|
};
|
||||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||||
#endif
|
#endif
|
||||||
|
@ -243,9 +243,9 @@ static void Log_Write_EKF1(void)
|
||||||
struct PACKED log_EKF2 {
|
struct PACKED log_EKF2 {
|
||||||
LOG_PACKET_HEADER;
|
LOG_PACKET_HEADER;
|
||||||
uint32_t time_ms;
|
uint32_t time_ms;
|
||||||
int16_t accX;
|
int8_t accX;
|
||||||
int16_t accY;
|
int8_t accY;
|
||||||
int16_t accZ;
|
int8_t accZ;
|
||||||
int16_t windN;
|
int16_t windN;
|
||||||
int16_t windE;
|
int16_t windE;
|
||||||
int16_t magN;
|
int16_t magN;
|
||||||
|
@ -272,9 +272,9 @@ static void Log_Write_EKF2(void)
|
||||||
struct log_EKF2 pkt = {
|
struct log_EKF2 pkt = {
|
||||||
LOG_PACKET_HEADER_INIT(LOG_EKF2_MSG),
|
LOG_PACKET_HEADER_INIT(LOG_EKF2_MSG),
|
||||||
time_ms : hal.scheduler->millis(),
|
time_ms : hal.scheduler->millis(),
|
||||||
accX : (int16_t)(100*accelBias.x),
|
accX : (int8_t)(100*accelBias.x),
|
||||||
accY : (int16_t)(100*accelBias.y),
|
accY : (int8_t)(100*accelBias.y),
|
||||||
accZ : (int16_t)(100*accelBias.z),
|
accZ : (int8_t)(100*accelBias.z),
|
||||||
windN : (int16_t)(100*wind.x),
|
windN : (int16_t)(100*wind.x),
|
||||||
windE : (int16_t)(100*wind.y),
|
windE : (int16_t)(100*wind.y),
|
||||||
magN : (int16_t)(magNED.x),
|
magN : (int16_t)(magNED.x),
|
||||||
|
|
Loading…
Reference in New Issue