diff --git a/docs/assets/airframes/multicopter/amovlab_f410/amovlabf410_drone_v1.15.4.params b/docs/assets/airframes/multicopter/amovlab_f410/amovlabf410_drone_v1.15.4.params index 53bb8f5a01f..4cea5fc9d4b 100644 --- a/docs/assets/airframes/multicopter/amovlab_f410/amovlabf410_drone_v1.15.4.params +++ b/docs/assets/airframes/multicopter/amovlab_f410/amovlabf410_drone_v1.15.4.params @@ -1056,9 +1056,6 @@ 1 1 UAVCAN_EC_REV 0 6 1 1 UAVCAN_ENABLE 2 6 1 1 UAVCAN_LGT_ANTCL 2 6 -1 1 UAVCAN_LGT_LAND 0 6 -1 1 UAVCAN_LGT_NAV 3 6 -1 1 UAVCAN_LGT_STROB 1 6 1 1 UAVCAN_NODE_ID 1 6 1 1 UAVCAN_PUB_ARM 0 6 1 1 UAVCAN_PUB_MBD 0 6 diff --git a/docs/en/dronecan/index.md b/docs/en/dronecan/index.md index 13898c9dc3f..00ba948cbc6 100644 --- a/docs/en/dronecan/index.md +++ b/docs/en/dronecan/index.md @@ -281,6 +281,19 @@ PX4 DroneCAN parameters: Select the specific CAN interface(s) used for ESC data output using the [UAVCAN_ESC_IFACE](../advanced_config/parameter_reference.md#UAVCAN_ESC_IFACE) parameter (all that all interfaces are selected by default). Note that DroneCAN ESCs should be on their own dedicated CAN interface(s) because ESC messages can saturate the bus and starve other nodes of bandwidth. +### Lights + +PX4 can control LEDs via DroneCAN [LightsCommand](https://dronecan.github.io/Specification/7._List_of_standard_data_types/#lightscommand) messages. + +Configuration: + +1. Set [UAVCAN_LGT_NUM](../advanced_config/parameter_reference.md#UAVCAN_LGT_NUM) to the number of lights (0 disables). You might need to reopen the ground station to have parameters for new instances available. +2. For each light slot (0 to NUM-1), set: + - `UAVCAN_LGT_IDx`: The `light_id` matching your peripheral. + - `UAVCAN_LGT_FNx`: `Status` for system status colours, or `Anti-collision` for white beacon. +3. For anti-collision lights, [UAVCAN_LGT_ANTCL](../advanced_config/parameter_reference.md#UAVCAN_LGT_ANTCL) controls when they illuminate (off, armed, prearmed, always on). +4. Reboot for any changes to take effect. + ## QGC CANNODE Parameter Configuration QGroundControl can inspect and modify parameters belonging to CAN devices attached to the flight controller, provided the device are connected to the flight controller before QGC is started. diff --git a/docs/en/peripherals/vertiq.md b/docs/en/peripherals/vertiq.md index 82c7611f161..9a9f80c575a 100644 --- a/docs/en/peripherals/vertiq.md +++ b/docs/en/peripherals/vertiq.md @@ -38,6 +38,19 @@ Instructions for integrating the motor/ESC using with DroneCAN can be found in [ These instructions walk you through setting the correct parameters for enabling the flight controller's DroneCAN drivers, setting the correct configuration parameters for communication with Vertiq modules on the DroneCAN bus, ESC configuration, and testing that your flight controller can properly control your modules over DroneCAN. +#### LED Configuration for Vertiq Modules + +::: info +This configuration is only required if you have the optional [Vertiq LED module add-on](https://www.vertiq.co/add-ons). +Standard Vertiq ESC modules do not include LEDs. +::: + +Vertiq LED Add-on modules have two LEDs per ESC (RGB for status, White for anti-collision). +See [DroneCAN Lights](../dronecan/index.md#lights) for configuration instructions. + +The `light_id` for each LED is calculated as: `esc_index × 3 + BASE_ID`, where `BASE_ID` is 1 for RGB and 2 for White. + + ### DShot/PWM Configuration Instructions for integrating the motor/ESC using PWM and DShot can be found in [PWM and DShot Control with a Flight Controller](https://iqmotion.readthedocs.io/en/latest/tutorials/pwm_control_flight_controller.html). diff --git a/src/drivers/uavcan/module.yaml b/src/drivers/uavcan/module.yaml index 0579870297b..9518e06494a 100644 --- a/src/drivers/uavcan/module.yaml +++ b/src/drivers/uavcan/module.yaml @@ -1,3 +1,5 @@ +__max_num_uavcan_lights: &max_num_uavcan_lights 3 # Needs to be equal to MAX_NUM_UAVCAN_LIGHTS constant + module_name: UAVCAN parameters: - group: UAVCAN @@ -24,6 +26,46 @@ parameters: min: 1 max: 255 reboot_required: true + UAVCAN_LGT_NUM: + description: + short: Number of UAVCAN lights to configure + long: | + Number of lights to control via UAVCAN LightsCommand messages. + Set to 0 to disable UAVCAN light control. + Each light uses two parameters: LGT_IDx for the light_id and LGT_FNx for the function. + type: int32 + min: 0 + max: *max_num_uavcan_lights + default: 1 + reboot_required: true + UAVCAN_LGT_ID${i}: + description: + short: Light ${i} ID + long: | + specifies the light_id value for light ${i} in UAVCAN LightsCommand messages. + This determines which physical LED responds to commands for this light slot. + type: int32 + num_instances: *max_num_uavcan_lights + instance_start: 0 + min: 0 + max: 255 + default: 0 + UAVCAN_LGT_FN${i}: + description: + short: Light ${i} function + long: | + Function assigned to light ${i}. + 0: Status - displays system status colors + 1: Anti-collision - white beacon controlled by LGT_ANTCL parameter + type: enum + num_instances: *max_num_uavcan_lights + instance_start: 0 + min: 0 + max: 1 + default: 0 + values: + 0: Status Light + 1: Anti-collision Light actuator_output: show_subgroups_if: 'UAVCAN_ENABLE>=3' config_parameters: diff --git a/src/drivers/uavcan/rgbled.cpp b/src/drivers/uavcan/rgbled.cpp index 5c4ee8dc219..3ee0e1225ee 100644 --- a/src/drivers/uavcan/rgbled.cpp +++ b/src/drivers/uavcan/rgbled.cpp @@ -32,6 +32,7 @@ ****************************************************************************/ #include "rgbled.hpp" +#include UavcanRGBController::UavcanRGBController(uavcan::INode &node) : ModuleParams(nullptr), @@ -44,6 +45,38 @@ UavcanRGBController::UavcanRGBController(uavcan::INode &node) : int UavcanRGBController::init() { + // Cache number of lights (0 disables the feature) + _num_lights = math::min(static_cast(_param_lgt_num.get()), MAX_NUM_UAVCAN_LIGHTS); + + if (_num_lights == 0) { + return 0; // Disabled, don't start timer + } + + // Cache parameter handles and values for each light + for (uint8_t i = 0; i < _num_lights; i++) { + char param_name[17]; + + // Light ID parameter + snprintf(param_name, sizeof(param_name), "UAVCAN_LGT_ID%u", i); + _light_id_params[i] = param_find(param_name); + + if (_light_id_params[i] != PARAM_INVALID) { + int32_t light_id = 0; + param_get(_light_id_params[i], &light_id); + _light_ids[i] = static_cast(light_id); + } + + // Light function parameter + snprintf(param_name, sizeof(param_name), "UAVCAN_LGT_FN%u", i); + _light_fn_params[i] = param_find(param_name); + + if (_light_fn_params[i] != PARAM_INVALID) { + int32_t light_fn = 0; + param_get(_light_fn_params[i], &light_fn); + _light_functions[i] = static_cast(light_fn); + } + } + // Setup timer and call back function for periodic updates _timer.setCallback(TimerCbBinder(this, &UavcanRGBController::periodic_update)); _timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / MAX_RATE_HZ)); @@ -52,142 +85,108 @@ int UavcanRGBController::init() void UavcanRGBController::periodic_update(const uavcan::TimerEvent &) { - bool publish_lights = false; - uavcan::equipment::indication::LightsCommand cmds; + // Early return if disabled or no lights configured + if (_num_lights == 0) { + return; + } + // Check for status color updates from led_controller LedControlData led_control_data; - if (_led_controller.update(led_control_data) == 1) { - publish_lights = true; + if (_led_controller.update(led_control_data) != 1) { + return; // No update, nothing to do + } - // RGB color in the standard 5-6-5 16-bit palette. - // Monocolor lights should interpret this as brightness setpoint: from zero (0, 0, 0) to full brightness (31, 63, 31). + // Compute status color from led_control_data + uavcan::equipment::indication::RGB565 status_color{}; + uint8_t brightness = led_control_data.leds[0].brightness; + + switch (led_control_data.leds[0].color) { + case led_control_s::COLOR_RED: + status_color = rgb888_to_rgb565(brightness, 0, 0); + break; + + case led_control_s::COLOR_GREEN: + status_color = rgb888_to_rgb565(0, brightness, 0); + break; + + case led_control_s::COLOR_BLUE: + status_color = rgb888_to_rgb565(0, 0, brightness); + break; + + case led_control_s::COLOR_AMBER: // make it the same as yellow + case led_control_s::COLOR_YELLOW: + status_color = rgb888_to_rgb565(brightness, brightness, 0); + break; + + case led_control_s::COLOR_PURPLE: + status_color = rgb888_to_rgb565(brightness, 0, brightness); + break; + + case led_control_s::COLOR_CYAN: + status_color = rgb888_to_rgb565(0, brightness, brightness); + break; + + case led_control_s::COLOR_WHITE: + status_color = rgb888_to_rgb565(brightness, brightness, brightness); + break; + + default: + case led_control_s::COLOR_OFF: + break; + } + + // Build and send light commands for all configured lights + uavcan::equipment::indication::LightsCommand light_command; + + for (uint8_t i = 0; i < _num_lights; i++) { uavcan::equipment::indication::SingleLightCommand cmd; + cmd.light_id = _light_ids[i]; - uint8_t brightness = led_control_data.leds[0].brightness; - - switch (led_control_data.leds[0].color) { - case led_control_s::COLOR_RED: - cmd.color.red = brightness >> 3; - cmd.color.green = 0; - cmd.color.blue = 0; + switch (_light_functions[i]) { + case LightFunction::Status: + cmd.color = status_color; break; - case led_control_s::COLOR_GREEN: - cmd.color.red = 0; - cmd.color.green = brightness >> 2; - cmd.color.blue = 0; - break; - - case led_control_s::COLOR_BLUE: - cmd.color.red = 0; - cmd.color.green = 0; - cmd.color.blue = brightness >> 3; - break; - - case led_control_s::COLOR_AMBER: // make it the same as yellow - - // FALLTHROUGH - case led_control_s::COLOR_YELLOW: - cmd.color.red = (brightness / 2) >> 3; - cmd.color.green = (brightness / 2) >> 2; - cmd.color.blue = 0; - break; - - case led_control_s::COLOR_PURPLE: - cmd.color.red = (brightness / 2) >> 3; - cmd.color.green = 0; - cmd.color.blue = (brightness / 2) >> 3; - break; - - case led_control_s::COLOR_CYAN: - cmd.color.red = 0; - cmd.color.green = (brightness / 2) >> 2; - cmd.color.blue = (brightness / 2) >> 3; - break; - - case led_control_s::COLOR_WHITE: - cmd.color.red = (brightness / 3) >> 3; - cmd.color.green = (brightness / 3) >> 2; - cmd.color.blue = (brightness / 3) >> 3; - break; - - default: // led_control_s::COLOR_OFF - cmd.color.red = 0; - cmd.color.green = 0; - cmd.color.blue = 0; + case LightFunction::AntiCollision: + uint8_t brigtness = is_anticolision_on() ? 255 : 0; + cmd.color = rgb888_to_rgb565(brigtness, brigtness, brigtness); break; } - cmds.commands.push_back(cmd); - + light_command.commands.push_back(cmd); } - if (_armed_sub.updated()) { - publish_lights = true; - - actuator_armed_s armed; - - if (_armed_sub.copy(&armed)) { - - /* Determine the current control mode - * If a light's control mode config >= current control mode, the light will be enabled - * Logic must match UAVCAN_LGT_* param values. - * @value 0 Always off - * @value 1 When autopilot is armed - * @value 2 When autopilot is prearmed - * @value 3 Always on - */ - uint8_t control_mode = 0; - - if (armed.armed) { - control_mode = 1; - - } else if (armed.prearmed) { - control_mode = 2; - - } else { - control_mode = 3; - } - - uavcan::equipment::indication::SingleLightCommand cmd; - - // Beacons - cmd.light_id = uavcan::equipment::indication::SingleLightCommand::LIGHT_ID_ANTI_COLLISION; - cmd.color = brightness_to_rgb565(_param_mode_anti_col.get() >= control_mode ? 255 : 0); - cmds.commands.push_back(cmd); - - // Strobes - cmd.light_id = uavcan::equipment::indication::SingleLightCommand::LIGHT_ID_STROBE; - cmd.color = brightness_to_rgb565(_param_mode_strobe.get() >= control_mode ? 255 : 0); - cmds.commands.push_back(cmd); - - // Nav lights - cmd.light_id = uavcan::equipment::indication::SingleLightCommand::LIGHT_ID_RIGHT_OF_WAY; - cmd.color = brightness_to_rgb565(_param_mode_nav.get() >= control_mode ? 255 : 0); - cmds.commands.push_back(cmd); - - // Landing lights - cmd.light_id = uavcan::equipment::indication::SingleLightCommand::LIGHT_ID_LANDING; - cmd.color = brightness_to_rgb565(_param_mode_land.get() >= control_mode ? 255 : 0); - cmds.commands.push_back(cmd); - } - } - - if (publish_lights) { - _uavcan_pub_lights_cmd.broadcast(cmds); - } + _uavcan_pub_lights_cmd.broadcast(light_command); } -uavcan::equipment::indication::RGB565 UavcanRGBController::brightness_to_rgb565(uint8_t brightness) +bool UavcanRGBController::is_anticolision_on() { - // RGB color in the standard 5-6-5 16-bit palette. - // Monocolor lights should interpret this as brightness setpoint: from zero (0, 0, 0) to full brightness (31, 63, 31). - uavcan::equipment::indication::RGB565 color; + actuator_armed_s actuator_armed{}; + _actuator_armed_sub.copy(&actuator_armed); - color.red = (31.0f * (float)brightness / 255.0f); - color.green = (62.0f * (float)brightness / 255.0f); - color.blue = (31.0f * (float)brightness / 255.0f); + switch (_param_uavcan_lgt_antcl.get()) { + case 3: // Always on + return true; - return color; + case 2: // When autopilot is prearmed + return actuator_armed.armed || actuator_armed.prearmed; + + case 1: // When autopilot is armed + return actuator_armed.armed; + + case 0: // Always off + default: + return false; + } +} + +uavcan::equipment::indication::RGB565 UavcanRGBController::rgb888_to_rgb565(uint8_t red, uint8_t green, uint8_t blue) +{ + // RGB565: Full brightness is (31, 63, 31), off is (0, 0, 0) + uavcan::equipment::indication::RGB565 rgb565{}; + rgb565.red = (red * 31 + 127) / 255; + rgb565.green = (green * 63 + 127) / 255; + rgb565.blue = (blue * 31 + 127) / 255; + return rgb565; } diff --git a/src/drivers/uavcan/rgbled.hpp b/src/drivers/uavcan/rgbled.hpp index be0bc876d38..ee189019e2f 100644 --- a/src/drivers/uavcan/rgbled.hpp +++ b/src/drivers/uavcan/rgbled.hpp @@ -54,9 +54,20 @@ private: // Max update rate to avoid excessive bus traffic static constexpr unsigned MAX_RATE_HZ = 20; + // Maximum number of configurable lights + static constexpr uint8_t MAX_NUM_UAVCAN_LIGHTS = 3; + + // Light function types + enum class LightFunction : uint8_t { + Status = 0, // System status colors from led_control + AntiCollision = 1 // White beacon based on arm state + }; + void periodic_update(const uavcan::TimerEvent &); - uavcan::equipment::indication::RGB565 brightness_to_rgb565(uint8_t brightness); + bool is_anticolision_on(); ///< Evaluates current on state of collision lights accordingt to UAVCAN_LGT_ANTCL + + uavcan::equipment::indication::RGB565 rgb888_to_rgb565(uint8_t red, uint8_t green, uint8_t blue); typedef uavcan::MethodBinder TimerCbBinder; @@ -65,14 +76,21 @@ private: uavcan::Publisher _uavcan_pub_lights_cmd; uavcan::TimerEventForwarder _timer; - uORB::Subscription _armed_sub{ORB_ID(actuator_armed)}; + uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)}; LedController _led_controller; + // Cached configuration (set during init, requires reboot to change) + uint8_t _num_lights{0}; + uint8_t _light_ids[MAX_NUM_UAVCAN_LIGHTS] {}; + LightFunction _light_functions[MAX_NUM_UAVCAN_LIGHTS] {}; + + // Cached parameter handles + param_t _light_id_params[MAX_NUM_UAVCAN_LIGHTS] {}; + param_t _light_fn_params[MAX_NUM_UAVCAN_LIGHTS] {}; + DEFINE_PARAMETERS( - (ParamInt) _param_mode_anti_col, - (ParamInt) _param_mode_strobe, - (ParamInt) _param_mode_nav, - (ParamInt) _param_mode_land + (ParamInt) _param_lgt_num, + (ParamInt) _param_uavcan_lgt_antcl ) }; diff --git a/src/drivers/uavcan/uavcan_params.c b/src/drivers/uavcan/uavcan_params.c index 99a38897e45..9dd90e2e4cb 100644 --- a/src/drivers/uavcan/uavcan_params.c +++ b/src/drivers/uavcan/uavcan_params.c @@ -135,7 +135,7 @@ PARAM_DEFINE_INT32(UAVCAN_ECU_FUELT, 1); * UAVCAN ANTI_COLLISION light operating mode * * This parameter defines the minimum condition under which the system will command - * the ANTI_COLLISION lights on + * lights with anti-collision function to turn on (white). * * 0 - Always off * 1 - When autopilot is armed @@ -153,72 +153,6 @@ PARAM_DEFINE_INT32(UAVCAN_ECU_FUELT, 1); */ PARAM_DEFINE_INT32(UAVCAN_LGT_ANTCL, 2); -/** - * UAVCAN STROBE light operating mode - * - * This parameter defines the minimum condition under which the system will command - * the STROBE lights on - * - * 0 - Always off - * 1 - When autopilot is armed - * 2 - When autopilot is prearmed - * 3 - Always on - * - * @min 0 - * @max 3 - * @value 0 Always off - * @value 1 When autopilot is armed - * @value 2 When autopilot is prearmed - * @value 3 Always on - * @reboot_required true - * @group UAVCAN - */ -PARAM_DEFINE_INT32(UAVCAN_LGT_STROB, 1); - -/** - * UAVCAN RIGHT_OF_WAY light operating mode - * - * This parameter defines the minimum condition under which the system will command - * the RIGHT_OF_WAY lights on - * - * 0 - Always off - * 1 - When autopilot is armed - * 2 - When autopilot is prearmed - * 3 - Always on - * - * @min 0 - * @max 3 - * @value 0 Always off - * @value 1 When autopilot is armed - * @value 2 When autopilot is prearmed - * @value 3 Always on - * @reboot_required true - * @group UAVCAN - */ -PARAM_DEFINE_INT32(UAVCAN_LGT_NAV, 3); - -/** - * UAVCAN LIGHT_ID_LANDING light operating mode - * - * This parameter defines the minimum condition under which the system will command - * the LIGHT_ID_LANDING lights on - * - * 0 - Always off - * 1 - When autopilot is armed - * 2 - When autopilot is prearmed - * 3 - Always on - * - * @min 0 - * @max 3 - * @value 0 Always off - * @value 1 When autopilot is armed - * @value 2 When autopilot is prearmed - * @value 3 Always on - * @reboot_required true - * @group UAVCAN - */ -PARAM_DEFINE_INT32(UAVCAN_LGT_LAND, 0); - /** * publish Arming Status stream * diff --git a/src/modules/commander/ModeManagement.cpp b/src/modules/commander/ModeManagement.cpp index ae2dee41ff9..b7212f05b82 100644 --- a/src/modules/commander/ModeManagement.cpp +++ b/src/modules/commander/ModeManagement.cpp @@ -107,7 +107,7 @@ uint8_t Modes::addExternalMode(const Modes::Mode &mode) int matching_idx = -1; for (int i = 0; i < MAX_NUM; ++i) { - char hash_param_name[20]; + char hash_param_name[17]; snprintf(hash_param_name, sizeof(hash_param_name), "COM_MODE%d_HASH", i); const param_t handle = param_find(hash_param_name); int32_t current_hash{}; @@ -164,7 +164,7 @@ uint8_t Modes::addExternalMode(const Modes::Mode &mode) if (new_mode_idx != -1 && !_modes[new_mode_idx].valid) { if (need_to_update_param) { - char hash_param_name[20]; + char hash_param_name[17]; snprintf(hash_param_name, sizeof(hash_param_name), "COM_MODE%d_HASH", new_mode_idx); const param_t handle = param_find(hash_param_name); diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp index 093185e2405..8a033770cec 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp @@ -80,7 +80,7 @@ void ActuatorEffectivenessControlSurfaces::updateParams() // Helper to check if a PWM center parameter is enabled, and clamp it to valid range auto check_pwm_center = [](const char *prefix, int channel) -> bool { - char param_name[20]; + char param_name[17]; snprintf(param_name, sizeof(param_name), "%s_CENT%d", prefix, channel); param_t param = param_find(param_name);