forked from Archive/PX4-Autopilot
VTOL: reject transition command in certain flight modes
This commit is contained in:
parent
a4c77056f6
commit
504794eddc
|
@ -156,18 +156,29 @@ VtolAttitudeControl::vehicle_cmd_poll()
|
|||
void
|
||||
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.
|
||||
vehicle_status_s vehicle_status = {};
|
||||
_vehicle_status_sub.copy(&vehicle_status);
|
||||
|
||||
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))) {
|
||||
result = vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED;
|
||||
|
||||
} else {
|
||||
_transition_command = int(_vehicle_cmd.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 = (uint8_t)vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
|
||||
command_ack.result = result;
|
||||
command_ack.target_system = _vehicle_cmd.source_system;
|
||||
command_ack.target_component = _vehicle_cmd.source_component;
|
||||
|
||||
|
|
|
@ -79,7 +79,7 @@
|
|||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_local_position_setpoint.h>
|
||||
#include <uORB/topics/vtol_vehicle_status.h>
|
||||
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include "standard.h"
|
||||
#include "tailsitter.h"
|
||||
#include "tiltrotor.h"
|
||||
|
@ -146,6 +146,7 @@ private:
|
|||
uORB::Subscription _v_att_sub{ORB_ID(vehicle_attitude)}; //vehicle attitude subscription
|
||||
uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; //vehicle control mode subscription
|
||||
uORB::Subscription _vehicle_cmd_sub{ORB_ID(vehicle_command)};
|
||||
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||
|
||||
uORB::Publication<actuator_controls_s> _actuators_0_pub{ORB_ID(actuator_controls_0)}; //input for the mixer (roll,pitch,yaw,thrust)
|
||||
uORB::Publication<actuator_controls_s> _actuators_1_pub{ORB_ID(actuator_controls_1)};
|
||||
|
|
Loading…
Reference in New Issue