Merge branch 'master' into inav_fix

21 files changed, 278 insertions, 126 deletions

diff --git a/src/drivers/drv_gps.h b/src/drivers/drv_gps.h
index 1dda8ef0b..398cf4870 100644
--- a/src/drivers/drv_gps.h
+++ b/src/drivers/drv_gps.h
@@ -51,8 +51,7 @@
 typedef enum {
 	GPS_DRIVER_MODE_NONE = 0,
 	GPS_DRIVER_MODE_UBX,
-	GPS_DRIVER_MODE_MTK,
-	GPS_DRIVER_MODE_NMEA,
+	GPS_DRIVER_MODE_MTK
 } gps_driver_mode_t;
 
 
diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h
index caf2b0f6e..2fab37dd2 100644
--- a/src/drivers/drv_tone_alarm.h
+++ b/src/drivers/drv_tone_alarm.h
@@ -144,6 +144,7 @@ enum {
 	TONE_ARMING_WARNING_TUNE,
 	TONE_BATTERY_WARNING_SLOW_TUNE,
 	TONE_BATTERY_WARNING_FAST_TUNE,
+	TONE_GPS_WARNING_TUNE,
 	TONE_NUMBER_OF_TUNES
 };
 
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index a84cb8e59..fc500a9ec 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -279,10 +279,6 @@ GPS::task_main()
 			_Helper = new MTK(_serial_fd, &_report);
 			break;
 
-		case GPS_DRIVER_MODE_NMEA:
-			//_Helper = new NMEA(); //TODO: add NMEA
-			break;
-
 		default:
 			break;
 		}
@@ -328,10 +324,6 @@ GPS::task_main()
 						mode_str = "MTK";
 						break;
 
-					case GPS_DRIVER_MODE_NMEA:
-						mode_str = "NMEA";
-						break;
-
 					default:
 						break;
 					}
@@ -362,9 +354,6 @@ GPS::task_main()
 			_mode = GPS_DRIVER_MODE_UBX;
 			break;
 
-			//				case GPS_DRIVER_MODE_NMEA:
-			//					_mode = GPS_DRIVER_MODE_UBX;
-			//					break;
 		default:
 			break;
 		}
@@ -400,10 +389,6 @@ GPS::print_info()
 		warnx("protocol: MTK");
 		break;
 
-	case GPS_DRIVER_MODE_NMEA:
-		warnx("protocol: NMEA");
-		break;
-
 	default:
 		break;
 	}
diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp
index 0b25b379f..86291901c 100644
--- a/src/drivers/gps/ubx.cpp
+++ b/src/drivers/gps/ubx.cpp
@@ -281,7 +281,6 @@ UBX::receive(unsigned timeout)
 				return 1;
 
 			} else {
-				warnx("ubx: timeout - no messages");
 				return -1;
 			}
 
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 6d4019f24..b1dd55dd7 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -1096,10 +1096,11 @@ fmu_start(void)
 void
 test(void)
 {
-	int	fd;
+	int	 fd;
         unsigned servo_count = 0;
 	unsigned pwm_value = 1000;
 	int	 direction = 1;
+	int	 ret;
         
 	fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
 
@@ -1114,9 +1115,9 @@ test(void)
 
 	warnx("Testing %u servos", (unsigned)servo_count);
 
-	int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
-	if (!console)
-		err(1, "failed opening console");
+	struct pollfd fds;
+	fds.fd = 0; /* stdin */
+	fds.events = POLLIN;
 
 	warnx("Press CTRL-C or 'c' to abort.");
 
@@ -1166,15 +1167,17 @@ test(void)
 
 		/* Check if user wants to quit */
 		char c;
-		if (read(console, &c, 1) == 1) {
-			if (c == 0x03 || c == 0x63) {
+		ret = poll(&fds, 1, 0);
+		if (ret > 0) {
+
+			read(0, &c, 1);
+			if (c == 0x03 || c == 0x63 || c == 'q') {
 				warnx("User abort\n");
                                 break;
 			}
 		}
 	}
 
-        close(console);
 	close(fd);
 
 	exit(0);
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index b66d425dd..133646051 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -591,6 +591,9 @@ PX4IO::init()
 	if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) &&
 	    (reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
 
+		/* get a status update from IO */
+		io_get_status();
+
 	    	mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART");
 	    	log("INAIR RESTART RECOVERY (needs commander app running)");
 
@@ -1509,7 +1512,7 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
 			 * even.
 			 */
 			unsigned total_len = sizeof(px4io_mixdata) + count;
-			if (total_len % 1) {
+			if (total_len % 2) {
 				msg->text[count] = '\0';
 				total_len++;
 			}
@@ -2128,10 +2131,9 @@ test(void)
 	if (ioctl(fd, PWM_SERVO_ARM, 0))
 		err(1, "failed to arm servos");
 
-	/* Open console directly to grab CTRL-C signal */
-	int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
-	if (!console)
-		err(1, "failed opening console");
+	struct pollfd fds;
+	fds.fd = 0; /* stdin */
+	fds.events = POLLIN;
 
 	warnx("Press CTRL-C or 'c' to abort.");
 
@@ -2172,10 +2174,12 @@ test(void)
 
 		/* Check if user wants to quit */
 		char c;
-		if (read(console, &c, 1) == 1) {
+		ret = poll(&fds, 1, 0);
+		if (ret > 0) {
+
+			read(0, &c, 1);
 			if (c == 0x03 || c == 0x63 || c == 'q') {
 				warnx("User abort\n");
-				close(console);
 				exit(0);
 			}
 		}
diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
index 930809036..f36f2091e 100644
--- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp
+++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
@@ -333,6 +333,7 @@ ToneAlarm::ToneAlarm() :
 	_default_tunes[TONE_ARMING_WARNING_TUNE] = "MNT75L1O2G";					//arming warning
 	_default_tunes[TONE_BATTERY_WARNING_SLOW_TUNE] = "MBNT100a8";					//battery warning slow
 	_default_tunes[TONE_BATTERY_WARNING_FAST_TUNE] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8";	//battery warning fast
+	_default_tunes[TONE_GPS_WARNING_TUNE] = "MFT255L4AAAL1F#";					//gps warning slow
 
 	_tune_names[TONE_STARTUP_TUNE] = "startup";			// startup tune
 	_tune_names[TONE_ERROR_TUNE] = "error";				// ERROR tone
@@ -342,6 +343,7 @@ ToneAlarm::ToneAlarm() :
 	_tune_names[TONE_ARMING_WARNING_TUNE] = "arming";		// arming warning
 	_tune_names[TONE_BATTERY_WARNING_SLOW_TUNE] = "slow_bat";	// battery warning slow
 	_tune_names[TONE_BATTERY_WARNING_FAST_TUNE] = "fast_bat";	// battery warning fast
+	_tune_names[TONE_GPS_WARNING_TUNE] = "gps_warning";	            // gps warning
 }
 
 ToneAlarm::~ToneAlarm()
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 1e86e8e24..01b7b84d0 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -470,8 +470,26 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		}
 
 	case VEHICLE_CMD_COMPONENT_ARM_DISARM:
-		// XXX implement later
-		result = VEHICLE_CMD_RESULT_DENIED;
+	{
+		transition_result_t arming_res = TRANSITION_NOT_CHANGED;
+		if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) {
+			if (safety->safety_switch_available && !safety->safety_off) {
+				print_reject_arm("NOT ARMING: Press safety switch first.");
+				arming_res = TRANSITION_DENIED;
+
+			} else {
+				arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
+			}
+
+			if (arming_res == TRANSITION_CHANGED) {
+				mavlink_log_info(mavlink_fd, "[cmd] ARMED by component arm cmd");
+				result = VEHICLE_CMD_RESULT_ACCEPTED;
+			} else {
+				mavlink_log_info(mavlink_fd, "[cmd] REJECTING component arm cmd");
+				result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+			}
+		}
+	}
 		break;
 
 	default:
@@ -806,16 +824,6 @@ int commander_thread_main(int argc, char *argv[])
 
 		check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status.condition_airspeed_valid), &status_changed);
 
-		orb_check(cmd_sub, &updated);
-
-		if (updated) {
-			/* got command */
-			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
-
-			/* handle it */
-			handle_command(&status, &safety, &control_mode, &cmd, &armed);
-		}
-
 		/* update safety topic */
 		orb_check(safety_sub, &updated);
 
@@ -1147,6 +1155,18 @@ int commander_thread_main(int argc, char *argv[])
 			}
 		}
 
+
+		/* handle commands last, as the system needs to be updated to handle them */
+		orb_check(cmd_sub, &updated);
+
+		if (updated) {
+			/* got command */
+			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
+
+			/* handle it */
+			handle_command(&status, &safety, &control_mode, &cmd, &armed);
+		}
+
 		/* evaluate the navigation state machine */
 		transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position);
 
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index cbcd4adfb..7a5c2dab2 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -531,7 +531,7 @@ int mavlink_thread_main(int argc, char *argv[])
 		case 'b':
 			baudrate = strtoul(optarg, NULL, 10);
 
-			if (baudrate == 0)
+			if (baudrate < 9600 || baudrate > 921600)
 				errx(1, "invalid baud rate '%s'", optarg);
 
 			break;
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index a3ef1d63b..7dd9e321f 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -102,6 +102,7 @@ struct vehicle_global_position_s hil_global_pos;
 struct vehicle_attitude_s hil_attitude;
 struct vehicle_gps_position_s hil_gps;
 struct sensor_combined_s hil_sensors;
+struct battery_status_s	hil_battery_status;
 static orb_advert_t pub_hil_global_pos = -1;
 static orb_advert_t pub_hil_attitude = -1;
 static orb_advert_t pub_hil_gps = -1;
@@ -111,6 +112,12 @@ static orb_advert_t pub_hil_accel = -1;
 static orb_advert_t pub_hil_mag = -1;
 static orb_advert_t pub_hil_baro = -1;
 static orb_advert_t pub_hil_airspeed = -1;
+static orb_advert_t pub_hil_battery = -1;
+
+/* packet counter */
+static int hil_counter = 0;
+static int hil_frames = 0;
+static uint64_t old_timestamp = 0;
 
 static orb_advert_t cmd_pub = -1;
 static orb_advert_t flow_pub = -1;
@@ -149,7 +156,8 @@ handle_message(mavlink_message_t *msg)
 				vcmd.param5 = cmd_mavlink.param5;
 				vcmd.param6 = cmd_mavlink.param6;
 				vcmd.param7 = cmd_mavlink.param7;
-				vcmd.command = cmd_mavlink.command;
+				// XXX do proper translation
+				vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command;
 				vcmd.target_system = cmd_mavlink.target_system;
 				vcmd.target_component = cmd_mavlink.target_component;
 				vcmd.source_system = msg->sysid;
@@ -207,7 +215,7 @@ handle_message(mavlink_message_t *msg)
 		vcmd.param5 = 0;
 		vcmd.param6 = 0;
 		vcmd.param7 = 0;
-		vcmd.command = MAV_CMD_DO_SET_MODE;
+		vcmd.command = VEHICLE_CMD_DO_SET_MODE;
 		vcmd.target_system = new_mode.target_system;
 		vcmd.target_component = MAV_COMP_ID_ALL;
 		vcmd.source_system = msg->sysid;
@@ -360,11 +368,6 @@ handle_message(mavlink_message_t *msg)
 			mavlink_hil_sensor_t imu;
 			mavlink_msg_hil_sensor_decode(msg, &imu);
 
-			/* packet counter */
-			static uint16_t hil_counter = 0;
-			static uint16_t hil_frames = 0;
-			static uint64_t old_timestamp = 0;
-
 			/* sensors general */
 			hil_sensors.timestamp = hrt_absolute_time();
 
@@ -391,9 +394,9 @@ handle_message(mavlink_message_t *msg)
 			hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16
 
 			/* adc */
-			hil_sensors.adc_voltage_v[0] = 0;
-			hil_sensors.adc_voltage_v[1] = 0;
-			hil_sensors.adc_voltage_v[2] = 0;
+			hil_sensors.adc_voltage_v[0] = 0.0f;
+			hil_sensors.adc_voltage_v[1] = 0.0f;
+			hil_sensors.adc_voltage_v[2] = 0.0f;
 
 			/* magnetometer */
 			float mga2ga = 1.0e-3f;
@@ -506,6 +509,18 @@ handle_message(mavlink_message_t *msg)
 				pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
 			}
 
+			/* fill in HIL battery status */
+			hil_battery_status.timestamp = hrt_absolute_time();
+			hil_battery_status.voltage_v = 11.1f;
+			hil_battery_status.current_a = 10.0f;
+
+			/* lazily publish the battery voltage */
+			if (pub_hil_battery > 0) {
+				orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status);
+			} else {
+				pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
+			}
+
 			// increment counters
 			hil_counter++;
 			hil_frames++;
@@ -640,6 +655,18 @@ handle_message(mavlink_message_t *msg)
 			} else {
 				orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel);
 			}
+
+			/* fill in HIL battery status */
+			hil_battery_status.timestamp = hrt_absolute_time();
+			hil_battery_status.voltage_v = 11.1f;
+			hil_battery_status.current_a = 10.0f;
+
+			/* lazily publish the battery voltage */
+			if (pub_hil_battery > 0) {
+				orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status);
+			} else {
+				pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
+			}
 		}
 
 		if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h
index e2e630046..c5cd0d03e 100644
--- a/src/modules/mavlink/orb_topics.h
+++ b/src/modules/mavlink/orb_topics.h
@@ -65,6 +65,7 @@
 #include <uORB/topics/telemetry_status.h>
 #include <uORB/topics/debug_key_value.h>
 #include <uORB/topics/airspeed.h>
+#include <uORB/topics/battery_status.h>
 #include <drivers/drv_rc_input.h>
 
 struct mavlink_subscriptions {
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
index a8add5d73..53cbf4430 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
@@ -199,7 +199,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 	struct vehicle_local_position_setpoint_s local_pos_sp;
 	memset(&local_pos_sp, 0, sizeof(local_pos_sp));
 	struct vehicle_global_position_setpoint_s global_pos_sp;
-	memset(&global_pos_sp, 0, sizeof(local_pos_sp));
+	memset(&global_pos_sp, 0, sizeof(global_pos_sp));
 	struct vehicle_global_velocity_setpoint_s global_vel_sp;
 	memset(&global_vel_sp, 0, sizeof(global_vel_sp));
 
@@ -414,6 +414,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 					}
 				}
 
+				/* copy yaw setpoint to vehicle_local_position_setpoint topic */
 				local_pos_sp.yaw = att_sp.yaw_body;
 
 				/* local position setpoint is valid and can be used for auto loiter after position controlled mode */
@@ -436,7 +437,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 						local_pos_sp.x = local_pos.x;
 						local_pos_sp.y = local_pos.y;
 						local_pos_sp.z = - params.takeoff_alt - params.takeoff_gap;
-						local_pos_sp.yaw = att.yaw;
 						att_sp.yaw_body = att.yaw;
 						mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z);
 					}
@@ -477,8 +477,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 							} else {
 								local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
 							}
-
-							local_pos_sp.yaw = global_pos_sp.yaw;
 							att_sp.yaw_body = global_pos_sp.yaw;
 
 							mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y);
@@ -491,7 +489,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 								local_pos_sp.x = local_pos.x;
 								local_pos_sp.y = local_pos.y;
 								local_pos_sp.yaw = att.yaw;
-								att_sp.yaw_body = global_pos_sp.yaw;
 							}
 
 							if (reset_auto_sp_z) {
@@ -515,6 +512,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 					reset_mission_sp = true;
 				}
 
+				/* copy yaw setpoint to vehicle_local_position_setpoint topic */
+				local_pos_sp.yaw = att_sp.yaw_body;
+
 				/* reset setpoints after AUTO mode */
 				reset_man_sp_xy = true;
 				reset_man_sp_z = true;
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 617b536f4..5c621cfb2 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -286,7 +286,7 @@ controls_tick() {
 		 * requested override.
 		 *
 		 */
-		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) > RC_CHANNEL_HIGH_THRESH))
+		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) < RC_CHANNEL_LOW_THRESH))
 			override = true;
 
 		if (override) {
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 0edd91b24..30aff7d20 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -221,7 +221,7 @@ mixer_tick(void)
 		}
 
 		/* do the calculations during the ramp for all at once */
-		if(esc_state == ESC_RAMP) {
+		if (esc_state == ESC_RAMP) {
 			ramp_promille = (1000*(hrt_elapsed_time(&esc_init_time)-ESC_INIT_TIME_US))/ESC_RAMP_TIME_US;
 		}
 
diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp
index df0dfe838..b1bb1a66d 100644
--- a/src/modules/systemlib/mixer/mixer.cpp
+++ b/src/modules/systemlib/mixer/mixer.cpp
@@ -142,6 +142,22 @@ NullMixer *
 NullMixer::from_text(const char *buf, unsigned &buflen)
 {
 	NullMixer *nm = nullptr;
+	const char *end = buf + buflen;
+
+	/* enforce that the mixer ends with space or a new line */
+	for (int i = buflen - 1; i >= 0; i--) {
+		if (buf[i] == '\0')
+			continue;
+
+		/* require a space or newline at the end of the buffer, fail on printable chars */
+		if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') {
+			/* found a line ending or space, so no split symbols / numbers. good. */
+			break;
+		} else {
+			return nm;
+		}
+
+	}
 
 	if ((buflen >= 2) && (buf[0] == 'Z') && (buf[1] == ':')) {
 		nm = new NullMixer;
diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp
index 8ded0b05c..2446cc3fb 100644
--- a/src/modules/systemlib/mixer/mixer_multirotor.cpp
+++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp
@@ -181,6 +181,23 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
 	char geomname[8];
 	int s[4];
 	int used;
+	const char *end = buf + buflen;
+
+	/* enforce that the mixer ends with space or a new line */
+	for (int i = buflen - 1; i >= 0; i--) {
+		if (buf[i] == '\0')
+			continue;
+
+		/* require a space or newline at the end of the buffer, fail on printable chars */
+		if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') {
+			/* found a line ending or space, so no split symbols / numbers. good. */
+			break;
+		} else {
+			debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen-1, buf[i]);
+			return nullptr;
+		}
+
+	}
 
 	if (sscanf(buf, "R: %s %d %d %d %d%n", geomname, &s[0], &s[1], &s[2], &s[3], &used) != 5) {
 		debug("multirotor parse failed on '%s'", buf);
diff --git a/src/modules/systemlib/mixer/mixer_simple.cpp b/src/modules/systemlib/mixer/mixer_simple.cpp
index 07dc5f37f..c8434f991 100644
--- a/src/modules/systemlib/mixer/mixer_simple.cpp
+++ b/src/modules/systemlib/mixer/mixer_simple.cpp
@@ -55,7 +55,7 @@
 #include "mixer.h"
 
 #define debug(fmt, args...)	do { } while(0)
-//#define debug(fmt, args...)	do { printf("[mixer] " fmt "\n", ##args); } while(0)
+// #define debug(fmt, args...)	do { printf("[mixer] " fmt "\n", ##args); } while(0)
 
 SimpleMixer::SimpleMixer(ControlCallback control_cb,
 			 uintptr_t cb_handle,
@@ -98,14 +98,17 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler
 {
 	int ret;
 	int s[5];
+	int n = -1;
 	
 	buf = findtag(buf, buflen, 'O');
-	if ((buf == nullptr) || (buflen < 12))
+	if ((buf == nullptr) || (buflen < 12)) {
+		debug("output parser failed finding tag, ret: '%s'", buf);
 		return -1;
+	}
 
-	if ((ret = sscanf(buf, "O: %d %d %d %d %d",
-			  &s[0], &s[1], &s[2], &s[3], &s[4])) != 5) {
-		debug("scaler parse failed on '%s' (got %d)", buf, ret);
+	if ((ret = sscanf(buf, "O: %d %d %d %d %d %n",
+			  &s[0], &s[1], &s[2], &s[3], &s[4], &n)) != 5) {
+		debug("out scaler parse failed on '%s' (got %d, consumed %d)", buf, ret, n);
 		return -1;
 	}
 	skipline(buf, buflen);
@@ -126,8 +129,10 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale
 	int s[5];
 
 	buf = findtag(buf, buflen, 'S');
-	if ((buf == nullptr) || (buflen < 16))
+	if ((buf == nullptr) || (buflen < 16)) {
+		debug("contorl parser failed finding tag, ret: '%s'", buf);
 		return -1;
+	}
 
 	if (sscanf(buf, "S: %u %u %d %d %d %d %d",
 		   &u[0], &u[1], &s[0], &s[1], &s[2], &s[3], &s[4]) != 7) {
@@ -156,6 +161,22 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c
 	int used;
 	const char *end = buf + buflen;
 
+	/* enforce that the mixer ends with space or a new line */
+	for (int i = buflen - 1; i >= 0; i--) {
+		if (buf[i] == '\0')
+			continue;
+
+		/* require a space or newline at the end of the buffer, fail on printable chars */
+		if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') {
+			/* found a line ending or space, so no split symbols / numbers. good. */
+			break;
+		} else {
+			debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen-1, buf[i]);
+			goto out;
+		}
+
+	}
+
 	/* get the base info for the mixer */
 	if (sscanf(buf, "M: %u%n", &inputs, &used) != 1) {
 		debug("simple parse failed on '%s'", buf);
@@ -173,22 +194,27 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c
 
 	mixinfo->control_count = inputs;
 
-	if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler))
+	if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler)) {
+		debug("simple mixer parser failed parsing out scaler tag, ret: '%s'", buf);
 		goto out;
+	}
 
 	for (unsigned i = 0; i < inputs; i++) {
 		if (parse_control_scaler(end - buflen, buflen,
 					 mixinfo->controls[i].scaler,
 					 mixinfo->controls[i].control_group,
-					 mixinfo->controls[i].control_index))
+					 mixinfo->controls[i].control_index)) {
+			debug("simple mixer parser failed parsing ctrl scaler tag, ret: '%s'", buf);
 			goto out;
+		}
+
 	}
 
 	sm = new SimpleMixer(control_cb, cb_handle, mixinfo);
 
 	if (sm != nullptr) {
 		mixinfo = nullptr;
-		debug("loaded mixer with %d inputs", inputs);
+		debug("loaded mixer with %d input(s)", inputs);
 
 	} else {
 		debug("could not allocate memory for mixer");
diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h
index a62e38db2..e6e4743c6 100644
--- a/src/modules/uORB/topics/vehicle_command.h
+++ b/src/modules/uORB/topics/vehicle_command.h
@@ -120,7 +120,7 @@ struct vehicle_command_s
 	float param5;			/**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum.   */
 	float param6;			/**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum.   */
 	float param7;			/**< Parameter 7, as defined by MAVLink VEHICLE_CMD enum.   */
-	uint16_t command;		/**< Command ID, as defined MAVLink by VEHICLE_CMD enum.   */
+	enum VEHICLE_CMD command;		/**< Command ID, as defined MAVLink by VEHICLE_CMD enum.   */
 	uint8_t target_system;		/**< System which should execute the command  */
 	uint8_t target_component;	/**< Component which should execute the command, 0 for all components  */
 	uint8_t source_system;		/**< System sending the command  */
diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h
index 0a7fb4e9d..1639a08c2 100644
--- a/src/modules/uORB/topics/vehicle_gps_position.h
+++ b/src/modules/uORB/topics/vehicle_gps_position.h
@@ -56,9 +56,9 @@
 struct vehicle_gps_position_s
 {
 	uint64_t timestamp_position;			/**< Timestamp for position information */
-	int32_t lat;					/**< Latitude in 1E7 degrees */
-	int32_t lon;					/**< Longitude in 1E7 degrees */
-	int32_t alt;					/**< Altitude in 1E3 meters (millimeters) above MSL  */
+	int32_t lat;					/**< Latitude in 1E-7 degrees */
+	int32_t lon;					/**< Longitude in 1E-7 degrees */
+	int32_t alt;					/**< Altitude in 1E-3 meters (millimeters) above MSL  */
 	
 	uint64_t timestamp_variance;
 	float s_variance_m_s;				/**< speed accuracy estimate m/s */
diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c
index 188fa4e37..608c9fff1 100644
--- a/src/systemcmds/esc_calib/esc_calib.c
+++ b/src/systemcmds/esc_calib/esc_calib.c
@@ -1,6 +1,7 @@
 /****************************************************************************
  *
  *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Julian Oes <joes@student.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -45,6 +46,7 @@
 #include <stdbool.h>
 #include <unistd.h>
 #include <fcntl.h>
+#include <poll.h>
 #include <sys/mount.h>
 #include <sys/ioctl.h>
 #include <sys/stat.h>
@@ -63,7 +65,7 @@
 static void	usage(const char *reason);
 __EXPORT int	esc_calib_main(int argc, char *argv[]);
 
-#define MAX_CHANNELS 8
+#define MAX_CHANNELS 14
 
 static void
 usage(const char *reason)
@@ -82,22 +84,26 @@ usage(const char *reason)
 int
 esc_calib_main(int argc, char *argv[])
 {
-	const char *dev = PWM_OUTPUT_DEVICE_PATH;
+	char *dev = PWM_OUTPUT_DEVICE_PATH;
 	char *ep;
 	bool channels_selected[MAX_CHANNELS] = {false};
 	int ch;
 	int ret;
 	char c;
 
+	struct pollfd fds;
+	fds.fd = 0; /* stdin */
+	fds.events = POLLIN;
+
 	if (argc < 2)
 		usage(NULL);
 
-	while ((ch = getopt(argc, argv, "d:")) != EOF) {
+	while ((ch = getopt(argc-1, argv, "d:")) != EOF) {
 		switch (ch) {
 		
 		case 'd':
 			dev = optarg;
-			argc--;
+			argc-=2;
 			break;
 
 		default:
@@ -105,7 +111,7 @@ esc_calib_main(int argc, char *argv[])
 		}
 	}
 
-	if(argc < 1) {
+	if(argc < 2) {
 		usage("no channels provided");
 	}
 
@@ -123,35 +129,33 @@ esc_calib_main(int argc, char *argv[])
 		}
 	}
 
-	/* Wait for confirmation */
-	int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
-	if (!console)
-		err(1, "failed opening console");
-
-	warnx("ATTENTION, please remove or fix props before starting calibration!\n"
+	printf("\nATTENTION, please remove or fix propellers before starting calibration!\n"
 		"\n"
-		"Also press the arming switch first for safety off\n"
+		"Make sure\n"
+		"\t - that the ESCs are not powered\n"
+		"\t - that safety is off (two short blinks)\n"
+		"\t - that the controllers are stopped\n"
 		"\n"
-		"Do you really want to start calibration: y or n?\n");
+		"Do you want to start calibration now: y or n?\n");
 
 	/* wait for user input */
 	while (1) {
 		
-		if (read(console, &c, 1) == 1) {
+		ret = poll(&fds, 1, 0);
+		if (ret > 0) {
+
+			read(0, &c, 1);
 			if (c == 'y' || c == 'Y') {
 				
 				break;
 			} else if (c == 0x03 || c == 0x63 || c == 'q') {
-				warnx("ESC calibration exited");
-				close(console);
+				printf("ESC calibration exited\n");
 				exit(0);
 			} else if (c == 'n' || c == 'N') {
-				warnx("ESC calibration aborted");
-				close(console);
+				printf("ESC calibration aborted\n");
 				exit(0);
 			} else {
-				warnx("Unknown input, ESC calibration aborted");
-				close(console);
+				printf("Unknown input, ESC calibration aborted\n");
 				exit(0);
 			} 
 		}
@@ -163,23 +167,14 @@ esc_calib_main(int argc, char *argv[])
 	int fd = open(dev, 0);
 	if (fd < 0)
 		err(1, "can't open %s", dev);
-
-	// XXX arming not necessaire at the moment
-	// /* Then arm */
-	// ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
-	// 	if (ret != OK)
-	// 		err(1, "PWM_SERVO_SET_ARM_OK");
-
-	// ret = ioctl(fd, PWM_SERVO_ARM, 0);
-	// 	if (ret != OK)
-	// 		err(1, "PWM_SERVO_ARM");
-
-
 	
 
 	/* Wait for user confirmation */
-	warnx("Set high PWM\n"
-	      "Connect battery now and hit ENTER after the ESCs confirm the first calibration step");
+	printf("\nHigh PWM set\n"
+		"\n"
+		"Connect battery now and hit ENTER after the ESCs confirm the first calibration step\n"
+		"\n");
+	fflush(stdout);
 
 	while (1) {
 
@@ -192,13 +187,15 @@ esc_calib_main(int argc, char *argv[])
 			}
 		}
 
-		if (read(console, &c, 1) == 1) {
+		ret = poll(&fds, 1, 0);
+		if (ret > 0) {
+
+			read(0, &c, 1);
 			if (c == 13) {
 				
 				break;
 			} else if (c == 0x03 || c == 0x63 || c == 'q') {
 				warnx("ESC calibration exited");
-				close(console);
 				exit(0);
 			}
 		}
@@ -209,7 +206,8 @@ esc_calib_main(int argc, char *argv[])
 	/* we don't need any more user input */
 	
 
-	warnx("Set low PWM, hit ENTER when finished");
+	printf("Low PWM set, hit ENTER when finished\n"
+		"\n");
 
 	while (1) {
 
@@ -222,13 +220,15 @@ esc_calib_main(int argc, char *argv[])
 			}
 		}
 
-		if (read(console, &c, 1) == 1) {
+		ret = poll(&fds, 1, 0);
+		if (ret > 0) {
+
+			read(0, &c, 1);
 			if (c == 13) {
 				
 				break;
 			} else if (c == 0x03 || c == 0x63 || c == 'q') {
-				warnx("ESC calibration exited");
-				close(console);
+				printf("ESC calibration exited\n");
 				exit(0);
 			}
 		}
@@ -237,14 +237,7 @@ esc_calib_main(int argc, char *argv[])
 	}
 
 	
-	warnx("ESC calibration finished");
-	close(console);
-
-	// XXX disarming not necessaire at the moment
-	/* Now disarm again */
-	// ret = ioctl(fd, PWM_SERVO_DISARM, 0);
-
-	
+	printf("ESC calibration finished\n");
 
 	exit(0);
-}
\ No newline at end of file
+}
diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c
index 588d648bd..f36c28061 100644
--- a/src/systemcmds/tests/tests_file.c
+++ b/src/systemcmds/tests/tests_file.c
@@ -38,7 +38,9 @@
  */
 
 #include <sys/stat.h>
+#include <dirent.h>
 #include <stdio.h>
+#include <stddef.h>
 #include <unistd.h>
 #include <fcntl.h>
 #include <systemlib/err.h>
@@ -52,7 +54,7 @@
 int
 test_file(int argc, char *argv[])
 {
-	const iterations = 10;
+	const iterations = 200;
 
 	/* check if microSD card is mounted */
 	struct stat buffer;
@@ -63,15 +65,52 @@ test_file(int argc, char *argv[])
 
 	uint8_t buf[512];
 	hrt_abstime start, end;
-	perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes");
+	perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes (aligned)");
 
 	int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
 	memset(buf, 0, sizeof(buf));
 
+	warnx("aligned write - please wait..");
+
+	if ((0x3 & (uintptr_t)buf))
+		warnx("memory is unaligned!");
+
 	start = hrt_absolute_time();
 	for (unsigned i = 0; i < iterations; i++) {
 		perf_begin(wperf);
 		write(fd, buf, sizeof(buf));
+		fsync(fd);
+		perf_end(wperf);
+	}
+	end = hrt_absolute_time();
+
+	warnx("%dKiB in %llu microseconds", iterations / 2, end - start);
+
+	perf_print_counter(wperf);
+	perf_free(wperf);
+
+	int ret = unlink("/fs/microsd/testfile");
+
+	if (ret)
+		err(1, "UNLINKING FILE FAILED");
+
+	warnx("unaligned write - please wait..");
+
+	struct {
+		uint8_t byte;
+		uint8_t unaligned[512];
+	} unaligned_buf;
+
+	if ((0x3 & (uintptr_t)unaligned_buf.unaligned) == 0)
+		warnx("creating unaligned memory failed.");
+
+	wperf = perf_alloc(PC_ELAPSED, "SD writes (unaligned)");
+
+	start = hrt_absolute_time();
+	for (unsigned i = 0; i < iterations; i++) {
+		perf_begin(wperf);
+		write(fd, unaligned_buf.unaligned, sizeof(unaligned_buf.unaligned));
+		fsync(fd);
 		perf_end(wperf);
 	}
 	end = hrt_absolute_time();
@@ -79,9 +118,29 @@ test_file(int argc, char *argv[])
 	close(fd);
 
 	warnx("%dKiB in %llu microseconds", iterations / 2, end - start);
+
 	perf_print_counter(wperf);
 	perf_free(wperf);
 
+	/* list directory */
+	DIR           *d;
+	struct dirent *dir;
+	d = opendir("/fs/microsd");
+	if (d) {
+
+		while ((dir = readdir(d)) != NULL) {
+			//printf("%s\n", dir->d_name);
+		}
+
+		closedir(d);
+
+		warnx("directory listing ok (FS mounted and readable)");
+
+	} else {
+		/* failed opening dir */
+		err(1, "FAILED LISTING MICROSD ROOT DIRECTORY");
+	}
+
 	return 0;
 }
 #if 0