fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-02 03:31:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

very much WIP, start to make mc att control p4 and ros compatible

Browse Source
This commit is contained in:
Thomas Gubler
2014-12-10 12:42:57 +01:00
parent e622430460
commit 03ba38d0a4
1 changed files with 231 additions and 225 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/mc_att_control_main.cpp
+231 -225
View File
@@ -71,17 +71,53 @@
#include "mc_att_control_base.h" #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 * Multicopter attitude control app start / stop handling function
* *
* @ingroup apps * @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 YAW_DEADZONE 0.05f
#define MIN_TAKEOFF_THRUST 0.2f #define MIN_TAKEOFF_THRUST 0.2f
#define RATES_I_LIMIT 0.3f #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 : class MulticopterAttitudeControl :
public MulticopterAttitudeControlBase public MulticopterAttitudeControlBase
{ {
@@ -96,15 +132,10 @@ public:
*/ */
~MulticopterAttitudeControl(); ~MulticopterAttitudeControl();
/**
* Start the sensors task.
*
* @return OK on success.
*/
int start();
void handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg); void handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg);
void spin() { n.spin(); }
private: private:
bool _task_should_exit; /**< if true, sensor task should exit */ bool _task_should_exit; /**< if true, sensor task should exit */
int _control_task; /**< task handle for sensor task */ 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 _v_rates_sp_pub; /**< rate setpoint publication */
orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */ orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */
px4::NodeHandle n;
struct { struct {
param_t roll_p; param_t roll_p;
param_t roll_rate_p; param_t roll_rate_p;
@@ -184,15 +217,6 @@ private:
*/ */
void arming_status_poll(); 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 namespace mc_att_control
@@ -204,7 +228,6 @@ namespace mc_att_control
#endif #endif
static const int ERROR = -1; static const int ERROR = -1;
MulticopterAttitudeControl *g_control;
} }
MulticopterAttitudeControl::MulticopterAttitudeControl() : MulticopterAttitudeControl::MulticopterAttitudeControl() :
@@ -224,6 +247,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_att_sp_pub(-1), _att_sp_pub(-1),
_v_rates_sp_pub(-1), _v_rates_sp_pub(-1),
_actuators_0_pub(-1), _actuators_0_pub(-1),
n(),
/* performance counters */ /* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
@@ -252,6 +276,25 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
/* fetch initial parameter values */ /* fetch initial parameter values */
parameters_update(); 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() MulticopterAttitudeControl::~MulticopterAttitudeControl()
@@ -275,7 +318,7 @@ MulticopterAttitudeControl::~MulticopterAttitudeControl()
} while (_control_task != -1); } while (_control_task != -1);
} }
mc_att_control::g_control = nullptr; // mc_att_control::g_control = nullptr;
} }
int int
@@ -415,200 +458,140 @@ MulticopterAttitudeControl::arming_status_poll()
} }
} }
void // void
MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) // MulticopterAttitudeControl::task_main()
{ // {
mc_att_control::g_control->task_main();
}
void
MulticopterAttitudeControl::task_main()
{
px4::NodeHandle n;
/* // [> wakeup source: vehicle attitude <]
* do subscriptions // struct pollfd fds[1];
*/
_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));
/* initialize parameters cache */ // fds[0].fd = _v_att_sub;
parameters_update(); // fds[0].events = POLLIN;
/* wakeup source: vehicle attitude */ // while (!_task_should_exit) {
struct pollfd fds[1];
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 */ // warnx("exit");
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit */ // _control_task = -1;
if (pret == 0) { // _exit(0);
continue; // }
}
/* 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;
}
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();
/* guard against too small (< 2ms) and too large (> 20ms) dt's */
if (dt < 0.002f) {
dt = 0.002f;
} else if (dt > 0.02f) {
dt = 0.02f;
}
/* copy attitude topic */
orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att);
/* check for updates in other topics */
parameter_update_poll();
vehicle_control_mode_poll();
arming_status_poll();
vehicle_manual_poll();
if (_v_control_mode.flag_control_attitude_enabled) {
control_attitude(dt);
/* publish the attitude setpoint if needed */
if (_publish_att_sp) {
_v_att_sp.timestamp = hrt_absolute_time();
if (_att_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub,
&_v_att_sp);
} else {
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint),
&_v_att_sp);
}
}
/* publish attitude rates setpoint */
_v_rates_sp.roll = _rates_sp(0);
_v_rates_sp.pitch = _rates_sp(1);
_v_rates_sp.yaw = _rates_sp(2);
_v_rates_sp.thrust = _thrust_sp;
_v_rates_sp.timestamp = hrt_absolute_time();
if (_v_rates_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
} else {
_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
}
} else {
/* attitude controller disabled, poll rates setpoint topic */
if (_v_control_mode.flag_control_manual_enabled) {
/* manual rates control - ACRO mode */
_rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x,
_manual_control_sp.r).emult(_params.acro_rate_max);
_thrust_sp = _manual_control_sp.z;
/* reset yaw setpoint after ACRO */
_reset_yaw_sp = true;
/* publish attitude rates setpoint */
_v_rates_sp.roll = _rates_sp(0);
_v_rates_sp.pitch = _rates_sp(1);
_v_rates_sp.yaw = _rates_sp(2);
_v_rates_sp.thrust = _thrust_sp;
_v_rates_sp.timestamp = hrt_absolute_time();
if (_v_rates_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
} else {
_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
}
} else {
/* attitude controller disabled, poll rates setpoint topic */
vehicle_rates_setpoint_poll();
_rates_sp(0) = _v_rates_sp.roll;
_rates_sp(1) = _v_rates_sp.pitch;
_rates_sp(2) = _v_rates_sp.yaw;
_thrust_sp = _v_rates_sp.thrust;
}
}
if (_v_control_mode.flag_control_rates_enabled) {
control_attitude_rates(dt);
/* publish actuator controls */
_actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f;
_actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
_actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
_actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
_actuators.timestamp = hrt_absolute_time();
if (!_actuators_0_circuit_breaker_enabled) {
if (_actuators_0_pub > 0) {
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
} else {
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
}
}
}
}
perf_end(_loop_perf);
}
warnx("exit");
_control_task = -1;
_exit(0);
}
void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg) { void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg) {
PX4_INFO("RCHandler class heard: [%llu]", msg.timestamp);
}
int perf_begin(_loop_perf);
MulticopterAttitudeControl::start()
{
ASSERT(_control_task == -1);
/* start the task */ /* run controller on attitude changes */
_control_task = task_spawn_cmd("mc_att_control", static uint64_t last_run = 0;
SCHED_DEFAULT, float dt = (hrt_absolute_time() - last_run) / 1000000.0f;
SCHED_PRIORITY_MAX - 5, last_run = hrt_absolute_time();
2000,
(main_t)&MulticopterAttitudeControl::task_main_trampoline,
nullptr);
if (_control_task < 0) { /* guard against too small (< 2ms) and too large (> 20ms) dt's */
warn("task start failed"); if (dt < 0.002f) {
return -errno; dt = 0.002f;
} else if (dt > 0.02f) {
dt = 0.02f;
} }
return OK; /* copy attitude topic */
orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att);
/* check for updates in other topics */
parameter_update_poll();
vehicle_control_mode_poll();
arming_status_poll();
vehicle_manual_poll();
if (_v_control_mode.flag_control_attitude_enabled) {
control_attitude(dt);
/* publish the attitude setpoint if needed */
if (_publish_att_sp) {
_v_att_sp.timestamp = hrt_absolute_time();
if (_att_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub,
&_v_att_sp);
} else {
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint),
&_v_att_sp);
}
}
/* publish attitude rates setpoint */
_v_rates_sp.roll = _rates_sp(0);
_v_rates_sp.pitch = _rates_sp(1);
_v_rates_sp.yaw = _rates_sp(2);
_v_rates_sp.thrust = _thrust_sp;
_v_rates_sp.timestamp = hrt_absolute_time();
if (_v_rates_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
} else {
_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
}
} else {
/* attitude controller disabled, poll rates setpoint topic */
if (_v_control_mode.flag_control_manual_enabled) {
/* manual rates control - ACRO mode */
_rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x,
_manual_control_sp.r).emult(_params.acro_rate_max);
_thrust_sp = _manual_control_sp.z;
/* reset yaw setpoint after ACRO */
_reset_yaw_sp = true;
/* publish attitude rates setpoint */
_v_rates_sp.roll = _rates_sp(0);
_v_rates_sp.pitch = _rates_sp(1);
_v_rates_sp.yaw = _rates_sp(2);
_v_rates_sp.thrust = _thrust_sp;
_v_rates_sp.timestamp = hrt_absolute_time();
if (_v_rates_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
} else {
_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
}
} else {
/* attitude controller disabled, poll rates setpoint topic */
vehicle_rates_setpoint_poll();
_rates_sp(0) = _v_rates_sp.roll;
_rates_sp(1) = _v_rates_sp.pitch;
_rates_sp(2) = _v_rates_sp.yaw;
_thrust_sp = _v_rates_sp.thrust;
}
}
if (_v_control_mode.flag_control_rates_enabled) {
control_attitude_rates(dt);
/* publish actuator controls */
_actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f;
_actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
_actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
_actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
_actuators.timestamp = hrt_absolute_time();
if (!_actuators_0_circuit_breaker_enabled) {
if (_actuators_0_pub > 0) {
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
} else {
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
}
}
}
} }
PX4_MAIN_FUNCTION(mc_att_control) PX4_MAIN_FUNCTION(mc_att_control)
@@ -621,44 +604,67 @@ PX4_MAIN_FUNCTION(mc_att_control)
if (!strcmp(argv[1], "start")) { if (!strcmp(argv[1], "start")) {
if (mc_att_control::g_control != nullptr) { if (thread_running) {
errx(1, "already 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) { daemon_task = task_spawn_cmd("mc_att_control",
errx(1, "alloc failed"); SCHED_DEFAULT,
} SCHED_PRIORITY_MAX - 5,
2000,
if (OK != mc_att_control::g_control->start()) { mc_attitude_thread_main,
delete mc_att_control::g_control; (argv) ? (const char **)&argv[2] : (const char **)NULL);
mc_att_control::g_control = nullptr;
err(1, "start failed");
}
exit(0); exit(0);
} }
if (!strcmp(argv[1], "stop")) { // if (!strcmp(argv[1], "stop")) {
if (mc_att_control::g_control == nullptr) { // if (mc_att_control::g_control == nullptr) {
errx(1, "not running"); // errx(1, "not running");
} // }
delete mc_att_control::g_control; // delete mc_att_control::g_control;
mc_att_control::g_control = nullptr; // mc_att_control::g_control = nullptr;
exit(0); // exit(0);
} // }
if (!strcmp(argv[1], "status")) { // if (!strcmp(argv[1], "status")) {
if (mc_att_control::g_control) { // if (mc_att_control::g_control) {
errx(0, "running"); // errx(0, "running");
} else { // } else {
errx(1, "not running"); // errx(1, "not running");
} // }
} // }
warnx("unrecognized command"); warnx("unrecognized command");
return 1; 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;
}