diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index ba9e01b84d..59c068d41e 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -304,7 +304,7 @@ static bool waypoint_valid(Location &wp) static void get_loiter_accel(int16_t accel_req_forward, int16_t accel_req_right) { - float z_accel_meas = -AP_INTERTIALNAV_GRAVITY * 100; // gravity in cm/s/s + float z_accel_meas = -GRAVITY_MSS * 100; // gravity in cm/s/s // update angle targets that will be passed to stabilize controller auto_roll = constrain((accel_req_right/(-z_accel_meas))*(18000/M_PI), -4500, 4500); diff --git a/libraries/AP_InertialNav/AP_InertialNav.cpp b/libraries/AP_InertialNav/AP_InertialNav.cpp index 741957b2c3..af0f58af1e 100644 --- a/libraries/AP_InertialNav/AP_InertialNav.cpp +++ b/libraries/AP_InertialNav/AP_InertialNav.cpp @@ -51,7 +51,7 @@ void AP_InertialNav::update(float dt) accel_ef = _ahrs->get_accel_ef(); // remove influence of gravity - accel_ef.z += AP_INTERTIALNAV_GRAVITY; + accel_ef.z += GRAVITY_MSS; accel_ef *= 100; // remove xy if not enabled diff --git a/libraries/AP_InertialNav/AP_InertialNav.h b/libraries/AP_InertialNav/AP_InertialNav.h index 395a6f8143..ad2f06d0dc 100644 --- a/libraries/AP_InertialNav/AP_InertialNav.h +++ b/libraries/AP_InertialNav/AP_InertialNav.h @@ -8,7 +8,6 @@ #include // ArduPilot Mega Barometer Library #include // FIFO buffer library -#define AP_INTERTIALNAV_GRAVITY 9.80665f #define AP_INTERTIALNAV_TC_XY 3.0f // default time constant for complementary filter's X & Y axis #define AP_INTERTIALNAV_TC_Z 5.0f // default time constant for complementary filter's Z axis diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Stub.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Stub.cpp index 1d3fd9ba17..1c6d3e6dca 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Stub.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Stub.cpp @@ -6,7 +6,7 @@ const extern AP_HAL::HAL& hal; AP_InertialSensor_Stub::AP_InertialSensor_Stub() { Vector3f accels; - accels.z = -9.808; + accels.z = -GRAVITY_MSS; set_accel(accels); }