Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Tracker: support SET_GPS_GLOBAL_ORIGIN message

Browse Source
This commit is contained in:
Randy Mackay 2017-09-18 14:38:26 +09:00
parent 4899401679
commit d45eb33980
3 changed files with 48 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
AntennaTracker/GCS_Mavlink.cpp
View File

@ -434,6 +434,22 @@ void GCS_MAVLINK_Tracker::handleMessage(mavlink_message_t* msg)
{ {
switch (msg->msgid) { switch (msg->msgid) {
case MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN:
{
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)) {
break;
}
Location ekf_origin {};
ekf_origin.lat = packet.latitude;
ekf_origin.lng = packet.longitude;
ekf_origin.alt = packet.altitude / 10;
tracker.set_ekf_origin(ekf_origin);
break;
}
// If we are currently operating as a proxy for a remote, // If we are currently operating as a proxy for a remote,
// alas we have to look inside each packet to see if it's for us or for the remote // alas we have to look inside each packet to see if it's for us or for the remote
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
@ -526,6 +542,10 @@ void GCS_MAVLINK_Tracker::handleMessage(mavlink_message_t* msg)
case MAV_CMD_GET_HOME_POSITION: case MAV_CMD_GET_HOME_POSITION:
send_home(tracker.ahrs.get_home()); send_home(tracker.ahrs.get_home());
Location ekf_origin;
if (tracker.ahrs.get_origin(ekf_origin)) {
send_ekf_origin(ekf_origin);
}
result = MAV_RESULT_ACCEPTED; result = MAV_RESULT_ACCEPTED;
break; break;

1
AntennaTracker/Tracker.h
View File

@ -250,6 +250,7 @@ private:
bool get_home_eeprom(struct Location &loc); bool get_home_eeprom(struct Location &loc);
void set_home_eeprom(struct Location temp); void set_home_eeprom(struct Location temp);
void set_home(struct Location temp); void set_home(struct Location temp);
void set_ekf_origin(const Location& loc);
void arm_servos(); void arm_servos();
void disarm_servos(); void disarm_servos();
void prepare_servos(); void prepare_servos();

27
AntennaTracker/system.cpp
View File

@ -166,6 +166,33 @@ void Tracker::set_home(struct Location temp)
set_home_eeprom(temp); set_home_eeprom(temp);
current_loc = temp; current_loc = temp;
gcs().send_home(temp); gcs().send_home(temp);
Location ekf_origin;
if (ahrs.get_origin(ekf_origin)) {
gcs().send_ekf_origin(ekf_origin);
}
}
// sets ekf_origin if it has not been set.
// should only be used when there is no GPS to provide an absolute position
void Tracker::set_ekf_origin(const Location& loc)
{
// check location is valid
if (!check_latlng(loc)) {
return;
}
// check EKF origin has already been set
Location ekf_origin;
if (ahrs.get_origin(ekf_origin)) {
return;
}
if (!ahrs.set_origin(loc)) {
return;
}
// send ekf origin to GCS
gcs().send_ekf_origin(loc);
} }
void Tracker::arm_servos() void Tracker::arm_servos()