mirror of https://github.com/ArduPilot/ardupilot
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:
parent
9de5ef72db
commit
2be23fff2b
|
@ -41,9 +41,12 @@ void ModeQRTL::update()
|
||||||
void ModeQRTL::run()
|
void ModeQRTL::run()
|
||||||
{
|
{
|
||||||
quadplane.vtol_position_controller();
|
quadplane.vtol_position_controller();
|
||||||
if (poscontrol.get_state() >= QuadPlane::QPOS_POSITION2) {
|
if (poscontrol.get_state() > QuadPlane::QPOS_POSITION2) {
|
||||||
// change target altitude to home alt
|
// change target altitude to home alt
|
||||||
plane.next_WP_loc.alt = plane.home.alt;
|
plane.next_WP_loc.alt = plane.home.alt;
|
||||||
|
}
|
||||||
|
if (poscontrol.get_state() >= QuadPlane::QPOS_POSITION2) {
|
||||||
|
// start landing logic
|
||||||
quadplane.verify_vtol_land();
|
quadplane.verify_vtol_land();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -2304,17 +2304,9 @@ void QuadPlane::vtol_position_controller(void)
|
||||||
case QPOS_POSITION1: {
|
case QPOS_POSITION1: {
|
||||||
setup_target_position();
|
setup_target_position();
|
||||||
|
|
||||||
if (tailsitter.enabled()) {
|
if (tailsitter.enabled() && tailsitter.in_vtol_transition()) {
|
||||||
if (tailsitter.in_vtol_transition()) {
|
|
||||||
break;
|
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);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
const Vector2f diff_wp = plane.current_loc.get_distance_NE(loc);
|
const Vector2f diff_wp = plane.current_loc.get_distance_NE(loc);
|
||||||
const float distance = diff_wp.length();
|
const float distance = diff_wp.length();
|
||||||
|
@ -2535,6 +2527,12 @@ void QuadPlane::vtol_position_controller(void)
|
||||||
float zero = 0;
|
float zero = 0;
|
||||||
float target_z = target_altitude_cm;
|
float target_z = target_altitude_cm;
|
||||||
pos_control->input_pos_vel_accel_z(target_z, zero, 0);
|
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 {
|
} else {
|
||||||
set_climb_rate_cms(0, false);
|
set_climb_rate_cms(0, false);
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue