From 925a6808e0e6f5a86555aebfd6faea9fef403b59 Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Wed, 25 May 2022 17:44:54 -0400 Subject: [PATCH] NXP demo board config. -LH --- boards/nxp/fmuk66-v3/init/rc.board_defaults | 8 + boards/nxp/fmuk66-v3/nxp_demo.px4board | 12 + boards/nxp/ucans32k146/init/rc.board_defaults | 1 + boards/nxp/ucans32k146/nxp_demo.px4board | 6 + msg/CMakeLists.txt | 1 + msg/led_control.msg | 5 +- msg/led_control.msg.old | 37 +++ msg/lighting_states.msg | 17 ++ src/drivers/cyphal/PublicationManager.hpp | 17 +- src/drivers/cyphal/SubscriptionManager.hpp | 17 +- src/drivers/drv_apa102.h | 71 +++++ src/drivers/extreme_switch/CMakeLists.txt | 45 +++ src/drivers/extreme_switch/ExtremeSwitch.cpp | 222 ++++++++++++++ src/drivers/extreme_switch/ExtremeSwitch.hpp | 96 ++++++ src/drivers/extreme_switch/Kconfig | 5 + src/drivers/lights/apa102/CMakeLists.txt | 41 +++ src/drivers/lights/apa102/Kconfig | 5 + src/drivers/lights/apa102/apa102.cpp | 275 ++++++++++++++++++ src/drivers/lights/apa102/apa102.hpp | 54 ++++ .../elm_lighting_module/CMakeLists.txt | 39 +++ src/examples/elm_lighting_module/Kconfig | 5 + .../elm_lighting_module/elm_lighting_module.c | 180 ++++++++++++ .../lighting_state_converter/CMakeLists.txt | 40 +++ src/modules/lighting_state_converter/Kconfig | 5 + .../lighting_state_converter.cpp | 271 +++++++++++++++++ .../lighting_state_converter.h | 97 ++++++ .../lighting_state_converter/parameters.c | 41 +++ 27 files changed, 1609 insertions(+), 4 deletions(-) create mode 100644 boards/nxp/fmuk66-v3/nxp_demo.px4board create mode 100644 boards/nxp/ucans32k146/nxp_demo.px4board create mode 100644 msg/led_control.msg.old create mode 100644 msg/lighting_states.msg create mode 100644 src/drivers/drv_apa102.h create mode 100644 src/drivers/extreme_switch/CMakeLists.txt create mode 100644 src/drivers/extreme_switch/ExtremeSwitch.cpp create mode 100644 src/drivers/extreme_switch/ExtremeSwitch.hpp create mode 100644 src/drivers/extreme_switch/Kconfig create mode 100644 src/drivers/lights/apa102/CMakeLists.txt create mode 100644 src/drivers/lights/apa102/Kconfig create mode 100644 src/drivers/lights/apa102/apa102.cpp create mode 100644 src/drivers/lights/apa102/apa102.hpp create mode 100644 src/examples/elm_lighting_module/CMakeLists.txt create mode 100644 src/examples/elm_lighting_module/Kconfig create mode 100644 src/examples/elm_lighting_module/elm_lighting_module.c create mode 100644 src/modules/lighting_state_converter/CMakeLists.txt create mode 100644 src/modules/lighting_state_converter/Kconfig create mode 100644 src/modules/lighting_state_converter/lighting_state_converter.cpp create mode 100644 src/modules/lighting_state_converter/lighting_state_converter.h create mode 100644 src/modules/lighting_state_converter/parameters.c diff --git a/boards/nxp/fmuk66-v3/init/rc.board_defaults b/boards/nxp/fmuk66-v3/init/rc.board_defaults index df700b4dc3..2e5e209013 100644 --- a/boards/nxp/fmuk66-v3/init/rc.board_defaults +++ b/boards/nxp/fmuk66-v3/init/rc.board_defaults @@ -9,3 +9,11 @@ param set-default BAT1_A_PER_V 15.391030303 rgbled_pwm start safety_button start +# pre-arm stuff +param set-default SYS_CTRL_ALLOC 1 +param set-default COM_PREARM_MODE 2 + +# Cyphal stuff +ifup can0 +cyphal start +elm_lighting_module diff --git a/boards/nxp/fmuk66-v3/nxp_demo.px4board b/boards/nxp/fmuk66-v3/nxp_demo.px4board new file mode 100644 index 0000000000..97b474d03e --- /dev/null +++ b/boards/nxp/fmuk66-v3/nxp_demo.px4board @@ -0,0 +1,12 @@ +CONFIG_DRIVERS_UAVCAN=n +CONFIG_DRIVERS_CYPHAL=y +CONFIG_CYPHAL_BMS_SUBSCRIBER=y +CONFIG_CYPHAL_ESC_SUBSCRIBER=y +CONFIG_CYPHAL_GNSS_SUBSCRIBER_0=y +CONFIG_CYPHAL_GNSS_SUBSCRIBER_1=y +CONFIG_CYPHAL_READINESS_PUBLISHER=y +CONFIG_CYPHAL_UORB_ACTUATOR_OUTPUTS_PUBLISHER=y +CONFIG_EXAMPLES_ELM_LIGHTING_MODULE=y +CONFIG_EXAMPLES_LIGHTING_STATES_PUBLISHER=y +CONFIG_MODULES_MICRODDS_CLIENT=y +CONFIG_MODULES_MICRORTPS_BRIDGE=y diff --git a/boards/nxp/ucans32k146/init/rc.board_defaults b/boards/nxp/ucans32k146/init/rc.board_defaults index f9775ee814..31e41871a9 100644 --- a/boards/nxp/ucans32k146/init/rc.board_defaults +++ b/boards/nxp/ucans32k146/init/rc.board_defaults @@ -7,3 +7,4 @@ pwm_out mode_pwm1 start ifup can0 cyphal start +lighting_state_converter start diff --git a/boards/nxp/ucans32k146/nxp_demo.px4board b/boards/nxp/ucans32k146/nxp_demo.px4board new file mode 100644 index 0000000000..802465cef8 --- /dev/null +++ b/boards/nxp/ucans32k146/nxp_demo.px4board @@ -0,0 +1,6 @@ +CONFIG_DRIVERS_LIGHTS_APA102=y +CONFIG_DRIVERS_CYPHAL=y +CONFIG_DRIVERS_UAVCANNODE_GPS_DEMO=n +CONFIG_MODULES_LIGHTING_STATE_CONVERTER=y +CONFIG_CYPHAL_NODE_CLIENT=y +CONFIG_CYPHAL_UORB_LIGHTING_STATES_SUBSCRIBER=y diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 2064d91ba2..57d7b7a1c9 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -104,6 +104,7 @@ set(msg_files landing_gear.msg landing_target_innovations.msg landing_target_pose.msg + lighting_states.msg led_control.msg log_message.msg logger_status.msg diff --git a/msg/led_control.msg b/msg/led_control.msg index 5bf968219d..13752b5b24 100644 --- a/msg/led_control.msg +++ b/msg/led_control.msg @@ -13,6 +13,7 @@ uint8 COLOR_PURPLE = 5 uint8 COLOR_AMBER = 6 uint8 COLOR_CYAN = 7 uint8 COLOR_WHITE = 8 +uint8 COLOR_DIM_RED = 9 # LED modes definitions uint8 MODE_OFF = 0 # turn LED off @@ -27,11 +28,11 @@ uint8 MODE_FLASH = 7 # two fast blinks (on/off) with timing as in MODE_BLINK_FAS uint8 MAX_PRIORITY = 2 # maxium priority (minimum is 0) -uint8 led_mask # bitmask which LED(s) to control, set to 0xff for all +uint16 led_mask # bitmask which LED(s) to control, set to 0xff for all uint8 color # see COLOR_* uint8 mode # see MODE_* uint8 num_blinks # how many times to blink (number of on-off cycles if mode is one of MODE_BLINK_*) . Set to 0 for infinite # in MODE_FLASH it is the number of cycles. Max number of blinks: 122 and max number of flash cycles: 20 uint8 priority # priority: higher priority events will override current lower priority events (see MAX_PRIORITY) -uint8 ORB_QUEUE_LENGTH = 8 # needs to match BOARD_MAX_LEDS +uint8 ORB_QUEUE_LENGTH = 16 # needs to match BOARD_MAX_LEDS diff --git a/msg/led_control.msg.old b/msg/led_control.msg.old new file mode 100644 index 0000000000..5bf968219d --- /dev/null +++ b/msg/led_control.msg.old @@ -0,0 +1,37 @@ +# LED control: control a single or multiple LED's. +# These are the externally visible LED's, not the board LED's + +uint64 timestamp # time since system start (microseconds) + +# colors +uint8 COLOR_OFF = 0 # this is only used in the drivers +uint8 COLOR_RED = 1 +uint8 COLOR_GREEN = 2 +uint8 COLOR_BLUE = 3 +uint8 COLOR_YELLOW = 4 +uint8 COLOR_PURPLE = 5 +uint8 COLOR_AMBER = 6 +uint8 COLOR_CYAN = 7 +uint8 COLOR_WHITE = 8 + +# LED modes definitions +uint8 MODE_OFF = 0 # turn LED off +uint8 MODE_ON = 1 # turn LED on +uint8 MODE_DISABLED = 2 # disable this priority (switch to lower priority setting) +uint8 MODE_BLINK_SLOW = 3 +uint8 MODE_BLINK_NORMAL = 4 +uint8 MODE_BLINK_FAST = 5 +uint8 MODE_BREATHE = 6 # continuously increase & decrease brightness (solid color if driver does not support it) +uint8 MODE_FLASH = 7 # two fast blinks (on/off) with timing as in MODE_BLINK_FAST and then off for a while + +uint8 MAX_PRIORITY = 2 # maxium priority (minimum is 0) + + +uint8 led_mask # bitmask which LED(s) to control, set to 0xff for all +uint8 color # see COLOR_* +uint8 mode # see MODE_* +uint8 num_blinks # how many times to blink (number of on-off cycles if mode is one of MODE_BLINK_*) . Set to 0 for infinite + # in MODE_FLASH it is the number of cycles. Max number of blinks: 122 and max number of flash cycles: 20 +uint8 priority # priority: higher priority events will override current lower priority events (see MAX_PRIORITY) + +uint8 ORB_QUEUE_LENGTH = 8 # needs to match BOARD_MAX_LEDS diff --git a/msg/lighting_states.msg b/msg/lighting_states.msg new file mode 100644 index 0000000000..c5be4fd64f --- /dev/null +++ b/msg/lighting_states.msg @@ -0,0 +1,17 @@ +# LED States for El Mandadero and NXP Buggy3 + +uint64 timestamp + +uint8 IDLE = 0 # Dim Red +uint8 BRAKE = 1 # Bright Red +uint8 REVERSE = 2 # [Top] White +uint8 TURN = 3 # [Top] Flashing Yellow +uint8 HAZARD = 4 # Flashing Yellow +uint8 NO_STATE = 255 # No state update + +# LED Panel IDs +# 0 - Right Front +# 1 - Left Front +# 2 - Left Rear +# 3 - Right Rear +uint8[10] state diff --git a/src/drivers/cyphal/PublicationManager.hpp b/src/drivers/cyphal/PublicationManager.hpp index b3d39ffbac..e57249fb32 100644 --- a/src/drivers/cyphal/PublicationManager.hpp +++ b/src/drivers/cyphal/PublicationManager.hpp @@ -64,13 +64,18 @@ #define CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER 0 #endif +#ifndef CONFIG_CYPHAL_UORB_LIGHTING_STATES_PUBLISHER +#define CONFIG_CYPHAL_UORB_LIGHTING_STATES_PUBLISHER 0 +#endif + /* Preprocessor calculation of publisher count */ #define UAVCAN_PUB_COUNT CONFIG_CYPHAL_GNSS_PUBLISHER + \ CONFIG_CYPHAL_ESC_CONTROLLER + \ CONFIG_CYPHAL_READINESS_PUBLISHER + \ CONFIG_CYPHAL_UORB_ACTUATOR_OUTPUTS_PUBLISHER + \ - CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER + CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER + \ + CONFIG_CYPHAL_UORB_LIGHTING_STATES_PUBLISHER #include #include @@ -159,6 +164,16 @@ private: "uorb.sensor_gps", 0 }, +#endif +#if CONFIG_CYPHAL_UORB_LIGHTING_STATES_PUBLISHER + { + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanPublisher * + { + return new uORB_over_UAVCAN_Publisher(handle, pmgr, ORB_ID(lighting_states)); + }, + "uorb.lighting_states", + 0 + }, #endif }; }; diff --git a/src/drivers/cyphal/SubscriptionManager.hpp b/src/drivers/cyphal/SubscriptionManager.hpp index 1e25a65d52..c4dde90f4d 100644 --- a/src/drivers/cyphal/SubscriptionManager.hpp +++ b/src/drivers/cyphal/SubscriptionManager.hpp @@ -61,13 +61,18 @@ #define CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER 0 #endif +#ifndef CONFIG_CYPHAL_UORB_LIGHTING_STATES_SUBSCRIBER +#define CONFIG_CYPHAL_UORB_LIGHTING_STATES_SUBSCRIBER 0 +#endif + /* Preprocessor calculation of Subscribers count */ #define UAVCAN_SUB_COUNT CONFIG_CYPHAL_ESC_SUBSCRIBER + \ CONFIG_CYPHAL_GNSS_SUBSCRIBER_0 + \ CONFIG_CYPHAL_GNSS_SUBSCRIBER_1 + \ CONFIG_CYPHAL_BMS_SUBSCRIBER + \ - CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER + CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER + \ + CONFIG_CYPHAL_UORB_LIGHTING_STATES_SUBSCRIBER #include #include @@ -179,6 +184,16 @@ private: "uorb.sensor_gps", 0 }, +#endif +#if CONFIG_CYPHAL_UORB_LIGHTING_STATES_SUBSCRIBER + { + [](CanardInstance & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * + { + return new uORB_over_UAVCAN_Subscriber(handle, pmgr, ORB_ID(lighting_states)); + }, + "uorb.lighting_states", + 0 + }, #endif }; }; diff --git a/src/drivers/drv_apa102.h b/src/drivers/drv_apa102.h new file mode 100644 index 0000000000..15548c0d44 --- /dev/null +++ b/src/drivers/drv_apa102.h @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * Copyright (C) 2021 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 drv_apa102.h + * + * APA102 LED driver interface. + * + */ + +#pragma once + +#include +#include +#include + +namespace apa102 +{ +class APA102LEDData +{ +public: + enum eRGB { + eB = 0, + eR = 1, + eG = 2 + }; + + typedef union { + uint8_t grb[3]; + uint32_t l; + } led_data_t; + + led_data_t data{}; + APA102LEDData() {data.l = 0;} + APA102LEDData(APA102LEDData &r) {data.l = r.data.l;} + + uint8_t &R() {return data.grb[eR];}; + uint8_t &G() {return data.grb[eG];}; + uint8_t &B() {return data.grb[eB];}; +}; +}; diff --git a/src/drivers/extreme_switch/CMakeLists.txt b/src/drivers/extreme_switch/CMakeLists.txt new file mode 100644 index 0000000000..be87985e9d --- /dev/null +++ b/src/drivers/extreme_switch/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# Copyright (c) 2021 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 drivers__extreme_switch + MAIN extreme_switch + COMPILE_FLAGS + #-DDEBUG_BUILD # uncomment for PX4_DEBUG output + #-O0 # uncomment when debugging + SRCS + ExtremeSwitch.cpp + ExtremeSwitch.hpp + DEPENDS + px4_work_queue + ) diff --git a/src/drivers/extreme_switch/ExtremeSwitch.cpp b/src/drivers/extreme_switch/ExtremeSwitch.cpp new file mode 100644 index 0000000000..aedad2ae74 --- /dev/null +++ b/src/drivers/extreme_switch/ExtremeSwitch.cpp @@ -0,0 +1,222 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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. + * + ****************************************************************************/ + +#include "ExtremeSwitch.hpp" + +ExtremeSwitch::ExtremeSwitch(int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode) : + SPI(DRV_DEVTYPE_UNUSED, MODULE_NAME, bus, device, spi_mode, bus_frequency), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::test1) +{ +} + +ExtremeSwitch::~ExtremeSwitch() +{ + perf_free(_loop_perf); + perf_free(_loop_interval_perf); +} + +int ExtremeSwitch::init() +{ + /* Let's not do this yet + // execute Run() on every sensor_accel publication + if (!_sensor_accel_sub.registerCallback()) { + PX4_ERR("sensor_accel callback registration failed"); + return false; + } + */ + + int ret = SPI::init(); + + if(ret != OK) { + printf("SPI::init() failed\n"); + DEVICE_DEBUG("SPI Init Failed"); + return -EIO; + } + + // alternatively, Run on fixed interval + ScheduleOnInterval(10000_us); // 2000 us interval, 200 Hz rate + + return PX4_OK; +} + +void ExtremeSwitch::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + perf_count(_loop_interval_perf); + + uint8_t buf; + uint8_t rbuf; + + printf("Starting loop\n\n"); + + + printf("Transferring OCR register \n"); + buf = 0b00000001; + transfer(&buf, &rbuf, sizeof(uint8_t)); + printf("rbuf: 0x%X\n", rbuf); + + px4_usleep(500000); + + printf("Transferring ON command \n"); + buf = 0b00010001; + transfer(&buf, &rbuf, sizeof(uint8_t)); + printf("rbuf: 0x%X\n", rbuf); + + px4_usleep(100000); + + printf("Transferring OFF command \n"); + buf = 0b00010000; + transfer(&buf, &rbuf, sizeof(uint8_t)); + printf("rbuf: 0x%X\n", rbuf); + + printf("Ending loop\n\n"); + + /* + * Just send a few pulses to enable the switch + * Wait a while between pulses + */ + /* + // Example + // update vehicle_status to check arming state + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.copy(&vehicle_status)) { + + const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + + if (armed && !_armed) { + PX4_WARN("vehicle armed due to %d", vehicle_status.latest_arming_reason); + + } else if (!armed && _armed) { + PX4_INFO("vehicle disarmed due to %d", vehicle_status.latest_disarming_reason); + } + + _armed = armed; + } + } + + + // Example + // grab latest accelerometer data + if (_sensor_accel_sub.updated()) { + sensor_accel_s accel; + + if (_sensor_accel_sub.copy(&accel)) { + // DO WORK + + // access parameter value (SYS_AUTOSTART) + if (_param_sys_autostart.get() == 1234) { + // do something if SYS_AUTOSTART is 1234 + } + } + } + + + // Example + // publish some data + orb_test_s data{}; + data.val = 314159; + data.timestamp = hrt_absolute_time(); + _orb_test_pub.publish(data); + */ + + + perf_end(_loop_perf); +} + +int ExtremeSwitch::task_spawn(int argc, char *argv[]) +{ + ExtremeSwitch *instance = new ExtremeSwitch(1, 0x10000000, 1000000, SPIDEV_MODE1); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init() == PX4_OK) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int ExtremeSwitch::print_status() +{ + perf_print_counter(_loop_perf); + perf_print_counter(_loop_interval_perf); + return 0; +} + +int ExtremeSwitch::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int ExtremeSwitch::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Example of a simple module running out of a work queue. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("extreme_switch", "template"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int extreme_switch_main(int argc, char *argv[]) +{ + return ExtremeSwitch::main(argc, argv); +} diff --git a/src/drivers/extreme_switch/ExtremeSwitch.hpp b/src/drivers/extreme_switch/ExtremeSwitch.hpp new file mode 100644 index 0000000000..0bfff797b0 --- /dev/null +++ b/src/drivers/extreme_switch/ExtremeSwitch.hpp @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +using namespace time_literals; + +class ExtremeSwitch : public device::SPI, public px4::ScheduledWorkItem, public ModuleBase +{ +public: + ExtremeSwitch(int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode); + ~ExtremeSwitch() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + int init(); + + int print_status() override; + +private: + void Run() override; + + /* + // Publications + uORB::Publication _orb_test_pub{ORB_ID(orb_test)}; + + // Subscriptions + uORB::SubscriptionCallbackWorkItem _sensor_accel_sub{this, ORB_ID(sensor_accel)}; // subscription that schedules ExtremeSwitch when updated + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; // regular subscription for additional data + */ + + + // Performance (perf) counters + perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; + + // Parameters + + + bool _armed{false}; +}; diff --git a/src/drivers/extreme_switch/Kconfig b/src/drivers/extreme_switch/Kconfig new file mode 100644 index 0000000000..6a784f4a42 --- /dev/null +++ b/src/drivers/extreme_switch/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_EXTREME_SWITCH + bool "extreme_switch" + default n + ---help--- + Enable support for extreme_switch \ No newline at end of file diff --git a/src/drivers/lights/apa102/CMakeLists.txt b/src/drivers/lights/apa102/CMakeLists.txt new file mode 100644 index 0000000000..0de5b9c581 --- /dev/null +++ b/src/drivers/lights/apa102/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2021 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 drivers__apa102 + MAIN apa102 + SRCS + apa102.cpp + DEPENDS + led + ) diff --git a/src/drivers/lights/apa102/Kconfig b/src/drivers/lights/apa102/Kconfig new file mode 100644 index 0000000000..3238117c78 --- /dev/null +++ b/src/drivers/lights/apa102/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_LIGHTS_APA102 + bool "apa102" + default n + ---help--- + Enable support for apa102 \ No newline at end of file diff --git a/src/drivers/lights/apa102/apa102.cpp b/src/drivers/lights/apa102/apa102.cpp new file mode 100644 index 0000000000..af6f6c1c6a --- /dev/null +++ b/src/drivers/lights/apa102/apa102.cpp @@ -0,0 +1,275 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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 apa102.cpp + * + * Driver for the the apa102 class of RGB LED drivers. + * this driver is based on the PX4 led driver + * + */ + +#include "apa102.hpp" + +// Constructor +APA102::APA102(unsigned int number_of_packages, int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode) : + SPI(DRV_DEVTYPE_UNUSED, MODULE_NAME, bus, device, spi_mode, bus_frequency), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default), + _number_of_packages(number_of_packages) +{ +} + +// Deconstructor +APA102::~APA102() +{ + //TODO: deinit SPI bus + //apa102_deinit(); +} + +// Init +// This runs after calling the constructor +int APA102::init() +{ + // Init SPI bus + int ret = SPI::init(); + + if (ret != OK) { + printf("SPI::init() failed\n"); + DEVICE_DEBUG("SPI init failed"); + return -EIO; + } + + // Fill buffer with zeros + for (uint8_t i = 0; i < (_number_of_packages * 4) + 8; i++) { + buf[i] = 0x00; + } + + // APA102LEDData is just the data format for the BRG LED + _leds = new apa102::APA102LEDData [_number_of_packages]; + + if (_leds == nullptr) { + return PX4_ERROR; + } + + ScheduleNow(); + return OK; +} + +// +int APA102::task_spawn(int argc, char *argv[]) +{ + int myoptind = 1; + int ch; + const char *myoptarg = nullptr; + unsigned int number_of_packages = BOARD_MAX_LEDS; + + while ((ch = px4_getopt(argc, argv, "n:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'n': + number_of_packages = atoi(myoptarg) + 1; + + if (number_of_packages > BOARD_MAX_LEDS) { + number_of_packages = BOARD_MAX_LEDS; + PX4_INFO("Number of packages can not exceed BOARD_MAX_LEDS"); + } + + break; + + default: + print_usage("unrecognized option"); + return 1; + } + } + + printf("Number of packages: %d\n", number_of_packages); + APA102 *instance = new APA102(number_of_packages, 1, 0, 4000000, SPIDEV_MODE0); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init() == PX4_OK) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + return PX4_ERROR; +} + +int APA102::print_status() +{ + + PX4_INFO("Controlling %i LEDs", _number_of_packages); + + return 0; +} + +int APA102::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This module is responsible for driving interfasing to the Neopixel Serial LED + +### Examples +It is typically started with: +$ apa102 -n 8 +To drive all available leds. +)DESCR_STR"); + +PRINT_MODULE_USAGE_NAME("newpixel", "driver"); +PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +return 0; +} + +int APA102::custom_command(int argc, char *argv[]) +{ + return print_usage("unrecognized option"); +} + +/** + * Main loop function + * This will run periodically by the scheduler + */ +void APA102::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + return; + } + + // Structure for uORB data + LedControlData led_control_data; + + // Get LED data from uORB (led_control) + if (_led_controller.update(led_control_data) == 1) { + + // Loop through each LED + for (unsigned int led = 1; led < math::min(_number_of_packages, arraySize(led_control_data.leds)); led++) { + + // Set brightness + uint8_t brightness = led_control_data.leds[led].brightness; + + /* Brightness is not 0-255, it is 5 bit, so 0-31. 256/32=8 */ + brightness = brightness / 8; + //printf("BRIGHTNESS: %d\n", brightness); + //printf("arraySize(%d)\n", arraySize(led_control_data.leds)); + + // Use data from uORB to set specific APA102LEDData fields + switch (led_control_data.leds[led].color) { + case led_control_s::COLOR_RED: + _leds[led].R() = 255; _leds[led].G() = 0; _leds[led].B() = 0; + break; + + case led_control_s::COLOR_DIM_RED: + _leds[led].R() = 16; _leds[led].G() = 0; _leds[led].B() = 0; + break; + + case led_control_s::COLOR_GREEN: + _leds[led].R() = 0; _leds[led].G() = 255; _leds[led].B() = 0; + break; + + case led_control_s::COLOR_BLUE: + _leds[led].R() = 0; _leds[led].G() = 0; _leds[led].B() = 255; + break; + + case led_control_s::COLOR_AMBER: //make it the same as yellow + case led_control_s::COLOR_YELLOW: + _leds[led].R() = 255; _leds[led].G() = 255; _leds[led].B() = 0; + break; + + case led_control_s::COLOR_PURPLE: + _leds[led].R() = 255; _leds[led].G() = 0; _leds[led].B() = 255; + break; + + case led_control_s::COLOR_CYAN: + _leds[led].R() = 0; _leds[led].G() = 255; _leds[led].B() = 255; + break; + + case led_control_s::COLOR_WHITE: + _leds[led].R() = 255; _leds[led].G() = 255; _leds[led].B() = 255; + break; + + default: // led_control_s::COLOR_OFF + _leds[led].R() = 0; _leds[led].G() = 0; _leds[led].B() = 0; + break; + } // end switch + + /* APA102 Frame + * 0x000000000 [start frame] (this is already done in init) + * 0xE0 + [brightness] + * 0xXXXXXX [bgr] + * 0xFFFFFFFF [end frame] + */ + + for(uint8_t i = 1; i < _number_of_packages; i++) + { + buf[4+(4*(i-1))] = 0b11100000 + brightness; + buf[5+(4*(i-1))] = _leds[i].B(); + buf[6+(4*(i-1))] = _leds[i].G(); + buf[7+(4*(i-1))] = _leds[i].R(); + } // end FOR loop + + // Fill with end frame + buf[8+(4*_number_of_packages)] = 0xFF; + buf[9+(4*_number_of_packages)] = 0xFF; + buf[10+(4*_number_of_packages)] = 0xFF; + buf[11+(4*_number_of_packages)] = 0xFF; + + } // end FOR loop + + transfer(buf, rbuf, (_number_of_packages * 4) + 8); + + } // end IF statement + + ScheduleDelayed(_led_controller.maximum_update_interval()); +} // end FUNCTION + +// Main function +// This runs when calling the command on the command line +extern "C" __EXPORT int apa102_main(int argc, char *argv[]) +{ + // This calls task_spawn + return APA102::main(argc, argv); +} diff --git a/src/drivers/lights/apa102/apa102.hpp b/src/drivers/lights/apa102/apa102.hpp new file mode 100644 index 0000000000..ee5db5910a --- /dev/null +++ b/src/drivers/lights/apa102/apa102.hpp @@ -0,0 +1,54 @@ +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +class APA102 : public device::SPI, public px4::ScheduledWorkItem, public ModuleBase +{ +public: + APA102(unsigned int number_of_packages, int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode); + virtual ~APA102(); + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + void Run() override; + + /** @see ModuleBase::print_status() */ + int print_status() override; + + + int init(); + int status(); + +private: + // We don't want to use BOARD_HAS_N_S_RGB_LED, SPI leds are external + unsigned int _number_of_packages; + + uint8_t buf[(BOARD_MAX_LEDS * 4) + 8]; + uint8_t rbuf[(BOARD_MAX_LEDS * 4) + 8]; + + // LedController is a driver that takes care of LEDs using the led_control ORB msg. + // Do we really want to use this? + LedController _led_controller; + + // ?? + APA102(const APA102 &) = delete; + APA102 operator=(const APA102 &) = delete; + + // What is this? + // _leds is the actual buffer I believe + apa102::APA102LEDData *_leds; +}; \ No newline at end of file diff --git a/src/examples/elm_lighting_module/CMakeLists.txt b/src/examples/elm_lighting_module/CMakeLists.txt new file mode 100644 index 0000000000..0150be3dc8 --- /dev/null +++ b/src/examples/elm_lighting_module/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# 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 examples__elm_lighting_module + MAIN elm_lighting_module + SRCS + elm_lighting_module.c + DEPENDS + ) diff --git a/src/examples/elm_lighting_module/Kconfig b/src/examples/elm_lighting_module/Kconfig new file mode 100644 index 0000000000..63336a1988 --- /dev/null +++ b/src/examples/elm_lighting_module/Kconfig @@ -0,0 +1,5 @@ +menuconfig EXAMPLES_ELM_LIGHTING_MODULE + bool "elm_lighting_module" + default n + ---help--- + Enable support for elm_lighting_module \ No newline at end of file diff --git a/src/examples/elm_lighting_module/elm_lighting_module.c b/src/examples/elm_lighting_module/elm_lighting_module.c new file mode 100644 index 0000000000..1dd4f8dc49 --- /dev/null +++ b/src/examples/elm_lighting_module/elm_lighting_module.c @@ -0,0 +1,180 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2019 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 elm_lighting_module.c + * Minimal application example for PX4 autopilot + * + * @author Landon Haugh + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +__EXPORT int elm_lighting_module_main(int argc, char *argv[]); + +int elm_lighting_module_main(int argc, char *argv[]) +{ + PX4_INFO("Hello Sky!"); + + /* advertise lighting_states topic */ + struct lighting_states_s lighting_states; + memset(&lighting_states, 255, sizeof(lighting_states)); + orb_advert_t lighting_states_pub = orb_advertise(ORB_ID(lighting_states), &lighting_states); + +/* + for (int i = 0; i < 8; i++) { + lighting_states.state[i] = 255; + } +*/ + for(int i = 0; i < 200; i++) { + px4_usleep(2000000); + + // Idle state + printf("Updating state to IDLE\n"); + lighting_states.state[0] = 0; + lighting_states.state[1] = 0; + lighting_states.state[2] = 0; + lighting_states.state[3] = 0; + lighting_states.state[4] = 2; + lighting_states.state[5] = 2; + lighting_states.state[6] = 2; + lighting_states.state[7] = 2; + lighting_states.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(lighting_states), lighting_states_pub, &lighting_states); + px4_usleep(2000000); + + + // Braking state (NO OTHER STATES) + printf("Updating state to BRAKE\n"); + lighting_states.state[0] = 1; + lighting_states.state[1] = 1; + lighting_states.state[2] = 1; + lighting_states.state[3] = 1; + lighting_states.state[4] = 2; + lighting_states.state[5] = 2; + lighting_states.state[6] = 2; + lighting_states.state[7] = 2; + lighting_states.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(lighting_states), lighting_states_pub, &lighting_states); + px4_usleep(2000000); + + + // Idle state while reversing + printf("Updating state to IDLE/REVERSE\n"); + lighting_states.state[0] = 0; + lighting_states.state[1] = 2; + lighting_states.state[2] = 0; + lighting_states.state[3] = 2; + lighting_states.state[4] = 2; + lighting_states.state[5] = 2; + lighting_states.state[6] = 2; + lighting_states.state[7] = 2; + lighting_states.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(lighting_states), lighting_states_pub, &lighting_states); + px4_usleep(2000000); + + + // Braking state while reversing + printf("Updating state to BRAKE/REVERSE\n"); + lighting_states.state[0] = 1; + lighting_states.state[1] = 2; + lighting_states.state[2] = 1; + lighting_states.state[3] = 2; + lighting_states.state[4] = 2; + lighting_states.state[5] = 2; + lighting_states.state[6] = 2; + lighting_states.state[7] = 2; + lighting_states.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(lighting_states), lighting_states_pub, &lighting_states); + px4_usleep(2000000); + + + // Idle state while turning + printf("Updating state to IDLE/TURN\n"); + lighting_states.state[0] = 0; + lighting_states.state[1] = 3; + lighting_states.state[2] = 0; + lighting_states.state[3] = 0; + lighting_states.state[4] = 2; + lighting_states.state[5] = 3; + lighting_states.state[6] = 2; + lighting_states.state[7] = 2; + lighting_states.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(lighting_states), lighting_states_pub, &lighting_states); + px4_usleep(2000000); + + + // Braking state while turning + printf("Updating state to BRAKE/TURN\n"); + lighting_states.state[0] = 0; + lighting_states.state[1] = 0; + lighting_states.state[2] = 0; + lighting_states.state[3] = 3; + lighting_states.state[4] = 2; + lighting_states.state[5] = 2; + lighting_states.state[6] = 2; + lighting_states.state[7] = 3; + lighting_states.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(lighting_states), lighting_states_pub, &lighting_states); + px4_usleep(2000000); + + + // Return to Idle + printf("Updating state to IDLE\n"); + lighting_states.state[0] = 0; + lighting_states.state[1] = 0; + lighting_states.state[2] = 0; + lighting_states.state[3] = 0; + lighting_states.state[4] = 2; + lighting_states.state[5] = 2; + lighting_states.state[6] = 2; + lighting_states.state[7] = 2; + lighting_states.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(lighting_states), lighting_states_pub, &lighting_states); + px4_usleep(2000000); + } + + PX4_INFO("exiting"); + + return 0; +} diff --git a/src/modules/lighting_state_converter/CMakeLists.txt b/src/modules/lighting_state_converter/CMakeLists.txt new file mode 100644 index 0000000000..1da55d5a65 --- /dev/null +++ b/src/modules/lighting_state_converter/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2018 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 modules__lighting_state_converter + MAIN lighting_state_converter + SRCS + lighting_state_converter.cpp + ) + diff --git a/src/modules/lighting_state_converter/Kconfig b/src/modules/lighting_state_converter/Kconfig new file mode 100644 index 0000000000..27fda6e4b0 --- /dev/null +++ b/src/modules/lighting_state_converter/Kconfig @@ -0,0 +1,5 @@ +menuconfig MODULES_LIGHTING_STATE_CONVERTER + bool "lighting_state_converter" + default n + ---help--- + Enable support for lighting_state_converter \ No newline at end of file diff --git a/src/modules/lighting_state_converter/lighting_state_converter.cpp b/src/modules/lighting_state_converter/lighting_state_converter.cpp new file mode 100644 index 0000000000..b8a9ecdbc1 --- /dev/null +++ b/src/modules/lighting_state_converter/lighting_state_converter.cpp @@ -0,0 +1,271 @@ +/**************************************************************************** + * + * Copyright (c) 2018 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. + * + ****************************************************************************/ + +#include "lighting_state_converter.h" + +#include +#include +#include + +#include +#include + + +int LightingStateConverter::print_status() +{ + PX4_INFO("Running"); + // TODO: print additional runtime information about the state of the module + + return 0; +} + +int LightingStateConverter::custom_command(int argc, char *argv[]) +{ + /* + if (!is_running()) { + print_usage("not running"); + return 1; + } + + // additional custom commands can be handled like this: + if (!strcmp(argv[0], "do-something")) { + get_instance()->do_something(); + return 0; + } + */ + + return print_usage("unknown command"); +} + + +int LightingStateConverter::task_spawn(int argc, char *argv[]) +{ + _task_id = px4_task_spawn_cmd("module", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 1024, + (px4_main_t)&run_trampoline, + (char *const *)argv); + + if (_task_id < 0) { + _task_id = -1; + return -errno; + } + + return 0; +} + +LightingStateConverter *LightingStateConverter::instantiate(int argc, char *argv[]) +{ + int example_param = 0; + bool example_flag = false; + bool error_flag = false; + + int myoptind = 1; + int ch; + const char *myoptarg = nullptr; + + // parse CLI arguments + while ((ch = px4_getopt(argc, argv, "p:f", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'p': + example_param = (int)strtol(myoptarg, nullptr, 10); + break; + + case 'f': + example_flag = true; + break; + + case '?': + error_flag = true; + break; + + default: + PX4_WARN("unrecognized flag"); + error_flag = true; + break; + } + } + + if (error_flag) { + return nullptr; + } + + LightingStateConverter *instance = new LightingStateConverter(example_param, example_flag); + + if (instance == nullptr) { + PX4_ERR("alloc failed"); + } + + return instance; +} + +LightingStateConverter::LightingStateConverter(int example_param, bool example_flag) + : ModuleParams(nullptr) +{ +} + +void LightingStateConverter::run() +{ + // Example: run the loop synchronized to the sensor_combined topic publication + int lighting_states_sub = orb_subscribe(ORB_ID(lighting_states)); + + px4_pollfd_struct_t fds[1]; + fds[0].fd = lighting_states_sub; + fds[0].events = POLLIN; + + // initialize parameters + parameters_update(true); + + _lighting_id_no = _param_lighting_id_no.get(); + + while (!should_exit()) { + + // wait for up to 1000ms for data + int pret = px4_poll(fds, (sizeof(fds) / sizeof(fds[0])), 1000); + + if (pret == 0) { + // Timeout: let the loop run anyway, don't do `continue` here + //PX4_ERR("Timeout on poll"); + + } else if (pret < 0) { + // this is undesirable but not much we can do + PX4_ERR("poll error %d, %d", pret, errno); + px4_usleep(50000); + continue; + + } else if (fds[0].revents & POLLIN) { + + struct lighting_states_s lighting_states; + orb_copy(ORB_ID(lighting_states), lighting_states_sub, &lighting_states); + // TODO: do something with the data... + + printf("Received state data. Contents: \n"); + for(int i = 0; i < 10; i++) { + printf("ID: %d State: %d\n", i, lighting_states.state[i]); + } + + for (int i = (_lighting_id_no - 1)*2; i < (_lighting_id_no*2); i++) { + printf("_lighting_id_no: %d\n", i); + if (lighting_states.state[i] != 255) { + led_control_s led_control; + if(i == (_lighting_id_no - 1)*2) led_control.led_mask = 0b0000011110000010; + if(i == (_lighting_id_no*2)-1) led_control.led_mask = 0b0000000001111100; + + if (lighting_states.state[i] == 0) { + led_control.color = led_control_s::COLOR_DIM_RED; + led_control.mode = led_control_s::MODE_ON; + } + + if (lighting_states.state[i] == 1) { + led_control.color = led_control_s::COLOR_RED; + led_control.mode = led_control_s::MODE_ON; + } + + if (lighting_states.state[i] == 2) { + led_control.color = led_control_s::COLOR_WHITE; + led_control.mode = led_control_s::MODE_ON; + } + + if (lighting_states.state[i] == 3) { + led_control.color = led_control_s::COLOR_YELLOW; + led_control.mode = led_control_s::MODE_BLINK_NORMAL; + led_control.num_blinks = 0; + } + + if (lighting_states.state[i] == 4) { + led_control.color = led_control_s::COLOR_YELLOW; + led_control.mode = led_control_s::MODE_BREATHE; + led_control.num_blinks = 0; + } + + led_control.timestamp = hrt_absolute_time(); + _led_control_pub.publish(led_control); + } + } + } + + parameters_update(); + } + + orb_unsubscribe(lighting_states_sub); +} + +void LightingStateConverter::parameters_update(bool force) +{ + // check for parameter updates + if (_parameter_update_sub.updated() || force) { + // clear update + parameter_update_s update; + _parameter_update_sub.copy(&update); + + // update parameters from storage + updateParams(); + } +} + +int LightingStateConverter::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Section that describes the provided module functionality. + +This is a template for a module running as a task in the background with start/stop/status functionality. + +### Implementation +Section describing the high-level implementation of this module. + +### Examples +CLI usage example: +$ module start -f -p 42 + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("module", "template"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAM_FLAG('f', "Optional example flag", true); + PRINT_MODULE_USAGE_PARAM_INT('p', 0, 0, 1000, "Optional example parameter", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +int lighting_state_converter_main(int argc, char *argv[]) +{ + return LightingStateConverter::main(argc, argv); +} diff --git a/src/modules/lighting_state_converter/lighting_state_converter.h b/src/modules/lighting_state_converter/lighting_state_converter.h new file mode 100644 index 0000000000..92f98069cc --- /dev/null +++ b/src/modules/lighting_state_converter/lighting_state_converter.h @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * Copyright (c) 2018 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +extern "C" __EXPORT int lighting_state_converter_main(int argc, char *argv[]); + + +class LightingStateConverter : public ModuleBase, public ModuleParams +{ +public: + LightingStateConverter(int example_param, bool example_flag); + + virtual ~LightingStateConverter() = default; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static LightingStateConverter *instantiate(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::run() */ + void run() override; + + /** @see ModuleBase::print_status() */ + int print_status() override; + +private: + + /** + * Check for parameter changes and update them if needed. + * @param parameter_update_sub uorb subscription to parameter_update + * @param force for a parameter update + */ + void parameters_update(bool force = false); + + + DEFINE_PARAMETERS( + (ParamInt) _param_lighting_id_no /**< example parameter */ + ) + + int _lighting_id_no; + + // Subscriptions + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Publication _led_control_pub{ORB_ID(led_control)}; + uORB::Subscription _lighting_states_sub{ORB_ID(lighting_states)}; + +}; + diff --git a/src/modules/lighting_state_converter/parameters.c b/src/modules/lighting_state_converter/parameters.c new file mode 100644 index 0000000000..0b071fb8f2 --- /dev/null +++ b/src/modules/lighting_state_converter/parameters.c @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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. + * + ****************************************************************************/ + +/** +* Lighting ID number. +* +* @boolean +* @reboot_required false +* @group id +*/ +PARAM_DEFINE_INT32(LIGHTING_ID_NO, 1); \ No newline at end of file