/*
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
}
/*
read the rangefinder and update height estimate
*/
void Plane::read_rangefinder(void)
{
#if RANGEFINDER_ENABLED == ENABLED
rangefinder.update();
if (should_log(MASK_LOG_SONAR))
Log_Write_Sonar();
rangefinder_height_update();
#endif
}
// read the sonars
void Rover::read_sonars(void)
{
sonar.update();
if (sonar.status() == RangeFinder::RangeFinder_NotConnected) {
// this makes it possible to disable sonar at runtime
return;
}
if (sonar.has_data(1)) {
// we have two sonars
obstacle.sonar1_distance_cm = sonar.distance_cm(0);
obstacle.sonar2_distance_cm = sonar.distance_cm(1);
if (obstacle.sonar1_distance_cm <= (uint16_t)g.sonar_trigger_cm &&
obstacle.sonar1_distance_cm <= (uint16_t)obstacle.sonar2_distance_cm) {
// we have an object on the left
if (obstacle.detected_count < 127) {
obstacle.detected_count++;
}
if (obstacle.detected_count == g.sonar_debounce) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Sonar1 obstacle %u cm",
(unsigned)obstacle.sonar1_distance_cm);
}
obstacle.detected_time_ms = hal.scheduler->millis();
obstacle.turn_angle = g.sonar_turn_angle;
} else if (obstacle.sonar2_distance_cm <= (uint16_t)g.sonar_trigger_cm) {
// we have an object on the right
if (obstacle.detected_count < 127) {
obstacle.detected_count++;
}
if (obstacle.detected_count == g.sonar_debounce) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Sonar2 obstacle %u cm",
(unsigned)obstacle.sonar2_distance_cm);
}
obstacle.detected_time_ms = hal.scheduler->millis();
obstacle.turn_angle = -g.sonar_turn_angle;
}
} else {
// we have a single sonar
obstacle.sonar1_distance_cm = sonar.distance_cm(0);
obstacle.sonar2_distance_cm = 0;
if (obstacle.sonar1_distance_cm <= (uint16_t)g.sonar_trigger_cm) {
// obstacle detected in front
if (obstacle.detected_count < 127) {
obstacle.detected_count++;
}
if (obstacle.detected_count == g.sonar_debounce) {
gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Sonar obstacle %u cm",
(unsigned)obstacle.sonar1_distance_cm);
}
obstacle.detected_time_ms = hal.scheduler->millis();
obstacle.turn_angle = g.sonar_turn_angle;
}
}
Log_Write_Sonar();
// no object detected - reset after the turn time
if (obstacle.detected_count >= g.sonar_debounce &&
hal.scheduler->millis() > obstacle.detected_time_ms + g.sonar_turn_time*1000) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Obstacle passed");
obstacle.detected_count = 0;
obstacle.turn_angle = 0;
}
}
