Skip to content

Commit

Permalink
fw_pos_control_l1: move to WQ with uORB callback scheduling
Browse files Browse the repository at this point in the history
  • Loading branch information
@dagar
dagar committed Aug 8, 2019
1 parent cab0aee commit c66fc85
Show file tree
Hide file tree
Showing 2 changed files with 123 additions and 132 deletions.
234 changes: 113 additions & 121 deletions src/modules/fw_pos_control_l1/FixedwingPositionControl.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
Expand Down Expand Up @@ -37,10 +37,15 @@ extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]);

FixedwingPositionControl::FixedwingPositionControl() :
ModuleParams(nullptr),
_loop_perf(perf_alloc(PC_ELAPSED, "fw_l1_control")),
WorkItem(px4::wq_configurations::att_pos_ctrl),
_loop_perf(perf_alloc(PC_ELAPSED, "fw_pos_control_l1: cycle")),
_loop_interval_perf(perf_alloc(PC_INTERVAL, "fw_pos_control_l1: interval")),
_launchDetector(this),
_runway_takeoff(this)
{
// limit to 50 Hz
_global_pos_sub.set_interval_ms(20);

_parameter_handles.l1_period = param_find("FW_L1_PERIOD");
_parameter_handles.l1_damping = param_find("FW_L1_DAMPING");
_parameter_handles.roll_slew_deg_sec = param_find("FW_L1_R_SLEW_MAX");
Expand Down Expand Up @@ -109,6 +114,18 @@ FixedwingPositionControl::FixedwingPositionControl() :
FixedwingPositionControl::~FixedwingPositionControl()
{
perf_free(_loop_perf);
perf_free(_loop_interval_perf);
}

bool
FixedwingPositionControl::init()
{
if (!_global_pos_sub.register_callback()) {
PX4_ERR("vehicle global position callback registration failed!");
return false;
}

return true;
}

int
Expand Down Expand Up @@ -1646,41 +1663,19 @@ FixedwingPositionControl::handle_command()
}

void
FixedwingPositionControl::run()
FixedwingPositionControl::Run()
{
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));

/* rate limit position updates to 50 Hz */
orb_set_interval(_global_pos_sub, 20);

/* abort on a nonzero return value from the parameter init */
if (parameters_update() != PX4_OK) {
/* parameter setup went wrong, abort */
PX4_ERR("aborting startup due to errors.");
if (should_exit()) {
_global_pos_sub.unregister_callback();
exit_and_cleanup();
return;
}

/* wakeup source(s) */
px4_pollfd_struct_t fds[1];

/* Setup of loop */
fds[0].fd = _global_pos_sub;
fds[0].events = POLLIN;
perf_begin(_loop_perf);
perf_count(_loop_interval_perf);

while (!should_exit()) {

/* 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;
}
/* only run controller if position changed */
if (_global_pos_sub.update(&_global_pos)) {

/* only update parameters if they changed */
bool params_updated = _params_sub.updated();
Expand All @@ -1694,94 +1689,87 @@ FixedwingPositionControl::run()
parameters_update();
}

/* only run controller if position changed */
if ((fds[0].revents & POLLIN) != 0) {
perf_begin(_loop_perf);

/* load local copies */
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
_local_pos_sub.update(&_local_pos);
_local_pos_sub.update(&_local_pos);

// handle estimator reset events. we only adjust setpoins for manual modes
if (_control_mode.flag_control_manual_enabled) {
if (_control_mode.flag_control_altitude_enabled && _global_pos.alt_reset_counter != _alt_reset_counter) {
_hold_alt += _global_pos.delta_alt;
// make TECS accept step in altitude and demanded altitude
_tecs.handle_alt_step(_global_pos.delta_alt, _global_pos.alt);
}
// handle estimator reset events. we only adjust setpoins for manual modes
if (_control_mode.flag_control_manual_enabled) {
if (_control_mode.flag_control_altitude_enabled && _global_pos.alt_reset_counter != _alt_reset_counter) {
_hold_alt += _global_pos.delta_alt;
// make TECS accept step in altitude and demanded altitude
_tecs.handle_alt_step(_global_pos.delta_alt, _global_pos.alt);
}

// adjust navigation waypoints in position control mode
if (_control_mode.flag_control_altitude_enabled && _control_mode.flag_control_velocity_enabled
&& _global_pos.lat_lon_reset_counter != _pos_reset_counter) {
// adjust navigation waypoints in position control mode
if (_control_mode.flag_control_altitude_enabled && _control_mode.flag_control_velocity_enabled
&& _global_pos.lat_lon_reset_counter != _pos_reset_counter) {

// reset heading hold flag, which will re-initialise position control
_hdg_hold_enabled = false;
}
// reset heading hold flag, which will re-initialise position control
_hdg_hold_enabled = false;
}
}

// update the reset counters in any case
_alt_reset_counter = _global_pos.alt_reset_counter;
_pos_reset_counter = _global_pos.lat_lon_reset_counter;
// update the reset counters in any case
_alt_reset_counter = _global_pos.alt_reset_counter;
_pos_reset_counter = _global_pos.lat_lon_reset_counter;

airspeed_poll();
_manual_control_sub.update(&_manual);
_pos_sp_triplet_sub.update(&_pos_sp_triplet);
vehicle_attitude_poll();
vehicle_command_poll();
vehicle_control_mode_poll();
_vehicle_land_detected_sub.update(&_vehicle_land_detected);
vehicle_status_poll();
_vehicle_acceleration_sub.update();
_vehicle_rates_sub.update();
airspeed_poll();
_manual_control_sub.update(&_manual);
_pos_sp_triplet_sub.update(&_pos_sp_triplet);
vehicle_attitude_poll();
vehicle_command_poll();
vehicle_control_mode_poll();
_vehicle_land_detected_sub.update(&_vehicle_land_detected);
vehicle_status_poll();
_vehicle_acceleration_sub.update();
_vehicle_rates_sub.update();

Vector2f curr_pos((float)_global_pos.lat, (float)_global_pos.lon);
Vector2f ground_speed(_global_pos.vel_n, _global_pos.vel_e);
Vector2f curr_pos((float)_global_pos.lat, (float)_global_pos.lon);
Vector2f ground_speed(_global_pos.vel_n, _global_pos.vel_e);

/*
* Attempt to control position, on success (= sensors present and not in manual mode),
* publish setpoint.
*/
if (control_position(curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current, _pos_sp_triplet.next)) {
_att_sp.timestamp = hrt_absolute_time();
/*
* Attempt to control position, on success (= sensors present and not in manual mode),
* publish setpoint.
*/
if (control_position(curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current, _pos_sp_triplet.next)) {
_att_sp.timestamp = hrt_absolute_time();

// add attitude setpoint offsets
_att_sp.roll_body += _parameters.rollsp_offset_rad;
_att_sp.pitch_body += _parameters.pitchsp_offset_rad;
// add attitude setpoint offsets
_att_sp.roll_body += _parameters.rollsp_offset_rad;
_att_sp.pitch_body += _parameters.pitchsp_offset_rad;

if (_control_mode.flag_control_manual_enabled) {
_att_sp.roll_body = constrain(_att_sp.roll_body, -_parameters.man_roll_max_rad, _parameters.man_roll_max_rad);
_att_sp.pitch_body = constrain(_att_sp.pitch_body, -_parameters.man_pitch_max_rad, _parameters.man_pitch_max_rad);
}
if (_control_mode.flag_control_manual_enabled) {
_att_sp.roll_body = constrain(_att_sp.roll_body, -_parameters.man_roll_max_rad, _parameters.man_roll_max_rad);
_att_sp.pitch_body = constrain(_att_sp.pitch_body, -_parameters.man_pitch_max_rad, _parameters.man_pitch_max_rad);
}

Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
q.copyTo(_att_sp.q_d);
_att_sp.q_d_valid = true;
Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
q.copyTo(_att_sp.q_d);
_att_sp.q_d_valid = true;

if (!_control_mode.flag_control_offboard_enabled ||
_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_velocity_enabled ||
_control_mode.flag_control_acceleration_enabled) {

/* lazily publish the setpoint only once available */
if (_attitude_sp_pub != nullptr) {
/* publish the attitude setpoint */
orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &_att_sp);

} else if (_attitude_setpoint_id != nullptr) {
/* advertise and publish */
_attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
}

// only publish status in full FW mode
if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
&& !_vehicle_status.in_transition_mode) {
status_publish();
}
if (!_control_mode.flag_control_offboard_enabled ||
_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_velocity_enabled ||
_control_mode.flag_control_acceleration_enabled) {

/* lazily publish the setpoint only once available */
if (_attitude_sp_pub != nullptr) {
/* publish the attitude setpoint */
orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &_att_sp);

} else if (_attitude_setpoint_id != nullptr) {
/* advertise and publish */
_attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
}
}

perf_end(_loop_perf);
// only publish status in full FW mode
if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
&& !_vehicle_status.in_transition_mode) {
status_publish();
}
}
}

perf_end(_loop_perf);
}
}

Expand Down Expand Up @@ -1951,26 +1939,27 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee
tecs_status_publish();
}

FixedwingPositionControl *FixedwingPositionControl::instantiate(int argc, char *argv[])
{
return new FixedwingPositionControl();
}

int FixedwingPositionControl::task_spawn(int argc, char *argv[])
{
_task_id = px4_task_spawn_cmd("fw_pos_control_l1",
SCHED_DEFAULT,
SCHED_PRIORITY_POSITION_CONTROL,
1810,
(px4_main_t)&run_trampoline,
(char *const *)argv);
FixedwingPositionControl *instance = new FixedwingPositionControl();

if (instance) {
_object.store(instance);
_task_id = task_id_is_work_queue;

if (_task_id < 0) {
_task_id = -1;
return -errno;
if (instance->init()) {
return PX4_OK;
}

} else {
PX4_ERR("alloc failed");
}

return 0;
delete instance;
_object.store(nullptr);
_task_id = -1;

return PX4_ERROR;
}

int FixedwingPositionControl::custom_command(int argc, char *argv[])
Expand Down Expand Up @@ -2003,6 +1992,9 @@ int FixedwingPositionControl::print_status()
{
PX4_INFO("Running");

perf_print_counter(_loop_perf);
perf_print_counter(_loop_interval_perf);

return 0;
}

Expand Down
21 changes: 10 additions & 11 deletions src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
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
Expand Down Expand Up @@ -65,8 +65,9 @@
#include <px4_module.h>
#include <px4_module_params.h>
#include <px4_posix.h>
#include <px4_tasks.h>
#include <px4_work_queue/WorkItem.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
Expand Down Expand Up @@ -123,36 +124,33 @@ static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH =
0.85f; ///< a throttle / pitch input above this value leads to the system switching to climbout mode
static constexpr float ALTHOLD_EPV_RESET_THRESH = 5.0f;

class FixedwingPositionControl final : public ModuleBase<FixedwingPositionControl>, public ModuleParams
class FixedwingPositionControl final : public ModuleBase<FixedwingPositionControl>, public ModuleParams,
public px4::WorkItem
{
public:
FixedwingPositionControl();
~FixedwingPositionControl() override;
FixedwingPositionControl(const FixedwingPositionControl &) = delete;
FixedwingPositionControl operator=(const FixedwingPositionControl &other) = delete;

/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);

/** @see ModuleBase */
static FixedwingPositionControl *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;

private:
orb_advert_t _mavlink_log_pub{nullptr};

int _global_pos_sub{-1};
uORB::SubscriptionCallbackWorkItem _global_pos_sub{this, ORB_ID(vehicle_global_position)};

uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; ///< control mode subscription */
uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)};
Expand Down Expand Up @@ -188,6 +186,7 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
SubscriptionData<vehicle_acceleration_s> _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};

perf_counter_t _loop_perf; ///< loop performance counter */
perf_counter_t _loop_interval_perf; ///< loop interval performance counter */

float _hold_alt{0.0f}; ///< hold altitude for altitude mode */
float _takeoff_ground_alt{0.0f}; ///< ground altitude at which plane was launched */
Expand Down

0 comments on commit c66fc85

Please sign in to comment.