Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 21:55:34 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

fw_pos_control_l1 move to ModuleBase

Browse Source
This commit is contained in:
Daniel Agar
2018-02-17 11:34:01 -05:00
committed by Lorenz Meier
parent 4aeb70fcfe
commit 458d8f7b02
2 changed files with 83 additions and 148 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+54 -117
View File
@@ -35,10 +35,8 @@
extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]);
FixedwingPositionControl *l1_control::g_control;
static int _control_task = -1; ///< task handle for sensor task */
FixedwingPositionControl::FixedwingPositionControl() :
SuperBlock(nullptr, "FW_POS"),
_sub_airspeed(ORB_ID(airspeed)),
_sub_sensors(ORB_ID(sensor_bias)),
_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control"))
@@ -103,27 +101,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
FixedwingPositionControl::~FixedwingPositionControl()
{
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 */
usleep(20000);
/* if we have given up, kill it */
if (++i > 50) {
px4_task_delete(_control_task);
break;
}
} while (_control_task != -1);
}
l1_control::g_control = nullptr;
perf_free(_loop_perf);
}
int
@@ -388,22 +366,6 @@ FixedwingPositionControl::position_setpoint_triplet_poll()
}
}
void
FixedwingPositionControl::task_main_trampoline(int argc, char *argv[])
{
l1_control::g_control = new FixedwingPositionControl();
if (l1_control::g_control == nullptr) {
PX4_WARN("OUT OF MEM");
return;
}
/* only returns on exit */
l1_control::g_control->task_main();
delete l1_control::g_control;
l1_control::g_control = nullptr;
}
float
FixedwingPositionControl::get_demanded_airspeed()
{
@@ -1491,7 +1453,7 @@ FixedwingPositionControl::handle_command()
}
void
FixedwingPositionControl::task_main()
FixedwingPositionControl::run()
{
/*
* do subscriptions
@@ -1526,8 +1488,7 @@ FixedwingPositionControl::task_main()
/* abort on a nonzero return value from the parameter init */
if (parameters_update() != PX4_OK) {
/* parameter setup went wrong, abort */
PX4_WARN("aborting startup due to errors.");
_task_should_exit = true;
PX4_ERR("aborting startup due to errors.");
}
/* wakeup source(s) */
@@ -1537,9 +1498,7 @@ FixedwingPositionControl::task_main()
fds[0].fd = _global_pos_sub;
fds[0].events = POLLIN;
_task_running = true;
while (!_task_should_exit) {
while (!should_exit()) {
/* wait for up to 500ms for data */
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
@@ -1676,12 +1635,6 @@ FixedwingPositionControl::task_main()
perf_end(_loop_perf);
}
}
_task_running = false;
PX4_WARN("exiting.\n");
_control_task = -1;
}
void
@@ -1910,79 +1863,63 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee
}
}
int
FixedwingPositionControl::start()
FixedwingPositionControl *FixedwingPositionControl::instantiate(int argc, char *argv[])
{
ASSERT(_control_task == -1);
return new FixedwingPositionControl();
}
/* start the task */
_control_task = px4_task_spawn_cmd("fw_pos_ctrl_l1",
SCHED_DEFAULT,
SCHED_PRIORITY_POSITION_CONTROL,
1810,
(px4_main_t)&FixedwingPositionControl::task_main_trampoline,
nullptr);
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);
if (_control_task < 0) {
warn("task start failed");
if (_task_id < 0) {
_task_id = -1;
return -errno;
}
return PX4_OK;
return 0;
}
int FixedwingPositionControl::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
}
int FixedwingPositionControl::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
fw_pos_control_l1 is the fixed wing position controller.
)DESCR_STR");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_NAME("fw_pos_control_l1", "controller");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
int FixedwingPositionControl::print_status()
{
PX4_INFO("Running");
return 0;
}
int fw_pos_control_l1_main(int argc, char *argv[])
{
if (argc < 2) {
PX4_WARN("usage: fw_pos_control_l1 {start|stop|status}");
return 1;
}
if (strcmp(argv[1], "start") == 0) {
if (l1_control::g_control != nullptr) {
PX4_WARN("already running");
return 1;
}
if (OK != FixedwingPositionControl::start()) {
warn("start failed");
return 1;
}
/* avoid memory fragmentation by not exiting start handler until the task has fully started */
while (l1_control::g_control == nullptr || !l1_control::g_control->task_running()) {
usleep(50000);
printf(".");
fflush(stdout);
}
printf("\n");
return 0;
}
if (strcmp(argv[1], "stop") == 0) {
if (l1_control::g_control == nullptr) {
PX4_WARN("not running");
return 1;
}
delete l1_control::g_control;
l1_control::g_control = nullptr;
return 0;
}
if (strcmp(argv[1], "status") == 0) {
if (l1_control::g_control != nullptr) {
PX4_INFO("running");
return 0;
}
PX4_WARN("not running");
return 1;
}
PX4_WARN("unrecognized command");
return 1;
return FixedwingPositionControl::main(argc, argv);
}
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+29 -31
View File
@@ -48,22 +48,24 @@
#ifndef FIXEDWINGPOSITIONCONTROL_HPP_
#define FIXEDWINGPOSITIONCONTROL_HPP_
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_posix.h>
#include <px4_tasks.h>
#include <cfloat>
#include "Landingslope.hpp"
#include "launchdetection/LaunchDetector.h"
#include "runway_takeoff/RunwayTakeoff.h"
#include <cfloat>
#include <controllib/block/BlockParam.hpp>
#include <controllib/blocks.hpp>
#include <drivers/drv_hrt.h>
#include <ecl/l1/ecl_l1_pos_controller.h>
#include <ecl/tecs/tecs.h>
#include <geo/geo.h>
#include <mathlib/mathlib.h>
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_module.h>
#include <px4_posix.h>
#include <px4_tasks.h>
#include <systemlib/perf_counter.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/airspeed.h>
@@ -71,16 +73,16 @@
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/sensor_bias.h>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/sensor_bias.h>
#include <uORB/topics/tecs_status.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/uORB.h>
#include <vtol_att_control/vtol_type.h>
@@ -120,34 +122,35 @@ using uORB::Subscription;
using namespace launchdetection;
using namespace runwaytakeoff;
class FixedwingPositionControl
class FixedwingPositionControl final : public control::SuperBlock, public ModuleBase<FixedwingPositionControl>
{
public:
FixedwingPositionControl();
~FixedwingPositionControl();
~FixedwingPositionControl() override;
FixedwingPositionControl(const FixedwingPositionControl &) = delete;
FixedwingPositionControl operator=(const FixedwingPositionControl &other) = delete;
/**
* Start the sensors task.
*
* @return OK on success.
*/
static int start();
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/**
* Task status
*
* @return true if the mainloop is running
*/
bool task_running() { return _task_running; }
/** @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;
/** @see ModuleBase::print_status() */
int print_status() override;
private:
orb_advert_t _mavlink_log_pub{nullptr};
bool _task_should_exit{false}; ///< if true, sensor task should exit */
bool _task_running{false}; ///< if true, task is running in its mainloop */
int _global_pos_sub{-1};
int _local_pos_sub{-1};
int _pos_sp_triplet_sub{-1};
@@ -474,9 +477,4 @@ private:
};
namespace l1_control
{
extern FixedwingPositionControl *g_control;
} // namespace l1_control
#endif // FIXEDWINGPOSITIONCONTROL_HPP_