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

Plane: obey Q_RTL_ALT when in POS1 and POS2 states 

this fixes the alt target for when user has disabled the quadplane
approach code
This commit is contained in:
Andrew Tridgell 2021-09-29 13:07:17 +10:00
parent 9de5ef72db
commit 2be23fff2b
2 changed files with 11 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduPlane/mode_qrtl.cpp
View File

@ -41,9 +41,12 @@ void ModeQRTL::update()
void ModeQRTL::run()
{
quadplane.vtol_position_controller();
if (poscontrol.get_state() >= QuadPlane::QPOS_POSITION2) {
if (poscontrol.get_state() > QuadPlane::QPOS_POSITION2) {
// change target altitude to home alt
plane.next_WP_loc.alt = plane.home.alt;
}
if (poscontrol.get_state() >= QuadPlane::QPOS_POSITION2) {
// start landing logic
quadplane.verify_vtol_land();
}
}

16
ArduPlane/quadplane.cpp
View File

@ -2304,18 +2304,10 @@ void QuadPlane::vtol_position_controller(void)
case QPOS_POSITION1: {
setup_target_position();
if (tailsitter.enabled()) {
if (tailsitter.in_vtol_transition()) {
break;
}
poscontrol.set_state(QPOS_POSITION2);
poscontrol.pilot_correction_done = false;
gcs().send_text(MAV_SEVERITY_INFO,"VTOL position2 started v=%.1f d=%.1f",
(double)ahrs.groundspeed(), (double)plane.auto_state.wp_distance);
if (tailsitter.enabled() && tailsitter.in_vtol_transition()) {
break;
}
const Vector2f diff_wp = plane.current_loc.get_distance_NE(loc);
const float distance = diff_wp.length();
@ -2535,6 +2527,12 @@ void QuadPlane::vtol_position_controller(void)
float zero = 0;
float target_z = target_altitude_cm;
pos_control->input_pos_vel_accel_z(target_z, zero, 0);
} else if (plane.control_mode == &plane.mode_qrtl) {
Location loc2 = loc;
loc2.change_alt_frame(Location::AltFrame::ABOVE_ORIGIN);
float target_z = loc2.alt;
float zero = 0;
pos_control->input_pos_vel_accel_z(target_z, zero, 0);
} else {
set_climb_rate_cms(0, false);
}