5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-06 16:08:28 -04:00

Sub: support SET_GPS_GLOBAL_ORIGIN message

This commit is contained in:
Randy Mackay 2017-09-18 14:36:38 +09:00
parent f029303d96
commit a477ef7cd6
3 changed files with 55 additions and 1 deletions

View File

@ -891,6 +891,22 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
break;
}
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;
sub.set_ekf_origin(ekf_origin);
break;
}
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: { // MAV ID: 66
handle_request_data_stream(msg, false);
break;
@ -1167,6 +1183,10 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
case MAV_CMD_GET_HOME_POSITION:
if (sub.ap.home_state != HOME_UNSET) {
send_home(sub.ahrs.get_home());
Location ekf_origin;
if (sub.ahrs.get_origin(ekf_origin)) {
send_ekf_origin(ekf_origin);
}
result = MAV_RESULT_ACCEPTED;
}
break;

View File

@ -542,6 +542,7 @@ private:
void set_home_to_current_location_inflight();
bool set_home_to_current_location(bool lock);
bool set_home(const Location& loc, bool lock);
void set_ekf_origin(const Location& loc);
bool far_from_EKF_origin(const Location& loc);
void set_system_time_from_GPS();
void exit_mission();

View File

@ -63,6 +63,12 @@ bool Sub::set_home(const Location& loc, bool lock)
return false;
}
// check if EKF origin has been set
Location ekf_origin;
if (!ahrs.get_origin(ekf_origin)) {
return false;
}
// set ahrs home (used for RTL)
ahrs.set_home(loc);
@ -90,13 +96,40 @@ bool Sub::set_home(const Location& loc, bool lock)
// log ahrs home and ekf origin dataflash
Log_Write_Home_And_Origin();
// send new home location to GCS
// send new home and ekf origin to GCS
gcs().send_home(loc);
gcs().send_ekf_origin(loc);
// return success
return true;
}
// sets ekf_origin if it has not been set.
// should only be used when there is no GPS to provide an absolute position
void Sub::set_ekf_origin(const Location& loc)
{
// check location is valid
if (!check_latlng(loc)) {
return;
}
// check if EKF origin has already been set
Location ekf_origin;
if (ahrs.get_origin(ekf_origin)) {
return;
}
if (!ahrs.set_origin(loc)) {
return;
}
// log ahrs home and ekf origin dataflash
Log_Write_Home_And_Origin();
// send ekf origin to GCS
gcs().send_ekf_origin(loc);
}
// far_from_EKF_origin - checks if a location is too far from the EKF origin
// returns true if too far
bool Sub::far_from_EKF_origin(const Location& loc)