mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
Tracker: support SET_GPS_GLOBAL_ORIGIN message
This commit is contained in:
parent
4899401679
commit
d45eb33980
@ -434,6 +434,22 @@ void GCS_MAVLINK_Tracker::handleMessage(mavlink_message_t* msg)
|
||||
{
|
||||
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,
|
||||
// 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:
|
||||
@ -526,6 +542,10 @@ void GCS_MAVLINK_Tracker::handleMessage(mavlink_message_t* msg)
|
||||
|
||||
case MAV_CMD_GET_HOME_POSITION:
|
||||
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;
|
||||
break;
|
||||
|
||||
|
@ -250,6 +250,7 @@ private:
|
||||
bool get_home_eeprom(struct Location &loc);
|
||||
void set_home_eeprom(struct Location temp);
|
||||
void set_home(struct Location temp);
|
||||
void set_ekf_origin(const Location& loc);
|
||||
void arm_servos();
|
||||
void disarm_servos();
|
||||
void prepare_servos();
|
||||
|
@ -166,6 +166,33 @@ void Tracker::set_home(struct Location temp)
|
||||
set_home_eeprom(temp);
|
||||
current_loc = 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()
|
||||
|
Loading…
Reference in New Issue
Block a user