From 5840ded76707707a9031774731bc691bbf532929 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 7 Dec 2012 12:33:50 +1100 Subject: [PATCH] AHRS: removed constrain() defines --- libraries/AP_AHRS/AP_AHRS.cpp | 1 - libraries/AP_AHRS/AP_AHRS_DCM.cpp | 2 -- libraries/AP_AHRS/AP_AHRS_MPU6000.cpp | 1 - 3 files changed, 4 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index b5bb812870..0fe562499d 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -87,7 +87,6 @@ bool AP_AHRS::airspeed_estimate(float *airspeed_ret) if (_wind_max > 0 && _gps && _gps->status() == GPS::GPS_OK) { // constrain the airspeed by the ground speed // and AHRS_WIND_MAX -#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) *airspeed_ret = constrain(*airspeed_ret, _gps->ground_speed*0.01 - _wind_max, _gps->ground_speed*0.01 + _wind_max); diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 6dde295825..22899885c7 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -18,8 +18,6 @@ extern const AP_HAL::HAL& hal; -#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) - // this is the speed in cm/s above which we first get a yaw lock with // the GPS #define GPS_SPEED_MIN 300 diff --git a/libraries/AP_AHRS/AP_AHRS_MPU6000.cpp b/libraries/AP_AHRS/AP_AHRS_MPU6000.cpp index cf6bc6eb97..3b7202d759 100644 --- a/libraries/AP_AHRS/AP_AHRS_MPU6000.cpp +++ b/libraries/AP_AHRS/AP_AHRS_MPU6000.cpp @@ -122,7 +122,6 @@ void AP_AHRS_MPU6000::drift_correction( float deltat ) // 0.65*0.04 / 0.005 = 5.2 float drift_limit = _mpu6000->get_gyro_drift_rate() * deltat / _gyro_bias_from_gravity_gain; -#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) errorRollPitch[0] = constrain(errorRollPitch[0], -drift_limit, drift_limit); errorRollPitch[1] = constrain(errorRollPitch[1],