Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 01:33:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: move handling of SET_GPS_GLOBAL_ORIGIN up

Browse Source
This commit is contained in:
Peter Barker 2017-09-19 12:30:32 +10:00 committed by Randy Mackay
parent b682f3f6d9
commit d0c2898ebc
4 changed files with 26 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/GCS_MAVLink/GCS.h
View File

@ -235,6 +235,7 @@ protected:
virtual AP_AdvancedFailsafe *get_advanced_failsafe() const { return nullptr; };
virtual bool set_mode(uint8_t mode) = 0;
virtual const AP_FWVersion &get_fwver() const = 0;
virtual void set_ekf_origin(const Location& loc) = 0;
bool waypoint_receiving; // currently receiving
// the following two variables are only here because of Tracker
@ -276,6 +277,7 @@ protected:
void handle_serial_control(const mavlink_message_t *msg);
void handle_common_message(mavlink_message_t *msg);
void handle_set_gps_global_origin(const mavlink_message_t *msg);
void handle_setup_signing(const mavlink_message_t *msg);
uint8_t handle_preflight_reboot(const mavlink_command_long_t &packet, bool disable_overrides);
MAV_RESULT handle_rc_bind(const mavlink_command_long_t &packet);

22
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -1796,6 +1796,24 @@ MAV_RESULT GCS_MAVLINK::handle_command_camera(const mavlink_command_long_t &pack
return result;
}
void GCS_MAVLINK::handle_set_gps_global_origin(const mavlink_message_t *msg)
{
mavlink_set_gps_global_origin_t packet;
mavlink_msg_set_gps_global_origin_decode(msg, &packet);
// sanity check location
if (!check_latlng(packet.latitude, packet.longitude)) {
// silently drop the request
return;
}
Location ekf_origin {};
ekf_origin.lat = packet.latitude;
ekf_origin.lng = packet.longitude;
ekf_origin.alt = packet.altitude / 10;
set_ekf_origin(ekf_origin);
}
/*
handle messages which don't require vehicle specific data
*/
@ -1814,6 +1832,10 @@ void GCS_MAVLINK::handle_common_message(mavlink_message_t *msg)
handle_common_param_message(msg);
break;
case MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN:
handle_set_gps_global_origin(msg);
break;
case MAVLINK_MSG_ID_DEVICE_OP_READ:
handle_device_op_read(msg);
break;

1
libraries/GCS_MAVLink/GCS_Dummy.h
View File

@ -30,6 +30,7 @@ protected:
AP_Camera *get_camera() const override { return nullptr; };
AP_ServoRelayEvents *get_servorelayevents() const override { return nullptr; }
const AP_FWVersion &get_fwver() const override { return fwver; }
void set_ekf_origin(const Location& loc) override { };
uint8_t sysid_my_gcs() const override { return 1; }
bool set_mode(uint8_t mode) override { return false; };

1
libraries/GCS_MAVLink/examples/routing/routing.cpp
View File

@ -40,6 +40,7 @@ protected:
uint8_t sysid_my_gcs() const override { return 1; }
bool set_mode(uint8_t mode) override { return false; };
const AP_FWVersion &get_fwver() const override { return fwver; }
void set_ekf_origin(const Location& loc) override { };
private: