forked from Archive/PX4-Autopilot
Merge pull request #609 from PX4/rc_status
RC status metadata cleanup and extension
This commit is contained in:
Lorenz Meier
commit
70afb3ca3b
|
|
@ -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
|
||||
|
|
@ -62,6 +62,11 @@
|
|||
*/
|
||||
#define RC_INPUT_MAX_CHANNELS 18
|
||||
|
||||
/**
|
||||
* Maximum RSSI value
|
||||
*/
|
||||
#define RC_INPUT_RSSI_MAX 255
|
||||
|
||||
/**
|
||||
* Input signal type, value is a control position from zero to 100
|
||||
* percent.
|
||||
|
|
@ -83,8 +88,11 @@ enum RC_INPUT_SOURCE {
|
|||
* on the board involved.
|
||||
*/
|
||||
struct rc_input_values {
|
||||
/** decoding time */
|
||||
uint64_t timestamp;
|
||||
/** publication time */
|
||||
uint64_t timestamp_publication;
|
||||
|
||||
/** last valid reception time */
|
||||
uint64_t timestamp_last_signal;
|
||||
|
||||
/** number of channels actually being seen */
|
||||
uint32_t channel_count;
|
||||
|
|
@ -92,6 +100,40 @@ struct rc_input_values {
|
|||
/** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 255: full reception */
|
||||
int32_t rssi;
|
||||
|
||||
/**
|
||||
* explicit failsafe flag: true on TX failure or TX out of range , false otherwise.
|
||||
* Only the true state is reliable, as there are some (PPM) receivers on the market going
|
||||
* into failsafe without telling us explicitly.
|
||||
* */
|
||||
bool rc_failsafe;
|
||||
|
||||
/**
|
||||
* RC receiver connection status: True,if no frame has arrived in the expected time, false otherwise.
|
||||
* True usally means that the receiver has been disconnected, but can also indicate a radio link loss on "stupid" systems.
|
||||
* Will remain false, if a RX with failsafe option continues to transmit frames after a link loss.
|
||||
* */
|
||||
bool rc_lost;
|
||||
|
||||
/**
|
||||
* Number of lost RC frames.
|
||||
* Note: intended purpose: observe the radio link quality if RSSI is not available
|
||||
* This value must not be used to trigger any failsafe-alike funtionality.
|
||||
* */
|
||||
uint16_t rc_lost_frame_count;
|
||||
|
||||
/**
|
||||
* Number of total RC frames.
|
||||
* Note: intended purpose: observe the radio link quality if RSSI is not available
|
||||
* This value must not be used to trigger any failsafe-alike funtionality.
|
||||
* */
|
||||
uint16_t rc_total_frame_count;
|
||||
|
||||
/**
|
||||
* Length of a single PPM frame.
|
||||
* Zero for non-PPM systems
|
||||
*/
|
||||
uint16_t rc_ppm_frame_length;
|
||||
|
||||
/** Input source */
|
||||
enum RC_INPUT_SOURCE input_source;
|
||||
|
||||
|
|
@ -107,8 +149,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 */
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
@ -626,7 +626,7 @@ PX4FMU::task_main()
|
|||
#ifdef HRT_PPM_CHANNEL
|
||||
|
||||
// see if we have new PPM input data
|
||||
if (ppm_last_valid_decode != rc_in.timestamp) {
|
||||
if (ppm_last_valid_decode != rc_in.timestamp_last_signal) {
|
||||
// we have a new PPM frame. Publish it.
|
||||
rc_in.channel_count = ppm_decoded_channels;
|
||||
|
||||
|
|
@ -638,7 +638,15 @@ PX4FMU::task_main()
|
|||
rc_in.values[i] = ppm_buffer[i];
|
||||
}
|
||||
|
||||
rc_in.timestamp = ppm_last_valid_decode;
|
||||
rc_in.timestamp_publication = ppm_last_valid_decode;
|
||||
rc_in.timestamp_last_signal = ppm_last_valid_decode;
|
||||
|
||||
rc_in.rc_ppm_frame_length = ppm_frame_length;
|
||||
rc_in.rssi = RC_INPUT_RSSI_MAX;
|
||||
rc_in.rc_failsafe = false;
|
||||
rc_in.rc_lost = false;
|
||||
rc_in.rc_lost_frame_count = 0;
|
||||
rc_in.rc_total_frame_count = 0;
|
||||
|
||||
/* lazily advertise on first publication */
|
||||
if (to_input_rc == 0) {
|
||||
|
|
|
|||
|
|
@ -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>
|
||||
|
|
@ -238,6 +239,7 @@ private:
|
|||
unsigned _update_interval; ///< Subscription interval limiting send rate
|
||||
bool _rc_handling_disabled; ///< If set, IO does not evaluate, but only forward the RC values
|
||||
unsigned _rc_chan_count; ///< Internal copy of the last seen number of RC channels
|
||||
uint64_t _rc_last_valid; ///< last valid timestamp
|
||||
|
||||
volatile int _task; ///< worker task id
|
||||
volatile bool _task_should_exit; ///< worker terminate flag
|
||||
|
|
@ -444,7 +446,7 @@ private:
|
|||
* @param vservo vservo register
|
||||
* @param vrssi vrssi register
|
||||
*/
|
||||
void io_handle_vservo(uint16_t vbatt, uint16_t ibatt);
|
||||
void io_handle_vservo(uint16_t vservo, uint16_t vrssi);
|
||||
|
||||
};
|
||||
|
||||
|
|
@ -467,6 +469,7 @@ PX4IO::PX4IO(device::Device *interface) :
|
|||
_update_interval(0),
|
||||
_rc_handling_disabled(false),
|
||||
_rc_chan_count(0),
|
||||
_rc_last_valid(0),
|
||||
_task(-1),
|
||||
_task_should_exit(false),
|
||||
_mavlink_fd(-1),
|
||||
|
|
@ -1357,7 +1360,10 @@ PX4IO::io_get_status()
|
|||
uint16_t regs[6];
|
||||
int ret;
|
||||
|
||||
/* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */
|
||||
/* get
|
||||
* STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT,
|
||||
* STATUS_VSERVO, STATUS_VRSSI, STATUS_PRSSI
|
||||
* in that order */
|
||||
ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, ®s[0], sizeof(regs) / sizeof(regs[0]));
|
||||
|
||||
if (ret != OK)
|
||||
|
|
@ -1394,7 +1400,8 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
|
|||
*
|
||||
* This should be the common case (9 channel R/C control being a reasonable upper bound).
|
||||
*/
|
||||
input_rc.timestamp = hrt_absolute_time();
|
||||
input_rc.timestamp_publication = hrt_absolute_time();
|
||||
|
||||
ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, ®s[0], prolog + 9);
|
||||
|
||||
if (ret != OK)
|
||||
|
|
@ -1404,13 +1411,25 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
|
|||
* Get the channel count any any extra channels. This is no more expensive than reading the
|
||||
* channel count once.
|
||||
*/
|
||||
channel_count = regs[0];
|
||||
channel_count = regs[PX4IO_P_RAW_RC_COUNT];
|
||||
|
||||
if (channel_count != _rc_chan_count)
|
||||
perf_count(_perf_chan_count);
|
||||
|
||||
_rc_chan_count = channel_count;
|
||||
|
||||
input_rc.rc_ppm_frame_length = regs[PX4IO_P_RAW_RC_DATA];
|
||||
input_rc.rssi = regs[PX4IO_P_RAW_RC_NRSSI];
|
||||
input_rc.rc_failsafe = (regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
|
||||
input_rc.rc_lost_frame_count = regs[PX4IO_P_RAW_LOST_FRAME_COUNT];
|
||||
input_rc.rc_total_frame_count = regs[PX4IO_P_RAW_FRAME_COUNT];
|
||||
|
||||
/* rc_lost has to be set before the call to this function */
|
||||
if (!input_rc.rc_lost && !input_rc.rc_failsafe)
|
||||
_rc_last_valid = input_rc.timestamp_publication;
|
||||
|
||||
input_rc.timestamp_last_signal = _rc_last_valid;
|
||||
|
||||
if (channel_count > 9) {
|
||||
ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, ®s[prolog + 9], channel_count - 9);
|
||||
|
||||
|
|
@ -1427,13 +1446,12 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
|
|||
int
|
||||
PX4IO::io_publish_raw_rc()
|
||||
{
|
||||
/* if no raw RC, just don't publish */
|
||||
if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK))
|
||||
return OK;
|
||||
|
||||
/* fetch values from IO */
|
||||
rc_input_values rc_val;
|
||||
rc_val.timestamp = hrt_absolute_time();
|
||||
|
||||
/* set the RC status flag ORDER MATTERS! */
|
||||
rc_val.rc_lost = !(_status & PX4IO_P_STATUS_FLAGS_RC_OK);
|
||||
|
||||
int ret = io_get_raw_rc_input(rc_val);
|
||||
|
||||
|
|
@ -1452,13 +1470,11 @@ PX4IO::io_publish_raw_rc()
|
|||
|
||||
} else {
|
||||
rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN;
|
||||
}
|
||||
|
||||
/* set RSSI */
|
||||
|
||||
if (rc_val.input_source != RC_INPUT_SOURCE_PX4IO_SBUS) {
|
||||
// XXX the correct scaling needs to be validated here
|
||||
rc_val.rssi = (_servorail_status.rssi_v / 3.3f) * UINT8_MAX;
|
||||
/* we do not know the RC input, only publish if RC OK flag is set */
|
||||
/* if no raw RC, just don't publish */
|
||||
if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK))
|
||||
return OK;
|
||||
}
|
||||
|
||||
/* lazily advertise on first publication */
|
||||
|
|
@ -1767,15 +1783,14 @@ PX4IO::print_status()
|
|||
printf("%u bytes free\n",
|
||||
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM));
|
||||
uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS);
|
||||
printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s%s\n",
|
||||
uint16_t io_status_flags = flags;
|
||||
printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n",
|
||||
flags,
|
||||
((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""),
|
||||
((flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? " SAFETY_OFF" : " SAFETY_SAFE"),
|
||||
((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""),
|
||||
((flags & PX4IO_P_STATUS_FLAGS_RC_OK) ? " RC_OK" : " RC_FAIL"),
|
||||
((flags & PX4IO_P_STATUS_FLAGS_RC_PPM) ? " PPM" : ""),
|
||||
(((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_STATUS_FLAGS_RC_DSM11))) ? " DSM10" : ""),
|
||||
(((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_STATUS_FLAGS_RC_DSM11)) ? " DSM11" : ""),
|
||||
((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? " SBUS" : ""),
|
||||
((flags & PX4IO_P_STATUS_FLAGS_FMU_OK) ? " FMU_OK" : " FMU_FAIL"),
|
||||
((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PWM_PASSTHROUGH" : ""),
|
||||
|
|
@ -1834,8 +1849,17 @@ PX4IO::print_status()
|
|||
|
||||
printf("\n");
|
||||
|
||||
if ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) {
|
||||
int frame_len = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_RC_DATA);
|
||||
flags = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_FLAGS);
|
||||
printf("R/C flags: 0x%04x%s%s%s%s%s\n", 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" : ""),
|
||||
((flags & PX4IO_P_RAW_RC_FLAGS_FRAME_DROP) ? " FRAME_DROP" : ""),
|
||||
((flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) ? " FAILSAFE" : ""),
|
||||
((flags & PX4IO_P_RAW_RC_FLAGS_MAPPING_OK) ? " MAPPING_OK" : "")
|
||||
);
|
||||
|
||||
if ((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) {
|
||||
int frame_len = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_DATA);
|
||||
printf("RC data (PPM frame len) %u us\n", frame_len);
|
||||
|
||||
if ((frame_len - raw_inputs * 2000 - 3000) < 0) {
|
||||
|
|
@ -1861,7 +1885,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%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 +2313,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;
|
||||
|
|
@ -2656,7 +2718,7 @@ monitor(void)
|
|||
/* clear screen */
|
||||
printf("\033[2J");
|
||||
|
||||
unsigned cancels = 3;
|
||||
unsigned cancels = 2;
|
||||
|
||||
for (;;) {
|
||||
pollfd fds[1];
|
||||
|
|
@ -2932,6 +2994,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'");
|
||||
}
|
||||
|
|
|
|||
|
|
@ -355,7 +355,7 @@ l_input_rc(const struct listener *l)
|
|||
for (unsigned i = 0; (i * port_width) < rc_raw.channel_count; i++) {
|
||||
/* Channels are sent in MAVLink main loop at a fixed interval */
|
||||
mavlink_msg_rc_channels_raw_send(chan,
|
||||
rc_raw.timestamp / 1000,
|
||||
rc_raw.timestamp_publication / 1000,
|
||||
i,
|
||||
(rc_raw.channel_count > (i * port_width) + 0) ? rc_raw.values[(i * port_width) + 0] : UINT16_MAX,
|
||||
(rc_raw.channel_count > (i * port_width) + 1) ? rc_raw.values[(i * port_width) + 1] : UINT16_MAX,
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 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
|
||||
|
|
@ -59,6 +59,11 @@ static perf_counter_t c_gather_ppm;
|
|||
void
|
||||
controls_init(void)
|
||||
{
|
||||
/* no channels */
|
||||
r_raw_rc_count = 0;
|
||||
system_state.rc_channels_timestamp_received = 0;
|
||||
system_state.rc_channels_timestamp_valid = 0;
|
||||
|
||||
/* DSM input (USART1) */
|
||||
dsm_init("/dev/ttyS0");
|
||||
|
||||
|
|
@ -97,35 +102,62 @@ controls_tick() {
|
|||
/* receive signal strenght indicator (RSSI). 0 = no connection, 255: perfect connection */
|
||||
uint16_t rssi = 0;
|
||||
|
||||
#ifdef ADC_RSSI
|
||||
if (r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) {
|
||||
unsigned counts = adc_measure(ADC_RSSI);
|
||||
if (counts != 0xffff) {
|
||||
/* use 1:1 scaling on 3.3V ADC input */
|
||||
unsigned mV = counts * 3300 / 4096;
|
||||
|
||||
/* scale to 0..253 */
|
||||
rssi = mV / 13;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
perf_begin(c_gather_dsm);
|
||||
uint16_t temp_count = r_raw_rc_count;
|
||||
bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count);
|
||||
if (dsm_updated) {
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
|
||||
r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
|
||||
r_raw_rc_count = temp_count & 0x7fff;
|
||||
if (temp_count & 0x8000)
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM11;
|
||||
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
|
||||
else
|
||||
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11;
|
||||
r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
|
||||
|
||||
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
|
||||
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
|
||||
|
||||
rssi = 255;
|
||||
}
|
||||
perf_end(c_gather_dsm);
|
||||
|
||||
perf_begin(c_gather_sbus);
|
||||
bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_RC_INPUT_CHANNELS);
|
||||
|
||||
bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS);
|
||||
|
||||
bool sbus_failsafe, sbus_frame_drop;
|
||||
bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS);
|
||||
|
||||
if (sbus_updated) {
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
|
||||
}
|
||||
|
||||
/* switch S.Bus output pin as needed */
|
||||
if (sbus_status != (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)) {
|
||||
#ifdef ENABLE_SBUS_OUT
|
||||
ENABLE_SBUS_OUT((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS));
|
||||
#endif
|
||||
rssi = 255;
|
||||
|
||||
if (sbus_frame_drop) {
|
||||
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP;
|
||||
rssi = 100;
|
||||
} else {
|
||||
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
|
||||
}
|
||||
|
||||
if (sbus_failsafe) {
|
||||
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
|
||||
rssi = 0;
|
||||
} else {
|
||||
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
perf_end(c_gather_sbus);
|
||||
|
|
@ -136,13 +168,12 @@ controls_tick() {
|
|||
* disable the PPM decoder completely if we have S.bus signal.
|
||||
*/
|
||||
perf_begin(c_gather_ppm);
|
||||
bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_status[PX4IO_P_STATUS_RC_DATA]);
|
||||
bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]);
|
||||
if (ppm_updated) {
|
||||
|
||||
/* XXX sample RSSI properly here */
|
||||
rssi = 255;
|
||||
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
|
||||
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
|
||||
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
|
||||
}
|
||||
perf_end(c_gather_ppm);
|
||||
|
||||
|
|
@ -150,6 +181,9 @@ controls_tick() {
|
|||
if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS)
|
||||
r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS;
|
||||
|
||||
/* store RSSI */
|
||||
r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi;
|
||||
|
||||
/*
|
||||
* In some cases we may have received a frame, but input has still
|
||||
* been lost.
|
||||
|
|
@ -161,97 +195,100 @@ controls_tick() {
|
|||
*/
|
||||
if (dsm_updated || sbus_updated || ppm_updated) {
|
||||
|
||||
/* update RC-received timestamp */
|
||||
system_state.rc_channels_timestamp = hrt_absolute_time();
|
||||
|
||||
/* record a bitmask of channels assigned */
|
||||
unsigned assigned_channels = 0;
|
||||
|
||||
/* map raw inputs to mapped inputs */
|
||||
/* XXX mapping should be atomic relative to protocol */
|
||||
for (unsigned i = 0; i < r_raw_rc_count; i++) {
|
||||
/* update RC-received timestamp */
|
||||
system_state.rc_channels_timestamp_received = hrt_absolute_time();
|
||||
|
||||
/* map the input channel */
|
||||
uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
|
||||
/* do not command anything in failsafe, kick in the RC loss counter */
|
||||
if (!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
|
||||
|
||||
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
|
||||
/* update RC-received timestamp */
|
||||
system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received;
|
||||
|
||||
uint16_t raw = r_raw_rc_values[i];
|
||||
/* map raw inputs to mapped inputs */
|
||||
/* XXX mapping should be atomic relative to protocol */
|
||||
for (unsigned i = 0; i < r_raw_rc_count; i++) {
|
||||
|
||||
int16_t scaled;
|
||||
/* map the input channel */
|
||||
uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
|
||||
|
||||
/*
|
||||
* 1) Constrain to min/max values, as later processing depends on bounds.
|
||||
*/
|
||||
if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
|
||||
raw = conf[PX4IO_P_RC_CONFIG_MIN];
|
||||
if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
|
||||
raw = conf[PX4IO_P_RC_CONFIG_MAX];
|
||||
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
|
||||
|
||||
/*
|
||||
* 2) Scale around the mid point differently for lower and upper range.
|
||||
*
|
||||
* This is necessary as they don't share the same endpoints and slope.
|
||||
*
|
||||
* First normalize to 0..1 range with correct sign (below or above center),
|
||||
* then scale to 20000 range (if center is an actual center, -10000..10000,
|
||||
* if parameters only support half range, scale to 10000 range, e.g. if
|
||||
* center == min 0..10000, if center == max -10000..0).
|
||||
*
|
||||
* As the min and max bounds were enforced in step 1), division by zero
|
||||
* cannot occur, as for the case of center == min or center == max the if
|
||||
* statement is mutually exclusive with the arithmetic NaN case.
|
||||
*
|
||||
* DO NOT REMOVE OR ALTER STEP 1!
|
||||
*/
|
||||
if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
|
||||
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
|
||||
uint16_t raw = r_raw_rc_values[i];
|
||||
|
||||
} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
|
||||
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
|
||||
int16_t scaled;
|
||||
|
||||
} else {
|
||||
/* in the configured dead zone, output zero */
|
||||
scaled = 0;
|
||||
}
|
||||
/*
|
||||
* 1) Constrain to min/max values, as later processing depends on bounds.
|
||||
*/
|
||||
if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
|
||||
raw = conf[PX4IO_P_RC_CONFIG_MIN];
|
||||
if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
|
||||
raw = conf[PX4IO_P_RC_CONFIG_MAX];
|
||||
|
||||
/* invert channel if requested */
|
||||
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
|
||||
scaled = -scaled;
|
||||
/*
|
||||
* 2) Scale around the mid point differently for lower and upper range.
|
||||
*
|
||||
* This is necessary as they don't share the same endpoints and slope.
|
||||
*
|
||||
* First normalize to 0..1 range with correct sign (below or above center),
|
||||
* then scale to 20000 range (if center is an actual center, -10000..10000,
|
||||
* if parameters only support half range, scale to 10000 range, e.g. if
|
||||
* center == min 0..10000, if center == max -10000..0).
|
||||
*
|
||||
* As the min and max bounds were enforced in step 1), division by zero
|
||||
* cannot occur, as for the case of center == min or center == max the if
|
||||
* statement is mutually exclusive with the arithmetic NaN case.
|
||||
*
|
||||
* DO NOT REMOVE OR ALTER STEP 1!
|
||||
*/
|
||||
if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
|
||||
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
|
||||
|
||||
/* and update the scaled/mapped version */
|
||||
unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
|
||||
if (mapped < PX4IO_CONTROL_CHANNELS) {
|
||||
} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
|
||||
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
|
||||
|
||||
/* invert channel if pitch - pulling the lever down means pitching up by convention */
|
||||
if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
|
||||
} else {
|
||||
/* in the configured dead zone, output zero */
|
||||
scaled = 0;
|
||||
}
|
||||
|
||||
/* invert channel if requested */
|
||||
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
|
||||
scaled = -scaled;
|
||||
|
||||
r_rc_values[mapped] = SIGNED_TO_REG(scaled);
|
||||
assigned_channels |= (1 << mapped);
|
||||
/* and update the scaled/mapped version */
|
||||
unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
|
||||
if (mapped < PX4IO_CONTROL_CHANNELS) {
|
||||
|
||||
/* invert channel if pitch - pulling the lever down means pitching up by convention */
|
||||
if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
|
||||
scaled = -scaled;
|
||||
|
||||
r_rc_values[mapped] = SIGNED_TO_REG(scaled);
|
||||
assigned_channels |= (1 << mapped);
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* set un-assigned controls to zero */
|
||||
for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
|
||||
if (!(assigned_channels & (1 << i)))
|
||||
r_rc_values[i] = 0;
|
||||
}
|
||||
/* set un-assigned controls to zero */
|
||||
for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
|
||||
if (!(assigned_channels & (1 << i)))
|
||||
r_rc_values[i] = 0;
|
||||
}
|
||||
|
||||
/*
|
||||
* If we got an update with zero channels, treat it as
|
||||
* a loss of input.
|
||||
*
|
||||
* This might happen if a protocol-based receiver returns an update
|
||||
* that contains no channels that we have mapped.
|
||||
*/
|
||||
if (assigned_channels == 0 || rssi == 0) {
|
||||
rc_input_lost = true;
|
||||
} else {
|
||||
/* set RC OK flag */
|
||||
/* set RC OK flag, as we got an update */
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
|
||||
|
||||
/* if we have enough channels (5) to control the vehicle, the mapping is ok */
|
||||
if (assigned_channels > 4) {
|
||||
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK;
|
||||
} else {
|
||||
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
@ -264,7 +301,7 @@ controls_tick() {
|
|||
* If we haven't seen any new control data in 200ms, assume we
|
||||
* have lost input.
|
||||
*/
|
||||
if (hrt_elapsed_time(&system_state.rc_channels_timestamp) > 200000) {
|
||||
if (hrt_elapsed_time(&system_state.rc_channels_timestamp_received) > 200000) {
|
||||
rc_input_lost = true;
|
||||
|
||||
/* clear the input-kind flags here */
|
||||
|
|
@ -272,24 +309,32 @@ controls_tick() {
|
|||
PX4IO_P_STATUS_FLAGS_RC_PPM |
|
||||
PX4IO_P_STATUS_FLAGS_RC_DSM |
|
||||
PX4IO_P_STATUS_FLAGS_RC_SBUS);
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
* Handle losing RC input
|
||||
*/
|
||||
if (rc_input_lost) {
|
||||
|
||||
/* this kicks in if the receiver is gone or the system went to failsafe */
|
||||
if (rc_input_lost || (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
|
||||
/* Clear the RC input status flag, clear manual override flag */
|
||||
r_status_flags &= ~(
|
||||
PX4IO_P_STATUS_FLAGS_OVERRIDE |
|
||||
PX4IO_P_STATUS_FLAGS_RC_OK);
|
||||
|
||||
/* Mark all channels as invalid, as we just lost the RX */
|
||||
r_rc_valid = 0;
|
||||
|
||||
/* Set the RC_LOST alarm */
|
||||
r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST;
|
||||
}
|
||||
|
||||
/* Mark the arrays as empty */
|
||||
/* this kicks in if the receiver is completely gone */
|
||||
if (rc_input_lost) {
|
||||
|
||||
/* Set channel count to zero */
|
||||
r_raw_rc_count = 0;
|
||||
r_rc_valid = 0;
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
|
|||
|
|
@ -111,7 +111,6 @@
|
|||
#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */
|
||||
#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */
|
||||
#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */
|
||||
#define PX4IO_P_STATUS_FLAGS_RC_DSM11 (1 << 13) /* DSM input is 11 bit data */
|
||||
|
||||
#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */
|
||||
#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */
|
||||
|
|
@ -128,8 +127,6 @@
|
|||
#define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */
|
||||
#define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */
|
||||
#define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */
|
||||
#define PX4IO_P_STATUS_NRSSI 9 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */
|
||||
#define PX4IO_P_STATUS_RC_DATA 10 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */
|
||||
|
||||
/* array of post-mix actuator outputs, -10000..10000 */
|
||||
#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
|
|
@ -140,7 +137,17 @@
|
|||
/* array of raw RC input values, microseconds */
|
||||
#define PX4IO_PAGE_RAW_RC_INPUT 4
|
||||
#define PX4IO_P_RAW_RC_COUNT 0 /* number of valid channels */
|
||||
#define PX4IO_P_RAW_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT channels from here */
|
||||
#define PX4IO_P_RAW_RC_FLAGS 1 /* RC detail status flags */
|
||||
#define PX4IO_P_RAW_RC_FLAGS_FRAME_DROP (1 << 0) /* single frame drop */
|
||||
#define PX4IO_P_RAW_RC_FLAGS_FAILSAFE (1 << 1) /* receiver is in failsafe mode */
|
||||
#define PX4IO_P_RAW_RC_FLAGS_RC_DSM11 (1 << 2) /* DSM decoding is 11 bit mode */
|
||||
#define PX4IO_P_RAW_RC_FLAGS_MAPPING_OK (1 << 3) /* Channel mapping is ok */
|
||||
|
||||
#define PX4IO_P_RAW_RC_NRSSI 2 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */
|
||||
#define PX4IO_P_RAW_RC_DATA 3 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */
|
||||
#define PX4IO_P_RAW_FRAME_COUNT 4 /* Number of total received frames (wrapping counter) */
|
||||
#define PX4IO_P_RAW_LOST_FRAME_COUNT 5 /* Number of total dropped frames (wrapping counter) */
|
||||
#define PX4IO_P_RAW_RC_BASE 6 /* CONFIG_RC_INPUT_COUNT channels from here */
|
||||
|
||||
/* array of scaled RC input values, -10000..10000 */
|
||||
#define PX4IO_PAGE_RC_INPUT 5
|
||||
|
|
@ -157,6 +164,10 @@
|
|||
/* setup page */
|
||||
#define PX4IO_PAGE_SETUP 50
|
||||
#define PX4IO_P_SETUP_FEATURES 0
|
||||
#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0) /* enable S.Bus v1 output */
|
||||
#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /* enable S.Bus v2 output */
|
||||
#define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /* enable PWM RSSI parsing */
|
||||
#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /* enable ADC RSSI parsing */
|
||||
|
||||
#define PX4IO_P_SETUP_ARMING 1 /* arming controls */
|
||||
#define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 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
|
||||
|
|
@ -96,8 +96,9 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */
|
|||
|
||||
#define r_raw_rc_count r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT]
|
||||
#define r_raw_rc_values (&r_page_raw_rc_input[PX4IO_P_RAW_RC_BASE])
|
||||
#define r_raw_rc_flags r_page_raw_rc_input[PX4IO_P_RAW_RC_FLAGS]
|
||||
#define r_rc_valid r_page_rc_input[PX4IO_P_RC_VALID]
|
||||
#define r_rc_values (&r_page_rc_input[PX4IO_P_RAW_RC_BASE])
|
||||
#define r_rc_values (&r_page_rc_input[PX4IO_P_RC_BASE])
|
||||
|
||||
#define r_setup_features r_page_setup[PX4IO_P_SETUP_FEATURES]
|
||||
#define r_setup_arming r_page_setup[PX4IO_P_SETUP_ARMING]
|
||||
|
|
@ -115,7 +116,8 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */
|
|||
*/
|
||||
struct sys_state_s {
|
||||
|
||||
volatile uint64_t rc_channels_timestamp;
|
||||
volatile uint64_t rc_channels_timestamp_received;
|
||||
volatile uint64_t rc_channels_timestamp_valid;
|
||||
|
||||
/**
|
||||
* Last FMU receive time, in microseconds since system boot
|
||||
|
|
@ -215,7 +217,7 @@ extern int dsm_init(const char *device);
|
|||
extern bool dsm_input(uint16_t *values, uint16_t *num_values);
|
||||
extern void dsm_bind(uint16_t cmd, int pulses);
|
||||
extern int sbus_init(const char *device);
|
||||
extern bool sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels);
|
||||
extern bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels);
|
||||
|
||||
/** global debug level for isr_debug() */
|
||||
extern volatile uint8_t debug_level;
|
||||
|
|
|
|||
|
|
@ -90,8 +90,6 @@ uint16_t r_page_status[] = {
|
|||
[PX4IO_P_STATUS_VSERVO] = 0,
|
||||
[PX4IO_P_STATUS_VRSSI] = 0,
|
||||
[PX4IO_P_STATUS_PRSSI] = 0,
|
||||
[PX4IO_P_STATUS_NRSSI] = 0,
|
||||
[PX4IO_P_STATUS_RC_DATA] = 0
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -116,6 +114,12 @@ uint16_t r_page_servos[PX4IO_SERVO_COUNT];
|
|||
uint16_t r_page_raw_rc_input[] =
|
||||
{
|
||||
[PX4IO_P_RAW_RC_COUNT] = 0,
|
||||
[PX4IO_P_RAW_RC_FLAGS] = 0,
|
||||
[PX4IO_P_RAW_RC_NRSSI] = 0,
|
||||
[PX4IO_P_RAW_RC_DATA] = 0,
|
||||
[PX4IO_P_RAW_FRAME_COUNT] = 0,
|
||||
[PX4IO_P_RAW_LOST_FRAME_COUNT] = 0,
|
||||
[PX4IO_P_RAW_RC_DATA] = 0,
|
||||
[PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0
|
||||
};
|
||||
|
||||
|
|
@ -144,7 +148,12 @@ uint16_t r_page_scratch[32];
|
|||
*/
|
||||
volatile uint16_t r_page_setup[] =
|
||||
{
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
|
||||
/* default to RSSI ADC functionality */
|
||||
[PX4IO_P_SETUP_FEATURES] = PX4IO_P_SETUP_FEATURES_ADC_RSSI,
|
||||
#else
|
||||
[PX4IO_P_SETUP_FEATURES] = 0,
|
||||
#endif
|
||||
[PX4IO_P_SETUP_ARMING] = 0,
|
||||
[PX4IO_P_SETUP_PWM_RATES] = 0,
|
||||
[PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50,
|
||||
|
|
@ -162,7 +171,14 @@ volatile uint16_t r_page_setup[] =
|
|||
[PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0,
|
||||
};
|
||||
|
||||
#define PX4IO_P_SETUP_FEATURES_VALID (0)
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
|
||||
#define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | \
|
||||
PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \
|
||||
PX4IO_P_SETUP_FEATURES_ADC_RSSI | \
|
||||
PX4IO_P_SETUP_FEATURES_PWM_RSSI)
|
||||
#else
|
||||
#define PX4IO_P_SETUP_FEATURES_VALID 0
|
||||
#endif
|
||||
#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \
|
||||
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
|
||||
PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \
|
||||
|
|
@ -438,9 +454,35 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
|||
case PX4IO_P_SETUP_FEATURES:
|
||||
|
||||
value &= PX4IO_P_SETUP_FEATURES_VALID;
|
||||
r_setup_features = value;
|
||||
|
||||
/* no implemented feature selection at this point */
|
||||
/* some of the options conflict - give S.BUS out precedence, then ADC RSSI, then PWM RSSI */
|
||||
|
||||
/* switch S.Bus output pin as needed */
|
||||
#ifdef ENABLE_SBUS_OUT
|
||||
ENABLE_SBUS_OUT(value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT));
|
||||
|
||||
/* disable the conflicting options */
|
||||
if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) {
|
||||
value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | PX4IO_P_SETUP_FEATURES_ADC_RSSI);
|
||||
}
|
||||
#endif
|
||||
|
||||
/* disable the conflicting options with ADC RSSI */
|
||||
if (value & (PX4IO_P_SETUP_FEATURES_ADC_RSSI)) {
|
||||
value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI |
|
||||
PX4IO_P_SETUP_FEATURES_SBUS1_OUT |
|
||||
PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
|
||||
}
|
||||
|
||||
/* disable the conflicting options with PWM RSSI (without effect here, but for completeness) */
|
||||
if (value & (PX4IO_P_SETUP_FEATURES_PWM_RSSI)) {
|
||||
value &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI |
|
||||
PX4IO_P_SETUP_FEATURES_SBUS1_OUT |
|
||||
PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
|
||||
}
|
||||
|
||||
/* apply changes */
|
||||
r_setup_features = value;
|
||||
|
||||
break;
|
||||
|
||||
|
|
|
|||
|
|
@ -87,7 +87,7 @@ static unsigned partial_frame_count;
|
|||
|
||||
unsigned sbus_frame_drops;
|
||||
|
||||
static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels);
|
||||
static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels);
|
||||
|
||||
int
|
||||
sbus_init(const char *device)
|
||||
|
|
@ -118,7 +118,7 @@ sbus_init(const char *device)
|
|||
}
|
||||
|
||||
bool
|
||||
sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels)
|
||||
sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels)
|
||||
{
|
||||
ssize_t ret;
|
||||
hrt_abstime now;
|
||||
|
|
@ -175,7 +175,7 @@ sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_
|
|||
* decode it.
|
||||
*/
|
||||
partial_frame_count = 0;
|
||||
return sbus_decode(now, values, num_values, rssi, max_channels);
|
||||
return sbus_decode(now, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels);
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
@ -215,7 +215,7 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
|
|||
};
|
||||
|
||||
static bool
|
||||
sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_values)
|
||||
sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values)
|
||||
{
|
||||
/* check frame boundary markers to avoid out-of-sync cases */
|
||||
if ((frame[0] != 0x0f)) {
|
||||
|
|
@ -289,20 +289,22 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint
|
|||
/* decode and handle failsafe and frame-lost flags */
|
||||
if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */
|
||||
/* report that we failed to read anything valid off the receiver */
|
||||
*rssi = 0;
|
||||
return false;
|
||||
*sbus_failsafe = true;
|
||||
*sbus_frame_drop = true;
|
||||
}
|
||||
else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */
|
||||
/* set a special warning flag or try to calculate some kind of RSSI information - to be implemented
|
||||
/* set a special warning flag
|
||||
*
|
||||
* Attention! This flag indicates a skipped frame only, not a total link loss! Handling this
|
||||
* condition as fail-safe greatly reduces the reliability and range of the radio link,
|
||||
* e.g. by prematurely issueing return-to-launch!!! */
|
||||
|
||||
*rssi = 100; // XXX magic number indicating bad signal, but not a signal loss (yet)
|
||||
*sbus_failsafe = false;
|
||||
*sbus_frame_drop = true;
|
||||
} else {
|
||||
*sbus_failsafe = false;
|
||||
*sbus_frame_drop = false;
|
||||
}
|
||||
|
||||
*rssi = 255;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1276,6 +1276,9 @@ Sensors::rc_poll()
|
|||
|
||||
orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
|
||||
|
||||
if (rc_input.rc_lost)
|
||||
return;
|
||||
|
||||
struct manual_control_setpoint_s manual_control;
|
||||
struct actuator_controls_s actuator_group_3;
|
||||
|
||||
|
|
@ -1320,7 +1323,7 @@ Sensors::rc_poll()
|
|||
channel_limit = _rc_max_chan_count;
|
||||
|
||||
/* we are accepting this message */
|
||||
_rc_last_valid = rc_input.timestamp;
|
||||
_rc_last_valid = rc_input.timestamp_last_signal;
|
||||
|
||||
/* Read out values from raw message */
|
||||
for (unsigned int i = 0; i < channel_limit; i++) {
|
||||
|
|
@ -1369,9 +1372,9 @@ Sensors::rc_poll()
|
|||
}
|
||||
|
||||
_rc.chan_count = rc_input.channel_count;
|
||||
_rc.timestamp = rc_input.timestamp;
|
||||
_rc.timestamp = rc_input.timestamp_last_signal;
|
||||
|
||||
manual_control.timestamp = rc_input.timestamp;
|
||||
manual_control.timestamp = rc_input.timestamp_last_signal;
|
||||
|
||||
/* roll input - rolling right is stick-wise and rotation-wise positive */
|
||||
manual_control.roll = limit_minus_one_to_one(_rc.chan[_rc.function[ROLL]].scaled);
|
||||
|
|
|
|||
|
|
@ -121,7 +121,7 @@ int test_rc(int argc, char *argv[])
|
|||
return ERROR;
|
||||
}
|
||||
|
||||
if (hrt_absolute_time() - rc_input.timestamp > 100000) {
|
||||
if (hrt_absolute_time() - rc_input.timestamp_last_signal > 100000) {
|
||||
warnx("TIMEOUT, less than 10 Hz updates");
|
||||
(void)close(_rc_sub);
|
||||
return ERROR;
|
||||
|
|
|
|||
Loading…
Reference in New Issue