dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

drivers/actuators: modalai_esc driver 

Co-authored-by: Travis Bottalico <travis@modalai.com>
Co-authored-by: akushley <akushley>
This commit is contained in:
Daniel Agar 2022-07-06 18:00:50 -04:00
parent fe9af6769c
commit f591988f32
16 changed files with 2949 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
boards/modalai/fc-v1/default.px4board
View File

@ -5,6 +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_ADC_ADS1115=y
CONFIG_DRIVERS_ADC_BOARD_ADC=y
CONFIG_COMMON_BAROMETERS=y
@ -42,6 +43,7 @@ CONFIG_MODULES_AIRSPEED_SELECTOR=y
CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
CONFIG_MODULES_CAMERA_FEEDBACK=y
CONFIG_MODULES_COMMANDER=y
CONFIG_MODULES_CONTROL_ALLOCATOR=y
CONFIG_MODULES_DATAMAN=y
CONFIG_MODULES_EKF2=y
CONFIG_MODULES_ESC_BATTERY=y
@ -50,6 +52,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y
CONFIG_MODULES_GYRO_CALIBRATION=y
CONFIG_MODULES_GYRO_FFT=y
CONFIG_MODULES_LAND_DETECTOR=y
@ -65,7 +68,6 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
CONFIG_MODULES_MC_POS_CONTROL=y
CONFIG_MODULES_MC_RATE_CONTROL=y
CONFIG_MODULES_CONTROL_ALLOCATOR=y
CONFIG_MODULES_NAVIGATOR=y
CONFIG_MODULES_RC_UPDATE=y
CONFIG_MODULES_ROVER_POS_CONTROL=y
@ -74,10 +76,9 @@ CONFIG_MODULES_SIH=y
CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
CONFIG_MODULES_UUV_ATT_CONTROL=y
CONFIG_MODULES_UUV_POS_CONTROL=y
CONFIG_MODULES_GIMBAL=y
CONFIG_MODULES_VTOL_ATT_CONTROL=y
CONFIG_SYSTEMCMDS_BL_UPDATE=y
CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
CONFIG_SYSTEMCMDS_BL_UPDATE=y
CONFIG_SYSTEMCMDS_DMESG=y
CONFIG_SYSTEMCMDS_DUMPFILE=y
CONFIG_SYSTEMCMDS_GPIO=y

7
boards/modalai/fc-v2/default.px4board
View File

@ -3,6 +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_ADC_ADS1115=y
CONFIG_DRIVERS_ADC_BOARD_ADC=y
CONFIG_COMMON_BAROMETERS=y
@ -40,6 +41,7 @@ CONFIG_MODULES_AIRSPEED_SELECTOR=y
CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
CONFIG_MODULES_CAMERA_FEEDBACK=y
CONFIG_MODULES_COMMANDER=y
CONFIG_MODULES_CONTROL_ALLOCATOR=y
CONFIG_MODULES_DATAMAN=y
CONFIG_MODULES_EKF2=y
CONFIG_MODULES_ESC_BATTERY=y
@ -48,6 +50,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y
CONFIG_MODULES_GYRO_CALIBRATION=y
CONFIG_MODULES_GYRO_FFT=y
CONFIG_MODULES_LAND_DETECTOR=y
@ -63,17 +66,15 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
CONFIG_MODULES_MC_POS_CONTROL=y
CONFIG_MODULES_MC_RATE_CONTROL=y
CONFIG_MODULES_CONTROL_ALLOCATOR=y
CONFIG_MODULES_NAVIGATOR=y
CONFIG_MODULES_RC_UPDATE=y
CONFIG_MODULES_ROVER_POS_CONTROL=y
CONFIG_MODULES_SENSORS=y
CONFIG_MODULES_SIH=y
CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
CONFIG_MODULES_GIMBAL=y
CONFIG_MODULES_VTOL_ATT_CONTROL=y
CONFIG_SYSTEMCMDS_BL_UPDATE=y
CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
CONFIG_SYSTEMCMDS_BL_UPDATE=y
CONFIG_SYSTEMCMDS_DMESG=y
CONFIG_SYSTEMCMDS_GPIO=y
CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y

3
src/drivers/actuators/Kconfig Normal file
View File

@ -0,0 +1,3 @@
menu "actuators"
rsource "*/Kconfig"
endmenu #actuators

50
src/drivers/actuators/modalai_esc/CMakeLists.txt Normal file
View File

@ -0,0 +1,50 @@
############################################################################
#
# Copyright (c) 2020 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.
#
############################################################################
px4_add_module(
MODULE drivers__actuators__modalai_esc
MAIN modalai_esc
SRCS
crc16.c
crc16.h
modalai_esc_serial.cpp
modalai_esc_serial.hpp
modalai_esc.cpp
modalai_esc.hpp
qc_esc_packet_types.h
qc_esc_packet.c
qc_esc_packet.h
DEPENDS
px4_work_queue
)

5
src/drivers/actuators/modalai_esc/Kconfig Normal file
View File

@ -0,0 +1,5 @@
menuconfig DRIVERS_ACTUATORS_MODALAI_ESC
bool "modalai_esc"
default n
---help---
Enable support for modalai_esc

95
src/drivers/actuators/modalai_esc/crc16.c Normal file
View File

@ -0,0 +1,95 @@
/****************************************************************************
* Copyright (c) 2017 The Linux Foundation. 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 The Linux Foundation nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY
* THIS LICENSE.
* 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.
* In addition Supplemental Terms apply. See the SUPPLEMENTAL file.
*
****************************************************************************/
#include <stdint.h>
#include "crc16.h"
// Look-up table for fast CRC16 computations
const uint16_t crc16_table[256] = {
0x0000, 0xc0c1, 0xc181, 0x0140, 0xc301, 0x03c0, 0x0280, 0xc241,
0xc601, 0x06c0, 0x0780, 0xc741, 0x0500, 0xc5c1, 0xc481, 0x0440,
0xcc01, 0x0cc0, 0x0d80, 0xcd41, 0x0f00, 0xcfc1, 0xce81, 0x0e40,
0x0a00, 0xcac1, 0xcb81, 0x0b40, 0xc901, 0x09c0, 0x0880, 0xc841,
0xd801, 0x18c0, 0x1980, 0xd941, 0x1b00, 0xdbc1, 0xda81, 0x1a40,
0x1e00, 0xdec1, 0xdf81, 0x1f40, 0xdd01, 0x1dc0, 0x1c80, 0xdc41,
0x1400, 0xd4c1, 0xd581, 0x1540, 0xd701, 0x17c0, 0x1680, 0xd641,
0xd201, 0x12c0, 0x1380, 0xd341, 0x1100, 0xd1c1, 0xd081, 0x1040,
0xf001, 0x30c0, 0x3180, 0xf141, 0x3300, 0xf3c1, 0xf281, 0x3240,
0x3600, 0xf6c1, 0xf781, 0x3740, 0xf501, 0x35c0, 0x3480, 0xf441,
0x3c00, 0xfcc1, 0xfd81, 0x3d40, 0xff01, 0x3fc0, 0x3e80, 0xfe41,
0xfa01, 0x3ac0, 0x3b80, 0xfb41, 0x3900, 0xf9c1, 0xf881, 0x3840,
0x2800, 0xe8c1, 0xe981, 0x2940, 0xeb01, 0x2bc0, 0x2a80, 0xea41,
0xee01, 0x2ec0, 0x2f80, 0xef41, 0x2d00, 0xedc1, 0xec81, 0x2c40,
0xe401, 0x24c0, 0x2580, 0xe541, 0x2700, 0xe7c1, 0xe681, 0x2640,
0x2200, 0xe2c1, 0xe381, 0x2340, 0xe101, 0x21c0, 0x2080, 0xe041,
0xa001, 0x60c0, 0x6180, 0xa141, 0x6300, 0xa3c1, 0xa281, 0x6240,
0x6600, 0xa6c1, 0xa781, 0x6740, 0xa501, 0x65c0, 0x6480, 0xa441,
0x6c00, 0xacc1, 0xad81, 0x6d40, 0xaf01, 0x6fc0, 0x6e80, 0xae41,
0xaa01, 0x6ac0, 0x6b80, 0xab41, 0x6900, 0xa9c1, 0xa881, 0x6840,
0x7800, 0xb8c1, 0xb981, 0x7940, 0xbb01, 0x7bc0, 0x7a80, 0xba41,
0xbe01, 0x7ec0, 0x7f80, 0xbf41, 0x7d00, 0xbdc1, 0xbc81, 0x7c40,
0xb401, 0x74c0, 0x7580, 0xb541, 0x7700, 0xb7c1, 0xb681, 0x7640,
0x7200, 0xb2c1, 0xb381, 0x7340, 0xb101, 0x71c0, 0x7080, 0xb041,
0x5000, 0x90c1, 0x9181, 0x5140, 0x9301, 0x53c0, 0x5280, 0x9241,
0x9601, 0x56c0, 0x5780, 0x9741, 0x5500, 0x95c1, 0x9481, 0x5440,
0x9c01, 0x5cc0, 0x5d80, 0x9d41, 0x5f00, 0x9fc1, 0x9e81, 0x5e40,
0x5a00, 0x9ac1, 0x9b81, 0x5b40, 0x9901, 0x59c0, 0x5880, 0x9841,
0x8801, 0x48c0, 0x4980, 0x8941, 0x4b00, 0x8bc1, 0x8a81, 0x4a40,
0x4e00, 0x8ec1, 0x8f81, 0x4f40, 0x8d01, 0x4dc0, 0x4c80, 0x8c41,
0x4400, 0x84c1, 0x8581, 0x4540, 0x8701, 0x47c0, 0x4680, 0x8641,
0x8201, 0x42c0, 0x4380, 0x8341, 0x4100, 0x81c1, 0x8081, 0x4040,
};
uint16_t crc16_init()
{
return 0xFFFF;
}
uint16_t crc16_byte(uint16_t prev_crc, const uint8_t new_byte)
{
uint8_t lut = (prev_crc ^ new_byte) & 0xFF;
return (prev_crc >> 8) ^ crc16_table[lut];
}
uint16_t crc16(uint16_t prev_crc, uint8_t const *input_buffer, uint16_t input_length)
{
uint16_t crc = prev_crc;
while (input_length--) {
crc = crc16_byte(crc, *input_buffer++);
}
return crc;
}

65
src/drivers/actuators/modalai_esc/crc16.h Normal file
View File

@ -0,0 +1,65 @@
/****************************************************************************
* Copyright (c) 2017 The Linux Foundation. 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 The Linux Foundation nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY
* THIS LICENSE.
* 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.
* In addition Supplemental Terms apply. See the SUPPLEMENTAL file.
*
****************************************************************************/
/*
* This file contains function prototypes for crc16 computations using polynomial 0x8005
*/
#ifndef CRC16_H_
#define CRC16_H_
#ifdef __cplusplus
extern "C"
{
#endif
#include <stdint.h>
// Returns the seed of crc output, which should be used when computing crc16 of a byte sequence
uint16_t crc16_init(void);
// Process one byte by providing crc16 from previous step and new byte to consume.
// Output is the new crc16 value
uint16_t crc16_byte(uint16_t prev_crc, const uint8_t new_byte);
// Process an array of bytes by providing crc16 from previous step (or seed), array of bytes and its length
// Output is the new crc16 value
uint16_t crc16(uint16_t prev_crc, uint8_t const *input_buffer, uint16_t input_length);
#ifdef __cplusplus
}
#endif
#endif //CRC16_H_

1492
src/drivers/actuators/modalai_esc/modalai_esc.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

237
src/drivers/actuators/modalai_esc/modalai_esc.hpp Normal file
View File

@ -0,0 +1,237 @@
/****************************************************************************
*
* Copyright (c) 2020 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 <drivers/drv_mixer.h>
#include <lib/cdev/CDev.hpp>
#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/esc_status.h>
#include "modalai_esc_serial.hpp"
#include "qc_esc_packet.h"
#include "qc_esc_packet_types.h"
class ModalaiEsc : public cdev::CDev, public ModuleBase<ModalaiEsc>, public OutputModuleInterface
{
public:
ModalaiEsc();
virtual ~ModalaiEsc();
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
/** @see ModuleBase::run() */
void Run() override;
/** @see ModuleBase::print_status() */
int print_status() override;
/** @see OutputModuleInterface */
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated) override;
/** @see OutputModuleInterface */
void mixerChanged() override;
virtual int ioctl(file *filp, int cmd, unsigned long arg);
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;
uint16_t repeats = 0;
uint16_t repeat_delay_us = 2000;
uint8_t retries = 0;
bool response = false;
uint16_t resp_delay_us = 1000;
bool print_feedback = false;
static const uint8_t BUF_SIZE = 128;
uint8_t buf[BUF_SIZE];
bool valid() const { return len > 0; }
void clear() { len = 0; }
};
int sendCommandThreadSafe(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 MODALAI_ESC_WRITE_WAIT_US = 200;
static constexpr uint32_t MODALAI_ESC_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 float MODALAI_ESC_MODE_DISABLED_SETPOINT = -0.1f;
static constexpr float MODALAI_ESC_MODE_THRESHOLD = 0.0f;
static constexpr float MODALAI_ESC_MODE_DEAD_ZONE_MIN = 0.0f;
static constexpr float MODALAI_ESC_MODE_DEAD_ZONE_MAX = 1.0f;
static constexpr float MODALAI_ESC_MODE_DEAD_ZONE_1 = 0.30f;
static constexpr float MODALAI_ESC_MODE_DEAD_ZONE_2 = 0.02f;
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 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); }
ModalaiEscSerial *_uart_port;
ModalaiEscSerial *_uart_port_bridge;
typedef struct {
int32_t config{MODALAI_ESC_UART_CONFIG};
int32_t mode{MODALAI_ESC_MODE};
float dead_zone_1{MODALAI_ESC_MODE_DEAD_ZONE_1};
float dead_zone_2{MODALAI_ESC_MODE_DEAD_ZONE_2};
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 motor_map[MODALAI_ESC_OUTPUT_CHANNELS] {1, 2, 3, 4};
} uart_esc_params_t;
struct EscChan {
int16_t rate_req;
uint8_t state;
uint16_t rate_meas;
uint8_t power_applied;
uint8_t led;
uint8_t cmd_counter;
float voltage; //Volts
float current; //Amps
float temperature; //deg C
hrt_abstime feedback_time;
};
typedef struct {
uint8_t number;
int8_t direction;
} ch_assign_t;
typedef struct {
led_control_s control{};
vehicle_control_mode_s mode{};
uint8_t led_mask;// TODO led_mask[MODALAI_ESC_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}};
MixingOutput _mixing_output{"MODALAI_ESC", MODALAI_ESC_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
int _class_instance{-1};
perf_counter_t _cycle_perf;
perf_counter_t _output_update_perf;
bool _outputs_on{false};
unsigned _current_update_rate{0};
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _led_update_sub{ORB_ID(led_control)};
//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;
int update_params();
int load_params(uart_esc_params_t *params, ch_assign_t *map);
bool _turtle_mode_en{false};
int32_t _rpm_fullscale{0};
manual_control_setpoint_s _manual_control_setpoint{};
uint16_t _cmd_id{0};
Command _current_cmd;
px4::atomic<Command *> _pending_cmd{nullptr};
EscChan _esc_chans[MODALAI_ESC_OUTPUT_CHANNELS] {};
Command _esc_cmd;
esc_status_s _esc_status;
EscPacket _fb_packet;
EscPacket _uart_bridge_packet;
led_rsc_t _led_rsc;
int _fb_idx;
static const uint8_t READ_BUF_SIZE = 128;
uint8_t _read_buf[READ_BUF_SIZE];
void updateLeds(vehicle_control_mode_s mode, led_control_s control);
int populateCommand(uart_esc_cmd_t cmd_type, uint8_t cmd_mask, Command *out_cmd);
int readResponse(Command *out_cmd);
int parseResponse(uint8_t *buf, uint8_t len, bool print_feedback);
int flushUartRx();
int checkForEscTimeout();
};

163
src/drivers/actuators/modalai_esc/modalai_esc_serial.cpp Normal file
View File

@ -0,0 +1,163 @@
/****************************************************************************
*
* Copyright (c) 2020 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 "string.h"
#include "modalai_esc_serial.hpp"
ModalaiEscSerial::ModalaiEscSerial()
{
}
ModalaiEscSerial::~ModalaiEscSerial()
{
if (_uart_fd >= 0) {
uart_close();
}
}
int ModalaiEscSerial::uart_open(const char *dev, speed_t speed)
{
if (_uart_fd >= 0) {
PX4_ERR("Port in use: %s (%i)", dev, errno);
return -1;
}
/* Open UART */
_uart_fd = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK);
if (_uart_fd < 0) {
PX4_ERR("Error opening port: %s (%i)", dev, errno);
return -1;
}
/* Back up the original UART configuration to restore it after exit */
int termios_state;
if ((termios_state = tcgetattr(_uart_fd, &_orig_cfg)) < 0) {
PX4_ERR("Error configuring port: tcgetattr %s: %d", dev, termios_state);
uart_close();
return -1;
}
/* Fill the struct for the new configuration */
tcgetattr(_uart_fd, &_cfg);
/* Disable output post-processing */
_cfg.c_oflag &= ~OPOST;
_cfg.c_cflag |= (CLOCAL | CREAD); /* ignore modem controls */
_cfg.c_cflag &= ~CSIZE;
_cfg.c_cflag |= CS8; /* 8-bit characters */
_cfg.c_cflag &= ~PARENB; /* no parity bit */
_cfg.c_cflag &= ~CSTOPB; /* only need 1 stop bit */
_cfg.c_cflag &= ~CRTSCTS; /* no hardware flowcontrol */
/* setup for non-canonical mode */
_cfg.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
_cfg.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
if (cfsetispeed(&_cfg, speed) < 0 || cfsetospeed(&_cfg, speed) < 0) {
PX4_ERR("Error configuring port: %s: %d (cfsetispeed, cfsetospeed)", dev, termios_state);
uart_close();
return -1;
}
if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &_cfg)) < 0) {
PX4_ERR("Error configuring port: %s (tcsetattr)", dev);
uart_close();
return -1;
}
_speed = speed;
return 0;
}
int ModalaiEscSerial::uart_set_baud(speed_t speed)
{
if (_uart_fd < 0) {
return -1;
}
if (cfsetispeed(&_cfg, speed) < 0) {
return -1;
}
if (tcsetattr(_uart_fd, TCSANOW, &_cfg) < 0) {
return -1;
}
_speed = speed;
return 0;
}
int ModalaiEscSerial::uart_close()
{
if (_uart_fd < 0) {
PX4_ERR("invalid state for closing");
return -1;
}
if (tcsetattr(_uart_fd, TCSANOW, &_orig_cfg)) {
PX4_ERR("failed restoring uart to original state");
}
if (close(_uart_fd)) {
PX4_ERR("error closing uart");
}
_uart_fd = -1;
return 0;
}
int ModalaiEscSerial::uart_write(FAR void *buf, size_t len)
{
if (_uart_fd < 0 || buf == NULL) {
PX4_ERR("invalid state for writing or buffer");
return -1;
}
return write(_uart_fd, buf, len);
}
int ModalaiEscSerial::uart_read(FAR void *buf, size_t len)
{
if (_uart_fd < 0 || buf == NULL) {
PX4_ERR("invalid state for reading or buffer");
return -1;
}
return read(_uart_fd, buf, len);
}

61
src/drivers/actuators/modalai_esc/modalai_esc_serial.hpp Normal file
View File

@ -0,0 +1,61 @@
/****************************************************************************
*
* Copyright (c) 2020 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 <errno.h>
#include <fcntl.h>
#include <termios.h>
class ModalaiEscSerial
{
public:
ModalaiEscSerial();
virtual ~ModalaiEscSerial();
int uart_open(const char *dev, speed_t speed);
int uart_set_baud(speed_t speed);
int uart_close();
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; }
private:
int _uart_fd = -1;
struct termios _orig_cfg;
struct termios _cfg;
int _speed = -1;
};

167
src/drivers/actuators/modalai_esc/parameters.c Normal file
View File

@ -0,0 +1,167 @@
/****************************************************************************
*
* Copyright (c) 2020-2022 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.
*
****************************************************************************/
/**
* UART ESC configuration
*
* Selects what type of UART ESC, if any, is being used.
*
* @reboot_required true
*
* @group UART ESC
* @value 0 - Disabled
* @value 1 - VOXL ESC
* @min 0
* @max 1
*/
PARAM_DEFINE_INT32(UART_ESC_CONFIG, 0);
/**
* UART ESC baud rate
*
* Default rate is 250Kbps, which is used in off-the-shelf MoadalAI ESC products.
*
* @group UART ESC
* @unit bit/s
*/
PARAM_DEFINE_INT32(UART_ESC_BAUD, 250000);
/**
* Motor mappings for ModalAI ESC M004
*
* HW Channel Idexes (PX4 Indexes) (note: silkscreen shows 0 indexed)
* 4(1) 3(4)
* [front]
* 1(3) 2(2)
*/
/**
* UART ESC Motor 1 Mapping. 1-4 (negative for reversal).
*
* @group UART ESC
* @min -4
* @max 4
*/
PARAM_DEFINE_INT32(UART_ESC_MOTOR1, 3);
/**
*UART ESC Motor 2 Mapping. 1-4 (negative for reversal).
*
* @group UART ESC
* @min -4
* @max 4
*/
PARAM_DEFINE_INT32(UART_ESC_MOTOR2, 2);
/**
* UART ESC Motor 3 Mapping. 1-4 (negative for reversal).
*
* @group UART ESC
* @min -4
* @max 4
*/
PARAM_DEFINE_INT32(UART_ESC_MOTOR3, 4);
/**
* UART ESC Motor 4 Mapping. 1-4 (negative for reversal).
*
* @group UART ESC
* @min -4
* @max 4
*/
PARAM_DEFINE_INT32(UART_ESC_MOTOR4, 1);
/**
* UART ESC RPM Min
*
* Minimum RPM for ESC
*
* @group UART ESC
* @unit RPM
*/
PARAM_DEFINE_INT32(UART_ESC_RPM_MIN, 5500);
/**
* UART ESC RPM Max
*
* Maximum RPM for ESC
*
* @group UART ESC
* @unit RPM
*/
PARAM_DEFINE_INT32(UART_ESC_RPM_MAX, 15000);
/**
* UART ESC Mode
*
* Selects what type of mode is enabled, if any
*
* @reboot_required true
*
* @group UART ESC
* @value 0 - None
* @value 1 - Turtle Mode enabled via AUX1
* @value 2 - Turtle Mode enabled via AUX2
* @min 0
* @max 2
*/
PARAM_DEFINE_INT32(UART_ESC_MODE, 0);
/**
* UART ESC Mode Deadzone 1.
*
* Must be greater than Deadzone 2.
* Absolute value of stick position needed to activate a motor.
*
* @group UART ESC Mode Deadzone 1
* @min 0.01
* @max 0.99
* @decimal 10
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(UART_ESC_DEAD1, 0.30f);
/**
* UART ESC Mode Deadzone 2.
*
* Must be less than Deadzone 1.
* Absolute value of stick position considered no longer on the X or Y axis,
* thus targetting a specific motor (single).
*
* @group UART ESC Mode Deadzone 2
* @min 0.01
* @max 0.99
* @decimal 10
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(UART_ESC_DEAD2, 0.02f);

254
src/drivers/actuators/modalai_esc/qc_esc_packet.c Normal file
View File

@ -0,0 +1,254 @@
/****************************************************************************
* Copyright (c) 2017 The Linux Foundation. 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 The Linux Foundation nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY
* THIS LICENSE.
* 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.
* In addition Supplemental Terms apply. See the SUPPLEMENTAL file.
*
****************************************************************************/
#include "crc16.h"
#include "qc_esc_packet.h"
#include "qc_esc_packet_types.h"
#include <string.h>
int32_t qc_esc_create_version_request_packet(uint8_t id, uint8_t *out, uint16_t out_size)
{
return qc_esc_create_packet(ESC_PACKET_TYPE_VERSION_REQUEST, &id, 1, out, out_size);
}
int32_t qc_esc_create_extended_version_request_packet(uint8_t id, uint8_t *out, uint16_t out_size)
{
return qc_esc_create_packet(ESC_PACKET_TYPE_VERSION_EXT_REQUEST, &id, 1, out, out_size);
}
int32_t qc_esc_create_reset_packet(uint8_t id, uint8_t *out, uint16_t out_size)
{
char payload[] = "RESET0";
payload[5] += id;
return qc_esc_create_packet(ESC_PACKET_TYPE_RESET_CMD, (uint8_t *)payload, 6 /*sizeof(payload)*/, out, out_size);
}
int32_t qc_esc_create_sound_packet(uint8_t frequency, uint8_t duration, uint8_t power, uint8_t mask, uint8_t *out,
uint16_t out_size)
{
uint8_t data[4] = {frequency, duration, power, mask};
return qc_esc_create_packet(ESC_PACKET_TYPE_SOUND_CMD, (uint8_t *) & (data[0]), 4, out, out_size);
}
int32_t qc_esc_create_led_control_packet(uint8_t led_byte_1, uint8_t led_byte_2, uint8_t *out, uint16_t out_size)
{
uint8_t data[2] = {led_byte_1, led_byte_2};
return qc_esc_create_packet(ESC_PACKET_TYPE_LED_CMD, (uint8_t *) & (data[0]), 2, out, out_size);
}
int32_t qc_esc_create_set_id_packet(uint8_t id, uint8_t *out, uint16_t out_size)
{
return qc_esc_create_packet(ESC_PACKET_TYPE_SET_ID_CMD, (uint8_t *)&id, 1, out, out_size);
}
int32_t qc_esc_create_pwm_packet4(int16_t pwm0, int16_t pwm1, int16_t pwm2, int16_t pwm3,
uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3,
uint8_t *out, uint16_t out_size)
{
return qc_esc_create_pwm_packet4_fb(pwm0, pwm1, pwm2, pwm3, led0, led1, led2, led3, -1, out, out_size);
}
int32_t qc_esc_create_pwm_packet4_fb(int16_t pwm0, int16_t pwm1, int16_t pwm2, int16_t pwm3,
uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3,
int32_t fb_id, uint8_t *out, uint16_t out_size)
{
uint16_t data[5];
uint16_t leds = 0;
if (fb_id != -1) { fb_id = fb_id % 4; }
//limit the pwm commands
if (pwm0 > 800) { pwm0 = 800; } if (pwm0 < -800) { pwm0 = -800; }
if (pwm1 > 800) { pwm1 = 800; } if (pwm1 < -800) { pwm1 = -800; }
if (pwm2 > 800) { pwm2 = 800; } if (pwm2 < -800) { pwm2 = -800; }
if (pwm3 > 800) { pwm3 = 800; } if (pwm3 < -800) { pwm3 = -800; }
//least significant bit is used for feedback request
pwm0 &= ~(0x0001); pwm1 &= ~(0x0001); pwm2 &= ~(0x0001); pwm3 &= ~(0x0001);
if (fb_id == 0) { pwm0 |= 0x0001; } if (fb_id == 1) { pwm1 |= 0x0001; }
if (fb_id == 2) { pwm2 |= 0x0001; } if (fb_id == 3) { pwm3 |= 0x0001; }
leds |= led0 & 0b00000111;
leds |= (led1 & 0b00000111) << 3;
leds |= ((uint16_t)(led2 & 0b00000111)) << 6;
leds |= ((uint16_t)(led3 & 0b00000111)) << 9;
data[0] = pwm0; data[1] = pwm1; data[2] = pwm2; data[3] = pwm3; data[4] = leds;
return qc_esc_create_packet(ESC_PACKET_TYPE_PWM_CMD, (uint8_t *) & (data[0]), 10, out, out_size);
}
int32_t qc_esc_create_rpm_packet4(int16_t rpm0, int16_t rpm1, int16_t rpm2, int16_t rpm3,
uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3,
uint8_t *out, uint16_t out_size)
{
return qc_esc_create_rpm_packet4_fb(rpm0, rpm1, rpm2, rpm3, led0, led1, led2, led3, -1, out, out_size);
}
int32_t qc_esc_create_rpm_packet4_fb(int16_t rpm0, int16_t rpm1, int16_t rpm2, int16_t rpm3,
uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3,
int32_t fb_id, uint8_t *out, uint16_t out_size)
{
uint16_t data[5];
uint16_t leds = 0;
if (fb_id != -1) { fb_id = fb_id % 4; }
//least significant bit is used for feedback request
rpm0 &= ~(0x0001); rpm1 &= ~(0x0001); rpm2 &= ~(0x0001); rpm3 &= ~(0x0001);
if (fb_id == 0) { rpm0 |= 0x0001; } if (fb_id == 1) { rpm1 |= 0x0001; }
if (fb_id == 2) { rpm2 |= 0x0001; } if (fb_id == 3) { rpm3 |= 0x0001; }
leds |= led0 & 0b00000111;
leds |= (led1 & 0b00000111) << 3;
leds |= ((uint16_t)(led2 & 0b00000111)) << 6;
leds |= ((uint16_t)(led3 & 0b00000111)) << 9;
data[0] = rpm0; data[1] = rpm1; data[2] = rpm2; data[3] = rpm3; data[4] = leds;
return qc_esc_create_packet(ESC_PACKET_TYPE_RPM_CMD, (uint8_t *) & (data[0]), 10, out, out_size);
}
int32_t qc_esc_create_packet(uint8_t type, uint8_t *data, uint16_t size, uint8_t *out, uint16_t out_size)
{
uint16_t packet_size = size + 5;
if (packet_size > 255) { return -1; }
if (out_size < packet_size) { return -2; }
out[0] = 0xAF;
out[1] = packet_size;
out[2] = type;
memcpy(&(out[3]), data, size);
uint16_t crc = crc16_init();
crc = crc16(crc, &(out[1]), packet_size - 3);
memcpy(&(out[packet_size - 2]), &crc, sizeof(uint16_t));
return packet_size;
}
//feed in a character and see if we got a complete packet
int16_t qc_esc_packet_process_char(uint8_t c, EscPacket *packet)
{
int16_t ret = ESC_NO_PACKET;
uint16_t chk_comp;
uint16_t chk_rcvd;
if (packet->len_received >= (sizeof(packet->buffer) - 1)) {
packet->len_received = 0;
}
//reset the packet and start parsing from beginning if length byte == header
//this can only happen if the packet is de-synced and last char of checksum
//ends up being equal to the header, in that case we can end up in endless loop
//unable to re-sync with the packet
if (packet->len_received == 1 && c == ESC_PACKET_HEADER) {
packet->len_received = 0;
}
switch (packet->len_received) {
case 0: //header
packet->bp = packet->buffer; //reset the pointer for storing data
qc_esc_packet_checksum_reset(packet); //reset the checksum to starting value
if (c != ESC_PACKET_HEADER) { //check the packet header
packet->len_received = 0;
ret = ESC_ERROR_BAD_HEADER;
break;
}
packet->len_received++;
*(packet->bp)++ = c;
break;
case 1: //length
packet->len_received++;
*(packet->bp)++ = c;
packet->len_expected = c;
if (packet->len_expected >= (sizeof(packet->buffer) - 1)) {
packet->len_received = 0;
ret = ESC_ERROR_BAD_LENGTH;
break;
}
qc_esc_packet_checksum_process_char(packet, c);
break;
default: //rest of the packet
packet->len_received++;
*(packet->bp)++ = c;
if (packet->len_received < (packet->len_expected - 1)) { //do not compute checksum of checksum (last 2 bytes)
qc_esc_packet_checksum_process_char(packet, c);
}
if (packet->len_received < packet->len_expected) { //waiting for more bytes
break;
}
//grab the computed checksum and compare against the received value
chk_comp = qc_esc_packet_checksum_get(packet);
memcpy(&chk_rcvd, packet->bp - 2, sizeof(uint16_t));
if (chk_comp == chk_rcvd) { ret = packet->len_received; }
else { ret = ESC_ERROR_BAD_CHECKSUM; }
packet->len_received = 0;
break;
}
return ret;
}

275
src/drivers/actuators/modalai_esc/qc_esc_packet.h Normal file
View File

@ -0,0 +1,275 @@
/****************************************************************************
* Copyright (c) 2017 The Linux Foundation. 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 The Linux Foundation nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY
* THIS LICENSE.
* 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.
* In addition Supplemental Terms apply. See the SUPPLEMENTAL file.
*
****************************************************************************/
/*
* This file contains function prototypes for Electronic Speed Controller (ESC) UART interface
*/
#ifndef QC_ESC_PACKET
#define QC_ESC_PACKET
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include "crc16.h"
// Define byte values that correspond to setting red, greed, and blue LEDs
#define QC_ESC_LED_RED_ON 1
#define QC_ESC_LED_GREEN_ON 2
#define QC_ESC_LED_BLUE_ON 4
// Header of the packet. Each packet must start with this header
#define ESC_PACKET_HEADER 0xAF
enum { ESC_ERROR_BAD_LENGTH = -3,
ESC_ERROR_BAD_HEADER = -2,
ESC_ERROR_BAD_CHECKSUM = -1,
ESC_NO_PACKET = 0
};
// Defines for the constatnt offsets of different parts of the packet
enum { ESC_PACKET_POS_HEADER1 = 0,
ESC_PACKET_POS_LENGTH,
ESC_PACKET_POS_TYPE,
ESC_PACKET_POS_DATA
};
// Definition of the structure that holds the state of the incoming data that is being recived (i.e. incomplete packets)
typedef struct {
uint8_t len_received; // Number of chars received so far
uint8_t len_expected; // Expected number of chars based on header
uint8_t *bp; // Pointer to the next write position in the buffer
uint16_t crc; // Accumulated CRC value so far
uint8_t buffer[64]; // Buffer to hold incoming data that is being parsed
} EscPacket;
// Definition of the response packet from ESC, containing the ESC version information
typedef struct {
uint8_t header;
uint8_t length; // Total length of the packet
uint8_t type; // This will be equal to ESC_PACKET_TYPE_VERSION_RESPONSE
uint8_t id; // ID of the ESC that responded
uint16_t sw_version; // Software version of the ESC firmware
uint16_t hw_version; // HW version of the board
uint32_t unique_id; // Unique ID of the ESC, if available
uint16_t crc;
} __attribute__((__packed__)) QC_ESC_VERSION_INFO;
typedef struct {
uint8_t header;
uint8_t length;
uint8_t type;
uint8_t id;
uint16_t sw_version;
uint16_t hw_version;
uint8_t unique_id[12];
char firmware_git_version[12];
char bootloader_git_version[12];
uint16_t bootloader_version;
uint16_t crc;
} __attribute__((__packed__)) QC_ESC_EXTENDED_VERSION_INFO;
// Definition of the feedback response packet from ESC
typedef struct {
uint8_t header;
uint8_t length; // Total length of the packet
uint8_t type; // This will be equal to ESC_PACKET_TYPE_FB_RESPONSE
uint8_t state; // bits 0:3 = state, bits 4:7 = ID
uint16_t rpm; // Current RPM of the motor
uint8_t cmd_counter; // Number of commands received by the ESC
uint8_t reserved0;
int8_t voltage; // Voltage = (-28)/34.0 + 9.0 = 8.176V. 0xE4 --> 228 (-28)
uint8_t reserved1;
uint16_t crc;
} __attribute__((__packed__)) QC_ESC_FB_RESPONSE;
// Definition of the feedback response packet from ESC
typedef struct {
uint8_t header;
uint8_t length; // Total length of the packet
uint8_t type; // This will be equal to ESC_PACKET_TYPE_FB_RESPONSE
uint8_t id_state; // bits 0:3 = state, bits 4:7 = ID
uint16_t rpm; // Current RPM of the motor
uint8_t cmd_counter; // Number of commands received by the ESC
uint8_t power; // Applied power [0..100]
uint16_t voltage; // Voltage measured by the ESC in mV
int16_t current; // Current measured by the ESC in 8mA resolution
int16_t temperature; // Temperature measured by the ESC in 0.01 degC resolution
uint16_t crc;
} __attribute__((__packed__)) QC_ESC_FB_RESPONSE_V2;
//-------------------------------------------------------------------------
//Below are functions for generating packets that would be outgoing to ESCs
//-------------------------------------------------------------------------
// Create a generic packet by providing all required components
// Inputs are packet type, input data array and its size, output array and maximum size of output array
// Resulting packet will be placed in the output data array together with appropriate header and checksum
// Output value represents total length of the created packet (if positive), otherwise error code
int32_t qc_esc_create_packet(uint8_t type, uint8_t *input_data, uint16_t input_size, uint8_t *out_data,
uint16_t out_data_size);
// Create a packet for requesting version information from ESC with desired id
// If an ESC with this id is connected and receives this command, it will reply with it's version information
int32_t qc_esc_create_version_request_packet(uint8_t id, uint8_t *out, uint16_t out_size);
int32_t qc_esc_create_extended_version_request_packet(uint8_t id, uint8_t *out, uint16_t out_size);
// Create a packet for requesting an ESC with desired id to reset
// When ESC with the particular id receives this command, and it's not spinning, ESC will reset
// This is useful for entering bootloader without removing power from the system
int32_t qc_esc_create_reset_packet(uint8_t id, uint8_t *out, uint16_t out_size);
// Create a packet for generating a tone packet (signals are applied to motor to generate sounds)
// Inputs are relative frequency (0-255), relative duration (0-255), power (0-255) and bit mask for which ESCs should play a tone
// Bit mask definition: if bit i is set to 1, then ESC with ID=i will generate the tone
// Note that tones can only be generated when motor is not spinning
int32_t qc_esc_create_sound_packet(uint8_t frequency, uint8_t duration, uint8_t power, uint8_t mask, uint8_t *out,
uint16_t out_size);
// Create a packet for standalone LED control
// Bit mask definition:
// led_byte_1 - bit0 = ESC0 Red, bit1 = ESC0, Green, bit2 = ESC0 Blue, bit3 = ESC1 Red, bit4 = ESC1 Green,
// bit5 = ESC1 Blue, bit6 = ESC2 Red, bit7 = ESC2 Green
// led_byte_2 - bit0 = ESC2 Blue, bit1 = ESC3 Red, bit2 = ESC3 Green, bit3 = ESC3 Blue, bits 4:7 = unused
// Note that control can only be sent when motor is not spinning
int32_t qc_esc_create_led_control_packet(uint8_t led_byte_1, uint8_t led_byte_2, uint8_t *out, uint16_t out_size);
// Create a packet for setting the ID of an ESC
// Return value is the length of generated packet (if positive), otherwise error code
// Note that all ESCs that will receive this command will be set to this ID
int32_t qc_esc_create_set_id_packet(uint8_t id, uint8_t *out, uint16_t out_size);
// Create a packet for sending open-loop command and LED command to 4 ESCs without requesting any feedback
// Return value is the length of generated packet (if positive), otherwise error code
int32_t qc_esc_create_pwm_packet4(int16_t pwm0, int16_t pwm1, int16_t pwm2, int16_t pwm3,
uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3,
uint8_t *out, uint16_t out_size);
// Create a packet for sending open-loop command and LED command to 4 ESCs, also request feedback from one ESC (with id=fb_id)
// Return value is the length of generated packet (if positive), otherwise error code
int32_t qc_esc_create_pwm_packet4_fb(int16_t pwm0, int16_t pwm1, int16_t pwm2, int16_t pwm3,
uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3,
int32_t fb_id, uint8_t *out, uint16_t out_size);
// Create a packet for sending closed-loop RPM command and LED command to 4 ESCs without requesting any feedback
// Return value is the length of generated packet (if positive), otherwise error code
int32_t qc_esc_create_rpm_packet4(int16_t rpm0, int16_t rpm1, int16_t rpm2, int16_t rpm3,
uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3,
uint8_t *out, uint16_t out_size);
// Create a packet for sending closed-loop RPM command and LED command to 4 ESCs, also request feedback from one ESC (with id=fb_id)
// Return value is the length of generated packet (if positive), otherwise error code
int32_t qc_esc_create_rpm_packet4_fb(int16_t rpm0, int16_t rpm1, int16_t rpm2, int16_t rpm3,
uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3,
int32_t fb_id, uint8_t *out, uint16_t out_size);
//-------------------------------------------------------------------------
// Below are functions for processing incoming packets
//-------------------------------------------------------------------------
// Feed one char and see if we have accumulated a complete packet
int16_t qc_esc_packet_process_char(uint8_t c, EscPacket *packet);
// Get a pointer to the packet type from a pointer to EscPacket
static inline uint8_t qc_esc_packet_get_type(EscPacket *packet) { return packet->buffer[ESC_PACKET_POS_TYPE]; }
// Get a pointer to the packet type from a uint8_t pointer that points to the raw packet data as it comes from UART port
static inline uint8_t qc_esc_packet_raw_get_type(uint8_t *packet) { return packet[ESC_PACKET_POS_TYPE]; }
//get a pointer to the packet payload from a pointer to EscPacket
static inline uint8_t *qc_esc_packet_get_data_ptr(EscPacket *packet) { return &(packet->buffer[ESC_PACKET_POS_DATA]); }
// Get a pointer to the packet payload from a uint8_t pointer that points to the raw packet data as it comes from UART port
static inline uint8_t *qc_esc_packet_raw_get_data_ptr(uint8_t *packet) { return &(packet[ESC_PACKET_POS_DATA]); }
// Get the total size (length) in bytes of the packet
static inline uint8_t qc_esc_packet_get_size(EscPacket *packet) { return packet->buffer[ESC_PACKET_POS_LENGTH]; }
// Get checksum of the packet from a pointer to EscPacket
static inline uint16_t qc_esc_packet_checksum_get(EscPacket *packet) { return packet->crc; }
// Calculate the checksum of a data array. Used for packet generation / processing
static inline uint16_t qc_esc_packet_checksum(uint8_t *buf, uint16_t size)
{
uint16_t crc = crc16_init();
return crc16(crc, buf, size);
}
// Reset the checksum of the incoming packet. Used internally for packet reception
static inline void qc_esc_packet_checksum_reset(EscPacket *packet) { packet->crc = crc16_init(); }
// Process one character for checksum calculation while receiving a packet (used internally for packet reception)
static inline void qc_esc_packet_checksum_process_char(EscPacket *packet, uint8_t c)
{
packet->crc = crc16_byte(packet->crc, c);
}
// Initialize an instance of an EscPacket. This should be called once before using an instance of EscPacket
static inline void qc_esc_packet_init(EscPacket *packet)
{
packet->len_received = 0;
packet->len_expected = 0;
packet->bp = 0;
qc_esc_packet_checksum_reset(packet);
}
// Reset status of the packet that is being parsed. Effectively, this achieves the same thing as _packet_init
// so that _packet_init may be redundant
static inline void qc_esc_packet_reset(EscPacket *packet)
{
packet->len_received = 0;
}
#endif //QC_ESC_PACKET
#ifdef __cplusplus
}
#endif

73
src/drivers/actuators/modalai_esc/qc_esc_packet_types.h Normal file
View File

@ -0,0 +1,73 @@
/****************************************************************************
* Copyright (c) 2017 The Linux Foundation. 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 The Linux Foundation nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY
* THIS LICENSE.
* 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.
* In addition Supplemental Terms apply. See the SUPPLEMENTAL file.
*
****************************************************************************/
/*
* This file contains the type values of all supported ESC UART packets
*/
#ifndef QC_ESC_PACKET_TYPES
#define QC_ESC_PACKET_TYPES
#define ESC_PACKET_TYPE_VERSION_REQUEST 0
#define ESC_PACKET_TYPE_PWM_CMD 1
#define ESC_PACKET_TYPE_RPM_CMD 2
#define ESC_PACKET_TYPE_SOUND_CMD 3
#define ESC_PACKET_TYPE_STEP_CMD 4
#define ESC_PACKET_TYPE_LED_CMD 5
#define ESC_PACKET_TYPE_RESET_CMD 10
#define ESC_PACKET_TYPE_SET_ID_CMD 11
#define ESC_PACKET_TYPE_SET_DIR_CMD 12
#define ESC_PACKET_TYPE_CONFIG_BOARD_REQUEST 20
#define ESC_PACKET_TYPE_CONFIG_USER_REQUEST 21
#define ESC_PACKET_TYPE_CONFIG_UART_REQUEST 22
#define ESC_PACKET_TYPE_CONFIG_TUNE_REQUEST 23
#define ESC_PACKET_TYPE_VERSION_EXT_REQUEST 24
#define ESC_PACKET_TYPE_SET_FEEDBACK_MODE 50 //reserved for future use
#define ESC_PACKET_TYPE_EEPROM_WRITE_UNLOCK 70
#define ESC_PACKET_TYPE_EEPROM_READ_UNLOCK 71
#define ESC_PACKET_TYPE_EEPROM_WRITE 72
#define ESC_PACKET_TYPE_VERSION_RESPONSE 109
#define ESC_PACKET_TYPE_PARAMS 110
#define ESC_PACKET_TYPE_BOARD_CONFIG 111
#define ESC_PACKET_TYPE_USER_CONFIG 112
#define ESC_PACKET_TYPE_UART_CONFIG 113
#define ESC_PACKET_TYPE_TUNE_CONFIG 114
#define ESC_PACKET_TYPE_FB_RESPONSE 128
#define ESC_PACKET_TYPE_VERSION_EXT_RESPONSE 131
#endif

1
src/lib/parameters/px4params/srcparser.py
View File

@ -364,6 +364,7 @@ class SourceParser(object):
'S', 'A/%', '(m/s^2)^2', 'm/m', 'tan(rad)^2', '(m/s)^2', 'm/rad',
'm/s^3/sqrt(Hz)', 'm/s/sqrt(Hz)', 's/(1000*PWM)', '%m/s', 'min', 'us/C',
'N/(m/s)', 'Nm/rad', 'Nm/(rad/s)', 'Nm', 'N',
'RPM',
'normalized_thrust/s', 'normalized_thrust', 'norm', 'SD',''])
for group in self.GetParamGroups():
for param in group.GetParams():