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

Copter: move handling of command-int MAV_CMD_DO_SET_HOME up

Browse Source
This commit is contained in:
Peter Barker 2018-07-06 10:56:43 +10:00 committed by Peter Barker
parent d8847a6192
commit 270fa2dfb9
2 changed files with 10 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
ArduCopter/GCS_Mavlink.cpp
View File

@ -559,6 +559,13 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_do_set_roi(const Location &roi_loc
return MAV_RESULT_ACCEPTED;
}
bool GCS_MAVLINK_Copter::set_home_to_current_location(bool lock) {
return copter.set_home_to_current_location(lock);
}
bool GCS_MAVLINK_Copter::set_home(const Location& loc, bool lock) {
return copter.set_home(loc, lock);
}
MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_int_t &packet)
{
switch(packet.command) {
@ -572,48 +579,6 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_i
#endif
return MAV_RESULT_UNSUPPORTED;
case MAV_CMD_DO_SET_HOME: {
// assume failure
if (is_equal(packet.param1, 1.0f)) {
// if param1 is 1, use current location
if (!copter.set_home_to_current_location(true)) {
return MAV_RESULT_FAILED;
}
return MAV_RESULT_ACCEPTED;
}
// 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_UNSUPPORTED;
}
// 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 += copter.ahrs.get_home().alt;
}
if (!copter.set_home(new_home_loc, true)) {
return MAV_RESULT_FAILED;
}
return MAV_RESULT_ACCEPTED;
}
default:
return GCS_MAVLINK::handle_command_int_packet(packet);
}

3
ArduCopter/GCS_Mavlink.h
View File

@ -38,6 +38,9 @@ protected:
void handle_mount_message(const mavlink_message_t* msg) 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;