1 files changed, 302 insertions, 175 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 04e6dd2cb..17db0f9c8 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -52,6 +52,7 @@
 #include <errno.h>
 #include <debug.h>
 #include <sys/prctl.h>
+#include <sys/stat.h>
 #include <string.h>
 #include <math.h>
 #include <poll.h>
@@ -150,6 +151,8 @@ static unsigned int failsafe_lowlevel_timeout_ms;
 static unsigned int leds_counter;
 /* To remember when last notification was sent */
 static uint64_t last_print_mode_reject_time = 0;
+/* if connected via USB */
+static bool on_usb_power = false;
 
 
 /* tasks waiting for low prio thread */
@@ -194,10 +197,9 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode
  */
 int commander_thread_main(int argc, char *argv[]);
 
+void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed);
 void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
 
-void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gps_position_s *gps_position);
-
 void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status);
 
 transition_result_t check_main_state_machine(struct vehicle_status_s *current_status);
@@ -205,6 +207,8 @@ transition_result_t check_main_state_machine(struct vehicle_status_s *current_st
 void print_reject_mode(const char *msg);
 void print_reject_arm(const char *msg);
 
+void print_status();
+
 transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode);
 
 /**
@@ -244,6 +248,7 @@ int commander_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
 			warnx("\tcommander is running\n");
+			print_status();
 
 		} else {
 			warnx("\tcommander not started\n");
@@ -265,11 +270,54 @@ void usage(const char *reason)
 	exit(1);
 }
 
+void print_status() {
+	warnx("usb powered: %s", (on_usb_power) ? "yes" : "no");
+
+	/* read all relevant states */
+	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
+	struct vehicle_status_s state;
+	orb_copy(ORB_ID(vehicle_status), state_sub, &state);
+
+	const char* armed_str;
+
+	switch (state.arming_state) {
+		case ARMING_STATE_INIT:
+			armed_str = "INIT";
+			break;
+		case ARMING_STATE_STANDBY:
+			armed_str = "STANDBY";
+			break;
+		case ARMING_STATE_ARMED:
+			armed_str = "ARMED";
+			break;
+		case ARMING_STATE_ARMED_ERROR:
+			armed_str = "ARMED_ERROR";
+			break;
+		case ARMING_STATE_STANDBY_ERROR:
+			armed_str = "STANDBY_ERROR";
+			break;
+		case ARMING_STATE_REBOOT:
+			armed_str = "REBOOT";
+			break;
+		case ARMING_STATE_IN_AIR_RESTORE:
+			armed_str = "IN_AIR_RESTORE";
+			break;
+		default:
+			armed_str = "ERR: UNKNOWN STATE";
+			break;
+	}
+
+
+	warnx("arming: %s", armed_str);
+}
+
 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 */
 	uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED;
 
+	/* only handle high-priority commands here */
+
 	/* request to set different system mode */
 	switch (cmd->command) {
 	case VEHICLE_CMD_DO_SET_MODE: {
@@ -374,96 +422,11 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			break;
 		}
 
-	case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
-		if (is_safe(status, safety, armed)) {
-
-			if (((int)(cmd->param1)) == 1) {
-				/* reboot */
-				up_systemreset();
-
-			} else if (((int)(cmd->param1)) == 3) {
-				/* reboot to bootloader */
-
-				// XXX implement
-				result = VEHICLE_CMD_RESULT_UNSUPPORTED;
-
-			} else {
-				result = VEHICLE_CMD_RESULT_DENIED;
-			}
-
-		} else {
-			result = VEHICLE_CMD_RESULT_DENIED;
-		}
-
+	case VEHICLE_CMD_COMPONENT_ARM_DISARM:
+		// XXX implement later
+		result = VEHICLE_CMD_RESULT_DENIED;
 		break;
 
-	case VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
-			low_prio_task_t new_low_prio_task = LOW_PRIO_TASK_NONE;
-
-			if ((int)(cmd->param1) == 1) {
-				/* gyro calibration */
-				new_low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION;
-
-			} else if ((int)(cmd->param2) == 1) {
-				/* magnetometer calibration */
-				new_low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION;
-
-			} else if ((int)(cmd->param3) == 1) {
-				/* zero-altitude pressure calibration */
-				//new_low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION;
-			} else if ((int)(cmd->param4) == 1) {
-				/* RC calibration */
-				new_low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION;
-
-			} else if ((int)(cmd->param5) == 1) {
-				/* accelerometer calibration */
-				new_low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION;
-
-			} else if ((int)(cmd->param6) == 1) {
-				/* airspeed calibration */
-				new_low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION;
-			}
-
-			/* check if we have new task and no other task is scheduled */
-			if (low_prio_task == LOW_PRIO_TASK_NONE && new_low_prio_task != LOW_PRIO_TASK_NONE) {
-				/* try to go to INIT/PREFLIGHT arming state */
-				if (TRANSITION_DENIED != arming_state_transition(status, safety, ARMING_STATE_INIT, armed)) {
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-					low_prio_task = new_low_prio_task;
-
-				} else {
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-
-			} else {
-				result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
-			}
-
-			break;
-		}
-
-	case VEHICLE_CMD_PREFLIGHT_STORAGE: {
-			low_prio_task_t new_low_prio_task = LOW_PRIO_TASK_NONE;
-
-			if (((int)(cmd->param1)) == 0) {
-				new_low_prio_task = LOW_PRIO_TASK_PARAM_LOAD;
-
-			} else if (((int)(cmd->param1)) == 1) {
-				new_low_prio_task = LOW_PRIO_TASK_PARAM_SAVE;
-			}
-
-			/* check if we have new task and no other task is scheduled */
-			if (low_prio_task == LOW_PRIO_TASK_NONE && new_low_prio_task != LOW_PRIO_TASK_NONE) {
-				result = VEHICLE_CMD_RESULT_ACCEPTED;
-				low_prio_task = new_low_prio_task;
-
-			} else {
-				result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
-			}
-
-			break;
-		}
-
 	default:
 		break;
 	}
@@ -472,6 +435,8 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 	if (result == VEHICLE_CMD_RESULT_ACCEPTED) {
 		tune_positive();
 
+	} else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) {
+		/* we do not care in the high prio loop about commands we don't know */
 	} else {
 		tune_negative();
 
@@ -484,19 +449,24 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		} else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) {
 			mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd->command);
 
-		} else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) {
-			mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd->command);
 		}
 	}
 
 	/* send any requested ACKs */
-	if (cmd->confirmation > 0) {
+	if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
 		/* send acknowledge command */
 		// XXX TODO
 	}
 
 }
 
+static struct vehicle_status_s status;
+
+/* armed topic */
+static struct actuator_armed_s armed;
+
+static struct safety_s safety;
+
 int commander_thread_main(int argc, char *argv[])
 {
 	/* not yet initialized */
@@ -536,14 +506,12 @@ int commander_thread_main(int argc, char *argv[])
 	}
 
 	/* Main state machine */
-	struct vehicle_status_s status;
 	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
 
 	/* armed topic */
-	struct actuator_armed_s armed;
 	orb_advert_t armed_pub;
 	/* Initialize armed with all false */
 	memset(&armed, 0, sizeof(armed));
@@ -644,7 +612,6 @@ int commander_thread_main(int argc, char *argv[])
 
 	/* Subscribe to safety topic */
 	int safety_sub = orb_subscribe(ORB_ID(safety));
-	struct safety_s safety;
 	memset(&safety, 0, sizeof(safety));
 	safety.safety_switch_available = false;
 	safety.safety_off = false;
@@ -719,6 +686,7 @@ int commander_thread_main(int argc, char *argv[])
 	start_time = hrt_absolute_time();
 
 	while (!thread_should_exit) {
+
 		/* update parameters */
 		orb_check(param_changed_sub, &updated);
 
@@ -838,7 +806,7 @@ int commander_thread_main(int argc, char *argv[])
 		if (updated) {
 			orb_copy(ORB_ID(battery_status), battery_sub, &battery);
 
-			warnx("bat v: %2.2f", battery.voltage_v);
+			// warnx("bat v: %2.2f", battery.voltage_v);
 
 			/* only consider battery voltage if system has been running 2s and battery voltage is not 0 */
 			if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 0.001f) {
@@ -883,26 +851,32 @@ int commander_thread_main(int argc, char *argv[])
 			status_changed = true;
 		}
 
-		toggle_status_leds(&status, &armed, &gps_position);
-
 		if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
 			/* compute system load */
 			uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time;
 
 			if (last_idle_time > 0)
-				status.load = 1000 - (interval_runtime / 1000);	//system load is time spent in non-idle
+				status.load = 1.0f - ((float)interval_runtime / 1e6f);	//system load is time spent in non-idle
 
 			last_idle_time = system_load.tasks[0].total_runtime;
+
+			/* check if board is connected via USB */
+			struct stat statbuf;
+			//on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0);
 		}
 
+		// XXX remove later
+		//warnx("bat remaining: %2.2f", status.battery_remaining);
+
 		/* if battery voltage is getting lower, warn using buzzer, etc. */
-		if (status.condition_battery_voltage_valid && status.battery_remaining < 0.15f && !low_battery_voltage_actions_done) {
+		if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) {
 			//TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS
 			if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
 				low_battery_voltage_actions_done = true;
 				mavlink_log_critical(mavlink_fd, "[cmd] WARNING: LOW BATTERY");
 				status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING;
 				status_changed = true;
+				battery_tune_played = false;
 			}
 
 			low_voltage_counter++;
@@ -913,7 +887,15 @@ int commander_thread_main(int argc, char *argv[])
 				critical_battery_voltage_actions_done = true;
 				mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY");
 				status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT;
-				arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
+				battery_tune_played = false;
+
+				if (armed.armed) {
+					// XXX not sure what should happen when voltage is low in flight
+					//arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
+				} else {
+					// XXX should we still allow to arm with critical battery?
+					//arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
+				}
 				status_changed = true;
 			}
 
@@ -1240,6 +1222,8 @@ int commander_thread_main(int argc, char *argv[])
 		if (arming_state_changed || main_state_changed || navigation_state_changed) {
 			mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state);
 			status_changed = true;
+		} else {
+			status_changed = false;
 		}
 
 		hrt_abstime t1 = hrt_absolute_time();
@@ -1259,12 +1243,15 @@ int commander_thread_main(int argc, char *argv[])
 			orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
 		}
 
-		/* publish vehicle status at least with 1 Hz */
-		if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) {
+		/* publish states (armed, control mode, vehicle status) at least with 5 Hz */
+		if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) {
 			status.counter++;
 			status.timestamp = t1;
 			orb_publish(ORB_ID(vehicle_status), status_pub, &status);
-			status_changed = false;
+			control_mode.timestamp = t1;
+			orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
+			armed.timestamp = t1;
+			orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
 		}
 
 		/* play arming and battery warning tunes */
@@ -1278,7 +1265,7 @@ int commander_thread_main(int argc, char *argv[])
 			if (tune_low_bat() == OK)
 				battery_tune_played = true;
 
-		} else if (status.battery_remaining == VEHICLE_BATTERY_WARNING_ALERT) {
+		} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
 			/* play tune on battery critical */
 			if (tune_critical_bat() == OK)
 				battery_tune_played = true;
@@ -1299,6 +1286,9 @@ int commander_thread_main(int argc, char *argv[])
 
 		fflush(stdout);
 		counter++;
+
+		toggle_status_leds(&status, &armed, arming_state_changed || status_changed);
+
 		usleep(COMMANDER_MONITORING_INTERVAL);
 	}
 
@@ -1342,46 +1332,94 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val
 }
 
 void
-toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gps_position_s *gps_position)
+toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed)
 {
-	if (leds_counter % 2 == 0) {
-		/* run at 10Hz, full cycle is 16 ticks = 10/16Hz */
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+	/* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */
+	if (armed->armed) {
+		/* armed, solid */
+		led_on(LED_BLUE);
+
+	} else if (armed->ready_to_arm) {
+		/* ready to arm, blink at 1Hz */
+		if (leds_counter % 20 == 0)
+			led_toggle(LED_BLUE);
+	} else {
+		/* not ready to arm, blink at 10Hz */
+		if (leds_counter % 2 == 0)
+			led_toggle(LED_BLUE);
+	}
+#endif
+
+	if (changed) {
+
+		int i;
+		rgbled_pattern_t pattern;
+		memset(&pattern, 0, sizeof(pattern));
+
 		if (armed->armed) {
 			/* armed, solid */
-			led_on(LED_AMBER);
+			if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
+				pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER;
+			} else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
+				pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
+			} else {
+				pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN :RGBLED_COLOR_GREEN;
+			}
+			pattern.duration[0] = 1000;
 
 		} else if (armed->ready_to_arm) {
-			/* ready to arm, blink at 2.5Hz */
-			if (leds_counter & 8) {
-				led_on(LED_AMBER);
+			for (i=0; i<3; i++) {
+				if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
+					pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER;
+				} else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
+					pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
+				} else {
+					pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN;
+				}
+				pattern.duration[i*2] = 200;
 
+				pattern.color[i*2+1] = RGBLED_COLOR_OFF;
+				pattern.duration[i*2+1] = 800;
+			}
+			if (status->condition_global_position_valid) {
+				pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE;
+				pattern.duration[i*2] = 1000;
+				pattern.color[i*2+1] = RGBLED_COLOR_OFF;
+				pattern.duration[i*2+1] = 800;
 			} else {
-				led_off(LED_AMBER);
+				for (i=3; i<6; i++) {
+					pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE;
+					pattern.duration[i*2] = 100;
+					pattern.color[i*2+1] = RGBLED_COLOR_OFF;
+					pattern.duration[i*2+1] = 100;
+				}
+				pattern.color[6*2] = RGBLED_COLOR_OFF;
+				pattern.duration[6*2] = 700;
 			}
 
 		} else {
+			for (i=0; i<3; i++) {
+				pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
+				pattern.duration[i*2] = 200;
+				pattern.color[i*2+1] = RGBLED_COLOR_OFF;
+				pattern.duration[i*2+1] = 200;
+			}
 			/* not ready to arm, blink at 10Hz */
-			led_toggle(LED_AMBER);
 		}
 
-		if (status->condition_global_position_valid) {
-			/* position estimated, solid */
-			led_on(LED_BLUE);
-
-		} else if (leds_counter == 6) {
-			/* waiting for position estimate, short blink at 1.25Hz */
-			led_on(LED_BLUE);
+		rgbled_set_pattern(&pattern);
+	}
 
-		} else {
-			/* no position estimator available, off */
-			led_off(LED_BLUE);
-		}
+	/* give system warnings on error LED, XXX maybe add memory usage warning too */
+	if (status->load > 0.95f) {
+		if (leds_counter % 2 == 0)
+			led_toggle(LED_AMBER);
+	} else {
+		led_off(LED_AMBER);
 	}
 
 	leds_counter++;
-
-	if (leds_counter >= 16)
-		leds_counter = 0;
 }
 
 void
@@ -1595,80 +1633,169 @@ void *commander_low_prio_loop(void *arg)
 	/* Set thread name */
 	prctl(PR_SET_NAME, "commander_low_prio", getpid());
 
-	low_prio_task = LOW_PRIO_TASK_NONE;
+	/* Subscribe to command topic */
+	int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
+	struct vehicle_command_s cmd;
+	memset(&cmd, 0, sizeof(cmd));
+
+	/* wakeup source(s) */
+	struct pollfd fds[1];
+
+	/* use the gyro to pace output - XXX BROKEN if we are using the L3GD20 */
+	fds[0].fd = cmd_sub;
+	fds[0].events = POLLIN;
 
 	while (!thread_should_exit) {
 
-		switch (low_prio_task) {
-		case LOW_PRIO_TASK_PARAM_LOAD:
+		/* wait for up to 100ms for data */
+		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000);
 
-			if (0 == param_load_default()) {
-				mavlink_log_info(mavlink_fd, "[cmd] parameters loaded");
+		/* timed out - periodic check for _task_should_exit, etc. */
+		if (pret == 0)
+			continue;
 
-			} else {
-				mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR");
-				tune_error();
-			}
+		/* 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);
+			continue;
+		}
 
-			low_prio_task = LOW_PRIO_TASK_NONE;
-			break;
+		/* if we reach here, we have a valid command */
+		orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
 
-		case LOW_PRIO_TASK_PARAM_SAVE:
+		/* ignore commands the high-prio loop handles */
+		if (cmd.command == VEHICLE_CMD_DO_SET_MODE ||
+		    cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM)
+			continue;
 
-			if (0 == param_save_default()) {
-				mavlink_log_info(mavlink_fd, "[cmd] parameters saved");
+		/* result of the command */
+		uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED;
 
-			} else {
-				mavlink_log_critical(mavlink_fd, "[cmd] parameters save error");
-				tune_error();
-			}
+		/* only handle low-priority commands here */
+		switch (cmd.command) {
+
+		case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
+				if (is_safe(&status, &safety, &armed)) {
 
-			low_prio_task = LOW_PRIO_TASK_NONE;
+					if (((int)(cmd.param1)) == 1) {
+						/* reboot */
+						systemreset(false);
+					} else if (((int)(cmd.param1)) == 3) {
+						/* reboot to bootloader */
+						systemreset(true);
+					} else {
+						result = VEHICLE_CMD_RESULT_DENIED;
+					}
+				
+				} else {
+					result = VEHICLE_CMD_RESULT_DENIED;
+				}
 			break;
 
-		case LOW_PRIO_TASK_GYRO_CALIBRATION:
+		case VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
 
-			do_gyro_calibration(mavlink_fd);
-			low_prio_task = LOW_PRIO_TASK_NONE;
-			break;
+				/* try to go to INIT/PREFLIGHT arming state */
 
-		case LOW_PRIO_TASK_MAG_CALIBRATION:
+				// XXX disable interrupts in arming_state_transition
+				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) {
+					result = VEHICLE_CMD_RESULT_DENIED;
+					break;
+				}
 
-			do_mag_calibration(mavlink_fd);
-			low_prio_task = LOW_PRIO_TASK_NONE;
-			break;
+				if ((int)(cmd.param1) == 1) {
+					/* gyro calibration */
+					do_gyro_calibration(mavlink_fd);
+					result = VEHICLE_CMD_RESULT_ACCEPTED;
 
-		case LOW_PRIO_TASK_ALTITUDE_CALIBRATION:
+				} else if ((int)(cmd.param2) == 1) {
+					/* magnetometer calibration */
+					do_mag_calibration(mavlink_fd);
+					result = VEHICLE_CMD_RESULT_ACCEPTED;
 
-			// do_baro_calibration(mavlink_fd);
-			low_prio_task = LOW_PRIO_TASK_NONE;
-			break;
+				} else if ((int)(cmd.param3) == 1) {
+					/* zero-altitude pressure calibration */
+					result = VEHICLE_CMD_RESULT_DENIED;
+					
+				} else if ((int)(cmd.param4) == 1) {
+					/* RC calibration */
+					result = VEHICLE_CMD_RESULT_DENIED;
 
-		case LOW_PRIO_TASK_RC_CALIBRATION:
+				} else if ((int)(cmd.param5) == 1) {
+					/* accelerometer calibration */
+					do_accel_calibration(mavlink_fd);
+					result = VEHICLE_CMD_RESULT_ACCEPTED;
 
-			// do_rc_calibration(mavlink_fd);
-			low_prio_task = LOW_PRIO_TASK_NONE;
-			break;
+				} else if ((int)(cmd.param6) == 1) {
+					/* airspeed calibration */
+					do_airspeed_calibration(mavlink_fd);
+					result = VEHICLE_CMD_RESULT_ACCEPTED;
+				}
 
-		case LOW_PRIO_TASK_ACCEL_CALIBRATION:
+				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
 
-			do_accel_calibration(mavlink_fd);
-			low_prio_task = LOW_PRIO_TASK_NONE;
-			break;
+				break;
+			}
 
-		case LOW_PRIO_TASK_AIRSPEED_CALIBRATION:
+		case VEHICLE_CMD_PREFLIGHT_STORAGE: {
 
-			do_airspeed_calibration(mavlink_fd);
-			low_prio_task = LOW_PRIO_TASK_NONE;
-			break;
+				if (((int)(cmd.param1)) == 0) {
+					if (0 == param_load_default()) {
+						mavlink_log_info(mavlink_fd, "[cmd] parameters loaded");
+						result = VEHICLE_CMD_RESULT_ACCEPTED;
+
+					} else {
+						mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR");
+						tune_error();
+						result = VEHICLE_CMD_RESULT_FAILED;
+					}
+
+				} else if (((int)(cmd.param1)) == 1) {
+					if (0 == param_save_default()) {
+						mavlink_log_info(mavlink_fd, "[cmd] parameters saved");
+						result = VEHICLE_CMD_RESULT_ACCEPTED;
+
+					} else {
+						mavlink_log_critical(mavlink_fd, "[cmd] parameters save error");
+						tune_error();
+						result = VEHICLE_CMD_RESULT_FAILED;
+					}
+				}
+
+				break;
+			}
 
-		case LOW_PRIO_TASK_NONE:
 		default:
-			/* slow down to 10Hz */
-			usleep(100000);
 			break;
 		}
 
+		/* supported command handling stop */
+		if (result == VEHICLE_CMD_RESULT_ACCEPTED) {
+			tune_positive();
+
+		} else {
+			tune_negative();
+
+			if (result == VEHICLE_CMD_RESULT_DENIED) {
+				mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command);
+
+			} else if (result == VEHICLE_CMD_RESULT_FAILED) {
+				mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command);
+
+			} else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) {
+				mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command);
+
+			} else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) {
+				mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command);
+			}
+		}
+
+		/* send any requested ACKs */
+		if (cmd.confirmation > 0 && cmd.command != VEHICLE_CMD_DO_SET_MODE
+			&& cmd.command != VEHICLE_CMD_COMPONENT_ARM_DISARM) {
+			/* send acknowledge command */
+			// XXX TODO
+		}
+
 	}
 
 	return 0;