forked from Archive/PX4-Autopilot
1.Add sbus driver for linux . This driver can be used to read the
inverted S.bus signal and fetch the data of each channel and publish it 2. Fix the bug of linux_pwm_out, when the protocol is pca9685, after the init method is executed,the method of determining the return value of init method is incorrect,this will cause the driver to fail 3. Add linux_sbus driver to other posix prefixes cmake files
This commit is contained in:
crossa
Nuno Marques
parent
b2a81ed35b
commit
17ba5dd04a
|
|
@ -16,6 +16,7 @@ set(config_module_list
|
|||
# Board support modules
|
||||
#
|
||||
drivers/device
|
||||
drivers/linux_sbus
|
||||
modules/sensors
|
||||
platforms/posix/drivers/df_ms5607_wrapper
|
||||
platforms/posix/drivers/df_mpu6050_wrapper
|
||||
|
|
|
|||
|
|
@ -22,6 +22,7 @@ set(config_module_list
|
|||
drivers/device
|
||||
drivers/boards/sitl
|
||||
drivers/led
|
||||
drivers/linux_sbus
|
||||
|
||||
systemcmds/param
|
||||
systemcmds/ver
|
||||
|
|
|
|||
|
|
@ -74,6 +74,7 @@ set(config_module_list
|
|||
#
|
||||
# PX4 drivers
|
||||
#
|
||||
drivers/linux_sbus
|
||||
drivers/gps
|
||||
drivers/navio_adc
|
||||
drivers/navio_sysfs_rc_in
|
||||
|
|
|
|||
|
|
@ -30,6 +30,7 @@ set(CONFIG_SHMEM "1")
|
|||
set(config_module_list
|
||||
drivers/device
|
||||
drivers/blinkm
|
||||
drivers/linux_sbus
|
||||
drivers/pwm_out_sim
|
||||
drivers/rgbled
|
||||
drivers/led
|
||||
|
|
|
|||
|
|
@ -22,6 +22,7 @@ set(CONFIG_SHMEM "1")
|
|||
set(config_module_list
|
||||
drivers/device
|
||||
drivers/blinkm
|
||||
drivers/linux_sbus
|
||||
drivers/pwm_out_sim
|
||||
drivers/rgbled
|
||||
drivers/led
|
||||
|
|
|
|||
|
|
@ -55,7 +55,7 @@
|
|||
|
||||
using namespace linux_pwm_out;
|
||||
|
||||
void PCA9685::init(int bus, int address)
|
||||
int PCA9685::init(int bus, int address)
|
||||
{
|
||||
_fd = open_fd(bus, address);
|
||||
reset();
|
||||
|
|
@ -65,6 +65,7 @@ void PCA9685::init(int bus, int address)
|
|||
/* 200HZ for 12bit Resolution, supported by most of the esc's */
|
||||
setPWMFreq(200);
|
||||
usleep(1000 * 1000);
|
||||
return 0;
|
||||
}
|
||||
|
||||
int PCA9685::send_output_pwm(const uint16_t *pwm, int num_outputs)
|
||||
|
|
|
|||
|
|
@ -87,10 +87,10 @@ public:
|
|||
int send_output_pwm(const uint16_t *pwm, int num_outputs) override;
|
||||
|
||||
|
||||
void init(int bus, int address);
|
||||
int init(int bus, int address);
|
||||
virtual ~PCA9685();
|
||||
|
||||
/// Sets PCA9685 mode to 00
|
||||
/** Sets PCA9685 mode to 00 */
|
||||
void reset();
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -211,12 +211,18 @@ void task_main(int argc, char *argv[])
|
|||
PX4_INFO("Starting PWM output in ocpoc_mmap mode");
|
||||
pwm_out = new OcpocMmapPWMOut(_max_num_outputs);
|
||||
|
||||
} else { // navio
|
||||
} else { /* navio */
|
||||
PX4_INFO("Starting PWM output in Navio mode");
|
||||
pwm_out = new NavioSysfsPWMOut(_device, _max_num_outputs);
|
||||
}
|
||||
|
||||
if (pwm_out->init() != 0) {
|
||||
/**
|
||||
* if the _protocol is "pca9685" and the driver is executed correctly，
|
||||
* the return value of "pwm_out->init()" will be higher than 0.
|
||||
*/
|
||||
bool check_pwm_device = (strcmp(_protocol, "pca9685") == 0) ? (0 > pwm_out->init()) : (pwm_out->init() != 0);
|
||||
|
||||
if (check_pwm_device) {
|
||||
PX4_ERR("PWM output init failed");
|
||||
delete pwm_out;
|
||||
return;
|
||||
|
|
@ -494,7 +500,7 @@ int linux_pwm_out_main(int argc, char *argv[])
|
|||
}
|
||||
}
|
||||
|
||||
// gets the parameters for the esc's pwm
|
||||
/** gets the parameters for the esc's pwm */
|
||||
param_get(param_find("PWM_DISARMED"), &linux_pwm_out::_pwm_disarmed);
|
||||
param_get(param_find("PWM_MIN"), &linux_pwm_out::_pwm_min);
|
||||
param_get(param_find("PWM_MAX"), &linux_pwm_out::_pwm_max);
|
||||
|
|
|
|||
|
|
@ -0,0 +1,42 @@
|
|||
############################################################################
|
||||
#
|
||||
# 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.
|
||||
#
|
||||
############################################################################
|
||||
px4_add_module(
|
||||
MODULE drivers__linux_sbus
|
||||
MAIN linux_sbus
|
||||
COMPILE_FLAGS
|
||||
SRCS
|
||||
linux_sbus.cpp
|
||||
DEPENDS
|
||||
platforms__common
|
||||
)
|
||||
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
|
||||
|
|
@ -0,0 +1,364 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2017 Mark Charl. All rights reserved.
|
||||
* Copyright (C) 2017 Fan.zhang. All rights reserved.
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
#include "linux_sbus.h"
|
||||
using namespace linux_sbus;
|
||||
//---------------------------------------------------------------------------------------------------------//
|
||||
int RcInput::init()
|
||||
{
|
||||
int i;
|
||||
|
||||
/**
|
||||
* initialize the data of each channel
|
||||
*/
|
||||
for (i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; ++i) {
|
||||
_data.values[i] = UINT16_MAX;
|
||||
}
|
||||
|
||||
_rcinput_pub = orb_advertise(ORB_ID(input_rc), &_data);
|
||||
|
||||
if (nullptr == _rcinput_pub) {
|
||||
PX4_WARN("error: advertise failed");
|
||||
return -1;
|
||||
}
|
||||
|
||||
/**
|
||||
* open the serial port
|
||||
*/
|
||||
_device_fd = open(_device, O_RDWR | O_NONBLOCK | O_CLOEXEC);
|
||||
|
||||
if (-1 == _device_fd) {
|
||||
PX4_ERR("Open SBUS input %s failed, status %d \n", _device,
|
||||
(int) _device_fd);
|
||||
fflush(stdout);
|
||||
return -1;
|
||||
}
|
||||
|
||||
struct termios2 tio { };
|
||||
|
||||
if (0 != ioctl(_device_fd, TCGETS2, &tio)) {
|
||||
close(_device_fd);
|
||||
_device_fd = -1;
|
||||
return -1;
|
||||
}
|
||||
|
||||
/**
|
||||
* Setting serial port,8E2, non-blocking.100Kbps
|
||||
*/
|
||||
tio.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL
|
||||
| IXON);
|
||||
tio.c_iflag |= (INPCK | IGNPAR);
|
||||
tio.c_oflag &= ~OPOST;
|
||||
tio.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
|
||||
tio.c_cflag &= ~(CSIZE | CRTSCTS | PARODD | CBAUD);
|
||||
/**
|
||||
* use BOTHER to specify speed directly in c_[io]speed member
|
||||
*/
|
||||
tio.c_cflag |= (CS8 | CSTOPB | CLOCAL | PARENB | BOTHER | CREAD);
|
||||
tio.c_ispeed = 100000;
|
||||
tio.c_ospeed = 100000;
|
||||
tio.c_cc[VMIN] = 25;
|
||||
tio.c_cc[VTIME] = 0;
|
||||
|
||||
if (0 != ioctl(_device_fd, TCSETS2, &tio)) {
|
||||
close(_device_fd);
|
||||
_device_fd = -1;
|
||||
return -1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
//---------------------------------------------------------------------------------------------------------//
|
||||
int RcInput::start(char *device, int channels)
|
||||
{
|
||||
int result = 0;
|
||||
strcpy(_device, device);
|
||||
PX4_WARN("Device %s , channels: %d \n", device, channels);
|
||||
_channels = channels;
|
||||
result = init();
|
||||
|
||||
if (0 != result) {
|
||||
PX4_WARN("error: RC initialization failed");
|
||||
return -1;
|
||||
}
|
||||
|
||||
_isRunning = true;
|
||||
result = work_queue(HPWORK, &_work, (worker_t) & RcInput::cycle_trampoline,
|
||||
this, 0);
|
||||
|
||||
if (result == -1) {
|
||||
_isRunning = false;
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
//---------------------------------------------------------------------------------------------------------//
|
||||
void RcInput::stop()
|
||||
{
|
||||
close(_device_fd);
|
||||
_shouldExit = true;
|
||||
}
|
||||
//---------------------------------------------------------------------------------------------------------//
|
||||
void RcInput::cycle_trampoline(void *arg)
|
||||
{
|
||||
RcInput *dev = reinterpret_cast<RcInput *>(arg);
|
||||
dev->_cycle();
|
||||
}
|
||||
//---------------------------------------------------------------------------------------------------------//
|
||||
void RcInput::_cycle()
|
||||
{
|
||||
_measure();
|
||||
|
||||
if (!_shouldExit) {
|
||||
work_queue(HPWORK, &_work, (worker_t) & RcInput::cycle_trampoline, this,
|
||||
USEC2TICK(RCINPUT_MEASURE_INTERVAL_US));
|
||||
}
|
||||
}
|
||||
//---------------------------------------------------------------------------------------------------------//
|
||||
void RcInput::_measure(void)
|
||||
{
|
||||
uint64_t ts;
|
||||
int nread;
|
||||
fd_set fds;
|
||||
FD_ZERO(&fds);
|
||||
FD_SET(_device_fd, &fds);
|
||||
/**
|
||||
*error counter to count the lost frame
|
||||
*/
|
||||
int count = 0; //
|
||||
|
||||
while (1) {
|
||||
nread = read(_device_fd, &_sbusData, sizeof(_sbusData));
|
||||
|
||||
if (25 == nread) {
|
||||
/**
|
||||
* Notice: most sbus rx device support sbus1
|
||||
*/
|
||||
if (0x0f == _sbusData[0] && 0x00 == _sbusData[24]) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
++count;
|
||||
usleep(RCINPUT_MEASURE_INTERVAL_US);
|
||||
}
|
||||
|
||||
/**
|
||||
* parse sbus data to pwm
|
||||
*/
|
||||
_channels_data[0] =
|
||||
(uint16_t)(((_sbusData[1] | _sbusData[2] << 8) & 0x07FF)
|
||||
* SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET;
|
||||
_channels_data[1] = (uint16_t)(((_sbusData[2] >> 3 | _sbusData[3] << 5)
|
||||
& 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET;
|
||||
_channels_data[2] = (uint16_t)(((_sbusData[3] >> 6 | _sbusData[4] << 2
|
||||
| _sbusData[5] << 10) & 0x07FF) * SBUS_SCALE_FACTOR + .5f)
|
||||
+ SBUS_SCALE_OFFSET;
|
||||
_channels_data[3] = (uint16_t)(((_sbusData[5] >> 1 | _sbusData[6] << 7)
|
||||
& 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET;
|
||||
_channels_data[4] = (uint16_t)(((_sbusData[6] >> 4 | _sbusData[7] << 4)
|
||||
& 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET;
|
||||
_channels_data[5] = (uint16_t)(((_sbusData[7] >> 7 | _sbusData[8] << 1
|
||||
| _sbusData[9] << 9) & 0x07FF) * SBUS_SCALE_FACTOR + .5f)
|
||||
+ SBUS_SCALE_OFFSET;
|
||||
_channels_data[6] = (uint16_t)(((_sbusData[9] >> 2 | _sbusData[10] << 6)
|
||||
& 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET;
|
||||
_channels_data[7] = (uint16_t)(((_sbusData[10] >> 5 | _sbusData[11] << 3)
|
||||
& 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET; // & the other 8 + 2 channels if you need them
|
||||
_channels_data[8] = (uint16_t)(((_sbusData[12] | _sbusData[13] << 8)
|
||||
& 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET;
|
||||
_channels_data[9] = (uint16_t)(((_sbusData[13] >> 3 | _sbusData[14] << 5)
|
||||
& 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET;
|
||||
_channels_data[10] = (uint16_t)(((_sbusData[14] >> 6 | _sbusData[15] << 2
|
||||
| _sbusData[16] << 10) & 0x07FF) * SBUS_SCALE_FACTOR + .5f)
|
||||
+ SBUS_SCALE_OFFSET;
|
||||
_channels_data[11] = (uint16_t)(((_sbusData[16] >> 1 | _sbusData[17] << 7)
|
||||
& 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET;
|
||||
_channels_data[12] = (uint16_t)(((_sbusData[17] >> 4 | _sbusData[18] << 4)
|
||||
& 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET;
|
||||
_channels_data[13] = (uint16_t)(((_sbusData[18] >> 7 | _sbusData[19] << 1
|
||||
| _sbusData[20] << 9) & 0x07FF) * SBUS_SCALE_FACTOR + .5f)
|
||||
+ SBUS_SCALE_OFFSET;
|
||||
_channels_data[14] = (uint16_t)(((_sbusData[20] >> 2 | _sbusData[21] << 6)
|
||||
& 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET;
|
||||
_channels_data[15] = (uint16_t)(((_sbusData[21] >> 5 | _sbusData[22] << 3)
|
||||
& 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET;
|
||||
int i = 0;
|
||||
|
||||
for (i = 0; i < _channels; ++i) {
|
||||
_data.values[i] = _channels_data[i];
|
||||
}
|
||||
|
||||
ts = hrt_absolute_time();
|
||||
_data.timestamp = ts;
|
||||
_data.timestamp_last_signal = ts;
|
||||
_data.channel_count = _channels;
|
||||
_data.rssi = 100;
|
||||
_data.rc_lost_frame_count = count;
|
||||
_data.rc_total_frame_count = 1;
|
||||
_data.rc_ppm_frame_length = 0;
|
||||
_data.rc_failsafe = (_sbusData[23] & (1 << 3)) ? true : false;
|
||||
_data.rc_lost = (_sbusData[23] & (1 << 2)) ? true : false;
|
||||
_data.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS;
|
||||
orb_publish(ORB_ID(input_rc), _rcinput_pub, &_data);
|
||||
}
|
||||
//---------------------------------------------------------------------------------------------------------//
|
||||
/**
|
||||
* Print the correct usage.
|
||||
*/
|
||||
static void linux_sbus::usage(const char *reason)
|
||||
{
|
||||
if (reason) {
|
||||
PX4_ERR("%s", reason);
|
||||
}
|
||||
|
||||
PX4_INFO("Usage: linux_sbus {start|stop|status} -d <device> -c <channel>");
|
||||
}
|
||||
//---------------------------------------------------------------------------------------------------------//
|
||||
int linux_sbus_main(int argc, char **argv)
|
||||
{
|
||||
int start;
|
||||
int command = -1;
|
||||
/**
|
||||
* ttyS1 for default, it can be changed through -d parameter
|
||||
*/
|
||||
char device[30] = "/dev/ttyS1";
|
||||
/**
|
||||
* 8 channel for default setting, it can be changed through -c parameter
|
||||
*/
|
||||
int max_channel = 8;
|
||||
|
||||
/**
|
||||
* Parse command line and get device and channels count from consolex
|
||||
*/
|
||||
for (start = 0; start < argc; ++start) {
|
||||
if (0 == strcmp(argv[start], "start")) {
|
||||
command = 0;
|
||||
continue;
|
||||
}
|
||||
|
||||
if (0 == strcmp(argv[start], "stop")) {
|
||||
command = 1;
|
||||
continue;
|
||||
}
|
||||
|
||||
if (0 == strcmp(argv[start], "status")) {
|
||||
command = 2;
|
||||
continue;
|
||||
}
|
||||
|
||||
if (0 == strcmp(argv[start], "-d")) {
|
||||
if (argc > (start + 1)) {
|
||||
strcpy(device, argv[start + 1]);
|
||||
}
|
||||
|
||||
continue;
|
||||
}
|
||||
|
||||
if (0 == strcmp(argv[start], "-c")) {
|
||||
if (argc > (start + 1)) {
|
||||
max_channel = atoi(argv[start + 1]);
|
||||
}
|
||||
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Channels count can't be higher than 16;
|
||||
*/
|
||||
max_channel = (max_channel > 16) ? 16 : max_channel;
|
||||
|
||||
if (0 == command) {
|
||||
if (nullptr != rc_input && rc_input->isRunning()) {
|
||||
PX4_WARN("running");
|
||||
return 0;
|
||||
}
|
||||
|
||||
rc_input = new RcInput();
|
||||
|
||||
/** Check if alloc worked. */
|
||||
if (nullptr == rc_input) {
|
||||
PX4_ERR("Sbus driver initialization failed");
|
||||
return -1;
|
||||
}
|
||||
|
||||
int ret = rc_input->start(device, max_channel);
|
||||
|
||||
if (ret != 0) {
|
||||
PX4_ERR("Linux sbus module failure");
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (1 == command) {
|
||||
if (rc_input == nullptr || !rc_input->isRunning()) {
|
||||
PX4_WARN("Not running \n");
|
||||
/* this is not an error */
|
||||
return 0;
|
||||
}
|
||||
|
||||
rc_input->stop();
|
||||
/**
|
||||
* Wait for task to die
|
||||
*/
|
||||
int i = 0;
|
||||
|
||||
do {
|
||||
/* wait up to 100ms */
|
||||
usleep(100000);
|
||||
} while (rc_input->isRunning() && ++i < 30);
|
||||
|
||||
delete rc_input;
|
||||
rc_input = nullptr;
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (2 == command) {
|
||||
if (rc_input != nullptr && rc_input->isRunning()) {
|
||||
PX4_INFO("running");
|
||||
|
||||
} else {
|
||||
PX4_INFO("Not running \n");
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
linux_sbus::usage(
|
||||
"Usage: linux_sbus start|stop|status -d <device> -c <channel>");
|
||||
return 0;
|
||||
}
|
||||
//---------------------------------------------------------------------------------------------------------//
|
||||
|
|
@ -0,0 +1,121 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2017 Mark Charl. All rights reserved.
|
||||
* Copyright (C) 2017 Fan.zhang. All rights reserved.
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <sys/stat.h>
|
||||
#include <sys/ipc.h>
|
||||
#include <fcntl.h>
|
||||
#include <stdio.h>
|
||||
#include <stdint.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <sys/ioctl.h>
|
||||
/*For terminal I/O interfaces, termbits.h from asm-generic versions of functions guarantees non-standard communication (100Khz, Non-blocking), not guaranteed by termios.h*/
|
||||
#include <asm-generic/termbits.h>
|
||||
#include <errno.h>
|
||||
#include <px4_config.h>
|
||||
#include <px4_workqueue.h>
|
||||
#include <px4_defines.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/input_rc.h>
|
||||
/* The interval between each frame is 4700us, do not change it. */
|
||||
#define RCINPUT_MEASURE_INTERVAL_US 4700
|
||||
/* define range mapping here, -+100% -> 1000..2000 */
|
||||
#define SBUS_RANGE_MIN 200.0f
|
||||
#define SBUS_RANGE_MAX 1800.0f
|
||||
#define SBUS_TARGET_MIN 1000.0f
|
||||
#define SBUS_TARGET_MAX 2000.0f
|
||||
/* pre-calculate the floating point stuff as far as possible at compile time */
|
||||
#define SBUS_SCALE_FACTOR ((SBUS_TARGET_MAX - SBUS_TARGET_MIN) / (SBUS_RANGE_MAX - SBUS_RANGE_MIN))
|
||||
#define SBUS_SCALE_OFFSET (int)(SBUS_TARGET_MIN - (SBUS_SCALE_FACTOR * SBUS_RANGE_MIN + 0.5f))
|
||||
|
||||
namespace linux_sbus
|
||||
{
|
||||
class RcInput
|
||||
{
|
||||
public:
|
||||
RcInput() :
|
||||
_shouldExit(false),
|
||||
_isRunning(false),
|
||||
_work { },
|
||||
_rcinput_pub(nullptr),
|
||||
_data { }, _sbusData { 0x0f, 0x01, 0x04, 0x20, 0x00,
|
||||
0xff, 0x07, 0x40, 0x00, 0x02,
|
||||
0x10, 0x80, 0x2c, 0x64, 0x21,
|
||||
0x0b, 0x59, 0x08, 0x40, 0x00,
|
||||
0x02, 0x10, 0x80, 0x00, 0x00 }
|
||||
{ }
|
||||
~RcInput()
|
||||
{
|
||||
work_cancel(HPWORK, &_work);
|
||||
_isRunning = false;
|
||||
close(_device_fd);
|
||||
}
|
||||
/** @return 0 on success, -errno on failure */
|
||||
int start(char *device, int channels);
|
||||
void stop();
|
||||
|
||||
/** Trampoline for the work queue. */
|
||||
static void cycle_trampoline(void *arg);
|
||||
|
||||
bool isRunning()
|
||||
{
|
||||
return _isRunning;
|
||||
}
|
||||
|
||||
private:
|
||||
void _cycle();
|
||||
void _measure();
|
||||
bool _shouldExit;
|
||||
bool _isRunning;
|
||||
struct work_s _work;
|
||||
orb_advert_t _rcinput_pub;
|
||||
struct input_rc_s _data;
|
||||
uint8_t _sbusData[25];
|
||||
int _channels;
|
||||
int _device_fd; /** serial port device to read SBUS; */
|
||||
int _channels_data[16]; /** 16 channels support; */
|
||||
uint8_t _buffer[25];
|
||||
char _device[30];
|
||||
bool _failsafe;
|
||||
bool _rc_loss;
|
||||
int init();
|
||||
};
|
||||
|
||||
static void usage(const char *reason);
|
||||
static RcInput *rc_input = nullptr;
|
||||
}
|
||||
extern "C" __EXPORT int linux_sbus_main(int argc, char **argv);
|
||||
Loading…
Reference in New Issue