/*
read the rangefinder and update height estimate
*/
void Plane::read_rangefinder(void)
{
#if RANGEFINDER_ENABLED == ENABLED
// notify the rangefinder of our approximate altitude above ground to allow it to power on
// during low-altitude flight when configured to power down during higher-altitude flight
float height;
#if AP_TERRAIN_AVAILABLE
if (terrain.status() == AP_Terrain::TerrainStatusOK && terrain.height_above_terrain(height, true)) {
rangefinder.set_estimated_terrain_height(height);
} else
#endif
{
// use the best available alt estimate via baro above home
if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH ||
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE ||
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) {
// ensure the rangefinder is powered-on when land alt is higher than home altitude.
// This is done using the target alt which we know is below us and we are sinking to it
height = height_above_target();
} else {
// otherwise just use the best available baro estimate above home.
height = relative_altitude();
}
rangefinder.set_estimated_terrain_height(height);
}
rangefinder.update();
if (should_log(MASK_LOG_SONAR))
Log_Write_Sonar();
rangefinder_height_update();
#endif
}
/*
return relative altitude in meters (relative to terrain, if available,
or home otherwise)
*/
float Plane::relative_ground_altitude(void)
{
#if AP_TERRAIN_AVAILABLE
float altitude;
if (terrain.status() == AP_Terrain::TerrainStatusOK && terrain.height_above_terrain(altitude, true)) {
return altitude;
}
#endif
return relative_altitude();
}
/*
return relative altitude in meters (relative to terrain, if available,
or home otherwise)
*/
float Plane::relative_ground_altitude(bool use_rangefinder_if_available)
{
if (use_rangefinder_if_available && rangefinder_state.in_range) {
return rangefinder_state.height_estimate;
}
#if AP_TERRAIN_AVAILABLE
float altitude;
if (terrain.status() == AP_Terrain::TerrainStatusOK && terrain.height_above_terrain(altitude, true)) {
return altitude;
}
#endif
return relative_altitude();
}
/*
update the offset between rangefinder height and terrain height
*/
void Plane::rangefinder_height_update(void)
{
uint16_t distance_cm = rangefinder.distance_cm();
float height_estimate = 0;
if ((rangefinder.status() == RangeFinder::RangeFinder_Good) && home_is_set != HOME_UNSET) {
// correct the range for attitude (multiply by DCM.c.z, which
// is cos(roll)*cos(pitch))
height_estimate = distance_cm * 0.01f * ahrs.get_dcm_matrix().c.z;
// we consider ourselves to be fully in range when we have 10
// good samples (0.2s)
if (rangefinder_state.in_range_count < 10) {
rangefinder_state.in_range_count++;
} else {
rangefinder_state.in_range = true;
}
} else {
rangefinder_state.in_range_count = 0;
rangefinder_state.in_range = false;
}
if (rangefinder_state.in_range) {
// base correction is the difference between baro altitude and
// rangefinder estimate
float correction = relative_altitude() - height_estimate;
#if AP_TERRAIN_AVAILABLE
// if we are terrain following then correction is based on terrain data
float terrain_altitude;
if ((target_altitude.terrain_following || g.terrain_follow) &&
terrain.height_above_terrain(terrain_altitude, true)) {
correction = terrain_altitude - height_estimate;
}
#endif
// remember the last correction. Use a low pass filter unless
// the old data is more than 5 seconds old
if (millis() - rangefinder_state.last_correction_time_ms > 5000) {
rangefinder_state.correction = correction;
} else {
rangefinder_state.correction = 0.8f*rangefinder_state.correction + 0.2f*correction;
}
rangefinder_state.last_correction_time_ms = millis();
}
}
/*
the height above field elevation that we pass to TECS
*/
float Plane::tecs_hgt_afe(void)
{
/*
pass the height above field elevation as the height above
the ground when in landing, which means that TECS gets the
rangefinder information and thus can know when the flare is
coming.
*/
float hgt_afe;
if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL ||
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
hgt_afe = height_above_target();
hgt_afe -= rangefinder_correction();
} else {
// when in normal flight we pass the hgt_afe as relative
// altitude to home
hgt_afe = relative_altitude();
}
return hgt_afe;
}
/*
stabilize the yaw axis. There are 3 modes of operation:
- hold a specific heading with ground steering
- rate controlled with ground steering
- yaw control for coordinated flight
*/
void Plane::stabilize_yaw(float speed_scaler)
{
if (control_mode == AUTO && flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) {
// in land final setup for ground steering
steering_control.ground_steering = true;
} else {
// otherwise use ground steering when no input control and we
// are below the GROUND_STEER_ALT
steering_control.ground_steering = (channel_roll->get_control_in() == 0 &&
fabsf(relative_altitude()) < g.ground_steer_alt);
if (control_mode == AUTO &&
(flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH ||
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE)) {
// don't use ground steering on landing approach
steering_control.ground_steering = false;
}
}
/*
first calculate steering_control.steering for a nose or tail
wheel.
We use "course hold" mode for the rudder when either in the
final stage of landing (when the wings are help level) or when
in course hold in FBWA mode (when we are below GROUND_STEER_ALT)
*/
if ((control_mode == AUTO && flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) ||
(steer_state.hold_course_cd != -1 && steering_control.ground_steering)) {
calc_nav_yaw_course();
} else if (steering_control.ground_steering) {
calc_nav_yaw_ground();
}
/*
now calculate steering_control.rudder for the rudder
*/
calc_nav_yaw_coordinated(speed_scaler);
}
/*
parachute_manual_release - trigger the release of the parachute,
after performing some checks for pilot error checks if the vehicle
is landed
*/
bool Plane::parachute_manual_release()
{
// exit immediately if parachute is not enabled
if (!parachute.enabled() || parachute.released()) {
return false;
}
// do not release if vehicle is not flying
if (!is_flying()) {
// warn user of reason for failure
gcs_send_text_P(MAV_SEVERITY_WARNING,PSTR("Parachute: not flying"));
return false;
}
if (relative_altitude() < parachute.alt_min()) {
gcs_send_text_fmt(PSTR("Parachute: too low"));
return false;
}
// if we get this far release parachute
parachute_release();
return true;
}
/*
update the offset between rangefinder height and terrain height
*/
void Plane::rangefinder_height_update(void)
{
float distance = rangefinder.distance_cm()*0.01f;
if ((rangefinder.status() == RangeFinder::RangeFinder_Good) && home_is_set != HOME_UNSET) {
if (!rangefinder_state.have_initial_reading) {
rangefinder_state.have_initial_reading = true;
rangefinder_state.initial_range = distance;
}
// correct the range for attitude (multiply by DCM.c.z, which
// is cos(roll)*cos(pitch))
rangefinder_state.height_estimate = distance * ahrs.get_rotation_body_to_ned().c.z;
// we consider ourselves to be fully in range when we have 10
// good samples (0.2s) that are different by 5% of the maximum
// range from the initial range we see. The 5% change is to
// catch Lidars that are giving a constant range, either due
// to misconfiguration or a faulty sensor
if (rangefinder_state.in_range_count < 10) {
if (fabsf(rangefinder_state.initial_range - distance) > 0.05f * rangefinder.max_distance_cm()*0.01f) {
rangefinder_state.in_range_count++;
}
} else {
rangefinder_state.in_range = true;
if (!rangefinder_state.in_use &&
(flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH ||
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE ||
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL ||
control_mode == QLAND ||
control_mode == QRTL ||
(control_mode == AUTO && plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_VTOL_LAND)) &&
g.rangefinder_landing) {
rangefinder_state.in_use = true;
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Rangefinder engaged at %.2fm", (double)rangefinder_state.height_estimate);
}
}
} else {
rangefinder_state.in_range_count = 0;
rangefinder_state.in_range = false;
}
if (rangefinder_state.in_range) {
// base correction is the difference between baro altitude and
// rangefinder estimate
float correction = relative_altitude() - rangefinder_state.height_estimate;
#if AP_TERRAIN_AVAILABLE
// if we are terrain following then correction is based on terrain data
float terrain_altitude;
if ((target_altitude.terrain_following || g.terrain_follow) &&
terrain.height_above_terrain(terrain_altitude, true)) {
correction = terrain_altitude - rangefinder_state.height_estimate;
}
#endif
// remember the last correction. Use a low pass filter unless
// the old data is more than 5 seconds old
uint32_t now = millis();
if (now - rangefinder_state.last_correction_time_ms > 5000) {
rangefinder_state.correction = correction;
rangefinder_state.initial_correction = correction;
auto_state.initial_land_slope = auto_state.land_slope;
} else {
rangefinder_state.correction = 0.8f*rangefinder_state.correction + 0.2f*correction;
if (fabsf(rangefinder_state.correction - rangefinder_state.initial_correction) > 30) {
// the correction has changed by more than 30m, reset use of Lidar. We may have a bad lidar
if (rangefinder_state.in_use) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Rangefinder disengaged at %.2fm", (double)rangefinder_state.height_estimate);
}
memset(&rangefinder_state, 0, sizeof(rangefinder_state));
}
}
rangefinder_state.last_correction_time_ms = now;
}
}
/*
set the flight stage
*/
void Plane::set_flight_stage(AP_SpdHgtControl::FlightStage fs)
{
if (fs == flight_stage) {
return;
}
switch (fs) {
case AP_SpdHgtControl::FLIGHT_LAND_APPROACH:
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Landing approach start at %.1fm", (double)relative_altitude());
auto_state.land_in_progress = true;
#if GEOFENCE_ENABLED == ENABLED
if (g.fence_autoenable == 1) {
if (! geofence_set_enabled(false, AUTO_TOGGLED)) {
gcs_send_text(MAV_SEVERITY_NOTICE, "Disable fence failed (autodisable)");
} else {
gcs_send_text(MAV_SEVERITY_NOTICE, "Fence disabled (autodisable)");
}
} else if (g.fence_autoenable == 2) {
if (! geofence_set_floor_enabled(false)) {
gcs_send_text(MAV_SEVERITY_NOTICE, "Disable fence floor failed (autodisable)");
} else {
gcs_send_text(MAV_SEVERITY_NOTICE, "Fence floor disabled (auto disable)");
}
}
#endif
break;
case AP_SpdHgtControl::FLIGHT_LAND_ABORT:
gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Landing aborted, climbing to %dm", auto_state.takeoff_altitude_rel_cm/100);
auto_state.land_in_progress = false;
break;
case AP_SpdHgtControl::FLIGHT_LAND_PREFLARE:
case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
auto_state.land_in_progress = true;
break;
case AP_SpdHgtControl::FLIGHT_NORMAL:
case AP_SpdHgtControl::FLIGHT_VTOL:
case AP_SpdHgtControl::FLIGHT_TAKEOFF:
auto_state.land_in_progress = false;
break;
}
flight_stage = fs;
if (should_log(MASK_LOG_MODE)) {
Log_Write_Status();
}
}
/*
set the flight stage
*/
void Plane::set_flight_stage(AP_SpdHgtControl::FlightStage fs)
{
if (fs == flight_stage) {
return;
}
switch (fs) {
case AP_SpdHgtControl::FLIGHT_LAND_APPROACH:
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Landing approach start at %.1fm", (double)relative_altitude());
#if GEOFENCE_ENABLED == ENABLED
if (g.fence_autoenable == 1) {
if (! geofence_set_enabled(false, AUTO_TOGGLED)) {
gcs_send_text(MAV_SEVERITY_NOTICE, "Disable fence failed (autodisable)");
} else {
gcs_send_text(MAV_SEVERITY_NOTICE, "Fence disabled (autodisable)");
}
} else if (g.fence_autoenable == 2) {
if (! geofence_set_floor_enabled(false)) {
gcs_send_text(MAV_SEVERITY_NOTICE, "Disable fence floor failed (autodisable)");
} else {
gcs_send_text(MAV_SEVERITY_NOTICE, "Fence floor disabled (auto disable)");
}
}
#endif
break;
case AP_SpdHgtControl::FLIGHT_LAND_ABORT:
gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Landing aborted via throttle. Climbing to %dm", auto_state.takeoff_altitude_rel_cm/100);
break;
case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
case AP_SpdHgtControl::FLIGHT_NORMAL:
case AP_SpdHgtControl::FLIGHT_TAKEOFF:
break;
}
flight_stage = fs;
}
