forked from Archive/PX4-Autopilot
drivers: remove tap_esc
- it's not used anymore - it would need a refactoring to use mixer_module
This commit is contained in:
Beat Küng
Daniel Agar
parent
8986264feb
commit
7e33d03470
|
|
@ -144,11 +144,6 @@ then
|
|||
set OUTPUT_DEV /dev/uavcan/esc
|
||||
fi
|
||||
|
||||
if [ $OUTPUT_MODE = tap_esc ]
|
||||
then
|
||||
set OUTPUT_DEV /dev/tap_esc
|
||||
fi
|
||||
|
||||
if mixer load ${OUTPUT_DEV} ${MIXER_FILE}
|
||||
then
|
||||
echo "INFO [init] Mixer: ${MIXER_FILE} on ${OUTPUT_DEV}"
|
||||
|
|
|
|||
|
|
@ -223,7 +223,6 @@ class Graph(object):
|
|||
('listener', r'.*', None, r'^(id)$'),
|
||||
('logger', r'.*', None, r'^(topic|sub\.metadata|_polling_topic_meta)$'),
|
||||
|
||||
('tap_esc', r'.*', r'\b_control_topics\[[0-9]\]=([^,)]+)', r'^_control_topics\[i\]$'),
|
||||
('snapdragon_pwm_out', r'.*', r'\b_controls_topics\[[0-9]\]=([^,)]+)', r'^_controls_topics\[i\]$'),
|
||||
]
|
||||
special_cases_sub = [(a, re.compile(b), re.compile(c) if c is not None else None, re.compile(d))
|
||||
|
|
|
|||
|
|
@ -45,7 +45,6 @@ px4_add_board(
|
|||
pwm_out
|
||||
rc_input
|
||||
roboclaw
|
||||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -44,7 +44,6 @@ px4_add_board(
|
|||
pwm_out
|
||||
rc_input
|
||||
roboclaw
|
||||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
#tone_alarm
|
||||
|
|
|
|||
|
|
@ -49,7 +49,6 @@ px4_add_board(
|
|||
rc_input
|
||||
roboclaw
|
||||
safety_button
|
||||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -52,7 +52,6 @@ px4_add_board(
|
|||
rc_input
|
||||
roboclaw
|
||||
safety_button
|
||||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -51,7 +51,6 @@ px4_add_board(
|
|||
px4io
|
||||
roboclaw
|
||||
rpm
|
||||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -51,7 +51,6 @@ px4_add_board(
|
|||
px4io
|
||||
roboclaw
|
||||
rpm
|
||||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -50,7 +50,6 @@ px4_add_board(
|
|||
px4io
|
||||
roboclaw
|
||||
rpm
|
||||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -50,7 +50,6 @@ px4_add_board(
|
|||
px4io
|
||||
roboclaw
|
||||
rpm
|
||||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -50,7 +50,6 @@ px4_add_board(
|
|||
pwm_out
|
||||
px4io
|
||||
roboclaw
|
||||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -53,7 +53,6 @@ px4_add_board(
|
|||
roboclaw
|
||||
rpm
|
||||
safety_button
|
||||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -29,7 +29,6 @@ px4_add_board(
|
|||
pwm_out_sim
|
||||
pwm_out
|
||||
rc_input
|
||||
tap_esc
|
||||
#telemetry # all available telemetry drivers
|
||||
#uavcan
|
||||
MODULES
|
||||
|
|
|
|||
|
|
@ -44,7 +44,6 @@ px4_add_board(
|
|||
rc_input
|
||||
roboclaw
|
||||
safety_button
|
||||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
#tone_alarm
|
||||
|
|
|
|||
|
|
@ -48,7 +48,6 @@ px4_add_board(
|
|||
rc_input
|
||||
roboclaw
|
||||
safety_button
|
||||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -48,7 +48,6 @@ px4_add_board(
|
|||
rc_input
|
||||
roboclaw
|
||||
safety_button
|
||||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -49,7 +49,6 @@ px4_add_board(
|
|||
roboclaw
|
||||
rpm
|
||||
#safety_button TODO
|
||||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -49,7 +49,6 @@ px4_add_board(
|
|||
roboclaw
|
||||
rpm
|
||||
#safety_button TODO
|
||||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -48,7 +48,6 @@ px4_add_board(
|
|||
rc_input
|
||||
roboclaw
|
||||
rpm
|
||||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -48,7 +48,6 @@ px4_add_board(
|
|||
px4io
|
||||
roboclaw
|
||||
rpm
|
||||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -46,7 +46,6 @@ px4_add_board(
|
|||
pwm_out
|
||||
px4io
|
||||
roboclaw
|
||||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -46,7 +46,6 @@ px4_add_board(
|
|||
rc_input
|
||||
roboclaw
|
||||
safety_button
|
||||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
#test_ppm # NOT Portable YET
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -45,7 +45,6 @@ px4_add_board(
|
|||
rc_input
|
||||
roboclaw
|
||||
safety_button
|
||||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
#test_ppm # NOT Portable YET
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -46,7 +46,6 @@ px4_add_board(
|
|||
rc_input
|
||||
roboclaw
|
||||
safety_button
|
||||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
#test_ppm # NOT Portable YET
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -45,7 +45,6 @@ px4_add_board(
|
|||
rc_input
|
||||
roboclaw
|
||||
safety_button
|
||||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
#test_ppm # NOT Portable YET
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -45,7 +45,6 @@ px4_add_board(
|
|||
rc_input
|
||||
roboclaw
|
||||
safety_button
|
||||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
#test_ppm # NOT Portable YET
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -44,7 +44,6 @@ px4_add_board(
|
|||
rc_input
|
||||
roboclaw
|
||||
safety_button
|
||||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
tone_alarm
|
||||
# uavcan
|
||||
|
|
|
|||
|
|
@ -35,7 +35,6 @@ px4_add_board(
|
|||
#pwm_out_sim
|
||||
pwm_out
|
||||
rc_input
|
||||
#tap_esc
|
||||
#telemetry # all available telemetry drivers
|
||||
telemetry/frsky_telemetry
|
||||
#test_ppm
|
||||
|
|
|
|||
|
|
@ -60,7 +60,6 @@ px4_add_board(
|
|||
pwm_out
|
||||
px4io
|
||||
#roboclaw
|
||||
#tap_esc
|
||||
#telemetry # all available telemetry drivers
|
||||
#test_ppm
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -56,7 +56,6 @@ px4_add_board(
|
|||
pwm_out
|
||||
px4io
|
||||
#roboclaw
|
||||
#tap_esc
|
||||
#telemetry # all available telemetry drivers
|
||||
#test_ppm
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -57,7 +57,6 @@ px4_add_board(
|
|||
pwm_out
|
||||
px4io
|
||||
roboclaw
|
||||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -53,7 +53,6 @@ px4_add_board(
|
|||
rc_input
|
||||
roboclaw
|
||||
safety_button
|
||||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -50,7 +50,6 @@ px4_add_board(
|
|||
pwm_out
|
||||
px4io
|
||||
roboclaw
|
||||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -54,7 +54,6 @@ px4_add_board(
|
|||
roboclaw
|
||||
rpm
|
||||
safety_button
|
||||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -54,7 +54,6 @@ px4_add_board(
|
|||
#roboclaw
|
||||
rpm
|
||||
safety_button
|
||||
#tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -54,7 +54,6 @@ px4_add_board(
|
|||
roboclaw
|
||||
rpm
|
||||
safety_button
|
||||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -45,7 +45,6 @@ px4_add_board(
|
|||
rc_input
|
||||
roboclaw
|
||||
safety_button
|
||||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
tone_alarm
|
||||
uavcan
|
||||
|
|
|
|||
|
|
@ -48,7 +48,6 @@ px4_add_board(
|
|||
rc_input
|
||||
#roboclaw
|
||||
safety_button
|
||||
#tap_esc
|
||||
#telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -52,7 +52,6 @@ px4_add_board(
|
|||
roboclaw
|
||||
rpm
|
||||
safety_button
|
||||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -54,7 +54,6 @@ px4_add_board(
|
|||
#roboclaw
|
||||
#rpm
|
||||
safety_button
|
||||
#tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
#test_ppm
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -56,7 +56,6 @@ px4_add_board(
|
|||
#roboclaw
|
||||
#rpm
|
||||
safety_button
|
||||
#tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
#test_ppm
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -54,7 +54,6 @@ px4_add_board(
|
|||
roboclaw
|
||||
rpm
|
||||
safety_button
|
||||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
#test_ppm
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -52,7 +52,6 @@ px4_add_board(
|
|||
roboclaw
|
||||
rpm
|
||||
safety_button
|
||||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -54,7 +54,6 @@ px4_add_board(
|
|||
roboclaw
|
||||
rpm
|
||||
safety_button
|
||||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -54,7 +54,6 @@ px4_add_board(
|
|||
roboclaw
|
||||
rpm
|
||||
safety_button
|
||||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -53,7 +53,6 @@ px4_add_board(
|
|||
roboclaw
|
||||
rpm
|
||||
safety_button
|
||||
tap_esc
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
|
|
|
|||
|
|
@ -46,7 +46,6 @@ px4_add_board(
|
|||
pwm_out_sim
|
||||
pwm_out
|
||||
#roboclaw
|
||||
#tap_esc
|
||||
rc_input
|
||||
telemetry # all available telemetry drivers
|
||||
#test_ppm
|
||||
|
|
|
|||
|
|
@ -1,44 +0,0 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2016 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__tap_esc
|
||||
MAIN tap_esc
|
||||
COMPILE_FLAGS
|
||||
-Wno-missing-field-initializers
|
||||
SRCS
|
||||
tap_esc.cpp
|
||||
tap_esc_common.cpp
|
||||
DEPENDS
|
||||
mixer
|
||||
)
|
||||
|
||||
|
|
@ -1,248 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2016 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 <board_config.h>
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
/* At the moment the only known use is with a current sensor */
|
||||
#define ESC_HAVE_CURRENT_SENSOR
|
||||
|
||||
#define TAP_ESC_MAX_PACKET_LEN 20
|
||||
#define TAP_ESC_MAX_MOTOR_NUM 8
|
||||
|
||||
#define PACKET_HEAD 0xfe
|
||||
|
||||
/* ESC_POS maps the values stored in the channelMapTable to reorder the ESC's
|
||||
* id so that that match the mux setting, so that the ressonder's data
|
||||
* will be read.
|
||||
* The index on channelMapTable[p] p is the physical ESC
|
||||
* The value it is set to is the logical value from ESC_POS[p]
|
||||
* Phy Log
|
||||
* 0 0
|
||||
* 1 1
|
||||
* 2 4
|
||||
* 3 3
|
||||
* 4 2
|
||||
* 5 5
|
||||
* ....
|
||||
*
|
||||
*/
|
||||
|
||||
// Circular from back right in CCW direction
|
||||
#define ESC_POS {0, 1, 4, 3, 2, 5, 7, 8}
|
||||
// 0 is CW, 1 is CCW
|
||||
#define ESC_DIR {0, 1, 0, 1, 0, 1, 0, 1}
|
||||
|
||||
#define RPMMAX 1900
|
||||
#define RPMMIN 1200
|
||||
#define RPMSTOPPED (RPMMIN - 10)
|
||||
|
||||
|
||||
#define MAX_BOOT_TIME_MS (550) // Minimum time to wait after Power on before sending commands
|
||||
|
||||
#pragma pack(push,1)
|
||||
|
||||
/****** Run ***********/
|
||||
|
||||
#define RUN_CHANNEL_VALUE_MASK (uint16_t)0x07ff
|
||||
#define RUN_RED_LED_ON_MASK (uint16_t)0x0800
|
||||
#define RUN_GREEN_LED_ON_MASK (uint16_t)0x1000
|
||||
#define RUN_BLUE_LED_ON_MASK (uint16_t)0x2000
|
||||
#define RUN_LED_ON_MASK (uint16_t)0x3800
|
||||
#define RUN_FEEDBACK_ENABLE_MASK (uint16_t)0x4000
|
||||
#define RUN_REVERSE_MASK (uint16_t)0x8000
|
||||
|
||||
typedef struct {
|
||||
|
||||
uint16_t rpm_flags[TAP_ESC_MAX_MOTOR_NUM];
|
||||
} RunReq;
|
||||
|
||||
typedef struct {
|
||||
uint8_t channelID;
|
||||
uint8_t ESCStatus;
|
||||
int16_t speed; // -32767 - 32768
|
||||
#if defined(ESC_HAVE_VOLTAGE_SENSOR)
|
||||
uint16_t voltage; // 0.00 - 100.00 V
|
||||
#endif
|
||||
#if defined(ESC_HAVE_CURRENT_SENSOR)
|
||||
uint16_t current; // 0.0 - 200.0 A
|
||||
#endif
|
||||
#if defined(ESC_HAVE_TEMPERATURE_SENSOR)
|
||||
uint8_t temperature; // 0 - 256 degree celsius
|
||||
#endif
|
||||
} RunInfoRepsonse;
|
||||
/****** Run ***********/
|
||||
|
||||
/****** ConFigInfoBasic ***********/
|
||||
typedef struct {
|
||||
uint8_t maxChannelInUse;
|
||||
uint8_t channelMapTable[TAP_ESC_MAX_MOTOR_NUM];
|
||||
uint8_t monitorMsgType;
|
||||
uint8_t controlMode;
|
||||
uint16_t minChannelValue;
|
||||
uint16_t maxChannelValue;
|
||||
} ConfigInfoBasicRequest;
|
||||
|
||||
typedef struct {
|
||||
uint8_t channelID;
|
||||
ConfigInfoBasicRequest resp;
|
||||
} ConfigInfoBasicResponse;
|
||||
|
||||
#define ESC_MASK_MAP_CHANNEL 0x0f
|
||||
#define ESC_MASK_MAP_RUNNING_DIRECTION 0xf0
|
||||
/****** ConFigInfoBasicResponse ***********/
|
||||
|
||||
/****** InfoRequest ***********/
|
||||
typedef enum {
|
||||
REQUEST_INFO_BASIC = 0,
|
||||
REQUEST_INFO_FUll,
|
||||
REQUEST_INFO_RUN,
|
||||
REQUEST_INFO_STUDY,
|
||||
REQUEST_INFO_COMM,
|
||||
REQUEST_INFO_DEVICE,
|
||||
} InfoTypes;
|
||||
|
||||
typedef struct {
|
||||
uint8_t channelID;
|
||||
uint8_t requestInfoType;
|
||||
} InfoRequest;
|
||||
|
||||
/****** InfoRequest ***********/
|
||||
|
||||
typedef struct {
|
||||
uint8_t head;
|
||||
uint8_t len;
|
||||
uint8_t msg_id;
|
||||
union {
|
||||
InfoRequest reqInfo;
|
||||
ConfigInfoBasicRequest reqConfigInfoBasic;
|
||||
RunReq reqRun;
|
||||
ConfigInfoBasicResponse rspConfigInfoBasic;
|
||||
RunInfoRepsonse rspRunInfo;
|
||||
uint8_t bytes[100];
|
||||
} d;
|
||||
uint8_t crc_data;
|
||||
|
||||
} EscPacket;
|
||||
|
||||
#define UART_BUFFER_SIZE 128
|
||||
typedef struct {
|
||||
uint8_t head;
|
||||
uint8_t tail;
|
||||
uint8_t dat_cnt;
|
||||
uint8_t esc_feedback_buf[UART_BUFFER_SIZE];
|
||||
} ESC_UART_BUF;
|
||||
|
||||
#pragma pack(pop)
|
||||
/******************************************************************************************
|
||||
* ESCBUS_MSG_ID_RUN_INFO packet
|
||||
*
|
||||
* Monitor message of ESCs while motor is running
|
||||
*
|
||||
* channelID: assigned channel number
|
||||
*
|
||||
* ESCStatus: status of ESC
|
||||
* Num Health status
|
||||
* 0 HEALTHY
|
||||
* 1 WARNING_LOW_VOLTAGE
|
||||
* 2 WARNING_OVER_CURRENT
|
||||
* 3 WARNING_OVER_HEAT
|
||||
* 4 ERROR_MOTOR_LOW_SPEED_LOSE_STEP
|
||||
* 5 ERROR_MOTOR_STALL
|
||||
* 6 ERROR_HARDWARE
|
||||
* 7 ERROR_LOSE_PROPELLER
|
||||
* 8 ERROR_OVER_CURRENT
|
||||
*
|
||||
* speed: -32767 - 32767 rpm
|
||||
*
|
||||
* temperature: 0 - 256 celsius degree (if available)
|
||||
* voltage: 0.00 - 100.00 V (if available)
|
||||
* current: 0.0 - 200.0 A (if available)
|
||||
*/
|
||||
|
||||
typedef enum {
|
||||
ESC_STATUS_HEALTHY,
|
||||
ESC_STATUS_WARNING_LOW_VOLTAGE,
|
||||
ESC_STATUS_WARNING_OVER_HEAT,
|
||||
ESC_STATUS_ERROR_MOTOR_LOW_SPEED_LOSE_STEP,
|
||||
ESC_STATUS_ERROR_MOTOR_STALL,
|
||||
ESC_STATUS_ERROR_HARDWARE,
|
||||
ESC_STATUS_ERROR_LOSE_PROPELLER,
|
||||
ESC_STATUS_ERROR_OVER_CURRENT,
|
||||
ESC_STATUS_ERROR_MOTOR_HIGH_SPEED_LOSE_STEP,
|
||||
ESC_STATUS_ERROR_LOSE_CMD,
|
||||
} ESCBUS_ENUM_ESC_STATUS;
|
||||
|
||||
|
||||
typedef enum {
|
||||
// messages or command to ESC
|
||||
ESCBUS_MSG_ID_CONFIG_BASIC = 0,
|
||||
ESCBUS_MSG_ID_CONFIG_FULL,
|
||||
ESCBUS_MSG_ID_RUN,
|
||||
ESCBUS_MSG_ID_TUNE,
|
||||
ESCBUS_MSG_ID_DO_CMD,
|
||||
// messages from ESC
|
||||
ESCBUS_MSG_ID_REQUEST_INFO,
|
||||
ESCBUS_MSG_ID_CONFIG_INFO_BASIC, // simple configuration info for request from flight controller
|
||||
ESCBUS_MSG_ID_CONFIG_INFO_FULL,// full configuration info for request from host such as computer
|
||||
ESCBUS_MSG_ID_RUN_INFO,// feedback message in RUN mode
|
||||
ESCBUS_MSG_ID_STUDY_INFO, // studied parameters in STUDY mode
|
||||
ESCBUS_MSG_ID_COMM_INFO, // communication method info
|
||||
ESCBUS_MSG_ID_DEVICE_INFO,// ESC device info
|
||||
ESCBUS_MSG_ID_ASSIGNED_ID, // never touch ESCBUS_MSG_ID_MAX_NUM
|
||||
//boot loader used
|
||||
PROTO_OK = 0x10, // INSYNC/OK - 'ok' response
|
||||
PROTO_FAILED = 0x11, // INSYNC/FAILED - 'fail' response
|
||||
|
||||
ESCBUS_MSG_ID_BOOT_SYNC = 0x21, // boot loader used
|
||||
PROTO_GET_DEVICE = 0x22, // get device ID bytes
|
||||
PROTO_CHIP_ERASE = 0x23, // erase program area and reset program address
|
||||
PROTO_PROG_MULTI = 0x27, // write bytes at program address and increment
|
||||
PROTO_GET_CRC = 0x29, // compute & return a CRC
|
||||
PROTO_BOOT = 0x30, // boot the application
|
||||
PROTO_GET_SOFTWARE_VERSION = 0x40,
|
||||
ESCBUS_MSG_ID_MAX_NUM,
|
||||
}
|
||||
ESCBUS_ENUM_MESSAGE_ID;
|
||||
|
||||
typedef enum {
|
||||
HEAD,
|
||||
LEN,
|
||||
ID,
|
||||
DATA,
|
||||
CRC,
|
||||
|
||||
} PARSR_ESC_STATE;
|
||||
|
|
@ -1,783 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2018 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 <stdint.h>
|
||||
|
||||
#include <px4_platform_common/defines.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/tasks.h>
|
||||
#include <px4_platform_common/getopt.h>
|
||||
#include <px4_platform_common/posix.h>
|
||||
#include <errno.h>
|
||||
|
||||
#include <math.h> // NAN
|
||||
#include <cstring>
|
||||
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
#include <lib/cdev/CDev.hpp>
|
||||
#include <perf/perf_counter.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionInterval.hpp>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/test_motor.h>
|
||||
#include <uORB/topics/input_rc.h>
|
||||
#include <uORB/topics/esc_status.h>
|
||||
#include <uORB/topics/multirotor_motor_limits.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_mixer.h>
|
||||
#include <lib/mixer/MixerGroup.hpp>
|
||||
|
||||
#include "tap_esc_common.h"
|
||||
|
||||
#include "drv_tap_esc.h"
|
||||
|
||||
#if !defined(BOARD_TAP_ESC_MODE)
|
||||
# define BOARD_TAP_ESC_MODE 0
|
||||
#endif
|
||||
|
||||
#if !defined(DEVICE_ARGUMENT_MAX_LENGTH)
|
||||
# define DEVICE_ARGUMENT_MAX_LENGTH 32
|
||||
#endif
|
||||
|
||||
// uorb update rate for control groups in miliseconds
|
||||
#if !defined(TAP_ESC_CTRL_UORB_UPDATE_INTERVAL)
|
||||
# define TAP_ESC_CTRL_UORB_UPDATE_INTERVAL 2 // [ms] min: 2, max: 100
|
||||
#endif
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
/*
|
||||
* This driver connects to TAP ESCs via serial.
|
||||
*/
|
||||
class TAP_ESC : public cdev::CDev, public ModuleBase<TAP_ESC>, public ModuleParams
|
||||
{
|
||||
public:
|
||||
TAP_ESC(char const *const device, uint8_t channels_count);
|
||||
virtual ~TAP_ESC();
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static TAP_ESC *instantiate(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;
|
||||
|
||||
virtual int init();
|
||||
virtual int ioctl(cdev::file_t *filp, int cmd, unsigned long arg);
|
||||
void cycle();
|
||||
|
||||
private:
|
||||
char _device[DEVICE_ARGUMENT_MAX_LENGTH];
|
||||
int _uart_fd = -1;
|
||||
static const uint8_t _device_mux_map[TAP_ESC_MAX_MOTOR_NUM];
|
||||
static const uint8_t _device_dir_map[TAP_ESC_MAX_MOTOR_NUM];
|
||||
bool _is_armed = false;
|
||||
int _armed_sub = -1;
|
||||
int _test_motor_sub = -1;
|
||||
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
|
||||
orb_advert_t _outputs_pub = nullptr;
|
||||
actuator_outputs_s _outputs = {};
|
||||
actuator_armed_s _armed = {};
|
||||
|
||||
perf_counter_t _perf_control_latency;
|
||||
|
||||
int _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
|
||||
actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
|
||||
orb_id_t _control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
|
||||
px4_pollfd_struct_t _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
|
||||
unsigned _poll_fds_num = 0;
|
||||
|
||||
orb_advert_t _esc_feedback_pub = nullptr;
|
||||
orb_advert_t _to_mixer_status = nullptr; ///< mixer status flags
|
||||
esc_status_s _esc_feedback = {};
|
||||
uint8_t _channels_count = 0; ///< nnumber of ESC channels
|
||||
uint8_t _responding_esc = 0;
|
||||
|
||||
MixerGroup *_mixers = nullptr;
|
||||
uint32_t _groups_required = 0;
|
||||
uint32_t _groups_subscribed = 0;
|
||||
ESC_UART_BUF _uartbuf = {};
|
||||
EscPacket _packet = {};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamInt<px4::params::MC_AIRMODE>) _param_mc_airmode ///< multicopter air-mode
|
||||
)
|
||||
|
||||
void subscribe();
|
||||
void send_esc_outputs(const uint16_t *pwm, const uint8_t motor_cnt);
|
||||
static int control_callback_trampoline(uintptr_t handle,
|
||||
uint8_t control_group, uint8_t control_index, float &input);
|
||||
inline int control_callback(uint8_t control_group, uint8_t control_index, float &input);
|
||||
};
|
||||
|
||||
const uint8_t TAP_ESC::_device_mux_map[TAP_ESC_MAX_MOTOR_NUM] = ESC_POS;
|
||||
const uint8_t TAP_ESC::_device_dir_map[TAP_ESC_MAX_MOTOR_NUM] = ESC_DIR;
|
||||
|
||||
TAP_ESC::TAP_ESC(char const *const device, uint8_t channels_count):
|
||||
CDev(nullptr),
|
||||
ModuleParams(nullptr),
|
||||
_perf_control_latency(perf_alloc(PC_ELAPSED, "tap_esc control latency")),
|
||||
_channels_count(channels_count)
|
||||
{
|
||||
strncpy(_device, device, sizeof(_device));
|
||||
_device[sizeof(_device) - 1] = '\0'; // Fix in case of overflow
|
||||
|
||||
_control_topics[0] = ORB_ID(actuator_controls_0);
|
||||
_control_topics[1] = ORB_ID(actuator_controls_1);
|
||||
_control_topics[2] = ORB_ID(actuator_controls_2);
|
||||
_control_topics[3] = ORB_ID(actuator_controls_3);
|
||||
memset(_controls, 0, sizeof(_controls));
|
||||
memset(_poll_fds, 0, sizeof(_poll_fds));
|
||||
|
||||
for (int i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; ++i) {
|
||||
_control_subs[i] = -1;
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {
|
||||
_outputs.output[i] = NAN;
|
||||
}
|
||||
|
||||
_outputs.noutputs = 0;
|
||||
}
|
||||
|
||||
TAP_ESC::~TAP_ESC()
|
||||
{
|
||||
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
|
||||
if (_control_subs[i] >= 0) {
|
||||
orb_unsubscribe(_control_subs[i]);
|
||||
_control_subs[i] = -1;
|
||||
}
|
||||
}
|
||||
|
||||
orb_unsubscribe(_armed_sub);
|
||||
orb_unsubscribe(_test_motor_sub);
|
||||
|
||||
orb_unadvertise(_outputs_pub);
|
||||
orb_unadvertise(_esc_feedback_pub);
|
||||
orb_unadvertise(_to_mixer_status);
|
||||
|
||||
tap_esc_common::deinitialise_uart(_uart_fd);
|
||||
|
||||
PX4_INFO("stopping");
|
||||
|
||||
perf_free(_perf_control_latency);
|
||||
}
|
||||
|
||||
/** @see ModuleBase */
|
||||
TAP_ESC *TAP_ESC::instantiate(int argc, char *argv[])
|
||||
{
|
||||
/* Parse arguments */
|
||||
const char *device = nullptr;
|
||||
uint8_t channels_count = 0;
|
||||
|
||||
int ch;
|
||||
int myoptind = 1;
|
||||
const char *myoptarg = nullptr;
|
||||
|
||||
if (argc < 2) {
|
||||
print_usage("not enough arguments");
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
while ((ch = px4_getopt(argc, argv, "d:n:", &myoptind, &myoptarg)) != EOF) {
|
||||
switch (ch) {
|
||||
case 'd':
|
||||
device = myoptarg;
|
||||
break;
|
||||
|
||||
case 'n':
|
||||
channels_count = atoi(myoptarg);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/* Sanity check on arguments */
|
||||
if (channels_count == 0) {
|
||||
print_usage("Channel count is invalid (0)");
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
if (device == nullptr || strlen(device) == 0) {
|
||||
print_usage("no device specified");
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
TAP_ESC *tap_esc = new TAP_ESC(device, channels_count);
|
||||
|
||||
if (tap_esc == nullptr) {
|
||||
PX4_ERR("failed to instantiate module");
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
if (tap_esc->init() != 0) {
|
||||
PX4_ERR("failed to initialize module");
|
||||
delete tap_esc;
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
return tap_esc;
|
||||
}
|
||||
|
||||
/** @see ModuleBase */
|
||||
int TAP_ESC::custom_command(int argc, char *argv[])
|
||||
{
|
||||
return print_usage("unknown command");
|
||||
}
|
||||
|
||||
int TAP_ESC::init()
|
||||
{
|
||||
int ret;
|
||||
|
||||
ret = tap_esc_common::initialise_uart(_device, _uart_fd);
|
||||
|
||||
if (ret != 0) {
|
||||
PX4_ERR("failed to initialise UART.");
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* Respect boot time required by the ESC FW */
|
||||
hrt_abstime uptime_us = hrt_absolute_time();
|
||||
|
||||
if (uptime_us < MAX_BOOT_TIME_MS * 1000) {
|
||||
usleep((MAX_BOOT_TIME_MS * 1000) - uptime_us);
|
||||
}
|
||||
|
||||
/* Issue Basic Config */
|
||||
EscPacket packet = {PACKET_HEAD, sizeof(ConfigInfoBasicRequest), ESCBUS_MSG_ID_CONFIG_BASIC};
|
||||
ConfigInfoBasicRequest &config = packet.d.reqConfigInfoBasic;
|
||||
memset(&config, 0, sizeof(ConfigInfoBasicRequest));
|
||||
config.maxChannelInUse = _channels_count;
|
||||
/* Enable closed-loop control if supported by the board */
|
||||
config.controlMode = BOARD_TAP_ESC_MODE;
|
||||
|
||||
/* Asign the id's to the ESCs to match the mux */
|
||||
for (uint8_t phy_chan_index = 0; phy_chan_index < _channels_count; phy_chan_index++) {
|
||||
config.channelMapTable[phy_chan_index] = _device_mux_map[phy_chan_index] &
|
||||
ESC_MASK_MAP_CHANNEL;
|
||||
config.channelMapTable[phy_chan_index] |= (_device_dir_map[phy_chan_index] << 4) &
|
||||
ESC_MASK_MAP_RUNNING_DIRECTION;
|
||||
}
|
||||
|
||||
config.maxChannelValue = RPMMAX;
|
||||
config.minChannelValue = RPMMIN;
|
||||
|
||||
ret = tap_esc_common::send_packet(_uart_fd, packet, 0);
|
||||
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* To Unlock the ESC from the Power up state we need to issue 10
|
||||
* ESCBUS_MSG_ID_RUN request with all the values 0;
|
||||
*/
|
||||
EscPacket unlock_packet = {PACKET_HEAD, _channels_count, ESCBUS_MSG_ID_RUN};
|
||||
unlock_packet.len *= sizeof(unlock_packet.d.reqRun.rpm_flags[0]);
|
||||
memset(unlock_packet.d.bytes, 0, sizeof(unlock_packet.d.bytes));
|
||||
|
||||
int unlock_times = 10;
|
||||
|
||||
while (unlock_times--) {
|
||||
|
||||
tap_esc_common::send_packet(_uart_fd, unlock_packet, -1);
|
||||
|
||||
/* Min Packet to Packet time is 1 Ms so use 2 */
|
||||
usleep(2000);
|
||||
}
|
||||
|
||||
/* do regular cdev init */
|
||||
ret = CDev::init();
|
||||
|
||||
/* advertise the mixed control outputs, insist on the first group output */
|
||||
_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs);
|
||||
_esc_feedback_pub = orb_advertise(ORB_ID(esc_status), &_esc_feedback);
|
||||
multirotor_motor_limits_s multirotor_motor_limits = {};
|
||||
_to_mixer_status = orb_advertise(ORB_ID(multirotor_motor_limits), &multirotor_motor_limits);
|
||||
|
||||
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
|
||||
_test_motor_sub = orb_subscribe(ORB_ID(test_motor));
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void TAP_ESC::subscribe()
|
||||
{
|
||||
/* subscribe/unsubscribe to required actuator control groups */
|
||||
uint32_t sub_groups = _groups_required & ~_groups_subscribed;
|
||||
uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
|
||||
_poll_fds_num = 0;
|
||||
|
||||
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
|
||||
if (sub_groups & (1 << i)) {
|
||||
PX4_DEBUG("subscribe to actuator_controls_%d", i);
|
||||
_control_subs[i] = orb_subscribe(_control_topics[i]);
|
||||
}
|
||||
|
||||
if (unsub_groups & (1 << i)) {
|
||||
PX4_DEBUG("unsubscribe from actuator_controls_%d", i);
|
||||
orb_unsubscribe(_control_subs[i]);
|
||||
_control_subs[i] = -1;
|
||||
}
|
||||
|
||||
if (_control_subs[i] >= 0) {
|
||||
_poll_fds[_poll_fds_num].fd = _control_subs[i];
|
||||
_poll_fds[_poll_fds_num].events = POLLIN;
|
||||
_poll_fds_num++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void TAP_ESC::send_esc_outputs(const uint16_t *pwm, const uint8_t motor_cnt)
|
||||
{
|
||||
uint16_t rpm[TAP_ESC_MAX_MOTOR_NUM] = {};
|
||||
|
||||
for (uint8_t i = 0; i < motor_cnt; i++) {
|
||||
rpm[i] = pwm[i];
|
||||
|
||||
if (rpm[i] > RPMMAX) {
|
||||
rpm[i] = RPMMAX;
|
||||
|
||||
} else if (rpm[i] < RPMSTOPPED) {
|
||||
rpm[i] = RPMSTOPPED;
|
||||
}
|
||||
}
|
||||
|
||||
rpm[_responding_esc] |= (RUN_FEEDBACK_ENABLE_MASK | RUN_BLUE_LED_ON_MASK);
|
||||
|
||||
EscPacket packet = {PACKET_HEAD, _channels_count, ESCBUS_MSG_ID_RUN};
|
||||
packet.len *= sizeof(packet.d.reqRun.rpm_flags[0]);
|
||||
|
||||
for (uint8_t i = 0; i < _channels_count; i++) {
|
||||
packet.d.reqRun.rpm_flags[i] = rpm[i];
|
||||
}
|
||||
|
||||
int ret = tap_esc_common::send_packet(_uart_fd, packet, _responding_esc);
|
||||
|
||||
if (++_responding_esc >= _channels_count) {
|
||||
_responding_esc = 0;
|
||||
}
|
||||
|
||||
if (ret < 1) {
|
||||
PX4_WARN("TX ERROR: ret: %d, errno: %d", ret, errno);
|
||||
}
|
||||
}
|
||||
|
||||
void TAP_ESC::cycle()
|
||||
{
|
||||
if (_groups_subscribed != _groups_required) {
|
||||
subscribe();
|
||||
_groups_subscribed = _groups_required;
|
||||
|
||||
/* Set uorb update rate */
|
||||
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
|
||||
if (_control_subs[i] >= 0) {
|
||||
orb_set_interval(_control_subs[i], TAP_ESC_CTRL_UORB_UPDATE_INTERVAL);
|
||||
PX4_DEBUG("New actuator update interval: %ums", TAP_ESC_CTRL_UORB_UPDATE_INTERVAL);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (_mixers) {
|
||||
_mixers->set_airmode((Mixer::Airmode)_param_mc_airmode.get());
|
||||
}
|
||||
|
||||
/* check if anything updated */
|
||||
int ret = px4_poll(_poll_fds, _poll_fds_num, 5);
|
||||
|
||||
/* this would be bad... */
|
||||
if (ret < 0) {
|
||||
PX4_ERR("poll error %d", errno);
|
||||
|
||||
} else { /* update even in the case of a timeout, to check for test_motor commands */
|
||||
|
||||
/* get controls for required topics */
|
||||
unsigned poll_id = 0;
|
||||
|
||||
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
|
||||
if (_control_subs[i] >= 0) {
|
||||
if (_poll_fds[poll_id].revents & POLLIN) {
|
||||
orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
|
||||
|
||||
}
|
||||
|
||||
poll_id++;
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t num_outputs = _channels_count;
|
||||
|
||||
/* can we mix? */
|
||||
if (_is_armed && _mixers != nullptr) {
|
||||
|
||||
/* do mixing */
|
||||
num_outputs = _mixers->mix(&_outputs.output[0], num_outputs);
|
||||
_outputs.noutputs = num_outputs;
|
||||
|
||||
/* publish mixer status */
|
||||
multirotor_motor_limits_s multirotor_motor_limits = {};
|
||||
multirotor_motor_limits.saturation_status = _mixers->get_saturation_status();
|
||||
|
||||
orb_publish(ORB_ID(multirotor_motor_limits), _to_mixer_status, &multirotor_motor_limits);
|
||||
|
||||
/* disable unused ports by setting their output to NaN */
|
||||
for (size_t i = num_outputs; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {
|
||||
_outputs.output[i] = NAN;
|
||||
}
|
||||
|
||||
/* iterate actuators */
|
||||
for (unsigned i = 0; i < num_outputs; i++) {
|
||||
/* last resort: catch NaN, INF and out-of-band errors */
|
||||
if (i < _outputs.noutputs && PX4_ISFINITE(_outputs.output[i])
|
||||
&& !_armed.lockdown && !_armed.manual_lockdown) {
|
||||
/* scale for PWM output 1200 - 1900us */
|
||||
_outputs.output[i] = (RPMMAX + RPMMIN) / 2 + ((RPMMAX - RPMMIN) / 2) * _outputs.output[i];
|
||||
math::constrain(_outputs.output[i], (float)RPMMIN, (float)RPMMAX);
|
||||
|
||||
} else {
|
||||
/*
|
||||
* Value is NaN, INF, or we are in lockdown - stop the motor.
|
||||
* This will be clearly visible on the servo status and will limit the risk of accidentally
|
||||
* spinning motors. It would be deadly in flight.
|
||||
*/
|
||||
_outputs.output[i] = RPMSTOPPED;
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
|
||||
_outputs.noutputs = num_outputs;
|
||||
_outputs.timestamp = hrt_absolute_time();
|
||||
|
||||
/* check for motor test commands */
|
||||
bool test_motor_updated;
|
||||
orb_check(_test_motor_sub, &test_motor_updated);
|
||||
|
||||
if (test_motor_updated) {
|
||||
struct test_motor_s test_motor;
|
||||
orb_copy(ORB_ID(test_motor), _test_motor_sub, &test_motor);
|
||||
|
||||
if (test_motor.action == test_motor_s::ACTION_STOP) {
|
||||
_outputs.output[test_motor.motor_number] = RPMSTOPPED;
|
||||
|
||||
} else {
|
||||
_outputs.output[test_motor.motor_number] = RPMSTOPPED + ((RPMMAX - RPMSTOPPED) * test_motor.value);
|
||||
}
|
||||
|
||||
PX4_INFO("setting motor %i to %.1lf", test_motor.motor_number,
|
||||
(double)_outputs.output[test_motor.motor_number]);
|
||||
}
|
||||
|
||||
/* set the invalid values to the minimum */
|
||||
for (unsigned i = 0; i < num_outputs; i++) {
|
||||
if (!PX4_ISFINITE(_outputs.output[i])) {
|
||||
_outputs.output[i] = RPMSTOPPED;
|
||||
}
|
||||
}
|
||||
|
||||
/* disable unused ports by setting their output to NaN */
|
||||
for (size_t i = num_outputs; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {
|
||||
_outputs.output[i] = NAN;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
uint16_t motor_out[TAP_ESC_MAX_MOTOR_NUM];
|
||||
|
||||
// We need to remap from the system default to what PX4's normal
|
||||
// scheme is
|
||||
switch (num_outputs) {
|
||||
case 4:
|
||||
motor_out[0] = (uint16_t)_outputs.output[2];
|
||||
motor_out[1] = (uint16_t)_outputs.output[1];
|
||||
motor_out[2] = (uint16_t)_outputs.output[0];
|
||||
motor_out[3] = (uint16_t)_outputs.output[3];
|
||||
break;
|
||||
|
||||
case 6:
|
||||
motor_out[0] = (uint16_t)_outputs.output[3];
|
||||
motor_out[1] = (uint16_t)_outputs.output[0];
|
||||
motor_out[2] = (uint16_t)_outputs.output[4];
|
||||
motor_out[3] = (uint16_t)_outputs.output[2];
|
||||
motor_out[4] = (uint16_t)_outputs.output[1];
|
||||
motor_out[5] = (uint16_t)_outputs.output[5];
|
||||
break;
|
||||
|
||||
default:
|
||||
|
||||
// Use the system defaults
|
||||
for (uint8_t i = 0; i < num_outputs; ++i) {
|
||||
motor_out[i] = (uint16_t)_outputs.output[i];
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
// Set remaining motors to RPMSTOPPED to be on the safe side
|
||||
for (uint8_t i = num_outputs; i < TAP_ESC_MAX_MOTOR_NUM; ++i) {
|
||||
motor_out[i] = RPMSTOPPED;
|
||||
}
|
||||
|
||||
_outputs.timestamp = hrt_absolute_time();
|
||||
|
||||
send_esc_outputs(motor_out, num_outputs);
|
||||
tap_esc_common::read_data_from_uart(_uart_fd, &_uartbuf);
|
||||
|
||||
if (!tap_esc_common::parse_tap_esc_feedback(&_uartbuf, &_packet)) {
|
||||
if (_packet.msg_id == ESCBUS_MSG_ID_RUN_INFO) {
|
||||
RunInfoRepsonse &feed_back_data = _packet.d.rspRunInfo;
|
||||
|
||||
if (feed_back_data.channelID < esc_status_s::CONNECTED_ESC_MAX) {
|
||||
_esc_feedback.esc[feed_back_data.channelID].esc_rpm = feed_back_data.speed;
|
||||
_esc_feedback.esc[feed_back_data.channelID].esc_state = feed_back_data.ESCStatus;
|
||||
_esc_feedback.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_SERIAL;
|
||||
_esc_feedback.counter++;
|
||||
_esc_feedback.esc_count = num_outputs;
|
||||
_esc_feedback.esc_online_flags = (1 << num_outputs) - 1;
|
||||
_esc_feedback.esc_armed_flags = (1 << num_outputs) - 1;
|
||||
|
||||
_esc_feedback.timestamp = hrt_absolute_time();
|
||||
|
||||
orb_publish(ORB_ID(esc_status), _esc_feedback_pub, &_esc_feedback);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* and publish for anyone that cares to see */
|
||||
orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs);
|
||||
|
||||
// use first valid timestamp_sample for latency tracking
|
||||
for (int i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
|
||||
const bool required = _groups_required & (1 << i);
|
||||
const hrt_abstime ×tamp_sample = _controls[i].timestamp_sample;
|
||||
|
||||
if (required && (timestamp_sample > 0)) {
|
||||
perf_set_elapsed(_perf_control_latency, _outputs.timestamp - timestamp_sample);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
bool updated;
|
||||
|
||||
orb_check(_armed_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
|
||||
|
||||
if (_is_armed != _armed.armed) {
|
||||
/* reset all outputs */
|
||||
for (size_t i = 0; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {
|
||||
_outputs.output[i] = NAN;
|
||||
}
|
||||
}
|
||||
|
||||
_is_armed = _armed.armed;
|
||||
|
||||
}
|
||||
|
||||
// check for parameter updates
|
||||
if (_parameter_update_sub.updated()) {
|
||||
// clear update
|
||||
parameter_update_s pupdate;
|
||||
_parameter_update_sub.copy(&pupdate);
|
||||
|
||||
// update parameters from storage
|
||||
updateParams();
|
||||
}
|
||||
}
|
||||
|
||||
int TAP_ESC::control_callback_trampoline(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input)
|
||||
{
|
||||
TAP_ESC *obj = (TAP_ESC *)handle;
|
||||
return obj->control_callback(control_group, control_index, input);
|
||||
}
|
||||
|
||||
int TAP_ESC::control_callback(uint8_t control_group, uint8_t control_index, float &input)
|
||||
{
|
||||
input = _controls[control_group].control[control_index];
|
||||
|
||||
/* limit control input */
|
||||
input = math::constrain(input, -1.f, 1.f);
|
||||
|
||||
/* throttle not arming - mark throttle input as invalid */
|
||||
if (_armed.prearmed && !_armed.armed) {
|
||||
if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE ||
|
||||
control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
|
||||
control_index == actuator_controls_s::INDEX_THROTTLE) {
|
||||
/* set the throttle to an invalid value */
|
||||
input = NAN;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int TAP_ESC::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
|
||||
{
|
||||
int ret = OK;
|
||||
|
||||
switch (cmd) {
|
||||
|
||||
case MIXERIOCRESET:
|
||||
if (_mixers != nullptr) {
|
||||
delete _mixers;
|
||||
_mixers = nullptr;
|
||||
_groups_required = 0;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case MIXERIOCLOADBUF: {
|
||||
const char *buf = (const char *)arg;
|
||||
unsigned buflen = strlen(buf);
|
||||
|
||||
if (_mixers == nullptr) {
|
||||
_mixers = new MixerGroup();
|
||||
}
|
||||
|
||||
if (_mixers == nullptr) {
|
||||
_groups_required = 0;
|
||||
ret = -ENOMEM;
|
||||
|
||||
} else {
|
||||
ret = _mixers->load_from_buf(control_callback_trampoline, (uintptr_t)this, buf, buflen);
|
||||
|
||||
if (ret != 0) {
|
||||
PX4_DEBUG("mixer load failed with %d", ret);
|
||||
delete _mixers;
|
||||
_mixers = nullptr;
|
||||
_groups_required = 0;
|
||||
ret = -EINVAL;
|
||||
|
||||
} else {
|
||||
|
||||
_mixers->groups_required(_groups_required);
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
ret = -ENOTTY;
|
||||
break;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
/** @see ModuleBase */
|
||||
void TAP_ESC::run()
|
||||
{
|
||||
// Main loop
|
||||
while (!should_exit()) {
|
||||
cycle();
|
||||
}
|
||||
}
|
||||
|
||||
/** @see ModuleBase */
|
||||
int TAP_ESC::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
/* start the task */
|
||||
_task_id = px4_task_spawn_cmd("tap_esc",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_ACTUATOR_OUTPUTS,
|
||||
1180,
|
||||
(px4_main_t)&run_trampoline,
|
||||
argv);
|
||||
|
||||
if (_task_id < 0) {
|
||||
PX4_ERR("task start failed");
|
||||
_task_id = -1;
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
// wait until task is up & running
|
||||
if (wait_until_running() < 0) {
|
||||
_task_id = -1;
|
||||
return -1;
|
||||
}
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
/** @see ModuleBase */
|
||||
int TAP_ESC::print_usage(const char *reason)
|
||||
{
|
||||
if (reason) {
|
||||
PX4_WARN("%s\n", reason);
|
||||
}
|
||||
|
||||
PRINT_MODULE_DESCRIPTION(
|
||||
R"DESCR_STR(
|
||||
### Description
|
||||
This module controls the TAP_ESC hardware via UART. It listens on the
|
||||
actuator_controls topics, does the mixing and writes the PWM outputs.
|
||||
|
||||
### Implementation
|
||||
Currently the module is implementd as a threaded version only, meaning that it
|
||||
runs in its own thread instead of on the work queue.
|
||||
|
||||
### Example
|
||||
The module is typically started with:
|
||||
tap_esc start -d /dev/ttyS2 -n <1-8>
|
||||
|
||||
)DESCR_STR");
|
||||
|
||||
PRINT_MODULE_USAGE_NAME("tap_esc", "driver");
|
||||
|
||||
PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the task");
|
||||
PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "<device>", "Device used to talk to ESCs", true);
|
||||
PRINT_MODULE_USAGE_PARAM_INT('n', 4, 0, 8, "Number of ESCs", true);
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int tap_esc_main(int argc, char *argv[])
|
||||
{
|
||||
return TAP_ESC::main(argc, argv);
|
||||
}
|
||||
|
|
@ -1,307 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2017 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/*
|
||||
* tap_esc_common.cpp
|
||||
*
|
||||
*/
|
||||
|
||||
#include "tap_esc_common.h"
|
||||
|
||||
#include <fcntl.h>
|
||||
#include <termios.h>
|
||||
|
||||
#include <systemlib/px4_macros.h> // arraySize
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
|
||||
#ifndef B250000
|
||||
#define B250000 250000
|
||||
#endif
|
||||
|
||||
namespace tap_esc_common
|
||||
{
|
||||
static uint8_t crc8_esc(uint8_t *p, uint8_t len);
|
||||
static uint8_t crc_packet(EscPacket &p);
|
||||
|
||||
void select_responder(uint8_t sel)
|
||||
{
|
||||
#if defined(GPIO_S0)
|
||||
px4_arch_gpiowrite(GPIO_S0, sel & 1);
|
||||
px4_arch_gpiowrite(GPIO_S1, sel & 2);
|
||||
px4_arch_gpiowrite(GPIO_S2, sel & 4);
|
||||
#endif
|
||||
}
|
||||
|
||||
int initialise_uart(const char *const device, int &uart_fd)
|
||||
{
|
||||
// open uart
|
||||
uart_fd = open(device, O_RDWR | O_NOCTTY | O_NONBLOCK);
|
||||
int termios_state = -1;
|
||||
|
||||
if (uart_fd < 0) {
|
||||
PX4_ERR("failed to open uart device!");
|
||||
return -1;
|
||||
}
|
||||
|
||||
// set baud rate
|
||||
int speed = B250000;
|
||||
struct termios uart_config;
|
||||
tcgetattr(uart_fd, &uart_config);
|
||||
|
||||
// clear ONLCR flag (which appends a CR for every LF)
|
||||
uart_config.c_oflag &= ~ONLCR;
|
||||
|
||||
// set baud rate
|
||||
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
|
||||
PX4_ERR("failed to set baudrate for %s: %d\n", device, termios_state);
|
||||
close(uart_fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
if ((termios_state = tcsetattr(uart_fd, TCSANOW, &uart_config)) < 0) {
|
||||
PX4_ERR("tcsetattr failed for %s\n", device);
|
||||
close(uart_fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// setup output flow control
|
||||
if (enable_flow_control(uart_fd, false)) {
|
||||
PX4_WARN("hardware flow disable failed");
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int deinitialise_uart(int &uart_fd)
|
||||
{
|
||||
int ret = close(uart_fd);
|
||||
|
||||
if (ret == 0) {
|
||||
uart_fd = -1;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int enable_flow_control(int uart_fd, bool enabled)
|
||||
{
|
||||
struct termios uart_config;
|
||||
|
||||
int ret = tcgetattr(uart_fd, &uart_config);
|
||||
|
||||
if (ret != 0) {
|
||||
PX4_ERR("error getting uart configuration");
|
||||
return ret;
|
||||
}
|
||||
|
||||
if (enabled) {
|
||||
uart_config.c_cflag |= CRTSCTS;
|
||||
|
||||
} else {
|
||||
uart_config.c_cflag &= ~CRTSCTS;
|
||||
}
|
||||
|
||||
return tcsetattr(uart_fd, TCSANOW, &uart_config);
|
||||
}
|
||||
|
||||
int send_packet(int uart_fd, EscPacket &packet, int responder)
|
||||
{
|
||||
if (responder >= 0) {
|
||||
select_responder(responder);
|
||||
}
|
||||
|
||||
int packet_len = crc_packet(packet);
|
||||
int ret = ::write(uart_fd, &packet.head, packet_len);
|
||||
|
||||
if (ret != packet_len) {
|
||||
PX4_WARN("TX ERROR: ret: %d, errno: %d", ret, errno);
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int read_data_from_uart(int uart_fd, ESC_UART_BUF *const uart_buf)
|
||||
{
|
||||
uint8_t tmp_serial_buf[UART_BUFFER_SIZE];
|
||||
|
||||
int len =::read(uart_fd, tmp_serial_buf, arraySize(tmp_serial_buf));
|
||||
|
||||
if (len > 0 && (uart_buf->dat_cnt + len < UART_BUFFER_SIZE)) {
|
||||
for (int i = 0; i < len; i++) {
|
||||
uart_buf->esc_feedback_buf[uart_buf->tail++] = tmp_serial_buf[i];
|
||||
uart_buf->dat_cnt++;
|
||||
|
||||
if (uart_buf->tail >= UART_BUFFER_SIZE) {
|
||||
uart_buf->tail = 0;
|
||||
}
|
||||
}
|
||||
|
||||
} else if (len < 0) {
|
||||
return len;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int parse_tap_esc_feedback(ESC_UART_BUF *const serial_buf, EscPacket *const packetdata)
|
||||
{
|
||||
static PARSR_ESC_STATE state = HEAD;
|
||||
static uint8_t data_index = 0;
|
||||
static uint8_t crc_data_cal;
|
||||
|
||||
if (serial_buf->dat_cnt > 0) {
|
||||
int count = serial_buf->dat_cnt;
|
||||
|
||||
for (int i = 0; i < count; i++) {
|
||||
switch (state) {
|
||||
case HEAD:
|
||||
if (serial_buf->esc_feedback_buf[serial_buf->head] == PACKET_HEAD) {
|
||||
packetdata->head = PACKET_HEAD; //just_keep the format
|
||||
state = LEN;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case LEN:
|
||||
if (serial_buf->esc_feedback_buf[serial_buf->head] < sizeof(packetdata->d)) {
|
||||
packetdata->len = serial_buf->esc_feedback_buf[serial_buf->head];
|
||||
state = ID;
|
||||
|
||||
} else {
|
||||
state = HEAD;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case ID:
|
||||
if (serial_buf->esc_feedback_buf[serial_buf->head] < ESCBUS_MSG_ID_MAX_NUM) {
|
||||
packetdata->msg_id = serial_buf->esc_feedback_buf[serial_buf->head];
|
||||
data_index = 0;
|
||||
state = DATA;
|
||||
|
||||
} else {
|
||||
state = HEAD;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case DATA:
|
||||
packetdata->d.bytes[data_index++] = serial_buf->esc_feedback_buf[serial_buf->head];
|
||||
|
||||
if (data_index >= packetdata->len) {
|
||||
|
||||
crc_data_cal = crc8_esc((uint8_t *)(&packetdata->len), packetdata->len + 2);
|
||||
state = CRC;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case CRC:
|
||||
if (crc_data_cal == serial_buf->esc_feedback_buf[serial_buf->head]) {
|
||||
packetdata->crc_data = serial_buf->esc_feedback_buf[serial_buf->head];
|
||||
|
||||
if (++serial_buf->head >= UART_BUFFER_SIZE) {
|
||||
serial_buf->head = 0;
|
||||
}
|
||||
|
||||
serial_buf->dat_cnt--;
|
||||
state = HEAD;
|
||||
return 0;
|
||||
}
|
||||
|
||||
state = HEAD;
|
||||
break;
|
||||
|
||||
default:
|
||||
state = HEAD;
|
||||
break;
|
||||
|
||||
}
|
||||
|
||||
if (++serial_buf->head >= UART_BUFFER_SIZE) {
|
||||
serial_buf->head = 0;
|
||||
}
|
||||
|
||||
serial_buf->dat_cnt--;
|
||||
}
|
||||
}
|
||||
|
||||
return -1;
|
||||
}
|
||||
|
||||
static uint8_t crc8_esc(uint8_t *p, uint8_t len)
|
||||
{
|
||||
uint8_t crc = 0;
|
||||
|
||||
for (uint8_t i = 0; i < len; i++) {
|
||||
crc = crc_table[crc^*p++];
|
||||
}
|
||||
|
||||
return crc;
|
||||
}
|
||||
|
||||
static uint8_t crc_packet(EscPacket &p)
|
||||
{
|
||||
/* Calculate the crc over Len,ID,data */
|
||||
p.d.bytes[p.len] = crc8_esc(&p.len, p.len + 2);
|
||||
return p.len + offsetof(EscPacket, d) + 1;
|
||||
}
|
||||
|
||||
|
||||
const uint8_t crc_table[256] = {
|
||||
0x00, 0xE7, 0x29, 0xCE, 0x52, 0xB5, 0x7B, 0x9C, 0xA4, 0x43, 0x8D, 0x6A,
|
||||
0xF6, 0x11, 0xDF, 0x38, 0xAF, 0x48, 0x86, 0x61, 0xFD, 0x1A, 0xD4, 0x33,
|
||||
0x0B, 0xEC, 0x22, 0xC5, 0x59, 0xBE, 0x70, 0x97, 0xB9, 0x5E, 0x90, 0x77,
|
||||
0xEB, 0x0C, 0xC2, 0x25, 0x1D, 0xFA, 0x34, 0xD3, 0x4F, 0xA8, 0x66, 0x81,
|
||||
0x16, 0xF1, 0x3F, 0xD8, 0x44, 0xA3, 0x6D, 0x8A, 0xB2, 0x55, 0x9B, 0x7C,
|
||||
0xE0, 0x07, 0xC9, 0x2E, 0x95, 0x72, 0xBC, 0x5B, 0xC7, 0x20, 0xEE, 0x09,
|
||||
0x31, 0xD6, 0x18, 0xFF, 0x63, 0x84, 0x4A, 0xAD, 0x3A, 0xDD, 0x13, 0xF4,
|
||||
0x68, 0x8F, 0x41, 0xA6, 0x9E, 0x79, 0xB7, 0x50, 0xCC, 0x2B, 0xE5, 0x02,
|
||||
0x2C, 0xCB, 0x05, 0xE2, 0x7E, 0x99, 0x57, 0xB0, 0x88, 0x6F, 0xA1, 0x46,
|
||||
0xDA, 0x3D, 0xF3, 0x14, 0x83, 0x64, 0xAA, 0x4D, 0xD1, 0x36, 0xF8, 0x1F,
|
||||
0x27, 0xC0, 0x0E, 0xE9, 0x75, 0x92, 0x5C, 0xBB, 0xCD, 0x2A, 0xE4, 0x03,
|
||||
0x9F, 0x78, 0xB6, 0x51, 0x69, 0x8E, 0x40, 0xA7, 0x3B, 0xDC, 0x12, 0xF5,
|
||||
0x62, 0x85, 0x4B, 0xAC, 0x30, 0xD7, 0x19, 0xFE, 0xC6, 0x21, 0xEF, 0x08,
|
||||
0x94, 0x73, 0xBD, 0x5A, 0x74, 0x93, 0x5D, 0xBA, 0x26, 0xC1, 0x0F, 0xE8,
|
||||
0xD0, 0x37, 0xF9, 0x1E, 0x82, 0x65, 0xAB, 0x4C, 0xDB, 0x3C, 0xF2, 0x15,
|
||||
0x89, 0x6E, 0xA0, 0x47, 0x7F, 0x98, 0x56, 0xB1, 0x2D, 0xCA, 0x04, 0xE3,
|
||||
0x58, 0xBF, 0x71, 0x96, 0x0A, 0xED, 0x23, 0xC4, 0xFC, 0x1B, 0xD5, 0x32,
|
||||
0xAE, 0x49, 0x87, 0x60, 0xF7, 0x10, 0xDE, 0x39, 0xA5, 0x42, 0x8C, 0x6B,
|
||||
0x53, 0xB4, 0x7A, 0x9D, 0x01, 0xE6, 0x28, 0xCF, 0xE1, 0x06, 0xC8, 0x2F,
|
||||
0xB3, 0x54, 0x9A, 0x7D, 0x45, 0xA2, 0x6C, 0x8B, 0x17, 0xF0, 0x3E, 0xD9,
|
||||
0x4E, 0xA9, 0x67, 0x80, 0x1C, 0xFB, 0x35, 0xD2, 0xEA, 0x0D, 0xC3, 0x24,
|
||||
0xB8, 0x5F, 0x91, 0x76
|
||||
};
|
||||
|
||||
} /* tap_esc_common */
|
||||
|
|
@ -1,106 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2017 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/*
|
||||
* tap_esc_common.h
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include "drv_tap_esc.h"
|
||||
|
||||
namespace tap_esc_common
|
||||
{
|
||||
/**
|
||||
* Select tap esc responder data for serial interface by 74hct151. GPIOs to be
|
||||
* defined in board_config.h
|
||||
* @param sel ID of the ESC (responder) of which feedback is requested
|
||||
*/
|
||||
void select_responder(uint8_t sel);
|
||||
|
||||
/**
|
||||
* Opens a device for use as UART.
|
||||
* @param device UNIX path of UART device
|
||||
* @param uart_fd file-descriptor of UART device
|
||||
* @return 0 on success, -1 on error
|
||||
*/
|
||||
int initialise_uart(const char *const device, int &uart_fd);
|
||||
|
||||
/**
|
||||
* Closes a device previously opened with initialise_uart().
|
||||
* @param uart_fd file-descriptor of UART device as provided by initialise_uart()
|
||||
* @return 0 on success, -1 on error
|
||||
*/
|
||||
int deinitialise_uart(int &uart_fd);
|
||||
|
||||
/**
|
||||
* Enables/disables flow control for open UART device.
|
||||
* @param uart_fd file-descriptor of UART device
|
||||
* @param enabled Set true for enabling and false for disabling flow control
|
||||
* @return 0 on success, -1 on error
|
||||
*/
|
||||
int enable_flow_control(int uart_fd, bool enabled);
|
||||
|
||||
/**
|
||||
* Sends a packet to all ESCs and requests a specific ESC to respond
|
||||
* @param uart_fd file-descriptor of UART device
|
||||
* @param packet Packet to be sent to ESCs. CRC information will be added.
|
||||
* @param responder ID of the ESC (responder) that should return feedback
|
||||
* @return On success number of bytes written, on error -1
|
||||
*/
|
||||
int send_packet(int uart_fd, EscPacket &packet, int responder);
|
||||
|
||||
/**
|
||||
* Read data from the UART into a buffer
|
||||
* @param uart_fd file-descriptor of UART device
|
||||
* @param uart_buf Buffer where incoming data will be stored
|
||||
* @return 0 on success, -1 on error
|
||||
*/
|
||||
int read_data_from_uart(int uart_fd, ESC_UART_BUF *const uart_buf);
|
||||
|
||||
/**
|
||||
* Parse feedback from an ESC
|
||||
* @param serial_buf Buffer where incoming data will be stored
|
||||
* @param packetdata Packet that will be populated with information from buffer
|
||||
* @return 0 on success, -1 on error
|
||||
*/
|
||||
int parse_tap_esc_feedback(ESC_UART_BUF *const serial_buf, EscPacket *const packetdata);
|
||||
|
||||
/**
|
||||
* Lookup-table for faster CRC computation when sending ESC packets.
|
||||
*/
|
||||
extern const uint8_t crc_table[];
|
||||
} /* tap_esc_common */
|
||||
Loading…
Reference in New Issue