mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
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 <scheuer.luca@gmail.com>
This commit is contained in:
vertiq-jordan
GitHub
parent
5ffe9c6de4
commit
06dfc4b782
4
.gitmodules
vendored
4
.gitmodules
vendored
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
80
src/drivers/actuators/vertiq_io/CMakeLists.txt
Normal file
80
src/drivers/actuators/vertiq_io/CMakeLists.txt
Normal file
@ -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
|
||||
)
|
||||
17
src/drivers/actuators/vertiq_io/Kconfig
Normal file
17
src/drivers/actuators/vertiq_io/Kconfig
Normal file
@ -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
|
||||
181
src/drivers/actuators/vertiq_io/entry_wrapper.hpp
Normal file
181
src/drivers/actuators/vertiq_io/entry_wrapper.hpp
Normal file
@ -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 <parameters/param.h>
|
||||
|
||||
#include <px4_log.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
|
||||
#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 <typename module_data_type, typename px4_data_type>
|
||||
class EntryWrapper : public AbstractEntryWrapper
|
||||
{
|
||||
public:
|
||||
|
||||
param_t _param;
|
||||
ClientEntry<module_data_type> *_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<module_data_type> *)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
|
||||
@ -0,0 +1 @@
|
||||
Subproject commit a9b700d50bdd06a837c74750ac3c4760937333df
|
||||
289
src/drivers/actuators/vertiq_io/module.yaml
Normal file
289
src/drivers/actuators/vertiq_io/module.yaml
Normal file
@ -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
|
||||
65
src/drivers/actuators/vertiq_io/vertiq_client_manager.cpp
Normal file
65
src/drivers/actuators/vertiq_io/vertiq_client_manager.cpp
Normal file
@ -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;
|
||||
}
|
||||
89
src/drivers/actuators/vertiq_io/vertiq_client_manager.hpp
Normal file
89
src/drivers/actuators/vertiq_io/vertiq_client_manager.hpp
Normal file
@ -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 <drivers/device/device.h>
|
||||
#include <lib/led/led.h>
|
||||
#include <lib/mixer_module/mixer_module.hpp>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
|
||||
#include <parameters/param.h>
|
||||
|
||||
#include <px4_log.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
|
||||
#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
|
||||
147
src/drivers/actuators/vertiq_io/vertiq_configuration_handler.cpp
Normal file
147
src/drivers/actuators/vertiq_io/vertiq_configuration_handler.cpp
Normal file
@ -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<float, float>(param_find("VTQ_MAX_VELOCITY"), &(_prop_input_parser_client->velocity_max_));
|
||||
AddNewClientEntry<float, float>(param_find("VTQ_MAX_VOLTS"), &(_prop_input_parser_client->volts_max_));
|
||||
AddNewClientEntry<uint8_t, int32_t>(param_find("VTQ_CONTROL_MODE"), &(_prop_input_parser_client->mode_));
|
||||
AddNewClientEntry<uint8_t, int32_t>(param_find("VTQ_MOTOR_DIR"), &(_prop_input_parser_client->sign_));
|
||||
AddNewClientEntry<uint8_t, int32_t>(param_find("VTQ_FC_DIR"), &(_prop_input_parser_client->flip_negative_));
|
||||
|
||||
#ifdef CONFIG_USE_IFCI_CONFIGURATION
|
||||
AddNewClientEntry<uint8_t, int32_t>(param_find("VTQ_THROTTLE_CVI"), &(_ifci_client->throttle_cvi_));
|
||||
#endif //CONFIG_USE_IFCI_CONFIGURATION
|
||||
|
||||
#ifdef CONFIG_USE_PULSING_CONFIGURATION
|
||||
AddNewClientEntry<uint8_t, int32_t> (param_find("VTQ_PULSE_V_MODE"),
|
||||
&(_pulsing_rectangular_input_parser_client->pulsing_voltage_mode_));
|
||||
AddNewClientEntry<uint8_t, int32_t>(param_find("VTQ_X_CVI"), &(_ifci_client->x_cvi_));
|
||||
AddNewClientEntry<uint8_t, int32_t>(param_find("VTQ_Y_CVI"), &(_ifci_client->y_cvi_));
|
||||
AddNewClientEntry<float, float>(param_find("VTQ_ZERO_ANGLE"), &(_voltage_superposition_client->zero_angle_));
|
||||
AddNewClientEntry<float, float>(param_find("VTQ_VELO_CUTOFF"),
|
||||
&(_voltage_superposition_client->velocity_cutoff_));
|
||||
AddNewClientEntry<float, float>(param_find("VTQ_TQUE_OFF_ANG"),
|
||||
&(_voltage_superposition_client->propeller_torque_offset_angle_));
|
||||
AddNewClientEntry<float, float>(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<EscPropellerInputParserClient>(_prop_input_parser_client, new_object_id);
|
||||
|
||||
#ifdef CONFIG_USE_IFCI_CONFIGURATION
|
||||
DestroyAndRecreateClient<IQUartFlightControllerInterfaceClient>(_ifci_client, new_object_id);
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_USE_PULSING_CONFIGURATION
|
||||
DestroyAndRecreateClient<VoltageSuperPositionClient>(_voltage_superposition_client, new_object_id);
|
||||
DestroyAndRecreateClient<PulsingRectangularInputParserClient>(_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;
|
||||
}
|
||||
182
src/drivers/actuators/vertiq_io/vertiq_configuration_handler.hpp
Normal file
182
src/drivers/actuators/vertiq_io/vertiq_configuration_handler.hpp
Normal file
@ -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 <lib/mixer_module/mixer_module.hpp>
|
||||
|
||||
#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 <typename client_type>
|
||||
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 <typename iquart_data_type, typename px4_data_type>
|
||||
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<iquart_data_type, px4_data_type>;
|
||||
_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
|
||||
391
src/drivers/actuators/vertiq_io/vertiq_io.cpp
Normal file
391
src/drivers/actuators/vertiq_io/vertiq_io.cpp
Normal file
@ -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_platform_common/log.h>
|
||||
|
||||
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("<device>", "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();
|
||||
}
|
||||
207
src/drivers/actuators/vertiq_io/vertiq_io.hpp
Normal file
207
src/drivers/actuators/vertiq_io/vertiq_io.hpp
Normal file
@ -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 <drivers/device/device.h>
|
||||
#include <lib/led/led.h>
|
||||
#include <lib/mixer_module/mixer_module.hpp>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
|
||||
#include <px4_log.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/getopt.h>
|
||||
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/led_control.h>
|
||||
#include <uORB/topics/esc_status.h>
|
||||
#include <uORB/topics/actuator_test.h>
|
||||
|
||||
#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_s> _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<px4::params::VTQ_BAUD>) _param_vertiq_baud,
|
||||
(ParamInt<px4::params::VTQ_TRGT_MOD_ID>) _param_vertiq_target_module_id,
|
||||
(ParamBool<px4::params::VTQ_REDO_READ>) _param_vertiq_trigger_read,
|
||||
(ParamInt<px4::params::VTQ_CONTROL_MODE>) _param_vertiq_control_mode,
|
||||
(ParamFloat<px4::params::VTQ_MAX_VELOCITY>) _param_vertiq_max_velo,
|
||||
(ParamFloat<px4::params::VTQ_MAX_VOLTS>) _param_vertiq_max_volts,
|
||||
(ParamInt<px4::params::VTQ_MOTOR_DIR>) _param_vertiq_motor_direction,
|
||||
(ParamInt<px4::params::VTQ_FC_DIR>) _param_vertiq_fc_direction
|
||||
#ifdef CONFIG_USE_IFCI_CONFIGURATION
|
||||
, (ParamInt<px4::params::VTQ_NUM_CVS>) _param_vertiq_number_of_cvs
|
||||
, (ParamInt<px4::params::VTQ_DISARM_VELO>) _param_vertiq_disarm_velocity
|
||||
, (ParamInt<px4::params::VTQ_DISARM_TRIG>) _param_vertiq_disarm_behavior
|
||||
, (ParamInt<px4::params::VTQ_ARM_BEHAVE>) _param_vertiq_arm_behavior
|
||||
, (ParamInt<px4::params::VTQ_THROTTLE_CVI>) _param_vertiq_throttle_cvi
|
||||
, (ParamInt<px4::params::VTQ_TELEM_IDS_1>) _param_vertiq_telem_ids_1
|
||||
, (ParamInt<px4::params::VTQ_TELEM_IDS_2>) _param_vertiq_telem_ids_2
|
||||
#endif //CONFIG_USE_IFCI_CONFIGURATION
|
||||
|
||||
#ifdef CONFIG_USE_PULSING_CONFIGURATION
|
||||
, (ParamInt<px4::params::VTQ_PULSE_V_MODE>) _param_vertiq_pulse_volt_mode
|
||||
, (ParamInt<px4::params::VTQ_X_CVI>) _param_vertiq_pulse_x_cvi
|
||||
, (ParamInt<px4::params::VTQ_Y_CVI>) _param_vertiq_pulse_y_cvi
|
||||
, (ParamFloat<px4::params::VTQ_ZERO_ANGLE>) _param_vertiq_pulse_zero_angle
|
||||
, (ParamFloat<px4::params::VTQ_VELO_CUTOFF>) _param_vertiq_pulse_velo_cutoff
|
||||
, (ParamFloat<px4::params::VTQ_TQUE_OFF_ANG>) _param_vertiq_pulse_torque_offset_angle
|
||||
, (ParamFloat<px4::params::VTQ_PULSE_V_LIM>) _param_vertiq_pulse_voltage_limit
|
||||
#endif //CONFIG_USE_PULSING_CONFIGURATION
|
||||
)
|
||||
};
|
||||
245
src/drivers/actuators/vertiq_io/vertiq_serial_interface.cpp
Normal file
245
src/drivers/actuators/vertiq_io/vertiq_serial_interface.cpp
Normal file
@ -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);
|
||||
}
|
||||
}
|
||||
132
src/drivers/actuators/vertiq_io/vertiq_serial_interface.hpp
Normal file
132
src/drivers/actuators/vertiq_io/vertiq_serial_interface.hpp
Normal file
@ -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 <px4_log.h>
|
||||
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <termios.h>
|
||||
#include <errno.h>
|
||||
|
||||
#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;
|
||||
|
||||
|
||||
};
|
||||
202
src/drivers/actuators/vertiq_io/vertiq_telemetry_manager.cpp
Normal file
202
src/drivers/actuators/vertiq_io/vertiq_telemetry_manager.cpp
Normal file
@ -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_s> *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;
|
||||
}
|
||||
152
src/drivers/actuators/vertiq_io/vertiq_telemetry_manager.hpp
Normal file
152
src/drivers/actuators/vertiq_io/vertiq_telemetry_manager.hpp
Normal file
@ -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 <drivers/device/device.h>
|
||||
#include <lib/led/led.h>
|
||||
#include <lib/mixer_module/mixer_module.hpp>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
|
||||
#include <px4_log.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/led_control.h>
|
||||
#include <uORB/topics/esc_status.h>
|
||||
#include <uORB/topics/actuator_test.h>
|
||||
|
||||
#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_s> *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
|
||||
Loading…
x
Reference in New Issue
Block a user