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

Plane: allow continued use of rangefinder data for 5s after loss of contact 

this allows short outages to be ridden out
This commit is contained in:
Andrew Tridgell 2014-08-27 11:50:07 +10:00
parent 5da5360dde
commit a6ee46086c
3 changed files with 57 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduPlane/ArduPlane.pde
View File

@ -307,6 +307,8 @@ static RangeFinder rangefinder;
static struct {
bool in_range;
float height_estimate;
float correction;
uint32_t last_correction_time_ms;
uint8_t in_range_count;
} rangefinder_state;

50
ArduPlane/altitude.pde
View File

@ -513,7 +513,8 @@ static float lookahead_adjustment(void)
*/
static float rangefinder_correction(void)
{
if (!rangefinder_state.in_range) {
if (hal.scheduler->millis() - rangefinder_state.last_correction_time_ms > 5000) {
// we haven't had any rangefinder data for 5s - don't use it
return 0;
}
@ -523,9 +524,46 @@ static float rangefinder_correction(void)
(flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH ||
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL));
if (!using_rangefinder) {
return false;
return 0;
}
return rangefinder_state.correction;
}
/*
update the offset between rangefinder height and terrain height
*/
static void rangefinder_height_update(void)
{
uint16_t distance_cm = rangefinder.distance_cm();
int16_t max_distance_cm = rangefinder.max_distance_cm();
if (rangefinder.healthy() && distance_cm < max_distance_cm) {
// correct the range for attitude (multiply by DCM.c.z, which
// is cos(roll)*cos(pitch))
float corrected_range = distance_cm * 0.01f * ahrs.get_dcm_matrix().c.z;
if (rangefinder_state.in_range_count == 0) {
// we've just come back into range, start with this value
rangefinder_state.height_estimate = corrected_range;
} else {
// low pass filter to reduce noise. This runs at 50Hz, so
// converges fast. We don't want too much filtering
// though, as it would introduce lag which would delay flaring
rangefinder_state.height_estimate = 0.75f * rangefinder_state.height_estimate + 0.25f * corrected_range;
}
// 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() - rangefinder_state.height_estimate;
@ -533,10 +571,14 @@ static float rangefinder_correction(void)
#if AP_TERRAIN_AVAILABLE
// if we are terrain following then correction is based on terrain data
float terrain_altitude;
if (g.terrain_follow && terrain.height_above_terrain(terrain_altitude, true)) {
if ((target_altitude.terrain_following || g.terrain_follow) &&
terrain.height_above_terrain(terrain_altitude, true)) {
correction = terrain_altitude - rangefinder_state.height_estimate;
}
#endif
return correction;
// remember the last correction
rangefinder_state.correction = correction;
rangefinder_state.last_correction_time_ms = hal.scheduler->millis();
}
}

29
ArduPlane/sensors.pde
View File

@ -20,35 +20,10 @@ static void read_rangefinder(void)
{
rangefinder.update();
uint16_t distance_cm = rangefinder.distance_cm();
int16_t max_distance_cm = rangefinder.max_distance_cm();
if (rangefinder.healthy() && distance_cm < max_distance_cm) {
// correct the range for attitude (multiply by DCM.c.z, which
// is cos(roll)*cos(pitch))
float corrected_range = distance_cm * 0.01f * ahrs.get_dcm_matrix().c.z;
if (rangefinder_state.in_range_count == 0) {
// we've just come back into range, start with this value
rangefinder_state.height_estimate = corrected_range;
} else {
// low pass filter to reduce noise. This runs at 50Hz, so
// converges fast. We don't want too much filtering
// though, as it would introduce lag which would delay flaring
rangefinder_state.height_estimate = 0.75f * rangefinder_state.height_estimate + 0.25f * corrected_range;
}
// 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 (should_log(MASK_LOG_SONAR))
Log_Write_Sonar();
rangefinder_height_update();
}
/*