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;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Location & Navigation // Location & Navigation
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Constants
const float radius_of_earth = 6378100; // meters
// Direction held during phases of takeoff and landing // 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 // 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)
case MAV_FRAME_LOCAL_NED: // local (relative to home position) case MAV_FRAME_LOCAL_NED: // local (relative to home position)
{ {
tell_command.lat = 1.0e7f*ToDeg(packet.x/ tell_command.lat = 1.0e7f*ToDeg(packet.x/
(radius_of_earth*cosf(ToRad(home.lat/1.0e7f)))) + home.lat; (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.lng = 1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng;
tell_command.alt = -packet.z*1.0e2f; tell_command.alt = -packet.z*1.0e2f;
tell_command.options = MASK_OPTIONS_RELATIVE_ALT; tell_command.options = MASK_OPTIONS_RELATIVE_ALT;
break; break;
@ -1558,8 +1558,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAV_FRAME_LOCAL: // local (relative to home position) case MAV_FRAME_LOCAL: // local (relative to home position)
{ {
tell_command.lat = 1.0e7f*ToDeg(packet.x/ tell_command.lat = 1.0e7f*ToDeg(packet.x/
(radius_of_earth*cosf(ToRad(home.lat/1.0e7f)))) + home.lat; (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.lng = 1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng;
tell_command.alt = packet.z*1.0e2f; tell_command.alt = packet.z*1.0e2f;
tell_command.options = MASK_OPTIONS_RELATIVE_ALT; tell_command.options = MASK_OPTIONS_RELATIVE_ALT;
break; break;

Loading…
Cancel
Save