diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index 3dd27a43d5..c9d99f97f3 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -164,10 +164,17 @@ void Plane::Log_Write_Attitude(void) targets.z = 0; //Plane does not have the concept of navyaw. This is a placeholder. DataFlash.Log_Write_Attitude(ahrs, targets); - DataFlash.Log_Write_PID(LOG_PIDR_MSG, rollController.get_pid_info()); - DataFlash.Log_Write_PID(LOG_PIDP_MSG, pitchController.get_pid_info()); - DataFlash.Log_Write_PID(LOG_PIDY_MSG, yawController.get_pid_info()); - DataFlash.Log_Write_PID(LOG_PIDS_MSG, steerController.get_pid_info()); + if (quadplane.in_vtol_mode()) { + DataFlash.Log_Write_PID(LOG_PIDR_MSG, quadplane.pid_rate_roll.get_pid_info() ); + DataFlash.Log_Write_PID(LOG_PIDP_MSG, quadplane.pid_rate_pitch.get_pid_info() ); + DataFlash.Log_Write_PID(LOG_PIDY_MSG, quadplane.pid_rate_yaw.get_pid_info() ); + DataFlash.Log_Write_PID(LOG_PIDA_MSG, quadplane.pid_accel_z.get_pid_info() ); + } else { + DataFlash.Log_Write_PID(LOG_PIDR_MSG, rollController.get_pid_info()); + DataFlash.Log_Write_PID(LOG_PIDP_MSG, pitchController.get_pid_info()); + DataFlash.Log_Write_PID(LOG_PIDY_MSG, yawController.get_pid_info()); + DataFlash.Log_Write_PID(LOG_PIDS_MSG, steerController.get_pid_info()); + } #if AP_AHRS_NAVEKF_AVAILABLE #if OPTFLOW == ENABLED diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index e8d5c97803..96822f48b8 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -989,6 +989,17 @@ bool QuadPlane::in_vtol_auto(void) } } +/* + are we in a VTOL mode? + */ +bool QuadPlane::in_vtol_mode(void) +{ + return (plane.control_mode == QSTABILIZE || + plane.control_mode == QHOVER || + plane.control_mode == QLOITER || + in_vtol_auto()); +} + /* handle auto-mode when auto_state.vtol_mode is true */ diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 8653770ca8..6a64831aca 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -47,6 +47,7 @@ public: bool verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd); bool verify_vtol_land(const AP_Mission::Mission_Command &cmd); bool in_vtol_auto(void); + bool in_vtol_mode(void); // vtol help for is_flying() bool is_flying(void);