Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: support SET_GPS_GLOBAL_ORIGIN message

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

20
ArduPlane/GCS_Mavlink.cpp
View File

@ -895,6 +895,22 @@ void GCS_MAVLINK_Plane::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;
plane.set_ekf_origin(ekf_origin);
break;
}
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
{
handle_request_data_stream(msg, true);
@ -1185,6 +1201,10 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
case MAV_CMD_GET_HOME_POSITION:
if (plane.home_is_set != HOME_UNSET) {
send_home(plane.ahrs.get_home());
Location ekf_origin;
if (plane.ahrs.get_origin(ekf_origin)) {
send_ekf_origin(ekf_origin);
}
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;

1
ArduPlane/Plane.h
View File

@ -885,6 +885,7 @@ private:
void set_guided_WP(void);
void init_home();
void update_home();
void set_ekf_origin(const Location& loc);
void do_RTL(int32_t alt);
bool verify_takeoff();
bool verify_loiter_unlim();

26
ArduPlane/commands.cpp
View File

@ -145,3 +145,29 @@ void Plane::update_home()
}
barometer.update_calibration();
}
// sets ekf_origin if it has not been set.
// should only be used when there is no GPS to provide an absolute position
void Plane::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);
}

5
ArduPlane/commands_logic.cpp
View File

@ -906,6 +906,11 @@ void Plane::do_set_home(const AP_Mission::Mission_Command& cmd)
home_is_set = HOME_SET_NOT_LOCKED;
Log_Write_Home_And_Origin();
gcs().send_home(cmd.content.location);
// send ekf origin if set
Location ekf_origin;
if (ahrs.get_origin(ekf_origin)) {
gcs().send_ekf_origin(ekf_origin);
}
}
}