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

Plane: Quadplane: QRTL climb to QRTL alt first if in Q mode

This commit is contained in:
Iampete1 2022-05-31 02:38:55 +01:00 committed by Andrew Tridgell
parent 4e438464d7
commit d251c51ca1
3 changed files with 104 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduPlane/mode.h
View File

@ -599,6 +599,13 @@ public:
protected:
bool _enter() override;
private:
enum class SubMode {
climb,
RTL,
} submode;
};
class ModeQAcro : public Mode

123
ArduPlane/mode_qrtl.cpp
View File

@ -11,28 +11,52 @@ bool ModeQRTL::_enter()
plane.set_mode(plane.mode_qland, ModeReason::QLAND_INSTEAD_OF_RTL);
return true;
}
submode = SubMode::RTL;
plane.prev_WP_loc = plane.current_loc;
// clear QPOS state
quadplane.poscontrol.set_state(QuadPlane::QPOS_NONE);
const int32_t RTL_alt_abs_cm = plane.home.alt + quadplane.qrtl_alt*100UL;
if (quadplane.motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) {
// VTOL motors are active, either in VTOL flight or assisted flight
Location destination = plane.rally.calc_best_rally_or_home_location(plane.current_loc, RTL_alt_abs_cm);
const float dist = plane.current_loc.get_distance(destination);
const float radius = MAX(fabsf(plane.aparm.loiter_radius), fabsf(plane.g.rtl_radius));
const float dist_to_climb = quadplane.qrtl_alt - plane.relative_ground_altitude(plane.g.rangefinder_landing);
if (dist < 1.5*radius) {
// we're close to destination and already running VTOL motors, don't transition and don't climb
gcs().send_text(MAV_SEVERITY_INFO,"VTOL position1 d=%.1f r=%.1f", dist, radius);
poscontrol.set_state(QuadPlane::QPOS_POSITION1);
} else if (is_positive(dist_to_climb)) {
// climb before returning, only next waypoint altitude is used
submode = SubMode::climb;
plane.next_WP_loc = plane.current_loc;
#if AP_TERRAIN_AVAILABLE
if (plane.terrain_enabled_in_mode(mode_number())) {
plane.next_WP_loc.set_alt_cm(quadplane.qrtl_alt * 100UL, Location::AltFrame::ABOVE_TERRAIN);
return true;
}
#endif
plane.next_WP_loc.set_alt_cm(plane.current_loc.alt + dist_to_climb * 100UL, plane.current_loc.get_alt_frame());
return true;
}
}
// use do_RTL() to setup next_WP_loc
plane.do_RTL(plane.home.alt + quadplane.qrtl_alt*100UL);
plane.prev_WP_loc = plane.current_loc;
pos_control->set_accel_desired_xy_cmss(Vector2f());
pos_control->init_xy_controller();
plane.do_RTL(RTL_alt_abs_cm);
quadplane.poscontrol_init_approach();
float dist = plane.next_WP_loc.get_distance(plane.current_loc);
const float radius = MAX(fabsf(plane.aparm.loiter_radius), fabsf(plane.g.rtl_radius));
if (dist < 1.5*radius &&
quadplane.motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) {
// we're close to destination and already running VTOL motors, don't transition
gcs().send_text(MAV_SEVERITY_INFO,"VTOL position1 d=%.1f r=%.1f", dist, radius);
poscontrol.set_state(QuadPlane::QPOS_POSITION1);
}
int32_t from_alt;
int32_t to_alt;
if (plane.current_loc.get_alt_cm(Location::AltFrame::ABSOLUTE,from_alt) && plane.next_WP_loc.get_alt_cm(Location::AltFrame::ABSOLUTE,to_alt)) {
poscontrol.slow_descent = from_alt > to_alt;
return true;
}
// defualt back to old method
// default back to old method
poscontrol.slow_descent = (plane.current_loc.alt > plane.next_WP_loc.alt);
return true;
}
@ -47,20 +71,63 @@ void ModeQRTL::update()
*/
void ModeQRTL::run()
{
quadplane.vtol_position_controller();
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();
}
switch (submode) {
case SubMode::climb: {
// request zero velocity
Vector2f vel, accel;
pos_control->input_vel_accel_xy(vel, accel);
quadplane.run_xy_controller();
// when in approach allow stick mixing
if (quadplane.poscontrol.get_state() == QuadPlane::QPOS_AIRBRAKE ||
quadplane.poscontrol.get_state() == QuadPlane::QPOS_APPROACH) {
plane.stabilize_stick_mixing_fbw();
// nav roll and pitch are controller by position controller
plane.nav_roll_cd = pos_control->get_roll_cd();
plane.nav_pitch_cd = pos_control->get_pitch_cd();
if (quadplane.transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd)) {
pos_control->set_externally_limited_xy();
}
// weathervane with no pilot input
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
plane.nav_pitch_cd,
quadplane.get_weathervane_yaw_rate_cds());
// climb at full WP nav speed
quadplane.set_climb_rate_cms(quadplane.wp_nav->get_default_speed_up(), false);
quadplane.run_z_controller();
ftype alt_diff;
if (!plane.current_loc.get_alt_distance(plane.next_WP_loc, alt_diff) || is_positive(alt_diff)) {
// climb finshed or cant get alt diff, head home
submode = SubMode::RTL;
plane.prev_WP_loc = plane.current_loc;
plane.do_RTL(plane.home.alt + quadplane.qrtl_alt*100UL);
quadplane.poscontrol_init_approach();
if (plane.current_loc.get_alt_distance(plane.next_WP_loc, alt_diff)) {
poscontrol.slow_descent = is_positive(alt_diff);
} else {
// default back to old method
poscontrol.slow_descent = (plane.current_loc.alt > plane.next_WP_loc.alt);
}
}
break;
}
case SubMode::RTL: {
quadplane.vtol_position_controller();
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();
}
// when in approach allow stick mixing
if (quadplane.poscontrol.get_state() == QuadPlane::QPOS_AIRBRAKE ||
quadplane.poscontrol.get_state() == QuadPlane::QPOS_APPROACH) {
plane.stabilize_stick_mixing_fbw();
}
break;
}
}
}
@ -72,7 +139,7 @@ bool ModeQRTL::update_target_altitude()
/*
update height target in approach
*/
if (plane.quadplane.poscontrol.get_state() != QuadPlane::QPOS_APPROACH) {
if ((submode != SubMode::RTL) || (plane.quadplane.poscontrol.get_state() != QuadPlane::QPOS_APPROACH)) {
return false;
}
@ -99,7 +166,7 @@ bool ModeQRTL::update_target_altitude()
// only nudge during approach
bool ModeQRTL::allows_throttle_nudging() const
{
return plane.quadplane.poscontrol.get_state() == QuadPlane::QPOS_APPROACH;
return (submode == SubMode::RTL) && (plane.quadplane.poscontrol.get_state() == QuadPlane::QPOS_APPROACH);
}
#endif

3
ArduPlane/quadplane.cpp
View File

@ -2187,6 +2187,7 @@ void QuadPlane::poscontrol_init_approach(void)
}
poscontrol.pilot_correction_done = false;
poscontrol.xy_correction.zero();
poscontrol.slow_descent = false;
}
/*
@ -3473,13 +3474,13 @@ void QuadPlane::guided_start(void)
setup_target_position();
int32_t from_alt;
int32_t to_alt;
poscontrol_init_approach();
if (plane.current_loc.get_alt_cm(Location::AltFrame::ABSOLUTE,from_alt) && plane.next_WP_loc.get_alt_cm(Location::AltFrame::ABSOLUTE,to_alt)) {
poscontrol.slow_descent = from_alt > to_alt;
} else {
// default back to old method
poscontrol.slow_descent = (plane.current_loc.alt > plane.next_WP_loc.alt);
}
poscontrol_init_approach();
}
/*