|
|
@ -21,6 +21,7 @@ |
|
|
|
#include <GCS_MAVLink/GCS.h> |
|
|
|
#include <GCS_MAVLink/GCS.h> |
|
|
|
#include <AP_HAL/AP_HAL.h> |
|
|
|
#include <AP_HAL/AP_HAL.h> |
|
|
|
#include <AP_LandingGear/AP_LandingGear.h> |
|
|
|
#include <AP_LandingGear/AP_LandingGear.h> |
|
|
|
|
|
|
|
#include <AP_GPS/AP_GPS.h> |
|
|
|
|
|
|
|
|
|
|
|
void AP_Landing::type_slope_do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude) |
|
|
|
void AP_Landing::type_slope_do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude) |
|
|
|
{ |
|
|
|
{ |
|
|
@ -92,14 +93,15 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n |
|
|
|
(!rangefinder_state_in_range && wp_proportion >= 1) || |
|
|
|
(!rangefinder_state_in_range && wp_proportion >= 1) || |
|
|
|
probably_crashed) { |
|
|
|
probably_crashed) { |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
const AP_GPS &gps = AP::gps(); |
|
|
|
if (type_slope_stage != SLOPE_STAGE_FINAL) { |
|
|
|
if (type_slope_stage != SLOPE_STAGE_FINAL) { |
|
|
|
type_slope_flags.post_stats = true; |
|
|
|
type_slope_flags.post_stats = true; |
|
|
|
if (is_flying && (AP_HAL::millis()-last_flying_ms) > 3000) { |
|
|
|
if (is_flying && (AP_HAL::millis()-last_flying_ms) > 3000) { |
|
|
|
gcs().send_text(MAV_SEVERITY_CRITICAL, "Flare crash detected: speed=%.1f", (double)AP::gps().ground_speed()); |
|
|
|
gcs().send_text(MAV_SEVERITY_CRITICAL, "Flare crash detected: speed=%.1f", (double)gps.ground_speed()); |
|
|
|
} else { |
|
|
|
} else { |
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "Flare %.1fm sink=%.2f speed=%.1f dist=%.1f", |
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "Flare %.1fm sink=%.2f speed=%.1f dist=%.1f", |
|
|
|
(double)height, (double)sink_rate, |
|
|
|
(double)height, (double)sink_rate, |
|
|
|
(double)AP::gps().ground_speed(), |
|
|
|
(double)gps.ground_speed(), |
|
|
|
(double)current_loc.get_distance(next_WP_loc)); |
|
|
|
(double)current_loc.get_distance(next_WP_loc)); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
@ -114,7 +116,7 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
if (AP::gps().ground_speed() < 3) { |
|
|
|
if (gps.ground_speed() < 3) { |
|
|
|
// reload any airspeed or groundspeed parameters that may have
|
|
|
|
// reload any airspeed or groundspeed parameters that may have
|
|
|
|
// been set for landing. We don't do this till ground
|
|
|
|
// been set for landing. We don't do this till ground
|
|
|
|
// speed drops below 3.0 m/s as otherwise we will change
|
|
|
|
// speed drops below 3.0 m/s as otherwise we will change
|
|
|
|