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

Merge pull request #2570 from wingtra/ros_multiplatform_in_posix 

Ros multiplatform in posix
This commit is contained in:
Lorenz Meier 2015-07-16 19:44:05 +02:00
parent 5a2ed4a476 2bcbda49ac
commit 620108bc9b
10 changed files with 88 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
makefiles/posix/config_posix_sitl.mk
View File

@ -46,6 +46,8 @@ MODULES += modules/position_estimator_inav
MODULES += modules/navigator
MODULES += modules/mc_pos_control
MODULES += modules/mc_att_control
MODULES += modules/mc_pos_control_multiplatform
MODULES += modules/mc_att_control_multiplatform
MODULES += modules/land_detector
#

14
src/modules/mc_att_control_multiplatform/mc_att_control.cpp
View File

@ -62,7 +62,7 @@ static const int ERROR = -1;
}
MulticopterAttitudeControl::MulticopterAttitudeControl() :
MulticopterAttitudeControlMultiplatform::MulticopterAttitudeControlMultiplatform() :
MulticopterAttitudeControlBase(),
_task_should_exit(false),
_actuators_0_circuit_breaker_enabled(false),
@ -104,23 +104,23 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
/*
* do subscriptions
*/
_v_att = _n.subscribe<px4_vehicle_attitude>(&MulticopterAttitudeControl::handle_vehicle_attitude, this, 0);
_v_att = _n.subscribe<px4_vehicle_attitude>(&MulticopterAttitudeControlMultiplatform::handle_vehicle_attitude, this, 0);
_v_att_sp = _n.subscribe<px4_vehicle_attitude_setpoint>(0);
_v_rates_sp = _n.subscribe<px4_vehicle_rates_setpoint>(0);
_v_control_mode = _n.subscribe<px4_vehicle_control_mode>(0);
_parameter_update = _n.subscribe<px4_parameter_update>(
&MulticopterAttitudeControl::handle_parameter_update, this, 1000);
&MulticopterAttitudeControlMultiplatform::handle_parameter_update, this, 1000);
_manual_control_sp = _n.subscribe<px4_manual_control_setpoint>(0);
_armed = _n.subscribe<px4_actuator_armed>(0);
_v_status = _n.subscribe<px4_vehicle_status>(0);
}
MulticopterAttitudeControl::~MulticopterAttitudeControl()
MulticopterAttitudeControlMultiplatform::~MulticopterAttitudeControlMultiplatform()
{
}
int
MulticopterAttitudeControl::parameters_update()
MulticopterAttitudeControlMultiplatform::parameters_update()
{
/* roll gains */
_params.att_p(0) = _params_handles.roll_p.update();
@ -153,12 +153,12 @@ MulticopterAttitudeControl::parameters_update()
return OK;
}
void MulticopterAttitudeControl::handle_parameter_update(const px4_parameter_update &msg)
void MulticopterAttitudeControlMultiplatform::handle_parameter_update(const px4_parameter_update &msg)
{
parameters_update();
}
void MulticopterAttitudeControl::handle_vehicle_attitude(const px4_vehicle_attitude &msg) {
void MulticopterAttitudeControlMultiplatform::handle_vehicle_attitude(const px4_vehicle_attitude &msg) {
perf_begin(_loop_perf);

6
src/modules/mc_att_control_multiplatform/mc_att_control.h
View File

@ -64,19 +64,19 @@
#include "mc_att_control_base.h"
class MulticopterAttitudeControl :
class MulticopterAttitudeControlMultiplatform :
public MulticopterAttitudeControlBase
{
public:
/**
* Constructor
*/
MulticopterAttitudeControl();
MulticopterAttitudeControlMultiplatform();
/**
* Destructor, also kills the sensors task.
*/
~MulticopterAttitudeControl();
~MulticopterAttitudeControlMultiplatform();
/* Callbacks for topics */
void handle_vehicle_attitude(const px4_vehicle_attitude &msg);

13
src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp
View File

@ -54,19 +54,24 @@
#include "mc_att_control.h"
bool thread_running = false; /**< Deamon status flag */
bool mc_att_control_thread_running = false; /**< Deamon status flag */
#if defined(__PX4_ROS)
int main(int argc, char **argv)
#else
int mc_att_control_start_main(int argc, char **argv); // Prototype for missing declearation error with nuttx
int mc_att_control_start_main(int argc, char **argv)
#endif
{
px4::init(argc, argv, "mc_att_control_m");
PX4_INFO("starting");
MulticopterAttitudeControl attctl;
thread_running = true;
MulticopterAttitudeControlMultiplatform attctl;
mc_att_control_thread_running = true;
attctl.spin();
PX4_INFO("exiting.");
thread_running = false;
mc_att_control_thread_running = false;
return 0;
}

33
src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp
View File

@ -41,57 +41,58 @@
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
extern bool thread_running;
int daemon_task; /**< Handle of deamon task / thread */
extern bool mc_att_control_thread_running;
int mc_att_control_daemon_task; /**< Handle of deamon task / thread */
namespace px4
{
bool task_should_exit = false;
bool mc_att_control_task_should_exit = false;
}
using namespace px4;
extern int main(int argc, char **argv);
extern int mc_att_control_start_main(int argc, char **argv);
extern "C" __EXPORT int mc_att_control_m_main(int argc, char *argv[]);
int mc_att_control_m_main(int argc, char *argv[])
{
if (argc < 2) {
errx(1, "usage: mc_att_control_m {start|stop|status}");
warnx("usage: mc_att_control_m {start|stop|status}");
return 1;
}
if (!strcmp(argv[1], "start")) {
if (thread_running) {
if (mc_att_control_thread_running) {
warnx("already running");
/* this is not an error */
exit(0);
return 0;
}
task_should_exit = false;
daemon_task = px4_task_spawn_cmd("mc_att_control_m",
mc_att_control_task_should_exit = false;
warnx("ok now btak running");
mc_att_control_daemon_task = px4_task_spawn_cmd("mc_att_control_m",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
1900,
main,
mc_att_control_start_main,
(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
exit(0);
return 0;
}
if (!strcmp(argv[1], "stop")) {
task_should_exit = true;
exit(0);
mc_att_control_task_should_exit = true;
return 0;
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
if (mc_att_control_thread_running) {
warnx("is running");
} else {
warnx("not started");
}
exit(0);
return 0;
}
warnx("unrecognized command");

36
src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
View File

@ -49,7 +49,7 @@
#define SIGMA 0.000001f
#define MIN_DIST 0.01f
MulticopterPositionControl::MulticopterPositionControl() :
MulticopterPositionControlMultiplatform::MulticopterPositionControlMultiplatform() :
_task_should_exit(false),
_control_task(-1),
@ -105,14 +105,14 @@ MulticopterPositionControl::MulticopterPositionControl() :
/*
* Do subscriptions
*/
_att = _n.subscribe<px4_vehicle_attitude>(&MulticopterPositionControl::handle_vehicle_attitude, this, 0);
_att = _n.subscribe<px4_vehicle_attitude>(&MulticopterPositionControlMultiplatform::handle_vehicle_attitude, this, 0);
_control_mode = _n.subscribe<px4_vehicle_control_mode>(0);
_parameter_update = _n.subscribe<px4_parameter_update>(
&MulticopterPositionControl::handle_parameter_update, this, 1000);
&MulticopterPositionControlMultiplatform::handle_parameter_update, this, 1000);
_manual_control_sp = _n.subscribe<px4_manual_control_setpoint>(0);
_armed = _n.subscribe<px4_actuator_armed>(0);
_local_pos = _n.subscribe<px4_vehicle_local_position>(0);
_pos_sp_triplet = _n.subscribe<px4_position_setpoint_triplet>(&MulticopterPositionControl::handle_position_setpoint_triplet, this, 0);
_pos_sp_triplet = _n.subscribe<px4_position_setpoint_triplet>(&MulticopterPositionControlMultiplatform::handle_position_setpoint_triplet, this, 0);
_local_pos_sp = _n.subscribe<px4_vehicle_local_position_setpoint>(0);
_global_vel_sp = _n.subscribe<px4_vehicle_global_velocity_setpoint>(0);
@ -139,12 +139,12 @@ MulticopterPositionControl::MulticopterPositionControl() :
_R.identity();
}
MulticopterPositionControl::~MulticopterPositionControl()
MulticopterPositionControlMultiplatform::~MulticopterPositionControlMultiplatform()
{
}
int
MulticopterPositionControl::parameters_update()
MulticopterPositionControlMultiplatform::parameters_update()
{
_params.thr_min = _params_handles.thr_min.update();
_params.thr_max = _params_handles.thr_max.update();
@ -180,7 +180,7 @@ MulticopterPositionControl::parameters_update()
float
MulticopterPositionControl::scale_control(float ctl, float end, float dz)
MulticopterPositionControlMultiplatform::scale_control(float ctl, float end, float dz)
{
if (ctl > dz) {
return (ctl - dz) / (end - dz);
@ -194,7 +194,7 @@ MulticopterPositionControl::scale_control(float ctl, float end, float dz)
}
void
MulticopterPositionControl::update_ref()
MulticopterPositionControlMultiplatform::update_ref()
{
if (_local_pos->data().ref_timestamp != _ref_timestamp) {
double lat_sp, lon_sp;
@ -221,7 +221,7 @@ MulticopterPositionControl::update_ref()
}
void
MulticopterPositionControl::reset_pos_sp()
MulticopterPositionControlMultiplatform::reset_pos_sp()
{
if (_reset_pos_sp) {
_reset_pos_sp = false;
@ -238,7 +238,7 @@ MulticopterPositionControl::reset_pos_sp()
}
void
MulticopterPositionControl::reset_alt_sp()
MulticopterPositionControlMultiplatform::reset_alt_sp()
{
if (_reset_alt_sp) {
_reset_alt_sp = false;
@ -255,7 +255,7 @@ MulticopterPositionControl::reset_alt_sp()
}
void
MulticopterPositionControl::limit_pos_sp_offset()
MulticopterPositionControlMultiplatform::limit_pos_sp_offset()
{
math::Vector<3> pos_sp_offs;
pos_sp_offs.zero();
@ -278,7 +278,7 @@ MulticopterPositionControl::limit_pos_sp_offset()
}
void
MulticopterPositionControl::control_manual(float dt)
MulticopterPositionControlMultiplatform::control_manual(float dt)
{
_sp_move_rate.zero();
@ -343,7 +343,7 @@ MulticopterPositionControl::control_manual(float dt)
}
void
MulticopterPositionControl::control_offboard(float dt)
MulticopterPositionControlMultiplatform::control_offboard(float dt)
{
if (_pos_sp_triplet->data().current.valid) {
if (_control_mode->data().flag_control_position_enabled && _pos_sp_triplet->data().current.position_valid) {
@ -390,7 +390,7 @@ MulticopterPositionControl::control_offboard(float dt)
}
bool
MulticopterPositionControl::cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r,
MulticopterPositionControlMultiplatform::cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r,
const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res)
{
/* project center of sphere on line */
@ -415,7 +415,7 @@ MulticopterPositionControl::cross_sphere_line(const math::Vector<3>& sphere_c, f
}
void
MulticopterPositionControl::control_auto(float dt)
MulticopterPositionControlMultiplatform::control_auto(float dt)
{
if (!_mode_auto) {
_mode_auto = true;
@ -546,12 +546,12 @@ MulticopterPositionControl::control_auto(float dt)
}
}
void MulticopterPositionControl::handle_parameter_update(const px4_parameter_update &msg)
void MulticopterPositionControlMultiplatform::handle_parameter_update(const px4_parameter_update &msg)
{
parameters_update();
}
void MulticopterPositionControl::handle_position_setpoint_triplet(const px4_position_setpoint_triplet &msg)
void MulticopterPositionControlMultiplatform::handle_position_setpoint_triplet(const px4_position_setpoint_triplet &msg)
{
/* Make sure that the position setpoint is valid */
if (!PX4_ISFINITE(_pos_sp_triplet->data().current.lat) ||
@ -561,7 +561,7 @@ void MulticopterPositionControl::handle_position_setpoint_triplet(const px4_posi
}
}
void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_attitude &msg)
void MulticopterPositionControlMultiplatform::handle_vehicle_attitude(const px4_vehicle_attitude &msg)
{
static bool reset_int_z = true;
static bool reset_int_z_manual = false;

6
src/modules/mc_pos_control_multiplatform/mc_pos_control.h
View File

@ -59,18 +59,18 @@
using namespace px4;
class MulticopterPositionControl
class MulticopterPositionControlMultiplatform
{
public:
/**
* Constructor
*/
MulticopterPositionControl();
MulticopterPositionControlMultiplatform();
/**
* Destructor, also kills task.
*/
~MulticopterPositionControl();
~MulticopterPositionControlMultiplatform();
/* Callbacks for topics */
void handle_vehicle_attitude(const px4_vehicle_attitude &msg);

13
src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp
View File

@ -46,18 +46,23 @@
#include "mc_pos_control.h"
bool thread_running = false; /**< Deamon status flag */
bool mc_pos_control_thread_running = false; /**< Deamon status flag */
#if defined(__PX4_ROS)
int main(int argc, char **argv)
#else
int mc_pos_control_start_main(int argc, char **argv); // Prototype for missing declearation error with nuttx
int mc_pos_control_start_main(int argc, char **argv)
#endif
{
px4::init(argc, argv, "mc_pos_control_m");
PX4_INFO("starting");
MulticopterPositionControl posctl;
thread_running = true;
MulticopterPositionControlMultiplatform posctl;
mc_pos_control_thread_running = true;
posctl.spin();
PX4_INFO("exiting.");
thread_running = false;
mc_pos_control_thread_running = false;
return 0;
}

28
src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp
View File

@ -41,15 +41,15 @@
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
extern bool thread_running;
int daemon_task; /**< Handle of deamon task / thread */
extern bool mc_pos_control_thread_running;
int mc_pos_control_daemon_task; /**< Handle of deamon task / thread */
namespace px4
{
bool task_should_exit = false;
bool mc_pos_control_task_should_exit = false;
}
using namespace px4;
extern int main(int argc, char **argv);
extern int mc_pos_control_start_main(int argc, char **argv);
extern "C" __EXPORT int mc_pos_control_m_main(int argc, char *argv[]);
int mc_pos_control_m_main(int argc, char *argv[])
@ -60,38 +60,38 @@ int mc_pos_control_m_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) {
if (thread_running) {
if (mc_pos_control_thread_running) {
warnx("already running");
/* this is not an error */
exit(0);
return 0;
}
task_should_exit = false;
mc_pos_control_task_should_exit = false;
daemon_task = px4_task_spawn_cmd("mc_pos_control_m",
mc_pos_control_daemon_task = px4_task_spawn_cmd("mc_pos_control_m",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2500,
main,
mc_pos_control_start_main,
(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
exit(0);
return 0;
}
if (!strcmp(argv[1], "stop")) {
task_should_exit = true;
exit(0);
mc_pos_control_task_should_exit = true;
return 0;
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
if (mc_pos_control_thread_running) {
warnx("is running");
} else {
warnx("not started");
}
exit(0);
return 0;
}
warnx("unrecognized command");

6
src/platforms/posix/px4_layer/px4_posix_impl.cpp
View File

@ -47,6 +47,7 @@
#include <unistd.h>
#include "systemlib/param/param.h"
#include "hrt_work.h"
#include <drivers/drv_hrt.h>
extern pthread_t _shell_task_id;
@ -77,5 +78,10 @@ void init(int argc, char *argv[], const char *app_name)
printf("App name: %s\n", app_name);
}
uint64_t get_time_micros()
{
return hrt_absolute_time();
}
}