ACM : revert back to the 2.6 climb rate calc

The current filter is outputting wild oscillations.
This commit is contained in:
Jason Short 2012-08-28 21:19:59 -07:00
parent 314ce65971
commit 91bc3705c1
1 changed files with 12 additions and 11 deletions

View File

@ -2134,9 +2134,9 @@ static void update_trig(void){
static void update_altitude()
{
static int16_t old_sonar_alt = 0;
static int32_t old_baro_alt = 0;
#if HIL_MODE == HIL_MODE_ATTITUDE
static int32_t old_baro_alt = 0;
// we are in the SIM, fake out the baro and Sonar
int16_t fake_relative_alt = g_gps->altitude - gps_base_alt;
baro_alt = fake_relative_alt;
@ -2156,18 +2156,19 @@ static void update_altitude()
baro_alt = read_barometer();
// calc the vertical accel rate
/*
* // 2.6 way
* int16_t temp = (baro_alt - old_baro_alt) * 10;
* baro_rate = (temp + baro_rate) >> 1;
* baro_rate = constrain(baro_rate, -300, 300);
* old_baro_alt = baro_alt;
*/
// 2.6 way
int16_t temp = (baro_alt - old_baro_alt) * 10;
baro_rate = (temp + baro_rate) >> 1;
baro_rate = constrain(baro_rate, -300, 300);
old_baro_alt = baro_alt;
// Using Tridge's new clamb rate calc
/*
int16_t temp = barometer.get_climb_rate() * 100;
baro_rate = (temp + baro_rate) >> 1;
baro_rate = constrain(baro_rate, -300, 300);
*/
#endif
// Note: sonar_alt is calculated in a faster loop and filtered with a mode filter