示例#1
0
/*
  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
}
示例#2
0
/*
  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();
}
示例#3
0
/*
  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();
}
示例#4
0
/*
  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();    
    }
}
示例#5
0
/*
  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;
}
示例#6
0
/*
  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);
}
示例#7
0
/*
  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;
}
示例#8
0
/*
  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;
    }
}
示例#9
0
/*
  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();
    }
}
示例#10
0
/*
  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;
}