From 91e10291b91e48c9679b3024b3a75b2fa9cd04ce Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Fri, 14 Feb 2020 13:19:36 -0300 Subject: [PATCH] Sub: level when going to(or arming in a) stabilized flight mode --- ArduSub/control_althold.cpp | 14 ++++++++++---- ArduSub/control_stabilize.cpp | 13 +++++++++---- 2 files changed, 19 insertions(+), 8 deletions(-) diff --git a/ArduSub/control_althold.cpp b/ArduSub/control_althold.cpp index 485f45783f..85c55ab957 100644 --- a/ArduSub/control_althold.cpp +++ b/ArduSub/control_althold.cpp @@ -20,8 +20,14 @@ bool Sub::althold_init() pos_control.set_target_to_stopping_point_z(); holding_depth = true; - last_roll = ahrs.roll_sensor; - last_pitch = ahrs.pitch_sensor; + if (prev_control_mode == STABILIZE) { + last_roll = ahrs.roll_sensor; + last_pitch = ahrs.pitch_sensor; + } else { + last_roll = 0; + last_pitch = 0; + } + last_yaw = ahrs.yaw_sensor; last_input_ms = AP_HAL::millis(); @@ -92,8 +98,8 @@ void Sub::althold_run() attitude_control.set_throttle_out(0.5 ,true, g.throttle_filt); attitude_control.relax_attitude_controllers(); pos_control.relax_alt_hold_controllers(); - last_roll = ahrs.roll_sensor; - last_pitch = ahrs.pitch_sensor; + last_roll = 0; + last_pitch = 0; last_yaw = ahrs.yaw_sensor; holding_depth = false; return; diff --git a/ArduSub/control_stabilize.cpp b/ArduSub/control_stabilize.cpp index 1e11f9da78..e93a94a63d 100644 --- a/ArduSub/control_stabilize.cpp +++ b/ArduSub/control_stabilize.cpp @@ -5,8 +5,13 @@ bool Sub::stabilize_init() { // set target altitude to zero for reporting pos_control.set_alt_target(0); - last_roll = ahrs.roll_sensor; - last_pitch = ahrs.pitch_sensor; + if (prev_control_mode == ALT_HOLD) { + last_roll = ahrs.roll_sensor; + last_pitch = ahrs.pitch_sensor; + } else { + last_roll = 0; + last_pitch = 0; + } last_yaw = ahrs.yaw_sensor; last_input_ms = AP_HAL::millis(); return true; @@ -21,8 +26,8 @@ void Sub::stabilize_run() motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); attitude_control.set_throttle_out(0,true,g.throttle_filt); attitude_control.relax_attitude_controllers(); - last_roll = ahrs.roll_sensor; - last_pitch = ahrs.pitch_sensor; + last_roll = 0; + last_pitch = 0; last_yaw = ahrs.yaw_sensor; return; }