Browse Source

Copter: Add Attitude Rate PID logging functionality.

master
Robert Lefebvre 10 years ago committed by Andrew Tridgell
parent
commit
dc69fe5c3b
  1. 6
      ArduCopter/ArduCopter.pde

6
ArduCopter/ArduCopter.pde

@ -972,6 +972,9 @@ static void ten_hz_logging_loop() @@ -972,6 +972,9 @@ static void ten_hz_logging_loop()
if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) {
Log_Write_Attitude();
Log_Write_Rate();
DataFlash.Log_Write_PID(LOG_PIDR_MSG, g.pid_rate_roll.get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDP_MSG, g.pid_rate_pitch.get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDY_MSG, g.pid_rate_yaw.get_pid_info() );
}
if (should_log(MASK_LOG_MOTBATT)) {
Log_Write_MotBatt();
@ -1000,6 +1003,9 @@ static void fifty_hz_logging_loop() @@ -1000,6 +1003,9 @@ static void fifty_hz_logging_loop()
if (should_log(MASK_LOG_ATTITUDE_FAST)) {
Log_Write_Attitude();
Log_Write_Rate();
DataFlash.Log_Write_PID(LOG_PIDR_MSG, g.pid_rate_roll.get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDP_MSG, g.pid_rate_pitch.get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDY_MSG, g.pid_rate_yaw.get_pid_info() );
}
// log IMU data if we're not already logging at the higher rate

Loading…
Cancel
Save