diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index 8e2fad2e1d..382124c85f 100644 --- a/APMrover2/APMrover2.pde +++ b/APMrover2/APMrover2.pde @@ -363,7 +363,6 @@ static int32_t gps_base_alt; //////////////////////////////////////////////////////////////////////////////// // Constants const float radius_of_earth = 6378100; // meters -const float gravity = 9.81; // meters/ sec^2 // true if we have a position estimate from AHRS diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde index 07814b3864..bd3f2fb96f 100644 --- a/APMrover2/GCS_Mavlink.pde +++ b/APMrover2/GCS_Mavlink.pde @@ -355,9 +355,9 @@ static void NOINLINE send_raw_imu1(mavlink_channel_t chan) mavlink_msg_raw_imu_send( chan, micros(), - accel.x * 1000.0 / gravity, - accel.y * 1000.0 / gravity, - accel.z * 1000.0 / gravity, + accel.x * 1000.0 / GRAVITY_MSS, + accel.y * 1000.0 / GRAVITY_MSS, + accel.z * 1000.0 / GRAVITY_MSS, gyro.x * 1000.0, gyro.y * 1000.0, gyro.z * 1000.0, @@ -1647,9 +1647,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // m/s/s Vector3f accels; - accels.x = packet.xacc * (gravity/1000.0); - accels.y = packet.yacc * (gravity/1000.0); - accels.z = packet.zacc * (gravity/1000.0); + accels.x = packet.xacc * (GRAVITY_MSS/1000.0); + accels.y = packet.yacc * (GRAVITY_MSS/1000.0); + accels.z = packet.zacc * (GRAVITY_MSS/1000.0); ins.set_gyro(gyros);