mirror of https://github.com/ArduPilot/ardupilot
Logging input to find this freeze bug
This commit is contained in:
parent
d4a9808a61
commit
8e582b4169
|
@ -724,30 +724,20 @@ static void Log_Write_Control_Tuning()
|
||||||
DataFlash.WriteByte(HEAD_BYTE2);
|
DataFlash.WriteByte(HEAD_BYTE2);
|
||||||
DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG);
|
DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG);
|
||||||
|
|
||||||
// yaw
|
DataFlash.WriteInt(g.rc_1.control_in); // 0
|
||||||
DataFlash.WriteInt(dcm.yaw_sensor/100); //1
|
DataFlash.WriteInt(g.rc_2.control_in); // 1
|
||||||
DataFlash.WriteInt(nav_yaw/100); //2
|
DataFlash.WriteInt(g.rc_3.control_in); // 2
|
||||||
DataFlash.WriteInt(yaw_error/100); //3
|
DataFlash.WriteInt(g.rc_4.control_in); // 3
|
||||||
|
|
||||||
// Alt hold
|
|
||||||
DataFlash.WriteInt(sonar_alt); // 4
|
DataFlash.WriteInt(sonar_alt); // 4
|
||||||
DataFlash.WriteInt(baro_alt); // 5
|
DataFlash.WriteInt(baro_alt); // 5
|
||||||
DataFlash.WriteInt(next_WP.alt); // 6
|
DataFlash.WriteInt(next_WP.alt); // 6
|
||||||
|
|
||||||
DataFlash.WriteInt(nav_throttle); // 7
|
DataFlash.WriteInt(nav_throttle); // 7
|
||||||
DataFlash.WriteInt(angle_boost); // 8
|
DataFlash.WriteInt(angle_boost); // 8
|
||||||
DataFlash.WriteInt(manual_boost); // 9
|
DataFlash.WriteInt(manual_boost); // 9
|
||||||
DataFlash.WriteInt(climb_rate); // 10
|
DataFlash.WriteInt(climb_rate); // 10
|
||||||
|
DataFlash.WriteInt(g.rc_3.servo_out); // 11
|
||||||
#if HIL_MODE == HIL_MODE_ATTITUDE
|
DataFlash.WriteInt(g.pi_alt_hold.get_integrator()); // 12
|
||||||
DataFlash.WriteInt(0); //11
|
DataFlash.WriteInt(g.pi_throttle.get_integrator()); // 13
|
||||||
#else
|
|
||||||
DataFlash.WriteInt((int)(barometer.RawPress - barometer._offset_press));//11
|
|
||||||
#endif
|
|
||||||
|
|
||||||
DataFlash.WriteInt(g.rc_3.servo_out); //12
|
|
||||||
DataFlash.WriteInt(g.pi_alt_hold.get_integrator()); //13
|
|
||||||
DataFlash.WriteInt(g.pi_throttle.get_integrator()); //14
|
|
||||||
|
|
||||||
DataFlash.WriteByte(END_BYTE);
|
DataFlash.WriteByte(END_BYTE);
|
||||||
}
|
}
|
||||||
|
@ -759,11 +749,11 @@ static void Log_Read_Control_Tuning()
|
||||||
|
|
||||||
Serial.printf_P(PSTR("CTUN, "));
|
Serial.printf_P(PSTR("CTUN, "));
|
||||||
|
|
||||||
for(int8_t i = 1; i < 14; i++ ){
|
for(int8_t i = 0; i < 13; i++ ){
|
||||||
temp = DataFlash.ReadInt();
|
temp = DataFlash.ReadInt();
|
||||||
Serial.printf("%d, ", temp);
|
Serial.printf("%d, ", temp);
|
||||||
}
|
}
|
||||||
|
// read 13
|
||||||
temp = DataFlash.ReadInt();
|
temp = DataFlash.ReadInt();
|
||||||
Serial.printf("%d\n", temp);
|
Serial.printf("%d\n", temp);
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue