From 4b1bbaa11469824c0766fc0ed21ea69628459be6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 13 Feb 2017 11:18:42 +0100 Subject: [PATCH] posix: remove rgbledsim driver which does nothing In addition this is almost a copy of the rgbled driver --- cmake/configs/posix_sitl_default.cmake | 1 - posix-configs/SITL/init/ekf2/iris | 1 - posix-configs/SITL/init/ekf2/iris_opt_flow | 1 - posix-configs/SITL/init/ekf2/multiple_iris | 1 - posix-configs/SITL/init/ekf2/plane | 1 - posix-configs/SITL/init/ekf2/solo | 1 - posix-configs/SITL/init/ekf2/standard_vtol | 1 - posix-configs/SITL/init/ekf2/tailsitter | 1 - posix-configs/SITL/init/ekf2/typhoon_h480 | 1 - posix-configs/SITL/init/inav/iris | 1 - posix-configs/SITL/init/inav/iris_opt_flow | 1 - posix-configs/SITL/init/lpe/iris | 1 - posix-configs/SITL/init/lpe/iris_opt_flow | 1 - posix-configs/SITL/init/lpe/plane | 1 - posix-configs/SITL/init/lpe/solo | 1 - posix-configs/SITL/init/lpe/standard_vtol | 1 - posix-configs/SITL/init/lpe/typhoon_h480 | 1 - posix-configs/SITL/init/rcS_gazebo_delta_wing | 1 - posix-configs/SITL/init/shell/iris | 1 - posix-configs/SITL/init/test/test_template.in | 1 - posix-configs/SITL/init/test/tests_all | 1 - .../posix/drivers/rgbledsim/CMakeLists.txt | 42 - .../posix/drivers/rgbledsim/rgbled.cpp | 802 ------------------ 23 files changed, 865 deletions(-) delete mode 100644 src/platforms/posix/drivers/rgbledsim/CMakeLists.txt delete mode 100644 src/platforms/posix/drivers/rgbledsim/rgbled.cpp diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake index 52d947df68..08955dd621 100644 --- a/cmake/configs/posix_sitl_default.cmake +++ b/cmake/configs/posix_sitl_default.cmake @@ -17,7 +17,6 @@ set(config_module_list platforms/posix/drivers/gpssim platforms/posix/drivers/gyrosim platforms/posix/drivers/ledsim - platforms/posix/drivers/rgbledsim platforms/posix/drivers/tonealrmsim # diff --git a/posix-configs/SITL/init/ekf2/iris b/posix-configs/SITL/init/ekf2/iris index 60e0229e1b..4382ad79ef 100644 --- a/posix-configs/SITL/init/ekf2/iris +++ b/posix-configs/SITL/init/ekf2/iris @@ -45,7 +45,6 @@ param set EKF2_ANGERR_INIT 0.01 param set EKF2_MAG_TYPE 1 replay tryapplyparams simulator start -s -rgbledsim start tone_alarm start gyrosim start accelsim start diff --git a/posix-configs/SITL/init/ekf2/iris_opt_flow b/posix-configs/SITL/init/ekf2/iris_opt_flow index f55a783a92..3d3f123dda 100644 --- a/posix-configs/SITL/init/ekf2/iris_opt_flow +++ b/posix-configs/SITL/init/ekf2/iris_opt_flow @@ -47,7 +47,6 @@ param set EKF2_AID_MASK 2 param set EKF2_HGT_MODE 2 replay tryapplyparams simulator start -s -rgbledsim start tone_alarm start gyrosim start accelsim start diff --git a/posix-configs/SITL/init/ekf2/multiple_iris b/posix-configs/SITL/init/ekf2/multiple_iris index 1af538e9d0..6ee4f37452 100644 --- a/posix-configs/SITL/init/ekf2/multiple_iris +++ b/posix-configs/SITL/init/ekf2/multiple_iris @@ -45,7 +45,6 @@ param set EKF2_ANGERR_INIT 0.01 param set EKF2_MAG_TYPE 1 replay tryapplyparams simulator start -s -u _SIMPORT_ -rgbledsim start tone_alarm start gyrosim start accelsim start diff --git a/posix-configs/SITL/init/ekf2/plane b/posix-configs/SITL/init/ekf2/plane index 4a367861bc..569ce80e4d 100644 --- a/posix-configs/SITL/init/ekf2/plane +++ b/posix-configs/SITL/init/ekf2/plane @@ -51,7 +51,6 @@ param set RWTO_TKOFF 1 replay tryapplyparams simulator start -s -rgbledsim start tone_alarm start gyrosim start accelsim start diff --git a/posix-configs/SITL/init/ekf2/solo b/posix-configs/SITL/init/ekf2/solo index 16370bca8d..a35401cae6 100644 --- a/posix-configs/SITL/init/ekf2/solo +++ b/posix-configs/SITL/init/ekf2/solo @@ -42,7 +42,6 @@ param set EKF2_ANGERR_INIT 0.01 param set EKF2_MAG_TYPE 1 replay tryapplyparams simulator start -s -rgbledsim start tone_alarm start gyrosim start accelsim start diff --git a/posix-configs/SITL/init/ekf2/standard_vtol b/posix-configs/SITL/init/ekf2/standard_vtol index 653baf9d87..971b8b58cd 100644 --- a/posix-configs/SITL/init/ekf2/standard_vtol +++ b/posix-configs/SITL/init/ekf2/standard_vtol @@ -59,7 +59,6 @@ param set EKF2_ANGERR_INIT 0.01 param set EKF2_MAG_TYPE 1 replay tryapplyparams simulator start -s -rgbledsim start tone_alarm start gyrosim start accelsim start diff --git a/posix-configs/SITL/init/ekf2/tailsitter b/posix-configs/SITL/init/ekf2/tailsitter index 83ed7b3e2b..c38c4accde 100644 --- a/posix-configs/SITL/init/ekf2/tailsitter +++ b/posix-configs/SITL/init/ekf2/tailsitter @@ -41,7 +41,6 @@ param set EKF2_ANGERR_INIT 0.01 param set EKF2_MAG_TYPE 1 replay tryapplyparams simulator start -s -rgbledsim start tone_alarm start gyrosim start accelsim start diff --git a/posix-configs/SITL/init/ekf2/typhoon_h480 b/posix-configs/SITL/init/ekf2/typhoon_h480 index 7ad337421e..d278ac9759 100644 --- a/posix-configs/SITL/init/ekf2/typhoon_h480 +++ b/posix-configs/SITL/init/ekf2/typhoon_h480 @@ -44,7 +44,6 @@ param set EKF2_ANGERR_INIT 0.01 param set EKF2_MAG_TYPE 1 replay tryapplyparams simulator start -s -rgbledsim start tone_alarm start gyrosim start accelsim start diff --git a/posix-configs/SITL/init/inav/iris b/posix-configs/SITL/init/inav/iris index 98ad73dfb9..0cc89f3326 100644 --- a/posix-configs/SITL/init/inav/iris +++ b/posix-configs/SITL/init/inav/iris @@ -45,7 +45,6 @@ param set COM_RC_IN_MODE 1 param set NAV_DLL_ACT 2 replay tryapplyparams simulator start -s -rgbledsim start tone_alarm start gyrosim start accelsim start diff --git a/posix-configs/SITL/init/inav/iris_opt_flow b/posix-configs/SITL/init/inav/iris_opt_flow index 7c249cf5e6..c0c9c29890 100644 --- a/posix-configs/SITL/init/inav/iris_opt_flow +++ b/posix-configs/SITL/init/inav/iris_opt_flow @@ -48,7 +48,6 @@ param set INAV_W_XY_GPS_V 0.0 param set INAV_W_Z_GPS_P 0.0 replay tryapplyparams simulator start -s -rgbledsim start tone_alarm start gyrosim start accelsim start diff --git a/posix-configs/SITL/init/lpe/iris b/posix-configs/SITL/init/lpe/iris index 621a24848d..095b2ad196 100644 --- a/posix-configs/SITL/init/lpe/iris +++ b/posix-configs/SITL/init/lpe/iris @@ -47,7 +47,6 @@ param set LPE_FUSION 247 replay tryapplyparams simulator start -s -rgbledsim start tone_alarm start gyrosim start accelsim start diff --git a/posix-configs/SITL/init/lpe/iris_opt_flow b/posix-configs/SITL/init/lpe/iris_opt_flow index e652bafca4..4869f246bb 100644 --- a/posix-configs/SITL/init/lpe/iris_opt_flow +++ b/posix-configs/SITL/init/lpe/iris_opt_flow @@ -65,7 +65,6 @@ param set LPE_FUSION 246 replay tryapplyparams simulator start -s -rgbledsim start tone_alarm start gyrosim start accelsim start diff --git a/posix-configs/SITL/init/lpe/plane b/posix-configs/SITL/init/lpe/plane index 6f1c4d5ae2..1d47faffe6 100644 --- a/posix-configs/SITL/init/lpe/plane +++ b/posix-configs/SITL/init/lpe/plane @@ -51,7 +51,6 @@ param set RWTO_TKOFF 1 replay tryapplyparams simulator start -s -rgbledsim start tone_alarm start gyrosim start accelsim start diff --git a/posix-configs/SITL/init/lpe/solo b/posix-configs/SITL/init/lpe/solo index f168f398e3..6bcfe544b3 100644 --- a/posix-configs/SITL/init/lpe/solo +++ b/posix-configs/SITL/init/lpe/solo @@ -47,7 +47,6 @@ param set LPE_FUSION 247 replay tryapplyparams simulator start -s -rgbledsim start tone_alarm start gyrosim start accelsim start diff --git a/posix-configs/SITL/init/lpe/standard_vtol b/posix-configs/SITL/init/lpe/standard_vtol index e0b49b1734..01f42f8793 100644 --- a/posix-configs/SITL/init/lpe/standard_vtol +++ b/posix-configs/SITL/init/lpe/standard_vtol @@ -56,7 +56,6 @@ param set COM_DISARM_LAND 5 param set MPC_ACC_HOR_MAX 2 replay tryapplyparams simulator start -s -rgbledsim start tone_alarm start gyrosim start accelsim start diff --git a/posix-configs/SITL/init/lpe/typhoon_h480 b/posix-configs/SITL/init/lpe/typhoon_h480 index ec432c87f4..d63d0b2b3d 100644 --- a/posix-configs/SITL/init/lpe/typhoon_h480 +++ b/posix-configs/SITL/init/lpe/typhoon_h480 @@ -44,7 +44,6 @@ param set EKF2_ANGERR_INIT 0.01 param set EKF2_MAG_TYPE 1 replay tryapplyparams simulator start -s -rgbledsim start tone_alarm start gyrosim start accelsim start diff --git a/posix-configs/SITL/init/rcS_gazebo_delta_wing b/posix-configs/SITL/init/rcS_gazebo_delta_wing index 47223f4627..4627dc3dbe 100644 --- a/posix-configs/SITL/init/rcS_gazebo_delta_wing +++ b/posix-configs/SITL/init/rcS_gazebo_delta_wing @@ -28,7 +28,6 @@ param set FW_THR_IDLE 0.6 param set FS_GCS_ENABLE 2 replay tryapplyparams simulator start -s -rgbledsim start tone_alarm start gyrosim start accelsim start diff --git a/posix-configs/SITL/init/shell/iris b/posix-configs/SITL/init/shell/iris index 0a988e29f6..7f647f0671 100644 --- a/posix-configs/SITL/init/shell/iris +++ b/posix-configs/SITL/init/shell/iris @@ -5,5 +5,4 @@ param set SYS_RESTART_TYPE 0 dataman start -rgbledsim start tone_alarm start diff --git a/posix-configs/SITL/init/test/test_template.in b/posix-configs/SITL/init/test/test_template.in index 4423085f33..ed0e441663 100644 --- a/posix-configs/SITL/init/test/test_template.in +++ b/posix-configs/SITL/init/test/test_template.in @@ -6,7 +6,6 @@ param set SYS_RESTART_TYPE 0 dataman start simulator start -t -rgbledsim start tone_alarm start gyrosim start accelsim start diff --git a/posix-configs/SITL/init/test/tests_all b/posix-configs/SITL/init/test/tests_all index 661b3048de..b771e84f28 100644 --- a/posix-configs/SITL/init/test/tests_all +++ b/posix-configs/SITL/init/test/tests_all @@ -5,7 +5,6 @@ param set SYS_RESTART_TYPE 0 dataman start -rgbledsim start tone_alarm start ver all diff --git a/src/platforms/posix/drivers/rgbledsim/CMakeLists.txt b/src/platforms/posix/drivers/rgbledsim/CMakeLists.txt deleted file mode 100644 index 9e1c888997..0000000000 --- a/src/platforms/posix/drivers/rgbledsim/CMakeLists.txt +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# 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 - 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 deleted file mode 100644 index 32e74e22bc..0000000000 --- a/src/platforms/posix/drivers/rgbledsim/rgbled.cpp +++ /dev/null @@ -1,802 +0,0 @@ -/**************************************************************************** - * - * 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 - -#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] = {0, 0}; - 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 = nullptr; - - while ((ch = px4_getopt(argc, argv, "a:b:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'a': - rgbledadr = strtol(myoptarg, nullptr, 0); - break; - - case 'b': - i2cdevice = strtol(myoptarg, nullptr, 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], nullptr, 0); - v.green = strtol(argv[3], nullptr, 0); - v.blue = strtol(argv[4], nullptr, 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; -}