Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Sub: move handling of command-int MAV_CMD_DO_SET_HOME up

This commit is contained in:
Peter Barker 2018-07-06 13:30:54 +10:00 committed by Peter Barker
parent bf5eae3f72
commit 44b7763caf
2 changed files with 8 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
ArduSub/GCS_Mavlink.cpp
View File

@ -620,55 +620,11 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_do_set_roi(const Location &roi_loc)
return MAV_RESULT_ACCEPTED;
}
MAV_RESULT GCS_MAVLINK_Sub::handle_command_int_packet(const mavlink_command_int_t &packet)
{
switch (packet.command) {
case MAV_CMD_DO_SET_HOME: {
// assume failure
if (is_equal(packet.param1, 1.0f)) {
// if param1 is 1, use current location
if (sub.set_home_to_current_location(true)) {
return MAV_RESULT_ACCEPTED;
}
return MAV_RESULT_FAILED;
}
// ensure param1 is zero
if (!is_zero(packet.param1)) {
return MAV_RESULT_FAILED;
}
// check frame type is supported
if (packet.frame != MAV_FRAME_GLOBAL &&
packet.frame != MAV_FRAME_GLOBAL_INT &&
packet.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT &&
packet.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) {
return MAV_RESULT_FAILED;
}
// sanity check location
if (!check_latlng(packet.x, packet.y)) {
return MAV_RESULT_FAILED;
}
Location new_home_loc {};
new_home_loc.lat = packet.x;
new_home_loc.lng = packet.y;
new_home_loc.alt = packet.z * 100;
// handle relative altitude
if (packet.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT || packet.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) {
if (!AP::ahrs().home_is_set()) {
// cannot use relative altitude if home is not set
return MAV_RESULT_FAILED;
}
new_home_loc.alt += sub.ahrs.get_home().alt;
}
if (sub.set_home(new_home_loc, true)) {
return MAV_RESULT_ACCEPTED;
}
return MAV_RESULT_FAILED;
}
default:
return GCS_MAVLINK::handle_command_int_packet(packet);
bool GCS_MAVLINK_Sub::set_home_to_current_location(bool lock) {
return sub.set_home_to_current_location(lock);
}
bool GCS_MAVLINK_Sub::set_home(const Location& loc, bool lock) {
return sub.set_home(loc, lock);
}

4
ArduSub/GCS_Mavlink.h
View File

@ -22,7 +22,6 @@ protected:
MAV_RESULT handle_command_do_set_roi(const Location &roi_loc) override;
MAV_RESULT _handle_command_preflight_calibration_baro() override;
MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_long_t &packet) override;
MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet) override;
MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet) override;
// override sending of scaled_pressure3 to send on-board temperature:
@ -33,6 +32,9 @@ protected:
bool vehicle_initialised() const override;
bool set_home_to_current_location(bool lock) override;
bool set_home(const Location& loc, bool lock) override;
private:
void handleMessage(mavlink_message_t * msg) override;