From d1dfa26903586577a1be209c4711c78df1bc5cac Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 24 Jan 2021 13:42:35 -0500 Subject: [PATCH] consume all available queued vehicle_commands --- src/modules/commander/Commander.cpp | 2 +- .../flight_mode_manager/FlightModeManager.cpp | 59 ++++---- .../FixedwingPositionControl.cpp | 30 ++-- .../FixedwingPositionControl.hpp | 6 - src/modules/mavlink/mavlink_main.cpp | 138 +++++++++--------- src/modules/mavlink/mavlink_main.h | 9 +- src/modules/mavlink/mavlink_messages.cpp | 27 ++-- src/modules/navigator/navigator_main.cpp | 2 +- .../TemperatureCompensationModule.cpp | 2 +- .../vtol_att_control_main.cpp | 64 ++++---- .../vtol_att_control/vtol_att_control_main.h | 3 - 11 files changed, 167 insertions(+), 175 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index ef1198902d..fbde16ecec 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2415,7 +2415,7 @@ Commander::run() } /* handle commands last, as the system needs to be updated to handle them */ - if (_cmd_sub.updated()) { + while (_cmd_sub.updated()) { /* got command */ const unsigned last_generation = _cmd_sub.get_last_generation(); vehicle_command_s cmd; diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index dce75b15ad..1d668e1d6f 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -440,43 +440,44 @@ void FlightModeManager::send_vehicle_cmd_do(uint8_t nav_state) void FlightModeManager::handleCommand() { // get command - vehicle_command_s command{}; - _vehicle_command_sub.copy(&command); + vehicle_command_s command; - // check what command it is - FlightTaskIndex desired_task = switchVehicleCommand(command.command); + while (_vehicle_command_sub.update(&command)) { + // check what command it is + FlightTaskIndex desired_task = switchVehicleCommand(command.command); - // ignore all unkown commands - if (desired_task != FlightTaskIndex::None) { - // switch to the commanded task - FlightTaskError switch_result = switchTask(desired_task); - uint8_t cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED; + // ignore all unkown commands + if (desired_task != FlightTaskIndex::None) { + // switch to the commanded task + FlightTaskError switch_result = switchTask(desired_task); + uint8_t cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED; - // if we are in/switched to the desired task - if (switch_result >= FlightTaskError::NoError) { - cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED; + // if we are in/switched to the desired task + if (switch_result >= FlightTaskError::NoError) { + cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED; - // if the task is running apply parameters to it and see if it rejects - if (isAnyTaskActive() && !_current_task.task->applyCommandParameters(command)) { - cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED; + // if the task is running apply parameters to it and see if it rejects + if (isAnyTaskActive() && !_current_task.task->applyCommandParameters(command)) { + cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED; - // if we just switched and parameters are not accepted, go to failsafe - if (switch_result >= FlightTaskError::NoError) { - switchTask(FlightTaskIndex::ManualPosition); - cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED; + // if we just switched and parameters are not accepted, go to failsafe + if (switch_result >= FlightTaskError::NoError) { + switchTask(FlightTaskIndex::ManualPosition); + cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED; + } } } - } - // send back acknowledgment - vehicle_command_ack_s command_ack{}; - command_ack.command = command.command; - command_ack.result = cmd_result; - command_ack.result_param1 = static_cast(switch_result); - command_ack.target_system = command.source_system; - command_ack.target_component = command.source_component; - command_ack.timestamp = hrt_absolute_time(); - _vehicle_command_ack_pub.publish(command_ack); + // send back acknowledgment + vehicle_command_ack_s command_ack{}; + command_ack.command = command.command; + command_ack.result = cmd_result; + command_ack.result_param1 = static_cast(switch_result); + command_ack.target_system = command.source_system; + command_ack.target_component = command.source_component; + command_ack.timestamp = hrt_absolute_time(); + _vehicle_command_ack_pub.publish(command_ack); + } } } diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 4b592cae5f..d75603d8fc 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -162,9 +162,19 @@ FixedwingPositionControl::vehicle_control_mode_poll() void FixedwingPositionControl::vehicle_command_poll() { - if (_vehicle_command_sub.updated()) { - _vehicle_command_sub.copy(&_vehicle_command); - handle_command(); + vehicle_command_s vehicle_command; + + while (_vehicle_command_sub.update(&vehicle_command)) { + if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND) { + // only abort landing before point of no return (horizontal and vertical) + if (_control_mode.flag_control_auto_enabled && + _pos_sp_triplet.current.valid && + (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND)) { + + abort_landing(true); + } + } + } } @@ -1597,20 +1607,6 @@ FixedwingPositionControl::get_tecs_thrust() return 0.0f; } -void -FixedwingPositionControl::handle_command() -{ - if (_vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND) { - // only abort landing before point of no return (horizontal and vertical) - if (_control_mode.flag_control_auto_enabled && - _pos_sp_triplet.current.valid && - _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { - - abort_landing(true); - } - } -} - void FixedwingPositionControl::Run() { diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index b6804ed79c..9c2271490e 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -170,7 +170,6 @@ private: position_setpoint_triplet_s _pos_sp_triplet {}; ///< triplet of mission items vehicle_attitude_s _att {}; ///< vehicle attitude setpoint vehicle_attitude_setpoint_s _att_sp {}; ///< vehicle attitude setpoint - vehicle_command_s _vehicle_command {}; ///< vehicle commands vehicle_control_mode_s _control_mode {}; ///< control mode vehicle_local_position_s _local_pos {}; ///< vehicle local position vehicle_land_detected_s _vehicle_land_detected {}; ///< vehicle land detected @@ -346,11 +345,6 @@ private: float get_demanded_airspeed(); float calculate_target_airspeed(float airspeed_demand, const Vector2f &ground_speed); - /** - * Handle incoming vehicle commands - */ - void handle_command(); - void reset_takeoff_state(bool force = false); void reset_landing_state(); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c68f8f965c..6183daef64 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -183,6 +183,13 @@ Mavlink::Mavlink() : if (_first_start_time == 0) { _first_start_time = hrt_absolute_time(); } + + // ensure topic exists, otherwise we might lose first queued commands + if (!orb_exists(ORB_ID(vehicle_command), 0)) { + orb_advertise_queue(ORB_ID(vehicle_command), nullptr, vehicle_command_s::ORB_QUEUE_LENGTH); + } + + _vehicle_command_sub.subscribe(); } Mavlink::~Mavlink() @@ -2135,19 +2142,6 @@ Mavlink::task_main(int argc, char *argv[]) pthread_mutex_init(&_message_buffer_mutex, nullptr); } - uORB::Subscription cmd_sub{ORB_ID(vehicle_command)}; - // ensure topic exists, otherwise we might lose first queued commands (leading to printf error's below) - orb_advertise_queue(ORB_ID(vehicle_command), nullptr, vehicle_command_s::ORB_QUEUE_LENGTH); - cmd_sub.subscribe(); - uORB::Subscription status_sub{ORB_ID(vehicle_status)}; - uORB::Subscription ack_sub{ORB_ID(vehicle_command_ack)}; - - /* command ack */ - uORB::Publication command_ack_pub{ORB_ID(vehicle_command_ack)}; - - vehicle_status_s status{}; - status_sub.copy(&status); - /* Activate sending the data by default (for the IRIDIUM mode it will be disabled after the first round of packages is sent)*/ _transmitting_enabled = true; _transmitting_enabled_commanded = true; @@ -2263,81 +2257,84 @@ Mavlink::task_main(int argc, char *argv[]) configure_sik_radio(); - if (status_sub.update(&status)) { - /* switch HIL mode if required */ - set_hil_enabled(status.hil_state == vehicle_status_s::HIL_STATE_ON); + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; - set_generate_virtual_rc_input(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_GENERATED); + if (_vehicle_status_sub.copy(&vehicle_status)) { + /* switch HIL mode if required */ + set_hil_enabled(vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON); - if (_mode == MAVLINK_MODE_IRIDIUM) { + set_generate_virtual_rc_input(vehicle_status.rc_input_mode == vehicle_status_s::RC_IN_MODE_GENERATED); - if (_transmitting_enabled && - status.high_latency_data_link_lost && - !_transmitting_enabled_commanded && - (_first_heartbeat_sent)) { + if (_mode == MAVLINK_MODE_IRIDIUM) { - _transmitting_enabled = false; - mavlink_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s", _device_name); + if (_transmitting_enabled && vehicle_status.high_latency_data_link_lost && + !_transmitting_enabled_commanded && _first_heartbeat_sent) { - } else if (!_transmitting_enabled && !status.high_latency_data_link_lost) { - _transmitting_enabled = true; - mavlink_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s", _device_name); + _transmitting_enabled = false; + mavlink_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s", _device_name); + + } else if (!_transmitting_enabled && !vehicle_status.high_latency_data_link_lost) { + _transmitting_enabled = true; + mavlink_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s", _device_name); + } } } } // vehicle_command - const unsigned last_generation = cmd_sub.get_last_generation(); - vehicle_command_s vehicle_cmd; + while (_vehicle_command_sub.updated()) { + const unsigned last_generation = _vehicle_command_sub.get_last_generation(); + vehicle_command_s vehicle_cmd; - if (cmd_sub.update(&vehicle_cmd)) { - - if (cmd_sub.get_last_generation() != last_generation + 1) { - PX4_ERR("vehicle_command lost, generation %d -> %d", last_generation, cmd_sub.get_last_generation()); - } - - if ((vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY) && - (_mode == MAVLINK_MODE_IRIDIUM)) { - if (vehicle_cmd.param1 > 0.5f) { - if (!_transmitting_enabled) { - mavlink_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s by command", - _device_name); - } - - _transmitting_enabled = true; - _transmitting_enabled_commanded = true; - - } else { - if (_transmitting_enabled) { - mavlink_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s by command", - _device_name); - } - - _transmitting_enabled = false; - _transmitting_enabled_commanded = false; + if (_vehicle_command_sub.update(&vehicle_cmd)) { + if (_vehicle_command_sub.get_last_generation() != last_generation + 1) { + PX4_ERR("vehicle_command lost, generation %d -> %d", last_generation, _vehicle_command_sub.get_last_generation()); } - // send positive command ack - vehicle_command_ack_s command_ack{}; - command_ack.timestamp = vehicle_cmd.timestamp; - command_ack.command = vehicle_cmd.command; - command_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED; - command_ack.from_external = !vehicle_cmd.from_external; - command_ack.target_system = vehicle_cmd.source_system; - command_ack.target_component = vehicle_cmd.source_component; + if ((vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY) && + (_mode == MAVLINK_MODE_IRIDIUM)) { + if (vehicle_cmd.param1 > 0.5f) { + if (!_transmitting_enabled) { + mavlink_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s by command", + _device_name); + } - command_ack_pub.publish(command_ack); + _transmitting_enabled = true; + _transmitting_enabled_commanded = true; + + } else { + if (_transmitting_enabled) { + mavlink_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s by command", + _device_name); + } + + _transmitting_enabled = false; + _transmitting_enabled_commanded = false; + } + + // send positive command ack + vehicle_command_ack_s command_ack{}; + command_ack.command = vehicle_cmd.command; + command_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED; + command_ack.from_external = !vehicle_cmd.from_external; + command_ack.target_system = vehicle_cmd.source_system; + command_ack.target_component = vehicle_cmd.source_component; + command_ack.timestamp = vehicle_cmd.timestamp; + _vehicle_command_ack_pub.publish(command_ack); + } } } /* send command ACK */ - uint16_t current_command_ack = 0; + bool cmd_logging_start_acknowledgement = false; - if (ack_sub.updated() && (get_free_tx_buf() >= MAVLINK_MSG_ID_COMMAND_ACK_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) { + if (_vehicle_command_ack_sub.updated()) { + static constexpr size_t COMMAND_ACK_TOTAL_LEN = MAVLINK_MSG_ID_COMMAND_ACK_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; vehicle_command_ack_s command_ack; - if (ack_sub.update(&command_ack)) { + while ((get_free_tx_buf() >= COMMAND_ACK_TOTAL_LEN) && _vehicle_command_ack_sub.update(&command_ack)) { if (!command_ack.from_external) { mavlink_command_ack_t msg; msg.result = command_ack.result; @@ -2346,13 +2343,16 @@ Mavlink::task_main(int argc, char *argv[]) msg.result_param2 = command_ack.result_param2; msg.target_system = command_ack.target_system; msg.target_component = command_ack.target_component; - current_command_ack = command_ack.command; // TODO: always transmit the acknowledge once it is only sent over the instance the command is received //bool _transmitting_enabled_temp = _transmitting_enabled; //_transmitting_enabled = true; mavlink_msg_command_ack_send_struct(get_channel(), &msg); //_transmitting_enabled = _transmitting_enabled_temp; + + if (command_ack.command == vehicle_command_s::VEHICLE_CMD_LOGGING_START) { + cmd_logging_start_acknowledgement = true; + } } } } @@ -2398,7 +2398,7 @@ Mavlink::task_main(int argc, char *argv[]) _mavlink_ulog_stop_requested = false; } else { - if (current_command_ack == vehicle_command_s::VEHICLE_CMD_LOGGING_START) { + if (cmd_logging_start_acknowledgement) { _mavlink_ulog->start_ack_received(); } @@ -2612,7 +2612,7 @@ void Mavlink::publish_telemetry_status() // telemetry_status is also updated from the receiver thread, but never the same fields _tstatus.timestamp = hrt_absolute_time(); - _telem_status_pub.publish(_tstatus); + _telemetry_status_pub.publish(_tstatus); _tstatus_updated = false; } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 491ee1758a..e33c5638a8 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -75,6 +75,9 @@ #include #include #include +#include +#include +#include #include "mavlink_command_sender.h" #include "mavlink_messages.h" @@ -530,9 +533,13 @@ private: orb_advert_t _mavlink_log_pub{nullptr}; - uORB::PublicationMulti _telem_status_pub{ORB_ID(telemetry_status)}; + uORB::Publication _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)}; + uORB::PublicationMulti _telemetry_status_pub{ORB_ID(telemetry_status)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; + uORB::Subscription _vehicle_command_ack_sub{ORB_ID(vehicle_command_ack)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; bool _task_running{true}; static bool _boot_complete; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index d608fa561e..a2917aabbd 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -506,7 +506,7 @@ public: } private: - uORB::Subscription _cmd_sub{ORB_ID(vehicle_command)}; + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; /* do not allow top copying this class */ MavlinkStreamCommandLong(MavlinkStreamCommandLong &) = delete; @@ -518,19 +518,28 @@ protected: bool send() override { - struct vehicle_command_s cmd; bool sent = false; - if (_cmd_sub.update(&cmd)) { + while ((_mavlink->get_free_tx_buf() >= get_size()) && _vehicle_command_sub.updated()) { - if (!cmd.from_external) { - PX4_DEBUG("sending command %d to %d/%d", cmd.command, cmd.target_system, cmd.target_component); + const unsigned last_generation = _vehicle_command_sub.get_last_generation(); + vehicle_command_s cmd; - MavlinkCommandSender::instance().handle_vehicle_command(cmd, _mavlink->get_channel()); - sent = true; + if (_vehicle_command_sub.update(&cmd)) { + if (_vehicle_command_sub.get_last_generation() != last_generation + 1) { + PX4_ERR("COMMAND_LONG vehicle_command lost, generation %d -> %d", last_generation, + _vehicle_command_sub.get_last_generation()); + } - } else { - PX4_DEBUG("not forwarding command %d to %d/%d", cmd.command, cmd.target_system, cmd.target_component); + if (!cmd.from_external) { + PX4_DEBUG("sending command %d to %d/%d", cmd.command, cmd.target_system, cmd.target_component); + + MavlinkCommandSender::instance().handle_vehicle_command(cmd, _mavlink->get_channel()); + sent = true; + + } else { + PX4_DEBUG("not forwarding command %d to %d/%d", cmd.command, cmd.target_system, cmd.target_component); + } } } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 061dd5e064..cb48a9d99b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -212,7 +212,7 @@ Navigator::run() _position_controller_status_sub.update(); _home_pos_sub.update(&_home_pos); - if (_vehicle_command_sub.updated()) { + while (_vehicle_command_sub.updated()) { const unsigned last_generation = _vehicle_command_sub.get_last_generation(); vehicle_command_s cmd{}; _vehicle_command_sub.copy(&cmd); diff --git a/src/modules/temperature_compensation/TemperatureCompensationModule.cpp b/src/modules/temperature_compensation/TemperatureCompensationModule.cpp index c337a37a81..dae395858d 100644 --- a/src/modules/temperature_compensation/TemperatureCompensationModule.cpp +++ b/src/modules/temperature_compensation/TemperatureCompensationModule.cpp @@ -181,7 +181,7 @@ void TemperatureCompensationModule::Run() perf_begin(_loop_perf); // Check if user has requested to run the calibration routine - if (_vehicle_command_sub.updated()) { + while (_vehicle_command_sub.updated()) { vehicle_command_s cmd; if (_vehicle_command_sub.copy(&cmd)) { 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 62126b5400..dd4d8cb6e2 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -139,52 +139,40 @@ VtolAttitudeControl::init() return true; } -/** -* Check for command updates. -*/ -void -VtolAttitudeControl::vehicle_cmd_poll() +void VtolAttitudeControl::vehicle_cmd_poll() { - if (_vehicle_cmd_sub.updated()) { - _vehicle_cmd_sub.copy(&_vehicle_cmd); - handle_command(); - } -} + vehicle_command_s vehicle_command; -/** -* Check received command -*/ -void -VtolAttitudeControl::handle_command() -{ - if (_vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION) { + while (_vehicle_cmd_sub.update(&vehicle_command)) { + if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION) { + vehicle_status_s vehicle_status{}; + _vehicle_status_sub.copy(&vehicle_status); - vehicle_status_s vehicle_status = {}; - _vehicle_status_sub.copy(&vehicle_status); + uint8_t result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED; - uint8_t result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED; + // deny any transition in auto takeoff mode, plus transition from RW to FW in land or RTL mode + if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF + || (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND + || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL))) { - // deny any transition in auto takeoff mode, plus transition from RW to FW in land or RTL mode - if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF - || (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING - && (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND - || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL))) { - result = vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED; + result = vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED; - } else { - _transition_command = int(_vehicle_cmd.param1 + 0.5f); - } + } else { + _transition_command = int(vehicle_command.param1 + 0.5f); + } - if (_vehicle_cmd.from_external) { - vehicle_command_ack_s command_ack{}; - command_ack.timestamp = hrt_absolute_time(); - command_ack.command = _vehicle_cmd.command; - command_ack.result = result; - command_ack.target_system = _vehicle_cmd.source_system; - command_ack.target_component = _vehicle_cmd.source_component; + if (vehicle_command.from_external) { + vehicle_command_ack_s command_ack{}; + command_ack.timestamp = hrt_absolute_time(); + command_ack.command = vehicle_command.command; + command_ack.result = result; + command_ack.target_system = vehicle_command.source_system; + command_ack.target_component = vehicle_command.source_component; - uORB::Publication command_ack_pub{ORB_ID(vehicle_command_ack)}; - command_ack_pub.publish(command_ack); + uORB::Publication command_ack_pub{ORB_ID(vehicle_command_ack)}; + command_ack_pub.publish(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 6b23d81e2a..34cd547b3e 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -172,7 +172,6 @@ private: position_setpoint_triplet_s _pos_sp_triplet{}; tecs_status_s _tecs_status{}; vehicle_attitude_s _v_att{}; //vehicle attitude - vehicle_command_s _vehicle_cmd{}; vehicle_control_mode_s _v_control_mode{}; //vehicle control mode vehicle_land_detected_s _land_detected{}; vehicle_local_position_s _local_pos{}; @@ -232,6 +231,4 @@ private: void vehicle_cmd_poll(); int parameters_update(); //Update local parameter cache - - void handle_command(); };