Merged IO feature branch

13 files changed, 253 insertions, 60 deletions

diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 33ed5ebc2..52412e20a 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -72,10 +72,12 @@
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/offboard_control_setpoint.h>
-#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/subsystem_info.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/parameter_update.h>
 #include <mavlink/mavlink_log.h>
 
 #include <systemlib/param/param.h>
@@ -1167,6 +1169,8 @@ int commander_thread_main(int argc, char *argv[])
 	failsafe_lowlevel_timeout_ms = 0;
 	param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms);
 
+	param_t _param_sys_type = param_find("MAV_TYPE");
+
 	/* welcome user */
 	printf("[commander] I am in command now!\n");
 
@@ -1199,6 +1203,8 @@ int commander_thread_main(int argc, char *argv[])
 	/* mark all signals lost as long as they haven't been found */
 	current_status.rc_signal_lost = true;
 	current_status.offboard_control_signal_lost = true;
+	/* allow manual override initially */
+	current_status.flag_external_manual_override_ok = true;
 
 	/* advertise to ORB */
 	stat_pub = orb_advertise(ORB_ID(vehicle_status), &current_status);
@@ -1249,9 +1255,15 @@ int commander_thread_main(int argc, char *argv[])
 	struct offboard_control_setpoint_s sp_offboard;
 	memset(&sp_offboard, 0, sizeof(sp_offboard));
 
-	int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
-	struct vehicle_gps_position_s gps;
-	memset(&gps, 0, sizeof(gps));
+	int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+	struct vehicle_global_position_s global_position;
+	memset(&global_position, 0, sizeof(global_position));
+	uint64_t last_global_position_time = 0;
+
+	int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
+	struct vehicle_local_position_s local_position;
+	memset(&local_position, 0, sizeof(local_position));
+	uint64_t last_local_position_time = 0;
 
 	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
 	struct sensor_combined_s sensors;
@@ -1262,6 +1274,11 @@ int commander_thread_main(int argc, char *argv[])
 	struct vehicle_command_s cmd;
 	memset(&cmd, 0, sizeof(cmd));
 
+	/* Subscribe to parameters changed topic */
+	int param_changed_sub = orb_subscribe(ORB_ID(parameter_update));
+	struct parameter_update_s param_changed;
+	memset(&param_changed, 0, sizeof(param_changed));
+
 	// uint8_t vehicle_state_previous = current_status.state_machine;
 	float voltage_previous = 0.0f;
 
@@ -1288,7 +1305,6 @@ int commander_thread_main(int argc, char *argv[])
 		if (new_data) {
 			orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard);
 		}
-		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);
@@ -1300,6 +1316,46 @@ int commander_thread_main(int argc, char *argv[])
 			handle_command(stat_pub, &current_status, &cmd);
 		}
 
+		/* update parameters */
+		orb_check(param_changed_sub, &new_data);
+		if (new_data) {
+			/* parameters changed */
+			orb_copy(ORB_ID(parameter_update), param_changed_sub, &param_changed);
+
+			/* update parameters */
+			if (!current_status.flag_system_armed) {
+				current_status.system_type = param_get(_param_sys_type, &(current_status.system_type));
+
+				/* disable manual override for all systems that rely on electronic stabilization */
+				if (current_status.system_type == MAV_TYPE_QUADROTOR ||
+				    current_status.system_type == MAV_TYPE_HEXAROTOR ||
+				    current_status.system_type == MAV_TYPE_OCTOROTOR) {
+					current_status.flag_external_manual_override_ok = false;
+				} else {
+					current_status.flag_external_manual_override_ok = true;
+				}
+				printf("param changed, val: %d, override: %s\n", current_status.system_type, (current_status.flag_external_manual_override_ok) ? "ON" : "OFF");
+			} else {
+				printf("ARMED, rejecting sys type change\n");
+			}
+		}
+
+		/* update global position estimate */
+		orb_check(global_position_sub, &new_data);
+		if (new_data) {
+			/* position changed */
+			orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
+			last_global_position_time = global_position.timestamp;
+		}
+
+		/* update local position estimate */
+		orb_check(local_position_sub, &new_data);
+		if (new_data) {
+			/* position changed */
+			orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
+			last_local_position_time = local_position.timestamp;
+		}
+
 		battery_voltage = sensors.battery_voltage_v;
 		battery_voltage_valid = sensors.battery_voltage_valid;
 
@@ -1406,6 +1462,32 @@ int commander_thread_main(int argc, char *argv[])
 
 		/* End battery voltage check */
 
+		bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok;
+		bool global_pos_valid = current_status.flag_global_position_valid;
+		bool local_pos_valid = current_status.flag_local_position_valid;
+
+		/* check for global or local position updates, set a timeout of 2s */
+		if (hrt_absolute_time() - last_global_position_time < 2000000) {
+			current_status.flag_vector_flight_mode_ok = true;
+			current_status.flag_global_position_valid = true;
+			// XXX check for controller status and home position as well
+		}
+		
+		if (hrt_absolute_time() - last_local_position_time < 2000000) {
+			current_status.flag_vector_flight_mode_ok = true;
+			current_status.flag_local_position_valid = true;
+			// XXX check for controller status and home position as well
+		}
+
+		/* consolidate state change, flag as changed if required */
+		if (vector_flight_mode_ok != current_status.flag_vector_flight_mode_ok ||
+		    global_pos_valid != current_status.flag_global_position_valid ||
+		    local_pos_valid != current_status.flag_local_position_valid) {
+			state_changed = true;
+		}
+
+
+
 		/* Check if last transition deserved an audio event */
 // #warning This code depends on state that is no longer? maintained
 // #if 0
@@ -1496,7 +1578,7 @@ int commander_thread_main(int argc, char *argv[])
 				}
 				//printf("RC: y:%i/t:%i s:%i chans: %i\n", rc_yaw_scale, rc_throttle_scale, mode_switch_rc_value, rc.chan_count);
 
-				if (sp_man.aux1_cam_pan_flaps < -STICK_ON_OFF_LIMIT) {
+				if (sp_man.aux1_cam_pan_flaps > STICK_ON_OFF_LIMIT) {
 					update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd);
 				} else {
 					if (sp_man.override_mode_switch > STICK_ON_OFF_LIMIT) {
@@ -1675,7 +1757,7 @@ int commander_thread_main(int argc, char *argv[])
 	buzzer_deinit();
 	close(sp_man_sub);
 	close(sp_offboard_sub);
-	close(gps_sub);
+	close(global_position_sub);
 	close(sensor_sub);
 	close(cmd_sub);
 
diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp
index ff610b0b9..dac0b5e84 100644
--- a/apps/drivers/px4fmu/fmu.cpp
+++ b/apps/drivers/px4fmu/fmu.cpp
@@ -341,8 +341,16 @@ PX4FMU::task_main()
 		/* handle update rate changes */
 		if (_current_update_rate != _update_rate) {
 			int update_rate_in_ms = int(1000 / _update_rate);
-			if (update_rate_in_ms < 2)
+			/* reject faster than 500 Hz updates */
+			if (update_rate_in_ms < 2) {
 				update_rate_in_ms = 2;
+				_update_rate = 500;
+			}
+			/* reject slower than 50 Hz updates */
+			if (update_rate_in_ms > 20) {
+				update_rate_in_ms = 20;
+				_update_rate = 50;
+			}
 			orb_set_interval(_t_actuators, update_rate_in_ms);
 			up_pwm_servo_set_rate(_update_rate);
 			_current_update_rate = _update_rate;
diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
index 31ed89ecc..1fb65413a 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -55,6 +55,7 @@
 #include <unistd.h>
 #include <termios.h>
 #include <fcntl.h>
+#include <math.h>
 
 #include <arch/board/board.h>
 
@@ -73,6 +74,7 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_controls_effective.h>
 #include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/rc_channels.h>
 
 #include <px4io/protocol.h>
@@ -89,10 +91,17 @@ public:
 
 	virtual int		ioctl(file *filp, int cmd, unsigned long arg);
 
+	/**
+	 * Set the PWM via serial update rate
+	 * @warning this directly affects CPU load
+	 */
+	int 			set_pwm_rate(int hz);
+
 	bool			dump_one;
 
 private:
 	static const unsigned	_max_actuators = PX4IO_OUTPUT_CHANNELS;
+	int 			_update_rate;	///< serial send rate in Hz
 
 	int			_serial_fd;	///< serial interface to PX4IO
 	hx_stream_t		_io_stream;	///< HX protocol stream
@@ -109,6 +118,8 @@ private:
 
 	int			_t_armed;	///< system armed control topic
 	actuator_armed_s	_armed;		///< system armed state
+	int 			_t_vstatus;	///< system / vehicle status
+	vehicle_status_s	_vstatus;	///< overall system state
 
 	orb_advert_t 		_to_input_rc;	///< rc inputs from io
 	rc_input_values		_input_rc;	///< rc input values
@@ -182,6 +193,7 @@ PX4IO	*g_dev;
 PX4IO::PX4IO() :
 	CDev("px4io", "/dev/px4io"),
 	dump_one(false),
+	_update_rate(50),
 	_serial_fd(-1),
 	_io_stream(nullptr),
 	_task(-1),
@@ -190,6 +202,7 @@ PX4IO::PX4IO() :
 	_t_actuators(-1),
 	_t_actuators_effective(-1),
 	_t_armed(-1),
+	_t_vstatus(-1),
 	_t_outputs(-1),
 	_mixers(nullptr),
 	_primary_pwm_device(false),
@@ -264,6 +277,17 @@ PX4IO::init()
 	return OK;
 }
 
+int
+PX4IO::set_pwm_rate(int hz)
+{
+	if (hz > 0 && hz <= 400) {
+		_update_rate = hz;
+		return OK;
+	} else {
+		return -EINVAL;
+	}
+}
+
 void
 PX4IO::task_main_trampoline(int argc, char *argv[])
 {
@@ -274,6 +298,7 @@ void
 PX4IO::task_main()
 {
 	log("starting");
+	int update_rate_in_ms;
 
 	/* open the serial port */
 	_serial_fd = ::open("/dev/ttyS2", O_RDWR);
@@ -311,12 +336,15 @@ PX4IO::task_main()
 	_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
 				     ORB_ID(actuator_controls_1));
 	/* convert the update rate in hz to milliseconds, rounding down if necessary */
-	//int update_rate_in_ms = int(1000 / _update_rate);
-	orb_set_interval(_t_actuators, 20);	/* XXX 50Hz hardcoded for now */
+	update_rate_in_ms = int(1000 / _update_rate);
+	orb_set_interval(_t_actuators, update_rate_in_ms);
 
 	_t_armed = orb_subscribe(ORB_ID(actuator_armed));
 	orb_set_interval(_t_armed, 200);		/* 5Hz update rate */
 
+	_t_vstatus = orb_subscribe(ORB_ID(vehicle_status));
+	orb_set_interval(_t_vstatus, 200);		/* 5Hz update rate max. */
+
 	/* advertise the limited control inputs */
 	memset(&_controls_effective, 0, sizeof(_controls_effective));
 	_t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_1),
@@ -332,13 +360,15 @@ PX4IO::task_main()
 	_to_input_rc = orb_advertise(ORB_ID(input_rc), &_input_rc);
 
 	/* poll descriptor */
-	pollfd fds[3];
+	pollfd fds[4];
 	fds[0].fd = _serial_fd;
 	fds[0].events = POLLIN;
 	fds[1].fd = _t_actuators;
 	fds[1].events = POLLIN;
 	fds[2].fd = _t_armed;
 	fds[2].events = POLLIN;
+	fds[3].fd = _t_vstatus;
+	fds[3].events = POLLIN;
 
 	log("ready");
 
@@ -367,7 +397,8 @@ PX4IO::task_main()
 			if (fds[1].revents & POLLIN) {
 
 				/* get controls */
-				orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
+				orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
+				     ORB_ID(actuator_controls_1), _t_actuators, &_controls);
 
 				/* mix */
 				if (_mixers != nullptr) {
@@ -381,8 +412,19 @@ PX4IO::task_main()
 
 
 					/* convert to PWM values */
-					for (unsigned i = 0; i < _max_actuators; i++)
-						_outputs.output[i] = 1500 + (600 * _outputs.output[i]);
+					for (unsigned i = 0; i < _max_actuators; i++) {
+						/* last resort: catch NaN, INF and out-of-band errors */
+						if (isfinite(_outputs.output[i]) && _outputs.output[i] >= -1.0f && _outputs.output[i] <= 1.0f) {
+							_outputs.output[i] = 1500 + (600 * _outputs.output[i]);
+						} else {
+							/*
+							 * Value is NaN, INF or out of band - set to the minimum value.
+							 * This will be clearly visible on the servo status and will limit the risk of accidentally
+							 * spinning motors. It would be deadly in flight.
+							 */
+							_outputs.output[i] = 900;
+						}
+					}
 
 					/* and flag for update */
 					_send_needed = true;
@@ -394,6 +436,11 @@ PX4IO::task_main()
 				orb_copy(ORB_ID(actuator_armed), _t_armed, &_armed);
 				_send_needed = true;
 			}
+
+			if (fds[3].revents & POLLIN) {
+				orb_copy(ORB_ID(vehicle_status), _t_vstatus, &_vstatus);
+				_send_needed = true;
+			}
 		}
 
 		/* send an update to IO if required */
@@ -528,8 +575,14 @@ PX4IO::io_send()
 
 	// XXX relays
 
-	/* armed and not locked down */
+	/* armed and not locked down -> arming ok */
 	cmd.arm_ok = (_armed.armed && !_armed.lockdown);
+	/* indicate that full autonomous position control / vector flight mode is available */
+	cmd.vector_flight_mode_ok = _vstatus.flag_vector_flight_mode_ok;
+	/* allow manual override on IO (not allowed for multirotors or other systems with SAS) */
+	cmd.manual_override_ok = _vstatus.flag_external_manual_override_ok;
+	/* set desired PWM output rate */
+	cmd.servo_rate = _update_rate;
 
 	ret = hx_stream_send(_io_stream, &cmd, sizeof(cmd));
 	if (ret)
@@ -697,7 +750,7 @@ test(void)
 void
 monitor(void)
 {
-	unsigned cancels = 4;
+	unsigned cancels = 3;
 	printf("Hit <enter> three times to exit monitor mode\n");
 
 	for (;;) {
@@ -718,6 +771,7 @@ monitor(void)
 			g_dev->dump_one = true;
 	}
 }
+
 }
 
 int
@@ -739,6 +793,16 @@ px4io_main(int argc, char *argv[])
 			errx(1, "driver init failed");
 		}
 
+		/* look for the optional pwm update rate for the supported modes */
+		if (strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0) {
+			if (argc > 2 + 1) {
+				g_dev->set_pwm_rate(atoi(argv[2 + 1]));
+			} else {
+				fprintf(stderr, "missing argument for pwm update rate (-u)\n");
+				return 1;
+			}
+		}
+
 		exit(0);
 	}
 
diff --git a/apps/fixedwing_att_control/fixedwing_att_control_main.c b/apps/fixedwing_att_control/fixedwing_att_control_main.c
index 8ca87ab53..3e1fc4952 100644
--- a/apps/fixedwing_att_control/fixedwing_att_control_main.c
+++ b/apps/fixedwing_att_control/fixedwing_att_control_main.c
@@ -183,13 +183,11 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 			gyro[1] = att.pitchspeed;
 			gyro[2] = att.yawspeed;
 
-			/* Control */
-			if (vstatus.state_machine == SYSTEM_STATE_AUTO) {
+			/* control */
 
+			if (vstatus.state_machine == SYSTEM_STATE_AUTO) {
 				/* attitude control */
 				fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
-				/* publish rate setpoint */
-				orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
 
 				/* angular rate control */
 				fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
@@ -219,11 +217,9 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 					att_sp.timestamp = hrt_absolute_time();
 				}
 
+				/* attitude control */
 				fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
 
-				/* publish rate setpoint */
-				orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
-
 				/* angular rate control */
 				fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
 
@@ -242,8 +238,14 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 			/* publish rates */
 			orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
 
-			/* publish actuator outputs */
-			orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
+			/* sanity check and publish actuator outputs */
+			if (isfinite(actuators.control[0]) &&
+			    isfinite(actuators.control[1]) &&
+			    isfinite(actuators.control[2]) &&
+			    isfinite(actuators.control[3]))
+			{
+				orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
+			}
 		}
 
 		printf("[fixedwing_att_control] exiting, stopping all motors.\n");
diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c
index fb59d07a1..58761e89c 100644
--- a/apps/mavlink/mavlink_receiver.c
+++ b/apps/mavlink/mavlink_receiver.c
@@ -261,8 +261,8 @@ handle_message(mavlink_message_t *msg)
 
 			offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid-1]   / (float)INT16_MAX;
 			offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid-1]  / (float)INT16_MAX;
-			offboard_control_sp.p3= (float)quad_motors_setpoint.yaw[mavlink_system.sysid-1]    / (float)INT16_MAX;
-			offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid-1]/(float)UINT16_MAX;
+			offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid-1]    / (float)INT16_MAX;
+			offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid-1] / (float)UINT16_MAX;
 
 			if (quad_motors_setpoint.thrust[mavlink_system.sysid-1] == 0) {
 				ml_armed = false;
@@ -301,7 +301,7 @@ handle_message(mavlink_message_t *msg)
 			/* Calculate Rotation Matrix */
 			//TODO: better clarification which app does this, atm we have a ekf for quadrotors which does this, but there is no such thing if fly in fixed wing mode
 
-			if(mavlink_system.type == MAV_TYPE_FIXED_WING) {
+			if (mavlink_system.type == MAV_TYPE_FIXED_WING) {
 				//TODO: assuming low pitch and roll values for now
 				hil_attitude.R[0][0] = cosf(hil_state.yaw);
 				hil_attitude.R[0][1] = sinf(hil_state.yaw);
diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c
index 5033aeb98..0fcf952ab 100644
--- a/apps/px4io/comms.c
+++ b/apps/px4io/comms.c
@@ -130,8 +130,10 @@ comms_main(void)
 			last_report_time = now;
 
 			/* populate the report */
-			for (unsigned i = 0; i < system_state.rc_channels; i++)
+			for (unsigned i = 0; i < system_state.rc_channels; i++) {
 				report.rc_channel[i] = system_state.rc_channel_data[i];
+			}
+
 			report.channel_count = system_state.rc_channels;
 			report.armed = system_state.armed;
 
@@ -168,26 +170,41 @@ comms_handle_command(const void *buffer, size_t length)
 	irqstate_t flags = irqsave();
 
 	/* fetch new PWM output values */
-	for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++)
+	for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++) {
 		system_state.fmu_channel_data[i] = cmd->servo_command[i];
+	}
 
-	/* if the IO is armed and FMU gets disarmed, IO must also disarm */
-	if(system_state.arm_ok && !cmd->arm_ok) {
+	/* if IO is armed and FMU gets disarmed, IO must also disarm */
+	if (system_state.arm_ok && !cmd->arm_ok) {
 		system_state.armed = false;
 	}
 
 	system_state.arm_ok = cmd->arm_ok;
+	system_state.vector_flight_mode_ok = cmd->vector_flight_mode_ok;
+	system_state.manual_override_ok = cmd->manual_override_ok;
 	system_state.mixer_fmu_available = true;
 	system_state.fmu_data_received = true;
 
+	/* set PWM update rate if changed (after limiting) */
+	uint16_t new_servo_rate = cmd->servo_rate;
 
-	/* handle changes signalled by FMU */
-//	if (!system_state.arm_ok && system_state.armed)
-//		system_state.armed = false;
+	/* reject faster than 500 Hz updates */
+	if (new_servo_rate > 500) {
+		new_servo_rate = 500;
+	}
+	/* reject slower than 50 Hz updates */
+	if (new_servo_rate < 50) {
+		new_servo_rate = 50;
+	}
+	if (system_state.servo_rate != new_servo_rate) {
+		up_pwm_servo_set_rate(new_servo_rate);
+		system_state.servo_rate = new_servo_rate;
+	}
 
 	/* XXX do relay changes here */	
-	for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++)
+	for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) {
 		system_state.relays[i] = cmd->relay_state[i];
+	}
 
 	irqrestore(flags);
 }
diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c
index e64f8b204..9db9a0fa5 100644
--- a/apps/px4io/controls.c
+++ b/apps/px4io/controls.c
@@ -110,12 +110,13 @@ controls_main(void)
 		if (!locked)
 			ppm_input();
 
-		/* force manual input override */
+		/* check for manual override status */
 		if (system_state.rc_channel_data[4] > RC_CHANNEL_HIGH_THRESH) {
-			system_state.mixer_use_fmu = false;
+			/* force manual input override */
+			system_state.mixer_manual_override = true;
 		} else {
 			/* override not engaged, use FMU */
-			system_state.mixer_use_fmu = true;
+			system_state.mixer_manual_override = false;
 		}
 
 		/*
diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c
index 4fa3e1d5d..5a644906b 100644
--- a/apps/px4io/mixer.c
+++ b/apps/px4io/mixer.c
@@ -85,7 +85,7 @@ mixer_tick(void)
 	/*
 	 * Decide which set of inputs we're using.
 	 */
-	if (system_state.mixer_use_fmu && system_state.mixer_fmu_available) {
+	if (!system_state.mixer_manual_override && system_state.mixer_fmu_available) {
 		/* we have recent control data from the FMU */
 		control_count = PX4IO_OUTPUT_CHANNELS;
 		control_values = &system_state.fmu_channel_data[0];
@@ -96,21 +96,22 @@ mixer_tick(void)
 
 			/* too many frames without FMU input, time to go to failsafe */
 			if (fmu_input_drops >= FMU_INPUT_DROP_LIMIT) {
-				system_state.mixer_use_fmu = false;
+				system_state.mixer_manual_override = true;
+				system_state.mixer_fmu_available = false;
 			}
 		} else {
 			fmu_input_drops = 0;
 			system_state.fmu_data_received = false;
 		}
 
-	} else if (system_state.rc_channels > 0) {
+	} else if (system_state.rc_channels > 0 && system_state.manual_override_ok) {
 		/* we have control data from an R/C input */
 		control_count = system_state.rc_channels;
 		control_values = &system_state.rc_channel_data[0];
-
 	} else {
 		/* we have no control input */
-		/* XXX builtin failsafe would activate here */
+
+		// XXX builtin failsafe would activate here 
 		control_count = 0;
 	}
 
diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h
index c704b1201..0bf6d4997 100644
--- a/apps/px4io/protocol.h
+++ b/apps/px4io/protocol.h
@@ -52,9 +52,12 @@ struct px4io_command {
 	uint16_t	f2i_magic;
 #define F2I_MAGIC	0x636d
 
-	uint16_t	servo_command[PX4IO_OUTPUT_CHANNELS];
-	bool		relay_state[PX4IO_RELAY_CHANNELS];
-	bool		arm_ok;
+	uint16_t	servo_command[PX4IO_OUTPUT_CHANNELS];	/**< servo output channels */
+	uint16_t	servo_rate;				/**< PWM output rate in Hz */
+	bool		relay_state[PX4IO_RELAY_CHANNELS];	/**< relay states as requested by FMU */
+	bool		arm_ok;					/**< FMU allows full arming */
+	bool		vector_flight_mode_ok;			/**< FMU aquired a valid position lock, ready for pos control */
+	bool		manual_override_ok;			/**< if true, IO performs a direct manual override */ 
 };
 
 /* config message from FMU to IO */
diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c
index a3ac9e3e7..4f68b3d2d 100644
--- a/apps/px4io/px4io.c
+++ b/apps/px4io/px4io.c
@@ -61,6 +61,8 @@ int user_start(int argc, char *argv[])
 {
 	/* reset all to zero */
 	memset(&system_state, 0, sizeof(system_state));
+	/* default to 50 Hz PWM outputs */
+	system_state.servo_rate = 50;
 
 	/* configure the high-resolution time/callout interface */
 	hrt_init();
diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h
index e0b7cdda1..84f5ba029 100644
--- a/apps/px4io/px4io.h
+++ b/apps/px4io/px4io.h
@@ -71,6 +71,7 @@ struct sys_state_s
 
 	bool		armed;			/* IO armed */
 	bool		arm_ok;			/* FMU says OK to arm */
+	uint16_t	servo_rate;		/* output rate of servos in Hz */
 
 	/**
 	 * Data from the remote control input(s)
@@ -92,7 +93,7 @@ struct sys_state_s
 	/**
 	 * If true, we are using the FMU controls, else RC input if available.
 	 */
-	bool		mixer_use_fmu;
+	bool		mixer_manual_override;
 
 	/**
 	 * If true, FMU input is available.
@@ -124,6 +125,16 @@ struct sys_state_s
 	 * If true, the connection to FMU has been lost for more than a timeout interval
 	 */
 	bool		fmu_lost;
+
+	/**
+	 * If true, FMU is ready for autonomous position control. Only used for LED indication
+	 */
+	bool vector_flight_mode_ok;
+
+	/**
+	 * If true, IO performs an on-board manual override with the RC channel values
+	 */
+	bool manual_override_ok;
 };
 
 extern struct sys_state_s system_state;
diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c
index 88e7aeac6..9ce4589b7 100644
--- a/apps/px4io/safety.c
+++ b/apps/px4io/safety.c
@@ -63,15 +63,16 @@ static unsigned counter = 0;
 /*
  * Define the various LED flash sequences for each system state.
  */
-#define LED_PATTERN_SAFE 			0xffff		// always on
-#define LED_PATTERN_FMU_ARMED 		0x4444		// slow blinking
-#define LED_PATTERN_IO_ARMED 		0x5555		// fast blinking 
-#define LED_PATTERN_IO_FMU_ARMED 	0x5050		// long off then double blink
+#define LED_PATTERN_SAFE 			0xffff		/**< always on 				*/
+#define LED_PATTERN_VECTOR_FLIGHT_MODE_OK 	0x3000		/**< always on with short break 	*/
+#define LED_PATTERN_FMU_ARMED 			0x4444		/**< slow blinking			*/
+#define LED_PATTERN_IO_ARMED 			0x5555		/**< fast blinking 			*/
+#define LED_PATTERN_IO_FMU_ARMED 		0x5050		/**< long off then double blink 	*/
 
 static unsigned blink_counter = 0;
 
 #define ARM_COUNTER_THRESHOLD	10
-#define DISARM_COUNTER_THRESHOLD	2
+#define DISARM_COUNTER_THRESHOLD	4
 
 static bool safety_button_pressed;
 
@@ -101,10 +102,6 @@ safety_check_button(void *arg)
 	 */
 	safety_button_pressed = BUTTON_SAFETY;
 
-	if(safety_button_pressed) {
-		//printf("Pressed, Arm counter: %d, Disarm counter: %d\n", arm_counter, disarm_counter);
-	}
-
 	/* Keep pressed for a while to arm */
 	if (safety_button_pressed && !system_state.armed) {
 		if (counter < ARM_COUNTER_THRESHOLD) {
@@ -139,6 +136,8 @@ safety_check_button(void *arg)
 		}
 	} else if (system_state.arm_ok) {
 		pattern = LED_PATTERN_FMU_ARMED;
+	} else if (system_state.vector_flight_mode_ok) {
+		pattern = LED_PATTERN_VECTOR_FLIGHT_MODE_OK;
 	}
 
 	/* Turn the LED on if we have a 1 at the current bit position */
@@ -165,7 +164,7 @@ failsafe_blink(void *arg)
 	static bool failsafe = false;
 
 	/* blink the failsafe LED if we don't have FMU input */
-	if (!system_state.mixer_use_fmu) {
+	if (!system_state.mixer_fmu_available) {
 		failsafe = !failsafe;
 	} else {
 		failsafe = false;
diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h
index b70b322d8..738ca644f 100644
--- a/apps/uORB/topics/vehicle_status.h
+++ b/apps/uORB/topics/vehicle_status.h
@@ -116,6 +116,7 @@ struct vehicle_status_s
 	commander_state_machine_t state_machine;	/**< current flight state, main state machine */
 	enum VEHICLE_FLIGHT_MODE flight_mode;		/**< current flight mode, as defined by mode switch */
 	enum VEHICLE_ATTITUDE_MODE attitute_mode;	/**< current attitude control mode, as defined by VEHICLE_ATTITUDE_MODE enum */
+	int32_t system_type;				/**< system type, inspired by MAVLinks MAV_TYPE enum */
 
 	// uint8_t mode;
 
@@ -165,8 +166,10 @@ struct vehicle_status_s
 	uint16_t errors_count4;
 
 //	bool remote_manual; /**< set to true by the commander when the manual-switch on the remote is set to manual */
-	bool gps_valid;     /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */
-
+	bool flag_global_position_valid;     /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */
+	bool flag_local_position_valid;
+	bool flag_vector_flight_mode_ok;			/**< position estimation, battery voltage and other critical subsystems are good for autonomous flight */
+	bool flag_external_manual_override_ok;
 };
 
 /**