diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 4e65aec2c1..b1b5fded3a 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013 - 2017 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -88,6 +88,7 @@ VtolAttitudeControl::VtolAttitudeControl() : _vtol_vehicle_status_pub(nullptr), _v_rates_sp_pub(nullptr), _v_att_sp_pub(nullptr), + _v_cmd_ack_pub(nullptr), _transition_command(vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC), _abort_front_transition(false) @@ -483,6 +484,31 @@ VtolAttitudeControl::handle_command() // update transition command if necessary if (_vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION) { _transition_command = int(_vehicle_cmd.param1 + 0.5f); + + // Report that we have received the command no matter what we actually do with it. + // This might not be optimal but is better than no response at all. + + if (_vehicle_cmd.from_external) { + vehicle_command_ack_s command_ack = { + .timestamp = hrt_absolute_time(), + .result_param2 = 0, + .command = _vehicle_cmd.command, + .result = (uint8_t)vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED, + .from_external = false, + .result_param1 = 0, + .target_system = _vehicle_cmd.source_system, + .target_component = _vehicle_cmd.source_component + }; + + if (_v_cmd_ack_pub == nullptr) { + _v_cmd_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, + vehicle_command_ack_s::ORB_QUEUE_LENGTH); + + } else { + orb_publish(ORB_ID(vehicle_command_ack), _v_cmd_ack_pub, &command_ack); + + } + } } } diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 8a34aca282..80c16eeb02 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -72,6 +72,7 @@ #include #include #include +#include #include #include #include @@ -159,6 +160,7 @@ private: orb_advert_t _vtol_vehicle_status_pub; orb_advert_t _v_rates_sp_pub; orb_advert_t _v_att_sp_pub; + orb_advert_t _v_cmd_ack_pub; //*******************data containers*********************************************************** struct vehicle_attitude_s _v_att; //vehicle attitude