mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-16 17:23:35 +08:00
Merge remote-tracking branch 'upstream/control_groups' into fw_autoland_att_tecs_navigator_termination_controlgroups
Conflicts: src/systemcmds/tests/module.mk src/systemcmds/tests/tests.h
This commit is contained in:
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2012, 2013 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
|
||||
@@ -60,7 +60,7 @@
|
||||
/**
|
||||
* Maximum number of R/C input channels in the system. S.Bus has up to 18 channels.
|
||||
*/
|
||||
#define RC_INPUT_MAX_CHANNELS 18
|
||||
#define RC_INPUT_MAX_CHANNELS 20
|
||||
|
||||
/**
|
||||
* Input signal type, value is a control position from zero to 100
|
||||
@@ -89,6 +89,9 @@ struct rc_input_values {
|
||||
/** number of channels actually being seen */
|
||||
uint32_t channel_count;
|
||||
|
||||
/** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 1000: full reception */
|
||||
int32_t rssi;
|
||||
|
||||
/** Input source */
|
||||
enum RC_INPUT_SOURCE input_source;
|
||||
|
||||
|
||||
@@ -316,7 +316,7 @@ private:
|
||||
};
|
||||
|
||||
L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
|
||||
SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 8000000),
|
||||
SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */),
|
||||
_call_interval(0),
|
||||
_reports(nullptr),
|
||||
_gyro_range_scale(0.0f),
|
||||
|
||||
@@ -486,7 +486,7 @@ private:
|
||||
|
||||
|
||||
LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
|
||||
SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 8000000),
|
||||
SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within safety margins for LSM303D */),
|
||||
_mag(new LSM303D_mag(this)),
|
||||
_call_accel_interval(0),
|
||||
_call_mag_interval(0),
|
||||
|
||||
@@ -170,7 +170,7 @@
|
||||
SPI speed
|
||||
*/
|
||||
#define MPU6000_LOW_BUS_SPEED 1000*1000
|
||||
#define MPU6000_HIGH_BUS_SPEED 10*1000*1000
|
||||
#define MPU6000_HIGH_BUS_SPEED 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for MPU6K */
|
||||
|
||||
class MPU6000_gyro;
|
||||
|
||||
@@ -505,17 +505,26 @@ out:
|
||||
|
||||
void MPU6000::reset()
|
||||
{
|
||||
// if the mpu6000 is initialised after the l3gd20 and lsm303d
|
||||
// then if we don't do an irqsave/irqrestore here the mpu6000
|
||||
// frequenctly comes up in a bad state where all transfers
|
||||
// come as zero
|
||||
irqstate_t state;
|
||||
state = irqsave();
|
||||
|
||||
// Chip reset
|
||||
write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
|
||||
up_udelay(10000);
|
||||
|
||||
// Wake up device and select GyroZ clock (better performance)
|
||||
// Wake up device and select GyroZ clock. Note that the
|
||||
// MPU6000 starts up in sleep mode, and it can take some time
|
||||
// for it to come out of sleep
|
||||
write_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
|
||||
up_udelay(1000);
|
||||
|
||||
// Disable I2C bus (recommended on datasheet)
|
||||
write_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS);
|
||||
irqrestore(state);
|
||||
|
||||
up_udelay(1000);
|
||||
|
||||
// SAMPLE RATE
|
||||
|
||||
@@ -121,7 +121,7 @@ MS5611_spi_interface(ms5611::prom_u &prom_buf)
|
||||
}
|
||||
|
||||
MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) :
|
||||
SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 6*1000*1000),
|
||||
SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz */),
|
||||
_prom(prom_buf)
|
||||
{
|
||||
}
|
||||
|
||||
+72
-37
@@ -237,6 +237,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
|
||||
|
||||
volatile int _task; ///< worker task id
|
||||
volatile bool _task_should_exit; ///< worker terminate flag
|
||||
@@ -244,7 +245,9 @@ private:
|
||||
int _mavlink_fd; ///< mavlink file descriptor. This is opened by class instantiation and Doesn't appear to be usable in main thread.
|
||||
int _thread_mavlink_fd; ///< mavlink file descriptor for thread.
|
||||
|
||||
perf_counter_t _perf_update; ///< local performance counter
|
||||
perf_counter_t _perf_update; ///<local performance counter for status updates
|
||||
perf_counter_t _perf_write; ///<local performance counter for PWM control writes
|
||||
perf_counter_t _perf_chan_count; ///<local performance counter for channel number changes
|
||||
|
||||
/* cached IO state */
|
||||
uint16_t _status; ///< Various IO status flags
|
||||
@@ -462,11 +465,14 @@ PX4IO::PX4IO(device::Device *interface) :
|
||||
_max_transfer(16), /* sensible default */
|
||||
_update_interval(0),
|
||||
_rc_handling_disabled(false),
|
||||
_rc_chan_count(0),
|
||||
_task(-1),
|
||||
_task_should_exit(false),
|
||||
_mavlink_fd(-1),
|
||||
_thread_mavlink_fd(-1),
|
||||
_perf_update(perf_alloc(PC_ELAPSED, "px4io update")),
|
||||
_perf_update(perf_alloc(PC_ELAPSED, "io update")),
|
||||
_perf_write(perf_alloc(PC_ELAPSED, "io write")),
|
||||
_perf_chan_count(perf_alloc(PC_COUNT, "io rc #")),
|
||||
_status(0),
|
||||
_alarms(0),
|
||||
_t_actuator_controls_0(-1),
|
||||
@@ -917,7 +923,23 @@ PX4IO::task_main()
|
||||
|
||||
/* re-upload RC input config as it may have changed */
|
||||
io_set_rc_config();
|
||||
|
||||
/* re-set the battery scaling */
|
||||
int32_t voltage_scaling_val;
|
||||
param_t voltage_scaling_param;
|
||||
|
||||
/* set battery voltage scaling */
|
||||
param_get(voltage_scaling_param = param_find("BAT_V_SCALE_IO"), &voltage_scaling_val);
|
||||
|
||||
/* send scaling voltage to IO */
|
||||
uint16_t scaling = voltage_scaling_val;
|
||||
int pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE, &scaling, 1);
|
||||
|
||||
if (pret != OK) {
|
||||
log("voltage scaling upload failed");
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
perf_end(_perf_update);
|
||||
@@ -1101,7 +1123,12 @@ PX4IO::io_set_rc_config()
|
||||
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
|
||||
input_map[ichan - 1] = 3;
|
||||
|
||||
ichan = 4;
|
||||
param_get(param_find("RC_MAP_MODE_SW"), &ichan);
|
||||
|
||||
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
|
||||
input_map[ichan - 1] = 4;
|
||||
|
||||
ichan = 5;
|
||||
|
||||
for (unsigned i = 0; i < _max_rc_input; i++)
|
||||
if (input_map[i] == -1)
|
||||
@@ -1379,6 +1406,11 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
|
||||
*/
|
||||
channel_count = regs[0];
|
||||
|
||||
if (channel_count != _rc_chan_count)
|
||||
perf_count(_perf_chan_count);
|
||||
|
||||
_rc_chan_count = channel_count;
|
||||
|
||||
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);
|
||||
|
||||
@@ -2227,7 +2259,10 @@ PX4IO::write(file * /*filp*/, const char *buffer, size_t len)
|
||||
count = _max_actuators;
|
||||
|
||||
if (count > 0) {
|
||||
|
||||
perf_begin(_perf_write);
|
||||
int ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count);
|
||||
perf_end(_perf_write);
|
||||
|
||||
if (ret != OK)
|
||||
return ret;
|
||||
@@ -2306,7 +2341,7 @@ void
|
||||
start(int argc, char *argv[])
|
||||
{
|
||||
if (g_dev != nullptr)
|
||||
errx(1, "already loaded");
|
||||
errx(0, "already loaded");
|
||||
|
||||
/* allocate the interface */
|
||||
device::Device *interface = get_interface();
|
||||
@@ -2639,6 +2674,39 @@ px4io_main(int argc, char *argv[])
|
||||
if_test((argc > 2) ? strtol(argv[2], NULL, 0) : 0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "forceupdate")) {
|
||||
/*
|
||||
force update of the IO firmware without requiring
|
||||
the user to hold the safety switch down
|
||||
*/
|
||||
if (argc <= 3) {
|
||||
warnx("usage: px4io forceupdate MAGIC filename");
|
||||
exit(1);
|
||||
}
|
||||
if (g_dev == nullptr) {
|
||||
warnx("px4io is not started, still attempting upgrade");
|
||||
} else {
|
||||
uint16_t arg = atol(argv[2]);
|
||||
int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg);
|
||||
if (ret != OK) {
|
||||
printf("reboot failed - %d\n", ret);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
// tear down the px4io instance
|
||||
delete g_dev;
|
||||
}
|
||||
|
||||
// upload the specified firmware
|
||||
const char *fn[2];
|
||||
fn[0] = argv[3];
|
||||
fn[1] = nullptr;
|
||||
PX4IO_Uploader *up = new PX4IO_Uploader;
|
||||
up->upload(&fn[0]);
|
||||
delete up;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
/* commands below here require a started driver */
|
||||
|
||||
if (g_dev == nullptr)
|
||||
@@ -2724,39 +2792,6 @@ px4io_main(int argc, char *argv[])
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "forceupdate")) {
|
||||
/*
|
||||
force update of the IO firmware without requiring
|
||||
the user to hold the safety switch down
|
||||
*/
|
||||
if (argc <= 3) {
|
||||
printf("usage: px4io forceupdate MAGIC filename\n");
|
||||
exit(1);
|
||||
}
|
||||
if (g_dev == nullptr) {
|
||||
printf("px4io is not started\n");
|
||||
exit(1);
|
||||
}
|
||||
uint16_t arg = atol(argv[2]);
|
||||
int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg);
|
||||
if (ret != OK) {
|
||||
printf("reboot failed - %d\n", ret);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
// tear down the px4io instance
|
||||
delete g_dev;
|
||||
|
||||
// upload the specified firmware
|
||||
const char *fn[2];
|
||||
fn[0] = argv[3];
|
||||
fn[1] = nullptr;
|
||||
PX4IO_Uploader *up = new PX4IO_Uploader;
|
||||
up->upload(&fn[0]);
|
||||
delete up;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "checkcrc")) {
|
||||
/*
|
||||
check IO CRC against CRC of a file
|
||||
|
||||
@@ -338,7 +338,12 @@ static void hrt_call_invoke(void);
|
||||
# define PPM_MIN_START 2500 /* shortest valid start gap */
|
||||
|
||||
/* decoded PPM buffer */
|
||||
#define PPM_MAX_CHANNELS 12
|
||||
#define PPM_MIN_CHANNELS 5
|
||||
#define PPM_MAX_CHANNELS 20
|
||||
|
||||
/* Number of same-sized frames required to 'lock' */
|
||||
#define PPM_CHANNEL_LOCK 2 /* should be less than the input timeout */
|
||||
|
||||
__EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS];
|
||||
__EXPORT unsigned ppm_decoded_channels = 0;
|
||||
__EXPORT uint64_t ppm_last_valid_decode = 0;
|
||||
@@ -440,7 +445,7 @@ hrt_ppm_decode(uint32_t status)
|
||||
if (status & SR_OVF_PPM)
|
||||
goto error;
|
||||
|
||||
/* how long since the last edge? */
|
||||
/* how long since the last edge? - this handles counter wrapping implicitely. */
|
||||
width = count - ppm.last_edge;
|
||||
ppm.last_edge = count;
|
||||
|
||||
@@ -455,14 +460,38 @@ hrt_ppm_decode(uint32_t status)
|
||||
*/
|
||||
if (width >= PPM_MIN_START) {
|
||||
|
||||
/* export the last set of samples if we got something sensible */
|
||||
if (ppm.next_channel > 4) {
|
||||
for (i = 0; i < ppm.next_channel && i < PPM_MAX_CHANNELS; i++)
|
||||
ppm_buffer[i] = ppm_temp_buffer[i];
|
||||
/*
|
||||
* If the number of channels changes unexpectedly, we don't want
|
||||
* to just immediately jump on the new count as it may be a result
|
||||
* of noise or dropped edges. Instead, take a few frames to settle.
|
||||
*/
|
||||
if (ppm.next_channel != ppm_decoded_channels) {
|
||||
static unsigned new_channel_count;
|
||||
static unsigned new_channel_holdoff;
|
||||
|
||||
ppm_decoded_channels = i;
|
||||
ppm_last_valid_decode = hrt_absolute_time();
|
||||
if (new_channel_count != ppm.next_channel) {
|
||||
/* start the lock counter for the new channel count */
|
||||
new_channel_count = ppm.next_channel;
|
||||
new_channel_holdoff = PPM_CHANNEL_LOCK;
|
||||
|
||||
} else if (new_channel_holdoff > 0) {
|
||||
/* this frame matched the last one, decrement the lock counter */
|
||||
new_channel_holdoff--;
|
||||
|
||||
} else {
|
||||
/* we have seen PPM_CHANNEL_LOCK frames with the new count, accept it */
|
||||
ppm_decoded_channels = new_channel_count;
|
||||
new_channel_count = 0;
|
||||
}
|
||||
|
||||
} else {
|
||||
/* frame channel count matches expected, let's use it */
|
||||
if (ppm.next_channel > PPM_MIN_CHANNELS) {
|
||||
for (i = 0; i < ppm.next_channel; i++)
|
||||
ppm_buffer[i] = ppm_temp_buffer[i];
|
||||
|
||||
ppm_last_valid_decode = hrt_absolute_time();
|
||||
}
|
||||
}
|
||||
|
||||
/* reset for the next frame */
|
||||
|
||||
@@ -1624,8 +1624,8 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c
|
||||
/* switch to failsafe mode */
|
||||
bool manual_control_old = control_mode->flag_control_manual_enabled;
|
||||
|
||||
if (!status->condition_landed) {
|
||||
/* in air: try to hold position */
|
||||
if (!status->condition_landed && status->condition_local_position_valid) {
|
||||
/* in air: try to hold position if possible */
|
||||
res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode);
|
||||
|
||||
} else {
|
||||
|
||||
@@ -66,7 +66,7 @@ controls_init(void)
|
||||
sbus_init("/dev/ttyS2");
|
||||
|
||||
/* default to a 1:1 input map, all enabled */
|
||||
for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
|
||||
for (unsigned i = 0; i < PX4IO_RC_INPUT_CHANNELS; i++) {
|
||||
unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
|
||||
|
||||
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = 0;
|
||||
@@ -94,6 +94,9 @@ controls_tick() {
|
||||
* other. Don't do that.
|
||||
*/
|
||||
|
||||
/* receive signal strenght indicator (RSSI). 0 = no connection, 1000: perfect connection */
|
||||
uint16_t rssi = 0;
|
||||
|
||||
perf_begin(c_gather_dsm);
|
||||
uint16_t temp_count = r_raw_rc_count;
|
||||
bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count);
|
||||
@@ -104,14 +107,15 @@ controls_tick() {
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM11;
|
||||
else
|
||||
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11;
|
||||
|
||||
rssi = 1000;
|
||||
}
|
||||
perf_end(c_gather_dsm);
|
||||
|
||||
perf_begin(c_gather_sbus);
|
||||
bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, PX4IO_CONTROL_CHANNELS /* XXX this should be INPUT channels, once untangled */);
|
||||
bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_RC_INPUT_CHANNELS);
|
||||
if (sbus_updated) {
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
|
||||
r_raw_rc_count = 8;
|
||||
}
|
||||
perf_end(c_gather_sbus);
|
||||
|
||||
@@ -122,10 +126,19 @@ controls_tick() {
|
||||
*/
|
||||
perf_begin(c_gather_ppm);
|
||||
bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count);
|
||||
if (ppm_updated)
|
||||
if (ppm_updated) {
|
||||
|
||||
/* XXX sample RSSI properly here */
|
||||
rssi = 1000;
|
||||
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
|
||||
}
|
||||
perf_end(c_gather_ppm);
|
||||
|
||||
/* limit number of channels to allowable data size */
|
||||
if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS)
|
||||
r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS;
|
||||
|
||||
/*
|
||||
* In some cases we may have received a frame, but input has still
|
||||
* been lost.
|
||||
@@ -197,14 +210,16 @@ controls_tick() {
|
||||
|
||||
/* and update the scaled/mapped version */
|
||||
unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
|
||||
ASSERT(mapped < PX4IO_CONTROL_CHANNELS);
|
||||
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;
|
||||
/* 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);
|
||||
r_rc_values[mapped] = SIGNED_TO_REG(scaled);
|
||||
assigned_channels |= (1 << mapped);
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -221,7 +236,7 @@ controls_tick() {
|
||||
* This might happen if a protocol-based receiver returns an update
|
||||
* that contains no channels that we have mapped.
|
||||
*/
|
||||
if (assigned_channels == 0) {
|
||||
if (assigned_channels == 0 || rssi == 0) {
|
||||
rc_input_lost = true;
|
||||
} else {
|
||||
/* set RC OK flag */
|
||||
@@ -321,8 +336,8 @@ ppm_input(uint16_t *values, uint16_t *num_values)
|
||||
|
||||
/* PPM data exists, copy it */
|
||||
*num_values = ppm_decoded_channels;
|
||||
if (*num_values > PX4IO_CONTROL_CHANNELS)
|
||||
*num_values = PX4IO_CONTROL_CHANNELS;
|
||||
if (*num_values > PX4IO_RC_INPUT_CHANNELS)
|
||||
*num_values = PX4IO_RC_INPUT_CHANNELS;
|
||||
|
||||
for (unsigned i = 0; i < *num_values; i++)
|
||||
values[i] = ppm_buffer[i];
|
||||
|
||||
@@ -128,6 +128,7 @@
|
||||
#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, 1000: perfect reception */
|
||||
|
||||
/* array of post-mix actuator outputs, -10000..10000 */
|
||||
#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
|
||||
@@ -54,7 +54,7 @@
|
||||
#define PX4IO_SERVO_COUNT 8
|
||||
#define PX4IO_CONTROL_CHANNELS 8
|
||||
#define PX4IO_CONTROL_GROUPS 2
|
||||
#define PX4IO_RC_INPUT_CHANNELS 8 // XXX this should be 18 channels
|
||||
#define PX4IO_RC_INPUT_CHANNELS 18
|
||||
#define PX4IO_RC_MAPPED_CONTROL_CHANNELS 8 /**< This is the maximum number of channels mapped/used */
|
||||
|
||||
/*
|
||||
@@ -213,7 +213,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 max_channels);
|
||||
extern bool sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels);
|
||||
|
||||
/** global debug level for isr_debug() */
|
||||
extern volatile uint8_t debug_level;
|
||||
|
||||
@@ -54,6 +54,27 @@
|
||||
|
||||
#define SBUS_FRAME_SIZE 25
|
||||
#define SBUS_INPUT_CHANNELS 16
|
||||
#define SBUS_FLAGS_BYTE 23
|
||||
#define SBUS_FAILSAFE_BIT 3
|
||||
#define SBUS_FRAMELOST_BIT 2
|
||||
|
||||
/*
|
||||
Measured values with Futaba FX-30/R6108SB:
|
||||
-+100% on TX: PCM 1.100/1.520/1.950ms -> SBus raw values: 350/1024/1700 (100% ATV)
|
||||
-+140% on TX: PCM 0.930/1.520/2.112ms -> SBus raw values: 78/1024/1964 (140% ATV)
|
||||
-+152% on TX: PCM 0.884/1.520/2.160ms -> SBus raw values: 1/1024/2047 (140% ATV plus dirty tricks)
|
||||
*/
|
||||
|
||||
/* 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))
|
||||
|
||||
static int sbus_fd = -1;
|
||||
|
||||
@@ -66,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 max_channels);
|
||||
static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels);
|
||||
|
||||
int
|
||||
sbus_init(const char *device)
|
||||
@@ -97,7 +118,7 @@ sbus_init(const char *device)
|
||||
}
|
||||
|
||||
bool
|
||||
sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels)
|
||||
sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels)
|
||||
{
|
||||
ssize_t ret;
|
||||
hrt_abstime now;
|
||||
@@ -154,7 +175,7 @@ sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels)
|
||||
* decode it.
|
||||
*/
|
||||
partial_frame_count = 0;
|
||||
return sbus_decode(now, values, num_values, max_channels);
|
||||
return sbus_decode(now, values, num_values, rssi, max_channels);
|
||||
}
|
||||
|
||||
/*
|
||||
@@ -194,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 max_values)
|
||||
sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_values)
|
||||
{
|
||||
/* check frame boundary markers to avoid out-of-sync cases */
|
||||
if ((frame[0] != 0x0f) || (frame[24] != 0x00)) {
|
||||
@@ -202,15 +223,6 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint
|
||||
return false;
|
||||
}
|
||||
|
||||
/* if the failsafe or connection lost bit is set, we consider the frame invalid */
|
||||
if ((frame[23] & (1 << 2)) && /* signal lost */
|
||||
(frame[23] & (1 << 3))) { /* failsafe */
|
||||
|
||||
/* actively announce signal loss */
|
||||
*values = 0;
|
||||
return false;
|
||||
}
|
||||
|
||||
/* we have received something we think is a frame */
|
||||
last_frame_time = frame_time;
|
||||
|
||||
@@ -234,8 +246,9 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint
|
||||
}
|
||||
}
|
||||
|
||||
/* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
|
||||
values[channel] = (value / 2) + 998;
|
||||
|
||||
/* convert 0-2048 values to 1000-2000 ppm encoding in a not too sloppy fashion */
|
||||
values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR +.5f) + SBUS_SCALE_OFFSET;
|
||||
}
|
||||
|
||||
/* decode switch channels if data fields are wide enough */
|
||||
@@ -243,13 +256,31 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint
|
||||
chancount = 18;
|
||||
|
||||
/* channel 17 (index 16) */
|
||||
values[16] = (frame[23] & (1 << 0)) * 1000 + 998;
|
||||
values[16] = (frame[SBUS_FLAGS_BYTE] & (1 << 0)) * 1000 + 998;
|
||||
/* channel 18 (index 17) */
|
||||
values[17] = (frame[23] & (1 << 1)) * 1000 + 998;
|
||||
values[17] = (frame[SBUS_FLAGS_BYTE] & (1 << 1)) * 1000 + 998;
|
||||
}
|
||||
|
||||
/* note the number of channels decoded */
|
||||
*num_values = chancount;
|
||||
|
||||
/* 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;
|
||||
}
|
||||
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
|
||||
*
|
||||
* 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)
|
||||
}
|
||||
|
||||
*rssi = 1000;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
@@ -190,11 +190,30 @@ PARAM_DEFINE_FLOAT(RC15_MAX, 2000);
|
||||
PARAM_DEFINE_FLOAT(RC15_REV, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(RC15_DZ, 0.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(RC16_MIN, 1000);
|
||||
PARAM_DEFINE_FLOAT(RC16_TRIM, 1500);
|
||||
PARAM_DEFINE_FLOAT(RC16_MAX, 2000);
|
||||
PARAM_DEFINE_FLOAT(RC16_REV, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(RC16_DZ, 0.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(RC17_MIN, 1000);
|
||||
PARAM_DEFINE_FLOAT(RC17_TRIM, 1500);
|
||||
PARAM_DEFINE_FLOAT(RC17_MAX, 2000);
|
||||
PARAM_DEFINE_FLOAT(RC17_REV, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(RC17_DZ, 0.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(RC18_MIN, 1000);
|
||||
PARAM_DEFINE_FLOAT(RC18_TRIM, 1500);
|
||||
PARAM_DEFINE_FLOAT(RC18_MAX, 2000);
|
||||
PARAM_DEFINE_FLOAT(RC18_REV, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(RC18_DZ, 0.0f);
|
||||
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
|
||||
PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */
|
||||
#endif
|
||||
PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /* -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind */
|
||||
|
||||
PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000);
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
|
||||
PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
|
||||
#else
|
||||
@@ -226,3 +245,7 @@ PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /**< default function: camera azimuth / yaw
|
||||
PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f);
|
||||
PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f);
|
||||
PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f);
|
||||
|
||||
PARAM_DEFINE_INT32(RC_FS_CH, 0); /**< RC failsafe channel, 0 = disable */
|
||||
PARAM_DEFINE_INT32(RC_FS_MODE, 0); /**< RC failsafe mode: 0 = too low means signal loss, 1 = too high means signal loss */
|
||||
PARAM_DEFINE_FLOAT(RC_FS_THR, 800); /**< RC failsafe PWM threshold */
|
||||
|
||||
@@ -165,7 +165,7 @@ public:
|
||||
int start();
|
||||
|
||||
private:
|
||||
static const unsigned _rc_max_chan_count = RC_CHANNELS_MAX; /**< maximum number of r/c channels we handle */
|
||||
static const unsigned _rc_max_chan_count = RC_INPUT_MAX_CHANNELS; /**< maximum number of r/c channels we handle */
|
||||
|
||||
hrt_abstime _rc_last_valid; /**< last time we got a valid RC signal */
|
||||
|
||||
@@ -260,6 +260,10 @@ private:
|
||||
float rc_scale_yaw;
|
||||
float rc_scale_flaps;
|
||||
|
||||
int rc_fs_ch;
|
||||
int rc_fs_mode;
|
||||
float rc_fs_thr;
|
||||
|
||||
float battery_voltage_scaling;
|
||||
|
||||
} _parameters; /**< local copies of interesting parameters */
|
||||
@@ -305,6 +309,10 @@ private:
|
||||
param_t rc_scale_yaw;
|
||||
param_t rc_scale_flaps;
|
||||
|
||||
param_t rc_fs_ch;
|
||||
param_t rc_fs_mode;
|
||||
param_t rc_fs_thr;
|
||||
|
||||
param_t battery_voltage_scaling;
|
||||
|
||||
param_t board_rotation;
|
||||
@@ -517,6 +525,11 @@ Sensors::Sensors() :
|
||||
_parameter_handles.rc_scale_yaw = param_find("RC_SCALE_YAW");
|
||||
_parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS");
|
||||
|
||||
/* RC failsafe */
|
||||
_parameter_handles.rc_fs_ch = param_find("RC_FS_CH");
|
||||
_parameter_handles.rc_fs_mode = param_find("RC_FS_MODE");
|
||||
_parameter_handles.rc_fs_thr = param_find("RC_FS_THR");
|
||||
|
||||
/* gyro offsets */
|
||||
_parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF");
|
||||
_parameter_handles.gyro_offset[1] = param_find("SENS_GYRO_YOFF");
|
||||
@@ -589,7 +602,7 @@ Sensors::parameters_update()
|
||||
float tmpRevFactor = 0.0f;
|
||||
|
||||
/* rc values */
|
||||
for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) {
|
||||
for (unsigned int i = 0; i < _rc_max_chan_count; i++) {
|
||||
|
||||
param_get(_parameter_handles.min[i], &(_parameters.min[i]));
|
||||
param_get(_parameter_handles.trim[i], &(_parameters.trim[i]));
|
||||
@@ -668,6 +681,9 @@ Sensors::parameters_update()
|
||||
param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch));
|
||||
param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw));
|
||||
param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps));
|
||||
param_get(_parameter_handles.rc_fs_ch, &(_parameters.rc_fs_ch));
|
||||
param_get(_parameter_handles.rc_fs_mode, &(_parameters.rc_fs_mode));
|
||||
param_get(_parameter_handles.rc_fs_thr, &(_parameters.rc_fs_thr));
|
||||
|
||||
/* update RC function mappings */
|
||||
_rc.function[THROTTLE] = _parameters.rc_map_throttle - 1;
|
||||
@@ -1256,6 +1272,18 @@ Sensors::rc_poll()
|
||||
if (rc_input.channel_count < 4)
|
||||
return;
|
||||
|
||||
/* failsafe check */
|
||||
if (_parameters.rc_fs_ch != 0) {
|
||||
if (_parameters.rc_fs_mode == 0) {
|
||||
if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr)
|
||||
return;
|
||||
|
||||
} else if (_parameters.rc_fs_mode == 1) {
|
||||
if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr)
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
unsigned channel_limit = rc_input.channel_count;
|
||||
|
||||
if (channel_limit > _rc_max_chan_count)
|
||||
|
||||
@@ -295,10 +295,11 @@ perf_print_counter(perf_counter_t handle)
|
||||
case PC_ELAPSED: {
|
||||
struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
|
||||
|
||||
printf("%s: %llu events, %lluus elapsed, min %lluus max %lluus\n",
|
||||
printf("%s: %llu events, %lluus elapsed, %llu avg, min %lluus max %lluus\n",
|
||||
handle->name,
|
||||
pce->event_count,
|
||||
pce->time_total,
|
||||
pce->time_total / pce->event_count,
|
||||
pce->time_least,
|
||||
pce->time_most);
|
||||
break;
|
||||
|
||||
@@ -45,7 +45,7 @@
|
||||
#include <systemlib/rc_check.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
#include <uORB/topics/rc_channels.h>
|
||||
#include <drivers/drv_rc_input.h>
|
||||
|
||||
int rc_calibration_check(int mavlink_fd) {
|
||||
|
||||
@@ -66,7 +66,7 @@ int rc_calibration_check(int mavlink_fd) {
|
||||
|
||||
int channel_fail_count = 0;
|
||||
|
||||
for (int i = 0; i < RC_CHANNELS_MAX; i++) {
|
||||
for (int i = 0; i < RC_INPUT_MAX_CHANNELS; i++) {
|
||||
/* should the channel be enabled? */
|
||||
uint8_t count = 0;
|
||||
|
||||
|
||||
@@ -1,9 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Nils Wenzler <wenzlern@student.ethz.ch>
|
||||
* @author Ivan Ovinnikov <oivan@student.ethz.ch>
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
* Copyright (c) 2012, 2013 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
|
||||
@@ -47,13 +44,13 @@
|
||||
|
||||
/**
|
||||
* The number of RC channel inputs supported.
|
||||
* Current (Q1/2013) radios support up to 18 channels,
|
||||
* Current (Q4/2013) radios support up to 18 channels,
|
||||
* leaving at a sane value of 15.
|
||||
* This number can be greater then number of RC channels,
|
||||
* because single RC channel can be mapped to multiple
|
||||
* functions, e.g. for various mode switches.
|
||||
*/
|
||||
#define RC_CHANNELS_MAX 15
|
||||
#define RC_CHANNELS_MAPPED_MAX 15
|
||||
|
||||
/**
|
||||
* This defines the mapping of the RC functions.
|
||||
@@ -91,7 +88,7 @@ struct rc_channels_s {
|
||||
uint64_t timestamp_last_valid; /**< timestamp of last valid RC signal. */
|
||||
struct {
|
||||
float scaled; /**< Scaled to -1..1 (throttle: 0..1) */
|
||||
} chan[RC_CHANNELS_MAX];
|
||||
} chan[RC_CHANNELS_MAPPED_MAX];
|
||||
uint8_t chan_count; /**< number of valid channels */
|
||||
|
||||
/*String array to store the names of the functions*/
|
||||
|
||||
@@ -200,7 +200,7 @@ system_eval:
|
||||
led_toggle(leds, LED_BLUE);
|
||||
|
||||
/* display and sound error */
|
||||
for (int i = 0; i < 50; i++)
|
||||
for (int i = 0; i < 14; i++)
|
||||
{
|
||||
led_toggle(leds, LED_BLUE);
|
||||
led_toggle(leds, LED_AMBER);
|
||||
|
||||
@@ -24,10 +24,8 @@ SRCS = test_adc.c \
|
||||
test_uart_loopback.c \
|
||||
test_uart_send.c \
|
||||
test_mixer.cpp \
|
||||
test_dataman.c \
|
||||
test_file.c \
|
||||
tests_main.c \
|
||||
test_param.c \
|
||||
test_ppm_loopback.c
|
||||
|
||||
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
|
||||
test_ppm_loopback.c \
|
||||
test_rc.c
|
||||
|
||||
@@ -0,0 +1,146 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012, 2013 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 test_ppm_loopback.c
|
||||
* Tests the PWM outputs and PPM input
|
||||
*
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <sys/types.h>
|
||||
|
||||
#include <stdio.h>
|
||||
#include <poll.h>
|
||||
#include <stdlib.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
#include <debug.h>
|
||||
|
||||
#include <arch/board/board.h>
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
#include <drivers/drv_rc_input.h>
|
||||
#include <systemlib/err.h>
|
||||
|
||||
#include "tests.h"
|
||||
|
||||
#include <math.h>
|
||||
#include <float.h>
|
||||
|
||||
int test_rc(int argc, char *argv[])
|
||||
{
|
||||
|
||||
int _rc_sub = orb_subscribe(ORB_ID(input_rc));
|
||||
|
||||
/* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */
|
||||
struct rc_input_values rc_input;
|
||||
struct rc_input_values rc_last;
|
||||
orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
|
||||
usleep(100000);
|
||||
|
||||
/* open PPM input and expect values close to the output values */
|
||||
|
||||
bool rc_updated;
|
||||
orb_check(_rc_sub, &rc_updated);
|
||||
|
||||
warnx("Reading PPM values - press any key to abort");
|
||||
warnx("This test guarantees: 10 Hz update rates, no glitches (channel values), no channel count changes.");
|
||||
|
||||
if (rc_updated) {
|
||||
|
||||
/* copy initial set */
|
||||
for (unsigned i = 0; i < rc_input.channel_count; i++) {
|
||||
rc_last.values[i] = rc_input.values[i];
|
||||
}
|
||||
|
||||
rc_last.channel_count = rc_input.channel_count;
|
||||
|
||||
/* poll descriptor */
|
||||
struct pollfd fds[2];
|
||||
fds[0].fd = _rc_sub;
|
||||
fds[0].events = POLLIN;
|
||||
fds[1].fd = 0;
|
||||
fds[1].events = POLLIN;
|
||||
|
||||
while (true) {
|
||||
|
||||
int ret = poll(fds, 2, 200);
|
||||
|
||||
if (ret > 0) {
|
||||
|
||||
if (fds[0].revents & POLLIN) {
|
||||
|
||||
orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
|
||||
|
||||
/* go and check values */
|
||||
for (unsigned i = 0; i < rc_input.channel_count; i++) {
|
||||
if (fabsf(rc_input.values[i] - rc_last.values[i]) > 20) {
|
||||
warnx("comparison fail: RC: %d, expected: %d", rc_input.values[i], rc_last.values[i]);
|
||||
(void)close(_rc_sub);
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
rc_last.values[i] = rc_input.values[i];
|
||||
}
|
||||
|
||||
if (rc_last.channel_count != rc_input.channel_count) {
|
||||
warnx("channel count mismatch: last: %d, now: %d", rc_last.channel_count, rc_input.channel_count);
|
||||
(void)close(_rc_sub);
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
if (hrt_absolute_time() - rc_input.timestamp > 100000) {
|
||||
warnx("TIMEOUT, less than 10 Hz updates");
|
||||
(void)close(_rc_sub);
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
} else {
|
||||
/* key pressed, bye bye */
|
||||
return 0;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
warnx("failed reading RC input data");
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
warnx("PPM CONTINUITY TEST PASSED SUCCESSFULLY!");
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -78,7 +78,8 @@ static int accel(int argc, char *argv[]);
|
||||
static int gyro(int argc, char *argv[]);
|
||||
static int mag(int argc, char *argv[]);
|
||||
static int baro(int argc, char *argv[]);
|
||||
static int mpu6k(int argc, char *argv[]);
|
||||
static int accel1(int argc, char *argv[]);
|
||||
static int gyro1(int argc, char *argv[]);
|
||||
|
||||
/****************************************************************************
|
||||
* Private Data
|
||||
@@ -93,7 +94,8 @@ struct {
|
||||
{"gyro", "/dev/gyro", gyro},
|
||||
{"mag", "/dev/mag", mag},
|
||||
{"baro", "/dev/baro", baro},
|
||||
{"mpu6k", "/dev/mpu6k", mpu6k},
|
||||
{"accel1", "/dev/accel1", accel1},
|
||||
{"gyro1", "/dev/gyro1", gyro1},
|
||||
{NULL, NULL, NULL}
|
||||
};
|
||||
|
||||
@@ -137,7 +139,7 @@ accel(int argc, char *argv[])
|
||||
}
|
||||
|
||||
if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) {
|
||||
warnx("MPU6K acceleration values out of range!");
|
||||
warnx("ACCEL1 acceleration values out of range!");
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
@@ -149,20 +151,19 @@ accel(int argc, char *argv[])
|
||||
}
|
||||
|
||||
static int
|
||||
mpu6k(int argc, char *argv[])
|
||||
accel1(int argc, char *argv[])
|
||||
{
|
||||
printf("\tMPU6K: test start\n");
|
||||
printf("\tACCEL1: test start\n");
|
||||
fflush(stdout);
|
||||
|
||||
int fd;
|
||||
struct accel_report buf;
|
||||
struct gyro_report gyro_buf;
|
||||
int ret;
|
||||
|
||||
fd = open("/dev/accel_mpu6k", O_RDONLY);
|
||||
fd = open("/dev/accel1", O_RDONLY);
|
||||
|
||||
if (fd < 0) {
|
||||
printf("\tMPU6K: open fail, run <mpu6000 start> first.\n");
|
||||
printf("\tACCEL1: open fail, run <mpu6000 start> or <lsm303d start> first.\n");
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
@@ -173,45 +174,21 @@ mpu6k(int argc, char *argv[])
|
||||
ret = read(fd, &buf, sizeof(buf));
|
||||
|
||||
if (ret != sizeof(buf)) {
|
||||
printf("\tMPU6K: read1 fail (%d)\n", ret);
|
||||
printf("\tACCEL1: read1 fail (%d)\n", ret);
|
||||
return ERROR;
|
||||
|
||||
} else {
|
||||
printf("\tMPU6K accel: x:%8.4f\ty:%8.4f\tz:%8.4f m/s^2\n", (double)buf.x, (double)buf.y, (double)buf.z);
|
||||
printf("\tACCEL1 accel: x:%8.4f\ty:%8.4f\tz:%8.4f m/s^2\n", (double)buf.x, (double)buf.y, (double)buf.z);
|
||||
}
|
||||
|
||||
if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) {
|
||||
warnx("MPU6K acceleration values out of range!");
|
||||
warnx("ACCEL1 acceleration values out of range!");
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
/* Let user know everything is ok */
|
||||
printf("\tOK: MPU6K ACCEL passed all tests successfully\n");
|
||||
printf("\tOK: ACCEL1 passed all tests successfully\n");
|
||||
|
||||
close(fd);
|
||||
fd = open("/dev/gyro_mpu6k", O_RDONLY);
|
||||
|
||||
if (fd < 0) {
|
||||
printf("\tMPU6K GYRO: open fail, run <l3gd20 start> or <mpu6000 start> first.\n");
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
/* wait at least 5 ms, sensor should have data after that */
|
||||
usleep(5000);
|
||||
|
||||
/* read data - expect samples */
|
||||
ret = read(fd, &gyro_buf, sizeof(gyro_buf));
|
||||
|
||||
if (ret != sizeof(gyro_buf)) {
|
||||
printf("\tMPU6K GYRO: read fail (%d)\n", ret);
|
||||
return ERROR;
|
||||
|
||||
} else {
|
||||
printf("\tMPU6K GYRO rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)gyro_buf.x, (double)gyro_buf.y, (double)gyro_buf.z);
|
||||
}
|
||||
|
||||
/* Let user know everything is ok */
|
||||
printf("\tOK: MPU6K GYRO passed all tests successfully\n");
|
||||
close(fd);
|
||||
|
||||
return OK;
|
||||
@@ -255,6 +232,44 @@ gyro(int argc, char *argv[])
|
||||
return OK;
|
||||
}
|
||||
|
||||
static int
|
||||
gyro1(int argc, char *argv[])
|
||||
{
|
||||
printf("\tGYRO1: test start\n");
|
||||
fflush(stdout);
|
||||
|
||||
int fd;
|
||||
struct gyro_report buf;
|
||||
int ret;
|
||||
|
||||
fd = open("/dev/gyro1", O_RDONLY);
|
||||
|
||||
if (fd < 0) {
|
||||
printf("\tGYRO1: open fail, run <l3gd20 start> or <mpu6000 start> first.\n");
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
/* wait at least 5 ms, sensor should have data after that */
|
||||
usleep(5000);
|
||||
|
||||
/* read data - expect samples */
|
||||
ret = read(fd, &buf, sizeof(buf));
|
||||
|
||||
if (ret != sizeof(buf)) {
|
||||
printf("\tGYRO1: read fail (%d)\n", ret);
|
||||
return ERROR;
|
||||
|
||||
} else {
|
||||
printf("\tGYRO1 rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)buf.x, (double)buf.y, (double)buf.z);
|
||||
}
|
||||
|
||||
/* Let user know everything is ok */
|
||||
printf("\tOK: GYRO1 passed all tests successfully\n");
|
||||
close(fd);
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
static int
|
||||
mag(int argc, char *argv[])
|
||||
{
|
||||
|
||||
@@ -108,7 +108,7 @@ extern int test_param(int argc, char *argv[]);
|
||||
extern int test_bson(int argc, char *argv[]);
|
||||
extern int test_file(int argc, char *argv[]);
|
||||
extern int test_mixer(int argc, char *argv[]);
|
||||
extern int test_dataman(int argc, char *argv[]);
|
||||
extern int test_rc(int argc, char *argv[]);
|
||||
|
||||
__END_DECLS
|
||||
|
||||
|
||||
@@ -105,6 +105,7 @@ const struct {
|
||||
{"bson", test_bson, 0},
|
||||
{"file", test_file, 0},
|
||||
{"mixer", test_mixer, OPT_NOJIGTEST | OPT_NOALLTEST},
|
||||
{"rc", test_rc, OPT_NOJIGTEST | OPT_NOALLTEST},
|
||||
{"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST},
|
||||
{"dataman", test_dataman, OPT_NOALLTEST},
|
||||
{NULL, NULL, 0}
|
||||
|
||||
Reference in New Issue
Block a user