Minor but important fixes across system

8 files changed, 239 insertions, 156 deletions

diff --git a/apps/ardrone_control/ardrone_control.c b/apps/ardrone_control/ardrone_control.c
index e49a0ae29..f80fdf971 100644
--- a/apps/ardrone_control/ardrone_control.c
+++ b/apps/ardrone_control/ardrone_control.c
@@ -166,6 +166,9 @@ int ardrone_control_main(int argc, char *argv[])
 	ar_init_motors(ardrone_write, &gpios);
 	int counter = 0;
 
+	/* Led animation */
+	int led_counter = 0;
+
 	/* pthread for position control */
 	pthread_t position_control_thread;
 	position_control_thread_started = false;
@@ -223,41 +226,46 @@ int ardrone_control_main(int argc, char *argv[])
 
 		}
 
-		//fancy led animation...
-		static int blubb = 0;
-
-		if (counter % 20 == 0) {
-			if (blubb == 0) ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0);
+		if (counter % 30 == 0) {
+			if (led_counter == 0) ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0);
 
-			if (blubb == 1) ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0 , 0);
+			if (led_counter == 1) ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0 , 0);
 
-			if (blubb == 2) ar_set_leds(ardrone_write, 1, 0, 0, 0, 0, 0, 0 , 0);
+			if (led_counter == 2) ar_set_leds(ardrone_write, 1, 0, 0, 0, 0, 0, 0 , 0);
 
-			if (blubb == 3) ar_set_leds(ardrone_write, 0, 0, 0, 1, 0, 0, 0 , 0);
+			if (led_counter == 3) ar_set_leds(ardrone_write, 0, 0, 0, 1, 0, 0, 0 , 0);
 
-			if (blubb == 4) ar_set_leds(ardrone_write, 0, 0, 1, 1, 0, 0, 0 , 0);
+			if (led_counter == 4) ar_set_leds(ardrone_write, 0, 0, 1, 1, 0, 0, 0 , 0);
 
-			if (blubb == 5) ar_set_leds(ardrone_write, 0, 0, 1, 0, 0, 0, 0 , 0);
+			if (led_counter == 5) ar_set_leds(ardrone_write, 0, 0, 1, 0, 0, 0, 0 , 0);
 
-			if (blubb == 6) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 1, 0 , 0);
+			if (led_counter == 6) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 1, 0 , 0);
 
-			if (blubb == 7) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 1, 0 , 0);
+			if (led_counter == 7) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 1, 0 , 0);
 
-			if (blubb == 8) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 0, 0 , 0);
+			if (led_counter == 8) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 0, 0 , 0);
 
-			if (blubb == 9) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 0 , 1);
+			if (led_counter == 9) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 0 , 1);
 
-			if (blubb == 10) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 1);
+			if (led_counter == 10) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 1);
 
-			if (blubb == 11) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 0);
+			if (led_counter == 11) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 0);
 
-			blubb++;
+			led_counter++;
 
-			if (blubb == 12) blubb = 0;
+			if (led_counter == 12) led_counter = 0;
 		}
 
 		/* run at approximately 200 Hz */
 		usleep(5000);
+
+		// This is a hardcore debug code piece to validate
+		// the motor interface
+		// uint8_t motorSpeedBuf[5] = {1, 2, 3, 4, 5};
+		// ar_get_motor_packet(motorSpeedBuf, 20, 20, 20, 20);
+		// write(ardrone_write, motorSpeedBuf, 5);
+		// usleep(15000);
+
 		counter++;
 	}
 
diff --git a/apps/ardrone_control/ardrone_motor_control.c b/apps/ardrone_control/ardrone_motor_control.c
index 25847e1da..f13427eea 100644
--- a/apps/ardrone_control/ardrone_motor_control.c
+++ b/apps/ardrone_control/ardrone_motor_control.c
@@ -33,7 +33,8 @@
  ****************************************************************************/
 
 /*
- * @file Implementation of AR.Drone 1.0 / 2.0 motor control interface
+ * @file ardrone_motor_control.c
+ * Implementation of AR.Drone 1.0 / 2.0 motor control interface
  */
 
 
@@ -97,23 +98,23 @@ int ar_multiplexing_init()
 {
 	int		fd;
 	
-	fd = open("/dev/gpio", O_RDONLY | O_NONBLOCK);
+	fd = open(GPIO_DEVICE_PATH, 0);
 
 	if (fd < 0) {
 		printf("GPIO: open fail\n");
 		return fd;
 	}
 
+	/* deactivate all outputs */
+	int ret = 0;
+	ret += ioctl(fd, GPIO_SET, motor_gpios);
+
 	if (ioctl(fd, GPIO_SET_OUTPUT, motor_gpios) != 0) {
 		printf("GPIO: output set fail\n");
 		close(fd);
 		return -1;
 	}
 
-	/* deactivate all outputs */
-	int ret = 0;
-	ret += ioctl(fd, GPIO_SET, motor_gpios);
-
 	if (ret < 0) {
 		printf("GPIO: clearing pins fail\n");
 		close(fd);
@@ -167,12 +168,15 @@ int ar_select_motor(int fd, uint8_t motor)
 		ret += ioctl(fd, GPIO_CLEAR, motor_gpios);
 
 	} else {
+		/* deselect all */
+		ret += ioctl(fd, GPIO_SET, motor_gpios);
+
 		/* select reqested motor */	
 		ret += ioctl(fd, GPIO_CLEAR, motor_gpio[motor - 1]);
 
 		/* deselect all others */
-		gpioset = motor_gpios ^ motor_gpio[motor - 1];
-		ret += ioctl(fd, GPIO_SET, gpioset);
+		// gpioset = motor_gpios ^ motor_gpio[motor - 1];
+		// ret += ioctl(fd, GPIO_SET, gpioset);
 	}
 
 	return ret;
diff --git a/apps/ardrone_control/attitude_control.c b/apps/ardrone_control/attitude_control.c
index 2d2596d6d..f96d22fe6 100644
--- a/apps/ardrone_control/attitude_control.c
+++ b/apps/ardrone_control/attitude_control.c
@@ -93,7 +93,7 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit
 	static PID_t nick_controller;
 	static PID_t roll_controller;
 
-	static const float min_gas = 1;
+	static const float min_gas = 10;
 	static const float max_gas = 512;
 	static uint16_t motor_pwm[4] = {0, 0, 0, 0};
 	static float motor_calc[4] = {0.0f, 0.0f, 0.0f, 0.0f};
@@ -201,11 +201,11 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit
 
 		// don't turn around the wrong side (only works if yaw angle is between +- 180 degree)
 		if (yaw_e > M_PI) {
-			yaw_e -= 2.0f * M_PI;
+			yaw_e -= 2.0f * M_PI_F;
 		}
 
 		if (yaw_e < -M_PI) {
-			yaw_e += 2.0f * M_PI;
+			yaw_e += 2.0f * M_PI_F;
 		}
 
 		attitude_setpoint_navigationframe_from_positioncontroller.z = pid_calculate(&yaw_pos_controller, 0, yaw_e, 0, CONTROL_PID_ATTITUDE_INTERVAL);
@@ -234,9 +234,9 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit
 		attitude_setpoint_bodyframe.z = attitude_setpoint_bodyframe_from_positioncontroller.z;
 
 	} else if (current_state == SYSTEM_STATE_MANUAL) {
-		attitude_setpoint_bodyframe.x = -((float)rc->chan[rc->function[ROLL]].scale / 10000.0f) * 3.14159265 / 8.0f;
-		attitude_setpoint_bodyframe.y = -((float)rc->chan[rc->function[PITCH]].scale / 10000.0f) * 3.14159265 / 8.0f;
-		attitude_setpoint_bodyframe.z = -((float)rc->chan[rc->function[YAW]].scale / 10000.0f) * 3.14159265;
+		attitude_setpoint_bodyframe.x = -((float)rc->chan[rc->function[ROLL]].scale / 10000.0f) * M_PI_F / 8.0f;
+		attitude_setpoint_bodyframe.y = -((float)rc->chan[rc->function[PITCH]].scale / 10000.0f) * M_PI_F / 8.0f;
+		attitude_setpoint_bodyframe.z = -((float)rc->chan[rc->function[YAW]].scale / 10000.0f) * M_PI_F;
 	}
 
 	/* add an attitude offset which needs to be estimated somewhere */
@@ -282,13 +282,13 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit
 
 	// FLYING MODES
 	if (current_state == SYSTEM_STATE_MANUAL) {
-		motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale;
+		motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale / 20000.0f;
 
 	} else if (current_state == SYSTEM_STATE_GROUND_READY || current_state == SYSTEM_STATE_STABILIZED || current_state == SYSTEM_STATE_AUTO || current_state == SYSTEM_STATE_MISSION_ABORT) {
-		motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale;	//TODO
+		motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale / 20000.0f;	//TODO
 
 	} else if (current_state == SYSTEM_STATE_EMCY_LANDING) {
-		motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale;	//TODO
+		motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale / 20000.0f;	//TODO
 
 	} else if (current_state == SYSTEM_STATE_EMCY_CUTOFF) {
 		motor_thrust = 0;	//immediately cut off thrust!
@@ -297,10 +297,12 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit
 		motor_thrust = 0;	// Motor thrust must be zero in any other mode!
 	}
 
+	if (status->rc_signal_lost) motor_thrust = 0;
+
 	// Convertion to motor-step units
 	motor_thrust *= zcompensation;
-	motor_thrust *= max_gas / 20000.0f; //TODO: check this
-	motor_thrust += (max_gas - min_gas) / 2.f;
+	/* scale up from 0..1 to 10..512) */
+	motor_thrust *= ((float)max_gas - min_gas);
 
 	//limit control output
 	//yawspeed
@@ -418,10 +420,25 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit
 	motor_pwm[2] = (uint16_t) motor_calc[2];
 	motor_pwm[3] = (uint16_t) motor_calc[3];
 
+	// Keep motors spinning while armed
+
+	motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : 10;
+	motor_pwm[1] = (motor_pwm[1] > 0) ? motor_pwm[1] : 10;
+	motor_pwm[2] = (motor_pwm[2] > 0) ? motor_pwm[2] : 10;
+	motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : 10;
+
+	/* Failsafe logic - should never be necessary */
+	motor_pwm[0] = (motor_pwm[0] <= 512) ? motor_pwm[0] : 512;
+	motor_pwm[1] = (motor_pwm[1] <= 512) ? motor_pwm[1] : 512;
+	motor_pwm[2] = (motor_pwm[2] <= 512) ? motor_pwm[2] : 512;
+	motor_pwm[3] = (motor_pwm[3] <= 512) ? motor_pwm[3] : 512;
+
 	//SEND MOTOR COMMANDS
-	uint8_t motorSpeedBuf[5] = {1, 2, 3, 4, 5};
-	ar_get_motor_packet(motorSpeedBuf, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
-	write(ardrone_write, motorSpeedBuf, 5);
+	if (motor_skip_counter % 5 == 0) {
+		uint8_t motorSpeedBuf[5] = {1, 2, 3, 4, 5};
+		ar_get_motor_packet(motorSpeedBuf, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
+		write(ardrone_write, motorSpeedBuf, 5);
+	}
 
 	motor_skip_counter++;
 
diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 3236c2313..dc5647223 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -82,6 +82,7 @@ extern struct system_load_s system_load;
 #define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
 
 #define STICK_ON_OFF_LIMIT 7500
+#define STICK_THRUST_RANGE 20000
 #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
 
@@ -239,7 +240,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *current_status)
 	struct sensor_combined_s raw;
 
 	/* 30 seconds */
-	const uint64_t calibration_interval_us = 10 * 1000000;
+	const uint64_t calibration_interval_us = 45 * 1000000;
 	unsigned int calibration_counter = 0;
 
 	const int peak_samples = 2000;
@@ -266,6 +267,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *current_status)
 		mag_minima[1][i] = INT16_MAX;
 		mag_minima[2][i] = INT16_MAX;
 	}
+
+	mavlink_log_info(mavlink_fd, "[commander] Please rotate in all directions.");
 	
 	uint64_t calibration_start = hrt_absolute_time();
 	while ((hrt_absolute_time() - calibration_start) < calibration_interval_us) {
@@ -455,35 +458,47 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 	switch (cmd->command) {
 		case MAV_CMD_DO_SET_MODE:
 		{
-			update_state_machine_mode_request(status_pub, current_vehicle_status, (uint8_t)cmd->param1);
+			if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, (uint8_t)cmd->param1)) {
+				result = MAV_RESULT_ACCEPTED;
+			} else {
+				result = MAV_RESULT_DENIED;
+			}
+		}
+		break;
+
+		case MAV_CMD_COMPONENT_ARM_DISARM:
+		{
+			/* request to arm */
+			if ((int)cmd->param1 == 1) {
+				if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
+					result = MAV_RESULT_ACCEPTED;
+				} else {
+					result = MAV_RESULT_DENIED;
+				}
+			/* request to disarm */
+			} else if ((int)cmd->param1 == 0) {
+				if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
+					result = MAV_RESULT_ACCEPTED;
+				} else {
+					result = MAV_RESULT_DENIED;
+				}
+			}
+		}
+		break;
+
+		/* request for an autopilot reboot */
+		case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
+		{
+			// if ((int)cmd->param1 == 1) {
+			// 	if (OK == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_HALT)) {
+			// 		result = MAV_RESULT_ACCEPTED;//TODO: this has no effect
+			// 	} else {
+			// 		result = MAV_RESULT_DENIED;
+			// 	}
+			// }
+
 		}
 		break;
-//
-//		case MAV_CMD_COMPONENT_ARM_DISARM:
-//		{
-//			/* request to arm */
-//			if (cmd->param1 == 1.0f) {
-//				if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED))
-//					result = MAV_RESULT_ACCEPTED;
-//			/* request to disarm */
-//			} else if (cmd->param1 == 0.0f) {
-//				if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_STANDBY))
-//					result = MAV_RESULT_ACCEPTED;
-//			}
-//		}
-//		break;
-//
-//		/* request for an autopilot reboot */
-//		case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
-//		{
-//			if (cmd->param1 == 1.0f) {
-//				if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_HALT)) {
-//					result = MAV_RESULT_ACCEPTED;//TODO: this has no effect
-//				}
-//			}
-//
-//		}
-//		break;
 //
 //		/* request to land */
 //		case MAV_CMD_NAV_LAND:
@@ -523,7 +538,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 					do_state_update(status_pub, &current_status, SYSTEM_STATE_STANDBY);
 					result = MAV_RESULT_ACCEPTED;
 				} else {
-					mavlink_log_info(mavlink_fd, "[commander] REJECTING gyro calibration");
+					mavlink_log_critical(mavlink_fd, "[commander] REJECTING gyro calibration");
 					result = MAV_RESULT_DENIED;
 				}
 				handled = true;
@@ -540,7 +555,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 					do_state_update(status_pub, &current_status, SYSTEM_STATE_STANDBY);
 					result = MAV_RESULT_ACCEPTED;
 				} else {
-					mavlink_log_info(mavlink_fd, "[commander] REJECTING mag calibration");
+					mavlink_log_critical(mavlink_fd, "[commander] REJECTING mag calibration");
 					result = MAV_RESULT_DENIED;
 				}
 				handled = true;
@@ -837,13 +852,15 @@ int commander_main(int argc, char *argv[])
 	memset(&sensors, 0, sizeof(sensors));
 
 	uint8_t vehicle_state_previous = current_status.state_machine;
+	float voltage_previous = 0.0f;
 
 	uint64_t last_idle_time = 0;
+	unsigned int signal_loss_counter = 0;
 
 	/* now initialized */
 	commander_initialized = true;
 
-	while (1) { //TODO: this while loop needs cleanup, split into sub-functions
+	while (1) {
 
 		/* Get current values */
 		orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
@@ -859,7 +876,9 @@ int commander_main(int argc, char *argv[])
 		/* Slow but important 5 Hz checks */
 		if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) {
 			/* toggle activity (blue) led at 1 Hz in standby, 10 Hz in armed mode */
-			if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY || current_status.state_machine == SYSTEM_STATE_AUTO  || current_status.state_machine == SYSTEM_STATE_MANUAL)) {
+			if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY ||
+			     current_status.state_machine == SYSTEM_STATE_AUTO  ||
+			     current_status.state_machine == SYSTEM_STATE_MANUAL)) {
 				/* armed */
 				led_toggle(LED_BLUE);
 
@@ -888,7 +907,7 @@ int commander_main(int argc, char *argv[])
 			}
 
 			if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
-				//compute system load
+				/* compute system load */
 				uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time;
 
 				if (last_idle_time > 0)
@@ -978,7 +997,6 @@ int commander_main(int argc, char *argv[])
 		/* Check battery voltage */
 		/* write to sys_status */
 		current_status.voltage_battery = battery_voltage;
-		orb_publish(ORB_ID(vehicle_status), stat_pub, &current_status);
 
 		/* if battery voltage is getting lower, warn using buzzer, etc.  */
 		if (battery_voltage_valid && (battery_voltage < VOLTAGE_BATTERY_LOW_VOLTS && false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS
@@ -1010,44 +1028,67 @@ int commander_main(int argc, char *argv[])
 
 		/* Start RC state check */
 
-		int16_t chan3_scale =  rc.chan[rc.function[YAW]].scale;
-		int16_t chan2_scale =  rc.chan[rc.function[THROTTLE]].scale;
+		if (rc.chan_count > 4 && (hrt_absolute_time() - rc.timestamp) < 100000) {
 
-		/* check if left stick is in lower left position --> switch to standby state */
-		if (chan3_scale > STICK_ON_OFF_LIMIT && chan2_scale < -STICK_ON_OFF_LIMIT) { //TODO: remove hardcoded values
-			if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
-				update_state_machine_disarm(stat_pub, &current_status);
-				stick_on_counter = 0;
+			/* quadrotor specific logic - check against system type in the future */
 
-			} else {
-				stick_off_counter++;
-				stick_on_counter = 0;
+			int16_t rc_yaw_scale =  rc.chan[rc.function[YAW]].scale;
+			int16_t rc_throttle_scale =  rc.chan[rc.function[THROTTLE]].scale;
+			int16_t mode_switch_rc_value = rc.chan[rc.function[OVERRIDE]].scale;
+
+			/* check if left stick is in lower left position --> switch to standby state */
+			if (rc_yaw_scale < -STICK_ON_OFF_LIMIT && rc_throttle_scale < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values
+				if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
+					update_state_machine_disarm(stat_pub, &current_status);
+					stick_on_counter = 0;
+
+				} else {
+					stick_off_counter++;
+					stick_on_counter = 0;
+				}
 			}
-		}
 
-		/* check if left stick is in lower right position --> arm */
-		if (chan3_scale < -STICK_ON_OFF_LIMIT && chan2_scale < -STICK_ON_OFF_LIMIT) { //TODO: remove hardcoded values
-			if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
-				update_state_machine_arm(stat_pub, &current_status);
-				stick_on_counter = 0;
+			/* check if left stick is in lower right position --> arm */
+			if (rc_yaw_scale > STICK_ON_OFF_LIMIT && rc_throttle_scale < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values
+				if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
+					update_state_machine_arm(stat_pub, &current_status);
+					stick_on_counter = 0;
 
-			} else {
-				stick_on_counter++;
-				stick_off_counter = 0;
+				} else {
+					stick_on_counter++;
+					stick_off_counter = 0;
+				}
 			}
-		}
+			//printf("RC: y:%i/t:%i s:%i chans: %i\n", rc_yaw_scale, rc_throttle_scale, mode_switch_rc_value, rc.chan_count);
 
-		/* Check the value of the rc channel of the mode switch */
-		mode_switch_rc_value = rc.chan[rc.function[OVERRIDE]].scale;
+			/* Check the value of the rc channel of the mode switch */
+			mode_switch_rc_value = rc.chan[rc.function[OVERRIDE]].scale;
 
-		if (mode_switch_rc_value > STICK_ON_OFF_LIMIT) {
-			update_state_machine_mode_manual(stat_pub, &current_status);
+			if (mode_switch_rc_value > STICK_ON_OFF_LIMIT) {
+				update_state_machine_mode_manual(stat_pub, &current_status);
 
-		} else if (mode_switch_rc_value < -STICK_ON_OFF_LIMIT) {
-			update_state_machine_mode_auto(stat_pub, &current_status);
+			} else if (mode_switch_rc_value < -STICK_ON_OFF_LIMIT) {
+				update_state_machine_mode_auto(stat_pub, &current_status);
+
+			} else {
+				update_state_machine_mode_stabilized(stat_pub, &current_status);
+			}
+
+			/* handle the case where RC signal was regained */
+			if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[commander] RECOVERY - RC SIGNAL GAINED!");
+			current_status.rc_signal_lost = false;
+			current_status.rc_signal_lost_interval = 0;
 
 		} else {
-			update_state_machine_mode_stabilized(stat_pub, &current_status);
+			static uint64_t last_print_time = 0;
+			/* print error message for first RC glitch and then every 2 s / 2000 ms) */
+			if (!current_status.rc_signal_lost || ((hrt_absolute_time() - last_print_time) > 3000000)) {
+				mavlink_log_critical(mavlink_fd, "[commander] CRITICAL - NO REMOTE SIGNAL!");
+				last_print_time = hrt_absolute_time();
+			}
+			/* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
+			current_status.rc_signal_lost = true;
+			current_status.rc_signal_lost_interval = hrt_absolute_time() - rc.timestamp;
 		}
 
 		/* End mode switch */
@@ -1057,12 +1098,19 @@ int commander_main(int argc, char *argv[])
 
 		current_status.counter++;
 		current_status.timestamp = hrt_absolute_time();
-		orb_publish(ORB_ID(vehicle_status), stat_pub, &current_status);
-
-
+		if (voltage_previous != current_status.voltage_battery) orb_publish(ORB_ID(vehicle_status), stat_pub, &current_status);
+
+		/* If full run came back clean, transition to standby */
+		if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT &&
+		    current_status.preflight_gyro_calibration == false &&
+		    current_status.preflight_mag_calibration == false) {
+		    	/* All ok, no calibration going on, go to standby */
+			do_state_update(stat_pub, &current_status, SYSTEM_STATE_STANDBY);
+		}
 
 		/* Store old modes to detect and act on state transitions */
 		vehicle_state_previous = current_status.state_machine;
+		voltage_previous = current_status.voltage_battery;
 
 		fflush(stdout);
 		counter++;
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c
index a71bb797d..ebae1c61c 100644
--- a/apps/commander/state_machine_helper.c
+++ b/apps/commander/state_machine_helper.c
@@ -417,6 +417,7 @@ void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_
 
 void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status)
 {
+	// XXX CHANGE BACK
 	if (current_status->state_machine == SYSTEM_STATE_STANDBY) {
 		printf("[commander] arming\n");
 		do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
@@ -466,61 +467,55 @@ void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *cur
 
 uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, uint8_t mode)
 {
-	commander_state_machine_t current_system_state = current_status->state_machine;
-
 	printf("in update state request\n");
 	uint8_t ret = 1;
 
-	/* Switch on HIL if in standby */
-	if ((current_system_state == SYSTEM_STATE_STANDBY) && (mode & MAV_MODE_FLAG_HIL_ENABLED)) {
-		/* Enable HIL on request */
-		current_status->mode |= MAV_MODE_FLAG_HIL_ENABLED;
-		ret = OK;
-		state_machine_publish(status_pub, current_status);
-		printf("[commander] Enabling HIL\n");
-	}
-
-	/* NEVER actually switch off HIL without reboot */
-	if ((current_status->mode & MAV_MODE_FLAG_HIL_ENABLED) && !(mode & MAV_MODE_FLAG_HIL_ENABLED)) {
-		fprintf(stderr, "[commander] DENYING request to switch of HIL. Please power cycle (safety reasons)\n");
-		ret = ERROR;
-	}
+				current_status->mode |= MAV_MODE_FLAG_SAFETY_ARMED;
+			/* Set manual input enabled flag */
+			current_status->mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+			do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
 
-	//TODO: clarify mapping between mavlink enum MAV_MODE and the state machine, then add more decisions to the switch (see also the system_state_loop function in mavlink.c)
-	switch (mode) {
-	case MAV_MODE_MANUAL_ARMED: //= SYSTEM_STATE_ARMED
-		if (current_system_state == SYSTEM_STATE_STANDBY) {
+	/* vehicle is disarmed, mode requests arming */
+	if (!(current_status->mode & VEHICLE_MODE_FLAG_SAFETY_ARMED) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
+		/* only arm in standby state */
+		// XXX REMOVE
+		if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
 			/* Set armed flag */
 			current_status->mode |= MAV_MODE_FLAG_SAFETY_ARMED;
 			/* Set manual input enabled flag */
 			current_status->mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
 			do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
 			ret = OK;
+			printf("[commander] arming due to command request\n");
 		}
+	}
 
-		break;
-
-	case MAV_MODE_MANUAL_DISARMED:
-		if (current_system_state == SYSTEM_STATE_GROUND_READY) {
+	/* vehicle is armed, mode requests disarming */
+	//if ((current_status->mode & VEHICLE_MODE_FLAG_SAFETY_ARMED) && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
+		/* only disarm in ground ready */
+		//if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
 			/* Clear armed flag, leave manual input enabled */
-			current_status->mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
-			do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
-			ret = OK;
-		}
-
-		break;
+		//	current_status->mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
+		//	do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
+		//	ret = OK;
+		//	printf("[commander] disarming due to command request\n");
+		//}
+	//}
 
-	default:
-		break;
+	/* Switch on HIL if in standby */
+	if ((current_status->state_machine == SYSTEM_STATE_STANDBY) && (mode & MAV_MODE_FLAG_HIL_ENABLED)) {
+		/* Enable HIL on request */
+		current_status->mode |= MAV_MODE_FLAG_HIL_ENABLED;
+		ret = OK;
+		state_machine_publish(status_pub, current_status);
+		printf("[commander] Enabling HIL\n");
 	}
 
-	#warning STATE MACHINE IS INCOMPLETE HERE
-
-//	if(ret != 0) //Debug
-//	{
-//		strcpy(mavlink_statustext_msg_content.values[0].string_value, "Commander: command rejected");
-//		global_data_send_mavlink_message_out(&mavlink_statustext_msg_content);
-//	}
+	/* NEVER actually switch off HIL without reboot */
+	if ((current_status->mode & MAV_MODE_FLAG_HIL_ENABLED) && !(mode & MAV_MODE_FLAG_HIL_ENABLED)) {
+		fprintf(stderr, "[commander] DENYING request to switch of HIL. Please power cycle (safety reasons)\n");
+		ret = ERROR;
+	}
 
 	return ret;
 }
diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c
index 461b9805d..b456c4dfb 100644
--- a/apps/sensors/sensors.c
+++ b/apps/sensors/sensors.c
@@ -116,6 +116,7 @@ static void sensors_timer_loop(void *arg);
 #ifdef CONFIG_HRT_PPM
 extern uint16_t ppm_buffer[];
 extern unsigned ppm_decoded_channels;
+extern uint64_t ppm_last_valid_decode;
 #endif
 
 /* file handle that will be used for subscribing */
@@ -705,21 +706,23 @@ int sensors_main(int argc, char *argv[])
 			/* PPM */
 			if (ppmcounter == 5) {
 
-				/* Read out values from HRT */
-				for (int i = 0; i < ppm_decoded_channels; i++) {
-					rc.chan[i].raw = ppm_buffer[i];
-					/* Set the range to +-, then scale up */
-					rc.chan[i].scale = (ppm_buffer[i] - rc.chan[i].mid) * rc.chan[i].scaling_factor;
-				}
-
-				rc.chan_count = ppm_decoded_channels;
-
-				rc.timestamp = hrt_absolute_time();
-				/* publish a few lines of code later if set to true */
-				ppm_updated = true;
+				/* require at least two channels
+				 * to consider the signal valid
+				 */
+				if (ppm_decoded_channels > 1 && (hrt_absolute_time() - ppm_last_valid_decode) < 45000) {
+					/* Read out values from HRT */
+					for (int i = 0; i < ppm_decoded_channels; i++) {
+						rc.chan[i].raw = ppm_buffer[i];
+						/* Set the range to +-, then scale up */
+						rc.chan[i].scale = (ppm_buffer[i] - rc.chan[i].mid) * rc.chan[i].scaling_factor;
+					}
 
+					rc.chan_count = ppm_decoded_channels;
 
-				//TODO: XXX check the mode switch channel and eventually send a request to the commander (see implementation in commander and mavlink)
+					rc.timestamp = ppm_last_valid_decode;
+					/* publish a few lines of code later if set to true */
+					ppm_updated = true;
+				}
 				ppmcounter = 0;
 			}
 
diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h
index d92b80046..933b58e8b 100644
--- a/apps/uORB/topics/vehicle_status.h
+++ b/apps/uORB/topics/vehicle_status.h
@@ -110,6 +110,9 @@ struct vehicle_status_s
 	bool preflight_gyro_calibration;		/**< true if gyro calibration is requested */
 	bool preflight_mag_calibration;			/**< true if mag calibration is requested */
 
+	bool rc_signal_lost;				/**< true if no operator override channel is available */
+	uint64_t rc_signal_lost_interval;		/**< interval in microseconds since when no RC signal is available */
+
 	/* see SYS_STATUS mavlink message for the following */
 	uint32_t onboard_control_sensors_present;
 	uint32_t onboard_control_sensors_enabled;
diff --git a/nuttx/configs/px4fmu/src/up_hrt.c b/nuttx/configs/px4fmu/src/up_hrt.c
index 35aecfe08..507358f8b 100644
--- a/nuttx/configs/px4fmu/src/up_hrt.c
+++ b/nuttx/configs/px4fmu/src/up_hrt.c
@@ -310,6 +310,7 @@ static void		hrt_call_invoke(void);
 #define PPM_MAX_CHANNELS	12
 uint16_t ppm_buffer[PPM_MAX_CHANNELS];
 unsigned ppm_decoded_channels;
+uint64_t ppm_last_valid_decode = 0;
 
 /* PPM edge history */
 uint16_t ppm_edge_history[32];
@@ -427,6 +428,8 @@ hrt_ppm_decode(uint32_t status)
 			for (i = 0; i < ppm.next_channel && i < PPM_MAX_CHANNELS; i++)
 				ppm_buffer[i] = ppm_temp_buffer[i];
 			ppm_decoded_channels = i;
+			ppm_last_valid_decode = hrt_absolute_time();
+
 		}
 
 		/* reset for the next frame */
@@ -485,6 +488,8 @@ hrt_ppm_decode(uint32_t status)
 error:
 	/* we don't like the state of the decoder, reset it and try again */
 	ppm.phase = UNSYNCH;
+	ppm_decoded_channels = 0;
+
 }
 #endif /* CONFIG_HRT_PPM */