Sub: make SET_ATTITUDE_TARGET changes persistent

In the sense that it no longer "times out"
This commit is contained in:
Willian Galvani 2022-04-28 16:27:55 -03:00
parent 9faf1ef273
commit 9a6fb5232e

View File

@ -54,6 +54,9 @@ void Sub::handle_attitude()
target_roll = 100 * degrees(target_roll);
target_pitch = 100 * degrees(target_pitch);
target_yaw = 100 * degrees(target_yaw);
last_roll = target_roll;
last_pitch = target_pitch;
last_pilot_heading = target_yaw;
attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, target_yaw, true);
} else {
// If we don't have a mavlink attitude target, we use the pilot's input instead