drivers/actuators/modalai_esc --> modal_io and rename UART_ESC prefix to MODAL_IO (#20995)

This commit is contained in:
modaltb
2023-01-24 17:24:58 -08:00
committed by GitHub
parent 60de5b3ea4
commit 20b7a8c9f6
23 changed files with 222 additions and 229 deletions
+1 -1
View File
@@ -5,7 +5,7 @@ CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6"
CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4"
CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1"
CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS3"
CONFIG_DRIVERS_ACTUATORS_MODALAI_ESC=y
CONFIG_DRIVERS_ACTUATORS_MODAL_IO=y
CONFIG_DRIVERS_ADC_ADS1115=y
CONFIG_DRIVERS_ADC_BOARD_ADC=y
CONFIG_COMMON_BAROMETERS=y
+2 -2
View File
@@ -3,9 +3,9 @@
# ModalAI FC-v1 specific board sensors init
#------------------------------------------------------------------------------
if param greater UART_ESC_CONFIG 0
if param greater MODAL_IO_CONFIG 0
then
modalai_esc start
modal_io start
fi
board_adc start
+1 -1
View File
@@ -264,7 +264,7 @@
#define BOARD_NUM_IO_TIMERS 5
// J4 / TELEM3 / UART2
#define MODALAI_ESC_DEFAULT_PORT "/dev/ttyS1"
#define MODAL_IO_DEFAULT_PORT "/dev/ttyS1"
__BEGIN_DECLS
+1 -1
View File
@@ -3,7 +3,7 @@ CONFIG_BOARD_ARCHITECTURE="cortex-m7"
CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0"
CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6"
CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4"
CONFIG_DRIVERS_ACTUATORS_MODALAI_ESC=y
CONFIG_DRIVERS_ACTUATORS_MODAL_IO=y
CONFIG_DRIVERS_ADC_ADS1115=y
CONFIG_DRIVERS_ADC_BOARD_ADC=y
CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP201XX=y
+2 -2
View File
@@ -3,9 +3,9 @@
# ModalAI FC-v2 specific board sensors init
#------------------------------------------------------------------------------
if param greater UART_ESC_CONFIG 0
if param greater MODAL_IO_CONFIG 0
then
modalai_esc start
modal_io start
fi
board_adc start
+1 -1
View File
@@ -339,7 +339,7 @@
#define BOARD_NUM_IO_TIMERS 5
// J1 / TELEM1 / USART7
#define MODALAI_ESC_DEFAULT_PORT "/dev/ttyS6"
#define MODAL_IO_DEFAULT_PORT "/dev/ttyS6"
__BEGIN_DECLS
+1 -1
View File
@@ -1,6 +1,6 @@
CONFIG_PLATFORM_QURT=y
CONFIG_BOARD_TOOLCHAIN="qurt"
CONFIG_DRIVERS_ACTUATORS_MODALAI_ESC=y
CONFIG_DRIVERS_ACTUATORS_MODAL_IO=y
CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
CONFIG_DRIVERS_QSHELL_QURT=y
CONFIG_MODULES_EKF2=y
+1 -1
View File
@@ -62,4 +62,4 @@
/*
* Default port for the ESC
*/
#define MODALAI_ESC_DEFAULT_PORT "2"
#define MODAL_IO_DEFAULT_PORT "2"
+1 -1
View File
@@ -20,4 +20,4 @@ muorb start
qshell icm42688p start -s
qshell modalai_esc start
qshell modal_io start
@@ -32,16 +32,16 @@
############################################################################
px4_add_module(
MODULE drivers__actuators__modalai_esc
MAIN modalai_esc
MODULE drivers__actuators__modal_io
MAIN modal_io
SRCS
crc16.c
crc16.h
modalai_esc_serial.cpp
modalai_esc_serial.hpp
modalai_esc.cpp
modalai_esc.hpp
modal_io_serial.cpp
modal_io_serial.hpp
modal_io.cpp
modal_io.hpp
qc_esc_packet_types.h
qc_esc_packet.c
qc_esc_packet.h
+5
View File
@@ -0,0 +1,5 @@
menuconfig DRIVERS_ACTUATORS_MODAL_IO
bool "modal_io"
default n
---help---
Enable support for modal_io
@@ -48,16 +48,16 @@
#include <uORB/topics/esc_status.h>
#include <uORB/topics/actuator_test.h>
#include "modalai_esc_serial.hpp"
#include "modal_io_serial.hpp"
#include "qc_esc_packet.h"
#include "qc_esc_packet_types.h"
class ModalaiEsc : public ModuleBase<ModalaiEsc>, public OutputModuleInterface
class ModalIo : public ModuleBase<ModalIo>, public OutputModuleInterface
{
public:
ModalaiEsc();
virtual ~ModalaiEsc();
ModalIo();
virtual ~ModalIo();
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
@@ -80,13 +80,6 @@ public:
virtual int init();
typedef enum {
UART_ESC_RESET,
UART_ESC_VERSION,
UART_ESC_TONE,
UART_ESC_LED
} uart_esc_cmd_t;
struct Command {
uint16_t id = 0;
uint8_t len = 0;
@@ -107,50 +100,50 @@ public:
int send_cmd_thread_safe(Command *cmd);
private:
static constexpr uint32_t MODALAI_ESC_UART_CONFIG = 1;
static constexpr uint32_t MODALAI_ESC_DEFAULT_BAUD = 250000;
static constexpr uint16_t MODALAI_ESC_OUTPUT_CHANNELS = 4;
static constexpr uint16_t MODALAI_ESC_OUTPUT_DISABLED = 0;
static constexpr uint32_t MODAL_IO_UART_CONFIG = 1;
static constexpr uint32_t MODAL_IO_DEFAULT_BAUD = 250000;
static constexpr uint16_t MODAL_IO_OUTPUT_CHANNELS = 4;
static constexpr uint16_t MODAL_IO_OUTPUT_DISABLED = 0;
static constexpr uint32_t MODALAI_ESC_WRITE_WAIT_US = 200;
static constexpr uint32_t MODALAI_ESC_DISCONNECT_TIMEOUT_US = 500000;
static constexpr uint32_t MODAL_IO_WRITE_WAIT_US = 200;
static constexpr uint32_t MODAL_IO_DISCONNECT_TIMEOUT_US = 500000;
static constexpr uint16_t DISARMED_VALUE = 0;
static constexpr uint16_t MODALAI_ESC_PWM_MIN = 0;
static constexpr uint16_t MODALAI_ESC_PWM_MAX = 800;
static constexpr uint16_t MODALAI_ESC_DEFAULT_RPM_MIN = 5000;
static constexpr uint16_t MODALAI_ESC_DEFAULT_RPM_MAX = 17000;
static constexpr uint16_t MODAL_IO_PWM_MIN = 0;
static constexpr uint16_t MODAL_IO_PWM_MAX = 800;
static constexpr uint16_t MODAL_IO_DEFAULT_RPM_MIN = 5000;
static constexpr uint16_t MODAL_IO_DEFAULT_RPM_MAX = 17000;
static constexpr float MODALAI_ESC_MODE_DISABLED_SETPOINT = -0.1f;
static constexpr float MODALAI_ESC_MODE_THRESHOLD = 0.0f;
static constexpr float MODAL_IO_MODE_DISABLED_SETPOINT = -0.1f;
static constexpr float MODAL_IO_MODE_THRESHOLD = 0.0f;
static constexpr uint32_t MODALAI_ESC_MODE = 0;
static constexpr uint32_t MODALAI_ESC_MODE_TURTLE_AUX1 = 1;
static constexpr uint32_t MODALAI_ESC_MODE_TURTLE_AUX2 = 2;
static constexpr uint32_t MODALAI_ESC_MODE_UART_BRIDGE = 3;
static constexpr uint32_t MODAL_IO_MODE = 0;
static constexpr uint32_t MODAL_IO_MODE_TURTLE_AUX1 = 1;
static constexpr uint32_t MODAL_IO_MODE_TURTLE_AUX2 = 2;
static constexpr uint32_t MODAL_IO_MODE_UART_BRIDGE = 3;
//static constexpr uint16_t max_pwm(uint16_t pwm) { return math::min(pwm, MODALAI_ESC_PWM_MAX); }
//static constexpr uint16_t max_rpm(uint16_t rpm) { return math::min(rpm, MODALAI_ESC_RPM_MAX); }
//static constexpr uint16_t max_pwm(uint16_t pwm) { return math::min(pwm, MODAL_IO_PWM_MAX); }
//static constexpr uint16_t max_rpm(uint16_t rpm) { return math::min(rpm, MODAL_IO_RPM_MAX); }
ModalaiEscSerial *_uart_port;
ModalaiEscSerial *_uart_port_bridge;
ModalIoSerial *_uart_port;
ModalIoSerial *_uart_port_bridge;
typedef struct {
int32_t config{MODALAI_ESC_UART_CONFIG};
int32_t mode{MODALAI_ESC_MODE};
int32_t config{MODAL_IO_UART_CONFIG};
int32_t mode{MODAL_IO_MODE};
int32_t turtle_motor_expo{35};
int32_t turtle_motor_deadband{20};
int32_t turtle_motor_percent{90};
float turtle_stick_minf{0.15f};
float turtle_cosphi{0.99f};
int32_t baud_rate{MODALAI_ESC_DEFAULT_BAUD};
int32_t rpm_min{MODALAI_ESC_DEFAULT_RPM_MIN};
int32_t rpm_max{MODALAI_ESC_DEFAULT_RPM_MAX};
int32_t function_map[MODALAI_ESC_OUTPUT_CHANNELS] {0, 0, 0, 0};
int32_t motor_map[MODALAI_ESC_OUTPUT_CHANNELS] {1, 2, 3, 4};
int32_t direction_map[MODALAI_ESC_OUTPUT_CHANNELS] {1, 1, 1, 1};
} uart_esc_params_t;
int32_t baud_rate{MODAL_IO_DEFAULT_BAUD};
int32_t rpm_min{MODAL_IO_DEFAULT_RPM_MIN};
int32_t rpm_max{MODAL_IO_DEFAULT_RPM_MAX};
int32_t function_map[MODAL_IO_OUTPUT_CHANNELS] {0, 0, 0, 0};
int32_t motor_map[MODAL_IO_OUTPUT_CHANNELS] {1, 2, 3, 4};
int32_t direction_map[MODAL_IO_OUTPUT_CHANNELS] {1, 1, 1, 1};
} modal_io_params_t;
struct EscChan {
int16_t rate_req;
@@ -173,13 +166,13 @@ private:
typedef struct {
led_control_s control{};
vehicle_control_mode_s mode{};
uint8_t led_mask;// TODO led_mask[MODALAI_ESC_OUTPUT_CHANNELS];
uint8_t led_mask;// TODO led_mask[MODAL_IO_OUTPUT_CHANNELS];
bool breath_en;
uint8_t breath_counter;
bool test;
} led_rsc_t;
ch_assign_t _output_map[MODALAI_ESC_OUTPUT_CHANNELS] {{1, 1}, {2, 1}, {3, 1}, {4, 1}};
ch_assign_t _output_map[MODAL_IO_OUTPUT_CHANNELS] {{1, 1}, {2, 1}, {3, 1}, {4, 1}};
MixingOutput _mixing_output;
perf_counter_t _cycle_perf;
@@ -198,9 +191,9 @@ private:
//uORB::Publication<actuator_outputs_s> _outputs_debug_pub{ORB_ID(actuator_outputs_debug)};
uORB::Publication<esc_status_s> _esc_status_pub{ORB_ID(esc_status)};
uart_esc_params_t _parameters;
modal_io_params_t _parameters;
int update_params();
int load_params(uart_esc_params_t *params, ch_assign_t *map);
int load_params(modal_io_params_t *params, ch_assign_t *map);
bool _turtle_mode_en{false};
int32_t _rpm_turtle_min{0};
@@ -211,7 +204,7 @@ private:
Command _current_cmd;
px4::atomic<Command *> _pending_cmd{nullptr};
EscChan _esc_chans[MODALAI_ESC_OUTPUT_CHANNELS];
EscChan _esc_chans[MODAL_IO_OUTPUT_CHANNELS];
Command _esc_cmd;
esc_status_s _esc_status;
EscPacket _fb_packet;
@@ -38,23 +38,23 @@
*
* @reboot_required true
*
* @group UART ESC
* @group MODAL IO
* @value 0 - Disabled
* @value 1 - VOXL ESC
* @min 0
* @max 1
*/
PARAM_DEFINE_INT32(UART_ESC_CONFIG, 0);
PARAM_DEFINE_INT32(MODAL_IO_CONFIG, 0);
/**
* UART ESC baud rate
*
* Default rate is 250Kbps, which is used in off-the-shelf MoadalAI ESC products.
*
* @group UART ESC
* @group MODAL IO
* @unit bit/s
*/
PARAM_DEFINE_INT32(UART_ESC_BAUD, 250000);
PARAM_DEFINE_INT32(MODAL_IO_BAUD, 250000);
/**
* Motor mappings for ModalAI ESC
@@ -68,31 +68,31 @@ PARAM_DEFINE_INT32(UART_ESC_BAUD, 250000);
// The following are auto generated params from control allocator pattern, put here for reference
// Default ESC1 to motor2
//PARAM_DEFINE_INT32(UART_ESC_FUNC1, 102);
//PARAM_DEFINE_INT32(MODAL_IO_FUNC1, 102);
//PARAM_DEFINE_INT32(UART_ESC_FUNC2, 103);
//PARAM_DEFINE_INT32(MODAL_IO_FUNC2, 103);
//PARAM_DEFINE_INT32(UART_ESC_FUNC3, 101);
//PARAM_DEFINE_INT32(MODAL_IO_FUNC3, 101);
//PARAM_DEFINE_INT32(UART_ESC_FUNC4, 104);
//PARAM_DEFINE_INT32(MODAL_IO_FUNC4, 104);
/**
* UART ESC RPM Min
*
* Minimum RPM for ESC
*
* @group UART ESC
* @group MODAL IO
*/
PARAM_DEFINE_INT32(UART_ESC_RPM_MIN, 5500);
PARAM_DEFINE_INT32(MODAL_IO_RPM_MIN, 5500);
/**
* UART ESC RPM Max
*
* Maximum RPM for ESC
*
* @group UART ESC
* @group MODAL IO
*/
PARAM_DEFINE_INT32(UART_ESC_RPM_MAX, 15000);
PARAM_DEFINE_INT32(MODAL_IO_RPM_MAX, 15000);
/**
* UART ESC Mode
@@ -101,7 +101,7 @@ PARAM_DEFINE_INT32(UART_ESC_RPM_MAX, 15000);
*
* @reboot_required true
*
* @group UART ESC
* @group MODAL IO
* @value 0 - None
* @value 1 - Turtle Mode enabled via AUX1
* @value 2 - Turtle Mode enabled via AUX2
@@ -109,95 +109,95 @@ PARAM_DEFINE_INT32(UART_ESC_RPM_MAX, 15000);
* @min 0
* @max 2
*/
PARAM_DEFINE_INT32(UART_ESC_MODE, 0);
PARAM_DEFINE_INT32(MODAL_IO_MODE, 0);
/**
* UART ESC ID 1 Spin Direction Flag
*
* @group UART ESC
* @group MODAL IO
* @value 0 - Default
* @value 1 - Reverse
*/
PARAM_DEFINE_INT32(UART_ESC_SDIR1, 0);
PARAM_DEFINE_INT32(MODAL_IO_SDIR1, 0);
/**
* UART ESC ID 2 Spin Direction Flag
*
* @group UART ESC
* @group MODAL IO
* @value 0 - Default
* @value 1 - Reverse
*/
PARAM_DEFINE_INT32(UART_ESC_SDIR2, 0);
PARAM_DEFINE_INT32(MODAL_IO_SDIR2, 0);
/**
* UART ESC ID 3 Spin Direction Flag
*
* @group UART ESC
* @group MODAL IO
* @value 0 - Default
* @value 1 - Reverse
*/
PARAM_DEFINE_INT32(UART_ESC_SDIR3, 0);
PARAM_DEFINE_INT32(MODAL_IO_SDIR3, 0);
/**
* UART ESC ID 4 Spin Direction Flag
*
* @group UART ESC
* @group MODAL IO
* @value 0 - Default
* @value 1 - Reverse
*/
PARAM_DEFINE_INT32(UART_ESC_SDIR4, 0);
PARAM_DEFINE_INT32(MODAL_IO_SDIR4, 0);
/**
* UART ESC Turtle Mode Crash Flip Motor Percent
*
* @group UART ESC
* @group MODAL IO
* @min 1
* @max 100
* @decimal 10
* @increment 1
*/
PARAM_DEFINE_INT32(UART_ESC_T_PERC, 90);
PARAM_DEFINE_INT32(MODAL_IO_T_PERC, 90);
/**
* UART ESC Turtle Mode Crash Flip Motor Deadband
*
* @group UART ESC
* @group MODAL IO
* @min 0
* @max 100
* @decimal 10
* @increment 1
*/
PARAM_DEFINE_INT32(UART_ESC_T_DEAD, 20);
PARAM_DEFINE_INT32(MODAL_IO_T_DEAD, 20);
/**
* UART ESC Turtle Mode Crash Flip Motor STICK_MINF
*
* @group UART ESC
* @group MODAL IO
* @min 0.0
* @max 100.0
* @decimal 10
* @increment 1.0
*/
PARAM_DEFINE_FLOAT(UART_ESC_T_MINF, 0.15);
PARAM_DEFINE_FLOAT(MODAL_IO_T_MINF, 0.15);
/**
* UART ESC Turtle Mode Crash Flip Motor expo
*
* @group UART ESC
* @group MODAL IO
* @min 0
* @max 100
* @decimal 10
* @increment 1
*/
PARAM_DEFINE_INT32(UART_ESC_T_EXPO, 35);
PARAM_DEFINE_INT32(MODAL_IO_T_EXPO, 35);
/**
* UART ESC Turtle Mode Cosphi
*
* @group UART ESC
* @group MODAL IO
* @min 0.000
* @max 1.000
* @decimal 10
* @increment 0.001
*/
PARAM_DEFINE_FLOAT(UART_ESC_T_COSP, 0.990);
PARAM_DEFINE_FLOAT(MODAL_IO_T_COSP, 0.990);
@@ -32,20 +32,20 @@
****************************************************************************/
#include "string.h"
#include "modalai_esc_serial.hpp"
#include "modal_io_serial.hpp"
ModalaiEscSerial::ModalaiEscSerial()
ModalIoSerial::ModalIoSerial()
{
}
ModalaiEscSerial::~ModalaiEscSerial()
ModalIoSerial::~ModalIoSerial()
{
if (_uart_fd >= 0) {
uart_close();
}
}
int ModalaiEscSerial::uart_open(const char *dev, speed_t speed)
int ModalIoSerial::uart_open(const char *dev, speed_t speed)
{
if (_uart_fd >= 0) {
PX4_ERR("Port in use: %s (%i)", dev, errno);
@@ -110,7 +110,7 @@ int ModalaiEscSerial::uart_open(const char *dev, speed_t speed)
return 0;
}
int ModalaiEscSerial::uart_set_baud(speed_t speed)
int ModalIoSerial::uart_set_baud(speed_t speed)
{
#ifndef __PX4_QURT
@@ -134,7 +134,7 @@ int ModalaiEscSerial::uart_set_baud(speed_t speed)
return -1;
}
int ModalaiEscSerial::uart_close()
int ModalIoSerial::uart_close()
{
#ifndef __PX4_QURT
@@ -158,7 +158,7 @@ int ModalaiEscSerial::uart_close()
return 0;
}
int ModalaiEscSerial::uart_write(FAR void *buf, size_t len)
int ModalIoSerial::uart_write(FAR void *buf, size_t len)
{
if (_uart_fd < 0 || buf == NULL) {
PX4_ERR("invalid state for writing or buffer");
@@ -172,7 +172,7 @@ int ModalaiEscSerial::uart_write(FAR void *buf, size_t len)
#endif
}
int ModalaiEscSerial::uart_read(FAR void *buf, size_t len)
int ModalIoSerial::uart_read(FAR void *buf, size_t len)
{
if (_uart_fd < 0 || buf == NULL) {
PX4_ERR("invalid state for reading or buffer");
@@ -43,11 +43,11 @@
#define FAR
#endif
class ModalaiEscSerial
class ModalIoSerial
{
public:
ModalaiEscSerial();
virtual ~ModalaiEscSerial();
ModalIoSerial();
virtual ~ModalIoSerial();
int uart_open(const char *dev, speed_t speed);
int uart_set_baud(speed_t speed);
@@ -55,7 +55,7 @@ public:
int uart_write(FAR void *buf, size_t len);
int uart_read(FAR void *buf, size_t len);
bool is_open() { return _uart_fd >= 0; };
int uart_get_baud() {return _speed; }
int uart_get_baud() {return _speed; }
private:
int _uart_fd = -1;
@@ -1,26 +1,26 @@
module_name: MODALAI ESC Output
module_name: MODAL IO Output
actuator_output:
show_subgroups_if: 'UART_ESC_CONFIG>0'
show_subgroups_if: 'MODAL_IO_CONFIG>0'
config_parameters:
- param: 'UART_ESC_CONFIG'
- param: 'MODAL_IO_CONFIG'
label: 'Configure'
function: 'enable'
- param: 'UART_ESC_BAUD'
- param: 'MODAL_IO_BAUD'
label: 'Bitrate'
- param: 'UART_ESC_RPM_MIN'
- param: 'MODAL_IO_RPM_MIN'
label: 'RPM Min'
- param: 'UART_ESC_RPM_MAX'
- param: 'MODAL_IO_RPM_MAX'
label: 'RPM Max'
- param: 'UART_ESC_SDIR1'
- param: 'MODAL_IO_SDIR1'
label: 'ESC 1 Spin Direction'
- param: 'UART_ESC_SDIR2'
- param: 'MODAL_IO_SDIR2'
label: 'ESC 2 Spin Direction'
- param: 'UART_ESC_SDIR3'
- param: 'MODAL_IO_SDIR3'
label: 'ESC 3 Spin Direction'
- param: 'UART_ESC_SDIR4'
- param: 'MODAL_IO_SDIR4'
label: 'ESC 4 Spin Direction'
output_groups:
- param_prefix: UART_ESC
- param_prefix: MODAL_IO
group_label: 'ESCs'
channel_label: 'ESC'
num_channels: 4
@@ -1,5 +0,0 @@
menuconfig DRIVERS_ACTUATORS_MODALAI_ESC
bool "modalai_esc"
default n
---help---
Enable support for modalai_esc