Merged

1 files changed, 207 insertions, 183 deletions

diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 78fac5c22..43df6ceb2 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -114,8 +114,8 @@ static bool commander_initialized = false;
 static struct vehicle_status_s current_status; /**< Main state machine */
 static orb_advert_t stat_pub;
 
-static uint16_t nofix_counter = 0;
-static uint16_t gotfix_counter = 0;
+// static uint16_t nofix_counter = 0;
+// static uint16_t gotfix_counter = 0;
 
 static unsigned int failsafe_lowlevel_timeout_ms;
 
@@ -124,7 +124,6 @@ static bool thread_running = false;		/**< Deamon status flag */
 static int deamon_task;				/**< Handle of deamon task / thread */
 
 /* pthread loops */
-static void *command_handling_loop(void *arg);
 static void *orb_receive_loop(void *arg);
 
 __EXPORT int commander_main(int argc, char *argv[]);
@@ -164,7 +163,7 @@ static void usage(const char *reason);
  * @param a 	The array to sort
  * @param n 	The number of entries in the array
  */
-static void cal_bsort(float a[], int n);
+// static void cal_bsort(float a[], int n);
 
 static int buzzer_init()
 {
@@ -293,7 +292,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 	struct sensor_combined_s raw;
 
 	/* 30 seconds */
-	int calibration_interval_ms = 30 * 1000;
+	uint64_t calibration_interval = 30 * 1000 * 1000;
 	unsigned int calibration_counter = 0;
 
 	float mag_max[3] = {FLT_MIN, FLT_MIN, FLT_MIN};
@@ -308,42 +307,74 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 		0.0f,
 		1.0f,
 	};
-	if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null))
+	if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
 		warn("WARNING: failed to set scale / offsets for mag");
+		mavlink_log_info(mavlink_fd, "[commander] failed to set scale / offsets for mag");
+	}
+
 	close(fd);
 
-	mavlink_log_info(mavlink_fd, "[commander] Please rotate around X");
-	
 	uint64_t calibration_start = hrt_absolute_time();
-	while ((hrt_absolute_time() - calibration_start)/1000 < calibration_interval_ms) {
+
+	uint64_t axis_deadline = hrt_absolute_time();
+	uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
+
+	const char axislabels[3] = { 'X', 'Y', 'Z'};
+	int axis_index = 0;
+
+	while (hrt_absolute_time() < calibration_deadline) {
 
 		/* wait blocking for new data */
 		struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
 
+		/* user guidance */
+		if (hrt_absolute_time() > axis_deadline &&
+			axis_index < 3) {
+			char buf[50];
+			sprintf(buf, "[commander] Please rotate around %c", axislabels[axis_index]);
+			mavlink_log_info(mavlink_fd, buf);
+		
+			axis_deadline += calibration_interval / 3;
+			axis_index++;
+		}
+
+		if (!(axis_index < 3)) {
+			continue;
+		}
+
+		// int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time();
+
+		// if ((axis_left / 1000) == 0 && axis_left > 0) {
+		// 	char buf[50];
+		// 	sprintf(buf, "[commander] %d seconds left for axis %c", axis_left, axislabels[axis_index]);
+		// 	mavlink_log_info(mavlink_fd, buf);
+		// }
+
 		if (poll(fds, 1, 1000)) {
 			orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
 			/* get min/max values */
 
-			if (raw.magnetometer_ga[0] < mag_min[0]) {
-				mag_min[0] = raw.magnetometer_ga[0];
+			/* ignore other axes */
+			if (raw.magnetometer_ga[axis_index] < mag_min[axis_index]) {
+				mag_min[axis_index] = raw.magnetometer_ga[axis_index];
 			}
-			else if (raw.magnetometer_ga[0] > mag_max[0]) {
-				mag_max[0] = raw.magnetometer_ga[0];
+			else if (raw.magnetometer_ga[axis_index] > mag_max[axis_index]) {
+				mag_max[axis_index] = raw.magnetometer_ga[axis_index];
 			}
 
-			if (raw.magnetometer_ga[1] < mag_min[1]) {
-				mag_min[1] = raw.magnetometer_ga[1];
-			}
-			else if (raw.magnetometer_ga[1] > mag_max[1]) {
-				mag_max[1] = raw.magnetometer_ga[1];
-			}
+			// if (raw.magnetometer_ga[1] < mag_min[1]) {
+			// 	mag_min[1] = raw.magnetometer_ga[1];
+			// }
+			// else if (raw.magnetometer_ga[1] > mag_max[1]) {
+			// 	mag_max[1] = raw.magnetometer_ga[1];
+			// }
 
-			if (raw.magnetometer_ga[2] < mag_min[2]) {
-				mag_min[2] = raw.magnetometer_ga[2];
-			}
-			else if (raw.magnetometer_ga[2] > mag_max[2]) {
-				mag_max[2] = raw.magnetometer_ga[2];
-			}
+			// if (raw.magnetometer_ga[2] < mag_min[2]) {
+			// 	mag_min[2] = raw.magnetometer_ga[2];
+			// }
+			// else if (raw.magnetometer_ga[2] > mag_max[2]) {
+			// 	mag_max[2] = raw.magnetometer_ga[2];
+			// }
 
 			calibration_counter++;
 		} else {
@@ -353,10 +384,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 		}
 	}
 
-	usleep(200000);
-
-	mavlink_log_info(mavlink_fd, "[commander] mag calibration done");
-
 	/* disable calibration mode */
 	status->flag_preflight_mag_calibration = false;
 	state_machine_publish(status_pub, status, mavlink_fd);
@@ -379,51 +406,49 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 	mag_offset[1] = (mag_max[1] + mag_min[1]) / 2.0f;
 	mag_offset[2] = (mag_max[2] + mag_min[2]) / 2.0f;
 
-	if (!isfinite(mag_offset[0]) || !isfinite(mag_offset[1]) || !isfinite(mag_offset[2]))
-	{
-		mavlink_log_info(mavlink_fd, "[commander] mag cal aborted: NaN");
-	}
-
-	//char buf[52];
-
-	//sprintf(buf, "mag off x: %4.4f, y: %4.4f, z: %4.4f\n",(double)mag_offset[0],(double)mag_offset[0],(double)mag_offset[2]);
-	//mavlink_log_info(mavlink_fd, buf);
+	if (isfinite(mag_offset[0]) && isfinite(mag_offset[1]) && isfinite(mag_offset[2])) {
 
-	/* announce and set new offset */
-
-	if (param_set(param_find("SENS_MAG_XOFF"), &(mag_offset[0]))) {
-		fprintf(stderr, "[commander] Setting X mag offset failed!\n");
-	}
+		/* announce and set new offset */
 
-	if (param_set(param_find("SENS_MAG_YOFF"), &(mag_offset[1]))) {
-		fprintf(stderr, "[commander] Setting Y mag offset failed!\n");
-	}
+		if (param_set(param_find("SENS_MAG_XOFF"), &(mag_offset[0]))) {
+			fprintf(stderr, "[commander] Setting X mag offset failed!\n");
+		}
 
-	if (param_set(param_find("SENS_MAG_ZOFF"), &(mag_offset[2]))) {
-		fprintf(stderr, "[commander] Setting Z mag offset failed!\n");
-	}
+		if (param_set(param_find("SENS_MAG_YOFF"), &(mag_offset[1]))) {
+			fprintf(stderr, "[commander] Setting Y mag offset failed!\n");
+		}
 
-	fd = open(MAG_DEVICE_PATH, 0);
-	struct mag_scale mscale = {
-		mag_offset[0],
-		1.0f,
-		mag_offset[1],
-		1.0f,
-		mag_offset[2],
-		1.0f,
-	};
-	if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale))
-		warn("WARNING: failed to set scale / offsets for mag");
-	close(fd);
+		if (param_set(param_find("SENS_MAG_ZOFF"), &(mag_offset[2]))) {
+			fprintf(stderr, "[commander] Setting Z mag offset failed!\n");
+		}
 
-	/* auto-save to EEPROM */
-	int save_ret = pm_save_eeprom(false);
-	if(save_ret != 0) {
-		warn("WARNING: auto-save of params to EEPROM failed");
+		fd = open(MAG_DEVICE_PATH, 0);
+		struct mag_scale mscale = {
+			mag_offset[0],
+			1.0f,
+			mag_offset[1],
+			1.0f,
+			mag_offset[2],
+			1.0f,
+		};
+		if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale))
+			warn("WARNING: failed to set scale / offsets for mag");
+		close(fd);
+
+		/* auto-save to EEPROM */
+		int save_ret = pm_save_eeprom(false);
+		if(save_ret != 0) {
+			warn("WARNING: auto-save of params to EEPROM failed");
+		}
+		
+		// char buf[50];
+		// sprintf(buf, "[commander] mag cal: x:%d y:%d z:%d mGa", (int)(mag_offset[0]*1000), (int)(mag_offset[1]*1000), (int)(mag_offset[2]*1000));
+		// mavlink_log_info(mavlink_fd, buf);
+		mavlink_log_info(mavlink_fd, "[commander] mag calibration done");
+	} else {
+		mavlink_log_info(mavlink_fd, "[commander] mag calibration FAILED (NaN)");
 	}
 
-	mavlink_log_info(mavlink_fd, "[commander] mag cal finished");
-
 	close(sub_sensor_combined);
 }
 
@@ -477,45 +502,51 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
 	gyro_offset[1] = gyro_offset[1] / calibration_count;
 	gyro_offset[2] = gyro_offset[2] / calibration_count;
 
-	if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0]))) {
-		mavlink_log_critical(mavlink_fd, "[commander] Setting X gyro offset failed!");
-	}
-	
-	if (param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1]))) {
-		mavlink_log_critical(mavlink_fd, "[commander] Setting Y gyro offset failed!");
-	}
-
-	if (param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) {
-		mavlink_log_critical(mavlink_fd, "[commander] Setting Z gyro offset failed!");
-	}
+	/* exit gyro calibration mode */
+	status->flag_preflight_gyro_calibration = false;
+	state_machine_publish(status_pub, status, mavlink_fd);
 
-	/* set offsets to actual value */
-	fd = open(GYRO_DEVICE_PATH, 0);
-	struct gyro_scale gscale = { 
-		gyro_offset[0],
-		1.0f,
-		gyro_offset[1],
-		1.0f,
-		gyro_offset[2],
-		1.0f,
-	};
-	if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
-		warn("WARNING: failed to set scale / offsets for gyro");
-	close(fd);
+	if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) {
 
-	/* auto-save to EEPROM */
-	int save_ret = pm_save_eeprom(false);
-	if(save_ret != 0) {
-		warn("WARNING: auto-save of params to EEPROM failed");
-	}
+		if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0]))) {
+			mavlink_log_critical(mavlink_fd, "[commander] Setting X gyro offset failed!");
+		}
+		
+		if (param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1]))) {
+			mavlink_log_critical(mavlink_fd, "[commander] Setting Y gyro offset failed!");
+		}
 
-	/* exit to gyro calibration mode */
-	status->flag_preflight_gyro_calibration = false;
-	state_machine_publish(status_pub, status, mavlink_fd);
+		if (param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) {
+			mavlink_log_critical(mavlink_fd, "[commander] Setting Z gyro offset failed!");
+		}
 
-	mavlink_log_info(mavlink_fd, "[commander] gyro calibration finished");
+		/* set offsets to actual value */
+		fd = open(GYRO_DEVICE_PATH, 0);
+		struct gyro_scale gscale = { 
+			gyro_offset[0],
+			1.0f,
+			gyro_offset[1],
+			1.0f,
+			gyro_offset[2],
+			1.0f,
+		};
+		if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
+			warn("WARNING: failed to set scale / offsets for gyro");
+		close(fd);
+
+		/* auto-save to EEPROM */
+		int save_ret = pm_save_eeprom(false);
+		if(save_ret != 0) {
+			warn("WARNING: auto-save of params to EEPROM failed");
+		}
 
-	printf("[commander] gyro cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]);
+		// char buf[50];
+		// sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]);
+		// mavlink_log_info(mavlink_fd, buf);
+		mavlink_log_info(mavlink_fd, "[commander] gyro calibration done");
+	} else {
+		mavlink_log_info(mavlink_fd, "[commander] gyro calibration FAILED (NaN)");
+	}
 
 	close(sub_sensor_combined);
 }
@@ -570,61 +601,71 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status)
 	accel_offset[1] = accel_offset[1] / calibration_count;
 	accel_offset[2] = accel_offset[2] / calibration_count;
 
-	/* add the removed length from x / y to z, since we induce a scaling issue else */
-	float total_len = sqrtf(accel_offset[0]*accel_offset[0] + accel_offset[1]*accel_offset[1] + accel_offset[2]*accel_offset[2]);
+	if (isfinite(accel_offset[0]) && isfinite(accel_offset[1]) && isfinite(accel_offset[2])) {
+		
+		/* add the removed length from x / y to z, since we induce a scaling issue else */
+		float total_len = sqrtf(accel_offset[0]*accel_offset[0] + accel_offset[1]*accel_offset[1] + accel_offset[2]*accel_offset[2]);
 
-	/* if length is correct, zero results here */
-	accel_offset[2] = accel_offset[2] + total_len;
+		/* if length is correct, zero results here */
+		accel_offset[2] = accel_offset[2] + total_len;
 
-	float scale = 9.80665f / total_len;
+		float scale = 9.80665f / total_len;
 
-	if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offset[0]))) {
-		mavlink_log_critical(mavlink_fd, "[commander] Setting X accel offset failed!");
-	}
-	
-	if (param_set(param_find("SENS_ACC_YOFF"), &(accel_offset[1]))) {
-		mavlink_log_critical(mavlink_fd, "[commander] Setting Y accel offset failed!");
-	}
+		if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offset[0]))) {
+			mavlink_log_critical(mavlink_fd, "[commander] Setting X accel offset failed!");
+		}
+		
+		if (param_set(param_find("SENS_ACC_YOFF"), &(accel_offset[1]))) {
+			mavlink_log_critical(mavlink_fd, "[commander] Setting Y accel offset failed!");
+		}
 
-	if (param_set(param_find("SENS_ACC_ZOFF"), &(accel_offset[2]))) {
-		mavlink_log_critical(mavlink_fd, "[commander] Setting Z accel offset failed!");
-	}
+		if (param_set(param_find("SENS_ACC_ZOFF"), &(accel_offset[2]))) {
+			mavlink_log_critical(mavlink_fd, "[commander] Setting Z accel offset failed!");
+		}
 
-	if (param_set(param_find("SENS_ACC_XSCALE"), &(scale))) {
-		mavlink_log_critical(mavlink_fd, "[commander] Setting X accel offset failed!");
-	}
-	
-	if (param_set(param_find("SENS_ACC_YSCALE"), &(scale))) {
-		mavlink_log_critical(mavlink_fd, "[commander] Setting Y accel offset failed!");
-	}
+		if (param_set(param_find("SENS_ACC_XSCALE"), &(scale))) {
+			mavlink_log_critical(mavlink_fd, "[commander] Setting X accel offset failed!");
+		}
+		
+		if (param_set(param_find("SENS_ACC_YSCALE"), &(scale))) {
+			mavlink_log_critical(mavlink_fd, "[commander] Setting Y accel offset failed!");
+		}
 
-	if (param_set(param_find("SENS_ACC_ZSCALE"), &(scale))) {
-		mavlink_log_critical(mavlink_fd, "[commander] Setting Z accel offset failed!");
-	}
+		if (param_set(param_find("SENS_ACC_ZSCALE"), &(scale))) {
+			mavlink_log_critical(mavlink_fd, "[commander] Setting Z accel offset failed!");
+		}
 
-	fd = open(ACCEL_DEVICE_PATH, 0);
-	struct accel_scale ascale = {
-		accel_offset[0],
-		scale,
-		accel_offset[1],
-		scale,
-		accel_offset[2],
-		scale,
-	};
-	if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale))
-		warn("WARNING: failed to set scale / offsets for accel");
-	close(fd);
+		fd = open(ACCEL_DEVICE_PATH, 0);
+		struct accel_scale ascale = {
+			accel_offset[0],
+			scale,
+			accel_offset[1],
+			scale,
+			accel_offset[2],
+			scale,
+		};
+		if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale))
+			warn("WARNING: failed to set scale / offsets for accel");
+		close(fd);
+
+		/* auto-save to EEPROM */
+		int save_ret = pm_save_eeprom(false);
+		if(save_ret != 0) {
+			warn("WARNING: auto-save of params to EEPROM failed");
+		}
 
-	/* auto-save to EEPROM */
-	int save_ret = pm_save_eeprom(false);
-	if(save_ret != 0) {
-		warn("WARNING: auto-save of params to EEPROM failed");
+		//char buf[50];
+		//sprintf(buf, "[commander] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]);
+		//mavlink_log_info(mavlink_fd, buf);
+		mavlink_log_info(mavlink_fd, "[commander] accel calibration done");
+	} else {
+		mavlink_log_info(mavlink_fd, "[commander] accel calibration FAILED (NaN)");
 	}
-	/* exit to gyro calibration mode */
+
+	/* exit accel calibration mode */
 	status->flag_preflight_accel_calibration = false;
 	state_machine_publish(status_pub, status, mavlink_fd);
-	mavlink_log_info(mavlink_fd, "[commander] acceleration calibration finished");
-	printf("[commander] accel calibration: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0],(double)accel_offset[1], (double)accel_offset[2]);
+
 	close(sub_sensor_combined);
 }
 
@@ -742,8 +783,10 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 
 				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
 					mavlink_log_info(mavlink_fd, "[commander] CMD starting gyro calibration");
+					ioctl(buzzer, TONE_SET_ALARM, 2);
 					do_gyro_calibration(status_pub, &current_status);
 					mavlink_log_info(mavlink_fd, "[commander] CMD finished gyro calibration");
+					ioctl(buzzer, TONE_SET_ALARM, 2);
 					do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
 					result = MAV_RESULT_ACCEPTED;
 				} else {
@@ -760,8 +803,10 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 
 				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
 					mavlink_log_info(mavlink_fd, "[commander] CMD starting mag calibration");
+					ioctl(buzzer, TONE_SET_ALARM, 2);
 					do_mag_calibration(status_pub, &current_status);
 					mavlink_log_info(mavlink_fd, "[commander] CMD finished mag calibration");
+					ioctl(buzzer, TONE_SET_ALARM, 2);
 					do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
 					result = MAV_RESULT_ACCEPTED;
 				} else {
@@ -823,37 +868,6 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 
 }
 
-/**
- * Handle commands sent by the ground control station via MAVLink.
- */
-static void *command_handling_loop(void *arg)
-{
-	/* Set thread name */
-	prctl(PR_SET_NAME, "commander cmd handler", getpid());
-
-	/* Subscribe to command topic */
-	int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
-	struct vehicle_command_s cmd;
-
-	while (!thread_should_exit) {
-		struct pollfd fds[1] = { { .fd = cmd_sub, .events = POLLIN } };
-
-		if (poll(fds, 1, 5000) == 0) {
-			/* timeout, but this is no problem, silently ignore */
-		} else {
-			/* got command */
-			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
-
-			/* handle it */
-			handle_command(stat_pub, &current_status, &cmd);
-		}
-	}
-
-	close(cmd_sub);
-
-	return NULL;
-}
-
 static void *orb_receive_loop(void *arg)  //handles status information coming from subsystems (present, enabled, health), these values do not indicate the quality (variance) of the signal
 {
 	/* Set thread name */
@@ -1002,7 +1016,7 @@ int commander_thread_main(int argc, char *argv[])
 	printf("[commander] I am in command now!\n");
 
 	/* pthreads for command and subsystem info handling */
-	pthread_t command_handling_thread;
+	// pthread_t command_handling_thread;
 	pthread_t subsystem_info_thread;
 
 	/* initialize */
@@ -1044,11 +1058,6 @@ int commander_thread_main(int argc, char *argv[])
 	mavlink_log_info(mavlink_fd, "[commander] system is running");
 
 	/* create pthreads */
-	pthread_attr_t command_handling_attr;
-	pthread_attr_init(&command_handling_attr);
-	pthread_attr_setstacksize(&command_handling_attr, 6000);
-	pthread_create(&command_handling_thread, &command_handling_attr, command_handling_loop, NULL);
-
 	pthread_attr_t subsystem_info_attr;
 	pthread_attr_init(&subsystem_info_attr);
 	pthread_attr_setstacksize(&subsystem_info_attr, 2048);
@@ -1093,6 +1102,11 @@ int commander_thread_main(int argc, char *argv[])
 	struct sensor_combined_s sensors;
 	memset(&sensors, 0, sizeof(sensors));
 
+	/* Subscribe to command topic */
+	int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
+	struct vehicle_command_s cmd;
+	memset(&cmd, 0, sizeof(cmd));
+
 	// uint8_t vehicle_state_previous = current_status.state_machine;
 	float voltage_previous = 0.0f;
 
@@ -1122,6 +1136,15 @@ int commander_thread_main(int argc, char *argv[])
 		orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps);
 		orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
 
+		orb_check(cmd_sub, &new_data);
+		if (new_data) {
+			/* got command */
+			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
+
+			/* handle it */
+			handle_command(stat_pub, &current_status, &cmd);
+		}
+
 		battery_voltage = sensors.battery_voltage_v;
 		battery_voltage_valid = sensors.battery_voltage_valid;
 
@@ -1480,7 +1503,7 @@ int commander_thread_main(int argc, char *argv[])
 	}
 
 	/* wait for threads to complete */
-	pthread_join(command_handling_thread, NULL);
+	// pthread_join(command_handling_thread, NULL);
 	pthread_join(subsystem_info_thread, NULL);
 
 	/* close fds */
@@ -1490,6 +1513,7 @@ int commander_thread_main(int argc, char *argv[])
 	close(sp_offboard_sub);
 	close(gps_sub);
 	close(sensor_sub);
+	close(cmd_sub);
 
 	printf("[commander] exiting..\n");
 	fflush(stdout);