Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 11:59:17 +08:00
Code Issues Actions 20 Packages Projects Releases Wiki Activity

fw_att_control: move to WQ with uORB callback scheduling

Browse Source
This commit is contained in:
Daniel Agar
2019-08-07 22:54:44 -04:00
committed by GitHub
parent 0955fd2d58
commit cab0aee2a0
2 changed files with 375 additions and 403 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+46 -73
View File
@@ -45,10 +45,9 @@ using namespace time_literals;
extern "C" __EXPORT int fw_att_control_main(int argc, char *argv[]);
FixedwingAttitudeControl::FixedwingAttitudeControl() :
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "fwa_dt")),
_nonfinite_input_perf(perf_alloc(PC_COUNT, "fwa_nani")),
_nonfinite_output_perf(perf_alloc(PC_COUNT, "fwa_nano"))
WorkItem(px4::wq_configurations::att_pos_ctrl),
_loop_perf(perf_alloc(PC_ELAPSED, "fw_att_control: cycle")),
_loop_interval_perf(perf_alloc(PC_INTERVAL, "fw_att_control: interval"))
{
// check if VTOL first
vehicle_status_poll();
@@ -128,18 +127,23 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_pitch_ctrl.set_max_rate_pos(_parameters.acro_max_y_rate_rad);
_pitch_ctrl.set_max_rate_neg(_parameters.acro_max_y_rate_rad);
_yaw_ctrl.set_max_rate(_parameters.acro_max_z_rate_rad);
// subscriptions
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
}
FixedwingAttitudeControl::~FixedwingAttitudeControl()
{
orb_unsubscribe(_att_sub);
perf_free(_loop_perf);
perf_free(_nonfinite_input_perf);
perf_free(_nonfinite_output_perf);
perf_free(_loop_interval_perf);
}
bool
FixedwingAttitudeControl::init()
{
if (!_att_sub.register_callback()) {
PX4_ERR("vehicle attitude callback registration failed!");
return false;
}
return true;
}
int
@@ -280,6 +284,7 @@ FixedwingAttitudeControl::vehicle_manual_poll()
const bool is_fixed_wing = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
if (_vcontrol_mode.flag_control_manual_enabled && (!is_tailsitter_transition || is_fixed_wing)) {
// Always copy the new manual setpoint, even if it wasn't updated, to fill the _actuators with valid values
if (_manual_sub.copy(&_manual)) {
@@ -413,10 +418,6 @@ float FixedwingAttitudeControl::get_airspeed_and_update_scaling()
&& (hrt_elapsed_time(&_airspeed_sub.get().timestamp) < 1_s)
&& !_vehicle_status.aspd_use_inhibit;
if (!airspeed_valid) {
perf_count(_nonfinite_input_perf);
}
// if no airspeed measurement is available out best guess is to use the trim airspeed
float airspeed = _parameters.airspeed_trim;
@@ -444,20 +445,21 @@ float FixedwingAttitudeControl::get_airspeed_and_update_scaling()
const float airspeed_constrained = math::constrain(airspeed, _parameters.airspeed_min, _parameters.airspeed_max);
_airspeed_scaling = _parameters.airspeed_trim / airspeed_constrained;
return airspeed;
}
void FixedwingAttitudeControl::run()
void FixedwingAttitudeControl::Run()
{
/* wakeup source */
px4_pollfd_struct_t fds[1];
if (should_exit()) {
_att_sub.unregister_callback();
exit_and_cleanup();
return;
}
/* Setup of loop */
fds[0].fd = _att_sub;
fds[0].events = POLLIN;
perf_begin(_loop_perf);
perf_count(_loop_interval_perf);
while (!should_exit()) {
if (_att_sub.update(&_att)) {
/* only update parameters if they changed */
bool params_updated = _params_sub.updated();
@@ -471,36 +473,11 @@ void FixedwingAttitudeControl::run()
parameters_update();
}
/* wait for up to 500ms for data */
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit, etc. */
if (pret == 0) {
continue;
}
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
PX4_WARN("poll error %d, %d", pret, errno);
continue;
}
perf_begin(_loop_perf);
/* only run controller if attitude changed */
if (fds[0].revents & POLLIN) {
static uint64_t last_run = 0;
float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
float deltaT = math::constrain((hrt_elapsed_time(&last_run) / 1e6f), 0.01f, 0.1f);
last_run = hrt_absolute_time();
/* guard against too large deltaT's */
if (deltaT > 1.0f) {
deltaT = 0.01f;
}
/* load local copies */
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
/* get current rotation matrix and euler angles from control state quaternions */
matrix::Dcmf R = matrix::Quatf(_att.q);
@@ -580,7 +557,8 @@ void FixedwingAttitudeControl::run()
/* if we are in rotary wing mode, do nothing */
if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.is_vtol) {
continue;
perf_end(_loop_perf);
return;
}
control_flaps(deltaT);
@@ -708,7 +686,6 @@ void FixedwingAttitudeControl::run()
if (!PX4_ISFINITE(roll_u)) {
_roll_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
}
float pitch_u = _pitch_ctrl.control_euler_rate(control_input);
@@ -716,7 +693,6 @@ void FixedwingAttitudeControl::run()
if (!PX4_ISFINITE(pitch_u)) {
_pitch_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
}
float yaw_u = 0.0f;
@@ -738,7 +714,6 @@ void FixedwingAttitudeControl::run()
if (!PX4_ISFINITE(yaw_u)) {
_yaw_ctrl.reset_integrator();
_wheel_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
}
/* throttle passed through if it is finite and if no engine failure was detected */
@@ -761,9 +736,6 @@ void FixedwingAttitudeControl::run()
_actuators.control[actuator_controls_s::INDEX_THROTTLE] *= _battery_scale;
}
} else {
perf_count(_nonfinite_input_perf);
}
/*
@@ -857,7 +829,6 @@ void FixedwingAttitudeControl::run()
}
perf_end(_loop_perf);
}
}
void FixedwingAttitudeControl::control_flaps(const float dt)
@@ -917,26 +888,27 @@ void FixedwingAttitudeControl::control_flaps(const float dt)
}
}
FixedwingAttitudeControl *FixedwingAttitudeControl::instantiate(int argc, char *argv[])
{
return new FixedwingAttitudeControl();
}
int FixedwingAttitudeControl::task_spawn(int argc, char *argv[])
{
_task_id = px4_task_spawn_cmd("fw_att_controol",
SCHED_DEFAULT,
SCHED_PRIORITY_ATTITUDE_CONTROL,
1500,
(px4_main_t)&run_trampoline,
(char *const *)argv);
FixedwingAttitudeControl *instance = new FixedwingAttitudeControl();
if (_task_id < 0) {
_task_id = -1;
return -errno;
if (instance) {
_object.store(instance);
_task_id = task_id_is_work_queue;
if (instance->init()) {
return PX4_OK;
}
return 0;
} else {
PX4_ERR("alloc failed");
}
delete instance;
_object.store(nullptr);
_task_id = -1;
return PX4_ERROR;
}
int FixedwingAttitudeControl::custom_command(int argc, char *argv[])
@@ -970,7 +942,8 @@ int FixedwingAttitudeControl::print_status()
{
PX4_INFO("Running");
// perf?
perf_print_counter(_loop_perf);
perf_print_counter(_loop_interval_perf);
return 0;
}
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
+10 -11
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
@@ -46,7 +46,9 @@
#include <px4_tasks.h>
#include <parameters/param.h>
#include <perf/perf_counter.h>
#include <px4_work_queue/WorkItem.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
@@ -67,7 +69,7 @@ using matrix::Quatf;
using uORB::SubscriptionData;
class FixedwingAttitudeControl final : public ModuleBase<FixedwingAttitudeControl>
class FixedwingAttitudeControl final : public ModuleBase<FixedwingAttitudeControl>, public px4::WorkItem
{
public:
FixedwingAttitudeControl();
@@ -76,24 +78,22 @@ public:
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static FixedwingAttitudeControl *instantiate(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
/** @see ModuleBase::run() */
void run() override;
/** @see ModuleBase::print_status() */
int print_status() override;
void Run() override;
bool init();
private:
int _att_sub{-1}; /**< vehicle attitude */
uORB::SubscriptionCallbackWorkItem _att_sub{this, ORB_ID(vehicle_attitude)}; /**< vehicle attitude */
uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; /**< vehicle attitude setpoint */
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; /**< battery status subscription */
@@ -128,8 +128,7 @@ private:
vehicle_status_s _vehicle_status {}; /**< vehicle status */
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */
perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */
perf_counter_t _loop_interval_perf; /**< loop interval performance counter */
float _flaps_applied{0.0f};
float _flaperons_applied{0.0f};