Merge branch 'fixedwing_l1' of github.com:PX4/Firmware into fixedwing_l1

2 files changed, 44 insertions, 13 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 333fe30ae..5eeb018fd 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -144,8 +144,8 @@ static int mavlink_fd;
 
 /* flags */
 static bool commander_initialized = false;
-static bool thread_should_exit = false;		/**< daemon exit flag */
-static bool thread_running = false;		/**< daemon status flag */
+static volatile bool thread_should_exit = false;		/**< daemon exit flag */
+static volatile bool thread_running = false;		/**< daemon status flag */
 static int daemon_task;				/**< Handle of daemon task / thread */
 
 static unsigned int leds_counter;
@@ -230,7 +230,7 @@ int commander_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 
 		if (thread_running) {
-			warnx("commander already running\n");
+			warnx("commander already running");
 			/* this is not an error */
 			exit(0);
 		}
@@ -242,21 +242,38 @@ int commander_main(int argc, char *argv[])
 					     3000,
 					     commander_thread_main,
 					     (argv) ? (const char **)&argv[2] : (const char **)NULL);
+
+		while (!thread_running) {
+			usleep(200);
+		}
+
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "stop")) {
+
+		if (!thread_running)
+			errx(0, "commander already stopped");
+
 		thread_should_exit = true;
+
+		while (thread_running) {
+			usleep(200000);
+			warnx(".");
+		}
+
+		warnx("terminated.");
+
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
-			warnx("\tcommander is running\n");
+			warnx("\tcommander is running");
 			print_status();
 
 		} else {
-			warnx("\tcommander not started\n");
+			warnx("\tcommander not started");
 		}
 
 		exit(0);
@@ -326,6 +343,9 @@ void print_status()
 	warnx("arming: %s", armed_str);
 }
 
+static orb_advert_t control_mode_pub;
+static orb_advert_t status_pub;
+
 void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
 {
 	/* result of the command */
@@ -339,6 +359,10 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			uint8_t base_mode = (uint8_t) cmd->param1;
 			uint8_t custom_main_mode = (uint8_t) cmd->param2;
 
+			/* set HIL state */
+			hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
+			hil_state_transition(new_hil_state, status_pub, status, control_mode_pub, control_mode, mavlink_fd);
+
 			// TODO remove debug code
 			//mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode);
 			/* set arming state */
@@ -526,7 +550,6 @@ int commander_thread_main(int argc, char *argv[])
 	}
 
 	/* Main state machine */
-	orb_advert_t status_pub;
 	/* make sure we are in preflight state */
 	memset(&status, 0, sizeof(status));
 	status.condition_landed = true;	// initialize to safe value
@@ -538,7 +561,7 @@ int commander_thread_main(int argc, char *argv[])
 
 	/* flags for control apps */
 	struct vehicle_control_mode_s control_mode;
-	orb_advert_t control_mode_pub;
+	
 
 	/* Initialize all flags to false */
 	memset(&control_mode, 0, sizeof(control_mode));
@@ -595,16 +618,20 @@ int commander_thread_main(int argc, char *argv[])
 
 	mavlink_log_info(mavlink_fd, "[cmd] started");
 
+	int ret;
+
 	pthread_attr_t commander_low_prio_attr;
 	pthread_attr_init(&commander_low_prio_attr);
 	pthread_attr_setstacksize(&commander_low_prio_attr, 2992);
 
 	struct sched_param param;
 	(void)pthread_attr_getschedparam(&commander_low_prio_attr, &param);
+
 	/* low priority */
 	param.sched_priority = SCHED_PRIORITY_DEFAULT - 50;
 	(void)pthread_attr_setschedparam(&commander_low_prio_attr, &param);
 	pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL);
+	pthread_attr_destroy(&commander_low_prio_attr);
 
 	/* Start monitoring loop */
 	unsigned counter = 0;
@@ -1200,7 +1227,12 @@ int commander_thread_main(int argc, char *argv[])
 	}
 
 	/* wait for threads to complete */
-	pthread_join(commander_low_prio_thread, NULL);
+	ret = pthread_join(commander_low_prio_thread, NULL);
+	if (ret) {
+		warn("join failed", ret);
+	}
+
+	rgbled_set_mode(RGBLED_MODE_OFF);
 
 	/* close fds */
 	led_deinit();
@@ -1218,9 +1250,6 @@ int commander_thread_main(int argc, char *argv[])
 	close(param_changed_sub);
 	close(battery_sub);
 
-	warnx("exiting");
-	fflush(stdout);
-
 	thread_running = false;
 
 	return 0;
@@ -1628,7 +1657,7 @@ void *commander_low_prio_loop(void *arg)
 	while (!thread_should_exit) {
 
 		/* wait for up to 100ms for data */
-		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000);
+		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200);
 
 		/* timed out - periodic check for _task_should_exit, etc. */
 		if (pret == 0)
@@ -1785,5 +1814,5 @@ void *commander_low_prio_loop(void *arg)
 
 	close(cmd_sub);
 
-	return 0;
+	return NULL;
 }
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 316b06127..8ce31550f 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -504,6 +504,8 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 		current_control_mode->timestamp = hrt_absolute_time();
 		orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode);
 
+		// XXX also set lockdown here
+
 		ret = OK;
 
 	} else {