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..2d8d3a0cfa 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 94abaa2067438c8b3c300c1bec05688cda5ff2e8 +Subproject commit 2d8d3a0cfaffdd833e63c6394eb1c8febb4f301a 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..34552c3d33 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 @@ -136,7 +139,7 @@ int buzzer_init() buzzer = px4_open(TONEALARM0_DEVICE_PATH, O_WRONLY); if (buzzer < 0) { - warnx("Buzzer: px4_open fail\n"); + PX4_WARN("Buzzer: px4_open fail\n"); return ERROR; } @@ -264,30 +267,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 +298,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 +310,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 1d66e45fa1..a82ffb8684 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -926,7 +926,7 @@ Sensors::accel_init() { DevHandle h_accel; DevMgr::getHandle(ACCEL0_DEVICE_PATH, h_accel); - + if (!h_accel.isValid()) { warnx("FATAL: no accelerometer found: %s (%d)", ACCEL0_DEVICE_PATH, h_accel.getError()); return ERROR; @@ -971,7 +971,7 @@ Sensors::mag_init() DevHandle h_mag; DevMgr::getHandle(MAG0_DEVICE_PATH, h_mag); - + if (!h_mag.isValid()) { warnx("FATAL: no magnetometer found: %s (%d)", MAG0_DEVICE_PATH, h_mag.getError()); return ERROR; diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index 1ba06148cb..99a54fc85f 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -349,16 +349,19 @@ ACCELSIM::~ACCELSIM() int ACCELSIM::init() { + PX4_INFO("ACCELSIM::init"); int ret = -1; struct mag_report mrp = {}; struct accel_report arp = {}; + PX4_INFO("ACCELSIM::init before VirtDevObj::init()"); /* do SIM init first */ if (VirtDevObj::init() != 0) { PX4_WARN("SIM init failed"); goto out; } + PX4_INFO("ACCELSIM::init after VirtDevObj::init()"); /* allocate basic report buffers */ _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); 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");