aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Sub: refactor depth control of control_althold to a function

Browse Source
This commit is contained in:
Willian Galvani 2020-08-03 00:31:50 -03:00 committed by Jacob Walser
parent 8325de0845
commit 7fa8a455d7
2 changed files with 8 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduSub/Sub.h
View File

@ -509,6 +509,7 @@ private:
bool stabilize_init(void); bool stabilize_init(void);
void stabilize_run(); void stabilize_run();
void control_depth();
bool manual_init(void); bool manual_init(void);
void manual_run(); void manual_run();
void failsafe_sensors_check(void); void failsafe_sensors_check(void);

10
ArduSub/control_althold.cpp
View File

@ -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 // Hold actual position until zero derivative is detected
static bool engageStopZ = true; static bool engageStopZ = true;
// Get last user velocity direction to check for zero derivative points // Get last user velocity direction to check for zero derivative points
@ -125,7 +132,4 @@ void Sub::althold_run()
pos_control.update_z_controller(); pos_control.update_z_controller();
} }
motors.set_forward(channel_forward->norm_input());
motors.set_lateral(channel_lateral->norm_input());
} }