From 7fa8a455d72a46421bd4396f97d96638ba71f126 Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Mon, 3 Aug 2020 00:31:50 -0300 Subject: [PATCH] Sub: refactor depth control of control_althold to a function --- ArduSub/Sub.h | 1 + ArduSub/control_althold.cpp | 10 +++++++--- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 197ec12da9..5a8493d154 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -509,6 +509,7 @@ private: bool stabilize_init(void); void stabilize_run(); + void control_depth(); bool manual_init(void); void manual_run(); void failsafe_sensors_check(void); diff --git a/ArduSub/control_althold.cpp b/ArduSub/control_althold.cpp index e0eef9ede5..d6fcf7ccda 100644 --- a/ArduSub/control_althold.cpp +++ b/ArduSub/control_althold.cpp @@ -96,6 +96,13 @@ void Sub::althold_run() } } + control_depth(); + + motors.set_forward(channel_forward->norm_input()); + motors.set_lateral(channel_lateral->norm_input()); +} + +void Sub::control_depth() { // Hold actual position until zero derivative is detected static bool engageStopZ = true; // Get last user velocity direction to check for zero derivative points @@ -125,7 +132,4 @@ void Sub::althold_run() pos_control.update_z_controller(); } - - motors.set_forward(channel_forward->norm_input()); - motors.set_lateral(channel_lateral->norm_input()); }