add uav lights node

.gitmodules

@@ -9,4 +9,4 @@
 	url = [email protected]:PonomarevDA/tools.git
 [submodule "Libs/stm32-cube-project"]
 	path = Libs/stm32-cube-project
-	url = https://github.com/RaccoonLabHardware/mini-v2-software.git
+	url = https://github.com/RaccoonLabHardware/lights-v0-software.git

Src/cyphal_application/CMakeLists.txt

@@ -1,10 +1,11 @@
 set(applicationSourceCode
-    Src/periphery/pwm/pwm.cpp
+    Src/periphery/adc/adc.cpp
     Src/periphery/led/led.cpp
+    Src/periphery/ws2812/ws2812.c
 
-    Src/cyphal_application/setpoint/setpoint.cpp
-    Src/cyphal_application/feedback/feedback.cpp
+    Src/cyphal_application/lights/lights.cpp
     Src/cyphal_application/application.cpp
+    Libs/Cyphal/Udral/rgbled.cpp
 )
 set(applicationHeaders
     Src
@@ -14,6 +15,5 @@ set(applicationHeaders
 
 list(APPEND cyphalRegisters
     ${CMAKE_CURRENT_LIST_DIR}/params.yaml
-    ${CMAKE_CURRENT_LIST_DIR}/setpoint/params.yaml
-    ${CMAKE_CURRENT_LIST_DIR}/feedback/params.yaml
+    ${CMAKE_CURRENT_LIST_DIR}/lights/params.yaml
 )

Src/cyphal_application/README.md

@@ -2,8 +2,7 @@ The node has the following interface:
 
 | №  | Type | Message | Topic name  |
 | -- | ---- | ------- | ----------- |
-|   1 | sub | reg.udral.service.actuator.common.sp.Vector31 | setpoint | {'type': 'Port', 'data_type': 'reg.udral.service.actuator.common.sp.Vector31', 'enum_base': 'PARAM_SUB_SETPOINT'}|
-|   2 | pub | reg.udral.service.actuator.common.Feedback.0.1 | feedback | {'type': 'Port', 'data_type': 'reg.udral.service.actuator.common.Feedback.0.1', 'enum_base': 'PARAM_PUB_FEEDBACK_1'}|
+|   1 | sub | reg.udral.physics.optics.HighColor.0.1 | lights | {'type': 'Port', 'data_type': 'reg.udral.physics.optics.HighColor.0.1', 'enum_base': 'RGBLED'}|
 
 The node has the following registers:
 

Src/cyphal_application/application.cpp

@@ -8,16 +8,16 @@
 #include "main.h"
 #include "string_params.hpp"
 #include "params.hpp"
-#include "setpoint/setpoint.hpp"
-#include "feedback/feedback.hpp"
+#include "lights/lights.hpp"
 #include "periphery/led/led.hpp"
 
+
 void init_persistent_storage() {
     paramsInit(static_cast<uint8_t>(IntParamsIndexes::INTEGER_PARAMS_AMOUNT), NUM_OF_STR_PARAMS);
     paramsLoadFromFlash();
 
     auto node_name_param_idx = static_cast<ParamIndex_t>(IntParamsIndexes::INTEGER_PARAMS_AMOUNT);
-    paramsSetStringValue(node_name_param_idx, 19, (const uint8_t*)"co.raccoonlab.mini");
+    paramsSetStringValue(node_name_param_idx, 21, (const uint8_t*)"co.raccoonlab.lights");
 }
 
 void application_entry_point() {
@@ -28,19 +28,13 @@ void application_entry_point() {
     cyphal::Cyphal cyphal;
     int init_res = cyphal.init();
 
-    SetpointSubscriber setpoint(&cyphal);
-    init_res |= setpoint.init();
-
-    FeedbackPublisher feedback(&cyphal);
-    init_res |= feedback.init();
+    RgbLights lights(&cyphal);
+    init_res |= lights.init();
 
     while (true) {
-        auto led_color = (init_res >= 0) ? LedColor::BLUE_COLOR : LedColor::RED_COLOR;
-        LedPeriphery::toggle(led_color);
+        LedPeriphery::toggle();
 
         cyphal.process();
-
-        auto crnt_time_ms = HAL_GetTick();
-        feedback.process(crnt_time_ms);
+        lights.update();
     }
 }

Src/cyphal_application/lights/lights.cpp

@@ -0,0 +1,64 @@
+/// This software is distributed under the terms of the MIT License.
+/// Copyright (c) 2023 Dmitry Ponomarev.
+/// Author: Dmitry Ponomarev <[email protected]>
+
+#include "lights.hpp"
+#include <algorithm>
+#include "cyphal.hpp"
+#include "params.hpp"
+#include "main.h"
+#include "periphery/ws2812/ws2812.h"
+#include "periphery/adc/adc.hpp"
+
+extern TIM_HandleTypeDef htim2;
+
+static constexpr uint8_t RED_SCALE = 256 / (reg_udral_physics_optics_HighColor_0_1_MAX_RED + 1);
+static constexpr uint8_t GREEN_SCALE = 256 / (reg_udral_physics_optics_HighColor_0_1_MAX_GREEN + 1);
+static constexpr uint8_t BLUE_SCALE = 256 / (reg_udral_physics_optics_HighColor_0_1_MAX_BLUE + 1);
+
+
+int8_t RgbLights::init() {
+    int8_t res;
+
+    res = ws2812bInit(32, &htim2, TIM_CHANNEL_3);
+    if (res < 0) {
+        return res;
+    }
+
+    res = _rgbled_sub.init();
+    if (res < 0) {
+        return res;
+    }
+
+    res = AdcPeriphery::init();
+    if (res < 0) {
+        return res;
+    }
+
+    return 0;
+};
+
+void RgbLights::update() {
+    static uint32_t next_time_ms = 0;
+    if (HAL_GetTick() < next_time_ms) {
+        return;
+    }
+    next_time_ms = HAL_GetTick() + 10;
+
+    auto color = _rgbled_sub.get();
+    static Leds_Color_t leds;
+
+    // static uint8_t counter = 0;
+    // leds.colors[counter].shades.red = color.red * RED_SCALE;
+    // leds.colors[counter].shades.green = color.green * GREEN_SCALE;
+    // leds.colors[counter].shades.blue = color.blue * BLUE_SCALE;
+    // counter = (counter + 1) % 32;
+
+    for (uint_fast8_t led_idx = 0; led_idx < 32; led_idx++) {
+        leds.colors[led_idx].shades.red = color.red * RED_SCALE;
+        leds.colors[led_idx].shades.green = color.green * GREEN_SCALE;
+        leds.colors[led_idx].shades.blue = color.blue * BLUE_SCALE;
+    }
+    ws2812bSetColors(&leds);
+    ws2812bStartOnce();
+}

Src/cyphal_application/lights/lights.hpp

@@ -0,0 +1,23 @@
+/// This software is distributed under the terms of the MIT License.
+/// Copyright (c) 2023 Dmitry Ponomarev.
+/// Author: Dmitry Ponomarev <[email protected]>
+
+#ifndef SRC_CYPHAL_APPLICATION_LIGHTS_LIGHTS_HPP_
+#define SRC_CYPHAL_APPLICATION_LIGHTS_LIGHTS_HPP_
+
+#include "cyphal_subscribers.hpp"
+#include "Udral/rgbled.hpp"
+#include "Udral/circuit_status.hpp"
+
+class RgbLights {
+public:
+    RgbLights(cyphal::Cyphal* driver) : _rgbled_sub(driver), _temp_pub(driver, 0) {};
+    int8_t init();
+    void update();
+private:
+
+    cyphal::HighColorSubscriber _rgbled_sub;
+    RaccoonLab::CircuitStatusTemperaturePublisher _temp_pub;
+};
+
+#endif  // SRC_CYPHAL_APPLICATION_LIGHTS_LIGHTS_HPP_

Src/cyphal_application/setpoint/params.yaml → Src/cyphal_application/lights/params.yaml

@@ -3,7 +3,7 @@
 # - String register with name `uavcan.pub.setpoint.type`
 # The generated registers have proper flags, data type and min, max and default values.
 # They correspond the standard: https://github.com/OpenCyphal/public_regulated_data_types/blob/master/uavcan/register/384.Access.1.0.dsdl
-uavcan.sub.setpoint:
+uavcan.sub.lights:
   type: Port
-  data_type: reg.udral.service.actuator.common.sp.Vector31
-  enum_base: PARAM_SUB_SETPOINT
+  data_type: reg.udral.physics.optics.HighColor.0.1
+  enum_base: RGBLED

Src/periphery/adc/adc.cpp

@@ -0,0 +1,28 @@
+/// This software is distributed under the terms of the MIT License.
+/// Copyright (c) 2022-2023 Dmitry Ponomarev.
+/// Author: Dmitry Ponomarev <[email protected]>
+
+#include "adc.hpp"
+#include "main.h"
+
+extern ADC_HandleTypeDef hadc1;
+
+int8_t AdcPeriphery::init() {
+    HAL_ADCEx_Calibration_Start(&hadc1);
+    _is_adc_already_inited = true;
+    return 0;
+}
+
+uint16_t AdcPeriphery::get(AdcChannel channel) {
+    if (!_is_adc_already_inited || channel >= AdcChannel::ADC_NUMBER_OF_CNANNELS) {
+        return 0;
+    }
+
+    HAL_ADC_Start(&hadc1);
+    uint8_t channels_amount = static_cast<uint8_t>(AdcChannel::ADC_NUMBER_OF_CNANNELS);
+    uint16_t _adc_raw[static_cast<uint8_t>(AdcChannel::ADC_NUMBER_OF_CNANNELS)];
+    for (size_t ch_idx = 0; ch_idx < channels_amount; ch_idx++) {
+        _adc_raw[ch_idx] = (uint16_t)HAL_ADC_GetValue(&hadc1);
+    }
+    return _adc_raw[static_cast<uint8_t>(channel)];
+}