Blimp: handle MAV_CMD_NAV_TAKEOFF via command_long and command_in

This commit is contained in:
Peter Barker 2023-10-18 17:30:32 +11:00 committed by Peter Barker
parent a97adcf9cd
commit 146485f7cf
2 changed files with 8 additions and 10 deletions

View File

@ -475,22 +475,20 @@ MAV_RESULT GCS_MAVLINK_Blimp::handle_command_int_packet(const mavlink_command_in
switch (packet.command) {
case MAV_CMD_DO_REPOSITION:
return handle_command_int_do_reposition(packet);
case MAV_CMD_NAV_TAKEOFF:
return MAV_RESULT_ACCEPTED;
default:
return GCS_MAVLINK::handle_command_int_packet(packet, msg);
}
}
MAV_RESULT GCS_MAVLINK_Blimp::handle_command_long_packet(const mavlink_command_long_t &packet, const mavlink_message_t &msg)
bool GCS_MAVLINK_Blimp::mav_frame_for_command_long(MAV_FRAME &frame, MAV_CMD packet_command) const
{
switch (packet.command) {
case MAV_CMD_NAV_TAKEOFF: {
return MAV_RESULT_ACCEPTED;
}
default:
return GCS_MAVLINK::handle_command_long_packet(packet, msg);
if (packet_command == MAV_CMD_NAV_TAKEOFF) {
frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
return true;
}
return GCS_MAVLINK::mav_frame_for_command_long(frame, packet_command);
}
void GCS_MAVLINK_Blimp::handleMessage(const mavlink_message_t &msg)

View File

@ -27,9 +27,9 @@ protected:
MAV_RESULT handle_command_do_set_roi(const Location &roi_loc) override;
MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override;
MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet, const mavlink_message_t &msg) override;
MAV_RESULT handle_command_int_do_reposition(const mavlink_command_int_t &packet);
bool mav_frame_for_command_long(MAV_FRAME &frame, MAV_CMD packet_command) const override;
bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED;
bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED;