diff --git a/makefiles/posix/config_posix_sitl.mk b/makefiles/posix/config_posix_sitl.mk index 14678aa81a..909c6d74e2 100644 --- a/makefiles/posix/config_posix_sitl.mk +++ b/makefiles/posix/config_posix_sitl.mk @@ -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 # diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp index f5341155b8..63515f497d 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp @@ -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(&MulticopterAttitudeControl::handle_vehicle_attitude, this, 0); + _v_att = _n.subscribe(&MulticopterAttitudeControlMultiplatform::handle_vehicle_attitude, this, 0); _v_att_sp = _n.subscribe(0); _v_rates_sp = _n.subscribe(0); _v_control_mode = _n.subscribe(0); _parameter_update = _n.subscribe( - &MulticopterAttitudeControl::handle_parameter_update, this, 1000); + &MulticopterAttitudeControlMultiplatform::handle_parameter_update, this, 1000); _manual_control_sp = _n.subscribe(0); _armed = _n.subscribe(0); _v_status = _n.subscribe(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); diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.h b/src/modules/mc_att_control_multiplatform/mc_att_control.h index aab41f97ba..de059fc9f3 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control.h +++ b/src/modules/mc_att_control_multiplatform/mc_att_control.h @@ -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); diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp index 5a79a8c6b2..8fc6609e6f 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp @@ -54,19 +54,23 @@ #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) +#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; } diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp index 93efd90e7f..a0e6926cf4 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp @@ -41,57 +41,58 @@ #include #include -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"); diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp index d5f8e668ce..e9bc2346cc 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -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(&MulticopterPositionControl::handle_vehicle_attitude, this, 0); + _att = _n.subscribe(&MulticopterPositionControlMultiplatform::handle_vehicle_attitude, this, 0); _control_mode = _n.subscribe(0); _parameter_update = _n.subscribe( - &MulticopterPositionControl::handle_parameter_update, this, 1000); + &MulticopterPositionControlMultiplatform::handle_parameter_update, this, 1000); _manual_control_sp = _n.subscribe(0); _armed = _n.subscribe(0); _local_pos = _n.subscribe(0); - _pos_sp_triplet = _n.subscribe(&MulticopterPositionControl::handle_position_setpoint_triplet, this, 0); + _pos_sp_triplet = _n.subscribe(&MulticopterPositionControlMultiplatform::handle_position_setpoint_triplet, this, 0); _local_pos_sp = _n.subscribe(0); _global_vel_sp = _n.subscribe(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; diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h index 4f786c4298..d14d8b23e9 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h @@ -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); diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp index 0b5775736e..ceece71720 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp @@ -46,18 +46,22 @@ #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) +#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; } diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp index 92416249d8..3a58863847 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp @@ -41,15 +41,15 @@ #include #include -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"); diff --git a/src/platforms/posix/px4_layer/px4_posix_impl.cpp b/src/platforms/posix/px4_layer/px4_posix_impl.cpp index 243716abe6..94387f648b 100644 --- a/src/platforms/posix/px4_layer/px4_posix_impl.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_impl.cpp @@ -47,6 +47,7 @@ #include #include "systemlib/param/param.h" #include "hrt_work.h" +#include 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(); +} + }