fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-30 03:40:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

VTOL: explicitly start all FW & MC controllers in VTOL mode

Browse Source
This commit is contained in:
Daniel Agar
2019-11-30 12:58:36 -05:00
committed by GitHub
parent e50a50876f
commit 0cc250194d
12 changed files with 149 additions and 240 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_rate_control/MulticopterRateControl.cpp
+17 -29
View File
@@ -42,9 +42,10 @@ using namespace matrix;
using namespace time_literals;
using math::radians;
MulticopterRateControl::MulticopterRateControl() :
MulticopterRateControl::MulticopterRateControl(bool vtol) :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
_actuators_0_pub(vtol ? ORB_ID(actuator_controls_virtual_mc) : ORB_ID(actuator_controls_0)),
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
{
_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
@@ -97,23 +98,6 @@ MulticopterRateControl::parameters_updated()
_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled_by_val(_param_cbrk_rate_ctrl.get(), CBRK_RATE_CTRL_KEY);
}
void
MulticopterRateControl::vehicle_status_poll()
{
/* check if there is new status information */
if (_vehicle_status_sub.update(&_vehicle_status)) {
/* set correct uORB ID, depending on if vehicle is VTOL or not */
if (_actuators_id == nullptr) {
if (_vehicle_status.is_vtol) {
_actuators_id = ORB_ID(actuator_controls_virtual_mc);
} else {
_actuators_id = ORB_ID(actuator_controls_0);
}
}
}
}
float
MulticopterRateControl::get_landing_gear_state()
{
@@ -183,7 +167,7 @@ MulticopterRateControl::Run()
}
}
vehicle_status_poll();
_vehicle_status_sub.update(&_vehicle_status);
const bool manual_control_updated = _manual_control_sp_sub.update(&_manual_control_sp);
@@ -326,14 +310,14 @@ MulticopterRateControl::Run()
}
actuators.timestamp = hrt_absolute_time();
orb_publish_auto(_actuators_id, &_actuators_0_pub, &actuators, nullptr, ORB_PRIO_DEFAULT);
_actuators_0_pub.publish(actuators);
} else if (_v_control_mode.flag_control_termination_enabled) {
if (!_vehicle_status.is_vtol) {
// publish actuator controls
actuator_controls_s actuators{};
actuators.timestamp = hrt_absolute_time();
orb_publish_auto(_actuators_id, &_actuators_0_pub, &actuators, nullptr, ORB_PRIO_DEFAULT);
_actuators_0_pub.publish(actuators);
}
}
}
@@ -343,7 +327,15 @@ MulticopterRateControl::Run()
int MulticopterRateControl::task_spawn(int argc, char *argv[])
{
MulticopterRateControl *instance = new MulticopterRateControl();
bool vtol = false;
if (argc > 1) {
if (strcmp(argv[1], "vtol") == 0) {
vtol = true;
}
}
MulticopterRateControl *instance = new MulticopterRateControl(vtol);
if (instance) {
_object.store(instance);
@@ -394,19 +386,15 @@ The controller has a PID loop for angular rate error.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME(MODULE_NAME, "controller");
PRINT_MODULE_USAGE_NAME("mc_rate_control", "controller");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
/**
* Multicopter rate control app start / stop handling function
*/
extern "C" __EXPORT int mc_rate_control_main(int argc, char *argv[]);
int mc_rate_control_main(int argc, char *argv[])
extern "C" __EXPORT int mc_rate_control_main(int argc, char *argv[])
{
return MulticopterRateControl::main(argc, argv);
}