bool Plane::verify_continue_and_change_alt()
{
//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 (abs(adjusted_altitude_cm() - next_WP_loc.alt) <= 500) {
return true;
}
// 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);
return false;
}
bool Plane::verify_change_alt()
{
if( (condition_rate>=0 && adjusted_altitude_cm() >= condition_value) ||
(condition_rate<=0 && adjusted_altitude_cm() <= condition_value)) {
condition_value = 0;
return true;
}
// condition_rate is climb rate in cm/s.
// We divide by 10 because this function is called at 10hz
change_target_altitude(condition_rate/10);
return false;
}
/*
set the target altitude to the current altitude, with ALT_OFFSET adjustment
*/
void Plane::set_target_altitude_current_adjusted(void)
{
set_target_altitude_current();
// use adjusted_altitude_cm() to take account of ALTITUDE_OFFSET
target_altitude.amsl_cm = adjusted_altitude_cm();
}
/*
* return true if we have breached the geo-fence maximum altiude
*/
bool Plane::geofence_check_maxalt(void)
{
if (g.fence_maxalt <= g.fence_minalt) {
return false;
}
if (g.fence_maxalt == 0) {
return false;
}
return (adjusted_altitude_cm() > (g.fence_maxalt*100.0f) + home.alt);
}
/*
return error between target altitude and current altitude
*/
int32_t Plane::calc_altitude_error_cm(void)
{
#if AP_TERRAIN_AVAILABLE
float terrain_height;
if (target_altitude.terrain_following &&
terrain.height_above_terrain(terrain_height, true)) {
return target_altitude.lookahead*100 + target_altitude.terrain_alt_cm - (terrain_height*100);
}
#endif
return target_altitude.amsl_cm - adjusted_altitude_cm();
}
/*
process a DO_CHANGE_ALT request
*/
void Plane::do_change_alt(const AP_Mission::Mission_Command& cmd)
{
condition_rate = labs((int)cmd.content.location.lat); // climb rate in cm/s
condition_value = cmd.content.location.alt; // To-Do: ensure this altitude is an absolute altitude?
if (condition_value < adjusted_altitude_cm()) {
condition_rate = -condition_rate;
}
set_target_altitude_current_adjusted();
change_target_altitude(condition_rate/10);
next_WP_loc.alt = condition_value; // For future nav calculations
reset_offset_altitude();
setup_glide_slope();
}
void Plane::adjust_landing_slope_for_rangefinder_bump(void)
{
#if RANGEFINDER_ENABLED == ENABLED
// check the rangefinder correction for a large change. When found, recalculate the glide slope. This is done by
// determining the slope from your current location to the land point then following that back up to the approach
// altitude and moving the prev_wp to that location. From there
float correction_delta = fabsf(rangefinder_state.last_stable_correction) - fabsf(rangefinder_state.correction);
if (g.land_slope_recalc_shallow_threshold <= 0 ||
fabsf(correction_delta) < g.land_slope_recalc_shallow_threshold) {
return;
}
rangefinder_state.last_stable_correction = rangefinder_state.correction;
float corrected_alt_m = (adjusted_altitude_cm() - next_WP_loc.alt)*0.01f - rangefinder_state.correction;
float total_distance_m = get_distance(prev_WP_loc, next_WP_loc);
float top_of_glide_slope_alt_m = total_distance_m * corrected_alt_m / auto_state.wp_distance;
prev_WP_loc.alt = top_of_glide_slope_alt_m*100 + next_WP_loc.alt;
// re-calculate auto_state.land_slope with updated prev_WP_loc
setup_landing_glide_slope();
if (rangefinder_state.correction >= 0) { // we're too low or object is below us
// correction positive means we're too low so we should continue on with
// the newly computed shallower slope instead of pitching/throttling up
} else if (g.land_slope_recalc_steep_threshold_to_abort > 0) {
// correction negative means we're too high and need to point down (and speed up) to re-align
// to land on target. A large negative correction means we would have to dive down a lot and will
// generating way too much speed that we can not bleed off in time. It is better to remember
// the large baro altitude offset and abort the landing to come around again with the correct altitude
// offset and "perfect" slope.
// calculate projected slope with projected alt
float new_slope_deg = degrees(atan(auto_state.land_slope));
float initial_slope_deg = degrees(atan(auto_state.initial_land_slope));
// is projected slope too steep?
if (new_slope_deg - initial_slope_deg > g.land_slope_recalc_steep_threshold_to_abort) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Steep landing slope (%.0fm %.1fdeg)",
(double)rangefinder_state.correction, (double)(new_slope_deg - initial_slope_deg));
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "aborting landing!");
auto_state.land_alt_offset = rangefinder_state.correction;
auto_state.commanded_go_around = 1;
g.land_slope_recalc_steep_threshold_to_abort = 0; // disable this feature so we only perform it once
}
}
#endif
}
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;
}
/*
return the height in meters above the next_WP_loc altitude
*/
float Plane::height_above_target(void)
{
float target_alt = next_WP_loc.alt*0.01;
if (!next_WP_loc.relative_alt) {
target_alt -= ahrs.get_home().alt*0.01f;
}
#if AP_TERRAIN_AVAILABLE
// also record the terrain altitude if possible
float terrain_altitude;
if (next_WP_loc.terrain_alt &&
terrain.height_above_terrain(terrain_altitude, true)) {
return terrain_altitude - target_alt;
}
#endif
return (adjusted_altitude_cm()*0.01f - ahrs.get_home().alt*0.01f) - target_alt;
}
/*
return home-relative altitude adjusted for ALT_OFFSET This is useful
during long flights to account for barometer changes from the GCS,
or to adjust the flying height of a long mission
*/
int32_t Plane::adjusted_relative_altitude_cm(void)
{
return adjusted_altitude_cm() - home.alt;
}
