Tested mc_att_control_m and mc_pos_control_m in jmavsim.OK.

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
 
 #

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<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);
 

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);

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;
 }
 

src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp

@@ -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");

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<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;

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);

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;
 }

src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp

@@ -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");

src/platforms/posix/px4_layer/px4_posix_impl.cpp

@@ -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();
+}
+
 }