ardupilot/ArduPlane/mode_qrtl.cpp

89 lines
2.9 KiB
C++

#include "mode.h"
#include "Plane.h"
bool ModeQRTL::_enter()
{
return plane.mode_qstabilize._enter();
}
/*
handle QRTL mode
*/
void ModeQRTL::init()
{
// 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();
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;
}
// defualt back to old method
poscontrol.slow_descent = (plane.current_loc.alt > plane.next_WP_loc.alt);
}
void ModeQRTL::update()
{
plane.mode_qstabilize.update();
}
/*
handle QRTL mode
*/
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;
quadplane.verify_vtol_land();
}
}
/*
update target altitude for QRTL profile
*/
bool ModeQRTL::update_target_altitude()
{
/*
update height target in approach
*/
if (plane.quadplane.poscontrol.get_state() != QuadPlane::QPOS_APPROACH) {
return false;
}
/*
initially approach at RTL_ALT_CM, then drop down to QRTL_ALT based on maximum sink rate from TECS,
giving time to lose speed before we transition
*/
const float radius = MAX(fabsf(plane.aparm.loiter_radius), fabsf(plane.g.rtl_radius));
const float rtl_alt_delta = MAX(0, plane.g.RTL_altitude_cm*0.01 - plane.quadplane.qrtl_alt);
const float sink_time = rtl_alt_delta / MAX(0.6*plane.SpdHgt_Controller->get_max_sinkrate(), 1);
const float sink_dist = plane.aparm.airspeed_cruise_cm * 0.01 * sink_time;
const float dist = plane.auto_state.wp_distance;
const float rad_min = 2*radius;
const float rad_max = 20*radius;
float alt = linear_interpolate(0, rtl_alt_delta,
dist,
rad_min, MAX(rad_min, MIN(rad_max, rad_min+sink_dist)));
Location loc = plane.next_WP_loc;
loc.alt += alt*100;
plane.set_target_altitude_location(loc);
return true;
}