Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Sub: refactor depth control of control_althold to a function

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);
void stabilize_run();
void control_depth();
bool manual_init(void);
void manual_run();
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
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());
}