diff --git a/ArduPlane/HIL_Xplane.pde b/ArduPlane/HIL_Xplane.pde index 4f01c19028..d87b01c344 100644 --- a/ArduPlane/HIL_Xplane.pde +++ b/ArduPlane/HIL_Xplane.pde @@ -93,8 +93,6 @@ HIL_XPLANE::send_message(uint8_t id, uint32_t param) break; case MSG_LOCATION: break; - case MSG_LOCAL_LOCATION: - break; case MSG_GPS_RAW: break; case MSG_SERVO_OUT: diff --git a/ArduPlane/Mavlink_Common.h b/ArduPlane/Mavlink_Common.h index 9923ab8f98..32ee10ec50 100644 --- a/ArduPlane/Mavlink_Common.h +++ b/ArduPlane/Mavlink_Common.h @@ -149,20 +149,6 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) crosstrack_error); } -static void NOINLINE send_local_location(mavlink_channel_t chan) -{ - Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now - mavlink_msg_local_position_send( - chan, - micros(), - ToRad((current_loc.lat - home.lat) / 1.0e7) * radius_of_earth, - ToRad((current_loc.lng - home.lng) / 1.0e7) * radius_of_earth * cos(ToRad(home.lat / 1.0e7)), - (current_loc.alt - home.alt) / 1.0e2, - g_gps->ground_speed / 1.0e2 * rot.a.x, - g_gps->ground_speed / 1.0e2 * rot.b.x, - g_gps->ground_speed / 1.0e2 * rot.c.x); -} - static void NOINLINE send_gps_raw(mavlink_channel_t chan) { mavlink_msg_gps_raw_send( @@ -348,11 +334,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ } break; - case MSG_LOCAL_LOCATION: - CHECK_PAYLOAD_SIZE(LOCAL_POSITION); - send_local_location(chan); - break; - case MSG_GPS_RAW: CHECK_PAYLOAD_SIZE(GPS_RAW); send_gps_raw(chan); diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 1f638da505..7d0aac6c2b 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -163,7 +163,6 @@ #define MSG_ATTITUDE_CORRECT 0xb1 #define MSG_POSITION_SET 0xb2 #define MSG_ATTITUDE_SET 0xb3 -#define MSG_LOCAL_LOCATION 0xb4 #define MSG_RETRY_DEFERRED 0xff #define SEVERITY_LOW 1