diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 52bfe9fd3e..7eb3830ddd 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -286,8 +286,7 @@ AP_InertialSensor::_init_accel(void (*flash_leds_cb)(bool on)) } // null gravity from the Z accel - // TO-DO: replace with gravity #define form location.cpp - accel_offset.z += GRAVITY; + accel_offset.z += GRAVITY_MSS; total_change = fabsf(prev.x - accel_offset.x) + fabsf(prev.y - accel_offset.y) + fabsf(prev.z - accel_offset.z); max_offset = (accel_offset.x > accel_offset.y) ? accel_offset.x : accel_offset.y; @@ -422,7 +421,7 @@ bool AP_InertialSensor::_calibrate_accel( Vector3f accel_sample[6], // reset beta[0] = beta[1] = beta[2] = 0; - beta[3] = beta[4] = beta[5] = 1.0f/GRAVITY; + beta[3] = beta[4] = beta[5] = 1.0f/GRAVITY_MSS; while( num_iterations < 20 && change > eps ) { num_iterations++; @@ -452,9 +451,9 @@ bool AP_InertialSensor::_calibrate_accel( Vector3f accel_sample[6], } // copy results out - accel_scale.x = beta[3] * GRAVITY; - accel_scale.y = beta[4] * GRAVITY; - accel_scale.z = beta[5] * GRAVITY; + accel_scale.x = beta[3] * GRAVITY_MSS; + accel_scale.y = beta[4] * GRAVITY_MSS; + accel_scale.z = beta[5] * GRAVITY_MSS; accel_offsets.x = beta[0] * accel_scale.x; accel_offsets.y = beta[1] * accel_scale.y; accel_offsets.z = beta[2] * accel_scale.z; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 96caf92daa..b6d6caaf1d 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -3,7 +3,6 @@ #ifndef __AP_INERTIAL_SENSOR_H__ #define __AP_INERTIAL_SENSOR_H__ -#define GRAVITY 9.80665f // Gyro and Accelerometer calibration criteria #define AP_INERTIAL_SENSOR_ACCEL_TOT_MAX_OFFSET_CHANGE 4.0f #define AP_INERTIAL_SENSOR_ACCEL_MAX_OFFSET 250.0f diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index 15e820a20a..09669b830c 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp @@ -6,7 +6,7 @@ extern const AP_HAL::HAL& hal; // MPU6000 accelerometer scaling -#define MPU6000_ACCEL_SCALE_1G (GRAVITY / 4096.0f) +#define MPU6000_ACCEL_SCALE_1G (GRAVITY_MSS / 4096.0f) // MPU 6000 registers #define MPUREG_XG_OFFS_TC 0x00 diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp index 26adf2b945..1749ec1257 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp @@ -24,7 +24,7 @@ const float AP_InertialSensor_Oilpan::_adc_constraint = 900; // Tested value : 418 // Oilpan accelerometer scaling & offsets -#define OILPAN_ACCEL_SCALE_1G (GRAVITY * 2.0f / (2465.0f - 1617.0f)) +#define OILPAN_ACCEL_SCALE_1G (GRAVITY_MSS * 2.0f / (2465.0f - 1617.0f)) #define OILPAN_RAW_ACCEL_OFFSET ((2465.0f + 1617.0f) * 0.5f) #define OILPAN_RAW_GYRO_OFFSET 1658.0f diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h index 98937819e0..f6678c94b4 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h @@ -39,8 +39,6 @@ private: static const int8_t _sensor_signs[6]; static const uint8_t _gyro_temp_ch; - static const float _gravity; - static const float _gyro_gain_x; static const float _gyro_gain_y; static const float _gyro_gain_z;