diff --git a/sw/airborne/airspeed.c b/sw/airborne/airspeed.c index 6e9d012e3b..bb1b28d5f6 100644 --- a/sw/airborne/airspeed.c +++ b/sw/airborne/airspeed.c @@ -10,10 +10,8 @@ #include "airspeed_ets.h" #endif -#ifdef USE_AIRSPEED +#if defined USE_AIRSPEED || defined MEASURE_AIRSPEED uint16_t adc_airspeed_val; -#else -#error "You compiled the airspeed.c file but did not USE_AIRSPEED, which is needed in other *.c files" #endif #ifdef ADC_CHANNEL_AIRSPEED @@ -42,8 +40,15 @@ void airspeed_init( void ) { void airspeed_update( void ) { #ifndef SITL #ifdef ADC_CHANNEL_AIRSPEED - adc_airspeed_val = (buf_airspeed.sum / buf_airspeed.av_nb_sample) - AIRSPEED_ZERO; - float airspeed = AIRSPEED_SCALE * adc_airspeed_val; + adc_airspeed_val = buf_airspeed.sum / buf_airspeed.av_nb_sample; +#ifdef AIRSPEED_QUADRATIC_SCALE + float airspeed = (adc_airspeed_val - AIRSPEED_BIAS); + if (airspeed <= 0.0f) + airspeed = 0.0f; + airspeed = sqrt(airspeed) * AIRSPEED_QUADRATIC_SCALE; +#else + float airspeed = AIRSPEED_SCALE * (adc_airspeed_val - AIRSPEED_BIAS); +#endif EstimatorSetAirspeed(airspeed); #elif defined(USE_AIRSPEED_ETS) EstimatorSetAirspeed(airspeed_ets); diff --git a/sw/airborne/airspeed.h b/sw/airborne/airspeed.h index 2cfa31ae4f..9f32a13282 100644 --- a/sw/airborne/airspeed.h +++ b/sw/airborne/airspeed.h @@ -3,7 +3,7 @@ #include -#ifdef USE_AIRSPEED +#if defined USE_AIRSPEED || defined MEASURE_AIRSPEED extern uint16_t adc_airspeed_val; #endif diff --git a/sw/airborne/estimator.h b/sw/airborne/estimator.h index cadb3042f7..a906a7a2e7 100644 --- a/sw/airborne/estimator.h +++ b/sw/airborne/estimator.h @@ -120,7 +120,7 @@ extern void alt_kalman( float ); #endif -#ifdef USE_AIRSPEED +#if defined USE_AIRSPEED || defined MEASURE_AIRSPEED #define EstimatorSetAirspeed(airspeed) { estimator_airspeed = airspeed; } #endif