Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-07 16:33:58 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: Improve attitude control in depth hold

Browse Source 
This adds a specific case for yaw input only, and adds a missing
set to 'last_input_ms' which made the controls feel "sticky"
This commit is contained in:
Willian Galvani 2019-12-09 19:02:30 -03:00 committed by Jacob Walser
parent 265d848021
commit deccd051a0
1 changed files with 7 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduSub/control_althold.cpp
View File

@ -55,14 +55,20 @@ void Sub::handle_attitude()
// If we don't have a mavlink attitude target, we use the pilot's input instead
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());
target_yaw = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (abs(target_roll) > 50 || abs(target_pitch) > 50 || abs(target_yaw) > 50) {
if (abs(target_roll) > 50 || abs(target_pitch) > 50) {
last_roll = ahrs.roll_sensor;
last_pitch = ahrs.pitch_sensor;
last_yaw = ahrs.yaw_sensor;
last_input_ms = tnow;
attitude_control.input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
} else if (abs(target_yaw) > 50) {
// if only yaw is being controlled, don't update pitch and roll
attitude_control.input_rate_bf_roll_pitch_yaw(0, 0, target_yaw);
last_yaw = ahrs.yaw_sensor;
last_input_ms = tnow;
} else if (tnow < last_input_ms + 250) {
// just brake for a few mooments so we don't bounce
last_yaw = ahrs.yaw_sensor;
attitude_control.input_rate_bf_roll_pitch_yaw(0, 0, 0);
} else {
// Lock attitude