forked from Archive/PX4-Autopilot
very much WIP, start to make mc att control p4 and ros compatible
This commit is contained in:
Thomas Gubler
parent
e622430460
commit
03ba38d0a4
|
|
@ -71,17 +71,53 @@
|
|||
|
||||
#include "mc_att_control_base.h"
|
||||
|
||||
static bool thread_running = false; /**< Deamon status flag */
|
||||
static int daemon_task; /**< Handle of deamon task / thread */
|
||||
|
||||
using namespace px4;
|
||||
|
||||
/**
|
||||
* Multicopter attitude control app start / stop handling function
|
||||
*
|
||||
* @ingroup apps
|
||||
*/
|
||||
extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]);
|
||||
|
||||
PX4_MAIN_FUNCTION(mc_att_control);
|
||||
|
||||
int mc_attitude_thread_main(int argc, char *argv[]);
|
||||
|
||||
#define YAW_DEADZONE 0.05f
|
||||
#define MIN_TAKEOFF_THRUST 0.2f
|
||||
#define RATES_I_LIMIT 0.3f
|
||||
|
||||
void handle_vehicle_attitude2(const PX4_TOPIC_T(rc_channels) &msg) {
|
||||
PX4_INFO("RCHandler class heard: [%llu]", msg.timestamp);
|
||||
}
|
||||
|
||||
|
||||
namespace px4
|
||||
{
|
||||
bool task_should_exit = false;
|
||||
}
|
||||
|
||||
// PX4_MAIN_FUNCTION(mc_att_control) {
|
||||
// px4::init(argc, argv, "listener");
|
||||
|
||||
// px4::NodeHandle n;
|
||||
|
||||
// PX4_SUBSCRIBE(n, rc_channels, handle_vehicle_attitude2, 1000);
|
||||
|
||||
/**
|
||||
* px4::spin() will enter a loop, pumping callbacks. With this version, all
|
||||
* callbacks will be called from within this thread (the main one). px4::spin()
|
||||
* will exit when Ctrl-C is pressed, or the node is shutdown by the master.
|
||||
*/
|
||||
// n.spin();
|
||||
// PX4_INFO("finished, returning");
|
||||
|
||||
// return 0;
|
||||
// }
|
||||
|
||||
class MulticopterAttitudeControl :
|
||||
public MulticopterAttitudeControlBase
|
||||
{
|
||||
|
|
@ -96,15 +132,10 @@ public:
|
|||
*/
|
||||
~MulticopterAttitudeControl();
|
||||
|
||||
/**
|
||||
* Start the sensors task.
|
||||
*
|
||||
* @return OK on success.
|
||||
*/
|
||||
int start();
|
||||
|
||||
void handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg);
|
||||
|
||||
void spin() { n.spin(); }
|
||||
|
||||
private:
|
||||
bool _task_should_exit; /**< if true, sensor task should exit */
|
||||
int _control_task; /**< task handle for sensor task */
|
||||
|
|
@ -123,6 +154,8 @@ private:
|
|||
orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */
|
||||
orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */
|
||||
|
||||
px4::NodeHandle n;
|
||||
|
||||
struct {
|
||||
param_t roll_p;
|
||||
param_t roll_rate_p;
|
||||
|
|
@ -184,15 +217,6 @@ private:
|
|||
*/
|
||||
void arming_status_poll();
|
||||
|
||||
/**
|
||||
* Shim for calling task_main from task_create.
|
||||
*/
|
||||
static void task_main_trampoline(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Main attitude control task.
|
||||
*/
|
||||
void task_main();
|
||||
};
|
||||
|
||||
namespace mc_att_control
|
||||
|
|
@ -204,7 +228,6 @@ namespace mc_att_control
|
|||
#endif
|
||||
static const int ERROR = -1;
|
||||
|
||||
MulticopterAttitudeControl *g_control;
|
||||
}
|
||||
|
||||
MulticopterAttitudeControl::MulticopterAttitudeControl() :
|
||||
|
|
@ -224,6 +247,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
|
|||
_att_sp_pub(-1),
|
||||
_v_rates_sp_pub(-1),
|
||||
_actuators_0_pub(-1),
|
||||
n(),
|
||||
|
||||
/* performance counters */
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
|
||||
|
|
@ -252,6 +276,25 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
|
|||
|
||||
/* fetch initial parameter values */
|
||||
parameters_update();
|
||||
|
||||
/*
|
||||
* do subscriptions
|
||||
*/
|
||||
// _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
||||
PX4_SUBSCRIBE(n, vehicle_attitude, MulticopterAttitudeControl::handle_vehicle_attitude, this, 0);
|
||||
// _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
|
||||
PX4_SUBSCRIBE(n, vehicle_attitude_setpoint, 0);
|
||||
// _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
|
||||
PX4_SUBSCRIBE(n, vehicle_rates_setpoint, 0);
|
||||
// _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
|
||||
PX4_SUBSCRIBE(n, vehicle_control_mode, 0);
|
||||
// _params_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
PX4_SUBSCRIBE(n, parameter_update, 0);
|
||||
// _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||
PX4_SUBSCRIBE(n, manual_control_setpoint, 0);
|
||||
// _armed_sub = orb_subscribe(ORB_ID(actuator_armed));
|
||||
PX4_SUBSCRIBE(n, actuator_armed, 0);
|
||||
|
||||
}
|
||||
|
||||
MulticopterAttitudeControl::~MulticopterAttitudeControl()
|
||||
|
|
@ -275,7 +318,7 @@ MulticopterAttitudeControl::~MulticopterAttitudeControl()
|
|||
} while (_control_task != -1);
|
||||
}
|
||||
|
||||
mc_att_control::g_control = nullptr;
|
||||
// mc_att_control::g_control = nullptr;
|
||||
}
|
||||
|
||||
int
|
||||
|
|
@ -415,60 +458,34 @@ MulticopterAttitudeControl::arming_status_poll()
|
|||
}
|
||||
}
|
||||
|
||||
void
|
||||
MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[])
|
||||
{
|
||||
mc_att_control::g_control->task_main();
|
||||
}
|
||||
// void
|
||||
// MulticopterAttitudeControl::task_main()
|
||||
// {
|
||||
|
||||
void
|
||||
MulticopterAttitudeControl::task_main()
|
||||
{
|
||||
px4::NodeHandle n;
|
||||
|
||||
/*
|
||||
* do subscriptions
|
||||
*/
|
||||
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
|
||||
// PX4_SUBSCRIBE(n, vehicle_attitude_setpoint, MulticopterAttitudeControl::handle_vehicle_attitude, this 0);
|
||||
_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
|
||||
_v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
||||
_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
|
||||
_params_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
|
||||
// [> wakeup source: vehicle attitude <]
|
||||
// struct pollfd fds[1];
|
||||
|
||||
/* initialize parameters cache */
|
||||
parameters_update();
|
||||
// fds[0].fd = _v_att_sub;
|
||||
// fds[0].events = POLLIN;
|
||||
|
||||
/* wakeup source: vehicle attitude */
|
||||
struct pollfd fds[1];
|
||||
// while (!_task_should_exit) {
|
||||
|
||||
fds[0].fd = _v_att_sub;
|
||||
fds[0].events = POLLIN;
|
||||
|
||||
while (!_task_should_exit) {
|
||||
// perf_end(_loop_perf);
|
||||
// }
|
||||
|
||||
/* wait for up to 100ms for data */
|
||||
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
|
||||
// warnx("exit");
|
||||
|
||||
/* timed out - periodic check for _task_should_exit */
|
||||
if (pret == 0) {
|
||||
continue;
|
||||
}
|
||||
// _control_task = -1;
|
||||
// _exit(0);
|
||||
// }
|
||||
|
||||
/* this is undesirable but not much we can do - might want to flag unhappy status */
|
||||
if (pret < 0) {
|
||||
warn("poll error %d, %d", pret, errno);
|
||||
/* sleep a bit before next try */
|
||||
usleep(100000);
|
||||
continue;
|
||||
}
|
||||
void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg) {
|
||||
|
||||
perf_begin(_loop_perf);
|
||||
|
||||
/* run controller on attitude changes */
|
||||
if (fds[0].revents & POLLIN) {
|
||||
static uint64_t last_run = 0;
|
||||
float dt = (hrt_absolute_time() - last_run) / 1000000.0f;
|
||||
last_run = hrt_absolute_time();
|
||||
|
|
@ -575,40 +592,6 @@ MulticopterAttitudeControl::task_main()
|
|||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
perf_end(_loop_perf);
|
||||
}
|
||||
|
||||
warnx("exit");
|
||||
|
||||
_control_task = -1;
|
||||
_exit(0);
|
||||
}
|
||||
|
||||
void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg) {
|
||||
PX4_INFO("RCHandler class heard: [%llu]", msg.timestamp);
|
||||
}
|
||||
|
||||
int
|
||||
MulticopterAttitudeControl::start()
|
||||
{
|
||||
ASSERT(_control_task == -1);
|
||||
|
||||
/* start the task */
|
||||
_control_task = task_spawn_cmd("mc_att_control",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
2000,
|
||||
(main_t)&MulticopterAttitudeControl::task_main_trampoline,
|
||||
nullptr);
|
||||
|
||||
if (_control_task < 0) {
|
||||
warn("task start failed");
|
||||
return -errno;
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
PX4_MAIN_FUNCTION(mc_att_control)
|
||||
|
|
@ -621,44 +604,67 @@ PX4_MAIN_FUNCTION(mc_att_control)
|
|||
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
if (mc_att_control::g_control != nullptr) {
|
||||
errx(1, "already running");
|
||||
if (thread_running) {
|
||||
warnx("already running");
|
||||
/* this is not an error */
|
||||
exit(0);
|
||||
}
|
||||
|
||||
mc_att_control::g_control = new MulticopterAttitudeControl;
|
||||
task_should_exit = false;
|
||||
|
||||
if (mc_att_control::g_control == nullptr) {
|
||||
errx(1, "alloc failed");
|
||||
}
|
||||
|
||||
if (OK != mc_att_control::g_control->start()) {
|
||||
delete mc_att_control::g_control;
|
||||
mc_att_control::g_control = nullptr;
|
||||
err(1, "start failed");
|
||||
}
|
||||
daemon_task = task_spawn_cmd("mc_att_control",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
2000,
|
||||
mc_attitude_thread_main,
|
||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
if (mc_att_control::g_control == nullptr) {
|
||||
errx(1, "not running");
|
||||
}
|
||||
// if (!strcmp(argv[1], "stop")) {
|
||||
// if (mc_att_control::g_control == nullptr) {
|
||||
// errx(1, "not running");
|
||||
// }
|
||||
|
||||
delete mc_att_control::g_control;
|
||||
mc_att_control::g_control = nullptr;
|
||||
exit(0);
|
||||
}
|
||||
// delete mc_att_control::g_control;
|
||||
// mc_att_control::g_control = nullptr;
|
||||
// exit(0);
|
||||
// }
|
||||
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
if (mc_att_control::g_control) {
|
||||
errx(0, "running");
|
||||
// if (!strcmp(argv[1], "status")) {
|
||||
// if (mc_att_control::g_control) {
|
||||
// errx(0, "running");
|
||||
|
||||
} else {
|
||||
errx(1, "not running");
|
||||
}
|
||||
}
|
||||
// } else {
|
||||
// errx(1, "not running");
|
||||
// }
|
||||
// }
|
||||
|
||||
warnx("unrecognized command");
|
||||
return 1;
|
||||
}
|
||||
|
||||
int mc_attitude_thread_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
warnx("starting");
|
||||
|
||||
MulticopterAttitudeControl attctl;
|
||||
|
||||
thread_running = true;
|
||||
|
||||
attctl.spin();
|
||||
|
||||
// while (!task_should_exit) {
|
||||
// attctl.update();
|
||||
// }
|
||||
|
||||
warnx("exiting.");
|
||||
|
||||
thread_running = false;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue