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: level when going to(or arming in a) stabilized flight mode

Browse Source
This commit is contained in:
Willian Galvani 2020-02-14 13:19:36 -03:00 committed by Jacob Walser
parent 3a1984a893
commit 91e10291b9
2 changed files with 19 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
ArduSub/control_althold.cpp
View File

@ -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;

13
ArduSub/control_stabilize.cpp
View File

@ -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;
}