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

Add support in the IO driver to control the S.Bus / RSSI port.

This commit is contained in:
Lorenz Meier 2014-01-26 14:22:54 +01:00
parent 16eb68f2e9
commit 731ab465b3
3 changed files with 163 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/drivers/drv_rc_input.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 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
@ -135,8 +135,12 @@ ORB_DECLARE(input_rc);
#define _RC_INPUT_BASE 0x2b00
/** Fetch R/C input values into (rc_input_values *)arg */
#define RC_INPUT_GET _IOC(_RC_INPUT_BASE, 0)
/** Enable RSSI input via ADC */
#define RC_INPUT_ENABLE_RSSI_ANALOG _IOC(_RC_INPUT_BASE, 1)
/** Enable RSSI input via PWM signal */
#define RC_INPUT_ENABLE_RSSI_PWM _IOC(_RC_INPUT_BASE, 2)
#endif /* _DRV_RC_INPUT_H */

58
src/drivers/drv_sbus.h Normal file
View File

@ -0,0 +1,58 @@
/****************************************************************************
*
* Copyright (c) 2014 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.
*
****************************************************************************/
/**
* @file drv_sbus.h
*
* Futaba S.BUS / S.BUS 2 compatible interface.
*/
#ifndef _DRV_SBUS_H
#define _DRV_SBUS_H
#include <stdint.h>
#include <sys/ioctl.h>
#include "drv_orb_dev.h"
/**
* Path for the default S.BUS device
*/
#define SBUS_DEVICE_PATH "/dev/sbus"
#define _SBUS_BASE 0x2c00
/** Enable S.BUS version 1 / 2 output (0 to disable) */
#define SBUS_SET_PROTO_VERSION _IOC(_SBUS_BASE, 0)
#endif /* _DRV_SBUS_H */

103
src/drivers/px4io/px4io.cpp
View File

@ -61,6 +61,7 @@
#include <drivers/device/device.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_sbus.h>
#include <drivers/drv_gpio.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_mixer.h>
@ -1824,9 +1825,11 @@ PX4IO::print_status()
uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT);
printf("%d raw R/C inputs", raw_inputs);
flags = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_FLAGS);
printf("status 0x%04x%s", flags,
printf("status 0x%04x%s%s%s%s", flags,
(((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11))) ? " DSM10" : ""),
(((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11)) ? " DSM11" : "")
(((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11)) ? " DSM11" : ""),
((flags & PX4IO_P_RAW_RC_FLAGS_FRAME_DROP) ? " FRAME_DROP" : ""),
((flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) ? " FAILSAFE" : "")
);
for (unsigned i = 0; i < raw_inputs; i++)
@ -1861,7 +1864,13 @@ PX4IO::print_status()
printf("\n");
/* setup and state */
printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES));
uint16_t features = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES);
printf("features 0x%04x%s%s\n", features,
((features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) ? " S.BUS1_OUT" : ""),
((features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) ? " S.BUS2_OUT" : ""),
((features & PX4IO_P_SETUP_FEATURES_PWM_RSSI) ? " RSSI_PWM" : ""),
((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "")
);
uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
printf("arming 0x%04x%s%s%s%s%s%s\n",
arming,
@ -2283,6 +2292,38 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
break;
case RC_INPUT_ENABLE_RSSI_ANALOG:
if (arg) {
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_ADC_RSSI);
} else {
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, PX4IO_P_SETUP_FEATURES_ADC_RSSI, 0);
}
break;
case RC_INPUT_ENABLE_RSSI_PWM:
if (arg) {
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_PWM_RSSI);
} else {
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, PX4IO_P_SETUP_FEATURES_PWM_RSSI, 0);
}
break;
case SBUS_SET_PROTO_VERSION:
if (arg == 1) {
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS1_OUT);
} else if (arg == 2) {
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
} else {
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT), 0);
}
break;
default:
/* not a recognized value */
ret = -ENOTTY;
@ -2932,6 +2973,60 @@ px4io_main(int argc, char *argv[])
if (!strcmp(argv[1], "bind"))
bind(argc, argv);
if (!strcmp(argv[1], "sbus1_out")) {
/* we can cheat and call the driver directly, as it
* doesn't reference filp in ioctl()
*/
int ret = g_dev->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 1);
if (ret != 0) {
errx(ret, "S.BUS v1 failed");
}
exit(0);
}
if (!strcmp(argv[1], "sbus2_out")) {
/* we can cheat and call the driver directly, as it
* doesn't reference filp in ioctl()
*/
int ret = g_dev->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 2);
if (ret != 0) {
errx(ret, "S.BUS v2 failed");
}
exit(0);
}
if (!strcmp(argv[1], "rssi_analog")) {
/* we can cheat and call the driver directly, as it
* doesn't reference filp in ioctl()
*/
int ret = g_dev->ioctl(nullptr, RC_INPUT_ENABLE_RSSI_ANALOG, 1);
if (ret != 0) {
errx(ret, "RSSI analog failed");
}
exit(0);
}
if (!strcmp(argv[1], "rssi_pwm")) {
/* we can cheat and call the driver directly, as it
* doesn't reference filp in ioctl()
*/
int ret = g_dev->ioctl(nullptr, RC_INPUT_ENABLE_RSSI_PWM, 1);
if (ret != 0) {
errx(ret, "RSSI PWM failed");
}
exit(0);
}
out:
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind', 'checkcrc', 'forceupdate' or 'update'");
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug <level>',\n"
"'recovery', 'limit <rate>', 'current', 'bind', 'checkcrc',\n"
"'forceupdate', 'update', 'sbus1_out', 'sbus2_out', 'rssi_analog' or 'rssi_pwm'");
}