Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-24 01:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: do not reset attitude targets between althold<->stabilize

Browse Source
This commit is contained in:
Willian Galvani 2022-04-15 13:13:12 -03:00
parent 69142b9ec5
commit 41f4beb36f
2 changed files with 8 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduSub/control_althold.cpp
View File

@ -20,8 +20,10 @@ bool Sub::althold_init()
// initialise position and desired velocity // initialise position and desired velocity
pos_control.init_z_controller_stopping_point(); pos_control.init_z_controller_stopping_point();
last_roll = 0; if(prev_control_mode != control_mode_t::STABILIZE) {
last_pitch = 0; last_roll = 0;
last_pitch = 0;
}
last_pilot_heading = ahrs.yaw_sensor; last_pilot_heading = ahrs.yaw_sensor;
last_input_ms = AP_HAL::millis(); last_input_ms = AP_HAL::millis();

6
ArduSub/control_stabilize.cpp
View File

@ -5,8 +5,10 @@ bool Sub::stabilize_init()
{ {
// set target altitude to zero for reporting // set target altitude to zero for reporting
pos_control.set_pos_target_z_cm(0); pos_control.set_pos_target_z_cm(0);
last_roll = 0; if(prev_control_mode != control_mode_t::ALT_HOLD) {
last_pitch = 0; last_roll = 0;
last_pitch = 0;
}
last_pilot_heading = ahrs.yaw_sensor; last_pilot_heading = ahrs.yaw_sensor;
return true; return true;
} }