Merged move of additional apps out of NuttX folders

src/drivers/blinkm/blinkm.cpp

@@ -0,0 +1,919 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file blinkm.cpp
+ *
+ * Driver for the BlinkM LED controller connected via I2C.
+ *
+ * Connect the BlinkM to I2C3 and put the following line to the rc startup-script:
+ * blinkm start
+ *
+ * To start the system monitor put in the next line after the blinkm start:
+ * blinkm systemmonitor
+ *
+ *
+ * Description:
+ * After startup, the Application checked how many lipo cells are connected to the System.
+ * The recognized number off cells, will be blinked 5 times in purple color.
+ * 2 Cells = 2 blinks
+ * ...
+ * 5 Cells = 5 blinks
+ * Now the Application will show the actual selected Flightmode, GPS-Fix and Battery Warnings and Alerts.
+ *
+ * System disarmed:
+ * The BlinkM should lit solid red.
+ *
+ * System armed:
+ * One message is made of 4 Blinks and a pause in the same length as the 4 blinks.
+ *
+ * X-X-X-X-_-_-_-_-_-_-
+ * -------------------------
+ * G G G M
+ * P P P O
+ * S S S D
+ *       E
+ *
+ * (X = on, _=off)
+ *
+ * The first 3 blinks indicates the status of the GPS-Signal (red):
+ * 0-4 satellites = X-X-X-X-_-_-_-_-_-_-
+ *   5 satellites = X-X-_-X-_-_-_-_-_-_-
+ *   6 satellites = X-_-_-X-_-_-_-_-_-_-
+ * >=7 satellites = _-_-_-X-_-_-_-_-_-_-
+ * If no GPS is found the first 3 blinks are white
+ *
+ * The fourth Blink indicates the Flightmode:
+ * MANUAL     : amber
+ * STABILIZED : yellow
+ * HOLD		  : blue
+ * AUTO       : green
+ *
+ * Battery Warning (low Battery Level):
+ * Continuously blinking in yellow X-X-X-X-X-X-X-X-X-X
+ *
+ * Battery Alert (critical Battery Level)
+ * Continuously blinking in red X-X-X-X-X-X-X-X-X-X
+ *
+ * General Error (no uOrb Data)
+ * Continuously blinking in white X-X-X-X-X-X-X-X-X-X
+ *
+ */
+
+#include <nuttx/config.h>
+
+#include <arch/board/board.h>
+#include <drivers/device/i2c.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <ctype.h>
+
+#include <drivers/drv_blinkm.h>
+
+#include <nuttx/wqueue.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <systemlib/systemlib.h>
+#include <poll.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_gps_position.h>
+
+static const float MAX_CELL_VOLTAGE	= 4.3f;
+static const int LED_ONTIME = 120;
+static const int LED_OFFTIME = 120;
+static const int LED_BLINK = 1;
+static const int LED_NOBLINK = 0;
+
+class BlinkM : public device::I2C
+{
+public:
+	BlinkM(int bus, int blinkm);
+	virtual ~BlinkM();
+
+
+	virtual int		init();
+	virtual int		probe();
+	virtual int		setMode(int mode);
+	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	static const char	*const script_names[];
+
+private:
+	enum ScriptID {
+		USER		= 0,
+		RGB,
+		WHITE_FLASH,
+		RED_FLASH,
+		GREEN_FLASH,
+		BLUE_FLASH,
+		CYAN_FLASH,
+		MAGENTA_FLASH,
+		YELLOW_FLASH,
+		BLACK,
+		HUE_CYCLE,
+		MOOD_LIGHT,
+		VIRTUAL_CANDLE,
+		WATER_REFLECTIONS,
+		OLD_NEON,
+		THE_SEASONS,
+		THUNDERSTORM,
+		STOP_LIGHT,
+		MORSE_CODE
+	};
+
+	enum ledColors {
+		LED_OFF,
+		LED_RED,
+		LED_YELLOW,
+		LED_PURPLE,
+		LED_GREEN,
+		LED_BLUE,
+		LED_WHITE,
+		LED_AMBER
+	};
+
+	work_s			_work;
+
+	int led_color_1;
+	int led_color_2;
+	int led_color_3;
+	int led_color_4;
+	int led_color_5;
+	int led_color_6;
+	int led_color_7;
+	int led_color_8;
+	int led_blink;
+
+	bool systemstate_run;
+
+	void 			setLEDColor(int ledcolor);
+	static void		led_trampoline(void *arg);
+	void			led();
+
+	int			set_rgb(uint8_t r, uint8_t g, uint8_t b);
+
+	int			fade_rgb(uint8_t r, uint8_t g, uint8_t b);
+	int			fade_hsb(uint8_t h, uint8_t s, uint8_t b);
+
+	int			fade_rgb_random(uint8_t r, uint8_t g, uint8_t b);
+	int			fade_hsb_random(uint8_t h, uint8_t s, uint8_t b);
+
+	int			set_fade_speed(uint8_t s);
+
+	int			play_script(uint8_t script_id);
+	int			play_script(const char *script_name);
+	int			stop_script();
+
+	int			write_script_line(uint8_t line, uint8_t ticks, uint8_t cmd, uint8_t arg1, uint8_t arg2, uint8_t arg3);
+	int			read_script_line(uint8_t line, uint8_t &ticks, uint8_t cmd[4]);
+	int			set_script(uint8_t length, uint8_t repeats);
+
+	int			get_rgb(uint8_t &r, uint8_t &g, uint8_t &b);
+
+	int			get_firmware_version(uint8_t version[2]);
+};
+
+/* for now, we only support one BlinkM */
+namespace
+{
+	BlinkM *g_blinkm;
+}
+
+/* list of script names, must match script ID numbers */
+const char *const BlinkM::script_names[] = {
+	"USER",
+	"RGB",
+	"WHITE_FLASH",
+	"RED_FLASH",
+	"GREEN_FLASH",
+	"BLUE_FLASH",
+	"CYAN_FLASH",
+	"MAGENTA_FLASH",
+	"YELLOW_FLASH",
+	"BLACK",
+	"HUE_CYCLE",
+	"MOOD_LIGHT",
+	"VIRTUAL_CANDLE",
+	"WATER_REFLECTIONS",
+	"OLD_NEON",
+	"THE_SEASONS",
+	"THUNDERSTORM",
+	"STOP_LIGHT",
+	"MORSE_CODE",
+	nullptr
+};
+
+
+extern "C" __EXPORT int blinkm_main(int argc, char *argv[]);
+
+BlinkM::BlinkM(int bus, int blinkm) :
+	I2C("blinkm", BLINKM_DEVICE_PATH, bus, blinkm, 100000),
+	led_color_1(LED_OFF),
+	led_color_2(LED_OFF),
+	led_color_3(LED_OFF),
+	led_color_4(LED_OFF),
+	led_color_5(LED_OFF),
+	led_color_6(LED_OFF),
+	led_color_7(LED_OFF),
+	led_color_8(LED_OFF),
+	led_blink(LED_NOBLINK),
+	systemstate_run(false)
+{
+	memset(&_work, 0, sizeof(_work));
+}
+
+BlinkM::~BlinkM()
+{
+}
+
+int
+BlinkM::init()
+{
+	int ret;
+	ret = I2C::init();
+
+	if (ret != OK) {
+		warnx("I2C init failed");
+		return ret;
+	}
+
+	stop_script();
+	set_rgb(0,0,0);
+
+	return OK;
+}
+
+int
+BlinkM::setMode(int mode)
+{
+	if(mode == 1) {
+		if(systemstate_run == false) {
+			stop_script();
+			set_rgb(0,0,0);
+			systemstate_run = true;
+			work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, 1);
+		}
+	} else {
+		systemstate_run = false;
+	}
+
+	return OK;
+}
+
+int
+BlinkM::probe()
+{
+	uint8_t version[2];
+	int ret;
+
+	ret = get_firmware_version(version);
+
+	if (ret == OK)
+		log("found BlinkM firmware version %c%c", version[1], version[0]);
+
+	return ret;
+}
+
+int
+BlinkM::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	int ret = ENOTTY;
+
+	switch (cmd) {
+	case BLINKM_PLAY_SCRIPT_NAMED:
+		if (arg == 0) {
+			ret = EINVAL;
+			break;
+		}
+		ret = play_script((const char *)arg);
+		break;
+
+	case BLINKM_PLAY_SCRIPT:
+		ret = play_script(arg);
+		break;
+
+	case BLINKM_SET_USER_SCRIPT: {
+		if (arg == 0) {
+			ret = EINVAL;
+			break;
+		}
+
+		unsigned lines = 0;
+		const uint8_t *script = (const uint8_t *)arg;
+
+		while ((lines < 50) && (script[1] != 0)) {
+			ret = write_script_line(lines, script[0], script[1], script[2], script[3], script[4]);
+			if (ret != OK)
+				break;
+			script += 5;
+		}
+		if (ret == OK)
+			ret = set_script(lines, 0);
+		break;
+	}
+
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+
+void
+BlinkM::led_trampoline(void *arg)
+{
+	BlinkM *bm = (BlinkM *)arg;
+
+	bm->led();
+}
+
+
+
+void
+BlinkM::led()
+{
+
+	static int vehicle_status_sub_fd;
+	static int vehicle_gps_position_sub_fd;
+
+	static int num_of_cells = 0;
+	static int detected_cells_runcount = 0;
+
+	static int t_led_color[8] = { 0, 0, 0, 0, 0, 0, 0, 0};
+	static int t_led_blink = 0;
+	static int led_thread_runcount=0;
+	static int led_interval = 1000;
+
+	static int no_data_vehicle_status = 0;
+	static int no_data_vehicle_gps_position = 0;
+
+	static bool topic_initialized = false;
+	static bool detected_cells_blinked = false;
+	static bool led_thread_ready = true;
+
+	int num_of_used_sats = 0;
+
+	if(!topic_initialized) {
+		vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
+		orb_set_interval(vehicle_status_sub_fd, 1000);
+
+		vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position));
+		orb_set_interval(vehicle_gps_position_sub_fd, 1000);
+
+		topic_initialized = true;
+	}
+
+	if(led_thread_ready == true) {
+		if(!detected_cells_blinked) {
+			if(num_of_cells > 0) {
+				t_led_color[0] = LED_PURPLE;
+			}
+			if(num_of_cells > 1) {
+				t_led_color[1] = LED_PURPLE;
+			}
+			if(num_of_cells > 2) {
+				t_led_color[2] = LED_PURPLE;
+			}
+			if(num_of_cells > 3) {
+				t_led_color[3] = LED_PURPLE;
+			}
+			if(num_of_cells > 4) {
+				t_led_color[4] = LED_PURPLE;
+			}
+			t_led_color[5] = LED_OFF;
+			t_led_color[6] = LED_OFF;
+			t_led_color[7] = LED_OFF;
+			t_led_blink = LED_BLINK;
+		} else {
+			t_led_color[0] = led_color_1;
+			t_led_color[1] = led_color_2;
+			t_led_color[2] = led_color_3;
+			t_led_color[3] = led_color_4;
+			t_led_color[4] = led_color_5;
+			t_led_color[5] = led_color_6;
+			t_led_color[6] = led_color_7;
+			t_led_color[7] = led_color_8;
+			t_led_blink = led_blink;
+		}
+		led_thread_ready = false;
+	}
+
+	if (led_thread_runcount & 1) {
+		if (t_led_blink)
+			setLEDColor(LED_OFF);
+		led_interval = LED_OFFTIME;
+	} else {
+		setLEDColor(t_led_color[(led_thread_runcount / 2) % 8]);
+		//led_interval = (led_thread_runcount & 1) : LED_ONTIME;
+		led_interval = LED_ONTIME;
+	}
+
+	if (led_thread_runcount == 15) {
+		/* obtained data for the first file descriptor */
+		struct vehicle_status_s vehicle_status_raw;
+		struct vehicle_gps_position_s vehicle_gps_position_raw;
+
+		memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw));
+		memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw));
+
+		bool new_data_vehicle_status;
+		bool new_data_vehicle_gps_position;
+
+		orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);
+
+		if (new_data_vehicle_status) {
+			orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw);
+			no_data_vehicle_status = 0;
+		} else {
+			no_data_vehicle_status++;
+			if(no_data_vehicle_status >= 3)
+				no_data_vehicle_status = 3;
+		}
+
+		orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position);
+
+		if (new_data_vehicle_gps_position) {
+			orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub_fd, &vehicle_gps_position_raw);
+			no_data_vehicle_gps_position = 0;
+		} else {
+			no_data_vehicle_gps_position++;
+			if(no_data_vehicle_gps_position >= 3)
+				no_data_vehicle_gps_position = 3;
+		}
+
+
+
+		/* get number of used satellites in navigation */
+		num_of_used_sats = 0;
+		//for(int satloop=0; satloop<20; satloop++) {
+		for(int satloop=0; satloop<sizeof(vehicle_gps_position_raw.satellite_used); satloop++) {
+			if(vehicle_gps_position_raw.satellite_used[satloop] == 1) {
+				num_of_used_sats++;
+			}
+		}
+
+		if(new_data_vehicle_status || no_data_vehicle_status < 3){
+			if(num_of_cells == 0) {
+				/* looking for lipo cells that are connected */
+				printf("<blinkm> checking cells\n");
+				for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) {
+					if(vehicle_status_raw.voltage_battery < num_of_cells * MAX_CELL_VOLTAGE) break;
+				}
+				printf("<blinkm> cells found:%d\n", num_of_cells);
+
+			} else {
+				if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
+					/* LED Pattern for battery low warning */
+					led_color_1 = LED_YELLOW;
+					led_color_2 = LED_YELLOW;
+					led_color_3 = LED_YELLOW;
+					led_color_4 = LED_YELLOW;
+					led_color_5 = LED_YELLOW;
+					led_color_6 = LED_YELLOW;
+					led_color_7 = LED_YELLOW;
+					led_color_8 = LED_YELLOW;
+					led_blink = LED_BLINK;
+
+				} else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
+					/* LED Pattern for battery critical alerting */
+					led_color_1 = LED_RED;
+					led_color_2 = LED_RED;
+					led_color_3 = LED_RED;
+					led_color_4 = LED_RED;
+					led_color_5 = LED_RED;
+					led_color_6 = LED_RED;
+					led_color_7 = LED_RED;
+					led_color_8 = LED_RED;
+					led_blink = LED_BLINK;
+
+				} else {
+					/* no battery warnings here */
+
+					if(vehicle_status_raw.flag_system_armed == false) {
+						/* system not armed */
+						led_color_1 = LED_RED;
+						led_color_2 = LED_RED;
+						led_color_3 = LED_RED;
+						led_color_4 = LED_RED;
+						led_color_5 = LED_RED;
+						led_color_6 = LED_RED;
+						led_color_7 = LED_RED;
+						led_color_8 = LED_RED;
+						led_blink = LED_NOBLINK;
+
+					} else {
+						/* armed system - initial led pattern */
+						led_color_1 = LED_RED;
+						led_color_2 = LED_RED;
+						led_color_3 = LED_RED;
+						led_color_4 = LED_OFF;
+						led_color_5 = LED_OFF;
+						led_color_6 = LED_OFF;
+						led_color_7 = LED_OFF;
+						led_color_8 = LED_OFF;
+						led_blink = LED_BLINK;
+
+						/* handle 4th led - flightmode indicator */
+						switch((int)vehicle_status_raw.flight_mode) {
+							case VEHICLE_FLIGHT_MODE_MANUAL:
+								led_color_4 = LED_AMBER;
+								break;
+
+							case VEHICLE_FLIGHT_MODE_STAB:
+								led_color_4 = LED_YELLOW;
+								break;
+
+							case VEHICLE_FLIGHT_MODE_HOLD:
+								led_color_4 = LED_BLUE;
+								break;
+
+							case VEHICLE_FLIGHT_MODE_AUTO:
+								led_color_4 = LED_GREEN;
+								break;
+						}
+
+						if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) {
+							/* handling used sat´s */
+							if(num_of_used_sats >= 7) {
+								led_color_1 = LED_OFF;
+								led_color_2 = LED_OFF;
+								led_color_3 = LED_OFF;
+							} else if(num_of_used_sats == 6) {
+								led_color_2 = LED_OFF;
+								led_color_3 = LED_OFF;
+							} else if(num_of_used_sats == 5) {
+								led_color_3 = LED_OFF;
+							}
+
+						} else {
+							/* no vehicle_gps_position data */
+							led_color_1 = LED_WHITE;
+							led_color_2 = LED_WHITE;
+							led_color_3 = LED_WHITE;
+
+						}
+
+					}
+				}
+			}
+		} else {
+			/* LED Pattern for general Error - no vehicle_status can retrieved */
+			led_color_1 = LED_WHITE;
+			led_color_2 = LED_WHITE;
+			led_color_3 = LED_WHITE;
+			led_color_4 = LED_WHITE;
+			led_color_5 = LED_WHITE;
+			led_color_6 = LED_WHITE;
+			led_color_7 = LED_WHITE;
+			led_color_8 = LED_WHITE;
+			led_blink = LED_BLINK;
+
+		}
+
+		/*
+		printf( "<blinkm> Volt:%8.4f\tArmed:%4u\tMode:%4u\tCells:%4u\tNDVS:%4u\tNDSAT:%4u\tSats:%4u\tFix:%4u\tVisible:%4u\n",
+		vehicle_status_raw.voltage_battery,
+		vehicle_status_raw.flag_system_armed,
+		vehicle_status_raw.flight_mode,
+		num_of_cells,
+		no_data_vehicle_status,
+		no_data_vehicle_gps_position,
+		num_of_used_sats,
+		vehicle_gps_position_raw.fix_type,
+		vehicle_gps_position_raw.satellites_visible);
+		*/
+
+		led_thread_runcount=0;
+		led_thread_ready = true;
+		led_interval = LED_OFFTIME;
+
+		if(detected_cells_runcount < 4){
+			detected_cells_runcount++;
+		} else {
+			detected_cells_blinked = true;
+		}
+
+	} else {
+		led_thread_runcount++;
+	}
+
+	if(systemstate_run == true) {
+		/* re-queue ourselves to run again later */
+		work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, led_interval);
+	} else {
+		stop_script();
+		set_rgb(0,0,0);
+	}
+}
+
+void BlinkM::setLEDColor(int ledcolor) {
+	switch (ledcolor) {
+		case LED_OFF:	// off
+			set_rgb(0,0,0);
+			break;
+		case LED_RED:	// red
+			set_rgb(255,0,0);
+			break;
+		case LED_YELLOW:	// yellow
+			set_rgb(255,70,0);
+			break;
+		case LED_PURPLE:	// purple
+			set_rgb(255,0,255);
+			break;
+		case LED_GREEN:	// green
+			set_rgb(0,255,0);
+			break;
+		case LED_BLUE:	// blue
+			set_rgb(0,0,255);
+			break;
+		case LED_WHITE:	// white
+			set_rgb(255,255,255);
+			break;
+		case LED_AMBER:	// amber
+			set_rgb(255,20,0);
+			break;
+	}
+}
+
+int
+BlinkM::set_rgb(uint8_t r, uint8_t g, uint8_t b)
+{
+	const uint8_t msg[4] = { 'n', r, g, b };
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+BlinkM::fade_rgb(uint8_t r, uint8_t g, uint8_t b)
+{
+	const uint8_t msg[4] = { 'c', r, g, b };
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+BlinkM::fade_hsb(uint8_t h, uint8_t s, uint8_t b)
+{
+	const uint8_t msg[4] = { 'h', h, s, b };
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+BlinkM::fade_rgb_random(uint8_t r, uint8_t g, uint8_t b)
+{
+	const uint8_t msg[4] = { 'C', r, g, b };
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+BlinkM::fade_hsb_random(uint8_t h, uint8_t s, uint8_t b)
+{
+	const uint8_t msg[4] = { 'H', h, s, b };
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+BlinkM::set_fade_speed(uint8_t s)
+{
+	const uint8_t msg[2] = { 'f', s };
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+BlinkM::play_script(uint8_t script_id)
+{
+	const uint8_t msg[4] = { 'p', script_id, 0, 0 };
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+BlinkM::play_script(const char *script_name)
+{
+	/* handle HTML colour encoding */
+	if (isxdigit(script_name[0]) && (strlen(script_name) == 6)) {
+		char code[3];
+		uint8_t r, g, b;
+
+		code[2] = '\0';
+
+		code[0] = script_name[1];
+		code[1] = script_name[2];
+		r = strtol(code, 0, 16);
+		code[0] = script_name[3];
+		code[1] = script_name[4];
+		g = strtol(code, 0, 16);
+		code[0] = script_name[5];
+		code[1] = script_name[6];
+		b = strtol(code, 0, 16);
+
+		stop_script();
+		return set_rgb(r, g, b);
+	}
+
+	for (unsigned i = 0; script_names[i] != nullptr; i++)
+		if (!strcasecmp(script_name, script_names[i]))
+			return play_script(i);
+
+	return -1;
+}
+
+int
+BlinkM::stop_script()
+{
+	const uint8_t msg[1] = { 'o' };
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+BlinkM::write_script_line(uint8_t line, uint8_t ticks, uint8_t cmd, uint8_t arg1, uint8_t arg2, uint8_t arg3)
+{
+	const uint8_t msg[8] = { 'W', 0, line, ticks, cmd, arg1, arg2, arg3 };
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+BlinkM::read_script_line(uint8_t line, uint8_t &ticks, uint8_t cmd[4])
+{
+	const uint8_t msg[3] = { 'R', 0, line };
+	uint8_t result[5];
+
+	int ret = transfer(msg, sizeof(msg), result, sizeof(result));
+
+	if (ret == OK) {
+		ticks = result[0];
+		cmd[0] = result[1];
+		cmd[1] = result[2];
+		cmd[2] = result[3];
+		cmd[3] = result[4];
+	}
+
+	return ret;
+}
+
+int
+BlinkM::set_script(uint8_t len, uint8_t repeats)
+{
+	const uint8_t msg[4] = { 'L', 0, len, repeats };
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+BlinkM::get_rgb(uint8_t &r, uint8_t &g, uint8_t &b)
+{
+	const uint8_t msg = 'g';
+	uint8_t result[3];
+
+	int ret = transfer(&msg, sizeof(msg), result, sizeof(result));
+
+	if (ret == OK) {
+		r = result[0];
+		g = result[1];
+		b = result[2];
+	}
+
+	return ret;
+}
+
+int
+BlinkM::get_firmware_version(uint8_t version[2])
+{
+	const uint8_t msg = 'Z';
+
+	return transfer(&msg, sizeof(msg), version, 2);
+}
+
+void blinkm_usage() {
+	fprintf(stderr, "missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}\n");
+	fprintf(stderr, "options:\n");
+	fprintf(stderr, "\t-b --bus i2cbus (3)\n");
+	fprintf(stderr, "\t-a --blinkmaddr blinkmaddr (9)\n");
+}
+
+int
+blinkm_main(int argc, char *argv[])
+{
+
+	int i2cdevice = PX4_I2C_BUS_EXPANSION;
+	int blinkmadr = 9;
+
+	int x;
+
+	for (x = 1; x < argc; x++) {
+		if (strcmp(argv[x], "-b") == 0 || strcmp(argv[x], "--bus") == 0) {
+			if (argc > x + 1) {
+				i2cdevice = atoi(argv[x + 1]);
+			}
+		}
+
+		if (strcmp(argv[x], "-a") == 0 || strcmp(argv[x], "--blinkmaddr") == 0) {
+			if (argc > x + 1) {
+				blinkmadr = atoi(argv[x + 1]);
+			}
+		}
+
+	}
+
+	if (!strcmp(argv[1], "start")) {
+		if (g_blinkm != nullptr)
+			errx(1, "already started");
+
+		g_blinkm = new BlinkM(i2cdevice, blinkmadr);
+
+		if (g_blinkm == nullptr)
+			errx(1, "new failed");
+
+		if (OK != g_blinkm->init()) {
+			delete g_blinkm;
+			g_blinkm = nullptr;
+			errx(1, "init failed");
+		}
+
+		exit(0);
+	}
+
+
+	if (g_blinkm == nullptr) {
+		fprintf(stderr, "not started\n");
+		blinkm_usage();
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "systemstate")) {
+		g_blinkm->setMode(1);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "ledoff")) {
+		g_blinkm->setMode(0);
+		exit(0);
+	}
+
+
+	if (!strcmp(argv[1], "list")) {
+		for (unsigned i = 0; BlinkM::script_names[i] != nullptr; i++)
+			fprintf(stderr, "    %s\n", BlinkM::script_names[i]);
+		fprintf(stderr, "    <html color number>\n");
+		exit(0);
+	}
+
+	/* things that require access to the device */
+	int fd = open(BLINKM_DEVICE_PATH, 0);
+	if (fd < 0)
+		err(1, "can't open BlinkM device");
+
+	g_blinkm->setMode(0);
+	if (ioctl(fd, BLINKM_PLAY_SCRIPT_NAMED, (unsigned long)argv[1]) == OK)
+		exit(0);
+
+	blinkm_usage();
+	exit(0);
+}

src/drivers/blinkm/module.mk

@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# BlinkM I2C LED driver
+#
+
+MODULE_COMMAND	= blinkm
+
+SRCS		= blinkm.cpp

src/drivers/bma180/bma180.cpp

@@ -0,0 +1,929 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file bma180.cpp
+ * Driver for the Bosch BMA 180 MEMS accelerometer connected via SPI.
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+#include <nuttx/arch.h>
+#include <nuttx/wqueue.h>
+#include <nuttx/clock.h>
+
+#include <drivers/drv_hrt.h>
+#include <arch/board/board.h>
+
+#include <drivers/device/spi.h>
+#include <drivers/drv_accel.h>
+
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+#define DIR_READ			(1<<7)
+#define DIR_WRITE			(0<<7)
+
+#define ADDR_CHIP_ID			0x00
+#define CHIP_ID				0x03
+
+#define ADDR_ACC_X_LSB			0x02
+#define ADDR_ACC_Y_LSB			0x04
+#define ADDR_ACC_Z_LSB			0x06
+#define ADDR_TEMPERATURE		0x08
+
+#define ADDR_CTRL_REG0			0x0D
+#define REG0_WRITE_ENABLE		0x10
+
+#define ADDR_RESET			0x10
+#define SOFT_RESET				0xB6
+
+#define ADDR_BW_TCS			0x20
+#define BW_TCS_BW_MASK				(0xf<<4)
+#define BW_TCS_BW_10HZ				(0<<4)
+#define BW_TCS_BW_20HZ				(1<<4)
+#define BW_TCS_BW_40HZ				(2<<4)
+#define BW_TCS_BW_75HZ				(3<<4)
+#define BW_TCS_BW_150HZ				(4<<4)
+#define BW_TCS_BW_300HZ				(5<<4)
+#define BW_TCS_BW_600HZ				(6<<4)
+#define BW_TCS_BW_1200HZ			(7<<4)
+
+#define ADDR_HIGH_DUR			0x27
+#define HIGH_DUR_DIS_I2C			(1<<0)
+
+#define ADDR_TCO_Z			0x30
+#define TCO_Z_MODE_MASK				0x3
+
+#define ADDR_GAIN_Y			0x33
+#define GAIN_Y_SHADOW_DIS			(1<<0)
+
+#define ADDR_OFFSET_LSB1		0x35
+#define OFFSET_LSB1_RANGE_MASK			(7<<1)
+#define OFFSET_LSB1_RANGE_1G			(0<<1)
+#define OFFSET_LSB1_RANGE_2G			(2<<1)
+#define OFFSET_LSB1_RANGE_3G			(3<<1)
+#define OFFSET_LSB1_RANGE_4G			(4<<1)
+#define OFFSET_LSB1_RANGE_8G			(5<<1)
+#define OFFSET_LSB1_RANGE_16G			(6<<1)
+
+#define ADDR_OFFSET_T			0x37
+#define OFFSET_T_READOUT_12BIT			(1<<0)
+
+extern "C" { __EXPORT int bma180_main(int argc, char *argv[]); }
+
+class BMA180 : public device::SPI
+{
+public:
+	BMA180(int bus, spi_dev_e device);
+	virtual ~BMA180();
+
+	virtual int		init();
+
+	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
+	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	/**
+	 * Diagnostics - print some basic information about the driver.
+	 */
+	void			print_info();
+
+protected:
+	virtual int		probe();
+
+private:
+
+	struct hrt_call		_call;
+	unsigned		_call_interval;
+
+	unsigned		_num_reports;
+	volatile unsigned	_next_report;
+	volatile unsigned	_oldest_report;
+	struct accel_report	*_reports;
+
+	struct accel_scale	_accel_scale;
+	float			_accel_range_scale;
+	float			_accel_range_m_s2;
+	orb_advert_t		_accel_topic;
+
+	unsigned		_current_lowpass;
+	unsigned		_current_range;
+
+	perf_counter_t		_sample_perf;
+
+	/**
+	 * Start automatic measurement.
+	 */
+	void			start();
+
+	/**
+	 * Stop automatic measurement.
+	 */
+	void			stop();
+
+	/**
+	 * Static trampoline from the hrt_call context; because we don't have a
+	 * generic hrt wrapper yet.
+	 *
+	 * Called by the HRT in interrupt context at the specified rate if
+	 * automatic polling is enabled.
+	 *
+	 * @param arg		Instance pointer for the driver that is polling.
+	 */
+	static void		measure_trampoline(void *arg);
+
+	/**
+	 * Fetch measurements from the sensor and update the report ring.
+	 */
+	void			measure();
+
+	/**
+	 * Read a register from the BMA180
+	 *
+	 * @param		The register to read.
+	 * @return		The value that was read.
+	 */
+	uint8_t			read_reg(unsigned reg);
+
+	/**
+	 * Write a register in the BMA180
+	 *
+	 * @param reg		The register to write.
+	 * @param value		The new value to write.
+	 */
+	void			write_reg(unsigned reg, uint8_t value);
+
+	/**
+	 * Modify a register in the BMA180
+	 *
+	 * Bits are cleared before bits are set.
+	 *
+	 * @param reg		The register to modify.
+	 * @param clearbits	Bits in the register to clear.
+	 * @param setbits	Bits in the register to set.
+	 */
+	void			modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
+
+	/**
+	 * Set the BMA180 measurement range.
+	 *
+	 * @param max_g		The maximum G value the range must support.
+	 * @return		OK if the value can be supported, -ERANGE otherwise.
+	 */
+	int			set_range(unsigned max_g);
+
+	/**
+	 * Set the BMA180 internal lowpass filter frequency.
+	 *
+	 * @param frequency	The internal lowpass filter frequency is set to a value
+	 *			equal or greater to this.
+	 *			Zero selects the highest frequency supported.
+	 * @return		OK if the value can be supported.
+	 */
+	int			set_lowpass(unsigned frequency);
+};
+
+/* helper macro for handling report buffer indices */
+#define INCREMENT(_x, _lim)	do { _x++; if (_x >= _lim) _x = 0; } while(0)
+
+
+BMA180::BMA180(int bus, spi_dev_e device) :
+	SPI("BMA180", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 8000000),
+	_call_interval(0),
+	_num_reports(0),
+	_next_report(0),
+	_oldest_report(0),
+	_reports(nullptr),
+	_accel_range_scale(0.0f),
+	_accel_range_m_s2(0.0f),
+	_accel_topic(-1),
+	_current_lowpass(0),
+	_current_range(0),
+	_sample_perf(perf_alloc(PC_ELAPSED, "bma180_read"))
+{
+	// enable debug() calls
+	_debug_enabled = true;
+
+	// default scale factors
+	_accel_scale.x_offset = 0;
+	_accel_scale.x_scale  = 1.0f;
+	_accel_scale.y_offset = 0;
+	_accel_scale.y_scale  = 1.0f;
+	_accel_scale.z_offset = 0;
+	_accel_scale.z_scale  = 1.0f;
+}
+
+BMA180::~BMA180()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* free any existing reports */
+	if (_reports != nullptr)
+		delete[] _reports;
+
+	/* delete the perf counter */
+	perf_free(_sample_perf);
+}
+
+int
+BMA180::init()
+{
+	int ret = ERROR;
+
+	/* do SPI init (and probe) first */
+	if (SPI::init() != OK)
+		goto out;
+
+	/* allocate basic report buffers */
+	_num_reports = 2;
+	_oldest_report = _next_report = 0;
+	_reports = new struct accel_report[_num_reports];
+
+	if (_reports == nullptr)
+		goto out;
+
+	/* advertise sensor topic */
+	memset(&_reports[0], 0, sizeof(_reports[0]));
+	_accel_topic = orb_advertise(ORB_ID(sensor_accel), &_reports[0]);
+
+	/* perform soft reset (p48) */
+	write_reg(ADDR_RESET, SOFT_RESET);
+
+	/* wait 10 ms (datasheet incorrectly lists 10 us on page 49) */
+	usleep(10000);
+
+	/* enable writing to chip config */
+	modify_reg(ADDR_CTRL_REG0, 0, REG0_WRITE_ENABLE);
+
+	/* disable I2C interface */
+	modify_reg(ADDR_HIGH_DUR, HIGH_DUR_DIS_I2C, 0);
+
+	/* switch to low-noise mode */
+	modify_reg(ADDR_TCO_Z, TCO_Z_MODE_MASK, 0);
+
+	/* disable 12-bit mode */
+	modify_reg(ADDR_OFFSET_T, OFFSET_T_READOUT_12BIT, 0);
+
+	/* disable shadow-disable mode */
+	modify_reg(ADDR_GAIN_Y, GAIN_Y_SHADOW_DIS, 0);
+
+	/* disable writing to chip config */
+	modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0);
+
+	if (set_range(4)) warnx("Failed setting range");
+
+	if (set_lowpass(75)) warnx("Failed setting lowpass");
+
+	if (read_reg(ADDR_CHIP_ID) == CHIP_ID) {
+		ret = OK;
+
+	} else {
+		ret = ERROR;
+	}
+
+out:
+	return ret;
+}
+
+int
+BMA180::probe()
+{
+	/* dummy read to ensure SPI state machine is sane */
+	read_reg(ADDR_CHIP_ID);
+
+	if (read_reg(ADDR_CHIP_ID) == CHIP_ID)
+		return OK;
+
+	return -EIO;
+}
+
+ssize_t
+BMA180::read(struct file *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct accel_report);
+	int ret = 0;
+
+	/* buffer must be large enough */
+	if (count < 1)
+		return -ENOSPC;
+
+	/* if automatic measurement is enabled */
+	if (_call_interval > 0) {
+
+		/*
+		 * While there is space in the caller's buffer, and reports, copy them.
+		 * Note that we may be pre-empted by the measurement code while we are doing this;
+		 * we are careful to avoid racing with it.
+		 */
+		while (count--) {
+			if (_oldest_report != _next_report) {
+				memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
+				ret += sizeof(_reports[0]);
+				INCREMENT(_oldest_report, _num_reports);
+			}
+		}
+
+		/* if there was no data, warn the caller */
+		return ret ? ret : -EAGAIN;
+	}
+
+	/* manual measurement */
+	_oldest_report = _next_report = 0;
+	measure();
+
+	/* measurement will have generated a report, copy it out */
+	memcpy(buffer, _reports, sizeof(*_reports));
+	ret = sizeof(*_reports);
+
+	return ret;
+}
+
+int
+BMA180::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+	case SENSORIOCSPOLLRATE: {
+			switch (arg) {
+
+				/* switching to manual polling */
+			case SENSOR_POLLRATE_MANUAL:
+				stop();
+				_call_interval = 0;
+				return OK;
+
+				/* external signalling not supported */
+			case SENSOR_POLLRATE_EXTERNAL:
+
+				/* zero would be bad */
+			case 0:
+				return -EINVAL;
+
+
+				/* set default/max polling rate */
+			case SENSOR_POLLRATE_MAX:
+			case SENSOR_POLLRATE_DEFAULT:
+				/* With internal low pass filters enabled, 250 Hz is sufficient */
+				return ioctl(filp, SENSORIOCSPOLLRATE, 250);
+
+				/* adjust to a legal polling interval in Hz */
+			default: {
+					/* do we need to start internal polling? */
+					bool want_start = (_call_interval == 0);
+
+					/* convert hz to hrt interval via microseconds */
+					unsigned ticks = 1000000 / arg;
+
+					/* check against maximum sane rate */
+					if (ticks < 1000)
+						return -EINVAL;
+
+					/* update interval for next measurement */
+					/* XXX this is a bit shady, but no other way to adjust... */
+					_call.period = _call_interval = ticks;
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start)
+						start();
+
+					return OK;
+				}
+			}
+		}
+
+	case SENSORIOCGPOLLRATE:
+		if (_call_interval == 0)
+			return SENSOR_POLLRATE_MANUAL;
+
+		return 1000000 / _call_interval;
+
+	case SENSORIOCSQUEUEDEPTH: {
+			/* account for sentinel in the ring */
+			arg++;
+
+			/* lower bound is mandatory, upper bound is a sanity check */
+			if ((arg < 2) || (arg > 100))
+				return -EINVAL;
+
+			/* allocate new buffer */
+			struct accel_report *buf = new struct accel_report[arg];
+
+			if (nullptr == buf)
+				return -ENOMEM;
+
+			/* reset the measurement state machine with the new buffer, free the old */
+			stop();
+			delete[] _reports;
+			_num_reports = arg;
+			_reports = buf;
+			start();
+
+			return OK;
+		}
+
+	case SENSORIOCGQUEUEDEPTH:
+		return _num_reports - 1;
+
+	case SENSORIOCRESET:
+		/* XXX implement */
+		return -EINVAL;
+
+	case ACCELIOCSSAMPLERATE:	/* sensor sample rate is not (really) adjustable */
+		return -EINVAL;
+
+	case ACCELIOCGSAMPLERATE:
+		return 1200;		/* always operating in low-noise mode */
+
+	case ACCELIOCSLOWPASS:
+		return set_lowpass(arg);
+
+	case ACCELIOCGLOWPASS:
+		return _current_lowpass;
+
+	case ACCELIOCSSCALE:
+		/* copy scale in */
+		memcpy(&_accel_scale, (struct accel_scale *) arg, sizeof(_accel_scale));
+		return OK;
+
+	case ACCELIOCGSCALE:
+		/* copy scale out */
+		memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale));
+		return OK;
+
+	case ACCELIOCSRANGE:
+		return set_range(arg);
+
+	case ACCELIOCGRANGE:
+		return _current_range;
+
+	default:
+		/* give it to the superclass */
+		return SPI::ioctl(filp, cmd, arg);
+	}
+}
+
+uint8_t
+BMA180::read_reg(unsigned reg)
+{
+	uint8_t cmd[2];
+
+	cmd[0] = reg | DIR_READ;
+
+	transfer(cmd, cmd, sizeof(cmd));
+
+	return cmd[1];
+}
+
+void
+BMA180::write_reg(unsigned reg, uint8_t value)
+{
+	uint8_t	cmd[2];
+
+	cmd[0] = reg | DIR_WRITE;
+	cmd[1] = value;
+
+	transfer(cmd, nullptr, sizeof(cmd));
+}
+
+void
+BMA180::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
+{
+	uint8_t	val;
+
+	val = read_reg(reg);
+	val &= ~clearbits;
+	val |= setbits;
+	write_reg(reg, val);
+}
+
+int
+BMA180::set_range(unsigned max_g)
+{
+	uint8_t rangebits;
+
+	if (max_g == 0)
+		max_g = 16;
+
+	if (max_g > 16)
+		return -ERANGE;
+
+	if (max_g <= 2) {
+		_current_range = 2;
+		rangebits = OFFSET_LSB1_RANGE_2G;
+
+	} else if (max_g <= 3) {
+		_current_range = 3;
+		rangebits = OFFSET_LSB1_RANGE_3G;
+
+	} else if (max_g <= 4) {
+		_current_range = 4;
+		rangebits = OFFSET_LSB1_RANGE_4G;
+
+	} else if (max_g <= 8) {
+		_current_range = 8;
+		rangebits = OFFSET_LSB1_RANGE_8G;
+
+	} else if (max_g <= 16) {
+		_current_range = 16;
+		rangebits = OFFSET_LSB1_RANGE_16G;
+
+	} else {
+		return -EINVAL;
+	}
+
+	/* set new range scaling factor */
+	_accel_range_m_s2 = _current_range * 9.80665f;
+	_accel_range_scale = _accel_range_m_s2 / 8192.0f;
+
+	/* enable writing to chip config */
+	modify_reg(ADDR_CTRL_REG0, 0, REG0_WRITE_ENABLE);
+
+	/* adjust sensor configuration */
+	modify_reg(ADDR_OFFSET_LSB1, OFFSET_LSB1_RANGE_MASK, rangebits);
+
+	/* block writing to chip config */
+	modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0);
+
+	/* check if wanted value is now in register */
+	return !((read_reg(ADDR_OFFSET_LSB1) & OFFSET_LSB1_RANGE_MASK) ==
+		 (OFFSET_LSB1_RANGE_MASK & rangebits));
+}
+
+int
+BMA180::set_lowpass(unsigned frequency)
+{
+	uint8_t	bwbits;
+
+	if (frequency > 1200) {
+		return -ERANGE;
+
+	} else if (frequency > 600) {
+		bwbits = BW_TCS_BW_1200HZ;
+
+	} else if (frequency > 300) {
+		bwbits = BW_TCS_BW_600HZ;
+
+	} else if (frequency > 150) {
+		bwbits = BW_TCS_BW_300HZ;
+
+	} else if (frequency > 75) {
+		bwbits = BW_TCS_BW_150HZ;
+
+	} else if (frequency > 40) {
+		bwbits = BW_TCS_BW_75HZ;
+
+	} else if (frequency > 20) {
+		bwbits = BW_TCS_BW_40HZ;
+
+	} else if (frequency > 10) {
+		bwbits = BW_TCS_BW_20HZ;
+
+	} else {
+		bwbits = BW_TCS_BW_10HZ;
+	}
+
+	/* enable writing to chip config */
+	modify_reg(ADDR_CTRL_REG0, 0, REG0_WRITE_ENABLE);
+
+	/* adjust sensor configuration */
+	modify_reg(ADDR_BW_TCS, BW_TCS_BW_MASK, bwbits);
+
+	/* block writing to chip config */
+	modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0);
+
+	/* check if wanted value is now in register */
+	return !((read_reg(ADDR_BW_TCS) & BW_TCS_BW_MASK) ==
+		 (BW_TCS_BW_MASK & bwbits));
+}
+
+void
+BMA180::start()
+{
+	/* make sure we are stopped first */
+	stop();
+
+	/* reset the report ring */
+	_oldest_report = _next_report = 0;
+
+	/* start polling at the specified rate */
+	hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&BMA180::measure_trampoline, this);
+}
+
+void
+BMA180::stop()
+{
+	hrt_cancel(&_call);
+}
+
+void
+BMA180::measure_trampoline(void *arg)
+{
+	BMA180 *dev = (BMA180 *)arg;
+
+	/* make another measurement */
+	dev->measure();
+}
+
+void
+BMA180::measure()
+{
+	/* BMA180 measurement registers */
+// #pragma pack(push, 1)
+// 	struct {
+// 		uint8_t		cmd;
+// 		int16_t	x;
+// 		int16_t	y;
+// 		int16_t	z;
+// 	} raw_report;
+// #pragma pack(pop)
+
+	accel_report		*report = &_reports[_next_report];
+
+	/* start the performance counter */
+	perf_begin(_sample_perf);
+
+	/*
+	 * Fetch the full set of measurements from the BMA180 in one pass;
+	 * starting from the X LSB.
+	 */
+	//raw_report.cmd = ADDR_ACC_X_LSB;
+	// XXX PX4DEV transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report));
+
+	/*
+	 * Adjust and scale results to SI units.
+	 *
+	 * Note that we ignore the "new data" bits.  At any time we read, each
+	 * of the axis measurements are the "most recent", even if we've seen
+	 * them before.  There is no good way to synchronise with the internal
+	 * measurement flow without using the external interrupt.
+	 */
+	_reports[_next_report].timestamp = hrt_absolute_time();
+	/*
+	 * y of board is x of sensor and x of board is -y of sensor
+	 * perform only the axis assignment here.
+	 * Two non-value bits are discarded directly
+	 */
+	report->y_raw  = read_reg(ADDR_ACC_X_LSB + 0);
+	report->y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8;
+	report->x_raw  = read_reg(ADDR_ACC_X_LSB + 2);
+	report->x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8;
+	report->z_raw  = read_reg(ADDR_ACC_X_LSB + 4);
+	report->z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8;
+
+	/* discard two non-value bits in the 16 bit measurement */
+	report->x_raw = (report->x_raw / 4);
+	report->y_raw = (report->y_raw / 4);
+	report->z_raw = (report->z_raw / 4);
+
+	/* invert y axis, due to 14 bit data no overflow can occur in the negation */
+	report->y_raw = -report->y_raw;
+
+	report->x = ((report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
+	report->y = ((report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
+	report->z = ((report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
+	report->scaling = _accel_range_scale;
+	report->range_m_s2 = _accel_range_m_s2;
+
+	/* post a report to the ring - note, not locked */
+	INCREMENT(_next_report, _num_reports);
+
+	/* if we are running up against the oldest report, fix it */
+	if (_next_report == _oldest_report)
+		INCREMENT(_oldest_report, _num_reports);
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	/* publish for subscribers */
+	orb_publish(ORB_ID(sensor_accel), _accel_topic, report);
+
+	/* stop the perf counter */
+	perf_end(_sample_perf);
+}
+
+void
+BMA180::print_info()
+{
+	perf_print_counter(_sample_perf);
+	printf("report queue:   %u (%u/%u @ %p)\n",
+	       _num_reports, _oldest_report, _next_report, _reports);
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace bma180
+{
+
+BMA180	*g_dev;
+
+void	start();
+void	test();
+void	reset();
+void	info();
+
+/**
+ * Start the driver.
+ */
+void
+start()
+{
+	int fd;
+
+	if (g_dev != nullptr)
+		errx(1, "already started");
+
+	/* create the driver */
+	g_dev = new BMA180(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_ACCEL);
+
+	if (g_dev == nullptr)
+		goto fail;
+
+	if (OK != g_dev->init())
+		goto fail;
+
+	/* set the poll rate to default, starts automatic data collection */
+	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		goto fail;
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+		goto fail;
+
+	exit(0);
+fail:
+
+	if (g_dev != nullptr) {
+		delete g_dev;
+		g_dev = nullptr;
+	}
+
+	errx(1, "driver start failed");
+}
+
+/**
+ * Perform some basic functional tests on the driver;
+ * make sure we can collect data from the sensor in polled
+ * and automatic modes.
+ */
+void
+test()
+{
+	int fd = -1;
+	struct accel_report a_report;
+	ssize_t sz;
+
+	/* get the driver */
+	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "%s open failed (try 'bma180 start' if the driver is not running)",
+		    ACCEL_DEVICE_PATH);
+
+	/* reset to manual polling */
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
+		err(1, "reset to manual polling");
+
+	/* do a simple demand read */
+	sz = read(fd, &a_report, sizeof(a_report));
+
+	if (sz != sizeof(a_report))
+		err(1, "immediate acc read failed");
+
+	warnx("single read");
+	warnx("time:     %lld", a_report.timestamp);
+	warnx("acc  x:  \t%8.4f\tm/s^2", (double)a_report.x);
+	warnx("acc  y:  \t%8.4f\tm/s^2", (double)a_report.y);
+	warnx("acc  z:  \t%8.4f\tm/s^2", (double)a_report.z);
+	warnx("acc  x:  \t%d\traw 0x%0x", (short)a_report.x_raw, (unsigned short)a_report.x_raw);
+	warnx("acc  y:  \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw);
+	warnx("acc  z:  \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw);
+	warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2,
+	      (double)(a_report.range_m_s2 / 9.81f));
+
+	/* XXX add poll-rate tests here too */
+
+	reset();
+	errx(0, "PASS");
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+	int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "failed ");
+
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
+		err(1, "driver reset failed");
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+		err(1, "driver poll restart failed");
+
+	exit(0);
+}
+
+/**
+ * Print a little info about the driver.
+ */
+void
+info()
+{
+	if (g_dev == nullptr)
+		errx(1, "BMA180: driver not running");
+
+	printf("state @ %p\n", g_dev);
+	g_dev->print_info();
+
+	exit(0);
+}
+
+
+} // namespace
+
+int
+bma180_main(int argc, char *argv[])
+{
+	/*
+	 * Start/load the driver.
+
+	 */
+	if (!strcmp(argv[1], "start"))
+		bma180::start();
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test"))
+		bma180::test();
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[1], "reset"))
+		bma180::reset();
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[1], "info"))
+		bma180::info();
+
+	errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'");
+}

src/drivers/bma180/module.mk

@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Makefile to build the BMA180 driver.
+#
+
+MODULE_COMMAND	= bma180
+
+SRCS		= bma180.cpp

src/drivers/gps/gps.cpp

@@ -0,0 +1,536 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file gps.cpp
+ * Driver for the GPS on a serial port
+ */
+
+#include <nuttx/clock.h>
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <nuttx/config.h>
+#include <nuttx/arch.h>
+#include <arch/board/board.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/device/i2c.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/scheduling_priorities.h>
+#include <systemlib/err.h>
+#include <drivers/drv_gps.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_gps_position.h>
+
+#include "ubx.h"
+#include "mtk.h"
+
+
+#define TIMEOUT_5HZ 500
+#define RATE_MEASUREMENT_PERIOD 5000000
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
+
+
+class GPS : public device::CDev
+{
+public:
+	GPS(const char* uart_path);
+	virtual ~GPS();
+
+	virtual int			init();
+
+	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	/**
+	 * Diagnostics - print some basic information about the driver.
+	 */
+	void				print_info();
+
+private:
+
+	bool				_task_should_exit;				///< flag to make the main worker task exit
+	int				_serial_fd;					///< serial interface to GPS
+	unsigned			_baudrate;					///< current baudrate
+	char				_port[20];					///< device / serial port path
+	volatile int			_task;						//< worker task
+	bool				_healthy;					///< flag to signal if the GPS is ok
+	bool 				_baudrate_changed;				///< flag to signal that the baudrate with the GPS has changed
+	bool				_mode_changed;					///< flag that the GPS mode has changed
+	gps_driver_mode_t		_mode;						///< current mode
+	GPS_Helper			*_Helper;					///< instance of GPS parser
+	struct vehicle_gps_position_s 	_report;					///< uORB topic for gps position
+	orb_advert_t			_report_pub;					///< uORB pub for gps position
+	float				_rate;						///< position update rate
+
+
+	/**
+	 * Try to configure the GPS, handle outgoing communication to the GPS
+	 */
+	void				config();
+
+	/**
+	 * Trampoline to the worker task
+	 */
+	static void			task_main_trampoline(void *arg);
+
+
+	/**
+	 * Worker task: main GPS thread that configures the GPS and parses incoming data, always running
+	 */
+	void				task_main(void);
+
+	/**
+	 * Set the baudrate of the UART to the GPS
+	 */
+	int				set_baudrate(unsigned baud);
+
+	/**
+	 * Send a reset command to the GPS
+	 */
+	void				cmd_reset();
+
+};
+
+
+/*
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int gps_main(int argc, char *argv[]);
+
+namespace
+{
+
+GPS	*g_dev;
+
+}
+
+
+GPS::GPS(const char* uart_path) :
+	CDev("gps", GPS_DEVICE_PATH),
+	_task_should_exit(false),
+	_healthy(false),
+	_mode_changed(false),
+	_mode(GPS_DRIVER_MODE_UBX),
+	_Helper(nullptr),
+	_report_pub(-1),
+	_rate(0.0f)
+{
+	/* store port name */
+	strncpy(_port, uart_path, sizeof(_port));
+	/* enforce null termination */
+	_port[sizeof(_port) - 1] = '\0';
+
+	/* we need this potentially before it could be set in task_main */
+	g_dev = this;
+	memset(&_report, 0, sizeof(_report));
+
+	_debug_enabled = true;
+}
+
+GPS::~GPS()
+{
+	/* tell the task we want it to go away */
+	_task_should_exit = true;
+
+	/* spin waiting for the task to stop */
+	for (unsigned i = 0; (i < 10) && (_task != -1); i++) {
+		/* give it another 100ms */
+		usleep(100000);
+	}
+
+	/* well, kill it anyway, though this will probably crash */
+	if (_task != -1)
+		task_delete(_task);
+	g_dev = nullptr;
+
+}
+
+int
+GPS::init()
+{
+	int ret = ERROR;
+
+	/* do regular cdev init */
+	if (CDev::init() != OK)
+		goto out;
+
+	/* start the GPS driver worker task */
+	_task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 2048, (main_t)&GPS::task_main_trampoline, nullptr);
+
+	if (_task < 0) {
+		warnx("task start failed: %d", errno);
+		return -errno;
+	}
+
+	ret = OK;
+out:
+	return ret;
+}
+
+int
+GPS::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	lock();
+
+	int ret = OK;
+
+	switch (cmd) {
+	case SENSORIOCRESET:
+		cmd_reset();
+		break;
+	}
+
+	unlock();
+
+	return ret;
+}
+
+void
+GPS::task_main_trampoline(void *arg)
+{
+	g_dev->task_main();
+}
+
+void
+GPS::task_main()
+{
+	log("starting");
+
+	/* open the serial port */
+	_serial_fd = ::open(_port, O_RDWR);
+
+	if (_serial_fd < 0) {
+		log("failed to open serial port: %s err: %d", _port, errno);
+		/* tell the dtor that we are exiting, set error code */
+		_task = -1;
+		_exit(1);
+	}
+
+	uint64_t last_rate_measurement = hrt_absolute_time();
+	unsigned last_rate_count = 0;
+
+	/* loop handling received serial bytes and also configuring in between */
+	while (!_task_should_exit) {
+
+		if (_Helper != nullptr) {
+			delete(_Helper);
+			/* set to zero to ensure parser is not used while not instantiated */
+			_Helper = nullptr;
+		}
+
+		switch (_mode) {
+			case GPS_DRIVER_MODE_UBX:
+				_Helper = new UBX(_serial_fd, &_report);
+				break;
+			case GPS_DRIVER_MODE_MTK:
+				_Helper = new MTK(_serial_fd, &_report);
+				break;
+			case GPS_DRIVER_MODE_NMEA:
+				//_Helper = new NMEA(); //TODO: add NMEA
+				break;
+			default:
+				break;
+		}
+		unlock();
+		if (_Helper->configure(_baudrate) == 0) {
+			unlock();
+			while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) {
+//				lock();
+				/* opportunistic publishing - else invalid data would end up on the bus */
+				if (_report_pub > 0) {
+					orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
+				} else {
+					_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
+				}
+
+				last_rate_count++;
+
+				/* measure update rate every 5 seconds */
+				if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
+					_rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f);
+					last_rate_measurement = hrt_absolute_time();
+					last_rate_count = 0;
+				}
+
+				if (!_healthy) {
+					warnx("module found");
+					_healthy = true;
+				}
+			}
+			if (_healthy) {
+				warnx("module lost");
+				_healthy = false;
+				_rate = 0.0f;
+			}
+
+			lock();
+		}
+		lock();
+
+		/* select next mode */
+		switch (_mode) {
+			case GPS_DRIVER_MODE_UBX:
+				_mode = GPS_DRIVER_MODE_MTK;
+				break;
+			case GPS_DRIVER_MODE_MTK:
+				_mode = GPS_DRIVER_MODE_UBX;
+				break;
+		//				case GPS_DRIVER_MODE_NMEA:
+		//					_mode = GPS_DRIVER_MODE_UBX;
+		//					break;
+			default:
+				break;
+		}
+
+	}
+	debug("exiting");
+
+	::close(_serial_fd);
+
+	/* tell the dtor that we are exiting */
+	_task = -1;
+	_exit(0);
+}
+
+
+
+void
+GPS::cmd_reset()
+{
+	//XXX add reset?
+}
+
+void
+GPS::print_info()
+{
+	switch (_mode) {
+		case GPS_DRIVER_MODE_UBX:
+			warnx("protocol: UBX");
+			break;
+		case GPS_DRIVER_MODE_MTK:
+			warnx("protocol: MTK");
+			break;
+		case GPS_DRIVER_MODE_NMEA:
+			warnx("protocol: NMEA");
+			break;
+		default:
+			break;
+	}
+	warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
+	if (_report.timestamp_position != 0) {
+		warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type,
+			(double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f));
+		warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
+		warnx("update rate: %6.2f Hz", (double)_rate);
+	}
+
+	usleep(100000);
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace gps
+{
+
+GPS	*g_dev;
+
+void	start(const char *uart_path);
+void	stop();
+void	test();
+void	reset();
+void	info();
+
+/**
+ * Start the driver.
+ */
+void
+start(const char *uart_path)
+{
+	int fd;
+
+	if (g_dev != nullptr)
+		errx(1, "already started");
+
+	/* create the driver */
+	g_dev = new GPS(uart_path);
+
+	if (g_dev == nullptr)
+		goto fail;
+
+	if (OK != g_dev->init())
+		goto fail;
+
+	/* set the poll rate to default, starts automatic data collection */
+	fd = open(GPS_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		errx(1, "Could not open device path: %s\n", GPS_DEVICE_PATH);
+		goto fail;
+	}
+	exit(0);
+
+fail:
+
+	if (g_dev != nullptr) {
+		delete g_dev;
+		g_dev = nullptr;
+	}
+
+	errx(1, "driver start failed");
+}
+
+/**
+ * Stop the driver.
+ */
+void
+stop()
+{
+	delete g_dev;
+	g_dev = nullptr;
+
+	exit(0);
+}
+
+/**
+ * Perform some basic functional tests on the driver;
+ * make sure we can collect data from the sensor in polled
+ * and automatic modes.
+ */
+void
+test()
+{
+
+	errx(0, "PASS");
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+	int fd = open(GPS_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "failed ");
+
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
+		err(1, "driver reset failed");
+
+	exit(0);
+}
+
+/**
+ * Print the status of the driver.
+ */
+void
+info()
+{
+	if (g_dev == nullptr)
+		errx(1, "driver not running");
+
+	g_dev->print_info();
+
+	exit(0);
+}
+
+} // namespace
+
+
+int
+gps_main(int argc, char *argv[])
+{
+
+	/* set to default */
+	char* device_name = GPS_DEFAULT_UART_PORT;
+
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(argv[1], "start")) {
+		/* work around getopt unreliability */
+		if (argc > 3) {
+			if (!strcmp(argv[2], "-d")) {
+				device_name = argv[3];
+			} else {
+				goto out;
+			}
+		}
+		gps::start(device_name);
+	}
+
+	if (!strcmp(argv[1], "stop"))
+		gps::stop();
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test"))
+		gps::test();
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[1], "reset"))
+		gps::reset();
+
+	/*
+	 * Print driver status.
+	 */
+	if (!strcmp(argv[1], "status"))
+		gps::info();
+
+out:
+	errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n]");
+}

src/drivers/gps/gps_helper.cpp

@@ -0,0 +1,92 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <termios.h>
+#include <errno.h>
+#include <systemlib/err.h>
+#include "gps_helper.h"
+
+/* @file gps_helper.cpp */
+
+int
+GPS_Helper::set_baudrate(const int &fd, unsigned baud)
+{
+	/* process baud rate */
+	int speed;
+
+	switch (baud) {
+	case 9600:   speed = B9600;   break;
+
+	case 19200:  speed = B19200;  break;
+
+	case 38400:  speed = B38400;  break;
+
+	case 57600:  speed = B57600;  break;
+
+	case 115200: speed = B115200; break;
+
+	warnx("try baudrate: %d\n", speed);
+
+	default:
+		warnx("ERROR: Unsupported baudrate: %d\n", baud);
+		return -EINVAL;
+	}
+	struct termios uart_config;
+	int termios_state;
+
+	/* fill the struct for the new configuration */
+	tcgetattr(fd, &uart_config);
+
+	/* clear ONLCR flag (which appends a CR for every LF) */
+	uart_config.c_oflag &= ~ONLCR;
+	/* no parity, one stop bit */
+	uart_config.c_cflag &= ~(CSTOPB | PARENB);
+
+	/* set baud rate */
+	if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
+		warnx("ERROR setting config: %d (cfsetispeed)\n", termios_state);
+		return -1;
+	}
+	if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
+		warnx("ERROR setting config: %d (cfsetospeed)\n", termios_state);
+		return -1;
+	}
+	if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
+		warnx("ERROR setting baudrate (tcsetattr)\n");
+		return -1;
+	}
+	/* XXX if resetting the parser here, ensure it does exist (check for null pointer) */
+	return 0;
+}

src/drivers/gps/gps_helper.h

@@ -0,0 +1,52 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/* @file gps_helper.h */
+
+#ifndef GPS_HELPER_H
+#define GPS_HELPER_H
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_gps_position.h>
+
+class GPS_Helper
+{
+public:
+	virtual int				configure(unsigned &baud) = 0;
+	virtual int 			receive(unsigned timeout) = 0;
+	int 					set_baudrate(const int &fd, unsigned baud);
+};
+
+#endif /* GPS_HELPER_H */

src/drivers/gps/module.mk

@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# GPS driver
+#
+
+MODULE_COMMAND	= gps
+
+SRCS		= gps.cpp \
+		  gps_helper.cpp \
+		  mtk.cpp \
+		  ubx.cpp

src/drivers/gps/mtk.cpp

@@ -0,0 +1,274 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/* @file mkt.cpp */
+
+#include <unistd.h>
+#include <stdio.h>
+#include <poll.h>
+#include <math.h>
+#include <string.h>
+#include <assert.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+
+#include "mtk.h"
+
+
+MTK::MTK(const int &fd, struct vehicle_gps_position_s *gps_position) :
+_fd(fd),
+_gps_position(gps_position),
+_mtk_revision(0)
+{
+	decode_init();
+}
+
+MTK::~MTK()
+{
+}
+
+int
+MTK::configure(unsigned &baudrate)
+{
+	/* set baudrate first */
+	if (GPS_Helper::set_baudrate(_fd, MTK_BAUDRATE) != 0)
+		return -1;
+
+	baudrate = MTK_BAUDRATE;
+
+	/* Write config messages, don't wait for an answer */
+	if (strlen(MTK_OUTPUT_5HZ) != write(_fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) {
+		warnx("mtk: config write failed");
+		return -1;
+	}
+	usleep(10000);
+
+	if (strlen(MTK_SET_BINARY) != write(_fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) {
+		warnx("mtk: config write failed");
+		return -1;
+	}
+	usleep(10000);
+
+	if (strlen(SBAS_ON) != write(_fd, SBAS_ON, strlen(SBAS_ON))) {
+		warnx("mtk: config write failed");
+		return -1;
+	}
+	usleep(10000);
+
+	if (strlen(WAAS_ON) != write(_fd, WAAS_ON, strlen(WAAS_ON))) {
+		warnx("mtk: config write failed");
+		return -1;
+	}
+	usleep(10000);
+
+	if (strlen(MTK_NAVTHRES_OFF) != write(_fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) {
+		warnx("mtk: config write failed");
+		return -1;
+	}
+
+	return 0;
+}
+
+int
+MTK::receive(unsigned timeout)
+{
+	/* poll descriptor */
+	pollfd fds[1];
+	fds[0].fd = _fd;
+	fds[0].events = POLLIN;
+
+	uint8_t buf[32];
+	gps_mtk_packet_t packet;
+
+	/* timeout additional to poll */
+	uint64_t time_started = hrt_absolute_time();
+
+	int j = 0;
+	ssize_t count = 0;
+
+	while (true) {
+
+		/* first read whatever is left */
+		if (j < count) {
+			/* pass received bytes to the packet decoder */
+			while (j < count) {
+				if (parse_char(buf[j], packet) > 0) {
+					handle_message(packet);
+					return 1;
+				}
+				/* in case we keep trying but only get crap from GPS */
+				if (time_started + timeout*1000 < hrt_absolute_time() ) {
+					return -1;
+				}
+				j++;
+			}
+			/* everything is read */
+			j = count = 0;
+		}
+
+		/* then poll for new data */
+		int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout);
+
+		if (ret < 0) {
+			/* something went wrong when polling */
+			return -1;
+
+		} else if (ret == 0) {
+			/* Timeout */
+			return -1;
+
+		} else if (ret > 0) {
+			/* if we have new data from GPS, go handle it */
+			if (fds[0].revents & POLLIN) {
+				/*
+				 * We are here because poll says there is some data, so this
+				 * won't block even on a blocking device.  If more bytes are
+				 * available, we'll go back to poll() again...
+				 */
+				count = ::read(_fd, buf, sizeof(buf));
+			}
+		}
+	}
+}
+
+void
+MTK::decode_init(void)
+{
+	_rx_ck_a = 0;
+	_rx_ck_b = 0;
+	_rx_count = 0;
+	_decode_state = MTK_DECODE_UNINIT;
+}
+int
+MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
+{
+	int ret = 0;
+
+	if (_decode_state == MTK_DECODE_UNINIT) {
+
+		if (b == MTK_SYNC1_V16) {
+			_decode_state = MTK_DECODE_GOT_CK_A;
+			_mtk_revision = 16;
+		} else if (b == MTK_SYNC1_V19) {
+			_decode_state = MTK_DECODE_GOT_CK_A;
+			_mtk_revision = 19;
+		}
+
+	} else if (_decode_state == MTK_DECODE_GOT_CK_A) {
+		if (b == MTK_SYNC2) {
+			_decode_state = MTK_DECODE_GOT_CK_B;
+
+		} else {
+			// Second start symbol was wrong, reset state machine
+			decode_init();
+		}
+
+	} else if (_decode_state == MTK_DECODE_GOT_CK_B) {
+		// Add to checksum
+		if (_rx_count < 33)
+			add_byte_to_checksum(b);
+
+		// Fill packet buffer
+		((uint8_t*)(&packet))[_rx_count] = b;
+		_rx_count++;
+
+		/* Packet size minus checksum, XXX ? */
+		if (_rx_count >= sizeof(packet)) {
+			/* Compare checksum */
+			if (_rx_ck_a == packet.ck_a && _rx_ck_b == packet.ck_b) {
+				ret = 1;
+			} else {
+				warnx("MTK Checksum invalid");
+				ret = -1;
+			}
+			// Reset state machine to decode next packet
+			decode_init();
+		}
+	}
+	return ret;
+}
+
+void
+MTK::handle_message(gps_mtk_packet_t &packet)
+{
+	if (_mtk_revision == 16) {
+		_gps_position->lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7
+		_gps_position->lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7
+	} else if (_mtk_revision == 19) {
+		_gps_position->lat = packet.latitude; // both degrees*1e7
+		_gps_position->lon = packet.longitude; // both degrees*1e7
+	} else {
+		warnx("mtk: unknown revision");
+		_gps_position->lat = 0;
+		_gps_position->lon = 0;
+	}
+	_gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm
+	_gps_position->fix_type = packet.fix_type;
+	_gps_position->eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit
+	_gps_position->epv_m = 0.0; //unknown in mtk custom mode
+	_gps_position->vel_m_s = ((float)packet.ground_speed)*1e-2f; // from cm/s to m/s
+	_gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad
+	_gps_position->satellites_visible = packet.satellites;
+
+	/* convert time and date information to unix timestamp */
+	struct tm timeinfo; //TODO: test this conversion
+	uint32_t timeinfo_conversion_temp;
+
+	timeinfo.tm_mday = packet.date * 1e-4;
+	timeinfo_conversion_temp = packet.date - timeinfo.tm_mday * 1e4;
+	timeinfo.tm_mon = timeinfo_conversion_temp * 1e-2 - 1;
+	timeinfo.tm_year = (timeinfo_conversion_temp - (timeinfo.tm_mon + 1) * 1e2) + 100;
+
+	timeinfo.tm_hour = packet.utc_time * 1e-7;
+	timeinfo_conversion_temp = packet.utc_time - timeinfo.tm_hour * 1e7;
+	timeinfo.tm_min = timeinfo_conversion_temp * 1e-5;
+	timeinfo_conversion_temp -= timeinfo.tm_min * 1e5;
+	timeinfo.tm_sec = timeinfo_conversion_temp * 1e-3;
+	timeinfo_conversion_temp -= timeinfo.tm_sec * 1e3;
+	time_t epoch = mktime(&timeinfo);
+
+	_gps_position->time_gps_usec = epoch * 1e6; //TODO: test this
+	_gps_position->time_gps_usec += timeinfo_conversion_temp * 1e3;
+	_gps_position->timestamp_position = _gps_position->timestamp_time = hrt_absolute_time();
+
+	return;
+}
+
+void
+MTK::add_byte_to_checksum(uint8_t b)
+{
+	_rx_ck_a = _rx_ck_a + b;
+	_rx_ck_b = _rx_ck_b + _rx_ck_a;
+}

src/drivers/gps/mtk.h

@@ -0,0 +1,124 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/* @file mtk.h */
+
+#ifndef MTK_H_
+#define MTK_H_
+
+#include "gps_helper.h"
+
+#define MTK_SYNC1_V16 0xd0
+#define MTK_SYNC1_V19 0xd1
+#define MTK_SYNC2 0xdd
+
+#define MTK_OUTPUT_5HZ		"$PMTK220,200*2C\r\n"
+#define MTK_SET_BINARY		"$PGCMD,16,0,0,0,0,0*6A\r\n"
+#define SBAS_ON	        	"$PMTK313,1*2E\r\n"
+#define WAAS_ON				"$PMTK301,2*2E\r\n"
+#define MTK_NAVTHRES_OFF 	"$PMTK397,0*23\r\n"
+
+#define MTK_TIMEOUT_5HZ 400
+#define MTK_BAUDRATE 38400
+
+typedef enum {
+	MTK_DECODE_UNINIT = 0,
+	MTK_DECODE_GOT_CK_A = 1,
+	MTK_DECODE_GOT_CK_B = 2
+} mtk_decode_state_t;
+
+/** the structures of the binary packets */
+#pragma pack(push, 1)
+
+typedef struct {
+	uint8_t payload; ///< Number of payload bytes
+	int32_t latitude;  ///< Latitude in degrees * 10^7
+	int32_t longitude; ///< Longitude in degrees * 10^7
+	uint32_t msl_altitude;  ///< MSL altitude in meters * 10^2
+	uint32_t ground_speed; ///< velocity in m/s
+	int32_t heading; ///< heading in degrees * 10^2
+	uint8_t satellites; ///< number of sattelites used
+	uint8_t fix_type;  ///< fix type: XXX correct for that
+	uint32_t date;
+	uint32_t utc_time;
+	uint16_t hdop; ///< horizontal dilution of position (without unit)
+	uint8_t ck_a;
+	uint8_t ck_b;
+} gps_mtk_packet_t;
+
+#pragma pack(pop)
+
+#define MTK_RECV_BUFFER_SIZE 40
+
+class MTK : public GPS_Helper
+{
+public:
+	MTK(const int &fd, struct vehicle_gps_position_s *gps_position);
+	~MTK();
+	int					receive(unsigned timeout);
+	int					configure(unsigned &baudrate);
+
+private:
+	/**
+	 * Parse the binary MTK packet
+	 */
+	int					parse_char(uint8_t b, gps_mtk_packet_t &packet);
+
+	/**
+	 * Handle the package once it has arrived
+	 */
+	void				handle_message(gps_mtk_packet_t &packet);
+
+	/**
+	 * Reset the parse state machine for a fresh start
+	 */
+	void				decode_init(void);
+
+	/**
+	 * While parsing add every byte (except the sync bytes) to the checksum
+	 */
+	void				add_byte_to_checksum(uint8_t);
+
+	int					_fd;
+	struct vehicle_gps_position_s *_gps_position;
+	mtk_decode_state_t	_decode_state;
+	uint8_t				_mtk_revision;
+	uint8_t				_rx_buffer[MTK_RECV_BUFFER_SIZE];
+	unsigned			_rx_count;
+	uint8_t 			_rx_ck_a;
+	uint8_t				_rx_ck_b;
+};
+
+#endif /* MTK_H_ */

src/drivers/gps/ubx.cpp

@@ -0,0 +1,755 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ubx.cpp
+ *
+ * U-Blox protocol implementation. Following u-blox 6/7 Receiver Description
+ * including Prototol Specification.
+ *
+ * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf
+ */
+
+#include <unistd.h>
+#include <stdio.h>
+#include <poll.h>
+#include <math.h>
+#include <string.h>
+#include <assert.h>
+#include <systemlib/err.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <drivers/drv_hrt.h>
+
+#include "ubx.h"
+
+#define UBX_CONFIG_TIMEOUT 100
+
+UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
+_fd(fd),
+_gps_position(gps_position),
+_waiting_for_ack(false)
+{
+	decode_init();
+}
+
+UBX::~UBX()
+{
+}
+
+int
+UBX::configure(unsigned &baudrate)
+{
+	_waiting_for_ack = true;
+
+	/* try different baudrates */
+	const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200};
+
+	for (int baud_i = 0; baud_i < 5; baud_i++) {
+		baudrate = baudrates_to_try[baud_i];
+		set_baudrate(_fd, baudrate);
+
+		/* Send a CFG-PRT message to set the UBX protocol for in and out
+		 * and leave the baudrate as it is, we just want an ACK-ACK from this
+		 */
+		type_gps_bin_cfg_prt_packet_t cfg_prt_packet;
+		/* Set everything else of the packet to 0, otherwise the module wont accept it */
+		memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet));
+
+		_clsID_needed = UBX_CLASS_CFG;
+		_msgID_needed = UBX_MESSAGE_CFG_PRT;
+
+		/* Define the package contents, don't change the baudrate */
+		cfg_prt_packet.clsID		= UBX_CLASS_CFG;
+		cfg_prt_packet.msgID		= UBX_MESSAGE_CFG_PRT;
+		cfg_prt_packet.length		= UBX_CFG_PRT_LENGTH;
+		cfg_prt_packet.portID		= UBX_CFG_PRT_PAYLOAD_PORTID;
+		cfg_prt_packet.mode			= UBX_CFG_PRT_PAYLOAD_MODE;
+		cfg_prt_packet.baudRate		= baudrate;
+		cfg_prt_packet.inProtoMask	= UBX_CFG_PRT_PAYLOAD_INPROTOMASK;
+		cfg_prt_packet.outProtoMask	= UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK;
+
+		send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet));
+
+		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
+			/* try next baudrate */
+			continue;
+		}
+
+		/* Send a CFG-PRT message again, this time change the baudrate */
+
+		cfg_prt_packet.clsID		= UBX_CLASS_CFG;
+		cfg_prt_packet.msgID		= UBX_MESSAGE_CFG_PRT;
+		cfg_prt_packet.length		= UBX_CFG_PRT_LENGTH;
+		cfg_prt_packet.portID		= UBX_CFG_PRT_PAYLOAD_PORTID;
+		cfg_prt_packet.mode			= UBX_CFG_PRT_PAYLOAD_MODE;
+		cfg_prt_packet.baudRate		= UBX_CFG_PRT_PAYLOAD_BAUDRATE;
+		cfg_prt_packet.inProtoMask	= UBX_CFG_PRT_PAYLOAD_INPROTOMASK;
+		cfg_prt_packet.outProtoMask	= UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK;
+
+		send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet));
+		
+		/* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */
+		receive(UBX_CONFIG_TIMEOUT);
+		
+		if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) {
+			set_baudrate(_fd, UBX_CFG_PRT_PAYLOAD_BAUDRATE);
+			baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE;
+		}
+
+		/* send a CFT-RATE message to define update rate */
+		type_gps_bin_cfg_rate_packet_t cfg_rate_packet;
+		memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet));
+
+		_clsID_needed = UBX_CLASS_CFG;
+		_msgID_needed = UBX_MESSAGE_CFG_RATE;
+
+		cfg_rate_packet.clsID		= UBX_CLASS_CFG;
+		cfg_rate_packet.msgID		= UBX_MESSAGE_CFG_RATE;
+		cfg_rate_packet.length		= UBX_CFG_RATE_LENGTH;
+		cfg_rate_packet.measRate	= UBX_CFG_RATE_PAYLOAD_MEASRATE;
+		cfg_rate_packet.navRate		= UBX_CFG_RATE_PAYLOAD_NAVRATE;
+		cfg_rate_packet.timeRef		= UBX_CFG_RATE_PAYLOAD_TIMEREF;
+
+		send_config_packet(_fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet));
+		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
+			/* try next baudrate */
+			continue;
+		}
+
+		/* send a NAV5 message to set the options for the internal filter */
+		type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet;
+		memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet));
+
+		_clsID_needed = UBX_CLASS_CFG;
+		_msgID_needed = UBX_MESSAGE_CFG_NAV5;
+
+		cfg_nav5_packet.clsID        = UBX_CLASS_CFG;
+		cfg_nav5_packet.msgID        = UBX_MESSAGE_CFG_NAV5;
+		cfg_nav5_packet.length       = UBX_CFG_NAV5_LENGTH;
+		cfg_nav5_packet.mask         = UBX_CFG_NAV5_PAYLOAD_MASK;
+		cfg_nav5_packet.dynModel     = UBX_CFG_NAV5_PAYLOAD_DYNMODEL;
+		cfg_nav5_packet.fixMode      = UBX_CFG_NAV5_PAYLOAD_FIXMODE;
+
+		send_config_packet(_fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet));
+		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
+			/* try next baudrate */
+			continue;
+		}
+
+		type_gps_bin_cfg_msg_packet_t cfg_msg_packet;
+		memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet));
+
+		_clsID_needed = UBX_CLASS_CFG;
+		_msgID_needed = UBX_MESSAGE_CFG_MSG;
+
+		cfg_msg_packet.clsID        = UBX_CLASS_CFG;
+		cfg_msg_packet.msgID        = UBX_MESSAGE_CFG_MSG;
+		cfg_msg_packet.length       = UBX_CFG_MSG_LENGTH;
+		/* Choose fast 5Hz rate for all messages except SVINFO which is big and not important */
+		cfg_msg_packet.rate[1]      = UBX_CFG_MSG_PAYLOAD_RATE1_5HZ;
+
+		cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
+		cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH;
+
+		send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
+		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
+			/* try next baudrate */
+			continue;
+		}
+
+		cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
+		cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC;
+
+		send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
+		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
+			/* try next baudrate */
+			continue;
+		}
+
+		cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
+		cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO;
+		/* For satelites info 1Hz is enough */
+		cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_1HZ;
+
+		send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
+		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
+			/* try next baudrate */
+			continue;
+		}
+
+		cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
+		cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL;
+
+		send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
+		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
+			/* try next baudrate */
+			continue;
+		}
+
+		cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
+		cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED;
+
+		send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
+		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
+			/* try next baudrate */
+			continue;
+		}
+//		cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
+//		cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP;
+
+//		cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM;
+//		cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI;
+
+		_waiting_for_ack = false;
+		return 0;
+	}
+	return -1;
+}
+
+int
+UBX::receive(unsigned timeout)
+{
+	/* poll descriptor */
+	pollfd fds[1];
+	fds[0].fd = _fd;
+	fds[0].events = POLLIN;
+
+	uint8_t buf[32];
+
+	/* timeout additional to poll */
+	uint64_t time_started = hrt_absolute_time();
+
+	int j = 0;
+	ssize_t count = 0;
+
+	while (true) {
+
+		/* pass received bytes to the packet decoder */
+		while (j < count) {
+			if (parse_char(buf[j]) > 0) {
+				/* return to configure during configuration or to the gps driver during normal work
+				 * if a packet has arrived */
+				 if (handle_message() > 0)
+					return 1;
+			}
+			/* in case we keep trying but only get crap from GPS */
+			if (time_started + timeout*1000 < hrt_absolute_time() ) {
+				return -1;
+			}
+			j++;
+		}
+
+		/* everything is read */
+		j = count = 0;
+
+		/* then poll for new data */
+		int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout);
+
+		if (ret < 0) {
+			/* something went wrong when polling */
+			return -1;
+
+		} else if (ret == 0) {
+			/* Timeout */
+			return -1;
+
+		} else if (ret > 0) {
+			/* if we have new data from GPS, go handle it */
+			if (fds[0].revents & POLLIN) {
+				/*
+				 * We are here because poll says there is some data, so this
+				 * won't block even on a blocking device.  If more bytes are
+				 * available, we'll go back to poll() again...
+				 */
+				count = ::read(_fd, buf, sizeof(buf));
+			}
+		}
+	}
+}
+
+int
+UBX::parse_char(uint8_t b)
+{
+	switch (_decode_state) {
+		/* First, look for sync1 */
+		case UBX_DECODE_UNINIT:
+			if (b == UBX_SYNC1) {
+				_decode_state = UBX_DECODE_GOT_SYNC1;
+			}
+			break;
+		/* Second, look for sync2 */
+		case UBX_DECODE_GOT_SYNC1:
+			if (b == UBX_SYNC2) {
+				_decode_state = UBX_DECODE_GOT_SYNC2;
+			} else {
+				/* Second start symbol was wrong, reset state machine */
+				decode_init();
+			}
+			break;
+		/* Now look for class */
+		case UBX_DECODE_GOT_SYNC2:
+			/* everything except sync1 and sync2 needs to be added to the checksum */
+			add_byte_to_checksum(b);
+			/* check for known class */
+			switch (b) {
+				case UBX_CLASS_ACK:
+					_decode_state = UBX_DECODE_GOT_CLASS;
+					_message_class = ACK;
+					break;
+
+				case UBX_CLASS_NAV:
+					_decode_state = UBX_DECODE_GOT_CLASS;
+					_message_class = NAV;
+					break;
+
+//				case UBX_CLASS_RXM:
+//					_decode_state = UBX_DECODE_GOT_CLASS;
+//					_message_class = RXM;
+//					break;
+
+				case UBX_CLASS_CFG:
+					_decode_state = UBX_DECODE_GOT_CLASS;
+					_message_class = CFG;
+					break;
+				default: //unknown class: reset state machine
+					decode_init();
+					break;
+			}
+			break;
+		case UBX_DECODE_GOT_CLASS:
+			add_byte_to_checksum(b);
+			switch (_message_class) {
+				case NAV:
+					switch (b) {
+					case UBX_MESSAGE_NAV_POSLLH:
+						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+						_message_id = NAV_POSLLH;
+						break;
+
+					case UBX_MESSAGE_NAV_SOL:
+						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+						_message_id = NAV_SOL;
+						break;
+
+					case UBX_MESSAGE_NAV_TIMEUTC:
+						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+						_message_id = NAV_TIMEUTC;
+						break;
+
+//					case UBX_MESSAGE_NAV_DOP:
+//						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+//						_message_id = NAV_DOP;
+//						break;
+
+					case UBX_MESSAGE_NAV_SVINFO:
+						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+						_message_id = NAV_SVINFO;
+						break;
+
+					case UBX_MESSAGE_NAV_VELNED:
+						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+						_message_id = NAV_VELNED;
+						break;
+
+					default: //unknown class: reset state machine, should not happen
+						decode_init();
+						break;
+					}
+					break;
+//				case RXM:
+//					switch (b) {
+//					case UBX_MESSAGE_RXM_SVSI:
+//						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+//						_message_id = RXM_SVSI;
+//						break;
+//
+//					default: //unknown class: reset state machine, should not happen
+//						decode_init();
+//						break;
+//					}
+//					break;
+
+				case CFG:
+					switch (b) {
+					case UBX_MESSAGE_CFG_NAV5:
+						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+						_message_id = CFG_NAV5;
+						break;
+
+					default: //unknown class: reset state machine, should not happen
+						decode_init();
+						break;
+					}
+					break;
+
+				case ACK:
+					switch (b) {
+					case UBX_MESSAGE_ACK_ACK:
+						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+						_message_id = ACK_ACK;
+						break;
+					case UBX_MESSAGE_ACK_NAK:
+						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+						_message_id = ACK_NAK;
+						break;
+					default: //unknown class: reset state machine, should not happen
+						decode_init();
+						break;
+					}
+					break;
+				default: //should not happen because we set the class
+					warnx("UBX Error, we set a class that we don't know");
+					decode_init();
+//					config_needed = true;
+					break;
+				}
+				break;
+			case UBX_DECODE_GOT_MESSAGEID:
+				add_byte_to_checksum(b);
+				_payload_size = b; //this is the first length byte
+				_decode_state = UBX_DECODE_GOT_LENGTH1;
+				break;
+			case UBX_DECODE_GOT_LENGTH1:
+				add_byte_to_checksum(b);
+				_payload_size += b << 8; // here comes the second byte of length
+				_decode_state = UBX_DECODE_GOT_LENGTH2;
+				break;
+			case UBX_DECODE_GOT_LENGTH2:
+				/* Add to checksum if not yet at checksum byte */
+				if (_rx_count < _payload_size)
+					add_byte_to_checksum(b);
+					_rx_buffer[_rx_count] = b;
+					/* once the payload has arrived, we can process the information */
+					if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes
+
+						/* compare checksum */
+						if (_rx_ck_a == _rx_buffer[_rx_count-1] && _rx_ck_b == _rx_buffer[_rx_count]) {
+							return 1;
+						} else {
+							decode_init();
+							return -1;
+							warnx("ubx: Checksum wrong");
+						}
+
+						return 1;
+					} else if (_rx_count < RECV_BUFFER_SIZE) {
+						_rx_count++;
+					} else {
+						warnx("ubx: buffer full");
+						decode_init();
+						return -1;
+					}
+				break;
+		default:
+			break;
+	}
+	return 0; //XXX ?
+}
+
+
+int
+UBX::handle_message()
+{
+	int ret = 0;
+
+	switch (_message_id) { //this enum is unique for all ids --> no need to check the class
+		case NAV_POSLLH: {
+//			printf("GOT NAV_POSLLH MESSAGE\n");
+			if (!_waiting_for_ack) {
+				gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) _rx_buffer;
+
+				_gps_position->lat = packet->lat;
+				_gps_position->lon = packet->lon;
+				_gps_position->alt = packet->height_msl;
+
+				_gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m
+				_gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m
+
+				/* Add timestamp to finish the report */
+				_gps_position->timestamp_position = hrt_absolute_time();
+				/* only return 1 when new position is available */
+				ret = 1;
+			}
+			break;
+		}
+
+		case NAV_SOL: {
+//			printf("GOT NAV_SOL MESSAGE\n");
+			if (!_waiting_for_ack) {
+				gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer;
+
+				_gps_position->fix_type = packet->gpsFix;
+				_gps_position->s_variance_m_s = packet->sAcc;
+				_gps_position->p_variance_m = packet->pAcc;
+
+				_gps_position->timestamp_variance = hrt_absolute_time();
+			}
+			break;
+		}
+
+//		case NAV_DOP: {
+////		printf("GOT NAV_DOP MESSAGE\n");
+//			gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) _rx_buffer;
+//
+//			_gps_position->eph_m =  packet->hDOP;
+//			_gps_position->epv =  packet->vDOP;
+//
+//			_gps_position->timestamp_posdilution = hrt_absolute_time();
+//
+//			_new_nav_dop = true;
+//
+//			break;
+//		}
+
+		case NAV_TIMEUTC: {
+//			printf("GOT NAV_TIMEUTC MESSAGE\n");
+
+			if (!_waiting_for_ack) {
+				gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer;
+
+				//convert to unix timestamp
+				struct tm timeinfo;
+				timeinfo.tm_year = packet->year - 1900;
+				timeinfo.tm_mon = packet->month - 1;
+				timeinfo.tm_mday = packet->day;
+				timeinfo.tm_hour = packet->hour;
+				timeinfo.tm_min = packet->min;
+				timeinfo.tm_sec = packet->sec;
+
+				time_t epoch = mktime(&timeinfo);
+
+				_gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
+				_gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f);
+
+				_gps_position->timestamp_time = hrt_absolute_time();
+			}
+			break;
+		}
+
+		case NAV_SVINFO: {
+//			printf("GOT NAV_SVINFO MESSAGE\n");
+
+			if (!_waiting_for_ack) {
+				//this is a more complicated message: the length depends on the number of satellites. This number is extracted from the first part of the message
+				const int length_part1 = 8;
+				char _rx_buffer_part1[length_part1];
+				memcpy(_rx_buffer_part1, _rx_buffer, length_part1);
+				gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer_part1;
+
+				//read checksum
+				const int length_part3 = 2;
+				char _rx_buffer_part3[length_part3];
+				memcpy(_rx_buffer_part3, &(_rx_buffer[_rx_count - 1]), length_part3);
+
+				//definitions needed to read numCh elements from the buffer:
+				const int length_part2 = 12;
+				gps_bin_nav_svinfo_part2_packet_t *packet_part2;
+				char _rx_buffer_part2[length_part2]; //for temporal storage
+
+				uint8_t satellites_used = 0;
+				int i;
+
+				for (i = 0; i < packet_part1->numCh; i++) { //for each channel
+
+					/* Get satellite information from the buffer */
+					memcpy(_rx_buffer_part2, &(_rx_buffer[length_part1 + i * length_part2]), length_part2);
+					packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) _rx_buffer_part2;
+
+
+					/* Write satellite information in the global storage */
+					_gps_position->satellite_prn[i] = packet_part2->svid;
+
+					//if satellite information is healthy store the data
+					uint8_t unhealthy = packet_part2->flags & 1 << 4; //flags is a bitfield
+
+					if (!unhealthy) {
+						if ((packet_part2->flags) & 1) { //flags is a bitfield
+							_gps_position->satellite_used[i] = 1;
+							satellites_used++;
+
+						} else {
+							_gps_position->satellite_used[i] = 0;
+						}
+
+						_gps_position->satellite_snr[i] = packet_part2->cno;
+						_gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev);
+						_gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f);
+
+					} else {
+						_gps_position->satellite_used[i] = 0;
+						_gps_position->satellite_snr[i] = 0;
+						_gps_position->satellite_elevation[i] = 0;
+						_gps_position->satellite_azimuth[i] = 0;
+					}
+
+				}
+
+				for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused
+					/* Unused channels have to be set to zero for e.g. MAVLink */
+					_gps_position->satellite_prn[i] = 0;
+					_gps_position->satellite_used[i] = 0;
+					_gps_position->satellite_snr[i] = 0;
+					_gps_position->satellite_elevation[i] = 0;
+					_gps_position->satellite_azimuth[i] = 0;
+				}
+				_gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones
+
+				/* set timestamp if any sat info is available */
+				if (packet_part1->numCh > 0) {
+					_gps_position->satellite_info_available = true;
+				} else {
+					_gps_position->satellite_info_available = false;
+				}
+				_gps_position->timestamp_satellites = hrt_absolute_time();
+			}
+
+			break;
+		}
+
+		case NAV_VELNED: {
+
+			if (!_waiting_for_ack) {
+				/* 35.15 NAV-VELNED (0x01 0x12) message (page 181 / 210 of reference manual */
+				gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer;
+
+				_gps_position->vel_m_s   = (float)packet->speed * 1e-2f;
+				_gps_position->vel_n_m_s = (float)packet->velN * 1e-2f; /* NED NORTH velocity */
+				_gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; /* NED EAST velocity */
+				_gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; /* NED DOWN velocity */
+				_gps_position->cog_rad   = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f;
+				_gps_position->c_variance_rad = (float)packet->cAcc * M_DEG_TO_RAD_F * 1e-5f;
+				_gps_position->vel_ned_valid = true;
+				_gps_position->timestamp_velocity = hrt_absolute_time();
+			}
+
+			break;
+		}
+
+//		case RXM_SVSI: {
+//				printf("GOT RXM_SVSI MESSAGE\n");
+//				const int length_part1 = 7;
+//				char _rx_buffer_part1[length_part1];
+//				memcpy(_rx_buffer_part1, _rx_buffer, length_part1);
+//				gps_bin_rxm_svsi_packet_t *packet = (gps_bin_rxm_svsi_packet_t *) _rx_buffer_part1;
+//
+//				_gps_position->satellites_visible = packet->numVis;
+//				_gps_position->counter++;
+//				_last_message_timestamps[RXM_SVSI - 1] = hrt_absolute_time();
+//
+//			break;
+//		}
+		case ACK_ACK: {
+//			printf("GOT ACK_ACK\n");
+			gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) _rx_buffer;
+
+			if (_waiting_for_ack) {
+				if (packet->clsID == _clsID_needed && packet->msgID == _msgID_needed) {
+					ret = 1;
+				}
+			}
+		}
+		break;
+
+		case ACK_NAK: {
+//			printf("GOT ACK_NAK\n");
+			warnx("UBX: Received: Not Acknowledged");
+			/* configuration obviously not successful */
+			ret = -1;
+			break;
+		}
+
+		default: //we don't know the message
+			warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id);
+			ret = -1;
+			break;
+		}
+	// end if _rx_count high enough
+	decode_init();
+	return ret; //XXX?
+}
+
+void
+UBX::decode_init(void)
+{
+	_rx_ck_a = 0;
+	_rx_ck_b = 0;
+	_rx_count = 0;
+	_decode_state = UBX_DECODE_UNINIT;
+	_message_class = CLASS_UNKNOWN;
+	_message_id = ID_UNKNOWN;
+	_payload_size = 0;
+}
+
+void
+UBX::add_byte_to_checksum(uint8_t b)
+{
+	_rx_ck_a = _rx_ck_a + b;
+	_rx_ck_b = _rx_ck_b + _rx_ck_a;
+}
+
+void
+UBX::add_checksum_to_message(uint8_t* message, const unsigned length)
+{
+	uint8_t ck_a = 0;
+	uint8_t ck_b = 0;
+	unsigned i;
+
+	for (i = 0; i < length-2; i++) {
+		ck_a = ck_a + message[i];
+		ck_b = ck_b + ck_a;
+	}
+	/* The checksum is written to the last to bytes of a message */
+	message[length-2] = ck_a;
+	message[length-1] = ck_b;
+}
+
+void
+UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length)
+{
+	ssize_t ret = 0;
+
+	/* Calculate the checksum now */
+	add_checksum_to_message(packet, length);
+
+	const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2};
+
+	/* Start with the two sync bytes */
+	ret += write(fd, sync_bytes, sizeof(sync_bytes));
+	ret += write(fd, packet, length);
+
+	if (ret != (int)length + (int)sizeof(sync_bytes)) // XXX is there a neater way to get rid of the unsigned signed warning?
+		warnx("ubx: config write fail");
+}

src/drivers/gps/ubx.h

@@ -0,0 +1,395 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/* @file U-Blox protocol definitions */
+
+#ifndef UBX_H_
+#define UBX_H_
+
+#include "gps_helper.h"
+
+#define UBX_SYNC1 0xB5
+#define UBX_SYNC2 0x62
+
+/* ClassIDs (the ones that are used) */
+#define UBX_CLASS_NAV 0x01
+//#define UBX_CLASS_RXM 0x02
+#define UBX_CLASS_ACK 0x05
+#define UBX_CLASS_CFG 0x06
+
+/* MessageIDs (the ones that are used) */
+#define UBX_MESSAGE_NAV_POSLLH 0x02
+#define UBX_MESSAGE_NAV_SOL 0x06
+#define UBX_MESSAGE_NAV_TIMEUTC 0x21
+//#define UBX_MESSAGE_NAV_DOP 0x04
+#define UBX_MESSAGE_NAV_SVINFO 0x30
+#define UBX_MESSAGE_NAV_VELNED 0x12
+//#define UBX_MESSAGE_RXM_SVSI 0x20
+#define UBX_MESSAGE_ACK_ACK 0x01
+#define UBX_MESSAGE_ACK_NAK 0x00
+#define UBX_MESSAGE_CFG_PRT 0x00
+#define UBX_MESSAGE_CFG_NAV5 0x24
+#define UBX_MESSAGE_CFG_MSG 0x01
+#define UBX_MESSAGE_CFG_RATE 0x08
+
+#define UBX_CFG_PRT_LENGTH 20
+#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01         /**< UART1 */
+#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0     /**< 0b0000100011010000: 8N1 */
+#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400      /**< always choose 38400 as GPS baudrate */
+#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01    /**< UBX in */
+#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01   /**< UBX out */
+
+#define UBX_CFG_RATE_LENGTH 6
+#define UBX_CFG_RATE_PAYLOAD_MEASRATE 200		/**< 200ms for 5Hz */
+#define UBX_CFG_RATE_PAYLOAD_NAVRATE 1			/**< cannot be changed */
+#define UBX_CFG_RATE_PAYLOAD_TIMEREF 0			/**< 0: UTC, 1: GPS time */
+
+
+#define UBX_CFG_NAV5_LENGTH 36
+#define UBX_CFG_NAV5_PAYLOAD_MASK 0x0001		/**< only update dynamic model and fix mode */
+#define UBX_CFG_NAV5_PAYLOAD_DYNMODEL 7			/**< 0: portable, 2: stationary, 3: pedestrian, 4: automotive, 5: sea, 6: airborne <1g, 7: airborne <2g, 8: airborne <4g */
+#define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2          /**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */
+
+#define UBX_CFG_MSG_LENGTH 8
+#define UBX_CFG_MSG_PAYLOAD_RATE1_5HZ 0x01 		/**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
+#define UBX_CFG_MSG_PAYLOAD_RATE1_1HZ 0x05		/**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
+
+#define UBX_MAX_PAYLOAD_LENGTH 500
+
+// ************
+/** the structures of the binary packets */
+#pragma pack(push, 1)
+
+typedef struct {
+	uint32_t time_milliseconds;		/**<  GPS Millisecond Time of Week */
+	int32_t lon;					/**<  Longitude * 1e-7, deg */
+	int32_t lat;					/**<  Latitude * 1e-7, deg */
+	int32_t height;					/**<  Height above Ellipsoid, mm */
+	int32_t height_msl;				/**<  Height above mean sea level, mm */
+	uint32_t hAcc;  				/**< Horizontal Accuracy Estimate, mm */
+	uint32_t vAcc;  				/**< Vertical Accuracy Estimate, mm */
+	uint8_t ck_a;
+	uint8_t ck_b;
+} gps_bin_nav_posllh_packet_t;
+
+typedef struct {
+	uint32_t time_milliseconds; 	/**< GPS Millisecond Time of Week */
+	int32_t time_nanoseconds;		/**< Fractional Nanoseconds remainder of rounded ms above, range -500000 .. 500000 */
+	int16_t week;					/**< GPS week (GPS time) */
+	uint8_t gpsFix;					/**< GPS Fix: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix */
+	uint8_t flags;
+	int32_t ecefX;
+	int32_t ecefY;
+	int32_t ecefZ;
+	uint32_t pAcc;
+	int32_t ecefVX;
+	int32_t ecefVY;
+	int32_t ecefVZ;
+	uint32_t sAcc;
+	uint16_t pDOP;
+	uint8_t reserved1;
+	uint8_t numSV;
+	uint32_t reserved2;
+	uint8_t ck_a;
+	uint8_t ck_b;
+} gps_bin_nav_sol_packet_t;
+
+typedef struct {
+	uint32_t time_milliseconds;		/**< GPS Millisecond Time of Week */
+	uint32_t time_accuracy; 		/**< Time Accuracy Estimate, ns */
+	int32_t time_nanoseconds; 		/**< Nanoseconds of second, range -1e9 .. 1e9 (UTC) */
+	uint16_t year; 					/**< Year, range 1999..2099 (UTC) */
+	uint8_t month; 					/**< Month, range 1..12 (UTC) */
+	uint8_t day; 					/**< Day of Month, range 1..31 (UTC) */
+	uint8_t hour; 					/**< Hour of Day, range 0..23 (UTC) */
+	uint8_t min; 					/**< Minute of Hour, range 0..59 (UTC) */
+	uint8_t sec; 					/**< Seconds of Minute, range 0..59 (UTC) */
+	uint8_t valid_flag; 			/**< Validity Flags (see ubx documentation) */
+	uint8_t ck_a;
+	uint8_t ck_b;
+} gps_bin_nav_timeutc_packet_t;
+
+//typedef struct {
+//	uint32_t time_milliseconds; 	/**<  GPS Millisecond Time of Week */
+//	uint16_t gDOP; 					/**< Geometric DOP (scaling 0.01) */
+//	uint16_t pDOP; 					/**< Position DOP (scaling 0.01) */
+//	uint16_t tDOP; 					/**< Time DOP (scaling 0.01) */
+//	uint16_t vDOP; 					/**< Vertical DOP (scaling 0.01) */
+//	uint16_t hDOP; 					/**< Horizontal DOP (scaling 0.01) */
+//	uint16_t nDOP; 					/**< Northing DOP (scaling 0.01) */
+//	uint16_t eDOP; 					/**< Easting DOP (scaling 0.01) */
+//	uint8_t ck_a;
+//	uint8_t ck_b;
+//} gps_bin_nav_dop_packet_t;
+
+typedef struct {
+	uint32_t time_milliseconds; 	/**<  GPS Millisecond Time of Week */
+	uint8_t numCh; 					/**< Number of channels */
+	uint8_t globalFlags;
+	uint16_t reserved2;
+
+} gps_bin_nav_svinfo_part1_packet_t;
+
+typedef struct {
+	uint8_t chn; 					/**< Channel number, 255 for SVs not assigned to a channel */
+	uint8_t svid; 					/**< Satellite ID */
+	uint8_t flags;
+	uint8_t quality;
+	uint8_t cno; 					/**< Carrier to Noise Ratio (Signal Strength), dbHz */
+	int8_t elev; 					/**< Elevation in integer degrees */
+	int16_t azim; 					/**< Azimuth in integer degrees */
+	int32_t prRes; 					/**< Pseudo range residual in centimetres */
+
+} gps_bin_nav_svinfo_part2_packet_t;
+
+typedef struct {
+	uint8_t ck_a;
+	uint8_t ck_b;
+} gps_bin_nav_svinfo_part3_packet_t;
+
+typedef struct {
+	uint32_t time_milliseconds; // GPS Millisecond Time of Week
+	int32_t velN; //NED north velocity, cm/s
+	int32_t velE; //NED east velocity, cm/s
+	int32_t velD; //NED down velocity, cm/s
+	uint32_t speed; //Speed (3-D), cm/s
+	uint32_t gSpeed; //Ground Speed (2-D), cm/s
+	int32_t heading; //Heading of motion 2-D, deg, scaling: 1e-5
+	uint32_t sAcc; //Speed Accuracy Estimate, cm/s
+	uint32_t cAcc; //Course / Heading Accuracy Estimate, scaling: 1e-5
+	uint8_t ck_a;
+	uint8_t ck_b;
+} gps_bin_nav_velned_packet_t;
+
+//typedef struct {
+//	int32_t time_milliseconds; 		/**< Measurement integer millisecond GPS time of week */
+//	int16_t week; 					/**< Measurement GPS week number */
+//	uint8_t numVis;					/**< Number of visible satellites */
+//
+//	//... rest of package is not used in this implementation
+//
+//} gps_bin_rxm_svsi_packet_t;
+
+typedef struct {
+	uint8_t clsID;
+	uint8_t msgID;
+	uint8_t ck_a;
+	uint8_t ck_b;
+} gps_bin_ack_ack_packet_t;
+
+typedef struct {
+	uint8_t clsID;
+	uint8_t msgID;
+	uint8_t ck_a;
+	uint8_t ck_b;
+} gps_bin_ack_nak_packet_t;
+
+typedef struct {
+	uint8_t clsID;
+	uint8_t msgID;
+	uint16_t length;
+	uint8_t portID;
+	uint8_t res0;
+	uint16_t res1;
+	uint32_t mode;
+	uint32_t baudRate;
+	uint16_t inProtoMask;
+	uint16_t outProtoMask;
+	uint16_t flags;
+	uint16_t pad;
+	uint8_t ck_a;
+	uint8_t ck_b;
+} type_gps_bin_cfg_prt_packet_t;
+
+typedef struct {
+	uint8_t clsID;
+	uint8_t msgID;
+	uint16_t length;
+	uint16_t measRate;
+	uint16_t navRate;
+	uint16_t timeRef;
+	uint8_t ck_a;
+	uint8_t ck_b;
+} type_gps_bin_cfg_rate_packet_t;
+
+typedef struct {
+	uint8_t clsID;
+	uint8_t msgID;
+	uint16_t length;
+	uint16_t mask;
+	uint8_t dynModel;
+	uint8_t fixMode;
+	int32_t fixedAlt;
+	uint32_t fixedAltVar;
+	int8_t minElev;
+	uint8_t drLimit;
+	uint16_t pDop;
+	uint16_t tDop;
+	uint16_t pAcc;
+	uint16_t tAcc;
+	uint8_t staticHoldThresh;
+	uint8_t dgpsTimeOut;
+	uint32_t reserved2;
+	uint32_t reserved3;
+	uint32_t reserved4;
+	uint8_t ck_a;
+	uint8_t ck_b;
+} type_gps_bin_cfg_nav5_packet_t;
+
+typedef struct {
+	uint8_t clsID;
+	uint8_t msgID;
+	uint16_t length;
+	uint8_t msgClass_payload;
+	uint8_t msgID_payload;
+	uint8_t rate[6];
+	uint8_t ck_a;
+	uint8_t ck_b;
+} type_gps_bin_cfg_msg_packet_t;
+
+
+// END the structures of the binary packets
+// ************
+
+typedef enum {
+	UBX_CONFIG_STATE_PRT = 0,
+	UBX_CONFIG_STATE_PRT_NEW_BAUDRATE,
+	UBX_CONFIG_STATE_RATE,
+	UBX_CONFIG_STATE_NAV5,
+	UBX_CONFIG_STATE_MSG_NAV_POSLLH,
+	UBX_CONFIG_STATE_MSG_NAV_TIMEUTC,
+	UBX_CONFIG_STATE_MSG_NAV_DOP,
+	UBX_CONFIG_STATE_MSG_NAV_SVINFO,
+	UBX_CONFIG_STATE_MSG_NAV_SOL,
+	UBX_CONFIG_STATE_MSG_NAV_VELNED,
+//	UBX_CONFIG_STATE_MSG_RXM_SVSI,
+	UBX_CONFIG_STATE_CONFIGURED
+} ubx_config_state_t;
+
+typedef enum {
+	CLASS_UNKNOWN = 0,
+	NAV = 1,
+	RXM = 2,
+	ACK = 3,
+	CFG = 4
+} ubx_message_class_t;
+
+typedef enum {
+	//these numbers do NOT correspond to the message id numbers of the ubx protocol
+	ID_UNKNOWN = 0,
+	NAV_POSLLH,
+	NAV_SOL,
+	NAV_TIMEUTC,
+//	NAV_DOP,
+	NAV_SVINFO,
+	NAV_VELNED,
+//	RXM_SVSI,
+	CFG_NAV5,
+	ACK_ACK,
+	ACK_NAK,
+} ubx_message_id_t;
+
+typedef enum {
+	UBX_DECODE_UNINIT = 0,
+	UBX_DECODE_GOT_SYNC1,
+	UBX_DECODE_GOT_SYNC2,
+	UBX_DECODE_GOT_CLASS,
+	UBX_DECODE_GOT_MESSAGEID,
+	UBX_DECODE_GOT_LENGTH1,
+	UBX_DECODE_GOT_LENGTH2
+} ubx_decode_state_t;
+
+//typedef type_gps_bin_ubx_state gps_bin_ubx_state_t;
+#pragma pack(pop)
+
+#define RECV_BUFFER_SIZE 500 //The NAV-SOL messages really need such a big buffer
+
+class UBX : public GPS_Helper
+{
+public:
+	UBX(const int &fd, struct vehicle_gps_position_s *gps_position);
+	~UBX();
+	int					receive(unsigned timeout);
+	int					configure(unsigned &baudrate);
+
+private:
+
+	/**
+	 * Parse the binary MTK packet
+	 */
+	int					parse_char(uint8_t b);
+
+	/**
+	 * Handle the package once it has arrived
+	 */
+	int					handle_message(void);
+
+	/**
+	 * Reset the parse state machine for a fresh start
+	 */
+	void				decode_init(void);
+
+	/**
+	 * While parsing add every byte (except the sync bytes) to the checksum
+	 */
+	void				add_byte_to_checksum(uint8_t);
+
+	/**
+	 * Add the two checksum bytes to an outgoing message
+	 */
+	void				add_checksum_to_message(uint8_t* message, const unsigned length);
+
+	/**
+	 * Helper to send a config packet
+	 */
+	void				send_config_packet(const int &fd, uint8_t *packet, const unsigned length);
+
+	int 				_fd;
+	struct vehicle_gps_position_s *_gps_position;
+	ubx_config_state_t	_config_state;
+	bool 				_waiting_for_ack;
+	uint8_t				_clsID_needed;
+	uint8_t				_msgID_needed;
+	ubx_decode_state_t	_decode_state;
+	uint8_t				_rx_buffer[RECV_BUFFER_SIZE];
+	unsigned			_rx_count;
+	uint8_t 			_rx_ck_a;
+	uint8_t				_rx_ck_b;
+	ubx_message_class_t _message_class;
+	ubx_message_id_t	_message_id;
+	unsigned			_payload_size;
+};
+
+#endif /* UBX_H_ */

src/drivers/hil/hil.cpp

@@ -0,0 +1,851 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file hil.cpp
+ *
+ * Driver/configurator for the virtual HIL port.
+ *
+ * This virtual driver emulates PWM / servo outputs for setups where
+ * the connected hardware does not provide enough or no PWM outputs.
+ *
+ * Its only function is to take actuator_control uORB messages,
+ * mix them with any loaded mixer and output the result to the
+ * actuator_output uORB topic. HIL can also be performed with normal
+ * PWM outputs, a special flag prevents the outputs to be operated
+ * during HIL mode. If HIL is not performed with a standalone FMU,
+ * but in a real system, it is NOT recommended to use this virtual
+ * driver. Use instead the normal FMU or IO driver.
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+
+#include <nuttx/arch.h>
+
+#include <drivers/device/device.h>
+#include <drivers/drv_pwm_output.h>
+#include <drivers/drv_gpio.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_mixer.h>
+
+#include <systemlib/systemlib.h>
+#include <systemlib/mixer/mixer.h>
+
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_outputs.h>
+
+#include <systemlib/err.h>
+
+class HIL : public device::CDev
+{
+public:
+	enum Mode {
+		MODE_2PWM,
+		MODE_4PWM,
+        MODE_8PWM,
+        MODE_12PWM,
+        MODE_16PWM,
+		MODE_NONE
+	};
+	HIL();
+	virtual ~HIL();
+
+	virtual int	ioctl(file *filp, int cmd, unsigned long arg);
+
+	virtual int	init();
+
+	int		set_mode(Mode mode);
+	int		set_pwm_rate(unsigned rate);
+
+private:
+	static const unsigned _max_actuators = 4;
+
+	Mode		_mode;
+	int 		_update_rate;
+	int 		_current_update_rate;
+	int		_task;
+	int		_t_actuators;
+	int		_t_armed;
+	orb_advert_t	_t_outputs;
+	unsigned	_num_outputs;
+	bool		_primary_pwm_device;
+
+	volatile bool	_task_should_exit;
+	bool		_armed;
+
+	MixerGroup	*_mixers;
+
+	actuator_controls_s _controls;
+
+	static void	task_main_trampoline(int argc, char *argv[]);
+	void		task_main() __attribute__((noreturn));
+
+	static int	control_callback(uintptr_t handle,
+			uint8_t control_group,
+			uint8_t control_index,
+			float &input);
+
+	int		pwm_ioctl(file *filp, int cmd, unsigned long arg);
+
+	struct GPIOConfig {
+		uint32_t	input;
+		uint32_t	output;
+		uint32_t	alt;
+	};
+
+	static const GPIOConfig	_gpio_tab[];
+	static const unsigned	_ngpio;
+
+	void		gpio_reset(void);
+	void		gpio_set_function(uint32_t gpios, int function);
+	void		gpio_write(uint32_t gpios, int function);
+	uint32_t	gpio_read(void);
+	int		gpio_ioctl(file *filp, int cmd, unsigned long arg);
+
+};
+
+namespace
+{
+
+HIL	*g_hil;
+
+} // namespace
+
+HIL::HIL() :
+	CDev("hilservo", PWM_OUTPUT_DEVICE_PATH/*"/dev/hil" XXXL*/),
+	_mode(MODE_NONE),
+	_update_rate(50),
+	_current_update_rate(0),
+	_task(-1),
+	_t_actuators(-1),
+	_t_armed(-1),
+	_t_outputs(0),
+	_num_outputs(0),
+	_primary_pwm_device(false),
+	_task_should_exit(false),
+	_armed(false),
+	_mixers(nullptr)
+{
+	_debug_enabled = true;
+}
+
+HIL::~HIL()
+{
+	if (_task != -1) {
+		/* tell the task we want it to go away */
+		_task_should_exit = true;
+
+		unsigned i = 10;
+		do {
+			/* wait 50ms - it should wake every 100ms or so worst-case */
+			usleep(50000);
+
+			/* if we have given up, kill it */
+			if (--i == 0) {
+				task_delete(_task);
+				break;
+			}
+
+		} while (_task != -1);
+	}
+
+	/* clean up the alternate device node */
+	if (_primary_pwm_device)
+		unregister_driver(PWM_OUTPUT_DEVICE_PATH);
+
+	g_hil = nullptr;
+}
+
+int
+HIL::init()
+{
+	int ret;
+
+	ASSERT(_task == -1);
+
+	/* do regular cdev init */
+	ret = CDev::init();
+
+	if (ret != OK)
+		return ret;
+
+	// XXX already claimed with CDEV
+	///* try to claim the generic PWM output device node as well - it's OK if we fail at this */
+	//ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
+	if (ret == OK) {
+		log("default PWM output device");
+		_primary_pwm_device = true;
+	}
+
+	/* reset GPIOs */
+	// gpio_reset();
+
+	/* start the HIL interface task */
+	_task = task_spawn("fmuhil",
+			   SCHED_DEFAULT,
+			   SCHED_PRIORITY_DEFAULT,
+			   2048,
+			   (main_t)&HIL::task_main_trampoline,
+			   nullptr);
+
+	if (_task < 0) {
+		debug("task start failed: %d", errno);
+		return -errno;
+	}
+
+	return OK;
+}
+
+void
+HIL::task_main_trampoline(int argc, char *argv[])
+{
+	g_hil->task_main();
+}
+
+int
+HIL::set_mode(Mode mode)
+{
+	/* 
+	 * Configure for PWM output.
+	 *
+	 * Note that regardless of the configured mode, the task is always
+	 * listening and mixing; the mode just selects which of the channels
+	 * are presented on the output pins.
+	 */
+	switch (mode) {
+	case MODE_2PWM:
+		debug("MODE_2PWM");
+		/* multi-port with flow control lines as PWM */
+		_update_rate = 50;	/* default output rate */
+		break;
+
+	case MODE_4PWM:
+		debug("MODE_4PWM");
+		/* multi-port as 4 PWM outs */
+		_update_rate = 50;	/* default output rate */
+		break;
+            
+    	case MODE_8PWM:
+            debug("MODE_8PWM");
+            /* multi-port as 8 PWM outs */
+            _update_rate = 50;	/* default output rate */
+            break;
+            
+        case MODE_12PWM:
+            debug("MODE_12PWM");
+            /* multi-port as 12 PWM outs */
+            _update_rate = 50;	/* default output rate */
+            break;
+            
+        case MODE_16PWM:
+            debug("MODE_16PWM");
+            /* multi-port as 16 PWM outs */
+            _update_rate = 50;	/* default output rate */
+            break;
+
+	case MODE_NONE:
+		debug("MODE_NONE");
+		/* disable servo outputs and set a very low update rate */
+		_update_rate = 10;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+	_mode = mode;
+	return OK;
+}
+
+int
+HIL::set_pwm_rate(unsigned rate)
+{
+	if ((rate > 500) || (rate < 10))
+		return -EINVAL;
+
+	_update_rate = rate;
+	return OK;
+}
+
+void
+HIL::task_main()
+{
+	/*
+	 * Subscribe to the appropriate PWM output topic based on whether we are the
+	 * primary PWM output or not.
+	 */
+	_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
+				     ORB_ID(actuator_controls_1));
+	/* force a reset of the update rate */
+	_current_update_rate = 0;
+
+	_t_armed = orb_subscribe(ORB_ID(actuator_armed));
+	orb_set_interval(_t_armed, 200);		/* 5Hz update rate */
+
+	/* advertise the mixed control outputs */
+	actuator_outputs_s outputs;
+	memset(&outputs, 0, sizeof(outputs));
+	/* advertise the mixed control outputs */
+	_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
+				   &outputs);
+
+	pollfd fds[2];
+	fds[0].fd = _t_actuators;
+	fds[0].events = POLLIN;
+	fds[1].fd = _t_armed;
+	fds[1].events = POLLIN;
+
+	unsigned num_outputs;
+
+	/* select the number of virtual outputs */
+	switch (_mode) {
+	case MODE_2PWM:
+		num_outputs = 2;
+		break;
+
+	case MODE_4PWM:
+		num_outputs = 4;
+		break;
+
+	case MODE_8PWM:
+	case MODE_12PWM:
+	case MODE_16PWM:
+		// XXX only support the lower 8 - trivial to extend
+		num_outputs = 8;
+		break;
+
+	case MODE_NONE:
+	default:
+		num_outputs = 0;
+		break;
+	}
+
+	log("starting");
+
+	/* loop until killed */
+	while (!_task_should_exit) {
+
+		/* 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)
+				update_rate_in_ms = 2;
+			orb_set_interval(_t_actuators, update_rate_in_ms);
+			// up_pwm_servo_set_rate(_update_rate);
+			_current_update_rate = _update_rate;
+		}
+
+		/* sleep waiting for data, but no more than a second */
+		int ret = ::poll(&fds[0], 2, 1000);
+
+		/* this would be bad... */
+		if (ret < 0) {
+			log("poll error %d", errno);
+			continue;
+		}
+
+		/* do we have a control update? */
+		if (fds[0].revents & POLLIN) {
+
+			/* get controls - must always do this to avoid spinning */
+			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
+
+			/* can we mix? */
+			if (_mixers != nullptr) {
+
+				/* do mixing */
+				outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
+				outputs.timestamp = hrt_absolute_time();
+
+				/* iterate actuators */
+				for (unsigned i = 0; i < num_outputs; i++) {
+
+					/* last resort: catch NaN, INF and out-of-band errors */
+					if (i < (unsigned)outputs.noutputs &&
+						isfinite(outputs.output[i]) &&
+						outputs.output[i] >= -1.0f &&
+						outputs.output[i] <= 1.0f) {
+						/* scale for PWM output 900 - 2100us */
+						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 publish for anyone that cares to see */
+				orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &outputs);
+			}
+		}
+
+		/* how about an arming update? */
+		if (fds[1].revents & POLLIN) {
+			actuator_armed_s aa;
+
+			/* get new value */
+			orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
+		}
+	}
+
+	::close(_t_actuators);
+	::close(_t_armed);
+
+	/* make sure servos are off */
+	// up_pwm_servo_deinit();
+
+	log("stopping");
+
+	/* note - someone else is responsible for restoring the GPIO config */
+
+	/* tell the dtor that we are exiting */
+	_task = -1;
+	_exit(0);
+}
+
+int
+HIL::control_callback(uintptr_t handle,
+				      uint8_t control_group,
+				      uint8_t control_index,
+				      float &input)
+{
+	const actuator_controls_s *controls = (actuator_controls_s *)handle;
+
+	input = controls->control[control_index];
+	return 0;
+}
+
+int
+HIL::ioctl(file *filp, int cmd, unsigned long arg)
+{
+	int ret;
+
+	debug("ioctl 0x%04x 0x%08x", cmd, arg);
+
+	// /* try it as a GPIO ioctl first */
+	// ret = HIL::gpio_ioctl(filp, cmd, arg);
+	// if (ret != -ENOTTY)
+	// 	return ret;
+
+	/* if we are in valid PWM mode, try it as a PWM ioctl as well */
+	switch(_mode) {
+	case MODE_2PWM:
+	case MODE_4PWM:
+        case MODE_8PWM:
+        case MODE_12PWM:
+        case MODE_16PWM:
+		ret = HIL::pwm_ioctl(filp, cmd, arg);
+		break;
+	default:
+		ret = -ENOTTY;
+		debug("not in a PWM mode");
+		break;
+	}
+
+	/* if nobody wants it, let CDev have it */
+	if (ret == -ENOTTY)
+		ret = CDev::ioctl(filp, cmd, arg);
+
+	return ret;
+}
+
+int
+HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
+{
+	int ret = OK;
+	// int channel;
+
+	lock();
+
+	switch (cmd) {
+	case PWM_SERVO_ARM:
+		// up_pwm_servo_arm(true);
+		break;
+
+	case PWM_SERVO_DISARM:
+		// up_pwm_servo_arm(false);
+		break;
+
+	case PWM_SERVO_SET_UPDATE_RATE:
+		// HIL always outputs at the alternate (usually faster) rate 
+		g_hil->set_pwm_rate(arg);
+		break;
+
+	case PWM_SERVO_SELECT_UPDATE_RATE:
+		// HIL always outputs at the alternate (usually faster) rate 
+		break;
+
+	case PWM_SERVO_SET(2):
+	case PWM_SERVO_SET(3):
+		if (_mode != MODE_4PWM) {
+			ret = -EINVAL;
+			break;
+		}
+
+		/* FALLTHROUGH */
+	case PWM_SERVO_SET(0):
+	case PWM_SERVO_SET(1):
+		if (arg < 2100) {
+			// channel = cmd - PWM_SERVO_SET(0);
+//			up_pwm_servo_set(channel, arg); XXX
+
+		} else {
+			ret = -EINVAL;
+		}
+
+		break;
+
+	case PWM_SERVO_GET(2):
+	case PWM_SERVO_GET(3):
+		if (_mode != MODE_4PWM) {
+			ret = -EINVAL;
+			break;
+		}
+
+		/* FALLTHROUGH */
+	case PWM_SERVO_GET(0):
+	case PWM_SERVO_GET(1): {
+			// channel = cmd - PWM_SERVO_SET(0);
+			// *(servo_position_t *)arg = up_pwm_servo_get(channel);
+			break;
+		}
+
+	case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): {
+		// no restrictions on output grouping
+		unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0);
+
+		*(uint32_t *)arg = (1 << channel);
+		break;
+	}
+
+	case MIXERIOCGETOUTPUTCOUNT:
+		if (_mode == MODE_4PWM) {
+			*(unsigned *)arg = 4;
+
+		} else {
+			*(unsigned *)arg = 2;
+		}
+
+		break;
+
+	case MIXERIOCRESET:
+		if (_mixers != nullptr) {
+			delete _mixers;
+			_mixers = nullptr;
+		}
+
+		break;
+
+	case MIXERIOCADDSIMPLE: {
+			mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
+
+			SimpleMixer *mixer = new SimpleMixer(control_callback,
+							     (uintptr_t)&_controls, mixinfo);
+
+			if (mixer->check()) {
+				delete mixer;
+				ret = -EINVAL;
+
+			} else {
+				if (_mixers == nullptr)
+					_mixers = new MixerGroup(control_callback,
+								 (uintptr_t)&_controls);
+
+				_mixers->add_mixer(mixer);
+			}
+
+			break;
+		}
+
+	case MIXERIOCLOADBUF: {
+			const char *buf = (const char *)arg;
+			unsigned buflen = strnlen(buf, 1024);
+
+			if (_mixers == nullptr)
+				_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
+
+			if (_mixers == nullptr) {
+				ret = -ENOMEM;
+
+			} else {
+
+				ret = _mixers->load_from_buf(buf, buflen);
+
+				if (ret != 0) {
+					debug("mixer load failed with %d", ret);
+					delete _mixers;
+					_mixers = nullptr;
+					ret = -EINVAL;
+				}
+			}
+			break;
+		}
+
+
+	default:
+		ret = -ENOTTY;
+		break;
+	}
+
+	unlock();
+
+	return ret;
+}
+
+namespace
+{
+
+enum PortMode {
+	PORT_MODE_UNDEFINED = 0,
+	PORT1_MODE_UNSET,
+	PORT1_FULL_PWM,
+	PORT1_PWM_AND_SERIAL,
+	PORT1_PWM_AND_GPIO,
+	PORT2_MODE_UNSET,
+	PORT2_8PWM,
+	PORT2_12PWM,
+	PORT2_16PWM,
+};
+
+PortMode g_port_mode;
+
+int
+hil_new_mode(PortMode new_mode)
+{
+	// uint32_t gpio_bits;
+	
+
+//	/* reset to all-inputs */
+//	g_hil->ioctl(0, GPIO_RESET, 0);
+
+	// gpio_bits = 0;
+
+	HIL::Mode servo_mode = HIL::MODE_NONE;
+
+	switch (new_mode) {
+	case PORT_MODE_UNDEFINED:
+	case PORT1_MODE_UNSET:
+    	case PORT2_MODE_UNSET:
+            /* nothing more to do here */
+            break;
+
+	case PORT1_FULL_PWM:
+		/* select 4-pin PWM mode */
+		servo_mode = HIL::MODE_4PWM;
+		break;
+
+	case PORT1_PWM_AND_SERIAL:
+		/* select 2-pin PWM mode */
+		servo_mode = HIL::MODE_2PWM;
+//		/* set RX/TX multi-GPIOs to serial mode */
+//		gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4;
+		break;
+
+	case PORT1_PWM_AND_GPIO:
+		/* select 2-pin PWM mode */
+		servo_mode = HIL::MODE_2PWM;
+		break;
+            
+        case PORT2_8PWM:
+            /* select 8-pin PWM mode */
+            servo_mode = HIL::MODE_8PWM;
+            break;
+            
+        case PORT2_12PWM:
+            /* select 12-pin PWM mode */
+            servo_mode = HIL::MODE_12PWM;
+            break;
+            
+        case PORT2_16PWM:
+            /* select 16-pin PWM mode */
+            servo_mode = HIL::MODE_16PWM;
+            break;
+	}
+
+//	/* adjust GPIO config for serial mode(s) */
+//	if (gpio_bits != 0)
+//		g_hil->ioctl(0, GPIO_SET_ALT_1, gpio_bits);
+
+	/* (re)set the PWM output mode */
+	g_hil->set_mode(servo_mode);
+
+	return OK;
+}
+
+int
+hil_start(void)
+{
+	int ret = OK;
+
+	if (g_hil == nullptr) {
+
+		g_hil = new HIL;
+
+		if (g_hil == nullptr) {
+			ret = -ENOMEM;
+
+		} else {
+			ret = g_hil->init();
+
+			if (ret != OK) {
+				delete g_hil;
+				g_hil = nullptr;
+			}
+		}
+	}
+
+	return ret;
+}
+
+void
+test(void)
+{
+	int	fd;
+
+	fd = open(PWM_OUTPUT_DEVICE_PATH, 0);
+
+	if (fd < 0) {
+		puts("open fail");
+		exit(1);
+	}
+
+	ioctl(fd, PWM_SERVO_ARM, 0);
+	ioctl(fd, PWM_SERVO_SET(0), 1000);
+
+	close(fd);
+
+	exit(0);
+}
+
+void
+fake(int argc, char *argv[])
+{
+	if (argc < 5) {
+		puts("hil fake <roll> <pitch> <yaw> <thrust> (values -100 .. 100)");
+		exit(1);
+	}
+
+	actuator_controls_s ac;
+
+	ac.control[0] = strtol(argv[1], 0, 0) / 100.0f;
+
+	ac.control[1] = strtol(argv[2], 0, 0) / 100.0f;
+
+	ac.control[2] = strtol(argv[3], 0, 0) / 100.0f;
+
+	ac.control[3] = strtol(argv[4], 0, 0) / 100.0f;
+
+	orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac);
+
+	if (handle < 0) {
+		puts("advertise failed");
+		exit(1);
+	}
+
+	exit(0);
+}
+
+} // namespace
+
+extern "C" __EXPORT int hil_main(int argc, char *argv[]);
+
+int
+hil_main(int argc, char *argv[])
+{
+	PortMode new_mode = PORT_MODE_UNDEFINED;
+	const char *verb = argv[1];
+
+	if (hil_start() != OK)
+		errx(1, "failed to start the HIL driver");
+
+	/*
+	 * Mode switches.
+	 */
+
+ 	// this was all cut-and-pasted from the FMU driver; it's junk
+	if (!strcmp(verb, "mode_pwm")) {
+		new_mode = PORT1_FULL_PWM;
+
+	} else if (!strcmp(verb, "mode_pwm_serial")) {
+		new_mode = PORT1_PWM_AND_SERIAL;
+
+	} else if (!strcmp(verb, "mode_pwm_gpio")) {
+		new_mode = PORT1_PWM_AND_GPIO;
+
+	} else if (!strcmp(verb, "mode_port2_pwm8")) {
+		new_mode = PORT2_8PWM;
+
+	} else if (!strcmp(verb, "mode_port2_pwm12")) {
+		new_mode = PORT2_8PWM;
+
+	} else if (!strcmp(verb, "mode_port2_pwm16")) {
+		new_mode = PORT2_8PWM;
+	}
+
+	/* was a new mode set? */
+	if (new_mode != PORT_MODE_UNDEFINED) {
+
+		/* yes but it's the same mode */
+		if (new_mode == g_port_mode)
+			return OK;
+
+		/* switch modes */
+		return hil_new_mode(new_mode);
+	}
+
+	if (!strcmp(verb, "test"))
+		test();
+
+	if (!strcmp(verb, "fake"))
+		fake(argc - 1, argv + 1);
+
+
+	fprintf(stderr, "HIL: unrecognized command, try:\n");
+	fprintf(stderr, "  mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16\n");
+	return -EINVAL;
+}

src/drivers/hil/module.mk

@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Hardware in the Loop (HIL) simulation actuator output bank
+#
+
+MODULE_COMMAND	= hil
+
+SRCS		= hil.cpp

src/drivers/hmc5883/module.mk

@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# HMC5883 driver
+#
+
+MODULE_COMMAND	= hmc5883
+
+# XXX seems excessive, check if 2048 is sufficient
+MODULE_STACKSIZE	= 4096
+
+SRCS		= hmc5883.cpp

src/drivers/hott_telemetry/hott_telemetry_main.c

@@ -0,0 +1,306 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: Simon Wilks <sjwilks@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file hott_telemetry_main.c
+ * @author Simon Wilks <sjwilks@gmail.com>
+ *
+ * Graupner HoTT Telemetry implementation.
+ *
+ * The HoTT receiver polls each device at a regular interval at which point
+ * a data packet can be returned if necessary.
+ *
+ * TODO: Add support for at least the vario and GPS sensor data.
+ */
+
+#include <fcntl.h>
+#include <nuttx/config.h>
+#include <poll.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <termios.h>
+#include <sys/ioctl.h>
+#include <unistd.h>
+#include <systemlib/systemlib.h>
+
+#include "messages.h"
+
+static int thread_should_exit = false;		/**< Deamon exit flag */
+static int thread_running = false;		/**< Deamon status flag */
+static int deamon_task;				/**< Handle of deamon task / thread */
+static char *daemon_name = "hott_telemetry";
+static char *commandline_usage = "usage: hott_telemetry start|status|stop [-d <device>]";
+
+
+/* A little console messaging experiment - console helper macro */
+#define FATAL_MSG(_msg)		fprintf(stderr, "[%s] %s\n", daemon_name, _msg); exit(1);
+#define ERROR_MSG(_msg)		fprintf(stderr, "[%s] %s\n", daemon_name, _msg);
+#define INFO_MSG(_msg)		printf("[%s] %s\n", daemon_name, _msg);
+/**
+ * Deamon management function.
+ */
+__EXPORT int hott_telemetry_main(int argc, char *argv[]);
+
+/**
+ * Mainloop of deamon.
+ */
+int hott_telemetry_thread_main(int argc, char *argv[]);
+
+static int read_data(int uart, int *id);
+static int send_data(int uart, uint8_t *buffer, int size);
+
+static int open_uart(const char *device, struct termios *uart_config_original)
+{
+	/* baud rate */
+	int speed = B19200;
+	int uart;
+
+	/* open uart */
+	uart = open(device, O_RDWR | O_NOCTTY);
+
+	if (uart < 0) {
+		char msg[80];
+		sprintf(msg, "Error opening port: %s\n", device);
+		FATAL_MSG(msg);
+	}
+
+	/* Try to set baud rate */
+	struct termios uart_config;
+	int termios_state;
+
+	/* Back up the original uart configuration to restore it after exit */
+	char msg[80];
+
+	if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
+		sprintf(msg, "Error getting baudrate / termios config for %s: %d\n", device, termios_state);
+		close(uart);
+		FATAL_MSG(msg);
+	}
+
+	/* Fill the struct for the new configuration */
+	tcgetattr(uart, &uart_config);
+
+	/* Clear ONLCR flag (which appends a CR for every LF) */
+	uart_config.c_oflag &= ~ONLCR;
+
+	/* Set baud rate */
+	if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
+		sprintf(msg, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n",
+			device, termios_state);
+		close(uart);
+		FATAL_MSG(msg);
+	}
+
+	if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
+		sprintf(msg, "Error setting baudrate / termios config for %s (tcsetattr)\n", device);
+		close(uart);
+		FATAL_MSG(msg);
+	}
+
+	/* Activate single wire mode */
+	ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
+
+	return uart;
+}
+
+int read_data(int uart, int *id)
+{
+	const int timeout = 1000;
+	struct pollfd fds[] = { { .fd = uart, .events = POLLIN } };
+
+	char mode;
+
+	if (poll(fds, 1, timeout) > 0) {
+		/* Get the mode: binary or text  */
+		read(uart, &mode, 1);
+		/* Read the device ID being polled */
+		read(uart, id, 1);
+
+		/* if we have a binary mode request */
+		if (mode != BINARY_MODE_REQUEST_ID) {
+			return ERROR;
+		}
+
+	} else {
+		ERROR_MSG("UART timeout on TX/RX port");
+		return ERROR;
+	}
+
+	return OK;
+}
+
+int send_data(int uart, uint8_t *buffer, int size)
+{
+	usleep(POST_READ_DELAY_IN_USECS);
+
+	uint16_t checksum = 0;
+
+	for (int i = 0; i < size; i++) {
+		if (i == size - 1) {
+			/* Set the checksum: the first uint8_t is taken as the checksum. */
+			buffer[i] = checksum & 0xff;
+
+		} else {
+			checksum += buffer[i];
+		}
+
+		write(uart, &buffer[i], 1);
+
+		/* Sleep before sending the next byte. */
+		usleep(POST_WRITE_DELAY_IN_USECS);
+	}
+
+	/* A hack the reads out what was written so the next read from the receiver doesn't get it. */
+	/* TODO: Fix this!! */
+	uint8_t dummy[size];
+	read(uart, &dummy, size);
+
+	return OK;
+}
+
+int hott_telemetry_thread_main(int argc, char *argv[])
+{
+	INFO_MSG("starting");
+
+	thread_running = true;
+
+	char *device = "/dev/ttyS1";		/**< Default telemetry port: USART2 */
+
+	/* read commandline arguments */
+	for (int i = 0; i < argc && argv[i]; i++) {
+		if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set
+			if (argc > i + 1) {
+				device = argv[i + 1];
+
+			} else {
+				thread_running = false;
+				ERROR_MSG("missing parameter to -d");
+				ERROR_MSG(commandline_usage);
+				exit(1);
+			}
+		}
+	}
+
+	/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
+	struct termios uart_config_original;
+	int uart = open_uart(device, &uart_config_original);
+
+	if (uart < 0) {
+		ERROR_MSG("Failed opening HoTT UART, exiting.");
+		thread_running = false;
+		exit(ERROR);
+	}
+
+	messages_init();
+
+	uint8_t buffer[MESSAGE_BUFFER_SIZE];
+	int size = 0;
+	int id = 0;
+
+	while (!thread_should_exit) {
+		if (read_data(uart, &id) == OK) {
+			switch (id) {
+			case ELECTRIC_AIR_MODULE:
+				build_eam_response(buffer, &size);
+				break;
+
+			default:
+				continue;	// Not a module we support.
+			}
+
+			send_data(uart, buffer, size);
+		}
+	}
+
+	INFO_MSG("exiting");
+
+	close(uart);
+
+	thread_running = false;
+
+	return 0;
+}
+
+/**
+ * Process command line arguments and tart the daemon.
+ */
+int hott_telemetry_main(int argc, char *argv[])
+{
+	if (argc < 1) {
+		ERROR_MSG("missing command");
+		ERROR_MSG(commandline_usage);
+		exit(1);
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			INFO_MSG("deamon already running");
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		deamon_task = task_spawn("hott_telemetry",
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 40,
+					 2048,
+					 hott_telemetry_thread_main,
+					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			INFO_MSG("daemon is running");
+
+		} else {
+			INFO_MSG("daemon not started");
+		}
+
+		exit(0);
+	}
+
+	ERROR_MSG("unrecognized command");
+	ERROR_MSG(commandline_usage);
+	exit(1);
+}
+
+
+

src/drivers/hott_telemetry/messages.c

@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: Simon Wilks <sjwilks@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file messages.c
+ * @author Simon Wilks <sjwilks@gmail.com>
+ */
+
+#include "messages.h"
+
+#include <string.h>
+#include <systemlib/systemlib.h>
+#include <unistd.h>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/sensor_combined.h>
+
+static int battery_sub = -1;
+static int sensor_sub = -1;
+
+void messages_init(void)
+{
+	battery_sub = orb_subscribe(ORB_ID(battery_status));
+	sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+}
+
+void build_eam_response(uint8_t *buffer, int *size)
+{
+	/* get a local copy of the current sensor values */
+	struct sensor_combined_s raw;
+	memset(&raw, 0, sizeof(raw));
+	orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
+
+	/* get a local copy of the battery data */
+	struct battery_status_s battery;
+	memset(&battery, 0, sizeof(battery));
+	orb_copy(ORB_ID(battery_status), battery_sub, &battery);
+
+	struct eam_module_msg msg;
+	*size = sizeof(msg);
+	memset(&msg, 0, *size);
+
+	msg.start = START_BYTE;
+	msg.eam_sensor_id = ELECTRIC_AIR_MODULE;
+	msg.sensor_id = EAM_SENSOR_ID;
+	msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 20);
+	msg.temperature2 = TEMP_ZERO_CELSIUS;
+	msg.main_voltage_L = (uint8_t)(battery.voltage_v * 10);
+
+	uint16_t alt = (uint16_t)(raw.baro_alt_meter + 500);
+	msg.altitude_L = (uint8_t)alt & 0xff;
+	msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
+
+	msg.stop = STOP_BYTE;
+
+	memcpy(buffer, &msg, *size);
+}

src/drivers/hott_telemetry/messages.h

@@ -0,0 +1,124 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: Simon Wilks <sjwilks@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file messages.h
+ * @author Simon Wilks <sjwilks@gmail.com>
+ *
+ * Graupner HoTT Telemetry message generation.
+ *
+ */
+#ifndef MESSAGES_H_
+#define MESSAGES_H
+
+#include <stdlib.h>
+
+/* The buffer size used to store messages. This must be at least as big as the number of
+ * fields in the largest message struct.
+ */
+#define MESSAGE_BUFFER_SIZE 50
+
+/* The HoTT receiver demands a minimum 5ms period of silence after delivering its request.
+ * Note that the value specified here is lower than 5000 (5ms) as time is lost constucting
+ * the message after the read which takes some milliseconds.
+ */
+#define POST_READ_DELAY_IN_USECS	4000
+/* A pause of 3ms is required between each uint8_t sent back to the HoTT receiver. Much lower
+ * values can be used in practise though.
+ */
+#define POST_WRITE_DELAY_IN_USECS	2000
+
+// Protocol constants.
+#define BINARY_MODE_REQUEST_ID	0x80	// Binary mode request.
+#define START_BYTE		0x7c
+#define STOP_BYTE		0x7d
+#define TEMP_ZERO_CELSIUS	0x14
+
+/* Electric Air Module (EAM) constants. */
+#define ELECTRIC_AIR_MODULE	0x8e
+#define EAM_SENSOR_ID		0xe0
+
+/* The Electric Air Module message. */
+struct eam_module_msg {
+	uint8_t start;			/**< Start byte   				*/
+	uint8_t eam_sensor_id;		/**< EAM sensor ID 				*/
+	uint8_t warning;
+	uint8_t sensor_id;		/**< Sensor ID, why different? 			*/
+	uint8_t alarm_inverse1;
+	uint8_t alarm_inverse2;
+	uint8_t cell1_L;		/**< Lipo cell voltages. Not supported. 	*/
+	uint8_t cell2_L;
+	uint8_t cell3_L;
+	uint8_t cell4_L;
+	uint8_t cell5_L;
+	uint8_t cell6_L;
+	uint8_t cell7_L;
+	uint8_t cell1_H;
+	uint8_t cell2_H;
+	uint8_t cell3_H;
+	uint8_t cell4_H;
+	uint8_t cell5_H;
+	uint8_t cell6_H;
+	uint8_t cell7_H;
+	uint8_t batt1_voltage_L;	/**< Battery 1 voltage, lower 8-bits in steps of 0.02V 	*/
+	uint8_t batt1_voltage_H;
+	uint8_t batt2_voltage_L;	/**< Battery 2 voltage, lower 8-bits in steps of 0.02V 	*/
+	uint8_t batt2_voltage_H;
+	uint8_t temperature1;		/**< Temperature sensor 1. 20 = 0 degrees 		*/
+	uint8_t temperature2;
+	uint8_t altitude_L;		/**< Attitude (meters) lower 8-bits. 500 = 0 meters 	*/
+	uint8_t altitude_H;
+	uint8_t current_L;		/**< Current (mAh) lower 8-bits in steps of 0.1V 	*/
+	uint8_t current_H;
+	uint8_t main_voltage_L;		/**< Main power voltage lower 8-bits in steps of 0.1V 	*/
+	uint8_t main_voltage_H;
+	uint8_t battery_capacity_L;	/**< Used battery capacity in steps of 10mAh 		*/
+	uint8_t battery_capacity_H;
+	uint8_t climbrate_L;		/**< Climb rate in 0.01m/s. 0m/s = 30000 		*/
+	uint8_t climbrate_H;
+	uint8_t climbrate_3s;		/**< Climb rate in m/3sec. 0m/3sec = 120 		*/
+	uint8_t rpm_L;			/**< RPM Lower 8-bits In steps of 10 U/min 		*/
+	uint8_t rpm_H;
+	uint8_t electric_min;		/**< Flight time in minutes. 				*/
+	uint8_t electric_sec;		/**< Flight time in seconds. 				*/
+	uint8_t speed_L;		/**< Airspeed in km/h in steps of 1 km/h 		*/
+	uint8_t speed_H;
+	uint8_t stop;			/**< Stop byte 						*/
+	uint8_t checksum;		/**< Lower 8-bits of all bytes summed. 			*/
+};
+
+void messages_init(void);
+void build_eam_response(uint8_t *buffer, int *size);
+
+#endif /* MESSAGES_H_ */

src/drivers/hott_telemetry/module.mk

@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Graupner HoTT Telemetry application.
+#
+
+MODULE_COMMAND	= hott_telemetry
+
+SRCS		= hott_telemetry_main.c \
+		  messages.c

src/drivers/mb12xx/mb12xx.cpp

@@ -0,0 +1,840 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mb12xx.cpp
+ * @author Greg Hulands
+ *
+ * Driver for the Maxbotix sonar range finders connected via I2C.
+ */
+	 
+#include <nuttx/config.h>
+
+#include <drivers/device/i2c.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/wqueue.h>
+#include <nuttx/clock.h>
+
+#include <arch/board/board.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_range_finder.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/subsystem_info.h>
+
+/* Configuration Constants */
+#define MB12XX_BUS 			PX4_I2C_BUS_EXPANSION
+#define MB12XX_BASEADDR 	0x70 /* 7-bit address. 8-bit address is 0xE0 */
+
+/* MB12xx Registers addresses */
+
+#define MB12XX_TAKE_RANGE_REG	0x51		/* Measure range Register */
+#define MB12XX_SET_ADDRESS_1	0xAA		/* Change address 1 Register */
+#define MB12XX_SET_ADDRESS_2	0xA5		/* Change address 2 Register */
+
+/* Device limits */
+#define MB12XX_MIN_DISTANCE (0.20f)
+#define MB12XX_MAX_DISTANCE (7.65f)
+	 
+#define MB12XX_CONVERSION_INTERVAL 60000 /* 60ms */
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
+class MB12XX : public device::I2C
+{
+public:
+	MB12XX(int bus = MB12XX_BUS, int address = MB12XX_BASEADDR);
+	virtual ~MB12XX();
+	
+	virtual int 		init();
+	
+	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
+	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
+	
+	/**
+	* Diagnostics - print some basic information about the driver.
+	*/
+	void				print_info();
+	
+protected:
+	virtual int			probe();
+
+private:
+	float				_min_distance;
+	float				_max_distance;
+	work_s				_work;
+	unsigned			_num_reports;
+	volatile unsigned	_next_report;
+	volatile unsigned	_oldest_report;
+	range_finder_report	*_reports;
+	bool				_sensor_ok;
+	int					_measure_ticks;
+	bool				_collect_phase;
+	
+	orb_advert_t		_range_finder_topic;
+
+	perf_counter_t		_sample_perf;
+	perf_counter_t		_comms_errors;
+	perf_counter_t		_buffer_overflows;
+	
+	/**
+	* Test whether the device supported by the driver is present at a
+	* specific address.
+	*
+	* @param address	The I2C bus address to probe.
+	* @return		True if the device is present.
+	*/
+	int					probe_address(uint8_t address);
+	
+	/**
+	* Initialise the automatic measurement state machine and start it.
+	*
+	* @note This function is called at open and error time.  It might make sense
+	*       to make it more aggressive about resetting the bus in case of errors.
+	*/
+	void				start();
+	
+	/**
+	* Stop the automatic measurement state machine.
+	*/
+	void				stop();
+	
+	/**
+	* Set the min and max distance thresholds if you want the end points of the sensors
+	* range to be brought in at all, otherwise it will use the defaults MB12XX_MIN_DISTANCE
+	* and MB12XX_MAX_DISTANCE
+	*/
+	void				set_minimum_distance(float min);
+	void				set_maximum_distance(float max);
+	float				get_minimum_distance();
+	float				get_maximum_distance();
+	
+	/**
+	* Perform a poll cycle; collect from the previous measurement
+	* and start a new one.
+	*/
+	void				cycle();
+	int					measure();
+	int					collect();
+	/**
+	* Static trampoline from the workq context; because we don't have a
+	* generic workq wrapper yet.
+	*
+	* @param arg		Instance pointer for the driver that is polling.
+	*/
+	static void		cycle_trampoline(void *arg);
+	
+	
+};
+
+/* helper macro for handling report buffer indices */
+#define INCREMENT(_x, _lim)	do { _x++; if (_x >= _lim) _x = 0; } while(0)
+
+/*
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int mb12xx_main(int argc, char *argv[]);
+
+MB12XX::MB12XX(int bus, int address) :
+	I2C("MB12xx", RANGE_FINDER_DEVICE_PATH, bus, address, 100000),
+	_min_distance(MB12XX_MIN_DISTANCE),
+	_max_distance(MB12XX_MAX_DISTANCE),
+	_num_reports(0),
+	_next_report(0),
+	_oldest_report(0),
+	_reports(nullptr),
+	_sensor_ok(false),
+	_measure_ticks(0),
+	_collect_phase(false),
+	_range_finder_topic(-1),
+	_sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")),
+	_comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")),
+	_buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows"))
+{
+	// enable debug() calls
+	_debug_enabled = true;
+	
+	// work_cancel in the dtor will explode if we don't do this...
+	memset(&_work, 0, sizeof(_work));
+}
+
+MB12XX::~MB12XX()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* free any existing reports */
+	if (_reports != nullptr)
+		delete[] _reports;
+}
+
+int
+MB12XX::init()
+{
+	int ret = ERROR;
+
+	/* do I2C init (and probe) first */
+	if (I2C::init() != OK)
+		goto out;
+
+	/* allocate basic report buffers */
+	_num_reports = 2;
+	_reports = new struct range_finder_report[_num_reports];
+
+	if (_reports == nullptr)
+		goto out;
+
+	_oldest_report = _next_report = 0;
+
+	/* get a publish handle on the range finder topic */
+	memset(&_reports[0], 0, sizeof(_reports[0]));
+	_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &_reports[0]);
+
+	if (_range_finder_topic < 0)
+		debug("failed to create sensor_range_finder object. Did you start uOrb?");
+
+	ret = OK;
+	/* sensor is ok, but we don't really know if it is within range */
+	_sensor_ok = true;
+out:
+	return ret;
+}
+
+int
+MB12XX::probe()
+{
+	return measure();
+}
+
+void
+MB12XX::set_minimum_distance(float min)
+{
+	_min_distance = min;
+}	
+
+void
+MB12XX::set_maximum_distance(float max)
+{
+	_max_distance = max;
+}	
+
+float
+MB12XX::get_minimum_distance()
+{
+	return _min_distance;
+}
+
+float
+MB12XX::get_maximum_distance()
+{
+	return _max_distance;
+}
+
+int
+MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+	case SENSORIOCSPOLLRATE: {
+			switch (arg) {
+
+				/* switching to manual polling */
+			case SENSOR_POLLRATE_MANUAL:
+				stop();
+				_measure_ticks = 0;
+				return OK;
+
+				/* external signalling (DRDY) not supported */
+			case SENSOR_POLLRATE_EXTERNAL:
+
+				/* zero would be bad */
+			case 0:
+				return -EINVAL;
+
+				/* set default/max polling rate */
+			case SENSOR_POLLRATE_MAX:
+			case SENSOR_POLLRATE_DEFAULT: {
+					/* do we need to start internal polling? */
+					bool want_start = (_measure_ticks == 0);
+
+					/* set interval for next measurement to minimum legal value */
+					_measure_ticks = USEC2TICK(MB12XX_CONVERSION_INTERVAL);
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start)
+						start();
+
+					return OK;
+				}
+
+				/* adjust to a legal polling interval in Hz */
+			default: {
+					/* do we need to start internal polling? */
+					bool want_start = (_measure_ticks == 0);
+
+					/* convert hz to tick interval via microseconds */
+					unsigned ticks = USEC2TICK(1000000 / arg);
+
+					/* check against maximum rate */
+					if (ticks < USEC2TICK(MB12XX_CONVERSION_INTERVAL))
+						return -EINVAL;
+
+					/* update interval for next measurement */
+					_measure_ticks = ticks;
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start)
+						start();
+
+					return OK;
+				}
+			}
+		}
+
+	case SENSORIOCGPOLLRATE:
+		if (_measure_ticks == 0)
+			return SENSOR_POLLRATE_MANUAL;
+
+		return (1000 / _measure_ticks);
+
+	case SENSORIOCSQUEUEDEPTH: {
+			/* add one to account for the sentinel in the ring */
+			arg++;
+
+			/* lower bound is mandatory, upper bound is a sanity check */
+			if ((arg < 2) || (arg > 100))
+				return -EINVAL;
+
+			/* allocate new buffer */
+			struct range_finder_report *buf = new struct range_finder_report[arg];
+
+			if (nullptr == buf)
+				return -ENOMEM;
+
+			/* reset the measurement state machine with the new buffer, free the old */
+			stop();
+			delete[] _reports;
+			_num_reports = arg;
+			_reports = buf;
+			start();
+
+			return OK;
+		}
+
+	case SENSORIOCGQUEUEDEPTH:
+		return _num_reports - 1;
+		
+	case SENSORIOCRESET:
+		/* XXX implement this */
+		return -EINVAL;
+	
+	case RANGEFINDERIOCSETMINIUMDISTANCE:
+	{
+		set_minimum_distance(*(float *)arg);
+		return 0;
+	}
+	break;
+	case RANGEFINDERIOCSETMAXIUMDISTANCE:
+	{
+		set_maximum_distance(*(float *)arg);
+		return 0;
+	}
+	break;
+	default:
+		/* give it to the superclass */
+		return I2C::ioctl(filp, cmd, arg);
+	}
+}
+
+ssize_t
+MB12XX::read(struct file *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct range_finder_report);
+	int ret = 0;
+
+	/* buffer must be large enough */
+	if (count < 1)
+		return -ENOSPC;
+
+	/* if automatic measurement is enabled */
+	if (_measure_ticks > 0) {
+
+		/*
+		 * While there is space in the caller's buffer, and reports, copy them.
+		 * Note that we may be pre-empted by the workq thread while we are doing this;
+		 * we are careful to avoid racing with them.
+		 */
+		while (count--) {
+			if (_oldest_report != _next_report) {
+				memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
+				ret += sizeof(_reports[0]);
+				INCREMENT(_oldest_report, _num_reports);
+			}
+		}
+
+		/* if there was no data, warn the caller */
+		return ret ? ret : -EAGAIN;
+	}
+
+	/* manual measurement - run one conversion */
+	/* XXX really it'd be nice to lock against other readers here */
+	do {
+		_oldest_report = _next_report = 0;
+
+		/* trigger a measurement */
+		if (OK != measure()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* wait for it to complete */
+		usleep(MB12XX_CONVERSION_INTERVAL);
+
+		/* run the collection phase */
+		if (OK != collect()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* state machine will have generated a report, copy it out */
+		memcpy(buffer, _reports, sizeof(*_reports));
+		ret = sizeof(*_reports);
+
+	} while (0);
+
+	return ret;
+}
+
+int
+MB12XX::measure()
+{
+	int ret;
+
+	/*
+	 * Send the command to begin a measurement.
+	 */
+	uint8_t cmd = MB12XX_TAKE_RANGE_REG;
+	ret = transfer(&cmd, 1, nullptr, 0);
+
+	if (OK != ret)
+	{
+		perf_count(_comms_errors);
+		log("i2c::transfer returned %d", ret);
+		return ret;
+	}
+	ret = OK;
+	
+	return ret;
+}
+
+int
+MB12XX::collect()
+{
+	int	ret = -EIO;
+	
+	/* read from the sensor */
+	uint8_t val[2] = {0, 0};
+	
+	perf_begin(_sample_perf);
+	
+	ret = transfer(nullptr, 0, &val[0], 2);
+	
+	if (ret < 0)
+	{
+		log("error reading from sensor: %d", ret);
+		return ret;
+	}
+	
+	uint16_t distance = val[0] << 8 | val[1];
+	float si_units = (distance * 1.0f)/ 100.0f; /* cm to m */
+	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
+	_reports[_next_report].timestamp = hrt_absolute_time();
+	_reports[_next_report].distance = si_units;
+	_reports[_next_report].valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
+	
+	/* publish it */
+	orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &_reports[_next_report]);
+
+	/* post a report to the ring - note, not locked */
+	INCREMENT(_next_report, _num_reports);
+
+	/* if we are running up against the oldest report, toss it */
+	if (_next_report == _oldest_report) {
+		perf_count(_buffer_overflows);
+		INCREMENT(_oldest_report, _num_reports);
+	}
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	ret = OK;
+
+out:
+	perf_end(_sample_perf);
+	return ret;
+	
+	return ret;
+}
+
+void
+MB12XX::start()
+{
+	/* reset the report ring and state machine */
+	_collect_phase = false;
+	_oldest_report = _next_report = 0;
+
+	/* schedule a cycle to start things */
+	work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 1);
+	
+	/* notify about state change */
+	struct subsystem_info_s info = {
+		true,
+		true,
+		true,
+		SUBSYSTEM_TYPE_RANGEFINDER};
+	static orb_advert_t pub = -1;
+
+	if (pub > 0) {
+		orb_publish(ORB_ID(subsystem_info), pub, &info);
+	} else {
+		pub = orb_advertise(ORB_ID(subsystem_info), &info);
+	}
+}
+
+void
+MB12XX::stop()
+{
+	work_cancel(HPWORK, &_work);
+}
+
+void
+MB12XX::cycle_trampoline(void *arg)
+{
+	MB12XX *dev = (MB12XX *)arg;
+
+	dev->cycle();
+}
+
+void
+MB12XX::cycle()
+{
+	/* collection phase? */
+	if (_collect_phase) {
+
+		/* perform collection */
+		if (OK != collect()) {
+			log("collection error");
+			/* restart the measurement state machine */
+			start();
+			return;
+		}
+
+		/* next phase is measurement */
+		_collect_phase = false;
+
+		/*
+		 * Is there a collect->measure gap?
+		 */
+		if (_measure_ticks > USEC2TICK(MB12XX_CONVERSION_INTERVAL)) {
+
+			/* schedule a fresh cycle call when we are ready to measure again */
+			work_queue(HPWORK,
+				   &_work,
+				   (worker_t)&MB12XX::cycle_trampoline,
+				   this,
+				   _measure_ticks - USEC2TICK(MB12XX_CONVERSION_INTERVAL));
+
+			return;
+		}
+	}
+
+	/* measurement phase */
+	if (OK != measure())
+		log("measure error");
+
+	/* next phase is collection */
+	_collect_phase = true;
+
+	/* schedule a fresh cycle call when the measurement is done */
+	work_queue(HPWORK,
+		   &_work,
+		   (worker_t)&MB12XX::cycle_trampoline,
+		   this,
+		   USEC2TICK(MB12XX_CONVERSION_INTERVAL));
+}
+
+void
+MB12XX::print_info()
+{
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+	perf_print_counter(_buffer_overflows);
+	printf("poll interval:  %u ticks\n", _measure_ticks);
+	printf("report queue:   %u (%u/%u @ %p)\n",
+	       _num_reports, _oldest_report, _next_report, _reports);
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace mb12xx
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+const int ERROR = -1;
+
+MB12XX	*g_dev;
+
+void	start();
+void	stop();
+void	test();
+void	reset();
+void	info();
+
+/**
+ * Start the driver.
+ */
+void
+start()
+{
+	int fd;
+
+	if (g_dev != nullptr)
+		errx(1, "already started");
+
+	/* create the driver */
+	g_dev = new MB12XX(MB12XX_BUS);
+
+	if (g_dev == nullptr)
+		goto fail;
+
+	if (OK != g_dev->init())
+		goto fail;
+
+	/* set the poll rate to default, starts automatic data collection */
+	fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		goto fail;
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+		goto fail;
+
+	exit(0);
+
+fail:
+
+	if (g_dev != nullptr) 
+	{
+		delete g_dev;
+		g_dev = nullptr;
+	}
+
+	errx(1, "driver start failed");
+}
+
+/**
+ * Stop the driver
+ */
+void stop()
+{
+	if (g_dev != nullptr)
+	{
+		delete g_dev;
+		g_dev = nullptr;
+	}
+	else
+	{
+		errx(1, "driver not running");
+	}
+	exit(0);
+}
+
+/**
+ * Perform some basic functional tests on the driver;
+ * make sure we can collect data from the sensor in polled
+ * and automatic modes.
+ */
+void
+test()
+{
+	struct range_finder_report report;
+	ssize_t sz;
+	int ret;
+
+	int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "%s open failed (try 'mb12xx start' if the driver is not running", RANGE_FINDER_DEVICE_PATH);
+
+	/* do a simple demand read */
+	sz = read(fd, &report, sizeof(report));
+
+	if (sz != sizeof(report))
+		err(1, "immediate read failed");
+
+	warnx("single read");
+	warnx("measurement: %0.2f m", (double)report.distance);
+	warnx("time:        %lld", report.timestamp);
+
+	/* start the sensor polling at 2Hz */
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
+		errx(1, "failed to set 2Hz poll rate");
+
+	/* read the sensor 5x and report each value */
+	for (unsigned i = 0; i < 5; i++) {
+		struct pollfd fds;
+
+		/* wait for data to be ready */
+		fds.fd = fd;
+		fds.events = POLLIN;
+		ret = poll(&fds, 1, 2000);
+
+		if (ret != 1)
+			errx(1, "timed out waiting for sensor data");
+
+		/* now go get it */
+		sz = read(fd, &report, sizeof(report));
+
+		if (sz != sizeof(report))
+			err(1, "periodic read failed");
+
+		warnx("periodic read %u", i);
+		warnx("measurement: %0.3f", (double)report.distance);
+		warnx("time:        %lld", report.timestamp);
+	}
+
+	errx(0, "PASS");
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+	int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "failed ");
+
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
+		err(1, "driver reset failed");
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+		err(1, "driver poll restart failed");
+
+	exit(0);
+}
+
+/**
+ * Print a little info about the driver.
+ */
+void
+info()
+{
+	if (g_dev == nullptr)
+		errx(1, "driver not running");
+
+	printf("state @ %p\n", g_dev);
+	g_dev->print_info();
+
+	exit(0);
+}
+
+} // namespace
+
+int
+mb12xx_main(int argc, char *argv[])
+{
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(argv[1], "start"))
+		mb12xx::start();
+	
+	 /*
+	  * Stop the driver
+	  */
+	 if (!strcmp(argv[1], "stop"))
+		 mb12xx::stop();
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test"))
+		mb12xx::test();
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[1], "reset"))
+		mb12xx::reset();
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
+		mb12xx::info();
+
+	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
+}

src/drivers/mb12xx/module.mk

@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Makefile to build the Maxbotix Sonar driver.
+#
+
+MODULE_COMMAND	= mb12xx
+
+SRCS		= mb12xx.cpp

src/drivers/mpu6000/module.mk

@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Makefile to build the MPU6000 driver.
+#
+
+MODULE_COMMAND	= mpu6000
+
+# XXX seems excessive, check if 2048 is not sufficient
+MODULE_STACKSIZE	 = 4096
+
+SRCS		= mpu6000.cpp

src/drivers/ms5611/module.mk

@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# MS5611 driver
+#
+
+MODULE_COMMAND	= ms5611
+
+SRCS		= ms5611.cpp