mirror of https://github.com/ArduPilot/ardupilot
Sub: move handling of command-int MAV_CMD_DO_SET_HOME up
This commit is contained in:
parent
bf5eae3f72
commit
44b7763caf
|
@ -620,55 +620,11 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_do_set_roi(const Location &roi_loc)
|
||||||
return MAV_RESULT_ACCEPTED;
|
return MAV_RESULT_ACCEPTED;
|
||||||
}
|
}
|
||||||
|
|
||||||
MAV_RESULT GCS_MAVLINK_Sub::handle_command_int_packet(const mavlink_command_int_t &packet)
|
bool GCS_MAVLINK_Sub::set_home_to_current_location(bool lock) {
|
||||||
{
|
return sub.set_home_to_current_location(lock);
|
||||||
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(const Location& loc, bool lock) {
|
||||||
|
return sub.set_home(loc, lock);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -22,7 +22,6 @@ protected:
|
||||||
MAV_RESULT handle_command_do_set_roi(const Location &roi_loc) override;
|
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_baro() override;
|
||||||
MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_long_t &packet) 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;
|
MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet) override;
|
||||||
|
|
||||||
// override sending of scaled_pressure3 to send on-board temperature:
|
// override sending of scaled_pressure3 to send on-board temperature:
|
||||||
|
@ -33,6 +32,9 @@ protected:
|
||||||
|
|
||||||
bool vehicle_initialised() const override;
|
bool vehicle_initialised() const override;
|
||||||
|
|
||||||
|
bool set_home_to_current_location(bool lock) override;
|
||||||
|
bool set_home(const Location& loc, bool lock) override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
void handleMessage(mavlink_message_t * msg) override;
|
void handleMessage(mavlink_message_t * msg) override;
|
||||||
|
|
Loading…
Reference in New Issue