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
* 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;
}
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 (should_exit()) {
_actuator_inputs_fw.unregister_callback();
_actuator_inputs_mc.unregister_callback();
exit_and_cleanup();
return;
}
if (!_initialized) {
parameters_update(); // initialize parameter cache
_task_should_exit = !_vtol_type->init();
if (_vtol_type->init()) {
_initialized = true;
/* wakeup source*/
px4_pollfd_struct_t fds[1] = {};
fds[0].fd = _actuator_inputs_mc;
fds[0].events = POLLIN;
} else {
exit_and_cleanup();
return;
}
}
while (!_task_should_exit) {
perf_begin(_loop_perf);
perf_count(_loop_interval_perf);
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;
}
return OK;
} else {
PX4_ERR("alloc failed");
}
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);
}

80
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
* modification, are permitted provided that the following conditions
@ -46,26 +46,29 @@
* @author Andreas Antener <andreas@uaventure.com>
*
*/
#ifndef VTOL_ATT_CONTROL_MAIN_H
#define VTOL_ATT_CONTROL_MAIN_H
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_tasks.h>
#include <px4_posix.h>
#pragma once
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.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 <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/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/tecs_status.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
@ -75,25 +78,37 @@
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vtol_vehicle_status.h>
#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<VtolAttitudeControl>, 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<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)};
uORB::Publication<vehicle_attitude_setpoint_s> _v_att_sp_pub{ORB_ID(vehicle_attitude_setpoint)};
uORB::Publication<vtol_vehicle_status_s> _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
@ -206,18 +214,14 @@ private:
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