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

Use action_request to command RC VTOL transitions

This commit is contained in:
Matthias Grob 2021-10-19 18:26:18 +02:00
parent dcd26bd2b8
commit a349dae760
5 changed files with 30 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/action_request.msg
View File

@ -7,6 +7,8 @@ uint8 ACTION_TOGGLE_ARMING = 2
uint8 ACTION_UNKILL = 3
uint8 ACTION_KILL = 4
uint8 ACTION_SWITCH_MODE = 5
uint8 ACTION_VTOL_TRANSITION_TO_MULTICOPTER = 6
uint8 ACTION_VTOL_TRANSITION_TO_FIXEDWING = 7
uint8 source # how the request was triggered
uint8 SOURCE_RC_STICK_GESTURE = 0

17
src/modules/manual_control/ManualControl.cpp
View File

@ -232,10 +232,10 @@ void ManualControl::Run()
if (switches.transition_switch != _previous_switches.transition_switch) {
if (switches.transition_switch == manual_control_switches_s::SWITCH_POS_ON) {
send_vtol_transition_command(vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);
sendActionRequest(action_request_s::ACTION_VTOL_TRANSITION_TO_FIXEDWING, action_request_s::SOURCE_RC_SWITCH);
} else if (switches.transition_switch == manual_control_switches_s::SWITCH_POS_OFF) {
send_vtol_transition_command(vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC);
sendActionRequest(action_request_s::ACTION_VTOL_TRANSITION_TO_MULTICOPTER, action_request_s::SOURCE_RC_SWITCH);
}
}
@ -342,19 +342,6 @@ void ManualControl::publish_landing_gear(int8_t action)
landing_gear_pub.publish(landing_gear);
}
void ManualControl::send_vtol_transition_command(uint8_t action)
{
vehicle_command_s command{};
command.command = vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION;
command.param1 = action;
command.target_system = _param_mav_sys_id.get();
command.target_component = _param_mav_comp_id.get();
uORB::Publication<vehicle_command_s> command_pub{ORB_ID(vehicle_command)};
command.timestamp = hrt_absolute_time();
command_pub.publish(command);
}
int ManualControl::task_spawn(int argc, char *argv[])
{
ManualControl *instance = new ManualControl();

1
src/modules/manual_control/ManualControl.hpp
View File

@ -122,7 +122,6 @@ private:
void evaluateModeSlot(uint8_t mode_slot);
void sendActionRequest(int8_t action, int8_t source, int8_t mode = 0);
void publish_landing_gear(int8_t action);
void send_vtol_transition_command(uint8_t action);
uORB::Publication<action_request_s> _action_request_pub{ORB_ID(action_request)};
uORB::Publication<manual_control_setpoint_s> _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)};

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

@ -139,6 +139,27 @@ VtolAttitudeControl::init()
return true;
}
void VtolAttitudeControl::action_request_poll()
{
while (_action_request_sub.updated()) {
action_request_s action_request;
if (_action_request_sub.copy(&action_request)) {
switch (action_request.action) {
case action_request_s::ACTION_VTOL_TRANSITION_TO_MULTICOPTER:
_transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
_immediate_transition = false;
break;
case action_request_s::ACTION_VTOL_TRANSITION_TO_FIXEDWING:
_transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW;
_immediate_transition = false;
break;
}
}
}
}
void VtolAttitudeControl::vehicle_cmd_poll()
{
vehicle_command_s vehicle_command;
@ -181,27 +202,6 @@ void VtolAttitudeControl::vehicle_cmd_poll()
}
}
/*
* Returns true if fixed-wing mode is requested.
* Changed either via switch or via command.
*/
bool
VtolAttitudeControl::is_fixed_wing_requested()
{
bool to_fw = false;
if (_manual_control_switches.transition_switch != manual_control_switches_s::SWITCH_POS_NONE &&
_v_control_mode.flag_control_manual_enabled) {
to_fw = (_manual_control_switches.transition_switch == manual_control_switches_s::SWITCH_POS_ON);
} else {
// listen to transition commands if not in manual or mode switch is not mapped
to_fw = (_transition_command == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);
}
return to_fw;
}
void
VtolAttitudeControl::quadchute(QuadchuteReason reason)
{
@ -434,7 +434,6 @@ VtolAttitudeControl::Run()
}
_v_control_mode_sub.update(&_v_control_mode);
_manual_control_switches_sub.update(&_manual_control_switches);
_v_att_sub.update(&_v_att);
_local_pos_sub.update(&_local_pos);
_local_pos_sp_sub.update(&_local_pos_sp);
@ -442,6 +441,7 @@ VtolAttitudeControl::Run()
_airspeed_validated_sub.update(&_airspeed_validated);
_tecs_status_sub.update(&_tecs_status);
_land_detected_sub.update(&_land_detected);
action_request_poll();
vehicle_cmd_poll();
// check if mc and fw sp were updated
@ -451,24 +451,6 @@ VtolAttitudeControl::Run()
// update the vtol state machine which decides which mode we are in
_vtol_type->update_vtol_state();
// reset transition command if not auto control
if (_v_control_mode.flag_control_manual_enabled) {
if (_vtol_type->get_mode() == mode::ROTARY_WING) {
_transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
} else if (_vtol_type->get_mode() == mode::FIXED_WING) {
_transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW;
} else if (_vtol_type->get_mode() == mode::TRANSITION_TO_MC) {
/* We want to make sure that a mode change (manual>auto) during the back transition
* doesn't result in an unsafe state. This prevents the instant fall back to
* fixed-wing on the switch from manual to auto */
_transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
}
}
// check in which mode we are in and call mode specific functions
switch (_vtol_type->get_mode()) {
case mode::TRANSITION_TO_FW:

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

@ -64,9 +64,9 @@
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/action_request.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/airspeed_validated.h>
#include <uORB/topics/manual_control_switches.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/tecs_status.h>
@ -116,7 +116,7 @@ public:
bool init();
bool is_fixed_wing_requested();
bool is_fixed_wing_requested() { return _transition_command == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW; };
void quadchute(QuadchuteReason reason);
int get_transition_command() {return _transition_command;}
bool get_immediate_transition() {return _immediate_transition;}
@ -150,12 +150,12 @@ private:
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _action_request_sub{ORB_ID(action_request)};
uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; // airspeed subscription
uORB::Subscription _fw_virtual_att_sp_sub{ORB_ID(fw_virtual_attitude_setpoint)};
uORB::Subscription _land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _local_pos_sp_sub{ORB_ID(vehicle_local_position_setpoint)}; // setpoint subscription
uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; // sensor subscription
uORB::Subscription _manual_control_switches_sub{ORB_ID(manual_control_switches)}; //manual control setpoint subscription
uORB::Subscription _mc_virtual_att_sp_sub{ORB_ID(mc_virtual_attitude_setpoint)};
uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; // local position setpoint subscription
uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)};
@ -181,7 +181,6 @@ private:
actuator_controls_s _actuators_out_1{}; //actuator controls going to the fw mixer (used for elevons)
airspeed_validated_s _airspeed_validated{}; // airspeed
manual_control_switches_s _manual_control_switches{}; //manual control setpoint
position_setpoint_triplet_s _pos_sp_triplet{};
tecs_status_s _tecs_status{};
vehicle_attitude_s _v_att{}; //vehicle attitude
@ -243,6 +242,7 @@ private:
perf_counter_t _loop_perf; /**< loop performance counter */
void action_request_poll();
void vehicle_cmd_poll();
int parameters_update(); //Update local parameter cache