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

1 files changed, 212 insertions, 206 deletions

diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 631ca57e0..7a3b61482 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -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,200 +458,140 @@ 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));
-
-	/* initialize parameters cache */
-	parameters_update();
+	// [> wakeup source: vehicle attitude <]
+	// struct pollfd fds[1];
 
-	/* wakeup source: vehicle attitude */
-	struct pollfd fds[1];
+	// fds[0].fd = _v_att_sub;
+	// fds[0].events = POLLIN;
 
-	fds[0].fd = _v_att_sub;
-	fds[0].events = POLLIN;
+	// while (!_task_should_exit) {
 
-	while (!_task_should_exit) {
 
-		/* wait for up to 100ms for data */
-		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
+		// perf_end(_loop_perf);
+	// }
 
-		/* timed out - periodic check for _task_should_exit */
-		if (pret == 0) {
-			continue;
-		}
+	// warnx("exit");
 
-		/* 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;
-		}
+	// _control_task = -1;
+	// _exit(0);
+// }
 
-		perf_begin(_loop_perf);
+void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg) {
 
-		/* 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();
+	perf_begin(_loop_perf);
 
-			/* guard against too small (< 2ms) and too large (> 20ms) dt's */
-			if (dt < 0.002f) {
-				dt = 0.002f;
+	/* run controller on attitude changes */
+	static uint64_t last_run = 0;
+	float dt = (hrt_absolute_time() - last_run) / 1000000.0f;
+	last_run = hrt_absolute_time();
 
-			} else if (dt > 0.02f) {
-				dt = 0.02f;
-			}
+	/* 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);
+	/* 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();
+	/* 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);
+	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();
+		/* 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);
+			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);
-					}
-				}
+			} 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();
+		/* 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);
+		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 {
+			_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;
-				}
-			}
+	} 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;
 
-			if (_v_control_mode.flag_control_rates_enabled) {
-				control_attitude_rates(dt);
+			/* reset yaw setpoint after ACRO */
+			_reset_yaw_sp = true;
 
-				/* 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();
+			/* 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 (!_actuators_0_circuit_breaker_enabled) {
-					if (_actuators_0_pub > 0) {
-						orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
+			if (_v_rates_sp_pub > 0) {
+				orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
 
-					} else {
-						_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
-					}
-				}
+			} else {
+				_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
 			}
-		}
 
-		perf_end(_loop_perf);
+		} 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;
+		}
 	}
 
-	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);
-}
+	if (_v_control_mode.flag_control_rates_enabled) {
+		control_attitude_rates(dt);
 
-int
-MulticopterAttitudeControl::start()
-{
-	ASSERT(_control_task == -1);
+		/* 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();
 
-	/* 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 (!_actuators_0_circuit_breaker_enabled) {
+			if (_actuators_0_pub > 0) {
+				orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
 
-	if (_control_task < 0) {
-		warn("task start failed");
-		return -errno;
+			} else {
+				_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
+			}
+		}
 	}
-
-	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;
-
-		if (mc_att_control::g_control == nullptr) {
-			errx(1, "alloc failed");
-		}
+		task_should_exit = false;
 
-		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;
+}
+
+