Merge pull request #3206 from mcharleb/driver_framework_latest

Added changes to sensors.cpp for DriverFramework

cmake/configs/posix_sitl_simple.cmake

@@ -3,11 +3,9 @@ include(posix/px4_impl_posix)
 set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake)
 
 set(config_module_list
-	drivers/led
 	drivers/device
 	drivers/boards/sitl
 	drivers/pwm_out_sim
-	drivers/rgbled
 	platforms/common
 	platforms/posix/px4_layer
 	platforms/posix/work_queue
@@ -18,6 +16,8 @@ set(config_module_list
 	platforms/posix/drivers/airspeedsim
 	platforms/posix/drivers/barosim
 	platforms/posix/drivers/gyrosim
+	platforms/posix/drivers/rgbledsim
+	platforms/posix/drivers/ledsim
 	systemcmds/param
 	systemcmds/mixer
 	systemcmds/ver

posix-configs/SITL/init/rcS_jmavsim

@@ -27,7 +27,7 @@ param set MPC_XY_VEL_P 0.2
 param set MPC_XY_VEL_D 0.005
 param set SENS_BOARD_ROT 0
 param set COM_RC_IN_MODE 2
-rgbled start
+rgbledsim start
 tone_alarm start
 gyrosim start
 accelsim start

src/drivers/drv_led.h

@@ -43,6 +43,7 @@
 #include <stdint.h>
 #include <sys/ioctl.h>
 
+#define LED_BASE_DEVICE_PATH		"/dev/led"
 #define LED0_DEVICE_PATH		"/dev/led0"
 
 #define _LED_BASE		0x2800

src/drivers/drv_rgbled.h

@@ -42,6 +42,8 @@
 #include <stdint.h>
 #include <sys/ioctl.h>
 
+#define RGBLED_BASE_DEVICE_PATH "/dev/rgbled"
+
 /* more devices will be 1, 2, etc */
 #define RGBLED0_DEVICE_PATH "/dev/rgbled0"
 

src/modules/commander/PreflightCheck.cpp

@@ -69,21 +69,23 @@
 
 #include "PreflightCheck.h"
 
+#include "DevMgr.hpp"
+
+using namespace DriverFramework;
+
 namespace Commander
 {
 
-static int check_calibration(int fd, const char* param_template, int &devid);
+static int check_calibration(DevHandle &h, const char* param_template, int &devid)
-
-int check_calibration(int fd, const char* param_template, int &devid)
 {
 	bool calibration_found;
 
 	/* new style: ask device for calibration state */
-	int ret = px4_ioctl(fd, SENSORIOCCALTEST, 0);
+	int ret = h.ioctl(SENSORIOCCALTEST, 0);
 
 	calibration_found = (ret == OK);
 	
-	devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
+	devid = h.ioctl(DEVIOCGDEVICEID, 0);
 
 	char s[20];
 	int instance = 0;
@@ -120,9 +122,10 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
 
 	char s[30];
 	sprintf(s, "%s%u", MAG_BASE_DEVICE_PATH, instance);
-	int fd = px4_open(s, 0);
+	DevHandle h;
+	DevMgr::getHandle(s, h);
 
-	if (fd < 0) {
+	if (!h.isValid()) {
 		if (!optional) {
 			mavlink_and_console_log_critical(mavlink_fd,
 							 "PREFLIGHT FAIL: NO MAG SENSOR #%u", instance);
@@ -131,7 +134,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
 		return false;
 	}
 
-	int ret = check_calibration(fd, "CAL_MAG%u_ID", device_id);
+	int ret = check_calibration(h, "CAL_MAG%u_ID", device_id);
 
 	if (ret) {
 		mavlink_and_console_log_critical(mavlink_fd,
@@ -140,7 +143,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
 		goto out;
 	}
 
-	ret = px4_ioctl(fd, MAGIOCSELFTEST, 0);
+	ret = h.ioctl(MAGIOCSELFTEST, 0);
 
 	if (ret != OK) {
 		mavlink_and_console_log_critical(mavlink_fd,
@@ -150,7 +153,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
 	}
 
 out:
-	px4_close(fd);
+	DevMgr::releaseHandle(h);
 	return success;
 }
 
@@ -160,9 +163,10 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
 
 	char s[30];
 	sprintf(s, "%s%u", ACCEL_BASE_DEVICE_PATH, instance);
-	int fd = px4_open(s, O_RDONLY);
+	DevHandle h;
+	DevMgr::getHandle(s, h);
 
-	if (fd < 0) {
+	if (!h.isValid()) {
 		if (!optional) {
 			mavlink_and_console_log_critical(mavlink_fd,
 							 "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance);
@@ -171,7 +175,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
 		return false;
 	}
 
-	int ret = check_calibration(fd, "CAL_ACC%u_ID", device_id);
+	int ret = check_calibration(h, "CAL_ACC%u_ID", device_id);
 
 	if (ret) {
 		mavlink_and_console_log_critical(mavlink_fd,
@@ -180,7 +184,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
 		goto out;
 	}
 
-	ret = px4_ioctl(fd, ACCELIOCSELFTEST, 0);
+	ret = h.ioctl(ACCELIOCSELFTEST, 0);
 
 	if (ret != OK) {
 		mavlink_and_console_log_critical(mavlink_fd,
@@ -215,7 +219,9 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
 #endif
 
 out:
+#ifdef __PX4_NUTTX
 	px4_close(fd);
+#endif
 	return success;
 }
 
@@ -225,9 +231,10 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
 
 	char s[30];
 	sprintf(s, "%s%u", GYRO_BASE_DEVICE_PATH, instance);
-	int fd = px4_open(s, 0);
+	DevHandle h;
+	DevMgr::getHandle(s, h);
 
-	if (fd < 0) {
+	if (!h.isValid()) {
 		if (!optional) {
 			mavlink_and_console_log_critical(mavlink_fd,
 							 "PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance);
@@ -236,7 +243,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
 		return false;
 	}
 
-	int ret = check_calibration(fd, "CAL_GYRO%u_ID", device_id);
+	int ret = check_calibration(h, "CAL_GYRO%u_ID", device_id);
 
 	if (ret) {
 		mavlink_and_console_log_critical(mavlink_fd,
@@ -245,7 +252,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
 		goto out;
 	}
 
-	ret = px4_ioctl(fd, GYROIOCSELFTEST, 0);
+	ret = h.ioctl(GYROIOCSELFTEST, 0);
 
 	if (ret != OK) {
 		mavlink_and_console_log_critical(mavlink_fd,
@@ -255,7 +262,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
 	}
 
 out:
-	px4_close(fd);
+	DevMgr::releaseHandle(h);
 	return success;
 }
 
@@ -265,9 +272,10 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
 
 	char s[30];
 	sprintf(s, "%s%u", BARO_BASE_DEVICE_PATH, instance);
-	int fd = px4_open(s, 0);
+	DevHandle h;
+	DevMgr::getHandle(s, h);
 
-	if (fd < 0) {
+	if (!h.isValid()) {
 		if (!optional) {
 			mavlink_and_console_log_critical(mavlink_fd,
 							 "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance);
@@ -290,7 +298,7 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
 
 //out:
 
-	px4_close(fd);
+	DevMgr::releaseHandle(h);
 	return success;
 }
 

src/modules/commander/commander_helper.cpp

@@ -63,6 +63,9 @@
 #include <drivers/drv_rgbled.h>
 
 #include "commander_helper.h"
+#include "DevMgr.hpp"
+
+using namespace DriverFramework;
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -93,7 +96,7 @@ bool is_vtol(const struct vehicle_status_s * current_status) {
 		current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_OCTOROTOR);
 }
 
-static int buzzer = -1;
+static DevHandle h_buzzer;
 static hrt_abstime blink_msg_end = 0;	// end time for currently blinking LED message, 0 if no blink message
 static hrt_abstime tune_end = 0;		// end time of currently played tune, 0 for repeating tunes or silence
 static int tune_current = TONE_STOP_TUNE;		// currently playing tune, can be interrupted after tune_end
@@ -133,10 +136,11 @@ int buzzer_init()
 	tune_durations[TONE_NOTIFY_NEUTRAL_TUNE] = 500000;
 	tune_durations[TONE_ARMING_WARNING_TUNE] = 3000000;
 
-	buzzer = px4_open(TONEALARM0_DEVICE_PATH, O_WRONLY);
+	DevHandle h_buzzer;
+	DevMgr::getHandle(TONEALARM0_DEVICE_PATH, h_buzzer);
 
-	if (buzzer < 0) {
+	if (!h_buzzer.isValid()) {
-		warnx("Buzzer: px4_open fail\n");
+		PX4_WARN("Buzzer: px4_open fail\n");
 		return ERROR;
 	}
 
@@ -145,12 +149,12 @@ int buzzer_init()
 
 void buzzer_deinit()
 {
-	px4_close(buzzer);
+	DevMgr::releaseHandle(h_buzzer);
 }
 
 void set_tune_override(int tune)
 {
-	px4_ioctl(buzzer, TONE_SET_ALARM, tune);
+	h_buzzer.ioctl(TONE_SET_ALARM, tune);
 }
 
 void set_tune(int tune)
@@ -161,7 +165,7 @@ void set_tune(int tune)
 	if (tune_end == 0 || new_tune_duration != 0 || hrt_absolute_time() > tune_end) {
 		/* allow interrupting current non-repeating tune by the same tune */
 		if (tune != tune_current || new_tune_duration != 0) {
-			px4_ioctl(buzzer, TONE_SET_ALARM, tune);
+			h_buzzer.ioctl(TONE_SET_ALARM, tune);
 		}
 
 		tune_current = tune;
@@ -264,30 +268,30 @@ int blink_msg_state()
 	}
 }
 
-static int leds = -1;
+static DevHandle h_leds;
-static int rgbleds = -1;
+static DevHandle h_rgbleds;
 
 int led_init()
 {
 	blink_msg_end = 0;
 
 	/* first open normal LEDs */
-	leds = px4_open(LED0_DEVICE_PATH, 0);
+	DevMgr::getHandle(LED0_DEVICE_PATH, h_leds);
 
-	if (leds < 0) {
+	if (!h_leds.isValid()) {
-		warnx("LED: px4_open fail\n");
+		PX4_WARN("LED: getHandle fail\n");
 		return ERROR;
 	}
 
 	/* the blue LED is only available on FMUv1 & AeroCore but not FMUv2 */
-	(void)px4_ioctl(leds, LED_ON, LED_BLUE);
+	(void)h_leds.ioctl(LED_ON, LED_BLUE);
 
 	/* switch blue off */
 	led_off(LED_BLUE);
 
 	/* we consider the amber led mandatory */
-	if (px4_ioctl(leds, LED_ON, LED_AMBER)) {
+	if (h_leds.ioctl(LED_ON, LED_AMBER)) {
-		warnx("Amber LED: px4_ioctl fail\n");
+		PX4_WARN("Amber LED: ioctl fail\n");
 		return ERROR;
 	}
 
@@ -295,10 +299,11 @@ int led_init()
 	led_off(LED_AMBER);
 
 	/* then try RGB LEDs, this can fail on FMUv1*/
-	rgbleds = px4_open(RGBLED0_DEVICE_PATH, 0);
+	DevHandle h;
+	DevMgr::getHandle(RGBLED0_DEVICE_PATH, h_rgbleds);
 
-	if (rgbleds < 0) {
+	if (!h_rgbleds.isValid()) {
-		warnx("No RGB LED found at " RGBLED0_DEVICE_PATH);
+		PX4_WARN("No RGB LED found at " RGBLED0_DEVICE_PATH);
 	}
 
 	return 0;
@@ -306,64 +311,41 @@ int led_init()
 
 void led_deinit()
 {
-	if (leds >= 0) {
+	DevMgr::releaseHandle(h_leds);
-		px4_close(leds);
+	DevMgr::releaseHandle(h_rgbleds);
-	}
-
-	if (rgbleds >= 0) {
-		px4_close(rgbleds);
-	}
 }
 
 int led_toggle(int led)
 {
-	if (leds < 0) {
+	return h_leds.ioctl(LED_TOGGLE, led);
-		return leds;
-	}
-	return px4_ioctl(leds, LED_TOGGLE, led);
 }
 
 int led_on(int led)
 {
-	if (leds < 0) {
+	return h_leds.ioctl(LED_ON, led);
-		return leds;
-	}
-	return px4_ioctl(leds, LED_ON, led);
 }
 
 int led_off(int led)
 {
-	if (leds < 0) {
+	return h_leds.ioctl(LED_OFF, led);
-		return leds;
-	}
-	return px4_ioctl(leds, LED_OFF, led);
 }
 
 void rgbled_set_color(rgbled_color_t color)
 {
 
-	if (rgbleds < 0) {
+	h_rgbleds.ioctl(RGBLED_SET_COLOR, (unsigned long)color);
-		return;
-	}
-	px4_ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color);
 }
 
 void rgbled_set_mode(rgbled_mode_t mode)
 {
 
-	if (rgbleds < 0) {
+	h_rgbleds.ioctl(RGBLED_SET_MODE, (unsigned long)mode);
-		return;
-	}
-	px4_ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode);
 }
 
 void rgbled_set_pattern(rgbled_pattern_t *pattern)
 {
 
-	if (rgbleds < 0) {
+	h_rgbleds.ioctl(RGBLED_SET_PATTERN, (unsigned long)pattern);
-		return;
-	}
-	px4_ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern);
 }
 
 unsigned battery_get_n_cells() {

src/modules/sensors/sensors.cpp

@@ -97,6 +97,10 @@
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/rc_parameter_map.h>
 
+#include "DevMgr.hpp"
+
+using namespace DriverFramework;
+
 /**
  * Analog layout:
  * FMU:
@@ -898,23 +902,20 @@ Sensors::parameters_update()
 
 	/* update barometer qnh setting */
 	param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh));
-	int	barofd;
+	DevHandle h_baro;
-	barofd = px4_open(BARO0_DEVICE_PATH, 0);
+	DevMgr::getHandle(BARO0_DEVICE_PATH, h_baro);
 
-	if (barofd < 0) {
+	if (!h_baro.isValid()) {
-		warnx("ERROR: no barometer found on %s", BARO0_DEVICE_PATH);
+		warnx("ERROR: no barometer found on %s (%d)", BARO0_DEVICE_PATH, h_baro.getError());
 		return ERROR;
 
 	} else {
-		int baroret = px4_ioctl(barofd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100));
+		int baroret = h_baro.ioctl(BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100));
 
 		if (baroret) {
 			warnx("qnh could not be set");
-			px4_close(barofd);
 			return ERROR;
 		}
-
-		px4_close(barofd);
 	}
 
 	return OK;
@@ -923,23 +924,20 @@ Sensors::parameters_update()
 int
 Sensors::accel_init()
 {
-	int	fd;
+	DevHandle h_accel;
+	DevMgr::getHandle(ACCEL0_DEVICE_PATH, h_accel);
 
-	fd = px4_open(ACCEL0_DEVICE_PATH, 0);
+	if (!h_accel.isValid()) {
-
+		warnx("FATAL: no accelerometer found: %s (%d)", ACCEL0_DEVICE_PATH, h_accel.getError());
-	if (fd < 0) {
-		warnx("FATAL: no accelerometer found: %s", ACCEL0_DEVICE_PATH);
 		return ERROR;
 
 	} else {
 
 		/* set the accel internal sampling rate to default rate */
-		px4_ioctl(fd, ACCELIOCSSAMPLERATE, ACCEL_SAMPLERATE_DEFAULT);
+		h_accel.ioctl(ACCELIOCSSAMPLERATE, ACCEL_SAMPLERATE_DEFAULT);
 
 		/* set the driver to poll at default rate */
-		px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
+		h_accel.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
-
-		px4_close(fd);
 	}
 
 	return OK;
@@ -948,23 +946,20 @@ Sensors::accel_init()
 int
 Sensors::gyro_init()
 {
-	int	fd;
+	DevHandle h_gyro;
+	DevMgr::getHandle(GYRO0_DEVICE_PATH, h_gyro);
 
-	fd = px4_open(GYRO0_DEVICE_PATH, 0);
+	if (!h_gyro.isValid()) {
-
+		warnx("FATAL: no gyro found: %s (%d)", GYRO0_DEVICE_PATH, h_gyro.getError());
-	if (fd < 0) {
-		warnx("FATAL: no gyro found: %s", GYRO0_DEVICE_PATH);
 		return ERROR;
 
 	}
 
 	/* set the gyro internal sampling rate to default rate */
-	px4_ioctl(fd, GYROIOCSSAMPLERATE, GYRO_SAMPLERATE_DEFAULT);
+	h_gyro.ioctl(GYROIOCSSAMPLERATE, GYRO_SAMPLERATE_DEFAULT);
 
 	/* set the driver to poll at default rate */
-	px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
+	h_gyro.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
-
-	px4_close(fd);
 
 	return OK;
 }
@@ -972,32 +967,32 @@ Sensors::gyro_init()
 int
 Sensors::mag_init()
 {
-	int	fd;
 	int	ret;
 
-	fd = px4_open(MAG0_DEVICE_PATH, 0);
+	DevHandle h_mag;
+	DevMgr::getHandle(MAG0_DEVICE_PATH, h_mag);
 
-	if (fd < 0) {
+	if (!h_mag.isValid()) {
-		warnx("FATAL: no magnetometer found: %s", MAG0_DEVICE_PATH);
+		warnx("FATAL: no magnetometer found: %s (%d)", MAG0_DEVICE_PATH, h_mag.getError());
 		return ERROR;
 	}
 
 	/* try different mag sampling rates */
 
 
-	ret = px4_ioctl(fd, MAGIOCSSAMPLERATE, 150);
+	ret = h_mag.ioctl(MAGIOCSSAMPLERATE, 150);
 
 	if (ret == OK) {
 		/* set the pollrate accordingly */
-		px4_ioctl(fd, SENSORIOCSPOLLRATE, 150);
+		h_mag.ioctl(SENSORIOCSPOLLRATE, 150);
 
 	} else {
-		ret = px4_ioctl(fd, MAGIOCSSAMPLERATE, 100);
+		ret = h_mag.ioctl(MAGIOCSSAMPLERATE, 100);
 
 		/* if the slower sampling rate still fails, something is wrong */
 		if (ret == OK) {
 			/* set the driver to poll also at the slower rate */
-			px4_ioctl(fd, SENSORIOCSPOLLRATE, 100);
+			h_mag.ioctl(SENSORIOCSPOLLRATE, 100);
 
 		} else {
 			warnx("FATAL: mag sampling rate could not be set");
@@ -1005,27 +1000,22 @@ Sensors::mag_init()
 		}
 	}
 
-	px4_close(fd);
-
 	return OK;
 }
 
 int
 Sensors::baro_init()
 {
-	int	fd;
+	DevHandle h_baro;
+	DevMgr::getHandle(BARO0_DEVICE_PATH, h_baro);
 
-	fd = px4_open(BARO0_DEVICE_PATH, 0);
+	if (!h_baro.isValid()) {
-
+		warnx("FATAL: No barometer found: %s (%d)", BARO0_DEVICE_PATH, h_baro.getError());
-	if (fd < 0) {
-		warnx("FATAL: No barometer found: %s", BARO0_DEVICE_PATH);
 		return ERROR;
 	}
 
 	/* set the driver to poll at 150Hz */
-	px4_ioctl(fd, SENSORIOCSPOLLRATE, 150);
+	h_baro.ioctl(SENSORIOCSPOLLRATE, 150);
-
-	px4_close(fd);
 
 	return OK;
 }
@@ -1034,10 +1024,11 @@ int
 Sensors::adc_init()
 {
 
-	_fd_adc = px4_open(ADC0_DEVICE_PATH, O_RDONLY | O_NONBLOCK);
+	DevHandle h_adc;
+	DevMgr::getHandle(ADC0_DEVICE_PATH, h_adc);
 
-	if (_fd_adc < 0) {
+	if (!h_adc.isValid()) {
-		warnx("FATAL: no ADC found: %s", ADC0_DEVICE_PATH);
+		warnx("FATAL: no ADC found: %s (%d)", ADC0_DEVICE_PATH, h_adc.getError());
 		return ERROR;
 	}
 
@@ -1277,9 +1268,10 @@ Sensors::parameter_update_poll(bool forced)
 			res = ERROR;
 			(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
 
-			int fd = px4_open(str, 0);
+			DevHandle h;
+			DevMgr::getHandle(str, h);
 
-			if (fd < 0) {
+			if (!h.isValid()) {
 				continue;
 			}
 
@@ -1295,12 +1287,12 @@ Sensors::parameter_update_poll(bool forced)
 				failed = failed || (OK != param_get(param_find(str), &device_id));
 
 				if (failed) {
-					px4_close(fd);
+					DevMgr::releaseHandle(h);
 					continue;
 				}
 
 				/* if the calibration is for this device, apply it */
-				if (device_id == px4_ioctl(fd, DEVIOCGDEVICEID, 0)) {
+				if (device_id == h.ioctl(DEVIOCGDEVICEID, 0)) {
 					struct gyro_scale gscale = {};
 					(void)sprintf(str, "CAL_GYRO%u_XOFF", i);
 					failed = failed || (OK != param_get(param_find(str), &gscale.x_offset));
@@ -1320,7 +1312,7 @@ Sensors::parameter_update_poll(bool forced)
 
 					} else {
 						/* apply new scaling and offsets */
-						res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale);
+						res = h.ioctl(GYROIOCSSCALE, (long unsigned int)&gscale);
 
 						if (res) {
 							warnx(CAL_ERROR_APPLY_CAL_MSG, "gyro", i);
@@ -1337,8 +1329,6 @@ Sensors::parameter_update_poll(bool forced)
 			if (config_ok) {
 				gyro_count++;
 			}
-
-			px4_close(fd);
 		}
 
 		/* run through all accel sensors */
@@ -1347,9 +1337,10 @@ Sensors::parameter_update_poll(bool forced)
 			res = ERROR;
 			(void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
 
-			int fd = px4_open(str, 0);
+			DevHandle h;
+			DevMgr::getHandle(str, h);
 
-			if (fd < 0) {
+			if (!h.isValid()) {
 				continue;
 			}
 
@@ -1365,12 +1356,12 @@ Sensors::parameter_update_poll(bool forced)
 				failed = failed || (OK != param_get(param_find(str), &device_id));
 
 				if (failed) {
-					px4_close(fd);
+					DevMgr::releaseHandle(h);
 					continue;
 				}
 
 				/* if the calibration is for this device, apply it */
-				if (device_id == px4_ioctl(fd, DEVIOCGDEVICEID, 0)) {
+				if (device_id == h.ioctl(DEVIOCGDEVICEID, 0)) {
 					struct accel_scale gscale = {};
 					(void)sprintf(str, "CAL_ACC%u_XOFF", i);
 					failed = failed || (OK != param_get(param_find(str), &gscale.x_offset));
@@ -1390,7 +1381,7 @@ Sensors::parameter_update_poll(bool forced)
 
 					} else {
 						/* apply new scaling and offsets */
-						res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&gscale);
+						res = h.ioctl(ACCELIOCSSCALE, (long unsigned int)&gscale);
 
 						if (res) {
 							warnx(CAL_ERROR_APPLY_CAL_MSG, "accel", i);
@@ -1407,8 +1398,6 @@ Sensors::parameter_update_poll(bool forced)
 			if (config_ok) {
 				accel_count++;
 			}
-
-			px4_close(fd);
 		}
 
 		/* run through all mag sensors */
@@ -1423,9 +1412,10 @@ Sensors::parameter_update_poll(bool forced)
 			res = ERROR;
 			(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s);
 
-			int fd = px4_open(str, 0);
+			DevHandle h;
+			DevMgr::getHandle(str, h);
 
-			if (fd < 0) {
+			if (!h.isValid()) {
 				/* the driver is not running, abort */
 				continue;
 			}
@@ -1444,12 +1434,12 @@ Sensors::parameter_update_poll(bool forced)
 				(void)param_find(str);
 
 				if (failed) {
-					px4_close(fd);
+					DevMgr::releaseHandle(h);
 					continue;
 				}
 
 				/* if the calibration is for this device, apply it */
-				if (device_id == px4_ioctl(fd, DEVIOCGDEVICEID, 0)) {
+				if (device_id == h.ioctl(DEVIOCGDEVICEID, 0)) {
 					struct mag_scale gscale = {};
 					(void)sprintf(str, "CAL_MAG%u_XOFF", i);
 					failed = failed || (OK != param_get(param_find(str), &gscale.x_offset));
@@ -1466,7 +1456,7 @@ Sensors::parameter_update_poll(bool forced)
 
 					(void)sprintf(str, "CAL_MAG%u_ROT", i);
 
-					if (px4_ioctl(fd, MAGIOCGEXTERNAL, 0) <= 0) {
+					if (h.ioctl(MAGIOCGEXTERNAL, 0) <= 0) {
 						/* mag is internal */
 						_mag_rotation[s] = _board_rotation;
 						/* reset param to -1 to indicate internal mag */
@@ -1520,7 +1510,7 @@ Sensors::parameter_update_poll(bool forced)
 
 					} else {
 						/* apply new scaling and offsets */
-						res = px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&gscale);
+						res = h.ioctl(MAGIOCSSCALE, (long unsigned int)&gscale);
 
 						if (res) {
 							warnx(CAL_ERROR_APPLY_CAL_MSG, "mag", i);
@@ -1537,8 +1527,6 @@ Sensors::parameter_update_poll(bool forced)
 			if (config_ok) {
 				mag_count++;
 			}
-
-			px4_close(fd);
 		}
 
 		int fd = px4_open(AIRSPEED0_DEVICE_PATH, 0);

src/platforms/posix/drivers/gpssim/gpssim.cpp

@@ -62,6 +62,11 @@
 
 #include <simulator/simulator.h>
 
+#include "DevMgr.hpp"
+#include "VirtDevObj.hpp"
+
+using namespace DriverFramework;
+
 #define GPS_DRIVER_MODE_UBX_SIM
 #define GPSSIM_DEVICE_PATH "/dev/gpssim"
 
@@ -82,7 +87,7 @@ public:
 };
 
 
-class GPSSIM : public device::VDev
+class GPSSIM : public VirtDevObj
 {
 public:
 	GPSSIM(const char *uart_path, bool fake_gps, bool enable_sat_info);
@@ -90,13 +95,16 @@ public:
 
 	virtual int			init();
 
-	virtual int			ioctl(device::file_t *filp, int cmd, unsigned long arg);
+	virtual int			devIOCTL(unsigned long cmd, unsigned long arg);
 
 	/**
 	 * Diagnostics - print some basic information about the driver.
 	 */
 	void				print_info();
 
+protected:
+	virtual void			_measure() {}
+
 private:
 
 	bool				_task_should_exit;				///< flag to make the main worker task exit
@@ -115,6 +123,7 @@ private:
 	orb_advert_t			_report_sat_info_pub;				///< uORB pub for satellite info
 	float				_rate;						///< position update rate
 	bool				_fake_gps;					///< fake gps output
+	SyncObj				_sync;
 
 	/**
 	 * Try to configure the GPS, handle outgoing communication to the GPS
@@ -160,7 +169,7 @@ GPSSIM	*g_dev = nullptr;
 
 
 GPSSIM::GPSSIM(const char *uart_path, bool fake_gps, bool enable_sat_info) :
-	VDev("gps", GPSSIM_DEVICE_PATH),
+	VirtDevObj("gps", GPSSIM_DEVICE_PATH, nullptr, 0),
 	_task_should_exit(false),
 	//_healthy(false),
 	//_mode_changed(false),
@@ -188,8 +197,6 @@ GPSSIM::GPSSIM(const char *uart_path, bool fake_gps, bool enable_sat_info) :
 		_p_report_sat_info = &_Sat_Info->_data;
 		memset(_p_report_sat_info, 0, sizeof(*_p_report_sat_info));
 	}
-
-	_debug_enabled = true;
 }
 
 GPSSIM::~GPSSIM()
@@ -217,7 +224,7 @@ GPSSIM::init()
 	int ret = ERROR;
 
 	/* do regular cdev init */
-	if (VDev::init() != OK) {
+	if (VirtDevObj::init() != OK) {
 		goto out;
 	}
 
@@ -236,9 +243,9 @@ out:
 }
 
 int
-GPSSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg)
+GPSSIM::devIOCTL(unsigned long cmd, unsigned long arg)
 {
-	lock();
+	_sync.lock();
 
 	int ret = OK;
 
@@ -249,11 +256,11 @@ GPSSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 
 	default:
 		/* give it to parent if no one wants it */
-		ret = VDev::ioctl(filp, cmd, arg);
+		ret = VirtDevObj::devIOCTL(cmd, arg);
 		break;
 	}
 
-	unlock();
+	_sync.unlock();
 
 	return ret;
 }
@@ -319,7 +326,7 @@ GPSSIM::task_main()
 
 			//no time and satellite information simulated
 
-			if (!(_pub_blocked)) {
+			if (!(m_pub_blocked)) {
 				if (_report_gps_pos_pub != nullptr) {
 					orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
 
@@ -339,7 +346,7 @@ GPSSIM::task_main()
 			_report_gps_pos.timestamp_velocity = hrt_absolute_time();
 			_report_gps_pos.timestamp_time = hrt_absolute_time();
 
-			if (!(_pub_blocked)) {
+			if (!(m_pub_blocked)) {
 				if (_report_gps_pos_pub != nullptr) {
 					orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
 
@@ -354,7 +361,7 @@ GPSSIM::task_main()
 			while ((receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) {
 				/* opportunistic publishing - else invalid data would end up on the bus */
 
-				if (!(_pub_blocked)) {
+				if (!(m_pub_blocked)) {
 					orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
 
 					if (_p_report_sat_info) {
@@ -368,7 +375,8 @@ GPSSIM::task_main()
 				}
 			}
 
-			lock();
+			// FIXME - if ioctl is called then it will deadlock
+			_sync.lock();
 		}
 	}
 
@@ -440,7 +448,7 @@ void	info();
 void
 start(const char *uart_path, bool fake_gps, bool enable_sat_info)
 {
-	int fd;
+	DevHandle h;
 
 	/* create the driver */
 	g_dev = new GPSSIM(uart_path, fake_gps, enable_sat_info);
@@ -454,10 +462,10 @@ start(const char *uart_path, bool fake_gps, bool enable_sat_info)
 	}
 
 	/* set the poll rate to default, starts automatic data collection */
-	fd = px4_open(GPSSIM_DEVICE_PATH, O_RDONLY);
+	DevMgr::getHandle(GPSSIM_DEVICE_PATH, h);
 
-	if (fd < 0) {
+	if (!h.isValid()) {
-		PX4_ERR("open: %s\n", GPS0_DEVICE_PATH);
+		PX4_ERR("getHandle failed: %s", GPS0_DEVICE_PATH);
 		goto fail;
 	}
 
@@ -500,13 +508,13 @@ test()
 void
 reset()
 {
-	int fd = px4_open(GPSSIM_DEVICE_PATH, O_RDONLY);
+	DevHandle h;
-
+	DevMgr::getHandle(GPSSIM_DEVICE_PATH, h);
-	if (fd < 0) {
+	if (!h.isValid()) {
 		PX4_ERR("failed ");
 	}
 
-	if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
+	if (h.ioctl(SENSORIOCRESET, 0) < 0) {
 		PX4_ERR("reset failed");
 	}
 }

src/platforms/posix/drivers/ledsim/CMakeLists.txt

@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+px4_add_module(
+	MODULE platforms__posix__drivers__ledsim
+	COMPILE_FLAGS
+		-Os
+	SRCS
+		led.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/platforms/posix/drivers/ledsim/led.cpp

@@ -0,0 +1,141 @@
+/****************************************************************************
+ *
+ *   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 led.cpp
+ *
+ * LED driver.
+ */
+
+#include <px4_config.h>
+#include <drivers/drv_led.h>
+#include <stdio.h>
+
+#include "VirtDevObj.hpp"
+
+using namespace DriverFramework;
+
+#define DEVICE_DEBUG PX4_INFO
+
+/*
+ * Ideally we'd be able to get these from up_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
+__BEGIN_DECLS
+extern void led_init();
+extern void led_on(int led);
+extern void led_off(int led);
+extern void led_toggle(int led);
+__END_DECLS
+
+class LED : public VirtDevObj
+{
+public:
+	LED();
+	virtual ~LED();
+
+	virtual int		init();
+	virtual int		devIOCTL(unsigned long cmd, unsigned long arg);
+
+protected:
+	virtual void		_measure() {}
+};
+
+LED::LED() :
+	VirtDevObj("led", "/dev/ledsim", LED_BASE_DEVICE_PATH, 0)
+{
+	// force immediate init/device registration
+	init();
+}
+
+LED::~LED()
+{
+}
+
+int
+LED::init()
+{
+	DEVICE_DEBUG("LED::init");
+	int ret = VirtDevObj::init();
+	if (ret == 0) {
+		led_init();
+	}
+
+	return ret;
+}
+
+int
+LED::devIOCTL(unsigned long cmd, unsigned long arg)
+{
+	int result = OK;
+
+	switch (cmd) {
+	case LED_ON:
+		led_on(arg);
+		break;
+
+	case LED_OFF:
+		led_off(arg);
+		break;
+
+	case LED_TOGGLE:
+		led_toggle(arg);
+		break;
+
+
+	default:
+		result = VirtDevObj::devIOCTL(cmd, arg);
+	}
+
+	return result;
+}
+
+namespace
+{
+LED	*gLED;
+}
+
+void
+drv_led_start(void)
+{
+	if (gLED == nullptr) {
+		gLED = new LED;
+
+		if (gLED != nullptr) {
+			gLED->init();
+		}
+	}
+}

src/platforms/posix/drivers/rgbledsim/CMakeLists.txt

@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+px4_add_module(
+	MODULE platforms__posix__drivers__rgbledsim
+	MAIN rgbledsim
+	COMPILE_FLAGS
+		-Os
+	SRCS
+		rgbled.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/platforms/posix/drivers/rgbledsim/rgbled.cpp

@@ -0,0 +1,801 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2015 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 rgbled.cpp
+ *
+ * Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C.
+ *
+ * @author Julian Oes <julian@px4.io>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <px4_config.h>
+#include <px4_getopt.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 <px4_workqueue.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+#include <board_config.h>
+
+#include <drivers/drv_rgbled.h>
+
+#include "VirtDevObj.hpp"
+
+using namespace DriverFramework;
+
+#define DEVICE_LOG PX4_INFO
+#define RGBLED_ONTIME 120
+#define RGBLED_OFFTIME 120
+
+#define ADDR			PX4_I2C_OBDEV_LED	/**< I2C adress of TCA62724FMG */
+#define SUB_ADDR_START		0x01	/**< write everything (with auto-increment) */
+#define SUB_ADDR_PWM0		0x81	/**< blue     (without auto-increment) */
+#define SUB_ADDR_PWM1		0x82	/**< green    (without auto-increment) */
+#define SUB_ADDR_PWM2		0x83	/**< red      (without auto-increment) */
+#define SUB_ADDR_SETTINGS	0x84	/**< settings (without auto-increment)*/
+
+#define SETTING_NOT_POWERSAVE	0x01	/**< power-save mode not off */
+#define SETTING_ENABLE   	0x02	/**< on */
+
+
+class RGBLEDSIM : public VirtDevObj
+{
+public:
+	RGBLEDSIM(int bus, int rgbled);
+	virtual ~RGBLEDSIM();
+
+
+	virtual int		init();
+	virtual int		probe();
+	virtual int		info();
+	virtual int		devIOCTL(unsigned long cmd, unsigned long arg);
+
+	// Don't use the default periodic callback
+	virtual void 		_measure() {}
+
+private:
+	work_s			_work;
+
+	rgbled_mode_t		_mode;
+	rgbled_pattern_t	_pattern;
+
+	uint8_t			_r;
+	uint8_t			_g;
+	uint8_t			_b;
+	float			_brightness;
+	float			_max_brightness;
+
+	bool			_running;
+	int			_led_interval;
+	bool			_should_run;
+	int			_counter;
+	int			_param_sub;
+
+	void 			set_color(rgbled_color_t ledcolor);
+	void			set_mode(rgbled_mode_t mode);
+	void			set_pattern(rgbled_pattern_t *pattern);
+
+	static void		led_trampoline(void *arg);
+	void			led();
+
+	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);
+	void			update_params();
+
+	/**
+	 * Perform an I2C transaction to the device.
+	 *
+	 * At least one of send_len and recv_len must be non-zero.
+	 *
+	 * @param send		Pointer to bytes to send.
+	 * @param send_len	Number of bytes to send.
+	 * @param recv		Pointer to buffer for bytes received.
+	 * @param recv_len	Number of bytes to receive.
+	 * @return		OK if the transfer was successful, -errno
+	 *			otherwise.
+	 */
+	int		transfer(const uint8_t *send, unsigned send_len,
+				 uint8_t *recv, unsigned recv_len);
+
+};
+
+/* for now, we only support one RGBLEDSIM */
+namespace
+{
+RGBLEDSIM *g_rgbled = nullptr;
+}
+
+void rgbled_usage();
+
+extern "C" __EXPORT int rgbledsim_main(int argc, char *argv[]);
+
+RGBLEDSIM::RGBLEDSIM(int bus, int rgbled) :
+	VirtDevObj("rgbled", "/dev/rgbledsim", RGBLED_BASE_DEVICE_PATH, 0),
+	_mode(RGBLED_MODE_OFF),
+	_r(0),
+	_g(0),
+	_b(0),
+	_brightness(1.0f),
+	_max_brightness(1.0f),
+	_running(false),
+	_led_interval(0),
+	_should_run(false),
+	_counter(0),
+	_param_sub(-1)
+{
+	memset(&_work, 0, sizeof(_work));
+	memset(&_pattern, 0, sizeof(_pattern));
+}
+
+RGBLEDSIM::~RGBLEDSIM()
+{
+}
+
+int
+RGBLEDSIM::init()
+{
+	int ret;
+	ret = VirtDevObj::init();
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	/* switch off LED on start */
+	send_led_enable(false);
+	send_led_rgb();
+
+	return OK;
+}
+
+int
+RGBLEDSIM::probe()
+{
+	int ret;
+	bool on, powersave;
+	uint8_t r, g, b;
+
+	/**
+	   this may look strange, but is needed. There is a serial
+	   EEPROM (Microchip-24aa01) on the PX4FMU-v1 that responds to
+	   a bunch of I2C addresses, including the 0x55 used by this
+	   LED device. So we need to do enough operations to be sure
+	   we are talking to the right device. These 3 operations seem
+	   to be enough, as the 3rd one consistently fails if no
+	   RGBLEDSIM is on the bus.
+	 */
+
+	if ((ret = get(on, powersave, r, g, b)) != OK ||
+	    (ret = send_led_enable(false) != OK) ||
+	    (ret = send_led_enable(false) != OK)) {
+		return ret;
+	}
+
+	return ret;
+}
+
+int
+RGBLEDSIM::info()
+{
+	int ret;
+	bool on, powersave;
+	uint8_t r, g, b;
+
+	ret = get(on, powersave, r, g, b);
+
+	if (ret == OK) {
+		/* we don't care about power-save mode */
+		DEVICE_LOG("state: %s", on ? "ON" : "OFF");
+		DEVICE_LOG("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b);
+
+	} else {
+		PX4_WARN("failed to read led");
+	}
+
+	return ret;
+}
+
+int
+RGBLEDSIM::devIOCTL(unsigned long cmd, unsigned long arg)
+{
+	int ret = ENOTTY;
+
+	switch (cmd) {
+	case RGBLED_SET_RGB:
+		/* 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 mode */
+		set_mode((rgbled_mode_t)arg);
+		return OK;
+
+	case RGBLED_SET_PATTERN:
+		/* set a special pattern */
+		set_pattern((rgbled_pattern_t *)arg);
+		return OK;
+
+	default:
+		/* see if the parent class can make any use of it */
+		ret = VirtDevObj::devIOCTL(cmd, arg);
+		break;
+	}
+
+	return ret;
+}
+
+
+void
+RGBLEDSIM::led_trampoline(void *arg)
+{
+	RGBLEDSIM *rgbl = reinterpret_cast<RGBLEDSIM *>(arg);
+
+	rgbl->led();
+}
+
+/**
+ * Main loop function
+ */
+void
+RGBLEDSIM::led()
+{
+	if (!_should_run) {
+		_running = false;
+		return;
+	}
+
+	if (_param_sub < 0) {
+		_param_sub = orb_subscribe(ORB_ID(parameter_update));
+	}
+
+	if (_param_sub >= 0) {
+		bool updated = false;
+		orb_check(_param_sub, &updated);
+
+		if (updated) {
+			parameter_update_s pupdate;
+			orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate);
+			update_params();
+			// Immediately update to change brightness
+			send_led_rgb();
+		}
+	}
+
+	switch (_mode) {
+	case RGBLED_MODE_BLINK_SLOW:
+	case RGBLED_MODE_BLINK_NORMAL:
+	case RGBLED_MODE_BLINK_FAST:
+		if (_counter >= 2) {
+			_counter = 0;
+		}
+
+		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++;
+}
+
+/**
+ * Parse color constant and set _r _g _b values
+ */
+void
+RGBLEDSIM::set_color(rgbled_color_t color)
+{
+	switch (color) {
+	case RGBLED_COLOR_OFF:
+		_r = 0;
+		_g = 0;
+		_b = 0;
+		break;
+
+	case RGBLED_COLOR_RED:
+		_r = 255;
+		_g = 0;
+		_b = 0;
+		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:
+		PX4_WARN("color unknown");
+		break;
+	}
+}
+
+/**
+ * Set mode, if mode not changed has no any effect (doesn't reset blinks phase)
+ */
+void
+RGBLEDSIM::set_mode(rgbled_mode_t mode)
+{
+	if (mode != _mode) {
+		_mode = mode;
+
+		switch (mode) {
+		case RGBLED_MODE_OFF:
+			_should_run = false;
+			send_led_enable(false);
+			break;
+
+		case RGBLED_MODE_ON:
+			_brightness = 1.0f;
+			send_led_rgb();
+			send_led_enable(true);
+			break;
+
+		case RGBLED_MODE_BLINK_SLOW:
+			_should_run = true;
+			_counter = 0;
+			_led_interval = 2000;
+			_brightness = 1.0f;
+			send_led_rgb();
+			break;
+
+		case RGBLED_MODE_BLINK_NORMAL:
+			_should_run = true;
+			_counter = 0;
+			_led_interval = 500;
+			_brightness = 1.0f;
+			send_led_rgb();
+			break;
+
+		case RGBLED_MODE_BLINK_FAST:
+			_should_run = true;
+			_counter = 0;
+			_led_interval = 100;
+			_brightness = 1.0f;
+			send_led_rgb();
+			break;
+
+		case RGBLED_MODE_BREATHE:
+			_should_run = true;
+			_counter = 0;
+			_led_interval = 25;
+			send_led_enable(true);
+			break;
+
+		case RGBLED_MODE_PATTERN:
+			_should_run = true;
+			_counter = 0;
+			_brightness = 1.0f;
+			send_led_enable(true);
+			break;
+
+		default:
+			PX4_WARN("mode unknown");
+			break;
+		}
+
+		/* if it should run now, start the workq */
+		if (_should_run && !_running) {
+			_running = true;
+			work_queue(LPWORK, &_work, (worker_t)&RGBLEDSIM::led_trampoline, this, 1);
+		}
+
+	}
+}
+
+/**
+ * Set pattern for PATTERN mode, but don't change current mode
+ */
+void
+RGBLEDSIM::set_pattern(rgbled_pattern_t *pattern)
+{
+	memcpy(&_pattern, pattern, sizeof(rgbled_pattern_t));
+}
+
+/**
+ * Sent ENABLE flag to LED driver
+ */
+int
+RGBLEDSIM::send_led_enable(bool enable)
+{
+	uint8_t settings_byte = 0;
+
+	if (enable) {
+		settings_byte |= SETTING_ENABLE;
+	}
+
+	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
+RGBLEDSIM::send_led_rgb()
+{
+	/* To scale from 0..255 -> 0..15 shift right by 4 bits */
+	const uint8_t msg[6] = {
+		SUB_ADDR_PWM0, static_cast<uint8_t>((_b >> 4) * _brightness * _max_brightness + 0.5f),
+		SUB_ADDR_PWM1, static_cast<uint8_t>((_g >> 4) * _brightness * _max_brightness + 0.5f),
+		SUB_ADDR_PWM2, static_cast<uint8_t>((_r >> 4) * _brightness * _max_brightness + 0.5f)
+	};
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+RGBLEDSIM::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b)
+{
+	uint8_t result[2];
+	int ret;
+
+	ret = transfer(nullptr, 0, &result[0], 2);
+
+	if (ret == OK) {
+		on = result[0] & SETTING_ENABLE;
+		powersave = !(result[0] & SETTING_NOT_POWERSAVE);
+		/* XXX check, looks wrong */
+		r = (result[0] & 0x0f) << 4;
+		g = (result[1] & 0xf0);
+		b = (result[1] & 0x0f) << 4;
+	}
+
+	return ret;
+}
+
+void
+RGBLEDSIM::update_params()
+{
+	int32_t maxbrt = 15;
+	param_get(param_find("LED_RGB_MAXBRT"), &maxbrt);
+	maxbrt = maxbrt > 15 ? 15 : maxbrt;
+	maxbrt = maxbrt <  0 ?  0 : maxbrt;
+
+	// A minimum of 2 "on" steps is required for breathe effect
+	if (maxbrt == 1) {
+		maxbrt = 2;
+	}
+
+	_max_brightness = maxbrt / 15.0f;
+}
+
+void
+rgbled_usage()
+{
+	PX4_WARN("missing command: try 'start', 'test', 'info', 'off', 'stop', 'rgb 30 40 50'");
+	PX4_WARN("options:");
+	PX4_WARN("    -b i2cbus (%d)", PX4_I2C_BUS_LED);
+	PX4_WARN("    -a addr (0x%x)", ADDR);
+}
+
+int
+rgbledsim_main(int argc, char *argv[])
+{
+	int i2cdevice = -1;
+	int rgbledadr = ADDR; /* 7bit */
+
+	int ch;
+
+	/* jump over start/off/etc and look at options first */
+	int myoptind = 1;
+	const char *myoptarg = NULL;
+
+	while ((ch = px4_getopt(argc, argv, "a:b:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'a':
+			rgbledadr = strtol(myoptarg, NULL, 0);
+			break;
+
+		case 'b':
+			i2cdevice = strtol(myoptarg, NULL, 0);
+			break;
+
+		default:
+			rgbled_usage();
+			return 1;
+		}
+	}
+
+	if (myoptind >= argc) {
+		rgbled_usage();
+		return 1;
+	}
+
+	const char *verb = argv[myoptind];
+
+	int ret;
+
+	if (!strcmp(verb, "start")) {
+		if (g_rgbled != nullptr) {
+			PX4_WARN("already started");
+			return 1;
+		}
+
+		if (i2cdevice == -1) {
+			// try the external bus first
+			i2cdevice = PX4_I2C_BUS_EXPANSION;
+			g_rgbled = new RGBLEDSIM(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
+				if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) {
+					PX4_WARN("no RGB led on bus #%d", i2cdevice);
+					return 1;
+				}
+
+				i2cdevice = PX4_I2C_BUS_LED;
+			}
+		}
+
+		if (g_rgbled == nullptr) {
+			g_rgbled = new RGBLEDSIM(i2cdevice, rgbledadr);
+
+			if (g_rgbled == nullptr) {
+				PX4_WARN("new failed");
+				return 1;
+			}
+
+			if (OK != g_rgbled->init()) {
+				delete g_rgbled;
+				g_rgbled = nullptr;
+				PX4_WARN("no RGB led on bus #%d", i2cdevice);
+				return 1;
+			}
+		}
+
+		return 0;
+	}
+
+	/* need the driver past this point */
+	if (g_rgbled == nullptr) {
+		PX4_WARN("not started");
+		rgbled_usage();
+		return 1;
+	}
+
+	if (!strcmp(verb, "test")) {
+		DevHandle h;
+		DevMgr::getHandle(RGBLED0_DEVICE_PATH, h);
+
+		if (!h.isValid()) {
+			PX4_WARN("Unable to open " RGBLED0_DEVICE_PATH);
+			return 1;
+		}
+
+		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 = h.ioctl(RGBLED_SET_PATTERN, (unsigned long)&pattern);
+		ret = h.ioctl(RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_PATTERN);
+
+		DevMgr::releaseHandle(h);
+		return ret;
+	}
+
+	if (!strcmp(verb, "info")) {
+		g_rgbled->info();
+		return 0;
+	}
+
+	if (!strcmp(verb, "off") || !strcmp(verb, "stop")) {
+		DevHandle h;
+		DevMgr::getHandle(RGBLED0_DEVICE_PATH, h);
+
+		if (!h.isValid()) {
+			PX4_WARN("Unable to open " RGBLED0_DEVICE_PATH);
+			return 1;
+		}
+
+		ret = h.ioctl(RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF);
+		DevMgr::releaseHandle(h);
+
+		/* delete the rgbled object if stop was requested, in addition to turning off the LED. */
+		if (!strcmp(verb, "stop")) {
+			delete g_rgbled;
+			g_rgbled = nullptr;
+			return 0;
+		}
+
+		return ret;
+	}
+
+	if (!strcmp(verb, "rgb")) {
+		if (argc < 5) {
+			PX4_WARN("Usage: rgbled rgb <red> <green> <blue>");
+			return 1;
+		}
+
+		DevHandle h;
+		DevMgr::getHandle(RGBLED0_DEVICE_PATH, h);
+
+		if (!h.isValid()) {
+			PX4_WARN("Unable to open " RGBLED0_DEVICE_PATH);
+			return 1;
+		}
+
+		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 = h.ioctl(RGBLED_SET_RGB, (unsigned long)&v);
+		ret = h.ioctl(RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON);
+		DevMgr::releaseHandle(h);
+		return ret;
+	}
+
+	rgbled_usage();
+	return 1;
+}
+
+int RGBLEDSIM::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len)
+{
+	return 0;
+}

src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp

@@ -741,17 +741,17 @@ ToneAlarm	*g_dev;
 int
 play_tune(unsigned tune)
 {
-	int	fd, ret;
+	int ret;
 
-	fd = px4_open(TONEALARM0_DEVICE_PATH, 0);
+	DevHandle h;
+	DevMgr::getHandle(TONEALARM0_DEVICE_PATH, h);
 
-	if (fd < 0) {
+	if (!h.isValid()) {
-		PX4_WARN("Error: failed to open %s", TONEALARM0_DEVICE_PATH);
+		PX4_WARN("Error: failed to open %s (%d)", TONEALARM0_DEVICE_PATH, h.getError());
 		return 1;
 	}
 
-	ret = px4_ioctl(fd, TONE_SET_ALARM, tune);
+	ret = h.ioctl(TONE_SET_ALARM, tune);
-	px4_close(fd);
 
 	if (ret != 0) {
 		PX4_WARN("TONE_SET_ALARM");