dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

VTOL: reject transition command in certain flight modes

This commit is contained in:
Silvan Fuhrer 2020-05-20 21:54:31 +02:00 committed by GitHub
parent a4c77056f6
commit 504794eddc
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 18 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
src/modules/vtol_att_control/vtol_att_control_main.cpp
View File

@ -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;

3
src/modules/vtol_att_control/vtol_att_control_main.h
View File

@ -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)};