From b5a01532ae6741e439654e53dcbf27a7c0a36450 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 9 Aug 2019 12:44:11 -0400 Subject: [PATCH] vtol_att_control move to WQ with uORB callback scheduling (#12229) * add perf counters for cycle time and interval --- .../vtol_att_control_main.cpp | 311 ++++++------------ .../vtol_att_control/vtol_att_control_main.h | 88 ++--- 2 files changed, 151 insertions(+), 248 deletions(-) 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 de638dde14..97b604d963 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 - 2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -53,15 +53,10 @@ using namespace matrix; -namespace VTOL_att_control -{ -VtolAttitudeControl *g_control; -} - -/** -* Constructor -*/ -VtolAttitudeControl::VtolAttitudeControl() +VtolAttitudeControl::VtolAttitudeControl() : + WorkItem(px4::wq_configurations::rate_ctrl), + _loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control: cycle")), + _loop_interval_perf(perf_alloc(PC_INTERVAL, "vtol_att_control: interval")) { _vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/ @@ -106,66 +101,30 @@ VtolAttitudeControl::VtolAttitudeControl() _vtol_type = new Standard(this); } else { - _task_should_exit = true; + exit_and_cleanup(); } } -/** -* Destructor -*/ VtolAttitudeControl::~VtolAttitudeControl() { - if (_control_task != -1) { - /* task wakes up every 100ms or so at the longest */ - _task_should_exit = true; - - /* wait for a second for the task to quit at our request */ - unsigned i = 0; - - do { - /* wait 20ms */ - px4_usleep(20000); - - /* if we have given up, kill it */ - if (++i > 50) { - px4_task_delete(_control_task); - break; - } - } while (_control_task != -1); - } - - // free memory used by instances of base class VtolType - if (_vtol_type != nullptr) { - delete _vtol_type; - } - - VTOL_att_control::g_control = nullptr; + perf_free(_loop_perf); + perf_free(_loop_interval_perf); } -/** -* Check for inputs from mc attitude controller. -*/ -void VtolAttitudeControl::actuator_controls_mc_poll() +bool +VtolAttitudeControl::init() { - bool updated; - orb_check(_actuator_inputs_mc, &updated); - - if (updated) { - orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in); + if (!_actuator_inputs_mc.register_callback()) { + PX4_ERR("MC actuator controls callback registration failed!"); + return false; } -} -/** -* Check for inputs from fw attitude controller. -*/ -void VtolAttitudeControl::actuator_controls_fw_poll() -{ - bool updated; - orb_check(_actuator_inputs_fw, &updated); - - if (updated) { - orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in); + if (!_actuator_inputs_fw.register_callback()) { + PX4_ERR("FW actuator controls callback registration failed!"); + return false; } + + return true; } /** @@ -240,9 +199,6 @@ VtolAttitudeControl::is_fixed_wing_requested() return to_fw; } -/* - * Abort front transition - */ void VtolAttitudeControl::abort_front_transition(const char *reason) { @@ -253,9 +209,6 @@ VtolAttitudeControl::abort_front_transition(const char *reason) } } -/** -* Update parameters. -*/ int VtolAttitudeControl::parameters_update() { @@ -333,29 +286,35 @@ VtolAttitudeControl::parameters_update() return OK; } -int -VtolAttitudeControl::task_main_trampoline(int argc, char *argv[]) +void +VtolAttitudeControl::Run() { - VTOL_att_control::g_control->task_main(); - return 0; -} + if (should_exit()) { + _actuator_inputs_fw.unregister_callback(); + _actuator_inputs_mc.unregister_callback(); + exit_and_cleanup(); + return; + } -void VtolAttitudeControl::task_main() -{ - /* do subscriptions */ - _actuator_inputs_mc = orb_subscribe(ORB_ID(actuator_controls_virtual_mc)); - _actuator_inputs_fw = orb_subscribe(ORB_ID(actuator_controls_virtual_fw)); + if (!_initialized) { + parameters_update(); // initialize parameter cache - parameters_update(); // initialize parameter cache + if (_vtol_type->init()) { + _initialized = true; - _task_should_exit = !_vtol_type->init(); + } else { + exit_and_cleanup(); + return; + } + } - /* wakeup source*/ - px4_pollfd_struct_t fds[1] = {}; - fds[0].fd = _actuator_inputs_mc; - fds[0].events = POLLIN; + perf_begin(_loop_perf); + perf_count(_loop_interval_perf); - while (!_task_should_exit) { + bool updated_fw_in = _actuator_inputs_fw.update(&_actuators_fw_in); + bool updated_mc_in = _actuator_inputs_mc.update(&_actuators_mc_in); + + if (updated_fw_in || updated_mc_in) { /* only update parameters if they changed */ if (_params_sub.updated()) { /* read from param to clear updated flag */ @@ -366,35 +325,6 @@ void VtolAttitudeControl::task_main() parameters_update(); } - // run vtol_att on MC actuator publications, unless in full FW mode - switch (_vtol_type->get_mode()) { - case mode::TRANSITION_TO_FW: - case mode::TRANSITION_TO_MC: - case mode::ROTARY_WING: - fds[0].fd = _actuator_inputs_mc; - break; - - case mode::FIXED_WING: - fds[0].fd = _actuator_inputs_fw; - break; - } - - /* wait for up to 100ms for data */ - int pret = px4_poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 100); - - /* timed out - periodic check for _task_should_exit */ - if (pret == 0) { - continue; - } - - /* this is undesirable but not much we can do - might want to flag unhappy status */ - if (pret < 0) { - PX4_ERR("poll error %d, %d", pret, errno); - /* sleep a bit before next try */ - px4_usleep(100000); - continue; - } - _v_control_mode_sub.update(&_v_control_mode); _manual_control_sp_sub.update(&_manual_control_sp); _v_att_sub.update(&_v_att); @@ -405,8 +335,6 @@ void VtolAttitudeControl::task_main() _tecs_status_sub.update(&_tecs_status); _land_detected_sub.update(&_land_detected); vehicle_cmd_poll(); - actuator_controls_fw_poll(); - actuator_controls_mc_poll(); // update the vtol state machine which decides which mode we are in _vtol_type->update_vtol_state(); @@ -479,118 +407,89 @@ void VtolAttitudeControl::task_main() _v_control_mode.flag_control_rates_enabled || _v_control_mode.flag_control_manual_enabled) { - if (_v_att_sp_pub != nullptr) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_pub, &_v_att_sp); + _v_att_sp_pub.publish(_v_att_sp); - } else { - /* advertise and publish */ - _v_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp); + if (updated_mc_in) { + _actuators_0_pub.publish(_actuators_out_0); } - if (_actuators_0_pub != nullptr) { - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); - - } else { - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); - } - - if (_actuators_1_pub != nullptr) { - orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); - - } else { - _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); + if (updated_fw_in) { + _actuators_1_pub.publish(_actuators_out_1); } } - /*Advertise/Publish vtol vehicle status*/ + // Advertise/Publish vtol vehicle status _vtol_vehicle_status.timestamp = hrt_absolute_time(); - - if (_vtol_vehicle_status_pub != nullptr) { - orb_publish(ORB_ID(vtol_vehicle_status), _vtol_vehicle_status_pub, &_vtol_vehicle_status); - - } else { - _vtol_vehicle_status_pub = orb_advertise(ORB_ID(vtol_vehicle_status), &_vtol_vehicle_status); - } + _vtol_vehicle_status_pub.publish(_vtol_vehicle_status); } - PX4_WARN("exit"); - _control_task = -1; + perf_end(_loop_perf); } int -VtolAttitudeControl::start() +VtolAttitudeControl::task_spawn(int argc, char *argv[]) { - /* start the task */ - _control_task = px4_task_spawn_cmd("vtol_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_ATTITUDE_CONTROL + 1, - 1320, - (px4_main_t)&VtolAttitudeControl::task_main_trampoline, - nullptr); + VtolAttitudeControl *instance = new VtolAttitudeControl(); - if (_control_task < 0) { - PX4_WARN("task start failed"); - return -errno; + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); } - return OK; + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; } +int +VtolAttitudeControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int +VtolAttitudeControl::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +fw_att_control is the fixed wing attitude controller. +)DESCR_STR"); + + PRINT_MODULE_USAGE_COMMAND("start"); + + PRINT_MODULE_USAGE_NAME("vtol_att_control", "controller"); + + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +int +VtolAttitudeControl::print_status() +{ + PX4_INFO("Running"); + + perf_print_counter(_loop_perf); + perf_print_counter(_loop_interval_perf); + + return PX4_OK; +} int vtol_att_control_main(int argc, char *argv[]) { - if (argc < 2) { - PX4_WARN("usage: vtol_att_control {start|stop|status}"); - return 1; - } - - if (!strcmp(argv[1], "start")) { - - if (VTOL_att_control::g_control != nullptr) { - PX4_WARN("already running"); - return 0; - } - - VTOL_att_control::g_control = new VtolAttitudeControl; - - if (VTOL_att_control::g_control == nullptr) { - PX4_WARN("alloc failed"); - return 1; - } - - if (OK != VTOL_att_control::g_control->start()) { - delete VTOL_att_control::g_control; - VTOL_att_control::g_control = nullptr; - PX4_WARN("start failed"); - return 1; - } - - return 0; - } - - if (!strcmp(argv[1], "stop")) { - if (VTOL_att_control::g_control == nullptr) { - PX4_WARN("not running"); - return 0; - } - - delete VTOL_att_control::g_control; - VTOL_att_control::g_control = nullptr; - return 0; - } - - if (!strcmp(argv[1], "status")) { - if (VTOL_att_control::g_control) { - PX4_WARN("running"); - - } else { - PX4_WARN("not running"); - } - - return 0; - } - - PX4_WARN("unrecognized command"); - return 1; + return VtolAttitudeControl::main(argc, argv); } 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 8d4ddcb2bc..1f8ef9d3ce 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -46,26 +46,29 @@ * @author Andreas Antener * */ -#ifndef VTOL_ATT_CONTROL_MAIN_H -#define VTOL_ATT_CONTROL_MAIN_H -#include -#include -#include -#include +#pragma once #include #include #include -#include +#include +#include +#include #include -#include - +#include +#include +#include +#include +#include +#include #include +#include #include #include #include #include +#include #include #include #include @@ -75,25 +78,37 @@ #include #include #include -#include #include -#include "tiltrotor.h" -#include "tailsitter.h" #include "standard.h" - +#include "tailsitter.h" +#include "tiltrotor.h" extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]); - -class VtolAttitudeControl +class VtolAttitudeControl : public ModuleBase, public px4::WorkItem { public: VtolAttitudeControl(); ~VtolAttitudeControl(); - int start(); /* start the task and return OK on success */ + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + void Run() override; + + bool init(); + + /** @see ModuleBase::print_status() */ + int print_status() override; + bool is_fixed_wing_requested(); void abort_front_transition(const char *reason); @@ -116,15 +131,10 @@ public: struct Params *get_params() {return &_params;} - private: -//******************flags & handlers****************************************************** - bool _task_should_exit{false}; - int _control_task{-1}; //task handle for VTOL attitude controller - /* handlers for subscriptions */ - int _actuator_inputs_fw{-1}; //topic on which the fw_att_controller publishes actuator inputs - int _actuator_inputs_mc{-1}; //topic on which the mc_att_controller publishes actuator inputs + uORB::SubscriptionCallbackWorkItem _actuator_inputs_fw{this, ORB_ID(actuator_controls_virtual_fw)}; + uORB::SubscriptionCallbackWorkItem _actuator_inputs_mc{this, ORB_ID(actuator_controls_virtual_mc)}; uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; // airspeed subscription uORB::Subscription _fw_virtual_att_sp_sub{ORB_ID(fw_virtual_attitude_setpoint)}; @@ -140,14 +150,12 @@ private: uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; //vehicle control mode subscription uORB::Subscription _vehicle_cmd_sub{ORB_ID(vehicle_command)}; - //handlers for publishers - orb_advert_t _actuators_0_pub{nullptr}; //input for the mixer (roll,pitch,yaw,thrust) - orb_advert_t _mavlink_log_pub{nullptr}; // mavlink log uORB handle - orb_advert_t _v_att_sp_pub{nullptr}; - orb_advert_t _vtol_vehicle_status_pub{nullptr}; - orb_advert_t _actuators_1_pub{nullptr}; + uORB::Publication _actuators_0_pub{ORB_ID(actuator_controls_0)}; //input for the mixer (roll,pitch,yaw,thrust) + uORB::Publication _actuators_1_pub{ORB_ID(actuator_controls_1)}; + uORB::Publication _v_att_sp_pub{ORB_ID(vehicle_attitude_setpoint)}; + uORB::Publication _vtol_vehicle_status_pub{ORB_ID(vtol_vehicle_status)}; -//*******************data containers*********************************************************** + orb_advert_t _mavlink_log_pub{nullptr}; // mavlink log uORB handle vehicle_attitude_setpoint_s _v_att_sp{}; //vehicle attitude setpoint vehicle_attitude_setpoint_s _fw_virtual_att_sp{}; // virtual fw attitude setpoint @@ -166,7 +174,7 @@ private: 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{}; + vehicle_local_position_s _local_pos{}; vehicle_local_position_setpoint_s _local_pos_sp{}; vtol_vehicle_status_s _vtol_vehicle_status{}; @@ -201,23 +209,19 @@ private: /* for multicopters it is usual to have a non-zero idle speed of the engines * for fixed wings we want to have an idle speed of zero since we do not want * to waste energy when gliding. */ - int _transition_command{vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC}; - bool _abort_front_transition{false}; + int _transition_command{vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC}; + bool _abort_front_transition{false}; - VtolType *_vtol_type{nullptr}; // base class for different vtol types + VtolType *_vtol_type{nullptr}; // base class for different vtol types -//*****************Member functions*********************************************************************** + bool _initialized{false}; - void task_main(); //main task - static int task_main_trampoline(int argc, char *argv[]); //Shim for calling task_main from task_create. + perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _loop_interval_perf; /**< loop interval performance counter */ void vehicle_cmd_poll(); - void actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output - void actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output int parameters_update(); //Update local parameter cache void handle_command(); }; - -#endif