Browse Source

Plane: use RADIUS_OF_EARTH define

master
Andrew Tridgell 12 years ago
parent
commit
82cd391cf8
  1. 2
      ArduPlane/ArduPlane.pde
  2. 8
      ArduPlane/GCS_Mavlink.pde

2
ArduPlane/ArduPlane.pde

@ -362,8 +362,6 @@ static bool have_position; @@ -362,8 +362,6 @@ static bool have_position;
////////////////////////////////////////////////////////////////////////////////
// Location & Navigation
////////////////////////////////////////////////////////////////////////////////
// Constants
const float radius_of_earth = 6378100; // meters
// Direction held during phases of takeoff and landing
// deg * 100 dir of plane, A value of -1 indicates the course has not been set/is not in use

8
ArduPlane/GCS_Mavlink.pde

@ -1546,8 +1546,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) @@ -1546,8 +1546,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAV_FRAME_LOCAL_NED: // local (relative to home position)
{
tell_command.lat = 1.0e7f*ToDeg(packet.x/
(radius_of_earth*cosf(ToRad(home.lat/1.0e7f)))) + home.lat;
tell_command.lng = 1.0e7f*ToDeg(packet.y/radius_of_earth) + home.lng;
(RADIUS_OF_EARTH*cosf(ToRad(home.lat/1.0e7f)))) + home.lat;
tell_command.lng = 1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng;
tell_command.alt = -packet.z*1.0e2f;
tell_command.options = MASK_OPTIONS_RELATIVE_ALT;
break;
@ -1558,8 +1558,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) @@ -1558,8 +1558,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAV_FRAME_LOCAL: // local (relative to home position)
{
tell_command.lat = 1.0e7f*ToDeg(packet.x/
(radius_of_earth*cosf(ToRad(home.lat/1.0e7f)))) + home.lat;
tell_command.lng = 1.0e7f*ToDeg(packet.y/radius_of_earth) + home.lng;
(RADIUS_OF_EARTH*cosf(ToRad(home.lat/1.0e7f)))) + home.lat;
tell_command.lng = 1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng;
tell_command.alt = packet.z*1.0e2f;
tell_command.options = MASK_OPTIONS_RELATIVE_ALT;
break;

Loading…
Cancel
Save