diff --git a/cmake/configs/posix_sitl_simple.cmake b/cmake/configs/posix_sitl_simple.cmake index 581b82e827..4f8514b010 100644 --- a/cmake/configs/posix_sitl_simple.cmake +++ b/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 diff --git a/posix-configs/SITL/init/rcS_jmavsim b/posix-configs/SITL/init/rcS_jmavsim index 878eda3a6e..a893f0dc52 100644 --- a/posix-configs/SITL/init/rcS_jmavsim +++ b/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 diff --git a/src/drivers/drv_led.h b/src/drivers/drv_led.h index dbcfde91fa..0ebf228a2e 100644 --- a/src/drivers/drv_led.h +++ b/src/drivers/drv_led.h @@ -43,6 +43,7 @@ #include #include +#define LED_BASE_DEVICE_PATH "/dev/led" #define LED0_DEVICE_PATH "/dev/led0" #define _LED_BASE 0x2800 diff --git a/src/drivers/drv_rgbled.h b/src/drivers/drv_rgbled.h index 4e3de36abd..2194f8ed97 100644 --- a/src/drivers/drv_rgbled.h +++ b/src/drivers/drv_rgbled.h @@ -42,6 +42,8 @@ #include #include +#define RGBLED_BASE_DEVICE_PATH "/dev/rgbled" + /* more devices will be 1, 2, etc */ #define RGBLED0_DEVICE_PATH "/dev/rgbled0" diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 94abaa2067..737d2cbf2c 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 94abaa2067438c8b3c300c1bec05688cda5ff2e8 +Subproject commit 737d2cbf2c94210b964cf00d55903f1b9c52b2f6 diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 2a8a0695cf..c1cbfad910 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/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); - -int check_calibration(int fd, const char* param_template, int &devid) +static int check_calibration(DevHandle &h, 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; } diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index f411e1c128..a7ca5b231f 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -63,6 +63,9 @@ #include #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) { - warnx("Buzzer: px4_open fail\n"); + if (!h_buzzer.isValid()) { + 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 int rgbleds = -1; +static DevHandle h_leds; +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) { - warnx("LED: px4_open fail\n"); + if (!h_leds.isValid()) { + 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)) { - warnx("Amber LED: px4_ioctl fail\n"); + if (h_leds.ioctl(LED_ON, LED_AMBER)) { + 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) { - warnx("No RGB LED found at " RGBLED0_DEVICE_PATH); + if (!h_rgbleds.isValid()) { + 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) { - px4_close(leds); - } - - if (rgbleds >= 0) { - px4_close(rgbleds); - } + DevMgr::releaseHandle(h_leds); + DevMgr::releaseHandle(h_rgbleds); } int led_toggle(int led) { - if (leds < 0) { - return leds; - } - return px4_ioctl(leds, LED_TOGGLE, led); + return h_leds.ioctl(LED_TOGGLE, led); } int led_on(int led) { - if (leds < 0) { - return leds; - } - return px4_ioctl(leds, LED_ON, led); + return h_leds.ioctl(LED_ON, led); } int led_off(int led) { - if (leds < 0) { - return leds; - } - return px4_ioctl(leds, LED_OFF, led); + return h_leds.ioctl(LED_OFF, led); } void rgbled_set_color(rgbled_color_t color) { - if (rgbleds < 0) { - return; - } - px4_ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color); + h_rgbleds.ioctl(RGBLED_SET_COLOR, (unsigned long)color); } void rgbled_set_mode(rgbled_mode_t mode) { - if (rgbleds < 0) { - return; - } - px4_ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode); + h_rgbleds.ioctl(RGBLED_SET_MODE, (unsigned long)mode); } void rgbled_set_pattern(rgbled_pattern_t *pattern) { - if (rgbleds < 0) { - return; - } - px4_ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); + h_rgbleds.ioctl(RGBLED_SET_PATTERN, (unsigned long)pattern); } unsigned battery_get_n_cells() { diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index ab246e14a1..a82ffb8684 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -97,6 +97,10 @@ #include #include +#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; - barofd = px4_open(BARO0_DEVICE_PATH, 0); + DevHandle h_baro; + DevMgr::getHandle(BARO0_DEVICE_PATH, h_baro); - if (barofd < 0) { - warnx("ERROR: no barometer found on %s", BARO0_DEVICE_PATH); + if (!h_baro.isValid()) { + 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 (fd < 0) { - warnx("FATAL: no accelerometer found: %s", ACCEL0_DEVICE_PATH); + if (!h_accel.isValid()) { + warnx("FATAL: no accelerometer found: %s (%d)", ACCEL0_DEVICE_PATH, h_accel.getError()); 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); - - px4_close(fd); + h_accel.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); } 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 (fd < 0) { - warnx("FATAL: no gyro found: %s", GYRO0_DEVICE_PATH); + if (!h_gyro.isValid()) { + warnx("FATAL: no gyro found: %s (%d)", GYRO0_DEVICE_PATH, h_gyro.getError()); 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); - - px4_close(fd); + h_gyro.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); 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) { - warnx("FATAL: no magnetometer found: %s", MAG0_DEVICE_PATH); + if (!h_mag.isValid()) { + 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 (fd < 0) { - warnx("FATAL: No barometer found: %s", BARO0_DEVICE_PATH); + if (!h_baro.isValid()) { + warnx("FATAL: No barometer found: %s (%d)", BARO0_DEVICE_PATH, h_baro.getError()); return ERROR; } /* set the driver to poll at 150Hz */ - px4_ioctl(fd, SENSORIOCSPOLLRATE, 150); - - px4_close(fd); + h_baro.ioctl(SENSORIOCSPOLLRATE, 150); 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) { - warnx("FATAL: no ADC found: %s", ADC0_DEVICE_PATH); + if (!h_adc.isValid()) { + 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); diff --git a/src/platforms/posix/drivers/gpssim/gpssim.cpp b/src/platforms/posix/drivers/gpssim/gpssim.cpp index 889d793486..20e929ce88 100644 --- a/src/platforms/posix/drivers/gpssim/gpssim.cpp +++ b/src/platforms/posix/drivers/gpssim/gpssim.cpp @@ -62,6 +62,11 @@ #include +#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) { - PX4_ERR("open: %s\n", GPS0_DEVICE_PATH); + if (!h.isValid()) { + 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); - - if (fd < 0) { + DevHandle h; + DevMgr::getHandle(GPSSIM_DEVICE_PATH, h); + if (!h.isValid()) { PX4_ERR("failed "); } - if (ioctl(fd, SENSORIOCRESET, 0) < 0) { + if (h.ioctl(SENSORIOCRESET, 0) < 0) { PX4_ERR("reset failed"); } } diff --git a/src/platforms/posix/drivers/ledsim/CMakeLists.txt b/src/platforms/posix/drivers/ledsim/CMakeLists.txt new file mode 100644 index 0000000000..0f20b87988 --- /dev/null +++ b/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 : diff --git a/src/platforms/posix/drivers/ledsim/led.cpp b/src/platforms/posix/drivers/ledsim/led.cpp new file mode 100644 index 0000000000..f1d1021e65 --- /dev/null +++ b/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 +#include +#include + +#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(); + } + } +} diff --git a/src/platforms/posix/drivers/rgbledsim/CMakeLists.txt b/src/platforms/posix/drivers/rgbledsim/CMakeLists.txt new file mode 100644 index 0000000000..edb2392a3a --- /dev/null +++ b/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 : diff --git a/src/platforms/posix/drivers/rgbledsim/rgbled.cpp b/src/platforms/posix/drivers/rgbledsim/rgbled.cpp new file mode 100644 index 0000000000..bf839fbb52 --- /dev/null +++ b/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 + * @author Anton Babushkin + */ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include + +#include + +#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(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((_b >> 4) * _brightness * _max_brightness + 0.5f), + SUB_ADDR_PWM1, static_cast((_g >> 4) * _brightness * _max_brightness + 0.5f), + SUB_ADDR_PWM2, static_cast((_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 "); + 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; +} diff --git a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp index 6adffb9bf9..a30c07303d 100644 --- a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp +++ b/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) { - PX4_WARN("Error: failed to open %s", TONEALARM0_DEVICE_PATH); + if (!h.isValid()) { + PX4_WARN("Error: failed to open %s (%d)", TONEALARM0_DEVICE_PATH, h.getError()); return 1; } - ret = px4_ioctl(fd, TONE_SET_ALARM, tune); - px4_close(fd); + ret = h.ioctl(TONE_SET_ALARM, tune); if (ret != 0) { PX4_WARN("TONE_SET_ALARM");