diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp index c080251288..35ee0f074a 100644 --- a/AntennaTracker/GCS_Mavlink.cpp +++ b/AntennaTracker/GCS_Mavlink.cpp @@ -98,28 +98,6 @@ void Tracker::send_extended_status1(mavlink_channel_t chan) 0, 0, 0, 0); } -void Tracker::send_location(mavlink_channel_t chan) -{ - uint32_t fix_time; - if (gps.status() >= AP_GPS::GPS_OK_FIX_2D) { - fix_time = gps.last_fix_time_ms(); - } else { - fix_time = AP_HAL::millis(); - } - const Vector3f &vel = gps.velocity(); - mavlink_msg_global_position_int_send( - chan, - fix_time, - current_loc.lat, // in 1E7 degrees - current_loc.lng, // in 1E7 degrees - current_loc.alt * 10, // millimeters above sea level - 0, - vel.x * 100, // X speed cm/s (+ve North) - vel.y * 100, // Y speed cm/s (+ve East) - vel.z * 100, // Z speed cm/s (+ve Down) - ahrs.yaw_sensor); -} - void Tracker::send_nav_controller_output(mavlink_channel_t chan) { float alt_diff = (g.alt_source == ALT_SOURCE_BARO) ? nav_status.alt_difference_baro : nav_status.alt_difference_gps; @@ -161,11 +139,6 @@ bool GCS_MAVLINK_Tracker::try_send_message(enum ap_message id) { switch (id) { - case MSG_LOCATION: - CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT); - tracker.send_location(chan); - break; - case MSG_NAV_CONTROLLER_OUTPUT: CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT); tracker.send_nav_controller_output(chan); diff --git a/AntennaTracker/GCS_Mavlink.h b/AntennaTracker/GCS_Mavlink.h index b0471b1028..39a10727f1 100644 --- a/AntennaTracker/GCS_Mavlink.h +++ b/AntennaTracker/GCS_Mavlink.h @@ -26,6 +26,10 @@ protected: MAV_RESULT _handle_command_preflight_calibration_baro() override; + int32_t global_position_int_relative_alt() const { + return 0; // what if we have been picked up and carried somewhere? + } + private: void packetReceived(const mavlink_status_t &status, mavlink_message_t &msg) override; diff --git a/AntennaTracker/Tracker.h b/AntennaTracker/Tracker.h index ebe76161cf..4641179078 100644 --- a/AntennaTracker/Tracker.h +++ b/AntennaTracker/Tracker.h @@ -200,7 +200,6 @@ private: void one_second_loop(); void ten_hz_logging_loop(); void send_extended_status1(mavlink_channel_t chan); - void send_location(mavlink_channel_t chan); void send_nav_controller_output(mavlink_channel_t chan); void send_simstate(mavlink_channel_t chan); void gcs_data_stream_send(void);