Plane: log ATT on none quadplanes

This commit is contained in:
Iampete1 2022-07-17 10:37:33 +01:00 committed by Andrew Tridgell
parent d2caebf39c
commit 3f410be344
1 changed files with 5 additions and 1 deletions

View File

@ -20,9 +20,13 @@ void Plane::Log_Write_Attitude(void)
quadplane.attitude_control->get_attitude_target_quat().to_euler(targets.x, targets.y, targets.z); quadplane.attitude_control->get_attitude_target_quat().to_euler(targets.x, targets.y, targets.z);
targets *= degrees(100.0f); targets *= degrees(100.0f);
quadplane.ahrs_view->Write_AttitudeView(targets); quadplane.ahrs_view->Write_AttitudeView(targets);
} else { } else
#endif
{
ahrs.Write_Attitude(targets); ahrs.Write_Attitude(targets);
} }
#if HAL_QUADPLANE_ENABLED
if (AP_HAL::millis() - quadplane.last_att_control_ms < 100) { if (AP_HAL::millis() - quadplane.last_att_control_ms < 100) {
// log quadplane PIDs separately from fixed wing PIDs // log quadplane PIDs separately from fixed wing PIDs
logger.Write_PID(LOG_PIQR_MSG, quadplane.attitude_control->get_rate_roll_pid().get_pid_info()); logger.Write_PID(LOG_PIQR_MSG, quadplane.attitude_control->get_rate_roll_pid().get_pid_info());