aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: lowpass the rangefinder correction, not height

Browse Source 
this should produce less lag as the rangefinder correction should be
changing much less
This commit is contained in:
Andrew Tridgell 2014-08-27 18:25:17 +10:00
parent d6aa868cac
commit 186806c768
3 changed files with 16 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduPlane/ArduPlane.pde
View File

@ -306,7 +306,6 @@ static RangeFinder rangefinder;
static struct {
bool in_range;
float height_estimate;
float correction;
uint32_t last_correction_time_ms;
uint8_t in_range_count;

6
ArduPlane/Log.pde
View File

@ -354,7 +354,7 @@ struct PACKED log_Sonar {
float groundspeed;
uint8_t throttle;
uint8_t count;
float height_estimate;
float correction;
};
// Write a sonar packet
@ -369,7 +369,7 @@ static void Log_Write_Sonar()
groundspeed : gps.ground_speed(),
throttle : (uint8_t)(100 * channel_throttle->norm_output()),
count : rangefinder_state.in_range_count,
height_estimate : rangefinder_state.height_estimate
correction : rangefinder_state.correction
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
@ -540,7 +540,7 @@ static const struct LogStructure log_structure[] PROGMEM = {
{ LOG_NTUN_MSG, sizeof(log_Nav_Tuning),
"NTUN", "ICIccccfI", "TimeMS,Yaw,WpDist,TargBrg,NavBrg,AltErr,Arspd,Alt,GSpdCM" },
{ LOG_SONAR_MSG, sizeof(log_Sonar),
"SONR", "IffffBBf", "TimeMS,DistCM,Volt,BaroAlt,GSpd,Thr,Cnt,HEst" },
"SONR", "IffffBBf", "TimeMS,DistCM,Volt,BaroAlt,GSpd,Thr,Cnt,Corr" },
{ LOG_MODE_MSG, sizeof(log_Mode),
"MODE", "IMB", "TimeMS,Mode,ModeNum" },
{ LOG_CURRENT_MSG, sizeof(log_Current),

28
ArduPlane/altitude.pde
View File

@ -526,19 +526,12 @@ 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) {
float height_estimate = 0;
if (rangefinder.healthy() && distance_cm < max_distance_cm && home_is_set) {
// 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;
}
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) {
@ -554,19 +547,24 @@ static void rangefinder_height_update(void)
if (rangefinder_state.in_range) {
// base correction is the difference between baro altitude and
// rangefinder estimate
float correction = relative_altitude() - rangefinder_state.height_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 - rangefinder_state.height_estimate;
correction = terrain_altitude - height_estimate;
}
#endif
// remember the last correction
rangefinder_state.correction = correction;
// remember the last correction. Use a low pass filter unless
// the old data is more than 5 seconds old
if (hal.scheduler->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 = hal.scheduler->millis();
}
}