aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

GCS_MAVLink: handle setting of ekf origin in GCS_MAVLink

Browse Source
This commit is contained in:
Peter Barker 2018-05-16 13:38:15 +10:00 committed by Andrew Tridgell
parent 9a73f0c260
commit fc1775df75
4 changed files with 29 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/GCS_MAVLink/GCS.h
View File

@ -252,7 +252,7 @@ protected:
virtual AP_VisualOdom *get_visual_odom() 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;
void set_ekf_origin(const Location& loc);
virtual MAV_TYPE frame_type() const = 0;
virtual MAV_MODE base_mode() const = 0;

28
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -1877,6 +1877,34 @@ MAV_RESULT GCS_MAVLINK::handle_command_camera(const mavlink_command_long_t &pack
return result;
}
// sets ekf_origin if it has not been set.
// should only be used when there is no GPS to provide an absolute position
void GCS_MAVLINK::set_ekf_origin(const Location& loc)
{
// check location is valid
if (!check_latlng(loc)) {
return;
}
AP_AHRS &ahrs = AP::ahrs();
// 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
ahrs.Log_Write_Home_And_Origin();
// send ekf origin to GCS
send_ekf_origin(loc);
}
void GCS_MAVLINK::handle_set_gps_global_origin(const mavlink_message_t *msg)
{
mavlink_set_gps_global_origin_t packet;

1
libraries/GCS_MAVLink/GCS_Dummy.h
View File

@ -27,7 +27,6 @@ protected:
AP_Rally *get_rally() const override { return nullptr; };
AP_Camera *get_camera() 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

@ -36,7 +36,6 @@ 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 { };
// dummy information:
MAV_TYPE frame_type() const override { return MAV_TYPE_FIXED_WING; }