From 41f4beb36fc1a311bc9097d1cbfb2debec7eecdf Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Fri, 15 Apr 2022 13:13:12 -0300 Subject: [PATCH] Sub: do not reset attitude targets between althold<->stabilize --- ArduSub/control_althold.cpp | 6 ++++-- ArduSub/control_stabilize.cpp | 6 ++++-- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/ArduSub/control_althold.cpp b/ArduSub/control_althold.cpp index 0f5a57cd10..d3497c7770 100644 --- a/ArduSub/control_althold.cpp +++ b/ArduSub/control_althold.cpp @@ -20,8 +20,10 @@ bool Sub::althold_init() // initialise position and desired velocity pos_control.init_z_controller_stopping_point(); - last_roll = 0; - last_pitch = 0; + if(prev_control_mode != control_mode_t::STABILIZE) { + last_roll = 0; + last_pitch = 0; + } last_pilot_heading = ahrs.yaw_sensor; last_input_ms = AP_HAL::millis(); diff --git a/ArduSub/control_stabilize.cpp b/ArduSub/control_stabilize.cpp index bbb6605d2a..03db4a4580 100644 --- a/ArduSub/control_stabilize.cpp +++ b/ArduSub/control_stabilize.cpp @@ -5,8 +5,10 @@ bool Sub::stabilize_init() { // set target altitude to zero for reporting pos_control.set_pos_target_z_cm(0); - last_roll = 0; - last_pitch = 0; + if(prev_control_mode != control_mode_t::ALT_HOLD) { + last_roll = 0; + last_pitch = 0; + } last_pilot_heading = ahrs.yaw_sensor; return true; }