void Plane::do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd)
{
// select heading method. Either mission, gps bearing projection or yaw based
// If prev_WP_loc and next_WP_loc are different then an accurate wp based bearing can
// be computed. However, if we had just changed modes before this, such as an aborted landing
// via mode change, the prev and next wps are the same.
float bearing;
if (!locations_are_same(prev_WP_loc, next_WP_loc)) {
// use waypoint based bearing, this is the usual case
steer_state.hold_course_cd = -1;
} else if (ahrs.get_gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
// use gps ground course based bearing hold
steer_state.hold_course_cd = -1;
bearing = ahrs.get_gps().ground_course_cd() * 0.01f;
location_update(next_WP_loc, bearing, 1000); // push it out 1km
} else {
// use yaw based bearing hold
steer_state.hold_course_cd = wrap_360_cd(ahrs.yaw_sensor);
bearing = ahrs.yaw_sensor * 0.01f;
location_update(next_WP_loc, bearing, 1000); // push it out 1km
}
next_WP_loc.alt = cmd.content.location.alt + home.alt;
condition_value = cmd.p1;
reset_offset_altitude();
}
bool Plane::verify_continue_and_change_alt()
{
// is waypoint info not available and heading hold is?
if (locations_are_same(prev_WP_loc, next_WP_loc) &&
steer_state.hold_course_cd != -1) {
//keep flying the same course with fixed steering heading computed at start if cmd
nav_controller->update_heading_hold(steer_state.hold_course_cd);
}
else {
// Is the next_WP less than 200 m away?
if (get_distance(current_loc, next_WP_loc) < 200.0f) {
//push another 300 m down the line
int32_t next_wp_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc);
location_update(next_WP_loc, next_wp_bearing_cd * 0.01f, 300.0f);
}
//keep flying the same course
nav_controller->update_waypoint(prev_WP_loc, next_WP_loc);
}
//climbing?
if (condition_value == 1 && adjusted_altitude_cm() >= next_WP_loc.alt) {
return true;
}
//descending?
else if (condition_value == 2 &&
adjusted_altitude_cm() <= next_WP_loc.alt) {
return true;
}
//don't care if we're climbing or descending
else if (labs(adjusted_altitude_cm() - next_WP_loc.alt) <= 500) {
return true;
}
return false;
}
/*
handle auto-mode when auto_state.vtol_mode is true
*/
void QuadPlane::control_auto(const Location &loc)
{
if (!setup()) {
return;
}
Location origin = inertial_nav.get_origin();
Vector2f diff2d;
Vector3f target;
diff2d = location_diff(origin, loc);
target.x = diff2d.x * 100;
target.y = diff2d.y * 100;
target.z = loc.alt - origin.alt;
if (!locations_are_same(loc, last_auto_target) ||
loc.alt != last_auto_target.alt ||
millis() - last_loiter_ms > 500) {
wp_nav->set_wp_destination(target);
last_auto_target = loc;
}
last_loiter_ms = millis();
// initialize vertical speed and acceleration
pos_control->set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max);
pos_control->set_accel_z(pilot_accel_z);
if (plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_VTOL_TAKEOFF) {
/*
for takeoff we need to use the loiter controller wpnav controller takes over the descent rate
control
*/
float ekfGndSpdLimit, ekfNavVelGainScaler;
ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
// run loiter controller
wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd,
plane.nav_pitch_cd,
get_pilot_input_yaw_rate_cds(),
smoothing_gain);
// nav roll and pitch are controller by position controller
plane.nav_roll_cd = pos_control->get_roll();
plane.nav_pitch_cd = pos_control->get_pitch();
} else if (plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_VTOL_LAND &&
land_state >= QLAND_FINAL) {
/*
for land-final we use the loiter controller
*/
float ekfGndSpdLimit, ekfNavVelGainScaler;
ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
// run loiter controller
wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd,
plane.nav_pitch_cd,
get_pilot_input_yaw_rate_cds(),
smoothing_gain);
// nav roll and pitch are controller by position controller
plane.nav_roll_cd = pos_control->get_roll();
plane.nav_pitch_cd = pos_control->get_pitch();
} else if (plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_VTOL_LAND) {
/*
for land repositioning we run the loiter controller
*/
// also run fixed wing navigation
plane.nav_controller->update_waypoint(plane.prev_WP_loc, plane.next_WP_loc);
pos_control->set_xy_target(target.x, target.y);
float ekfGndSpdLimit, ekfNavVelGainScaler;
ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
// run loiter controller
wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// nav roll and pitch are controller by position controller
plane.nav_roll_cd = wp_nav->get_roll();
plane.nav_pitch_cd = wp_nav->get_pitch();
if (land_state == QLAND_POSITION) {
// during positioning we may be flying faster than the position
// controller normally wants to fly. We let that happen by
// limiting the pitch controller
land_wp_proportion = constrain_float(MAX(land_wp_proportion, plane.auto_state.wp_proportion), 0, 1);
int32_t limit = land_wp_proportion * plane.aparm.pitch_limit_max_cd;
plane.nav_pitch_cd = constrain_int32(plane.nav_pitch_cd, plane.aparm.pitch_limit_min_cd, limit);
wp_nav->set_speed_xy(constrain_float((1-land_wp_proportion)*20*100.0, 500, 2000));
}
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd,
plane.nav_pitch_cd,
get_pilot_input_yaw_rate_cds(),
smoothing_gain);
} else {
/*
this is full copter control of auto flight
*/
// run wpnav controller
wp_nav->update_wpnav();
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(),
wp_nav->get_pitch(),
wp_nav->get_yaw(),
true);
// nav roll and pitch are controller by loiter controller
plane.nav_roll_cd = wp_nav->get_roll();
plane.nav_pitch_cd = wp_nav->get_pitch();
}
switch (plane.mission.get_current_nav_cmd().id) {
case MAV_CMD_NAV_VTOL_LAND:
if (land_state == QLAND_POSITION) {
pos_control->set_alt_target_from_climb_rate(0, plane.G_Dt, false);
} else if (land_state > QLAND_POSITION && land_state < QLAND_FINAL) {
pos_control->set_alt_target_from_climb_rate(-wp_nav->get_speed_down(), plane.G_Dt, true);
} else {
pos_control->set_alt_target_from_climb_rate(-land_speed_cms, plane.G_Dt, true);
}
break;
case MAV_CMD_NAV_VTOL_TAKEOFF:
pos_control->set_alt_target_from_climb_rate(100, plane.G_Dt, true);
break;
default:
pos_control->set_alt_target_from_climb_rate_ff(assist_climb_rate_cms(), plane.G_Dt, false);
break;
}
pos_control->update_z_controller();
}
