// rtl_climb_start - initialise climb to RTL altitude
void Copter::rtl_climb_start()
{
rtl_state = RTL_InitialClimb;
rtl_state_complete = false;
rtl_alt = get_RTL_alt();
// initialise waypoint and spline controller
wp_nav.wp_and_spline_init();
// get horizontal stopping point
Vector3f destination;
wp_nav.get_wp_stopping_point_xy(destination);
#if AC_RALLY == ENABLED
// rally_point.alt will be the altitude of the nearest rally point or the RTL_ALT. uses absolute altitudes
Location rally_point = rally.calc_best_rally_or_home_location(current_loc, rtl_alt+ahrs.get_home().alt);
rally_point.alt -= ahrs.get_home().alt; // convert to altitude above home
rally_point.alt = max(rally_point.alt, current_loc.alt); // ensure we do not descend before reaching home
destination.z = pv_alt_above_origin(rally_point.alt);
#else
destination.z = pv_alt_above_origin(rtl_alt);
#endif
// set the destination
wp_nav.set_wp_destination(destination);
wp_nav.set_fast_waypoint(true);
// hold current yaw during initial climb
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
// rtl_descent_run - implements the final descent to the RTL_ALT
// called by rtl_run at 100hz or more
void Copter::rtl_descent_run()
{
int16_t roll_control = 0, pitch_control = 0;
float target_yaw_rate = 0;
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// set target to current position
wp_nav.init_loiter_target();
return;
}
// process pilot's input
if (!failsafe.radio) {
if (g.land_repositioning) {
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// process pilot's roll and pitch input
roll_control = channel_roll->control_in;
pitch_control = channel_pitch->control_in;
}
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
}
// process roll, pitch inputs
wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control);
// run loiter controller
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call z-axis position controller
pos_control.set_alt_target_with_slew(pv_alt_above_origin(g.rtl_alt_final), G_Dt);
pos_control.update_z_controller();
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
// check if we've reached within 20cm of final altitude
rtl_state_complete = fabsf(pv_alt_above_origin(g.rtl_alt_final) - inertial_nav.get_altitude()) < 20.0f;
}
// return altitude in cm above origin at which vehicle should return home
float Copter::rtl_compute_return_alt_above_origin(float rtl_return_dist)
{
// maximum of current altitude + climb_min and rtl altitude
float ret = MAX(current_loc.alt + MAX(0, g.rtl_climb_min), MAX(g.rtl_altitude, RTL_ALT_MIN));
if (g.rtl_cone_slope >= RTL_MIN_CONE_SLOPE) { // don't allow really shallow slopes
ret = MAX(current_loc.alt, MIN(ret, MAX(rtl_return_dist*g.rtl_cone_slope, current_loc.alt+RTL_ABS_MIN_CLIMB)));
}
#if AC_FENCE == ENABLED
// ensure not above fence altitude if alt fence is enabled
if ((fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) {
ret = MIN(ret, fence.get_safe_alt()*100.0f);
}
#endif
#if AC_RALLY == ENABLED
// rally_point.alt will be the altitude of the nearest rally point or the RTL_ALT. uses absolute altitudes
Location rally_point = rally.calc_best_rally_or_home_location(current_loc, ret+ahrs.get_home().alt);
rally_point.alt -= ahrs.get_home().alt; // convert to altitude above home
rally_point.alt = MAX(rally_point.alt, current_loc.alt); // ensure we do not descend before reaching home
ret = rally_point.alt;
#endif
return pv_alt_above_origin(ret);
}
void Copter::rtl_build_path()
{
// origin point is our stopping point
pos_control.get_stopping_point_xy(rtl_path.origin_point);
pos_control.get_stopping_point_z(rtl_path.origin_point);
// set return target to nearest rally point or home position
#if AC_RALLY == ENABLED
Location rally_point = rally.calc_best_rally_or_home_location(current_loc, 0);
rtl_path.return_target = pv_location_to_vector(rally_point);
#else
rtl_path.return_target = pv_location_to_vector(ahrs.get_home());
#endif
Vector3f return_vector = rtl_path.return_target-rtl_path.origin_point;
float rtl_return_dist = pythagorous2(return_vector.x, return_vector.y);
// compute return altitude
rtl_path.return_target.z = rtl_compute_return_alt_above_origin(rtl_return_dist);
// climb target is above our origin point at the return altitude
rtl_path.climb_target.x = rtl_path.origin_point.x;
rtl_path.climb_target.y = rtl_path.origin_point.y;
rtl_path.climb_target.z = rtl_path.return_target.z;
// descent target is below return target at rtl_alt_final
rtl_path.descent_target.x = rtl_path.return_target.x;
rtl_path.descent_target.y = rtl_path.return_target.y;
rtl_path.descent_target.z = pv_alt_above_origin(g.rtl_alt_final);
// set land flag
rtl_path.land = g.rtl_alt_final <= 0;
}
// get_land_descent_speed - high level landing logic
// returns climb rate (in cm/s) which should be passed to the position controller
// should be called at 100hz or higher
float Copter::get_land_descent_speed()
{
#if CONFIG_SONAR == ENABLED
bool sonar_ok = sonar_enabled && (sonar.status() == RangeFinder::RangeFinder_Good);
#else
bool sonar_ok = false;
#endif
// if we are above 10m and the sonar does not sense anything perform regular alt hold descent
if (pos_control.get_pos_target().z >= pv_alt_above_origin(LAND_START_ALT) && !(sonar_ok && sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
return pos_control.get_speed_down();
}else{
return -abs(g.land_speed);
}
}
// auto_takeoff_start - initialises waypoint controller to implement take-off
void Copter::auto_takeoff_start(float final_alt_above_home)
{
auto_mode = Auto_TakeOff;
// initialise wpnav destination
Vector3f target_pos = inertial_nav.get_position();
target_pos.z = pv_alt_above_origin(final_alt_above_home);
wp_nav.set_wp_destination(target_pos);
// initialise yaw
set_auto_yaw_mode(AUTO_YAW_HOLD);
// clear i term when we're taking off
set_throttle_takeoff();
}
// rtl_return_start - initialise return to home
void Copter::rtl_return_start()
{
rtl_state = RTL_ReturnHome;
rtl_state_complete = false;
// set target to above home/rally point
#if AC_RALLY == ENABLED
// rally_point will be the nearest rally point or home. uses absolute altitudes
Location rally_point = rally.calc_best_rally_or_home_location(current_loc, get_RTL_alt()+ahrs.get_home().alt);
rally_point.alt -= ahrs.get_home().alt; // convert to altitude above home
rally_point.alt = max(rally_point.alt, current_loc.alt); // ensure we do not descend before reaching home
Vector3f destination = pv_location_to_vector(rally_point);
#else
Vector3f destination = pv_location_to_vector(ahrs.get_home());
destination.z = pv_alt_above_origin(get_RTL_alt());
#endif
wp_nav.set_wp_destination(destination);
// initialise yaw to point home (maybe)
set_auto_yaw_mode(get_default_auto_yaw_mode(true));
}
// updates position controller's maximum altitude using fence and EKF limits
void Copter::update_poscon_alt_max()
{
float alt_limit_cm = 0.0f; // interpreted as no limit if left as zero
#if AC_FENCE == ENABLED
// set fence altitude limit in position controller
if ((fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) {
alt_limit_cm = pv_alt_above_origin(fence.get_safe_alt()*100.0f);
}
#endif
// get alt limit from EKF (limited during optical flow flight)
float ekf_limit_cm = 0.0f;
if (inertial_nav.get_hgt_ctrl_limit(ekf_limit_cm)) {
if ((alt_limit_cm <= 0.0f) || (ekf_limit_cm < alt_limit_cm)) {
alt_limit_cm = ekf_limit_cm;
}
}
// pass limit to pos controller
pos_control.set_alt_max(alt_limit_cm);
}
// pv_location_to_vector - convert lat/lon coordinates to a position vector
Vector3f Sub::pv_location_to_vector(const Location& loc)
{
const struct Location &origin = inertial_nav.get_origin();
float alt_above_origin = pv_alt_above_origin(loc.alt); // convert alt-relative-to-home to alt-relative-to-origin
return Vector3f((loc.lat-origin.lat) * LATLON_TO_CM, (loc.lng-origin.lng) * LATLON_TO_CM * scaleLongDown, alt_above_origin);
}
