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

vtol_att_control move to WQ with uORB callback scheduling (#12229) 

* add perf counters for cycle time and interval
This commit is contained in:
Daniel Agar 2019-08-09 12:44:11 -04:00 committed by GitHub
parent decb9a8982
commit b5a01532ae
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 151 additions and 248 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -53,15 +53,10 @@
using namespace matrix; using namespace matrix;
namespace VTOL_att_control VtolAttitudeControl::VtolAttitudeControl() :
{ WorkItem(px4::wq_configurations::rate_ctrl),
VtolAttitudeControl *g_control; _loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control: cycle")),
} _loop_interval_perf(perf_alloc(PC_INTERVAL, "vtol_att_control: interval"))
/**
* Constructor
*/
VtolAttitudeControl::VtolAttitudeControl()
{ {
_vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/ _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); _vtol_type = new Standard(this);
} else { } else {
_task_should_exit = true; exit_and_cleanup();
} }
} }
/**
* Destructor
*/
VtolAttitudeControl::~VtolAttitudeControl() VtolAttitudeControl::~VtolAttitudeControl()
{ {
if (_control_task != -1) { perf_free(_loop_perf);
/* task wakes up every 100ms or so at the longest */ perf_free(_loop_interval_perf);
_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;
} }
/** bool
* Check for inputs from mc attitude controller. VtolAttitudeControl::init()
*/
void VtolAttitudeControl::actuator_controls_mc_poll()
{ {
bool updated; if (!_actuator_inputs_mc.register_callback()) {
orb_check(_actuator_inputs_mc, &updated); PX4_ERR("MC actuator controls callback registration failed!");
return false;
if (updated) {
orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in);
} }
}
/** if (!_actuator_inputs_fw.register_callback()) {
* Check for inputs from fw attitude controller. PX4_ERR("FW actuator controls callback registration failed!");
*/ return false;
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);
} }
return true;
} }
/** /**
@ -240,9 +199,6 @@ VtolAttitudeControl::is_fixed_wing_requested()
return to_fw; return to_fw;
} }
/*
* Abort front transition
*/
void void
VtolAttitudeControl::abort_front_transition(const char *reason) VtolAttitudeControl::abort_front_transition(const char *reason)
{ {
@ -253,9 +209,6 @@ VtolAttitudeControl::abort_front_transition(const char *reason)
} }
} }
/**
* Update parameters.
*/
int int
VtolAttitudeControl::parameters_update() VtolAttitudeControl::parameters_update()
{ {
@ -333,29 +286,35 @@ VtolAttitudeControl::parameters_update()
return OK; return OK;
} }
int void
VtolAttitudeControl::task_main_trampoline(int argc, char *argv[]) VtolAttitudeControl::Run()
{ {
VTOL_att_control::g_control->task_main(); if (should_exit()) {
return 0; _actuator_inputs_fw.unregister_callback();
} _actuator_inputs_mc.unregister_callback();
exit_and_cleanup();
return;
}
void VtolAttitudeControl::task_main() if (!_initialized) {
{ parameters_update(); // initialize parameter cache
/* do subscriptions */
_actuator_inputs_mc = orb_subscribe(ORB_ID(actuator_controls_virtual_mc));
_actuator_inputs_fw = orb_subscribe(ORB_ID(actuator_controls_virtual_fw));
parameters_update(); // initialize parameter cache if (_vtol_type->init()) {
_initialized = true;
_task_should_exit = !_vtol_type->init(); } else {
exit_and_cleanup();
return;
}
}
/* wakeup source*/ perf_begin(_loop_perf);
px4_pollfd_struct_t fds[1] = {}; perf_count(_loop_interval_perf);
fds[0].fd = _actuator_inputs_mc;
fds[0].events = POLLIN;
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 */ /* only update parameters if they changed */
if (_params_sub.updated()) { if (_params_sub.updated()) {
/* read from param to clear updated flag */ /* read from param to clear updated flag */
@ -366,35 +325,6 @@ void VtolAttitudeControl::task_main()
parameters_update(); 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); _v_control_mode_sub.update(&_v_control_mode);
_manual_control_sp_sub.update(&_manual_control_sp); _manual_control_sp_sub.update(&_manual_control_sp);
_v_att_sub.update(&_v_att); _v_att_sub.update(&_v_att);
@ -405,8 +335,6 @@ void VtolAttitudeControl::task_main()
_tecs_status_sub.update(&_tecs_status); _tecs_status_sub.update(&_tecs_status);
_land_detected_sub.update(&_land_detected); _land_detected_sub.update(&_land_detected);
vehicle_cmd_poll(); vehicle_cmd_poll();
actuator_controls_fw_poll();
actuator_controls_mc_poll();
// update the vtol state machine which decides which mode we are in // update the vtol state machine which decides which mode we are in
_vtol_type->update_vtol_state(); _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_rates_enabled ||
_v_control_mode.flag_control_manual_enabled) { _v_control_mode.flag_control_manual_enabled) {
if (_v_att_sp_pub != nullptr) { _v_att_sp_pub.publish(_v_att_sp);
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_pub, &_v_att_sp);
} else { if (updated_mc_in) {
/* advertise and publish */ _actuators_0_pub.publish(_actuators_out_0);
_v_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp);
} }
if (_actuators_0_pub != nullptr) { if (updated_fw_in) {
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); _actuators_1_pub.publish(_actuators_out_1);
} 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);
} }
} }
/*Advertise/Publish vtol vehicle status*/ // Advertise/Publish vtol vehicle status
_vtol_vehicle_status.timestamp = hrt_absolute_time(); _vtol_vehicle_status.timestamp = hrt_absolute_time();
_vtol_vehicle_status_pub.publish(_vtol_vehicle_status);
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);
}
} }
PX4_WARN("exit"); perf_end(_loop_perf);
_control_task = -1;
} }
int int
VtolAttitudeControl::start() VtolAttitudeControl::task_spawn(int argc, char *argv[])
{ {
/* start the task */ VtolAttitudeControl *instance = new VtolAttitudeControl();
_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);
if (_control_task < 0) { if (instance) {
PX4_WARN("task start failed"); _object.store(instance);
return -errno; _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[]) int vtol_att_control_main(int argc, char *argv[])
{ {
if (argc < 2) { return VtolAttitudeControl::main(argc, argv);
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;
} }

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

@ -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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -46,26 +46,29 @@
* @author Andreas Antener <andreas@uaventure.com> * @author Andreas Antener <andreas@uaventure.com>
* *
*/ */
#ifndef VTOL_ATT_CONTROL_MAIN_H
#define VTOL_ATT_CONTROL_MAIN_H
#include <px4_config.h> #pragma once
#include <px4_defines.h>
#include <px4_tasks.h>
#include <px4_posix.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h> #include <drivers/drv_pwm_output.h>
#include <lib/ecl/geo/geo.h> #include <lib/ecl/geo/geo.h>
#include <mathlib/mathlib.h> #include <lib/mathlib/mathlib.h>
#include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h>
#include <matrix/math.hpp> #include <matrix/math.hpp>
#include <parameters/param.h> #include <px4_config.h>
#include <px4_defines.h>
#include <px4_module.h>
#include <px4_posix.h>
#include <px4_work_queue/WorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp> #include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_controls.h>
#include <uORB/topics/airspeed.h> #include <uORB/topics/airspeed.h>
#include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h> #include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/tecs_status.h> #include <uORB/topics/tecs_status.h>
#include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_attitude_setpoint.h>
@ -75,25 +78,37 @@
#include <uORB/topics/vehicle_land_detected.h> #include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h> #include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vtol_vehicle_status.h> #include <uORB/topics/vtol_vehicle_status.h>
#include "tiltrotor.h"
#include "tailsitter.h"
#include "standard.h" #include "standard.h"
#include "tailsitter.h"
#include "tiltrotor.h"
extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]); extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]);
class VtolAttitudeControl : public ModuleBase<VtolAttitudeControl>, public px4::WorkItem
class VtolAttitudeControl
{ {
public: public:
VtolAttitudeControl(); VtolAttitudeControl();
~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(); bool is_fixed_wing_requested();
void abort_front_transition(const char *reason); void abort_front_transition(const char *reason);
@ -116,15 +131,10 @@ public:
struct Params *get_params() {return &_params;} struct Params *get_params() {return &_params;}
private: private:
//******************flags & handlers******************************************************
bool _task_should_exit{false};
int _control_task{-1}; //task handle for VTOL attitude controller
/* handlers for subscriptions */ /* handlers for subscriptions */
int _actuator_inputs_fw{-1}; //topic on which the fw_att_controller publishes actuator inputs uORB::SubscriptionCallbackWorkItem _actuator_inputs_fw{this, ORB_ID(actuator_controls_virtual_fw)};
int _actuator_inputs_mc{-1}; //topic on which the mc_att_controller publishes actuator inputs uORB::SubscriptionCallbackWorkItem _actuator_inputs_mc{this, ORB_ID(actuator_controls_virtual_mc)};
uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; // airspeed subscription uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; // airspeed subscription
uORB::Subscription _fw_virtual_att_sp_sub{ORB_ID(fw_virtual_attitude_setpoint)}; 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 _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_cmd_sub{ORB_ID(vehicle_command)};
//handlers for publishers uORB::Publication<actuator_controls_s> _actuators_0_pub{ORB_ID(actuator_controls_0)}; //input for the mixer (roll,pitch,yaw,thrust)
orb_advert_t _actuators_0_pub{nullptr}; //input for the mixer (roll,pitch,yaw,thrust) uORB::Publication<actuator_controls_s> _actuators_1_pub{ORB_ID(actuator_controls_1)};
orb_advert_t _mavlink_log_pub{nullptr}; // mavlink log uORB handle uORB::Publication<vehicle_attitude_setpoint_s> _v_att_sp_pub{ORB_ID(vehicle_attitude_setpoint)};
orb_advert_t _v_att_sp_pub{nullptr}; uORB::Publication<vtol_vehicle_status_s> _vtol_vehicle_status_pub{ORB_ID(vtol_vehicle_status)};
orb_advert_t _vtol_vehicle_status_pub{nullptr};
orb_advert_t _actuators_1_pub{nullptr};
//*******************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 _v_att_sp{}; //vehicle attitude setpoint
vehicle_attitude_setpoint_s _fw_virtual_att_sp{}; // virtual fw 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_command_s _vehicle_cmd{};
vehicle_control_mode_s _v_control_mode{}; //vehicle control mode vehicle_control_mode_s _v_control_mode{}; //vehicle control mode
vehicle_land_detected_s _land_detected{}; 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{}; vehicle_local_position_setpoint_s _local_pos_sp{};
vtol_vehicle_status_s _vtol_vehicle_status{}; 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 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 * for fixed wings we want to have an idle speed of zero since we do not want
* to waste energy when gliding. */ * to waste energy when gliding. */
int _transition_command{vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC}; int _transition_command{vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC};
bool _abort_front_transition{false}; 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 perf_counter_t _loop_perf; /**< loop performance counter */
static int task_main_trampoline(int argc, char *argv[]); //Shim for calling task_main from task_create. perf_counter_t _loop_interval_perf; /**< loop interval performance counter */
void vehicle_cmd_poll(); 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 int parameters_update(); //Update local parameter cache
void handle_command(); void handle_command();
}; };
#endif