Browse Source

HIL: enable CTUN log message in ATTITUDE HIL

master
Andrew Tridgell 14 years ago
parent
commit
3176d4ffc5
  1. 14
      ArduCopter/ArduCopter.pde
  2. 8
      ArduCopter/Log.pde

14
ArduCopter/ArduCopter.pde

@ -691,15 +691,13 @@ static void medium_loop()
update_commands(); update_commands();
} }
#if HIL_MODE != HIL_MODE_ATTITUDE if(motor_armed){
if(motor_armed){ if (g.log_bitmask & MASK_LOG_ATTITUDE_MED)
if (g.log_bitmask & MASK_LOG_ATTITUDE_MED) Log_Write_Attitude();
Log_Write_Attitude();
if (g.log_bitmask & MASK_LOG_CTUN) if (g.log_bitmask & MASK_LOG_CTUN)
Log_Write_Control_Tuning(); Log_Write_Control_Tuning();
} }
#endif
// send all requested output streams with rates requested // send all requested output streams with rates requested
// between 5 and 45 Hz // between 5 and 45 Hz

8
ArduCopter/Log.pde

@ -641,7 +641,6 @@ static void Log_Read_Nav_Tuning()
// Write a control tuning packet. Total length : 22 bytes // Write a control tuning packet. Total length : 22 bytes
#if HIL_MODE != HIL_MODE_ATTITUDE
static void Log_Write_Control_Tuning() static void Log_Write_Control_Tuning()
{ {
DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE1);
@ -662,8 +661,12 @@ static void Log_Write_Control_Tuning()
DataFlash.WriteInt(angle_boost); //8 DataFlash.WriteInt(angle_boost); //8
DataFlash.WriteInt(manual_boost); //9 DataFlash.WriteInt(manual_boost); //9
//DataFlash.WriteInt((int)(accels_rot.z * 1000)); //10 //DataFlash.WriteInt((int)(accels_rot.z * 1000)); //10
DataFlash.WriteInt((int)(barometer.RawPress - barometer._offset_press)); //9
#if HIL_MODE == HIL_MODE_ATTITUDE
DataFlash.WriteInt(0);
#else
DataFlash.WriteInt((int)(barometer.RawPress - barometer._offset_press)); //9
#endif
DataFlash.WriteInt(g.rc_3.servo_out); //11 DataFlash.WriteInt(g.rc_3.servo_out); //11
DataFlash.WriteInt((int)g.pi_alt_hold.get_integrator()); //12 DataFlash.WriteInt((int)g.pi_alt_hold.get_integrator()); //12
@ -671,7 +674,6 @@ static void Log_Write_Control_Tuning()
DataFlash.WriteByte(END_BYTE); DataFlash.WriteByte(END_BYTE);
} }
#endif
// Read an control tuning packet // Read an control tuning packet
static void Log_Read_Control_Tuning() static void Log_Read_Control_Tuning()

Loading…
Cancel
Save