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

fw_pos_control_l1: move to WQ with uORB callback scheduling

This commit is contained in:
Daniel Agar 2019-08-07 23:13:21 -04:00 committed by GitHub
parent cab0aee2a0
commit c66fc85630
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 132 additions and 141 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

252
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 * 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
@ -37,10 +37,15 @@ extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]);
FixedwingPositionControl::FixedwingPositionControl() : FixedwingPositionControl::FixedwingPositionControl() :
ModuleParams(nullptr), 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), _launchDetector(this),
_runway_takeoff(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_period = param_find("FW_L1_PERIOD");
_parameter_handles.l1_damping = param_find("FW_L1_DAMPING"); _parameter_handles.l1_damping = param_find("FW_L1_DAMPING");
_parameter_handles.roll_slew_deg_sec = param_find("FW_L1_R_SLEW_MAX"); _parameter_handles.roll_slew_deg_sec = param_find("FW_L1_R_SLEW_MAX");
@ -109,6 +114,18 @@ FixedwingPositionControl::FixedwingPositionControl() :
FixedwingPositionControl::~FixedwingPositionControl() FixedwingPositionControl::~FixedwingPositionControl()
{ {
perf_free(_loop_perf); 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 int
@ -1646,41 +1663,19 @@ FixedwingPositionControl::handle_command()
} }
void void
FixedwingPositionControl::run() FixedwingPositionControl::Run()
{ {
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); if (should_exit()) {
_global_pos_sub.unregister_callback();
/* rate limit position updates to 50 Hz */ exit_and_cleanup();
orb_set_interval(_global_pos_sub, 20); return;
/* 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.");
} }
/* wakeup source(s) */ perf_begin(_loop_perf);
px4_pollfd_struct_t fds[1]; perf_count(_loop_interval_perf);
/* Setup of loop */ /* only run controller if position changed */
fds[0].fd = _global_pos_sub; if (_global_pos_sub.update(&_global_pos)) {
fds[0].events = POLLIN;
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 update parameters if they changed */ /* only update parameters if they changed */
bool params_updated = _params_sub.updated(); bool params_updated = _params_sub.updated();
@ -1694,94 +1689,87 @@ FixedwingPositionControl::run()
parameters_update(); parameters_update();
} }
/* only run controller if position changed */ _local_pos_sub.update(&_local_pos);
if ((fds[0].revents & POLLIN) != 0) {
perf_begin(_loop_perf);
/* load local copies */ // handle estimator reset events. we only adjust setpoins for manual modes
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); if (_control_mode.flag_control_manual_enabled) {
_local_pos_sub.update(&_local_pos); if (_control_mode.flag_control_altitude_enabled && _global_pos.alt_reset_counter != _alt_reset_counter) {
_hold_alt += _global_pos.delta_alt;
// handle estimator reset events. we only adjust setpoins for manual modes // make TECS accept step in altitude and demanded altitude
if (_control_mode.flag_control_manual_enabled) { _tecs.handle_alt_step(_global_pos.delta_alt, _global_pos.alt);
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) {
// reset heading hold flag, which will re-initialise position control
_hdg_hold_enabled = false;
}
} }
// update the reset counters in any case // adjust navigation waypoints in position control mode
_alt_reset_counter = _global_pos.alt_reset_counter; if (_control_mode.flag_control_altitude_enabled && _control_mode.flag_control_velocity_enabled
_pos_reset_counter = _global_pos.lat_lon_reset_counter; && _global_pos.lat_lon_reset_counter != _pos_reset_counter) {
airspeed_poll(); // reset heading hold flag, which will re-initialise position control
_manual_control_sub.update(&_manual); _hdg_hold_enabled = false;
_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);
/*
* 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;
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;
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();
}
}
} }
perf_end(_loop_perf);
} }
// 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();
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();
// 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);
}
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();
}
}
}
perf_end(_loop_perf);
} }
} }
@ -1951,26 +1939,27 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee
tecs_status_publish(); tecs_status_publish();
} }
FixedwingPositionControl *FixedwingPositionControl::instantiate(int argc, char *argv[])
{
return new FixedwingPositionControl();
}
int FixedwingPositionControl::task_spawn(int argc, char *argv[]) int FixedwingPositionControl::task_spawn(int argc, char *argv[])
{ {
_task_id = px4_task_spawn_cmd("fw_pos_control_l1", FixedwingPositionControl *instance = new FixedwingPositionControl();
SCHED_DEFAULT,
SCHED_PRIORITY_POSITION_CONTROL,
1810,
(px4_main_t)&run_trampoline,
(char *const *)argv);
if (_task_id < 0) { if (instance) {
_task_id = -1; _object.store(instance);
return -errno; _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;
return PX4_ERROR;
} }
int FixedwingPositionControl::custom_command(int argc, char *argv[]) int FixedwingPositionControl::custom_command(int argc, char *argv[])
@ -2003,6 +1992,9 @@ int FixedwingPositionControl::print_status()
{ {
PX4_INFO("Running"); PX4_INFO("Running");
perf_print_counter(_loop_perf);
perf_print_counter(_loop_interval_perf);
return 0; return 0;
} }

21
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 * 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
@ -65,8 +65,9 @@
#include <px4_module.h> #include <px4_module.h>
#include <px4_module_params.h> #include <px4_module_params.h>
#include <px4_posix.h> #include <px4_posix.h>
#include <px4_tasks.h> #include <px4_work_queue/WorkItem.hpp>
#include <uORB/Subscription.hpp> #include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#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>
@ -123,28 +124,25 @@ static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH =
0.85f; ///< a throttle / pitch input above this value leads to the system switching to climbout mode 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; 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: public:
FixedwingPositionControl(); FixedwingPositionControl();
~FixedwingPositionControl() override; ~FixedwingPositionControl() override;
FixedwingPositionControl(const FixedwingPositionControl &) = delete;
FixedwingPositionControl operator=(const FixedwingPositionControl &other) = delete;
/** @see ModuleBase */ /** @see ModuleBase */
static int task_spawn(int argc, char *argv[]); static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static FixedwingPositionControl *instantiate(int argc, char *argv[]);
/** @see ModuleBase */ /** @see ModuleBase */
static int custom_command(int argc, char *argv[]); static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */ /** @see ModuleBase */
static int print_usage(const char *reason = nullptr); static int print_usage(const char *reason = nullptr);
/** @see ModuleBase::run() */ void Run() override;
void run() override;
bool init();
/** @see ModuleBase::print_status() */ /** @see ModuleBase::print_status() */
int print_status() override; int print_status() override;
@ -152,7 +150,7 @@ public:
private: private:
orb_advert_t _mavlink_log_pub{nullptr}; 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 _control_mode_sub{ORB_ID(vehicle_control_mode)}; ///< control mode subscription */
uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)};
@ -188,6 +186,7 @@ private:
SubscriptionData<vehicle_acceleration_s> _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; SubscriptionData<vehicle_acceleration_s> _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
perf_counter_t _loop_perf; ///< loop performance counter */ 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 _hold_alt{0.0f}; ///< hold altitude for altitude mode */
float _takeoff_ground_alt{0.0f}; ///< ground altitude at which plane was launched */ float _takeoff_ground_alt{0.0f}; ///< ground altitude at which plane was launched */