Merge branch 'master' into mpc_yaw_fix

5 files changed, 373 insertions, 262 deletions

diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp
index ee1d472a2..fedff769b 100644
--- a/src/drivers/rgbled/rgbled.cpp
+++ b/src/drivers/rgbled/rgbled.cpp
@@ -1,6 +1,8 @@
 /****************************************************************************
  *
  *   Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: Julian Oes <joes@student.ethz.ch>
+ *           Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -36,7 +38,6 @@
  *
  * Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C.
  *
- *
  */
 
 #include <nuttx/config.h>
@@ -92,16 +93,14 @@ public:
 private:
 	work_s			_work;
 
-	rgbled_color_t		_color;
 	rgbled_mode_t		_mode;
 	rgbled_pattern_t	_pattern;
 
-	float			_brightness;
 	uint8_t			_r;
 	uint8_t			_g;
 	uint8_t			_b;
+	float			_brightness;
 
-	bool			_should_run;
 	bool			_running;
 	int			_led_interval;
 	int			_counter;
@@ -109,35 +108,33 @@ private:
 	void 			set_color(rgbled_color_t ledcolor);
 	void			set_mode(rgbled_mode_t mode);
 	void			set_pattern(rgbled_pattern_t *pattern);
-	void			set_brightness(float brightness);
 
 	static void		led_trampoline(void *arg);
 	void			led();
 
-	int			set(bool on, uint8_t r, uint8_t g, uint8_t b);
-	int			set_on(bool on);
-	int			set_rgb(uint8_t r, uint8_t g, uint8_t b);
-	int			get(bool &on, bool &not_powersave, uint8_t &r, uint8_t &g, uint8_t &b);
+	int			send_led_enable(bool enable);
+	int			send_led_rgb();
+	int			get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b);
 };
 
 /* for now, we only support one RGBLED */
 namespace
 {
-	RGBLED *g_rgbled;
+RGBLED *g_rgbled;
 }
 
+void rgbled_usage();
 
 extern "C" __EXPORT int rgbled_main(int argc, char *argv[]);
 
 RGBLED::RGBLED(int bus, int rgbled) :
 	I2C("rgbled", RGBLED_DEVICE_PATH, bus, rgbled, 100000),
-	_color(RGBLED_COLOR_OFF),
 	_mode(RGBLED_MODE_OFF),
-	_running(false),
-	_brightness(1.0f),
 	_r(0),
 	_g(0),
 	_b(0),
+	_brightness(1.0f),
+	_running(false),
 	_led_interval(0),
 	_counter(0)
 {
@@ -159,8 +156,9 @@ RGBLED::init()
 		return ret;
 	}
 
-	/* start off */
-	set(false, 0, 0, 0);
+	/* switch off LED on start */
+	send_led_enable(false);
+	send_led_rgb();
 
 	return OK;
 }
@@ -169,10 +167,10 @@ int
 RGBLED::probe()
 {
 	int ret;
-	bool on, not_powersave;
+	bool on, powersave;
 	uint8_t r, g, b;
 
-	ret = get(on, not_powersave, r, g, b);
+	ret = get(on, powersave, r, g, b);
 
 	return ret;
 }
@@ -181,15 +179,16 @@ int
 RGBLED::info()
 {
 	int ret;
-	bool on, not_powersave;
+	bool on, powersave;
 	uint8_t r, g, b;
 
-	ret = get(on, not_powersave, r, g, b);
+	ret = get(on, powersave, r, g, b);
 
 	if (ret == OK) {
 		/* we don't care about power-save mode */
 		log("state: %s", on ? "ON" : "OFF");
 		log("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b);
+
 	} else {
 		warnx("failed to read led");
 	}
@@ -201,28 +200,30 @@ int
 RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg)
 {
 	int ret = ENOTTY;
+
 	switch (cmd) {
 	case RGBLED_SET_RGB:
-		/* set the specified RGB values */
-		rgbled_rgbset_t rgbset;
-		memcpy(&rgbset, (rgbled_rgbset_t*)arg, sizeof(rgbset));
-		set_rgb(rgbset.red, rgbset.green, rgbset.blue);
-		set_mode(RGBLED_MODE_ON);
+		/* set the specified color */
+		_r = ((rgbled_rgbset_t *) arg)->red;
+		_g = ((rgbled_rgbset_t *) arg)->green;
+		_b = ((rgbled_rgbset_t *) arg)->blue;
+		send_led_rgb();
 		return OK;
 
 	case RGBLED_SET_COLOR:
 		/* set the specified color name */
 		set_color((rgbled_color_t)arg);
+		send_led_rgb();
 		return OK;
 
 	case RGBLED_SET_MODE:
-		/* set the specified blink speed */
+		/* set the specified mode */
 		set_mode((rgbled_mode_t)arg);
 		return OK;
 
 	case RGBLED_SET_PATTERN:
 		/* set a special pattern */
-		set_pattern((rgbled_pattern_t*)arg);
+		set_pattern((rgbled_pattern_t *)arg);
 		return OK;
 
 	default:
@@ -241,39 +242,54 @@ RGBLED::led_trampoline(void *arg)
 	rgbl->led();
 }
 
-
-
+/**
+ * Main loop function
+ */
 void
 RGBLED::led()
 {
 	switch (_mode) {
-		case RGBLED_MODE_BLINK_SLOW:
-		case RGBLED_MODE_BLINK_NORMAL:
-		case RGBLED_MODE_BLINK_FAST:
-			if(_counter % 2 == 0)
-				set_on(true);
-			else
-				set_on(false);
-			break;
-		case RGBLED_MODE_BREATHE:
-			if (_counter >= 30)
-				_counter = 0;
-			if (_counter <= 15) {
-				set_brightness(((float)_counter)*((float)_counter)/(15.0f*15.0f));
-			} else {
-				set_brightness(((float)(30-_counter))*((float)(30-_counter))/(15.0f*15.0f));
-			}
-			break;
-		case RGBLED_MODE_PATTERN:
-			/* don't run out of the pattern array and stop if the next frame is 0 */
-			if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0)
-				_counter = 0;
+	case RGBLED_MODE_BLINK_SLOW:
+	case RGBLED_MODE_BLINK_NORMAL:
+	case RGBLED_MODE_BLINK_FAST:
+		if (_counter >= 2)
+			_counter = 0;
 
-			set_color(_pattern.color[_counter]);
-			_led_interval = _pattern.duration[_counter];
-			break;
-		default:
-			break;
+		send_led_enable(_counter == 0);
+
+		break;
+
+	case RGBLED_MODE_BREATHE:
+
+		if (_counter >= 62)
+			_counter = 0;
+
+		int n;
+
+		if (_counter < 32) {
+			n = _counter;
+
+		} else {
+			n = 62 - _counter;
+		}
+
+		_brightness = n * n / (31.0f * 31.0f);
+		send_led_rgb();
+		break;
+
+	case RGBLED_MODE_PATTERN:
+
+		/* don't run out of the pattern array and stop if the next frame is 0 */
+		if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0)
+			_counter = 0;
+
+		set_color(_pattern.color[_counter]);
+		send_led_rgb();
+		_led_interval = _pattern.duration[_counter];
+		break;
+
+	default:
+		break;
 	}
 
 	_counter++;
@@ -282,181 +298,231 @@ RGBLED::led()
 	work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, _led_interval);
 }
 
+/**
+ * Parse color constant and set _r _g _b values
+ */
 void
-RGBLED::set_color(rgbled_color_t color) {
+RGBLED::set_color(rgbled_color_t color)
+{
+	switch (color) {
+	case RGBLED_COLOR_OFF:
+		_r = 0;
+		_g = 0;
+		_b = 0;
+		break;
 
-	_color = color;
+	case RGBLED_COLOR_RED:
+		_r = 255;
+		_g = 0;
+		_b = 0;
+		break;
 
-	switch (color) {
-		case RGBLED_COLOR_OFF:		// off
-			set_rgb(0,0,0);
-			break;
-		case RGBLED_COLOR_RED:		// red
-			set_rgb(255,0,0);
-			break;
-		case RGBLED_COLOR_YELLOW:	// yellow
-			set_rgb(255,70,0);
-			break;
-		case RGBLED_COLOR_PURPLE:	// purple
-			set_rgb(255,0,255);
-			break;
-		case RGBLED_COLOR_GREEN:	// green
-			set_rgb(0,255,0);
-			break;
-		case RGBLED_COLOR_BLUE:		// blue
-			set_rgb(0,0,255);
-			break;
-		case RGBLED_COLOR_WHITE:	// white
-			set_rgb(255,255,255);
-			break;
-		case RGBLED_COLOR_AMBER:	// amber
-			set_rgb(255,20,0);
-			break;
-		case RGBLED_COLOR_DIM_RED:	// red
-			set_rgb(90,0,0);
-			break;
-		case RGBLED_COLOR_DIM_YELLOW:	// yellow
-			set_rgb(80,30,0);
-			break;
-		case RGBLED_COLOR_DIM_PURPLE:	// purple
-			set_rgb(45,0,45);
-			break;
-		case RGBLED_COLOR_DIM_GREEN:	// green
-			set_rgb(0,90,0);
-			break;
-		case RGBLED_COLOR_DIM_BLUE:	// blue
-			set_rgb(0,0,90);
-			break;
-		case RGBLED_COLOR_DIM_WHITE:	// white
-			set_rgb(30,30,30);
-			break;
-		case RGBLED_COLOR_DIM_AMBER:	// amber
-			set_rgb(80,20,0);
-			break;
-		default:
-			warnx("color unknown");
-			break;
+	case RGBLED_COLOR_YELLOW:
+		_r = 255;
+		_g = 200;
+		_b = 0;
+		break;
+
+	case RGBLED_COLOR_PURPLE:
+		_r = 255;
+		_g = 0;
+		_b = 255;
+		break;
+
+	case RGBLED_COLOR_GREEN:
+		_r = 0;
+		_g = 255;
+		_b = 0;
+		break;
+
+	case RGBLED_COLOR_BLUE:
+		_r = 0;
+		_g = 0;
+		_b = 255;
+		break;
+
+	case RGBLED_COLOR_WHITE:
+		_r = 255;
+		_g = 255;
+		_b = 255;
+		break;
+
+	case RGBLED_COLOR_AMBER:
+		_r = 255;
+		_g = 80;
+		_b = 0;
+		break;
+
+	case RGBLED_COLOR_DIM_RED:
+		_r = 90;
+		_g = 0;
+		_b = 0;
+		break;
+
+	case RGBLED_COLOR_DIM_YELLOW:
+		_r = 80;
+		_g = 30;
+		_b = 0;
+		break;
+
+	case RGBLED_COLOR_DIM_PURPLE:
+		_r = 45;
+		_g = 0;
+		_b = 45;
+		break;
+
+	case RGBLED_COLOR_DIM_GREEN:
+		_r = 0;
+		_g = 90;
+		_b = 0;
+		break;
+
+	case RGBLED_COLOR_DIM_BLUE:
+		_r = 0;
+		_g = 0;
+		_b = 90;
+		break;
+
+	case RGBLED_COLOR_DIM_WHITE:
+		_r = 30;
+		_g = 30;
+		_b = 30;
+		break;
+
+	case RGBLED_COLOR_DIM_AMBER:
+		_r = 80;
+		_g = 20;
+		_b = 0;
+		break;
+
+	default:
+		warnx("color unknown");
+		break;
 	}
 }
 
+/**
+ * Set mode, if mode not changed has no any effect (doesn't reset blinks phase)
+ */
 void
 RGBLED::set_mode(rgbled_mode_t mode)
 {
-	_mode = mode;
+	if (mode != _mode) {
+		_mode = mode;
+		bool should_run = false;
 
-	switch (mode) {
+		switch (mode) {
 		case RGBLED_MODE_OFF:
-			_should_run = false;
-			set_on(false);
+			send_led_enable(false);
 			break;
+
 		case RGBLED_MODE_ON:
-			_should_run = false;
-			set_on(true);
+			_brightness = 1.0f;
+			send_led_rgb();
+			send_led_enable(true);
 			break;
+
 		case RGBLED_MODE_BLINK_SLOW:
-			_should_run = true;
+			should_run = true;
+			_counter = 0;
 			_led_interval = 2000;
+			_brightness = 1.0f;
+			send_led_rgb();
 			break;
+
 		case RGBLED_MODE_BLINK_NORMAL:
-			_should_run = true;
+			should_run = true;
+			_counter = 0;
 			_led_interval = 500;
+			_brightness = 1.0f;
+			send_led_rgb();
 			break;
+
 		case RGBLED_MODE_BLINK_FAST:
-			_should_run = true;
+			should_run = true;
+			_counter = 0;
 			_led_interval = 100;
+			_brightness = 1.0f;
+			send_led_rgb();
 			break;
+
 		case RGBLED_MODE_BREATHE:
-			_should_run = true;
-			set_on(true);
+			should_run = true;
 			_counter = 0;
-			_led_interval = 1000/15;
+			_led_interval = 25;
+			send_led_enable(true);
 			break;
+
 		case RGBLED_MODE_PATTERN:
-			_should_run = true;
-			set_on(true);
+			should_run = true;
 			_counter = 0;
+			_brightness = 1.0f;
+			send_led_enable(true);
 			break;
+
 		default:
 			warnx("mode unknown");
 			break;
-	}
+		}
 
-	/* if it should run now, start the workq */
-	if (_should_run && !_running) {
-		_running = true;
-		work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1);
-	}
-	/* if it should stop, then cancel the workq */
-	if (!_should_run && _running) {
-		_running = false;
-		work_cancel(LPWORK, &_work);
+		/* if it should run now, start the workq */
+		if (should_run && !_running) {
+			_running = true;
+			work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1);
+		}
+
+		/* if it should stop, then cancel the workq */
+		if (!should_run && _running) {
+			_running = false;
+			work_cancel(LPWORK, &_work);
+		}
 	}
 }
 
+/**
+ * Set pattern for PATTERN mode, but don't change current mode
+ */
 void
 RGBLED::set_pattern(rgbled_pattern_t *pattern)
 {
 	memcpy(&_pattern, pattern, sizeof(rgbled_pattern_t));
-
-	set_mode(RGBLED_MODE_PATTERN);
-}
-
-void
-RGBLED::set_brightness(float brightness) {
-
-	_brightness = brightness;
-	set_rgb(_r, _g, _b);
-}
-
-int
-RGBLED::set(bool on, uint8_t r, uint8_t g, uint8_t b)
-{
-	uint8_t settings_byte = 0;
-
-	if (on)
-		settings_byte |= SETTING_ENABLE;
-/* powersave not used */
-//	if (not_powersave)
-		settings_byte |= SETTING_NOT_POWERSAVE;
-
-	const uint8_t msg[5] = { SUB_ADDR_START, (uint8_t)(b*15/255), (uint8_t)(g*15/255), (uint8_t)(r*15/255), settings_byte};
-
-	return transfer(msg, sizeof(msg), nullptr, 0);
 }
 
+/**
+ * Sent ENABLE flag to LED driver
+ */
 int
-RGBLED::set_on(bool on)
+RGBLED::send_led_enable(bool enable)
 {
 	uint8_t settings_byte = 0;
 
-	if (on)
+	if (enable)
 		settings_byte |= SETTING_ENABLE;
 
-/* powersave not used */
-//	if (not_powersave)
-		settings_byte |= SETTING_NOT_POWERSAVE;
+	settings_byte |= SETTING_NOT_POWERSAVE;
 
 	const uint8_t msg[2] = { SUB_ADDR_SETTINGS, settings_byte};
 
 	return transfer(msg, sizeof(msg), nullptr, 0);
 }
 
+/**
+ * Send RGB PWM settings to LED driver according to current color and brightness
+ */
 int
-RGBLED::set_rgb(uint8_t r, uint8_t g, uint8_t b)
+RGBLED::send_led_rgb()
 {
-	/* save the RGB values in case we want to change the brightness later */
-	_r = r;
-	_g = g;
-	_b = b;
-
-	const uint8_t msg[6] = { SUB_ADDR_PWM0, (uint8_t)((float)b/255.0f*15.0f*_brightness), SUB_ADDR_PWM1, (uint8_t)((float)g/255.0f*15.0f*_brightness), SUB_ADDR_PWM2, (uint8_t)((float)r/255.0f*15.0f*_brightness)};
-
+	/* To scale from 0..255 -> 0..15 shift right by 4 bits */
+	const uint8_t msg[6] = {
+		SUB_ADDR_PWM0, (uint8_t)((int)(_b * _brightness) >> 4),
+		SUB_ADDR_PWM1, (uint8_t)((int)(_g * _brightness) >> 4),
+		SUB_ADDR_PWM2, (uint8_t)((int)(_r * _brightness) >> 4)
+	};
 	return transfer(msg, sizeof(msg), nullptr, 0);
 }
 
-
 int
-RGBLED::get(bool &on, bool &not_powersave, uint8_t &r, uint8_t &g, uint8_t &b)
+RGBLED::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b)
 {
 	uint8_t result[2];
 	int ret;
@@ -465,24 +531,23 @@ RGBLED::get(bool &on, bool &not_powersave, uint8_t &r, uint8_t &g, uint8_t &b)
 
 	if (ret == OK) {
 		on = result[0] & SETTING_ENABLE;
-		not_powersave = result[0] & SETTING_NOT_POWERSAVE;
+		powersave = !(result[0] & SETTING_NOT_POWERSAVE);
 		/* XXX check, looks wrong */
-		r = (result[0] & 0x0f)*255/15;
-		g = (result[1] & 0xf0)*255/15;
-		b = (result[1] & 0x0f)*255/15;
+		r = (result[0] & 0x0f) << 4;
+		g = (result[1] & 0xf0);
+		b = (result[1] & 0x0f) << 4;
 	}
 
 	return ret;
 }
 
-void rgbled_usage();
-
-
-void rgbled_usage() {
-	warnx("missing command: try 'start', 'test', 'info', 'stop'/'off', 'rgb 30 40 50'");
+void
+rgbled_usage()
+{
+	warnx("missing command: try 'start', 'test', 'info', 'off', 'rgb 30 40 50'");
 	warnx("options:");
 	warnx("    -b i2cbus (%d)", PX4_I2C_BUS_LED);
-	errx(0, "    -a addr (0x%x)", ADDR);
+	warnx("    -a addr (0x%x)", ADDR);
 }
 
 int
@@ -492,17 +557,21 @@ rgbled_main(int argc, char *argv[])
 	int rgbledadr = ADDR; /* 7bit */
 
 	int ch;
+
 	/* jump over start/off/etc and look at options first */
-	while ((ch = getopt(argc-1, &argv[1], "a:b:")) != EOF) {
+	while ((ch = getopt(argc - 1, &argv[1], "a:b:")) != EOF) {
 		switch (ch) {
 		case 'a':
 			rgbledadr = strtol(optarg, NULL, 0);
 			break;
+
 		case 'b':
 			i2cdevice = strtol(optarg, NULL, 0);
 			break;
+
 		default:
 			rgbled_usage();
+			exit(0);
 		}
 	}
 
@@ -519,17 +588,21 @@ rgbled_main(int argc, char *argv[])
 			// try the external bus first
 			i2cdevice = PX4_I2C_BUS_EXPANSION;
 			g_rgbled = new RGBLED(PX4_I2C_BUS_EXPANSION, rgbledadr);
+
 			if (g_rgbled != nullptr && OK != g_rgbled->init()) {
 				delete g_rgbled;
 				g_rgbled = nullptr;
 			}
+
 			if (g_rgbled == nullptr) {
 				// fall back to default bus
 				i2cdevice = PX4_I2C_BUS_LED;
 			}
 		}
+
 		if (g_rgbled == nullptr) {
 			g_rgbled = new RGBLED(i2cdevice, rgbledadr);
+
 			if (g_rgbled == nullptr)
 				errx(1, "new failed");
 
@@ -545,21 +618,24 @@ rgbled_main(int argc, char *argv[])
 
 	/* need the driver past this point */
 	if (g_rgbled == nullptr) {
-	    warnx("not started");
-	    rgbled_usage();
-	    exit(0);
+		warnx("not started");
+		rgbled_usage();
+		exit(0);
 	}
 
 	if (!strcmp(verb, "test")) {
 		fd = open(RGBLED_DEVICE_PATH, 0);
+
 		if (fd == -1) {
 			errx(1, "Unable to open " RGBLED_DEVICE_PATH);
 		}
 
-		rgbled_pattern_t pattern = { {RGBLED_COLOR_RED, RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_OFF},
-					     {200,              200,                200,               400             } };
+		rgbled_pattern_t pattern = { {RGBLED_COLOR_RED, RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_WHITE, RGBLED_COLOR_OFF, RGBLED_COLOR_OFF},
+			{500, 500, 500, 500, 1000, 0 }	// "0" indicates end of pattern
+		};
 
 		ret = ioctl(fd, RGBLED_SET_PATTERN, (unsigned long)&pattern);
+		ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_PATTERN);
 
 		close(fd);
 		exit(ret);
@@ -570,33 +646,39 @@ rgbled_main(int argc, char *argv[])
 		exit(0);
 	}
 
-	if (!strcmp(verb, "stop") || !strcmp(verb, "off")) {
-		/* although technically it doesn't stop, this is the excepted syntax */
+	if (!strcmp(verb, "off")) {
 		fd = open(RGBLED_DEVICE_PATH, 0);
+
 		if (fd == -1) {
 			errx(1, "Unable to open " RGBLED_DEVICE_PATH);
 		}
+
 		ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF);
 		close(fd);
 		exit(ret);
 	}
 
 	if (!strcmp(verb, "rgb")) {
+		if (argc < 5) {
+			errx(1, "Usage: rgbled rgb <red> <green> <blue>");
+		}
+
 		fd = open(RGBLED_DEVICE_PATH, 0);
+
 		if (fd == -1) {
 			errx(1, "Unable to open " RGBLED_DEVICE_PATH);
 		}
-		if (argc < 5) {
-			errx(1, "Usage: rgbled rgb <red> <green> <blue>");
-		}
+
 		rgbled_rgbset_t v;
 		v.red   = strtol(argv[2], NULL, 0);
 		v.green = strtol(argv[3], NULL, 0);
 		v.blue  = strtol(argv[4], NULL, 0);
 		ret = ioctl(fd, RGBLED_SET_RGB, (unsigned long)&v);
+		ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON);
 		close(fd);
 		exit(ret);
 	}
 
 	rgbled_usage();
+	exit(0);
 }
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index a2443b0f6..1e86e8e24 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -5,6 +5,7 @@
  *           Lorenz Meier <lm@inf.ethz.ch>
  *           Thomas Gubler <thomasgubler@student.ethz.ch>
  *           Julian Oes <joes@student.ethz.ch>
+ *           Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -198,7 +199,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode
  */
 int commander_thread_main(int argc, char *argv[]);
 
-void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed);
+void control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed);
 
 void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
 
@@ -650,7 +651,6 @@ int commander_thread_main(int argc, char *argv[])
 	bool critical_battery_voltage_actions_done = false;
 
 	uint64_t last_idle_time = 0;
-
 	uint64_t start_time = 0;
 
 	bool status_changed = true;
@@ -728,7 +728,7 @@ int commander_thread_main(int argc, char *argv[])
 	struct subsystem_info_s info;
 	memset(&info, 0, sizeof(info));
 
-	toggle_status_leds(&status, &armed, true);
+	control_status_leds(&status, &armed, true);
 
 	/* now initialized */
 	commander_initialized = true;
@@ -950,11 +950,9 @@ int commander_thread_main(int argc, char *argv[])
 				battery_tune_played = false;
 
 				if (armed.armed) {
-					// XXX not sure what should happen when voltage is low in flight
-					//arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
+					arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
 				} else {
-					// XXX should we still allow to arm with critical battery?
-					//arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
+					arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
 				}
 
 				status_changed = true;
@@ -1166,9 +1164,6 @@ int commander_thread_main(int argc, char *argv[])
 		if (arming_state_changed || main_state_changed || navigation_state_changed) {
 			mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state);
 			status_changed = true;
-
-		} else {
-			status_changed = false;
 		}
 
 		hrt_abstime t1 = hrt_absolute_time();
@@ -1228,7 +1223,19 @@ int commander_thread_main(int argc, char *argv[])
 		fflush(stdout);
 		counter++;
 
-		toggle_status_leds(&status, &armed, arming_state_changed || status_changed);
+		int blink_state = blink_msg_state();
+		if (blink_state > 0) {
+			/* blinking LED message, don't touch LEDs */
+			if (blink_state == 2) {
+				/* blinking LED message completed, restore normal state */
+				control_status_leds(&status, &armed, true);
+			}
+		} else {
+			/* normal state */
+			control_status_leds(&status, &armed, status_changed);
+		}
+
+		status_changed = false;
 
 		usleep(COMMANDER_MONITORING_INTERVAL);
 	}
@@ -1276,8 +1283,48 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val
 }
 
 void
-toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed)
+control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed)
 {
+	/* driving rgbled */
+	if (changed) {
+		bool set_normal_color = false;
+		/* set mode */
+		if (status->arming_state == ARMING_STATE_ARMED) {
+			rgbled_set_mode(RGBLED_MODE_ON);
+			set_normal_color = true;
+
+		} else if (status->arming_state == ARMING_STATE_ARMED_ERROR) {
+			rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
+			rgbled_set_color(RGBLED_COLOR_RED);
+
+		} else if (status->arming_state == ARMING_STATE_STANDBY) {
+			rgbled_set_mode(RGBLED_MODE_BREATHE);
+			set_normal_color = true;
+
+		} else {	// STANDBY_ERROR and other states
+			rgbled_set_mode(RGBLED_MODE_BLINK_NORMAL);
+			rgbled_set_color(RGBLED_COLOR_RED);
+		}
+
+		if (set_normal_color) {
+			/* set color */
+			if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) {
+				if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
+					rgbled_set_color(RGBLED_COLOR_AMBER);
+				}
+				/* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */
+
+			} else {
+				if (status->condition_local_position_valid) {
+					rgbled_set_color(RGBLED_COLOR_GREEN);
+
+				} else {
+					rgbled_set_color(RGBLED_COLOR_BLUE);
+				}
+			}
+		}
+	}
+
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 
 	/* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */
@@ -1298,54 +1345,6 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang
 
 #endif
 
-	if (changed) {
-		/* XXX TODO blink fast when armed and serious error occurs */
-
-		if (armed->armed) {
-			rgbled_set_mode(RGBLED_MODE_ON);
-
-		} else if (armed->ready_to_arm) {
-			rgbled_set_mode(RGBLED_MODE_BREATHE);
-
-		} else {
-			rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
-		}
-	}
-
-	if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) {
-		switch (status->battery_warning) {
-		case VEHICLE_BATTERY_WARNING_LOW:
-			rgbled_set_color(RGBLED_COLOR_YELLOW);
-			break;
-
-		case VEHICLE_BATTERY_WARNING_CRITICAL:
-			rgbled_set_color(RGBLED_COLOR_AMBER);
-			break;
-
-		default:
-			break;
-		}
-
-	} else {
-		switch (status->main_state) {
-		case MAIN_STATE_MANUAL:
-			rgbled_set_color(RGBLED_COLOR_WHITE);
-			break;
-
-		case MAIN_STATE_SEATBELT:
-		case MAIN_STATE_EASY:
-			rgbled_set_color(RGBLED_COLOR_GREEN);
-			break;
-
-		case MAIN_STATE_AUTO:
-			rgbled_set_color(RGBLED_COLOR_BLUE);
-			break;
-
-		default:
-			break;
-		}
-	}
-
 	/* give system warnings on error LED, XXX maybe add memory usage warning too */
 	if (status->load > 0.95f) {
 		if (leds_counter % 2 == 0)
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
index 017499cb5..565b4b66a 100644
--- a/src/modules/commander/commander_helper.cpp
+++ b/src/modules/commander/commander_helper.cpp
@@ -3,6 +3,7 @@
  *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
  *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
  *           Julian Oes <joes@student.ethz.ch>
+ *           Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -55,7 +56,6 @@
 #include <drivers/drv_led.h>
 #include <drivers/drv_rgbled.h>
 
-
 #include "commander_helper.h"
 
 /* oddly, ERROR is not defined for c++ */
@@ -64,21 +64,24 @@
 #endif
 static const int ERROR = -1;
 
+#define BLINK_MSG_TIME	700000	// 3 fast blinks
+
 bool is_multirotor(const struct vehicle_status_s *current_status)
 {
 	return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
-	    (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
-	    (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) ||
-	    (current_status->system_type == VEHICLE_TYPE_TRICOPTER));
+		(current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
+		(current_status->system_type == VEHICLE_TYPE_OCTOROTOR) ||
+		(current_status->system_type == VEHICLE_TYPE_TRICOPTER));
 }
 
 bool is_rotary_wing(const struct vehicle_status_s *current_status)
 {
 	return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER)
-	|| (current_status->system_type == VEHICLE_TYPE_COAXIAL);
+	       || (current_status->system_type == VEHICLE_TYPE_COAXIAL);
 }
 
 static int buzzer;
+static hrt_abstime blink_msg_end;
 
 int buzzer_init()
 {
@@ -104,16 +107,25 @@ void tune_error()
 
 void tune_positive()
 {
+	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
+	rgbled_set_color(RGBLED_COLOR_GREEN);
+	rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
 	ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_POSITIVE_TUNE);
 }
 
 void tune_neutral()
 {
+	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
+	rgbled_set_color(RGBLED_COLOR_WHITE);
+	rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
 	ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE);
 }
 
 void tune_negative()
 {
+	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
+	rgbled_set_color(RGBLED_COLOR_RED);
+	rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
 	ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE);
 }
 
@@ -132,18 +144,31 @@ int tune_critical_bat()
 	return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_FAST_TUNE);
 }
 
-
-
 void tune_stop()
 {
 	ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE);
 }
 
+int blink_msg_state()
+{
+	if (blink_msg_end == 0) {
+		return 0;
+
+	} else if (hrt_absolute_time() > blink_msg_end) {
+		return 2;
+
+	} else {
+		return 1;
+	}
+}
+
 static int leds;
 static int rgbleds;
 
 int led_init()
 {
+	blink_msg_end = 0;
+
 	/* first open normal LEDs */
 	leds = open(LED_DEVICE_PATH, 0);
 
@@ -159,6 +184,7 @@ int led_init()
 		warnx("Blue LED: ioctl fail\n");
 		return ERROR;
 	}
+
 #endif
 
 	if (ioctl(leds, LED_ON, LED_AMBER)) {
@@ -168,6 +194,7 @@ int led_init()
 
 	/* then try RGB LEDs, this can fail on FMUv1*/
 	rgbleds = open(RGBLED_DEVICE_PATH, 0);
+
 	if (rgbleds == -1) {
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
 		errx(1, "Unable to open " RGBLED_DEVICE_PATH);
@@ -203,19 +230,22 @@ int led_off(int led)
 	return ioctl(leds, LED_OFF, led);
 }
 
-void rgbled_set_color(rgbled_color_t color) {
+void rgbled_set_color(rgbled_color_t color)
+{
 
 	if (rgbleds != -1)
 		ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color);
 }
 
-void rgbled_set_mode(rgbled_mode_t mode) {
+void rgbled_set_mode(rgbled_mode_t mode)
+{
 
 	if (rgbleds != -1)
 		ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode);
 }
 
-void rgbled_set_pattern(rgbled_pattern_t *pattern) {
+void rgbled_set_pattern(rgbled_pattern_t *pattern)
+{
 
 	if (rgbleds != -1)
 		ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern);
diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h
index 027d0535f..e9514446c 100644
--- a/src/modules/commander/commander_helper.h
+++ b/src/modules/commander/commander_helper.h
@@ -62,6 +62,7 @@ int tune_arm(void);
 int tune_low_bat(void);
 int tune_critical_bat(void);
 void tune_stop(void);
+int blink_msg_state();
 
 int led_init(void);
 void led_deinit(void);
@@ -70,9 +71,7 @@ int led_on(int led);
 int led_off(int led);
 
 void rgbled_set_color(rgbled_color_t color);
-
 void rgbled_set_mode(rgbled_mode_t mode);
-
 void rgbled_set_pattern(rgbled_pattern_t *pattern);
 
 /**
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 222d1f45f..a3ef1d63b 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -574,6 +574,7 @@ handle_message(mavlink_message_t *msg)
 				orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
 			}
 
+			hil_global_pos.valid = true;
 			hil_global_pos.lat = hil_state.lat;
 			hil_global_pos.lon = hil_state.lon;
 			hil_global_pos.alt = hil_state.alt / 1000.0f;