fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 05:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_pos_control: move to WQ

Browse Source
This commit is contained in:
Daniel Agar
2019-10-02 19:44:46 -04:00
parent 3c623af903
commit 7baf474940
1 changed files with 80 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+80 -67
View File
@@ -36,20 +36,28 @@
* Multicopter position controller.
*/
#include <commander/px4_custom_mode.h>
#include <drivers/drv_hrt.h>
#include <lib/controllib/blocks.hpp>
#include <lib/FlightTasks/FlightTasks.hpp>
#include <lib/hysteresis/hysteresis.h>
#include <lib/mathlib/mathlib.h>
#include <lib/perf/perf_counter.h>
#include <lib/systemlib/mavlink_log.h>
#include <lib/WeatherVane/WeatherVane.hpp>
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_module_params.h>
#include <px4_tasks.h>
#include <px4_module.h>
#include <px4_module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <px4_posix.h>
#include <drivers/drv_hrt.h>
#include <lib/hysteresis/hysteresis.h>
#include <commander/px4_custom_mode.h>
#include <px4_tasks.h>
#include <uORB/Publication.hpp>
#include <uORB/PublicationQueued.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/home_position.h>
#include <uORB/topics/landing_gear.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
@@ -58,20 +66,13 @@
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_trajectory_waypoint.h>
#include <uORB/topics/landing_gear.h>
#include <float.h>
#include <mathlib/mathlib.h>
#include <systemlib/mavlink_log.h>
#include <controllib/blocks.hpp>
#include <lib/FlightTasks/FlightTasks.hpp>
#include <lib/WeatherVane/WeatherVane.hpp>
#include "PositionControl.hpp"
#include "Utility/ControlMath.hpp"
#include "Takeoff.hpp"
#include <float.h>
using namespace time_literals;
/**
@@ -80,27 +81,25 @@ using namespace time_literals;
extern "C" __EXPORT int mc_pos_control_main(int argc, char *argv[]);
class MulticopterPositionControl : public ModuleBase<MulticopterPositionControl>, public control::SuperBlock,
public ModuleParams
public ModuleParams, public px4::WorkItem
{
public:
MulticopterPositionControl();
~MulticopterPositionControl();
virtual ~MulticopterPositionControl() override;
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static MulticopterPositionControl *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;
void Run() override;
bool init();
/** @see ModuleBase::print_status() */
int print_status() override;
@@ -118,7 +117,7 @@ private:
uORB::Publication<vehicle_local_position_setpoint_s> _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; /**< vehicle local position setpoint publication */
uORB::Publication<vehicle_local_position_setpoint_s> _traj_sp_pub{ORB_ID(trajectory_setpoint)}; /**< trajectory setpoints publication */
int _local_pos_sub{-1}; /**< vehicle local position */
uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)}; /**< vehicle local position */
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
@@ -127,6 +126,8 @@ private:
uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)}; /**< vehicle attitude */
uORB::Subscription _home_pos_sub{ORB_ID(home_position)}; /**< home position */
hrt_abstime _time_stamp_last_loop{0}; /**< time stamp of last loop iteration */
int _task_failure_count{0}; /**< counter for task failures */
vehicle_status_s _vehicle_status{}; /**< vehicle status */
@@ -193,6 +194,9 @@ private:
WeatherVane *_wv_controller{nullptr};
perf_counter_t _cycle_perf;
perf_counter_t _interval_perf;
/**
* Update our local parameter cache.
* Parameter update can be forced when argument is true.
@@ -280,13 +284,17 @@ private:
MulticopterPositionControl::MulticopterPositionControl() :
SuperBlock(nullptr, "MPC"),
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl),
_vel_x_deriv(this, "VELD"),
_vel_y_deriv(this, "VELD"),
_vel_z_deriv(this, "VELD"),
_control(this)
_control(this),
_cycle_perf(perf_alloc_once(PC_ELAPSED, MODULE_NAME": cycle time")),
_interval_perf(perf_alloc_once(PC_INTERVAL, MODULE_NAME": interval"))
{
// fetch initial parameter values
parameters_update(true);
// set failsafe hysteresis
_failsafe_land_hysteresis.set_hysteresis_time_from(false, LOITER_TIME_BEFORE_DESCEND);
}
@@ -296,6 +304,24 @@ MulticopterPositionControl::~MulticopterPositionControl()
if (_wv_controller != nullptr) {
delete _wv_controller;
}
perf_free(_cycle_perf);
perf_free(_interval_perf);
}
bool
MulticopterPositionControl::init()
{
if (!_local_pos_sub.registerCallback()) {
PX4_ERR("vehicle_local_position callback registration failed!");
return false;
}
_local_pos_sub.set_interval_us(20_ms); // 50 Hz max update rate
_time_stamp_last_loop = hrt_absolute_time();
return true;
}
void
@@ -365,9 +391,6 @@ MulticopterPositionControl::parameters_update(bool force)
void
MulticopterPositionControl::poll_subscriptions()
{
// This is polled for, so all we need to do is a copy now.
orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
if (_vehicle_status_sub.update(&_vehicle_status)) {
// set correct uORB ID, depending on if vehicle is VTOL or not
@@ -491,44 +514,33 @@ MulticopterPositionControl::print_status()
PX4_INFO("Running, no flight task active");
}
perf_print_counter(_cycle_perf);
perf_print_counter(_interval_perf);
return 0;
}
void
MulticopterPositionControl::run()
MulticopterPositionControl::Run()
{
hrt_abstime time_stamp_last_loop = hrt_absolute_time(); // time stamp of last loop iteration
if (should_exit()) {
_local_pos_sub.unregisterCallback();
exit_and_cleanup();
return;
}
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
orb_set_interval(_local_pos_sub, 20); // 50 Hz updates
perf_begin(_cycle_perf);
perf_count(_interval_perf);
// get initial values for all parameters and subscribtions
parameters_update(true);
poll_subscriptions();
// setup file descriptor to poll the local position as loop wakeup source
px4_pollfd_struct_t poll_fd = {};
poll_fd.fd = _local_pos_sub;
poll_fd.events = POLLIN;
while (!should_exit()) {
// poll for new data on the local position state topic (wait for up to 20ms)
const int poll_return = px4_poll(&poll_fd, 1, 20);
// poll_return == 0: go through the loop anyway to copy manual input at 50 Hz
// this is undesirable but not much we can do
if (poll_return < 0) {
PX4_ERR("poll error %d %d", poll_return, errno);
continue;
}
if (_local_pos_sub.update(&_local_pos)) {
poll_subscriptions();
parameters_update(false);
// set _dt in controllib Block - the time difference since the last loop iteration in seconds
const hrt_abstime time_stamp_current = hrt_absolute_time();
setDt((time_stamp_current - time_stamp_last_loop) / 1e6f);
time_stamp_last_loop = time_stamp_current;
setDt((time_stamp_current - _time_stamp_last_loop) / 1e6f);
_time_stamp_last_loop = time_stamp_current;
const bool was_in_failsafe = _in_failsafe;
_in_failsafe = false;
@@ -724,7 +736,7 @@ MulticopterPositionControl::run()
}
}
orb_unsubscribe(_local_pos_sub);
perf_end(_cycle_perf);
}
void
@@ -1049,24 +1061,25 @@ void MulticopterPositionControl::send_vehicle_cmd_do(uint8_t nav_state)
int MulticopterPositionControl::task_spawn(int argc, char *argv[])
{
_task_id = px4_task_spawn_cmd("mc_pos_control",
SCHED_DEFAULT,
SCHED_PRIORITY_POSITION_CONTROL,
1900,
(px4_main_t)&run_trampoline,
(char *const *)argv);
MulticopterPositionControl *instance = new MulticopterPositionControl();
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;
}
} else {
PX4_ERR("alloc failed");
}
return 0;
}
delete instance;
_object.store(nullptr);
_task_id = -1;
MulticopterPositionControl *MulticopterPositionControl::instantiate(int argc, char *argv[])
{
return new MulticopterPositionControl();
return PX4_ERROR;
}
int MulticopterPositionControl::custom_command(int argc, char *argv[])