Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: add some safety to detect bad lidar readings 

we only accept a lidar if it changes by 5% of its full range, and we
reject a lidar again if the correction between barometric and lidar
range changes by more than 30m

This allows us to cope with faulty lidars which may give a constant
reading
This commit is contained in:
Andrew Tridgell 2015-09-16 11:14:26 +10:00
parent 36432e6515
commit 86e8c7ed2f
4 changed files with 41 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduPlane/Plane.h
View File

@ -194,8 +194,12 @@ private:
RangeFinder rangefinder {serial_manager};
struct {
bool in_range;
bool in_range:1;
bool have_initial_reading:1;
bool in_use:1;
float initial_range;
float correction;
float initial_correction;
uint32_t last_correction_time_ms;
uint8_t in_range_count;
} rangefinder_state;

31
ArduPlane/altitude.cpp
View File

@ -552,19 +552,34 @@ float Plane::rangefinder_correction(void)
*/
void Plane::rangefinder_height_update(void)
{
uint16_t distance_cm = rangefinder.distance_cm();
float distance = rangefinder.distance_cm()*0.01f;
float height_estimate = 0;
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))
height_estimate = distance_cm * 0.01f * ahrs.get_dcm_matrix().c.z;
height_estimate = distance * ahrs.get_dcm_matrix().c.z;
// we consider ourselves to be fully in range when we have 10
// good samples (0.2s)
// 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) {
rangefinder_state.in_range_count++;
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 &&
g.rangefinder_landing) {
rangefinder_state.in_use = true;
gcs_send_text_fmt(PSTR("Rangefinder engaged at %.2fm"), height_estimate);
}
}
} else {
rangefinder_state.in_range_count = 0;
@ -589,8 +604,16 @@ void Plane::rangefinder_height_update(void)
// the old data is more than 5 seconds old
if (millis() - rangefinder_state.last_correction_time_ms > 5000) {
rangefinder_state.correction = correction;
rangefinder_state.initial_correction = correction;
} 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(PSTR("Rangefinder disengaged at %.2fm"), height_estimate);
}
memset(&rangefinder_state, 0, sizeof(rangefinder_state));
}
}
rangefinder_state.last_correction_time_ms = millis();
}

5
ArduPlane/commands_logic.cpp
View File

@ -358,6 +358,11 @@ void Plane::do_land(const AP_Mission::Mission_Command& cmd)
// If no takeoff command has ever been used, default to a conservative 10deg
auto_state.takeoff_pitch_cd = 1000;
}
#if RANGEFINDER_ENABLED == ENABLED
// zero rangefinder state, start to accumulate good samples now
memset(&rangefinder_state, 0, sizeof(rangefinder_state));
#endif
}
void Plane::loiter_set_direction_wp(const AP_Mission::Mission_Command& cmd)

6
ArduPlane/landing.cpp
View File

@ -76,8 +76,10 @@ bool Plane::verify_land()
if (!is_flying() && (millis()-auto_state.last_flying_ms) > 3000) {
gcs_send_text_fmt(PSTR("Flare crash detected: speed=%.1f"), (double)gps.ground_speed());
} else {
gcs_send_text_fmt(PSTR("Flare %.1fm sink=%.2f speed=%.1f"),
(double)height, (double)auto_state.sink_rate, (double)gps.ground_speed());
gcs_send_text_fmt(PSTR("Flare %.1fm sink=%.2f speed=%.1f dist=%.1f"),
(double)height, (double)auto_state.sink_rate,
(double)gps.ground_speed(),
(double)get_distance(current_loc, next_WP_loc));
}
}
auto_state.land_complete = true;