From 06dfc4b78287d50e253afb886e77d5322f43d7f2 Mon Sep 17 00:00:00 2001 From: vertiq-jordan <120586722+vertiq-jordan@users.noreply.github.com> Date: Tue, 24 Sep 2024 11:44:35 -0400 Subject: [PATCH] drivers/actuators: new vertiq_io driver (#22892) * brought in the Vertiq Cpp API as a submodule. updated the serial rx handling so that we can parse out IQUART data packets --------- Co-authored-by: Luca Scheuer --- .gitmodules | 4 + boards/px4/fmu-v6c/default.px4board | 2 + .../actuators/vertiq_io/CMakeLists.txt | 80 ++++ src/drivers/actuators/vertiq_io/Kconfig | 17 + .../actuators/vertiq_io/entry_wrapper.hpp | 181 ++++++++ .../vertiq_io/iq-module-communication-cpp | 1 + src/drivers/actuators/vertiq_io/module.yaml | 289 +++++++++++++ .../vertiq_io/vertiq_client_manager.cpp | 65 +++ .../vertiq_io/vertiq_client_manager.hpp | 89 ++++ .../vertiq_configuration_handler.cpp | 147 +++++++ .../vertiq_configuration_handler.hpp | 182 ++++++++ src/drivers/actuators/vertiq_io/vertiq_io.cpp | 391 ++++++++++++++++++ src/drivers/actuators/vertiq_io/vertiq_io.hpp | 207 ++++++++++ .../vertiq_io/vertiq_serial_interface.cpp | 245 +++++++++++ .../vertiq_io/vertiq_serial_interface.hpp | 132 ++++++ .../vertiq_io/vertiq_telemetry_manager.cpp | 202 +++++++++ .../vertiq_io/vertiq_telemetry_manager.hpp | 152 +++++++ 17 files changed, 2386 insertions(+) create mode 100644 src/drivers/actuators/vertiq_io/CMakeLists.txt create mode 100644 src/drivers/actuators/vertiq_io/Kconfig create mode 100644 src/drivers/actuators/vertiq_io/entry_wrapper.hpp create mode 160000 src/drivers/actuators/vertiq_io/iq-module-communication-cpp create mode 100644 src/drivers/actuators/vertiq_io/module.yaml create mode 100644 src/drivers/actuators/vertiq_io/vertiq_client_manager.cpp create mode 100644 src/drivers/actuators/vertiq_io/vertiq_client_manager.hpp create mode 100644 src/drivers/actuators/vertiq_io/vertiq_configuration_handler.cpp create mode 100644 src/drivers/actuators/vertiq_io/vertiq_configuration_handler.hpp create mode 100644 src/drivers/actuators/vertiq_io/vertiq_io.cpp create mode 100644 src/drivers/actuators/vertiq_io/vertiq_io.hpp create mode 100644 src/drivers/actuators/vertiq_io/vertiq_serial_interface.cpp create mode 100644 src/drivers/actuators/vertiq_io/vertiq_serial_interface.hpp create mode 100644 src/drivers/actuators/vertiq_io/vertiq_telemetry_manager.cpp create mode 100644 src/drivers/actuators/vertiq_io/vertiq_telemetry_manager.hpp diff --git a/.gitmodules b/.gitmodules index a783b16938..8950bcc750 100644 --- a/.gitmodules +++ b/.gitmodules @@ -83,3 +83,7 @@ [submodule "boards/modalai/voxl2/libfc-sensor-api"] path = boards/modalai/voxl2/libfc-sensor-api url = https://gitlab.com/voxl-public/voxl-sdk/core-libs/libfc-sensor-api.git +[submodule "src/drivers/actuators/vertiq_io/iq-module-communication-cpp"] + path = src/drivers/actuators/vertiq_io/iq-module-communication-cpp + url = https://github.com/PX4/iq-module-communication-cpp.git + branch = master diff --git a/boards/px4/fmu-v6c/default.px4board b/boards/px4/fmu-v6c/default.px4board index adae635537..31445bc090 100644 --- a/boards/px4/fmu-v6c/default.px4board +++ b/boards/px4/fmu-v6c/default.px4board @@ -5,6 +5,8 @@ CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS6" CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS5" CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3" CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1" +CONFIG_DRIVERS_ACTUATORS_VERTIQ_IO=y +CONFIG_USE_IFCI_CONFIGURATION=y CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_MS5611=y CONFIG_DRIVERS_BATT_SMBUS=y diff --git a/src/drivers/actuators/vertiq_io/CMakeLists.txt b/src/drivers/actuators/vertiq_io/CMakeLists.txt new file mode 100644 index 0000000000..4bf66cc247 --- /dev/null +++ b/src/drivers/actuators/vertiq_io/CMakeLists.txt @@ -0,0 +1,80 @@ +############################################################################ +# +# Copyright (c) 2024 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. +# +############################################################################ + +#build our submodule as its own library so that we can avoid changing it at all +include_directories( + iq-module-communication-cpp/inc + ) + +#add all of our submodule sources +set(SOURCES + iq-module-communication-cpp/src/generic_interface.cpp + iq-module-communication-cpp/inc/client_communication.cpp + iq-module-communication-cpp/src/packet_finder.c + iq-module-communication-cpp/src/crc_helper.c + iq-module-communication-cpp/src/byte_queue.c + ) + +add_library(iq_communication ${SOURCES}) +add_dependencies(iq_communication prebuild_targets) + +if("${PX4_PLATFORM}" MATCHES "nuttx") + target_compile_definitions(iq_communication PUBLIC __NUTTX__) +endif() + +px4_add_git_submodule(TARGET git_iq-module-communication-cpp PATH "iq-module-communication-cpp") + +px4_add_module( + MODULE drivers__actuators__vertiq_io + MAIN vertiq_io + INCLUDES + COMPILE_FLAGS + SRCS + vertiq_io.cpp + vertiq_io.hpp + vertiq_serial_interface.cpp + vertiq_serial_interface.hpp + vertiq_telemetry_manager.cpp + vertiq_telemetry_manager.hpp + vertiq_client_manager.cpp + vertiq_client_manager.hpp + vertiq_configuration_handler.cpp + vertiq_configuration_handler.hpp + entry_wrapper.hpp + DEPENDS + px4_work_queue + mixer_module + iq_communication + MODULE_CONFIG + module.yaml + ) diff --git a/src/drivers/actuators/vertiq_io/Kconfig b/src/drivers/actuators/vertiq_io/Kconfig new file mode 100644 index 0000000000..014f44b92c --- /dev/null +++ b/src/drivers/actuators/vertiq_io/Kconfig @@ -0,0 +1,17 @@ +menuconfig DRIVERS_ACTUATORS_VERTIQ_IO + bool "vertiq_io" + default n + ---help--- + Enable support for vertiq_io + +if DRIVERS_ACTUATORS_VERTIQ_IO + config USE_IFCI_CONFIGURATION + bool "Include IFCI Configuration Parameters" + default n + + if USE_IFCI_CONFIGURATION + config USE_PULSING_CONFIGURATION + bool "Include Pulsing Module Configuration Parameters" + default n + endif +endif diff --git a/src/drivers/actuators/vertiq_io/entry_wrapper.hpp b/src/drivers/actuators/vertiq_io/entry_wrapper.hpp new file mode 100644 index 0000000000..2d44ea368c --- /dev/null +++ b/src/drivers/actuators/vertiq_io/entry_wrapper.hpp @@ -0,0 +1,181 @@ +/**************************************************************************** +* +* Copyright (c) 2024 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. +* +****************************************************************************/ + +#ifndef ENTRY_WRAPPER_HPP +#define ENTRY_WRAPPER_HPP + +#include + +#include +#include + +#include "vertiq_serial_interface.hpp" +#include "iq-module-communication-cpp/inc/client_communication.hpp" + +class AbstractEntryWrapper +{ +public: + AbstractEntryWrapper() {} + virtual void ConfigureStruct(param_t parameter, ClientEntryAbstract *entry) = 0; + virtual void SetNeedsInit() = 0; + virtual ClientEntryAbstract *GetClientEntry() = 0; + virtual void SendGet(VertiqSerialInterface *ser) = 0; + virtual void Update(VertiqSerialInterface *serial_interface) = 0; +}; + +template +class EntryWrapper : public AbstractEntryWrapper +{ +public: + + param_t _param; + ClientEntry *_entry; + bool _needs_init; + + px4_data_type _px4_value; + + EntryWrapper() {} + + /** + * @brief Initializes our EntryWrapper with a PX4 parameter and a Vetiq entry + * + * @param parameter A parameter stored in PX4. This can be found with the param_find function + * @param entry A pointer to a Vertiq client entry + */ + void ConfigureStruct(param_t parameter, ClientEntryAbstract *entry) + { + _param = parameter; + _entry = (ClientEntry *)entry; + _needs_init = true; + } + + /** + * @brief Sets our _needs_init flag high + */ + void SetNeedsInit() + { + _needs_init = true; + } + + /** + * @brief Returns a pointer to our Vertiq entry + * + * @return _entry + */ + ClientEntryAbstract *GetClientEntry() + { + return _entry; + } + + /** + * @brief Sends a get message to our Vertiq entry + * + * @param ser A pointer to our serial interface + */ + void SendGet(VertiqSerialInterface *ser) + { + _entry->get(*ser->GetIquartInterface()); + ser->ProcessSerialTx(); + } + + /** + * @brief Sends both a set and save message to our Vertiq entry with a new value + * + * @param ser A pointer to our serial interface + * @param value The new value we are setting and saving on the connected motor + */ + void SendSetAndSave(VertiqSerialInterface *ser, module_data_type value) + { + _entry->set(*ser->GetIquartInterface(), value); + _entry->save(*ser->GetIquartInterface()); + ser->ProcessSerialTx(); + } + + /** + * @brief Checks to see if two values are the same. We will treat all parameters as floats regardless of their given type + * + * @param val1 The first value to compare + * @param val2 The second value to compare + * @param tolerance A tolerance to use in order to tell if the values are the same + * @return true If the values are the same + * @return false If the values are different + */ + bool ValuesAreTheSame(float val1, float val2, float tolerance = 0.001f) + { + float diff = val1 - val2; + + if (diff < 0) { + return -diff < tolerance; + } + + return diff < tolerance; + } + + /** + * @brief Updates the relationship between the Vertiq and PX4 parameters. First, check to see if the connected motor sent data for us to read. Then, depending on the _needs_init + * flag, we either set the gotten value into the PX4 parameter, or we set the new PX4 value to the motor. + * + * @param serial_interface A pointer to a serial interface used to send data + */ + void Update(VertiqSerialInterface *serial_interface) + { + //Make sure we have a way to store the data from PX4 and from the module + module_data_type module_value = 0; + px4_data_type px4_value = 0; + + //If there's new data in the entry + if (_entry->IsFresh()) { + //Grab the data + module_value = _entry->get_reply(); + + //If we need to set the PX4 data to the module's value, otherwise set the module value + if (_needs_init) { + px4_value = (px4_data_type)module_value; + param_set(_param, &px4_value); + _needs_init = false; + + } else { + //Grab the PX4 value + param_get(_param, &px4_value); + + //If the value here is different from the module's value, set and save it + //Treat everything as a float to avoid type issues in comparison + if (!ValuesAreTheSame((float)px4_value, (float)module_value)) { + SendSetAndSave(serial_interface, px4_value); + } + } + } + } +}; + +#endif //ENTRY_WRAPPER_HPP diff --git a/src/drivers/actuators/vertiq_io/iq-module-communication-cpp b/src/drivers/actuators/vertiq_io/iq-module-communication-cpp new file mode 160000 index 0000000000..a9b700d50b --- /dev/null +++ b/src/drivers/actuators/vertiq_io/iq-module-communication-cpp @@ -0,0 +1 @@ +Subproject commit a9b700d50bdd06a837c74750ac3c4760937333df diff --git a/src/drivers/actuators/vertiq_io/module.yaml b/src/drivers/actuators/vertiq_io/module.yaml new file mode 100644 index 0000000000..cded74a8eb --- /dev/null +++ b/src/drivers/actuators/vertiq_io/module.yaml @@ -0,0 +1,289 @@ +module_name: Vertiq IO +serial_config: + - command: vertiq_io start -d ${SERIAL_DEV} + port_config_param: + name: VERTIQ_IO_CFG + group: Vertiq IO + +actuator_output: + output_groups: + - param_prefix: VTQ_IO + group_label: 'CVIs' + channel_label: 'CVI' + instance_start: 0 + standard_params: + disarmed: { min: 0, max: 65535, default: 0 } + min: { min: 0, max: 65535, default: 0 } + max: { min: 0, max: 65535, default: 65535 } + failsafe: { min: 0, max: 500 } + num_channels: 16 + +parameters: + - group: Vertiq IO + definitions: + VTQ_BAUD: + description: + short: The IQUART driver's baud rate + long: | + The baud rate (in bits per second) used by the serial port connected with IQUART communication + type: int32 + reboot_required: true + default: 115200 + VTQ_NUM_CVS: + description: + short: The number of Vertiq IFCI parameters to use + long: | + The total number of IFCI control variables being used across all connected modules + type: int32 + min: 0 + max: 16 + reboot_required: true + default: 0 + VTQ_DISARM_TRIG: + description: + short: The triggered behavior sent to the motors on PX4 disarm + long: | + The behavior triggered when the flight controller disarms. You have the option to trigger your motors' disarm behaviors, set all motors to coast, + or set a predefined throttle setpoint + type: enum + values: + 0: Send Explicit Disarm + 1: Coast Motors + 2: Set Predefined Velocity Setpoint + default: 0 + VTQ_DISARM_VELO: + description: + short: Velocity sent when DISARM_TRIGGER is Set Predefined Velocity Setpoint + long: This is the velocity that will be sent to all motors when PX4 is disarmed and DISARM_TRIGGER is Set Predefined Velocity Setpoint + type: int32 + min: 0 + max: 100 + default: 0 + VTQ_ARM_BEHAVE: + description: + short: The triggered behavior on PX4 arm + long: | + The behavior triggered when the flight controller arms. You have the option to use your motors' arming behaviors, or to force all of your motors to arm + type: enum + values: + 0: Use Motor Arm Behavior + 1: Send Explicit Arm Command + default: 0 + VTQ_TRGT_MOD_ID: + description: + short: The Module ID of the module you would like to communicate with + long: | + This is the value used as the target module ID of all configuration parameters (not operational parameters). The Vertiq module with the + module ID matching this value will react to all get and set requests from PX4. Any Vertiq client made with dynamic object IDs should + use this value to instantiate itself. + type: int32 + default: 0 + VTQ_REDO_READ: + description: + short: Reinitialize the target module's values into the PX4 parameters + long: | + Setting this value to true will reinitialize PX4's IQUART connected parameters to the value stored on the currently targeted motor. + This is especially useful if your flight controller powered on before your connected modules + type: boolean + default: 0 + VTQ_THROTTLE_CVI: + description: + short: Module Param - The module's Throttle Control Value Index + long: | + This represents the Control Value Index where the targeted module will look for throttle commands + type: int32 + min: 0 + max: 255 + default: 0 + VTQ_CONTROL_MODE: + description: + short: Module Param - The module's control mechanism + long: | + PWM Mode: Commands a fraction of battery voltage. This changes as the battery voltage changes. + This is the least safe mode because the upper throttle limit is determined by the battery voltage. + Voltage Mode: Commands a voltage. The motor will behave the same way throughout the life of a battery, assuming the commanded voltage is less than the battery voltage. + You must set the MAX_VOLTS parameter. + Velocity Mode: Closed-loop, commands a velocity. The controller will adjust the applied voltage so that the motor spins at the commanded velocity. + This mode has faster reaction times. Only use this if you know the properties of your propeller. You must set the MAX_VELOCITY parameter. + type: enum + values: + 0: PWM + 1: Voltage + 2: Velocity + default: 0 + VTQ_MAX_VELOCITY: + description: + short: Module Param - Maximum velocity when CONTROL_MODE is set to Velocity + long: | + Only relevant in Velocity Mode. This is the velocity the controller will command at full throttle. + type: float + default: 0 + VTQ_MAX_VOLTS: + description: + short: Module Param - Maximum voltage when CONTROL_MODE is set to Voltage + long: | + Only relevant in Voltage Mode. This is the voltage the controller will command at full throttle. + type: float + default: 0 + VTQ_MOTOR_DIR: + description: + short: Module Param - The direction that the module should spin + long: | + Set the targeted motor's spinning direction (clockwise vs. counter clockwise) and flight mode (2D non-reversible vs. 3D reversible) + type: enum + values: + 0: Unconfigured + 1: 3D Counter Clockwise + 2: 3D Clockwise + 3: 2D Counter Clockwise + 4: 2D Clockwise + default: 0 + VTQ_FC_DIR: + description: + short: Module Param - If the flight controller uses 2D or 3D communication + long: | + The FC and the ESC must agree upon the meaning of the signal coming out of the ESC. When FCs are in 3D mode + they re-map negative signals. This parameter keeps the FC and ESC in agreement. + type: enum + values: + 0: 2D + 1: 3D + default: 0 + VTQ_ZERO_ANGLE: + description: + short: Module Param - The encoder angle at which theta is zero + long: | + The encoder angle at which theta is zero. Adjust this number to change the location of 0 phase when pulsing. + type: float + default: 0 + VTQ_VELO_CUTOFF: + description: + short: Module Param - The minimum velocity required to allow pulsing + long: | + This is the velocity at which pulsing is allowed. Any velocity between VELOCITY_CUTOFF and -VELOCITY_CUTOFF will not pulse. + type: float + default: 0 + VTQ_TQUE_OFF_ANG: + description: + short: Module Param - Offsets pulse angle to allow for mechanical properties + long: | + This offsets where the pulse starts around the motor to allow for propeller mechanical properties. + type: float + default: 0 + VTQ_PULSE_V_MODE: + description: + short: Module Param - 0 = Supply Voltage Mode, 1 = Voltage Limit Mode + long: | + Supply Voltage Mode means that the maximum voltage applied to pulsing is the supplied voltage. Voltage Limit Mode + indicates that PULSE_VOLT_LIM is the maximum allowed voltage to apply towards pulsing. + type: enum + values: + 0: Supply Voltage Mode + 1: Voltage Limit Mode + default: 0 + VTQ_PULSE_V_LIM: + description: + short: Module Param - Max pulsing voltage limit when in Voltage Limit Mode + long: | + This sets the max pulsing voltage limit when in Voltage Limit Mode. + type: float + default: 0 + VTQ_X_CVI: + description: + short: Module Param - CVI for the X rectangular coordinate + long: | + This represents the Control Value Index where the targeted module will look for the X rectangular coordinate. + type: int32 + min: 0 + max: 255 + default: 0 + VTQ_Y_CVI: + description: + short: Module Param - CVI for the Y rectangular coordinate + long: | + This represents the Control Value Index where the targeted module will look for the Y rectangular coordinate. + type: int32 + min: 0 + max: 255 + default: 0 + VTQ_TELEM_IDS_1: + description: + short: Module IDs [0, 31] that you would like to request telemetry from + long: | + The module IDs [0, 31] that should be asked for telemetry. The data received from these IDs will be published via the esc_status topic. + type: bitmask + bit: + 0: Module ID 0 + 1: Module ID 1 + 2: Module ID 2 + 3: Module ID 3 + 4: Module ID 4 + 5: Module ID 5 + 6: Module ID 6 + 7: Module ID 7 + 8: Module ID 8 + 9: Module ID 9 + 10: Module ID 10 + 11: Module ID 11 + 12: Module ID 12 + 13: Module ID 13 + 14: Module ID 14 + 15: Module ID 15 + 16: Module ID 16 + 17: Module ID 17 + 18: Module ID 18 + 19: Module ID 19 + 20: Module ID 20 + 21: Module ID 21 + 22: Module ID 22 + 23: Module ID 23 + 24: Module ID 24 + 25: Module ID 25 + 26: Module ID 26 + 27: Module ID 27 + 28: Module ID 28 + 29: Module ID 29 + 30: Module ID 30 + 31: Module ID 31 + reboot_required: true + default: 0 + VTQ_TELEM_IDS_2: + description: + short: Module IDs [32, 62] that you would like to request telemetry from + long: | + The module IDs [32, 62] that should be asked for telemetry. The data received from these IDs will be published via the esc_status topic. + type: bitmask + bit: + 32: Module ID 32 + 33: Module ID 33 + 34: Module ID 34 + 35: Module ID 35 + 36: Module ID 36 + 37: Module ID 37 + 38: Module ID 38 + 39: Module ID 39 + 40: Module ID 40 + 41: Module ID 41 + 42: Module ID 42 + 43: Module ID 43 + 44: Module ID 44 + 45: Module ID 45 + 46: Module ID 46 + 47: Module ID 47 + 48: Module ID 48 + 49: Module ID 49 + 50: Module ID 50 + 51: Module ID 51 + 52: Module ID 52 + 53: Module ID 53 + 54: Module ID 54 + 55: Module ID 55 + 56: Module ID 56 + 57: Module ID 57 + 58: Module ID 58 + 59: Module ID 59 + 60: Module ID 60 + 61: Module ID 61 + 62: Module ID 62 + reboot_required: true + default: 0 diff --git a/src/drivers/actuators/vertiq_io/vertiq_client_manager.cpp b/src/drivers/actuators/vertiq_io/vertiq_client_manager.cpp new file mode 100644 index 0000000000..1c89bb38d8 --- /dev/null +++ b/src/drivers/actuators/vertiq_io/vertiq_client_manager.cpp @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "vertiq_client_manager.hpp" + +VertiqClientManager::VertiqClientManager(VertiqSerialInterface *serial_interface) : + _serial_interface(serial_interface) +{ +} + +void VertiqClientManager::HandleClientCommunication() +{ + //Called periodically in the main loop to handle all communications not handled directly by + //parameter setting + //Update our serial tx before we take in the rx + _serial_interface->ProcessSerialTx(); + + //Update our serial rx for all clients + _serial_interface->ProcessSerialRx(_client_array, _clients_in_use); +} + +void VertiqClientManager::AddNewClient(ClientAbstract *client) +{ + if (_clients_in_use < MAXIMUM_NUMBER_OF_CLIENTS) { + _client_array[_clients_in_use] = client; + _clients_in_use++; + + } else { + PX4_INFO("Could not add this client. Maximum number exceeded"); + } +} + +uint8_t VertiqClientManager::GetNumberOfClients() +{ + return _clients_in_use; +} diff --git a/src/drivers/actuators/vertiq_io/vertiq_client_manager.hpp b/src/drivers/actuators/vertiq_io/vertiq_client_manager.hpp new file mode 100644 index 0000000000..62cb4525e9 --- /dev/null +++ b/src/drivers/actuators/vertiq_io/vertiq_client_manager.hpp @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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. + * + ****************************************************************************/ +#ifndef VERTIQ_CLLIENT_MANAGER_HPP +#define VERTIQ_CLLIENT_MANAGER_HPP + +#include +#include +#include +#include + +#include + +#include +#include + +#include "vertiq_serial_interface.hpp" + +static const uint8_t _kBroadcastID = 63; + +class VertiqClientManager +{ +public: + /** + * @brief Construct a new VertiqClientManager object. It is responsible for accepting new Vertiq client objects and handling + * all of their communication processing + * + * @param serial_interface A pointer to a VertiqSerialInterface object + */ + VertiqClientManager(VertiqSerialInterface *serial_interface); + + /** + * @brief Handle the IQUART interface. Make sure that we update TX and RX buffers + */ + void HandleClientCommunication(); + + /** + * @brief Adds a new client to our array of Clients + */ + void AddNewClient(ClientAbstract *client); + + /** + * @brief Returns the number of clients added to our Clients array + * + * @return The value _clients_in_use + */ + uint8_t GetNumberOfClients(); + +private: + //We need a serial handler in order to talk over the serial port + VertiqSerialInterface *_serial_interface; + + //Some constants to help us out + static const uint8_t MAXIMUM_NUMBER_OF_CLIENTS = + 35; //These are clients that are used for module control/telemetry. They have a static Module ID + ClientAbstract *_client_array[MAXIMUM_NUMBER_OF_CLIENTS]; + uint8_t _clients_in_use = 0; +}; + +#endif diff --git a/src/drivers/actuators/vertiq_io/vertiq_configuration_handler.cpp b/src/drivers/actuators/vertiq_io/vertiq_configuration_handler.cpp new file mode 100644 index 0000000000..c80096ed2f --- /dev/null +++ b/src/drivers/actuators/vertiq_io/vertiq_configuration_handler.cpp @@ -0,0 +1,147 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "vertiq_configuration_handler.hpp" + +VertiqConfigurationHandler::VertiqConfigurationHandler(VertiqSerialInterface *ser, + VertiqClientManager *client_manager) : + _serial_interface(ser), + _client_manager(client_manager) +{ +} + +void VertiqConfigurationHandler::InitConfigurationClients(uint8_t object_id) +{ + _object_id_now = object_id; //Make sure we store the initial object ID + + _prop_input_parser_client = new EscPropellerInputParserClient(object_id); + _client_manager->AddNewClient(_prop_input_parser_client); + +#ifdef CONFIG_USE_IFCI_CONFIGURATION + _ifci_client = new IQUartFlightControllerInterfaceClient(object_id); + _client_manager->AddNewClient(_ifci_client); +#endif //CONFIG_USE_IFCI_CONFIGURATION + +#ifdef CONFIG_USE_PULSING_CONFIGURATION + _voltage_superposition_client = new VoltageSuperPositionClient(object_id); + _client_manager->AddNewClient(_voltage_superposition_client); + + _pulsing_rectangular_input_parser_client = new PulsingRectangularInputParserClient(object_id); + _client_manager->AddNewClient(_pulsing_rectangular_input_parser_client); +#endif //CONFIG_USE_PULSING_CONFIGURATION +} + +void VertiqConfigurationHandler::InitClientEntryWrappers() +{ + AddNewClientEntry(param_find("VTQ_MAX_VELOCITY"), &(_prop_input_parser_client->velocity_max_)); + AddNewClientEntry(param_find("VTQ_MAX_VOLTS"), &(_prop_input_parser_client->volts_max_)); + AddNewClientEntry(param_find("VTQ_CONTROL_MODE"), &(_prop_input_parser_client->mode_)); + AddNewClientEntry(param_find("VTQ_MOTOR_DIR"), &(_prop_input_parser_client->sign_)); + AddNewClientEntry(param_find("VTQ_FC_DIR"), &(_prop_input_parser_client->flip_negative_)); + +#ifdef CONFIG_USE_IFCI_CONFIGURATION + AddNewClientEntry(param_find("VTQ_THROTTLE_CVI"), &(_ifci_client->throttle_cvi_)); +#endif //CONFIG_USE_IFCI_CONFIGURATION + +#ifdef CONFIG_USE_PULSING_CONFIGURATION + AddNewClientEntry (param_find("VTQ_PULSE_V_MODE"), + &(_pulsing_rectangular_input_parser_client->pulsing_voltage_mode_)); + AddNewClientEntry(param_find("VTQ_X_CVI"), &(_ifci_client->x_cvi_)); + AddNewClientEntry(param_find("VTQ_Y_CVI"), &(_ifci_client->y_cvi_)); + AddNewClientEntry(param_find("VTQ_ZERO_ANGLE"), &(_voltage_superposition_client->zero_angle_)); + AddNewClientEntry(param_find("VTQ_VELO_CUTOFF"), + &(_voltage_superposition_client->velocity_cutoff_)); + AddNewClientEntry(param_find("VTQ_TQUE_OFF_ANG"), + &(_voltage_superposition_client->propeller_torque_offset_angle_)); + AddNewClientEntry(param_find("VTQ_PULSE_V_LIM"), + &(_pulsing_rectangular_input_parser_client->pulsing_voltage_limit_)); +#endif //CONFIG_USE_PULSING_CONFIGURATION +} + +void VertiqConfigurationHandler::UpdateClientsToNewObjId(uint8_t new_object_id) +{ + _object_id_now = new_object_id; + + DestroyAndRecreateClient(_prop_input_parser_client, new_object_id); + +#ifdef CONFIG_USE_IFCI_CONFIGURATION + DestroyAndRecreateClient(_ifci_client, new_object_id); +#endif + +#ifdef CONFIG_USE_PULSING_CONFIGURATION + DestroyAndRecreateClient(_voltage_superposition_client, new_object_id); + DestroyAndRecreateClient(_pulsing_rectangular_input_parser_client, new_object_id); +#endif +} + +void VertiqConfigurationHandler::MarkConfigurationEntriesForRefresh() +{ + for (uint8_t i = 0; i < _added_configuration_entry_wrappers; i++) { + _configuration_entry_wrappers[i]->SetNeedsInit(); + } +} + +void VertiqConfigurationHandler::UpdateIquartConfigParams() +{ + for (uint8_t i = 0; i < _added_configuration_entry_wrappers; i++) { + _configuration_entry_wrappers[i]->SendGet(_serial_interface); + //Ensure that these get messages get out + _client_manager->HandleClientCommunication(); + } + + //Now go ahead and grab responses, and update everyone to be on the same page, but do it quickly. + CoordinateIquartWithPx4Params(); +} + +void VertiqConfigurationHandler::CoordinateIquartWithPx4Params(hrt_abstime timeout) +{ + //Get the start time and the end time + hrt_abstime time_now = hrt_absolute_time(); + hrt_abstime end_time = time_now + timeout; + + while (time_now < end_time) { + for (uint8_t i = 0; i < _added_configuration_entry_wrappers; i++) { + _configuration_entry_wrappers[i]->Update(_serial_interface); + } + + //Update the time + time_now = hrt_absolute_time(); + + //Update our serial rx + _client_manager->HandleClientCommunication(); + } +} + +uint8_t VertiqConfigurationHandler::GetObjectIdNow() +{ + return _object_id_now; +} diff --git a/src/drivers/actuators/vertiq_io/vertiq_configuration_handler.hpp b/src/drivers/actuators/vertiq_io/vertiq_configuration_handler.hpp new file mode 100644 index 0000000000..57d7afa474 --- /dev/null +++ b/src/drivers/actuators/vertiq_io/vertiq_configuration_handler.hpp @@ -0,0 +1,182 @@ +/**************************************************************************** +* +* Copyright (c) 2024 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. +* +****************************************************************************/ +#ifndef VERTIQ_CONFIGURATION_CLIENT_HANDLER_HPP +#define VERTIQ_CONFIGURATION_CLIENT_HANDLER_HPP + +/** + Vertiq modules communicate through a system of clients and entries. Clients contain entries, and entries + specifiy some sort of Vertiq module parameter or control. Entries can be gotten (its value returned to the requester), set (the + module's internal value set to the value requested by the user), and saved (writes the currently stored module value to its persistent memory). + + This class exists to coordinate PX4 parameters with Vertiq entries +*/ + +#include + +#include "entry_wrapper.hpp" +#include "vertiq_client_manager.hpp" +#include "iq-module-communication-cpp/inc/esc_propeller_input_parser_client.hpp" + +#include "iq-module-communication-cpp/inc/arming_handler_client.hpp" +#include "iq-module-communication-cpp/inc/iquart_flight_controller_interface_client.hpp" + +#ifdef CONFIG_USE_PULSING_CONFIGURATION +#include "iq-module-communication-cpp/inc/voltage_superposition_client.hpp" +#include "iq-module-communication-cpp/inc/pulsing_rectangular_input_parser_client.hpp" +#endif //CONFIG_USE_PULSING_CONFIGURATION + +class VertiqConfigurationHandler +{ +public: + + /** + * @brief Construct a new Vertiq Configuration Handler object + * + * @param ser A pointer to our serial interface + * @param client_manager A pointer to our client manager + */ + VertiqConfigurationHandler(VertiqSerialInterface *ser, VertiqClientManager *client_manager); + + /** + * @brief Initialize all of our client entry wrappers + * + */ + void InitClientEntryWrappers(); + + /** + * @brief Set all of the IQUART configuration init flags to true + */ + void MarkConfigurationEntriesForRefresh(); + + /** + * @brief Send a Get command to all of the parameters involved in IQUART configuration, and make sure the PX4 parameters and module values agree + */ + void UpdateIquartConfigParams(); + + /** + * @brief Until the timeout is reached, keep trying to update the PX4 parameters to match, as appropraite, the value on the module. This can + * mean either setting the PX4 parameter to match the motor or vice versa + * + * @param timeout The maximum amount of time we can keep trying to get responses + */ + void CoordinateIquartWithPx4Params(hrt_abstime timeout = 100_ms); + + /** + * @brief Gives access to the object ID currently being used + * + * @return The value stored in _object_id_now + */ + uint8_t GetObjectIdNow(); + + /** + * @brief When the target module ID parameter changes, we need to delete and remake all of our configuration clients in order to make sure that they're talking to the + * correct motor. + * + * @param new_object_id The new target module ID that we should use to instantiate our new clients + */ + void UpdateClientsToNewObjId(uint8_t new_object_id); + + /** + * @brief Initialize all of the Vertiq Clients that we want to use + * + * @param object_id The object ID with which to initialize our clients + */ + void InitConfigurationClients(uint8_t object_id); + + /** + * @brief Delete and recreate a client with a new object ID + * + * @tparam client_type The actual type of Client you want to delete and remake + * @param client A pointer ref to the client being deleted + * @param object_id The object id we're making the new client with + */ + template + void DestroyAndRecreateClient(client_type *&client, uint8_t object_id) + { + if (client) { + delete client; + client = new client_type(object_id); + } + } + + /** + * @brief Creates and adds a new entry wrapper object to our array of entry wrappers + * + * @param px4_param A parameter stored in PX4. This can be found with the param_find function + * @param entry A pointer to a Vertiq client entry + */ + template + void AddNewClientEntry(param_t px4_param, ClientEntryAbstract *entry) + { + if (_added_configuration_entry_wrappers < MAX_CLIENT_ENTRIES) { + _configuration_entry_wrappers[_added_configuration_entry_wrappers] = new EntryWrapper; + _configuration_entry_wrappers[_added_configuration_entry_wrappers]->ConfigureStruct(px4_param, entry); + _added_configuration_entry_wrappers++; + + } else { + PX4_INFO("Could not add this entry. Maximum number exceeded"); + } + } + +private: + + uint8_t _object_id_now; //The object ID we're targeting right now + VertiqSerialInterface *_serial_interface; //We need a serial handler in order to talk over the serial port + VertiqClientManager *_client_manager; + +//////////////////////////////////////////////////////////////////////// +//Vertiq Client information + //Known Configuration Clients can be created as pointers to certain types of clients + EscPropellerInputParserClient *_prop_input_parser_client; + +#ifdef CONFIG_USE_IFCI_CONFIGURATION + //Make all of the clients that we need to talk to the IFCI config params + IQUartFlightControllerInterfaceClient *_ifci_client; +#endif //CONFIG_USE_IFCI_CONFIGURATION + +#ifdef CONFIG_USE_PULSING_CONFIGURATION + VoltageSuperPositionClient *_voltage_superposition_client; + PulsingRectangularInputParserClient *_pulsing_rectangular_input_parser_client; +#endif //CONFIG_USE_PULSING_CONFIGURATION +//////////////////////////////////////////////////////////////////////// + +//////////////////////////////////////////////////////////////////////// +//Vertiq Client Entry information + static const uint8_t MAX_CLIENT_ENTRIES = 40; + AbstractEntryWrapper *_configuration_entry_wrappers[MAX_CLIENT_ENTRIES]; + uint8_t _added_configuration_entry_wrappers = 0; +//////////////////////////////////////////////////////////////////////// + +}; + +#endif //VERTIQ_CONFIGURATION_CLIENT_HANDLER_HPP diff --git a/src/drivers/actuators/vertiq_io/vertiq_io.cpp b/src/drivers/actuators/vertiq_io/vertiq_io.cpp new file mode 100644 index 0000000000..80add143fd --- /dev/null +++ b/src/drivers/actuators/vertiq_io/vertiq_io.cpp @@ -0,0 +1,391 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "vertiq_io.hpp" + +#include + +px4::atomic_bool VertiqIo::_request_telemetry_init{false}; + +VertiqIo::VertiqIo(const char *port) : + OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default), + _serial_interface(), + _client_manager(&_serial_interface), + _telem_manager(&_client_manager), + _configuration_handler(&_serial_interface, &_client_manager), + _broadcast_prop_motor_control(_kBroadcastID), + _broadcast_arming_handler(_kBroadcastID), + _operational_ifci(_kBroadcastID) +{ + // store port name + strncpy(_port, port, sizeof(_port) - 1); + + // enforce null termination + _port[sizeof(_port) - 1] = '\0'; + + //Make sure we get the correct initial values for our parameters + updateParams(); + + _configuration_handler.InitConfigurationClients((uint8_t)_param_vertiq_target_module_id.get()); + _configuration_handler.InitClientEntryWrappers(); + + _client_manager.AddNewClient(&_operational_ifci); + _client_manager.AddNewClient(&_broadcast_arming_handler); + _client_manager.AddNewClient(&_broadcast_prop_motor_control); +} + +VertiqIo::~VertiqIo() +{ + //Be inactive + stop(); + + //Free our counters/timers + perf_free(_loop_perf); + perf_free(_loop_interval_perf); +} + +//called by our task_spawn function +bool VertiqIo::init() +{ + _serial_interface.InitSerial(_port, _param_vertiq_baud.get()); + +#ifdef CONFIG_USE_IFCI_CONFIGURATION + //Grab the number of IFCI control values the user wants to use + _cvs_in_use = (uint8_t)_param_vertiq_number_of_cvs.get(); + + //Grab the bitmask that we're going to use to decide who we get telemetry from + _telemetry_ids_1 = (uint32_t)_param_vertiq_telem_ids_1.get(); + _telemetry_ids_2 = (uint32_t)_param_vertiq_telem_ids_2.get(); + _telem_bitmask = ((uint64_t)(_telemetry_ids_2) << 32) | _telemetry_ids_1; + + _transmission_message.num_cvs = _cvs_in_use; +#endif + + //Initialize our telemetry handler + _telem_manager.Init(_telem_bitmask, (uint8_t)_param_vertiq_target_module_id.get()); + _telem_manager.StartPublishing(&_esc_status_pub); + + //Make sure we get our thread into execution + ScheduleNow(); + + return true; +} + +void VertiqIo::start() +{ + ScheduleNow(); +} + +void VertiqIo::stop() +{ + ScheduleClear(); +} + + +//This is the same as a while(1) loop +void VertiqIo::Run() +{ + //Start the loop timer + //Increment our loop counter + perf_begin(_loop_perf); + perf_count(_loop_interval_perf); + + //Handle IQUART reception and transmission + _client_manager.HandleClientCommunication(); + + // If we're supposed to ask for telemetry from someone + if (_telem_bitmask) { + //Grab the next ID, and if it's real get it ready to send out + uint16_t next_telem = _telem_manager.UpdateTelemetry(); + + if (next_telem != _impossible_module_id) { + _transmission_message.telem_byte = next_telem; + } + } + + //Get the most up to date version of our parameters/check if they've changed + parameters_update(); + + //Make sure we also update the mixing output to get the most up to date configuration + _mixing_output.update(); + _mixing_output.updateSubscriptions(true); + + //Go ahead and check to see if our actuator test has gotten anything new + if (_actuator_test_sub.updated()) { + _actuator_test_sub.copy(&_actuator_test); + + //Our test is active if anyone is giving us commands through the actuator test + _actuator_test_active = _actuator_test.action == actuator_test_s::ACTION_DO_CONTROL; + } + + //stop our timer + perf_end(_loop_perf); +} + +void VertiqIo::parameters_update() +{ + //If someone has changed any parameter in our module. Checked at 1Hz + if (_parameter_update_sub.updated()) { + //Grab the changed parameter with copy (which lowers the "changed" flag) + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + // If any parameter updated, call updateParams() to check if + // this class attributes need updating (and do so). + updateParams(); + + //If the target module ID changed, we need to update all of our parameters + if (_param_vertiq_target_module_id.get() != _configuration_handler.GetObjectIdNow()) { + _configuration_handler.UpdateClientsToNewObjId(_param_vertiq_target_module_id.get()); + + //Make sure we start a new read! + _param_vertiq_trigger_read.set(true); + _param_vertiq_trigger_read.commit_no_notification(); + } + + //If you're set to re-read from the motor, mark all of the IQUART parameters for reinitialization, reset the trigger, and then update the IFCI params + if (_param_vertiq_trigger_read.get()) { + _configuration_handler.MarkConfigurationEntriesForRefresh(); + _param_vertiq_trigger_read.set(false); + _param_vertiq_trigger_read.commit_no_notification(); + } + + _configuration_handler.UpdateIquartConfigParams(); + } +} + +void VertiqIo::OutputControls(uint16_t outputs[MAX_ACTUATORS]) +{ + //Put the mixer outputs into the output message + for (uint8_t i = 0; i < _transmission_message.num_cvs; i++) { + _transmission_message.commands[i] = outputs[i]; + } + + _operational_ifci.PackageIfciCommandsForTransmission(&_transmission_message, _output_message, &_output_len); + _operational_ifci.packed_command_.set(*_serial_interface.GetIquartInterface(), _output_message, _output_len); + _serial_interface.ProcessSerialTx(); +} + +bool VertiqIo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, + unsigned num_control_groups_updated) +{ +#ifdef CONFIG_USE_IFCI_CONFIGURATION + + if (_mixing_output.armed().armed) { + + if (_param_vertiq_arm_behavior.get() == FORCE_ARMING && _send_forced_arm) { + _broadcast_arming_handler.motor_armed_.set(*_serial_interface.GetIquartInterface(), 1); + _send_forced_arm = false; + } + + OutputControls(outputs); + + //We want to make sure that we send a valid telem request only once to ensure that we're not getting extraneous responses. + //So, here we'll set the telem request ID to something that no one will respond to. Another function will take charge of setting it to a + //proper value when necessary + _transmission_message.telem_byte = _impossible_module_id; + + } else if (_actuator_test_active) { + OutputControls(outputs); + + } else { + //Put the modules into coast + switch (_param_vertiq_disarm_behavior.get()) { + case TRIGGER_MOTOR_DISARM: + _broadcast_arming_handler.motor_armed_.set(*_serial_interface.GetIquartInterface(), 0); + break; + + case COAST_MOTOR: + _broadcast_prop_motor_control.ctrl_coast_.set(*_serial_interface.GetIquartInterface()); + break; + + case SEND_PREDEFINED_VELOCITY: + _broadcast_prop_motor_control.ctrl_velocity_.set(*_serial_interface.GetIquartInterface(), + _param_vertiq_disarm_velocity.get()); + break; + + default: + break; + } + + if (!_send_forced_arm) { + _send_forced_arm = true; + } + + _actuator_test_active = false; + } + + //Publish our esc status to uORB + _esc_status_pub.publish(_telem_manager.GetEscStatus()); +#endif + return true; +} + +void VertiqIo::print_info() +{ + perf_print_counter(_loop_perf); + perf_print_counter(_loop_interval_perf); + + _mixing_output.printStatus(); +} + +/** + * Local functions in support of the shell command. + */ +namespace vertiq_namespace +{ + +VertiqIo *g_dev{nullptr}; + +int start(const char *port); +int status(); +int stop(); +int usage(); + +int +start(const char *port) +{ + if (g_dev != nullptr) { + PX4_ERR("already started"); + return PX4_OK; + } + + // Instantiate the driver. + g_dev = new VertiqIo(port); + + if (g_dev == nullptr) { + PX4_ERR("driver start failed"); + return PX4_ERROR; + } + + if (!g_dev->init()) { + PX4_ERR("driver start failed"); + delete g_dev; + g_dev = nullptr; + return PX4_ERROR; + } + + return PX4_OK; +} + +int +status() +{ + if (g_dev == nullptr) { + PX4_ERR("driver not running"); + return 1; + } + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + return 0; +} + +int stop() +{ + if (g_dev != nullptr) { + PX4_INFO("stopping driver"); + delete g_dev; + g_dev = nullptr; + PX4_INFO("driver stopped"); + + } else { + PX4_ERR("driver not running"); + return 1; + } + + return PX4_OK; +} + +int +usage() +{ + PRINT_MODULE_USAGE_NAME("vertiq_io", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_ARG("", "UART device", false); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; + return PX4_OK; +} + +} // namespace + +extern "C" __EXPORT int vertiq_io_main(int argc, char *argv[]) +{ + int ch = 0; + const char *device_path = nullptr; + int myoptind = 1; + const char *myoptarg = nullptr; + + while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + + case 'd': + device_path = myoptarg; + break; + + default: + PX4_WARN("Unknown option!"); + return PX4_ERROR; + } + } + + if (myoptind >= argc) { + PX4_ERR("unrecognized command"); + return vertiq_namespace::usage(); + } + + if (!device_path) { + PX4_ERR("Missing device"); + return PX4_ERROR; + } + + if (!strcmp(argv[myoptind], "start")) { + if (strcmp(device_path, "") != 0) { + return vertiq_namespace::start(device_path); + + } else { + PX4_WARN("Please specify device path!"); + return vertiq_namespace::usage(); + } + + } else if (!strcmp(argv[myoptind], "stop")) { + return vertiq_namespace::stop(); + + } else if (!strcmp(argv[myoptind], "status")) { + return vertiq_namespace::status(); + } + + return vertiq_namespace::usage(); +} diff --git a/src/drivers/actuators/vertiq_io/vertiq_io.hpp b/src/drivers/actuators/vertiq_io/vertiq_io.hpp new file mode 100644 index 0000000000..db8d37053e --- /dev/null +++ b/src/drivers/actuators/vertiq_io/vertiq_io.hpp @@ -0,0 +1,207 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "vertiq_telemetry_manager.hpp" +#include "vertiq_client_manager.hpp" +#include "vertiq_serial_interface.hpp" +#include "vertiq_configuration_handler.hpp" + +#include "iq-module-communication-cpp/inc/propeller_motor_control_client.hpp" +#include "iq-module-communication-cpp/inc/brushless_drive_client.hpp" +#include "iq-module-communication-cpp/inc/arming_handler_client.hpp" + +#include "iq-module-communication-cpp/inc/esc_propeller_input_parser_client.hpp" +#include "iq-module-communication-cpp/inc/iquart_flight_controller_interface_client.hpp" + +#ifdef CONFIG_USE_PULSING_CONFIGURATION +#include "iq-module-communication-cpp/inc/voltage_superposition_client.hpp" +#include "iq-module-communication-cpp/inc/pulsing_rectangular_input_parser_client.hpp" +#endif //CONFIG_USE_PULSING_CONFIGURATION + +enum DISARM_BEHAVIORS {TRIGGER_MOTOR_DISARM, COAST_MOTOR, SEND_PREDEFINED_VELOCITY}; +enum ARM_BEHAVIORS {USE_MOTOR_ARMING, FORCE_ARMING}; + +class VertiqIo : public OutputModuleInterface +{ + +public: + /** + * @brief Create a new VertiqIo object + */ + VertiqIo(const char *port); + + /** + * @brief destruct a VertiqIo object + */ + ~VertiqIo(); + + /** + * @brief initialize the VertiqIo object. This will be called by the task_spawn function. Makes sure that the thread gets scheduled. + */ + bool init(); + + /** + * @brief Print information about how to use our module + * + */ + void print_info(); + + /** @see OutputModuleInterface */ + bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + unsigned num_outputs, unsigned num_control_groups_updated) override; + + /** + * @brief Used to package and transmit controls via IQUART + * @param outputs The output throttles calculated by the mixer + */ + void OutputControls(uint16_t outputs[MAX_ACTUATORS]); + +private: + + /** + * Check for parameter changes and update them if needed. + */ + void parameters_update(); + void Run() override; + void start(); + void stop(); + + char _port[20] {}; + + perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": output update interval")}; + + static const uint8_t MAX_SUPPORTABLE_IFCI_CVS = 16; + + static px4::atomic_bool + _request_telemetry_init; //Determines whether or not we should initialize or re-initialize the serial connection + + MixingOutput _mixing_output{"VTQ_IO", MAX_SUPPORTABLE_IFCI_CVS, *this, MixingOutput::SchedulingPolicy::Auto, false, false, 0}; + + static char _telemetry_device[20]; //The name of the device we're connecting to. this will be something like /dev/ttyS3 + + VertiqSerialInterface _serial_interface; //We need a serial handler in order to talk over the serial port + VertiqClientManager _client_manager; //We need someone who can manage our clients + VertiqTelemetryManager _telem_manager; //We need a telemetry handler + VertiqConfigurationHandler _configuration_handler; + +//////////////////////////////////////////////////////////////////////// +//Vertiq client information + + /** + Vertiq clients are used to communicate with various parameters available on the module. Our C++ library provides + simple objects used to interact with these clients. + + An example of a Vertiq client is documented here https://iqmotion.readthedocs.io/en/latest/modules/vertiq_2306_2200.html#propeller-motor-control. In this case, + Propeller Motor Control is the client, and its entries are specified in the message table (https://iqmotion.readthedocs.io/en/latest/modules/vertiq_2306_2200.html#id6). + You can find the C++ representation in ./src/drivers/actuators/vertiq_io/iq-module-communication-cpp/inc/propeller_motor_control_client.hpp + */ + + //Known Clients can be created as concrete objects + PropellerMotorControlClient _broadcast_prop_motor_control; + ArmingHandlerClient _broadcast_arming_handler; + IQUartFlightControllerInterfaceClient _operational_ifci; + IFCIPackedMessage _transmission_message; + static const uint16_t MAX_IFCI_MESSAGE = 40; //Up to 16 2 byte commands, one telemetry byte, plus 7 IQUART added bytes + uint8_t _output_message[MAX_IFCI_MESSAGE]; + uint8_t _output_len; +//////////////////////////////////////////////////////////////////////// + + uint8_t _cvs_in_use = 0; //Store the number of control variables that we're using + + //Store the telemetry bitmask for who we want to get telemetry from + uint64_t _telem_bitmask = 0; + uint32_t _telemetry_ids_1 = 0; + uint32_t _telemetry_ids_2 = 0; + + bool _send_forced_arm = true; + bool _actuator_test_active = false; + + uORB::Publication _esc_status_pub{ORB_ID(esc_status)}; //We want to publish our ESC Status to anyone who will listen + + // Subscriptions + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + //We need to know what's going on with the actuator test to make sure we handle it properly + uORB::Subscription _actuator_test_sub{ORB_ID(actuator_test)}; + actuator_test_s _actuator_test; + + //We need to bring in the parameters that we define in module.yaml in order to view them in the + //control station, as well as to use them in the firmware + DEFINE_PARAMETERS( + (ParamInt) _param_vertiq_baud, + (ParamInt) _param_vertiq_target_module_id, + (ParamBool) _param_vertiq_trigger_read, + (ParamInt) _param_vertiq_control_mode, + (ParamFloat) _param_vertiq_max_velo, + (ParamFloat) _param_vertiq_max_volts, + (ParamInt) _param_vertiq_motor_direction, + (ParamInt) _param_vertiq_fc_direction +#ifdef CONFIG_USE_IFCI_CONFIGURATION + , (ParamInt) _param_vertiq_number_of_cvs + , (ParamInt) _param_vertiq_disarm_velocity + , (ParamInt) _param_vertiq_disarm_behavior + , (ParamInt) _param_vertiq_arm_behavior + , (ParamInt) _param_vertiq_throttle_cvi + , (ParamInt) _param_vertiq_telem_ids_1 + , (ParamInt) _param_vertiq_telem_ids_2 +#endif //CONFIG_USE_IFCI_CONFIGURATION + +#ifdef CONFIG_USE_PULSING_CONFIGURATION + , (ParamInt) _param_vertiq_pulse_volt_mode + , (ParamInt) _param_vertiq_pulse_x_cvi + , (ParamInt) _param_vertiq_pulse_y_cvi + , (ParamFloat) _param_vertiq_pulse_zero_angle + , (ParamFloat) _param_vertiq_pulse_velo_cutoff + , (ParamFloat) _param_vertiq_pulse_torque_offset_angle + , (ParamFloat) _param_vertiq_pulse_voltage_limit +#endif //CONFIG_USE_PULSING_CONFIGURATION + ) +}; diff --git a/src/drivers/actuators/vertiq_io/vertiq_serial_interface.cpp b/src/drivers/actuators/vertiq_io/vertiq_serial_interface.cpp new file mode 100644 index 0000000000..c62327f00e --- /dev/null +++ b/src/drivers/actuators/vertiq_io/vertiq_serial_interface.cpp @@ -0,0 +1,245 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "vertiq_serial_interface.hpp" + +VertiqSerialInterface::VertiqSerialInterface() +{} + +void VertiqSerialInterface::DeinitSerial() +{ + if (_uart_fd >= 0) { + close(_uart_fd); + _uart_fd = -1; + } +} + +int VertiqSerialInterface::InitSerial(const char *uart_device, unsigned baud) +{ + //Make sure we're starting clean + DeinitSerial(); + + //Populate our version of the uart device name + strncpy(_port_in_use, uart_device, sizeof(_port_in_use)); + + //Open up the port with read/write permissions and O_NOCTTY which is, "/* Required by POSIX */" + _uart_fd = ::open(_port_in_use, O_RDWR | O_NOCTTY); + + if (_uart_fd < 0) { + PX4_ERR("failed to open serial port: %s err: %d", uart_device, errno); + return -errno; + } + + PX4_INFO("Opened serial port successfully"); + + //Now that we've opened the port, fully configure it for baud/bit num + return ConfigureSerialPeripheral(baud); +} + +int VertiqSerialInterface::ConfigureSerialPeripheral(unsigned baud) +{ + int speed; + + switch (baud) { + case 9600: + speed = B9600; + break; + + case 19200: + speed = B19200; + break; + + case 38400: + speed = B38400; + break; + + case 57600: + speed = B57600; + break; + + case 115200: + speed = B115200; + break; + + case 230400: + speed = B230400; + break; + + case 460800: + speed = B460800; + break; + + case 500000: + speed = B500000; + break; + + case 921600: + speed = B921600; + break; + + case 1000000: + speed = B1000000; + break; + + default: + return -EINVAL; + } + + struct termios uart_config; + + int termios_state; + + /* fill the struct for the new configuration */ + tcgetattr(_uart_fd, &uart_config); + + // + // Input flags - Turn off input processing + // + // convert break to null byte, no CR to NL translation, + // no NL to CR translation, don't mark parity errors or breaks + // no input parity check, don't strip high bit off, + // no XON/XOFF software flow control + // + uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | + INLCR | PARMRK | INPCK | ISTRIP | IXON); + // + // Output flags - Turn off output processing + // + // no CR to NL translation, no NL to CR-NL translation, + // no NL to CR translation, no column 0 CR suppression, + // no Ctrl-D suppression, no fill characters, no case mapping, + // no local output processing + // + // config.c_oflag &= ~(OCRNL | ONLCR | ONLRET | + // ONOCR | ONOEOT| OFILL | OLCUC | OPOST); + uart_config.c_oflag = 0; + + // + // No line processing + // + // echo off, echo newline off, canonical mode off, + // extended input processing off, signal chars off + // + uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); + + /* no parity, one stop bit, disable flow control */ + uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS); + + /* set baud rate */ + if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { + return -errno; + } + + if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { + return -errno; + } + + if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) { + return -errno; + } + + close(_uart_fd); + _uart_fd = -1; + + return 0; +} + +bool VertiqSerialInterface::CheckForRx() +{ + // read from the uart. This must be non-blocking, so check first if there is data available + _bytes_available = 0; + int ret = ioctl(_uart_fd, FIONREAD, (unsigned long)&_bytes_available); + + if (ret != 0) { + PX4_ERR("Reading error"); + return -1; + } + + return _bytes_available > 0; +} + +uint8_t *VertiqSerialInterface::ReadAndSetRxBytes() +{ + //Read the bytes available for us + read(_uart_fd, _rx_buf, _bytes_available); + + //Put the data into our IQUART handler + _iquart_interface.SetRxBytes(_rx_buf, _bytes_available); + return _rx_buf; +} + +void VertiqSerialInterface::ProcessSerialRx(ClientAbstract **client_array, uint8_t number_of_clients) +{ + //Make sure we can actually talk + ReOpenSerial(); + + //We have bytes + if (CheckForRx()) { + uint8_t *data_ptr = ReadAndSetRxBytes(); + + //While we've got packets to look at, give the packet to each of the clients so that each + //can decide what to do with it + while (_iquart_interface.PeekPacket(&data_ptr, &_bytes_available) == 1) { + + for (uint8_t i = 0; i < number_of_clients; i++) { + client_array[i]->ReadMsg(data_ptr, _bytes_available); + } + + _iquart_interface.DropPacket(); + } + + } +} + +void VertiqSerialInterface::ProcessSerialTx() +{ + //Make sure we can actually talk + ReOpenSerial(); + + //while there's stuff to write, write it + //Clients are responsible for adding TX messages to the buffer through get/set/save commands + while (_iquart_interface.GetTxBytes(_tx_buf, _bytes_available)) { + write(_uart_fd, _tx_buf, _bytes_available); + } +} + +GenericInterface *VertiqSerialInterface::GetIquartInterface() +{ + return &_iquart_interface; +} + +void VertiqSerialInterface::ReOpenSerial() +{ + if (_uart_fd < 0) { + _uart_fd = open(_port_in_use, O_RDWR | O_NOCTTY); + } +} diff --git a/src/drivers/actuators/vertiq_io/vertiq_serial_interface.hpp b/src/drivers/actuators/vertiq_io/vertiq_serial_interface.hpp new file mode 100644 index 0000000000..88e841c029 --- /dev/null +++ b/src/drivers/actuators/vertiq_io/vertiq_serial_interface.hpp @@ -0,0 +1,132 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "iq-module-communication-cpp/inc/generic_interface.hpp" +#include "iq-module-communication-cpp/inc/propeller_motor_control_client.hpp" + +class VertiqSerialInterface +{ +public: + /** + * @brief Construct a new Vertiq Serial Interface object + * + */ + VertiqSerialInterface(); + + /** + * @brief Initialize our serial peripheral + */ + int InitSerial(const char *uart_device, unsigned baud); + + /** + * Turn off and close the serial connection + */ + void DeinitSerial(); + + /** + * set the Baudrate + * @param baud + * @return 0 on success, <0 on error + */ + int ConfigureSerialPeripheral(unsigned baud); + + /** + * @brief Check to see if there are any valid IQUART packets for us to read + * + * @return true If there is a packet + * @return false If there is not a packet + */ + bool CheckForRx(); + + /** + * @brief Read a packet from our packet finder and return a pointer to the beginning of the data + * + * @return uint8_t* A pointer to the start of the packet + */ + uint8_t *ReadAndSetRxBytes(); + + /** + * @brief Send the data we got to all of our clients in order to keep everyone up to date with responses + * + * @param client_array A pointer to our array of clients + * @param number_of_clients The number of clients in the array + * @return int + */ + void ProcessSerialRx(ClientAbstract **client_array, uint8_t number_of_clients); + + /** + * @brief check to see if there is any data that we need to transmit over serial + */ + void ProcessSerialTx(); + + /** + * @brief give access to our iquart interface so that others can use it + * @return a pointer to our _iquart_itnerface object + */ + GenericInterface *GetIquartInterface(); + +private: + /** + * @brief Ensures that the serial port is open. Opens it if not + * + */ + void ReOpenSerial(); + + GenericInterface _iquart_interface; + + uint8_t _bytes_available; + + //Buffers for data to transmit or that we're receiving + uint8_t _rx_buf[256]; + uint8_t _tx_buf[256]; + + //The port that we're using for communication + int _uart_fd{-1}; + char _port_in_use[20] {}; + +#if ! defined(__PX4_QURT) + struct termios _orig_cfg; + struct termios _cfg; +#endif + int _speed = -1; + + +}; diff --git a/src/drivers/actuators/vertiq_io/vertiq_telemetry_manager.cpp b/src/drivers/actuators/vertiq_io/vertiq_telemetry_manager.cpp new file mode 100644 index 0000000000..fb2b5bd2e1 --- /dev/null +++ b/src/drivers/actuators/vertiq_io/vertiq_telemetry_manager.cpp @@ -0,0 +1,202 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "vertiq_telemetry_manager.hpp" + +VertiqTelemetryManager::VertiqTelemetryManager(VertiqClientManager *client_manager) : + _client_manager(client_manager), + _telem_state(UNPAUSED) +{ +} + +void VertiqTelemetryManager::Init(uint64_t telem_bitmask, uint8_t module_id) +{ + //On init, make sure to set our bitmask, and then go ahead and find the front and back 1s + _telem_bitmask = telem_bitmask; + FindTelemetryModuleIds(); + + _telem_interface = new IQUartFlightControllerInterfaceClient(module_id); + _client_manager->AddNewClient(_telem_interface); +} + +void VertiqTelemetryManager::FindTelemetryModuleIds() +{ + uint64_t temp_bitmask = _telem_bitmask; + + //We have a uint64 with bit positions representing module ids. If you see a 1, that's a module ID we need to get telemetry from + //Keep shifting values, and see who's a 1 + for (uint8_t i = 0; i < MAX_SUPPORTABLE_MODULE_IDS; i++) { + //We're only going to keep track of up to MAX_ESC_STATUS_ENTRIES + if (temp_bitmask & 0x0000000000000001 && (_number_of_module_ids_for_telem < MAX_ESC_STATUS_ENTRIES)) { + //we found one, and have spcae for it. Put it in our array + _module_ids_in_use[_number_of_module_ids_for_telem] = i; + _number_of_module_ids_for_telem++; + } + + temp_bitmask = temp_bitmask >> 1; + } +} + +void VertiqTelemetryManager::StartPublishing(uORB::Publication *esc_status_pub) +{ + //Initialize our ESC status publishing + _esc_status.timestamp = hrt_absolute_time(); + _esc_status.counter = 0; + _esc_status.esc_count = _number_of_module_ids_for_telem; + _esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_SERIAL; + _esc_status.esc_armed_flags = 0; + _esc_status.esc_online_flags = 0; + + for (unsigned i = 0; i < _number_of_module_ids_for_telem; i++) { + _esc_status.esc[i].timestamp = 0; + _esc_status.esc[i].esc_address = 0; + _esc_status.esc[i].esc_rpm = 0; + _esc_status.esc[i].esc_state = 0; + _esc_status.esc[i].esc_cmdcount = 0; + _esc_status.esc[i].esc_voltage = 0; + _esc_status.esc[i].esc_current = 0; + _esc_status.esc[i].esc_temperature = 0; + _esc_status.esc[i].esc_errorcount = 0; + _esc_status.esc[i].failures = 0; + _esc_status.esc[i].esc_power = 0; + } + + //Start advertising to the world + esc_status_pub->advertise(); +} + +uint16_t VertiqTelemetryManager::UpdateTelemetry() +{ + bool got_reply = false; + + //Get the current time to check for timeout + hrt_abstime time_now = hrt_absolute_time(); + + //We timed out for this request if the time since the last request going out is greater than our timeout period + bool timed_out = (time_now - _time_of_last_telem_request) > _telem_timeout; + + //We got a telemetry response + if (_telem_interface->telemetry_.IsFresh()) { + //grab the data + IFCITelemetryData telem_response = _telem_interface->telemetry_.get_reply(); + + // also update our internal report for logging + _esc_status.esc[_current_module_id_target_index].esc_address = _module_ids_in_use[_number_of_module_ids_for_telem]; + _esc_status.esc[_current_module_id_target_index].timestamp = time_now; + _esc_status.esc[_current_module_id_target_index].esc_rpm = telem_response.speed * 60.0f * M_1_PI_F * + 0.5f; //We get back rad/s, convert to rpm + _esc_status.esc[_current_module_id_target_index].esc_voltage = telem_response.voltage * 0.01; + _esc_status.esc[_current_module_id_target_index].esc_current = telem_response.current * 0.01; + _esc_status.esc[_current_module_id_target_index].esc_power = + _esc_status.esc[_current_module_id_target_index].esc_voltage * + _esc_status.esc[_current_module_id_target_index].esc_current; + _esc_status.esc[_current_module_id_target_index].esc_temperature = telem_response.mcu_temp * + 0.01; //"If you ask other escs for their temp, they're giving you the micro temp, so go with that" + _esc_status.esc[_current_module_id_target_index].esc_state = 0; //not implemented + _esc_status.esc[_current_module_id_target_index].esc_cmdcount = 0; //not implemented + _esc_status.esc[_current_module_id_target_index].failures = 0; //not implemented + + //Update the overall _esc_status timestamp and our counter + _esc_status.timestamp = time_now; + _esc_status.counter++; + + _esc_status.esc_armed_flags |= (0x01 << _current_module_id_target_index); + _esc_status.esc_online_flags |= (0x01 << _current_module_id_target_index); + + got_reply = true; + } + + //If we got a new response or if we ran out of time to get a response from this motor move on + if (got_reply || timed_out) { + //We can only be fully paused once the last telemetry attempt is done + if (_telem_state == PAUSE_REQUESTED) { + _telem_state = PAUSED; + } + + _time_of_last_telem_request = hrt_absolute_time(); + + uint16_t next_telem = FindNextMotorForTelemetry(); + + if (next_telem != _impossible_module_id) { + //We need to update the module ID we're going to listen to. So, kill the old one, and make it anew. + delete _telem_interface; + _telem_interface = new IQUartFlightControllerInterfaceClient(next_telem); + } + + //update the telem target + return next_telem; + } + + //Still waiting for a response or a timeout + return _impossible_module_id; +} + +uint16_t VertiqTelemetryManager::FindNextMotorForTelemetry() +{ + //If we're paused, we're just going to spit back an impossible module ID. Otherwise, go ahead and find the next target + if (_telem_state == UNPAUSED) { + //If our current index is the last module ID we've found, then go back to the beginning + //otherwise just increment + if (_current_module_id_target_index >= _number_of_module_ids_for_telem - 1) { + _current_module_id_target_index = 0; + + } else { + _current_module_id_target_index++; + } + + //Return the next target + return _module_ids_in_use[_current_module_id_target_index]; + } + + return _impossible_module_id; +} + +esc_status_s VertiqTelemetryManager::GetEscStatus() +{ + return _esc_status; +} + +void VertiqTelemetryManager::PauseTelemetry() +{ + _telem_state = PAUSE_REQUESTED; +} + +void VertiqTelemetryManager::UnpauseTelemetry() +{ + _telem_state = UNPAUSED; +} + +vertiq_telemetry_pause_states VertiqTelemetryManager::GetTelemetryPauseState() +{ + return _telem_state; +} diff --git a/src/drivers/actuators/vertiq_io/vertiq_telemetry_manager.hpp b/src/drivers/actuators/vertiq_io/vertiq_telemetry_manager.hpp new file mode 100644 index 0000000000..8d56d33a43 --- /dev/null +++ b/src/drivers/actuators/vertiq_io/vertiq_telemetry_manager.hpp @@ -0,0 +1,152 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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. + * + ****************************************************************************/ + +#ifndef VERTIQ_TELEMETRY_MANAGER_HPP +#define VERTIQ_TELEMETRY_MANAGER_HPP + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "vertiq_client_manager.hpp" + +#include "iq-module-communication-cpp/inc/iquart_flight_controller_interface_client.hpp" + +enum vertiq_telemetry_pause_states { + PAUSED, + PAUSE_REQUESTED, + UNPAUSED +}; + +static const uint8_t _impossible_module_id = 255; + +class VertiqTelemetryManager +{ +public: + + /** + * @brief Construct a new Vertiq Telemetry Manager object + * + * @param client_manager A pointer to the client manager so that we can add clients to it + */ + VertiqTelemetryManager(VertiqClientManager *client_manager); + + /** + * @brief Initialize the telemetry manager with the bitmask set in the PX4 parameters + * @param telem_bitmask The bitmask set in the PX4 parameters as VERTIQ_TEL_MSK + * @param module_id The target module ID at the time of init + */ + void Init(uint64_t telem_bitmask, uint8_t module_id); + + /** + * @brief Start publishing our esc status to the uORB topic + * + * @param esc_status_pub A pointer to our Publication + */ + void StartPublishing(uORB::Publication *esc_status_pub); + + /** + * @brief Part of initialization. Find the first and last positions that indicate modules to grab telemetry from + */ + void FindTelemetryModuleIds(); + + /** + * @brief Attempt to grab the telemetry response from the currently targeted module. Handles + * updating when to start requesting telemetry from the next module. + * + */ + uint16_t UpdateTelemetry(); + + /** + * @brief Determing the next module to request telemetry from + * + */ + uint16_t FindNextMotorForTelemetry(); + + /** + * @brief return access to our esc_status_s handler + * + * @return _esc_status: our instance of an esc_status_s + */ + esc_status_s GetEscStatus(); + + /** + * @brief Stops the telemetry manager from calculating the next target for telemetry + * + */ + void PauseTelemetry(); + + /** + * @brief Resumes looking for the next telemetry target + * + */ + void UnpauseTelemetry(); + + /** + * @brief Returns the paused state of the telemetry manager + * + */ + vertiq_telemetry_pause_states GetTelemetryPauseState(); + +private: + VertiqClientManager *_client_manager; + + vertiq_telemetry_pause_states _telem_state; //Keep track of whether or not we've paused telemetry + IQUartFlightControllerInterfaceClient *_telem_interface; //Used for reading responses from our telemetry targets + esc_status_s _esc_status; //We want to publish our ESC Status to anyone who will listen + static const uint8_t MAX_SUPPORTABLE_MODULE_IDS = 63; //[0, 62] //The max number of module IDs that we can support + static const uint8_t MAX_ESC_STATUS_ENTRIES = + 8; //The max number of esc status entries we can keep track of (per the esc_status_s type) + + //We need a way to store the module IDs that we're supposed to ask for telemetry from. We can have as many as 63. + uint8_t _module_ids_in_use[MAX_ESC_STATUS_ENTRIES]; + uint8_t _number_of_module_ids_for_telem = 0; + uint8_t _current_module_id_target_index = 0; + + uint64_t _telem_bitmask = 0; //Store the telemetry bitmask for who we want to get telemetry from + static const hrt_abstime _telem_timeout = 50_ms; //The amount of time (in ms) that we'll wait for a telemetry response + hrt_abstime _time_of_last_telem_request = 0; //The system time the last time that we got telemetry +}; + +#endif