build fixes

1 files changed, 115 insertions, 106 deletions

diff --git a/apps/multirotor_control/multirotor_control.c b/apps/multirotor_control/multirotor_control.c
index 3b54dead7..2edc1ba84 100644
--- a/apps/multirotor_control/multirotor_control.c
+++ b/apps/multirotor_control/multirotor_control.c
@@ -34,6 +34,7 @@
 
 /*
  * @file multirotor_control.c
+ *
  * Implementation of multirotor controllers
  */
 
@@ -46,8 +47,9 @@
 #include <fcntl.h>
 #include <errno.h>
 #include <debug.h>
-#include <termios.h>
+#include <getopt.h>
 #include <time.h>
+#include <poll.h>
 #include <sys/prctl.h>
 #include <arch/board/up_hrt.h>
 #include "multirotor_control.h"
@@ -55,144 +57,151 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/ardrone_control.h>
+#include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/rc_channels.h>
-#include <uORB/topics/sensor_combined.h>
 
-__EXPORT int ardrone_control_main(int argc, char *argv[]);
+#include <systemlib/perf_counter.h>
 
-/****************************************************************************
- * Internal Definitions
- ****************************************************************************/
+__EXPORT int multirotor_control_main(int argc, char *argv[]);
 
 
-enum {
+static enum {
 	CONTROL_MODE_RATES = 0,
 	CONTROL_MODE_ATTITUDE = 1,
 } control_mode;
 
-/****************************************************************************
- * Private Data
- ****************************************************************************/
 
-/*File descriptors */
-int ardrone_write;
-int gpios;
+static bool thread_should_exit;
+static int mc_task;
 
-bool position_control_thread_started;
+static int
+mc_thread_main(int argc, char *argv[])
+{
+	/* structures */
+	struct vehicle_status_s state;
+	struct vehicle_attitude_s att;
+	struct rc_channels_s rc;
+	struct actuator_controls_s actuators;
+	struct actuator_armed_s armed;
 
-/****************************************************************************
- * pthread loops
- ****************************************************************************/
-// static void *position_control_loop(void *arg)
-// {
-// 	struct vehicle_status_s *state = (struct vehicle_status_s *)arg;
-// 	// Set thread name
-// 	prctl(PR_SET_NAME, "ardrone pos ctrl", getpid());
-
-// 	while (1) {
-// 		if (state->state_machine == SYSTEM_STATE_AUTO) {
-// //			control_position();   //FIXME TODO XXX
-// 			/* temporary 50 Hz execution */
-// 			usleep(20000);
-
-// 		} else {
-// 			position_control_thread_started = false;
-// 			break;
-// 		}
-// 	}
-
-// 	return NULL;
-// }
+	/* subscribe to attitude, motor setpoints and system state */
+	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
+	int rc_sub = orb_subscribe(ORB_ID(rc_channels));
 
-/****************************************************************************
- * main
- ****************************************************************************/
+	/* rate-limit the attitude subscription to 200Hz to pace our loop */
+	orb_set_interval(att_sub, 5);
+	struct pollfd fds = { .fd = att_sub, .events = POLLIN };
+
+	/* publish actuator controls */
+	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
+		actuators.control[i] = 0.0f;
+	int actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
+	armed.armed = true;
+	int armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
+
+	/* register the perf counter */
+	perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_control");
 
-int ardrone_control_main(int argc, char *argv[])
-{
 	/* welcome user */
-	printf("[ardrone_control] Control started, taking over motors\n");
+	printf("[multirotor_control] starting\n");
 
-	/* default values for arguments */
-	char *ardrone_uart_name = "/dev/ttyS1";
-	control_mode = CONTROL_MODE_RATES;
+	while (!thread_should_exit) {
 
-	char *commandline_usage = "\tusage: ardrone_control -d ardrone-devicename -m mode\n\tmodes are:\n\t\trates\n\t\tattitude\n";
+		/* wait for a sensor update */
+		poll(&fds, 1, -1);
 
-	/* read commandline arguments */
-	int i;
+		perf_begin(mc_loop_perf);
 
-	for (i = 1; i < argc; i++) {
-		if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //ardrone set
-			if (argc > i + 1) {
-				ardrone_uart_name = argv[i + 1];
+		/* get a local copy of the vehicle state */
+		orb_copy(ORB_ID(vehicle_status), state_sub, &state);
 
-			} else {
-				printf(commandline_usage);
-				return 0;
-			}
+		/* get a local copy of rc inputs */
+		orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
 
-		} else if (strcmp(argv[i], "-m") == 0 || strcmp(argv[i], "--mode") == 0) {
-			if (argc > i + 1) {
-				if (strcmp(argv[i + 1], "rates") == 0) {
-					control_mode = CONTROL_MODE_RATES;
+		/* get a local copy of attitude */
+		orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
 
-				} else if (strcmp(argv[i + 1], "attitude") == 0) {
-					control_mode = CONTROL_MODE_ATTITUDE;
+		/* run the attitude controller */
+		multirotor_control_attitude(&rc, &att, &state, &actuators);
 
-				} else {
-					printf(commandline_usage);
-					return 0;
-				}
+		/* publish the result */
+		orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
 
-			} else {
-				printf(commandline_usage);
-				return 0;
-			}
-		}
+		perf_end(mc_loop_perf);
 	}
 
-	/* initialize motors */
-	
-	int counter = 0;
+	printf("[multirotor_control] stopping\r\n");
 
-	/* pthread for position control */
-	// pthread_t position_control_thread;
-	// position_control_thread_started = false;
+	/* kill all outputs */
+	armed.armed = false;
+	orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
+	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
+		actuators.control[i] = 0.0f;
+	orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
 
-	/* structures */
-	struct vehicle_status_s state;
-	struct vehicle_attitude_s att;
-	struct rc_channels_s rc;
-	struct sensor_combined_s raw;
 
-	/* subscribe to attitude, motor setpoints and system state */
-	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
-	int rc_sub = orb_subscribe(ORB_ID(rc_channels));
-	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+	close(att_sub);
+	close(state_sub);
+	close(rc_sub);
+	close(actuator_pub);
+	close(armed_pub);
 
-	/* publish AR.Drone motor control state */
-	// int ardrone_pub = orb_advertise(ORB_ID(ardrone_control), &ar_control);
+	perf_print_counter(mc_loop_perf);
+	perf_free(mc_loop_perf);
 
-	while (1) {
-		/* get a local copy of the vehicle state */
-		orb_copy(ORB_ID(vehicle_status), state_sub, &state);
-		/* get a local copy of rc */
-		orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
-		/* get a local copy of attitude */
-		orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
+	fflush(stdout);
+	exit(0);
+}
+
+static void
+usage(const char *reason)
+{
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+	fprintf(stderr, "usage: multirotor_control [-m <mode>] {start|stop}\n");
+	fprintf(stderr, "    <mode> is 'rates' or 'attitude'\n");
+	exit(1);
+}
+
+int multirotor_control_main(int argc, char *argv[])
+{
+	int	ch;
+
+	control_mode = CONTROL_MODE_RATES;
+
+	while ((ch = getopt(argc, argv, "m:")) != EOF) {
+		switch (ch) {
+		case 'm':
+			if (!strcmp(optarg, "rates")) {
+				control_mode = CONTROL_MODE_RATES;
+			} else if (!strcmp(optarg, "attitude")) {
+				control_mode = CONTROL_MODE_RATES;
+			} else {
+				usage("unrecognized -m value");
+			}
+		default:
+			usage("unrecognized option");
+		}
+	}
+	argc -= optind;
+	argv += optind;
+
+	if (argc < 1)
+		usage("missing command");
 
-		multirotor_control_attitude(&rc, &att, &state);
+	if (!strcmp(argv[1], "start")) {
 
-		/* run at approximately 200 Hz */
-		usleep(5000);
-		counter++;
+		thread_should_exit = false;
+		mc_task = task_create("multirotor_attitude", SCHED_PRIORITY_MAX - 15, 2048, mc_thread_main, NULL);
+		exit(0);
 	}
 
-	printf("[ardrone_control] ending now...\r\n");
-	fflush(stdout);
-	return 0;
-}
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+		exit(0);
+	}
 
+	usage("unrecognised command");
+	exit(1);
+}