mirror of https://github.com/ArduPilot/ardupilot
ACM: added nav_yaw to att logging
Removed unnecessary casting
This commit is contained in:
parent
90ccb6f3ef
commit
0e7794336e
|
@ -760,8 +760,7 @@ static void Log_Write_Attitude()
|
||||||
DataFlash.WriteInt((int)ahrs.pitch_sensor); // 4
|
DataFlash.WriteInt((int)ahrs.pitch_sensor); // 4
|
||||||
DataFlash.WriteInt(g.rc_4.control_in); // 5
|
DataFlash.WriteInt(g.rc_4.control_in); // 5
|
||||||
DataFlash.WriteInt((uint16_t)ahrs.yaw_sensor); // 6
|
DataFlash.WriteInt((uint16_t)ahrs.yaw_sensor); // 6
|
||||||
DataFlash.WriteInt(0); // 7 (this used to be compass.heading)
|
DataFlash.WriteInt((uint16_t)nav_yaw); // 7 (this used to be compass.heading)
|
||||||
|
|
||||||
DataFlash.WriteByte(END_BYTE);
|
DataFlash.WriteByte(END_BYTE);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -775,17 +774,16 @@ static void Log_Read_Attitude()
|
||||||
int16_t temp5 = DataFlash.ReadInt();
|
int16_t temp5 = DataFlash.ReadInt();
|
||||||
uint16_t temp6 = DataFlash.ReadInt();
|
uint16_t temp6 = DataFlash.ReadInt();
|
||||||
uint16_t temp7 = DataFlash.ReadInt();
|
uint16_t temp7 = DataFlash.ReadInt();
|
||||||
temp7 = wrap_360(temp7);
|
|
||||||
|
|
||||||
// 1 2 3 4 5 6 7
|
// 1 2 3 4 5 6 7
|
||||||
Serial.printf_P(PSTR("ATT, %d, %d, %d, %d, %d, %u, %u\n"),
|
Serial.printf_P(PSTR("ATT, %d, %d, %d, %d, %d, %u, %u\n"),
|
||||||
(int)temp1,
|
temp1,
|
||||||
(int)temp2,
|
temp2,
|
||||||
(int)temp3,
|
temp3,
|
||||||
(int)temp4,
|
temp4,
|
||||||
(int)temp5,
|
temp5,
|
||||||
(unsigned int)temp6,
|
temp6,
|
||||||
(unsigned int)temp7);
|
temp7);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Write a mode packet. Total length : 7 bytes
|
// Write a mode packet. Total length : 7 bytes
|
||||||
|
@ -890,13 +888,13 @@ static void Log_Read_PID()
|
||||||
|
|
||||||
// 1 2 3 4 5 6 7
|
// 1 2 3 4 5 6 7
|
||||||
Serial.printf_P(PSTR("PID-%d, %ld, %ld, %ld, %ld, %ld, %4.4f\n"),
|
Serial.printf_P(PSTR("PID-%d, %ld, %ld, %ld, %ld, %ld, %4.4f\n"),
|
||||||
(int)temp1, // pid id
|
temp1, // pid id
|
||||||
(long)temp2, // error
|
temp2, // error
|
||||||
(long)temp3, // p
|
temp3, // p
|
||||||
(long)temp4, // i
|
temp4, // i
|
||||||
(long)temp5, // d
|
temp5, // d
|
||||||
(long)temp6, // output
|
temp6, // output
|
||||||
temp7); // gain
|
temp7); // gain
|
||||||
}
|
}
|
||||||
|
|
||||||
// Read the DataFlash log memory
|
// Read the DataFlash log memory
|
||||||
|
|
Loading…
Reference in New Issue