aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/drivers/drv_rc_input.h4
-rw-r--r--src/drivers/px4io/px4io.cpp51
-rw-r--r--src/drivers/stm32/drv_hrt.c84
-rw-r--r--src/modules/px4iofirmware/controls.c16
-rw-r--r--src/modules/px4iofirmware/protocol.h3
-rw-r--r--src/modules/px4iofirmware/registers.c6
-rw-r--r--src/modules/px4iofirmware/sbus.c2
-rw-r--r--src/modules/sdlog2/sdlog2.c1
-rw-r--r--src/modules/sdlog2/sdlog2_messages.h3
-rw-r--r--src/modules/sensors/sensor_params.c1
-rw-r--r--src/modules/systemlib/perf_counter.c3
-rw-r--r--src/modules/systemlib/ppm_decode.h1
-rw-r--r--src/systemcmds/tests/module.mk3
-rw-r--r--src/systemcmds/tests/test_rc.c146
-rw-r--r--src/systemcmds/tests/test_sensors.c67
-rw-r--r--src/systemcmds/tests/tests.h1
-rw-r--r--src/systemcmds/tests/tests_main.c1
17 files changed, 331 insertions, 62 deletions
diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h
index 86e5a149a..66771faaa 100644
--- a/src/drivers/drv_rc_input.h
+++ b/src/drivers/drv_rc_input.h
@@ -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,7 +89,7 @@ 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 */
+ /** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 255: full reception */
int32_t rssi;
/** Input source */
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index db882e623..010272c6f 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -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
@@ -454,11 +457,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_actuators(-1),
@@ -894,7 +900,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);
@@ -1024,7 +1046,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)
@@ -1302,6 +1329,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, &regs[prolog + 9], channel_count - 9);
@@ -1692,6 +1724,16 @@ PX4IO::print_status()
printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i));
printf("\n");
+
+ if (raw_inputs > 0) {
+ int frame_len = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_RC_DATA);
+ printf("RC data (PPM frame len) %u us\n", frame_len);
+
+ if ((frame_len - raw_inputs * 2000 - 3000) < 0) {
+ printf("WARNING WARNING WARNING! This RC receiver does not allow safe frame detection.\n");
+ }
+ }
+
uint16_t mapped_inputs = io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_VALID);
printf("mapped R/C inputs 0x%04x", mapped_inputs);
@@ -2150,7 +2192,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;
diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c
index 1bd251bc2..5bfbe04b8 100644
--- a/src/drivers/stm32/drv_hrt.c
+++ b/src/drivers/stm32/drv_hrt.c
@@ -282,6 +282,10 @@ static void hrt_call_invoke(void);
* Note that we assume that M3 means STM32F1 (since we don't really care about the F2).
*/
# ifdef CONFIG_ARCH_CORTEXM3
+# undef GTIM_CCER_CC1NP
+# undef GTIM_CCER_CC2NP
+# undef GTIM_CCER_CC3NP
+# undef GTIM_CCER_CC4NP
# define GTIM_CCER_CC1NP 0
# define GTIM_CCER_CC2NP 0
# define GTIM_CCER_CC3NP 0
@@ -332,14 +336,20 @@ static void hrt_call_invoke(void);
/*
* PPM decoder tuning parameters
*/
-# define PPM_MAX_PULSE_WIDTH 550 /* maximum width of a valid pulse */
+# define PPM_MAX_PULSE_WIDTH 700 /* maximum width of a valid pulse */
# define PPM_MIN_CHANNEL_VALUE 800 /* shortest valid channel signal */
# define PPM_MAX_CHANNEL_VALUE 2200 /* longest valid channel signal */
-# define PPM_MIN_START 2500 /* shortest valid start gap */
+# define PPM_MIN_START 2400 /* shortest valid start gap (only 2nd part of pulse) */
/* 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 4 /* should be less than the input timeout */
+
__EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS];
+__EXPORT uint16_t ppm_frame_length = 0;
__EXPORT unsigned ppm_decoded_channels = 0;
__EXPORT uint64_t ppm_last_valid_decode = 0;
@@ -357,7 +367,8 @@ static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS];
struct {
uint16_t last_edge; /* last capture time */
uint16_t last_mark; /* last significant edge */
- unsigned next_channel;
+ uint16_t frame_start; /* the frame width */
+ unsigned next_channel; /* next channel index */
enum {
UNSYNCH = 0,
ARM,
@@ -440,9 +451,8 @@ 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;
ppm_edge_history[ppm_edge_next++] = width;
@@ -455,14 +465,39 @@ 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;
+
+ 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_decoded_channels = i;
- ppm_last_valid_decode = hrt_absolute_time();
+ ppm_last_valid_decode = hrt_absolute_time();
+ }
}
/* reset for the next frame */
@@ -471,13 +506,14 @@ hrt_ppm_decode(uint32_t status)
/* next edge is the reference for the first channel */
ppm.phase = ARM;
+ ppm.last_edge = count;
return;
}
switch (ppm.phase) {
case UNSYNCH:
/* we are waiting for a start pulse - nothing useful to do here */
- return;
+ break;
case ARM:
@@ -486,14 +522,23 @@ hrt_ppm_decode(uint32_t status)
goto error; /* pulse was too long */
/* record the mark timing, expect an inactive edge */
- ppm.last_mark = count;
- ppm.phase = INACTIVE;
- return;
+ ppm.last_mark = ppm.last_edge;
+
+ /* frame length is everything including the start gap */
+ ppm_frame_length = (uint16_t)(ppm.last_edge - ppm.frame_start);
+ ppm.frame_start = ppm.last_edge;
+ ppm.phase = ACTIVE;
+ break;
case INACTIVE:
+
+ /* we expect a short pulse */
+ if (width > PPM_MAX_PULSE_WIDTH)
+ goto error; /* pulse was too long */
+
/* this edge is not interesting, but now we are ready for the next mark */
ppm.phase = ACTIVE;
- return;
+ break;
case ACTIVE:
/* determine the interval from the last mark */
@@ -514,10 +559,13 @@ hrt_ppm_decode(uint32_t status)
ppm_temp_buffer[ppm.next_channel++] = interval;
ppm.phase = INACTIVE;
- return;
+ break;
}
+ ppm.last_edge = count;
+ return;
+
/* the state machine is corrupted; reset it */
error:
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 194d8aab9..ed29c8339 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -50,7 +50,7 @@
#define RC_CHANNEL_HIGH_THRESH 5000
#define RC_CHANNEL_LOW_THRESH -5000
-static bool ppm_input(uint16_t *values, uint16_t *num_values);
+static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len);
static perf_counter_t c_gather_dsm;
static perf_counter_t c_gather_sbus;
@@ -94,7 +94,7 @@ controls_tick() {
* other. Don't do that.
*/
- /* receive signal strenght indicator (RSSI). 0 = no connection, 1000: perfect connection */
+ /* receive signal strenght indicator (RSSI). 0 = no connection, 255: perfect connection */
uint16_t rssi = 0;
perf_begin(c_gather_dsm);
@@ -108,7 +108,7 @@ controls_tick() {
else
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11;
- rssi = 1000;
+ rssi = 255;
}
perf_end(c_gather_dsm);
@@ -125,11 +125,11 @@ 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);
+ bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_status[PX4IO_P_STATUS_RC_DATA]);
if (ppm_updated) {
/* XXX sample RSSI properly here */
- rssi = 1000;
+ rssi = 255;
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
}
@@ -319,7 +319,7 @@ controls_tick() {
}
static bool
-ppm_input(uint16_t *values, uint16_t *num_values)
+ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len)
{
bool result = false;
@@ -343,6 +343,10 @@ ppm_input(uint16_t *values, uint16_t *num_values)
/* clear validity */
ppm_last_valid_decode = 0;
+ /* store PPM frame length */
+ if (num_values)
+ *frame_len = ppm_frame_length;
+
/* good if we got any channels */
result = (*num_values > 0);
}
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 11e380397..500e0ed4b 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -124,7 +124,8 @@
#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 */
+#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 */
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 3f9e111ba..6aa3a5cd2 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -89,7 +89,9 @@ uint16_t r_page_status[] = {
[PX4IO_P_STATUS_IBATT] = 0,
[PX4IO_P_STATUS_VSERVO] = 0,
[PX4IO_P_STATUS_VRSSI] = 0,
- [PX4IO_P_STATUS_PRSSI] = 0
+ [PX4IO_P_STATUS_PRSSI] = 0,
+ [PX4IO_P_STATUS_NRSSI] = 0,
+ [PX4IO_P_STATUS_RC_DATA] = 0
};
/**
@@ -114,7 +116,7 @@ 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_BASE ... (PX4IO_P_RAW_RC_BASE + 24)] = 0 // XXX ensure we have enough space to decode beefy RX, will be replaced by patch soon
+ [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0 // XXX ensure we have enough space to decode beefy RX, will be replaced by patch soon
};
/**
diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
index 388502b40..3dcfe7f5b 100644
--- a/src/modules/px4iofirmware/sbus.c
+++ b/src/modules/px4iofirmware/sbus.c
@@ -280,7 +280,7 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint
*rssi = 100; // XXX magic number indicating bad signal, but not a signal loss (yet)
}
- *rssi = 1000;
+ *rssi = 255;
return true;
}
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index bbdef3677..da7586de0 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1208,6 +1208,7 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.msg_type = LOG_RC_MSG;
/* Copy only the first 8 channels of 14 */
memcpy(log_msg.body.log_RC.channel, buf.rc.chan, sizeof(log_msg.body.log_RC.channel));
+ log_msg.body.log_RC.channel_count = buf.rc.chan_count;
LOGBUFFER_WRITE_AND_COUNT(RC);
}
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 2bbde18fc..6bfc8606e 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -159,6 +159,7 @@ struct log_STAT_s {
#define LOG_RC_MSG 11
struct log_RC_s {
float channel[8];
+ uint8_t channel_count;
};
/* --- OUT0 - ACTUATOR_0 OUTPUT --- */
@@ -291,7 +292,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT(STAT, "BBBfffBB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn,Landed"),
- LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"),
+ LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"),
LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 2aa15420a..78d4b410a 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -195,6 +195,7 @@ 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
diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c
index bf84b7945..b4ca0ed3e 100644
--- a/src/modules/systemlib/perf_counter.c
+++ b/src/modules/systemlib/perf_counter.c
@@ -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;
diff --git a/src/modules/systemlib/ppm_decode.h b/src/modules/systemlib/ppm_decode.h
index 6c5e15345..5a1ad84da 100644
--- a/src/modules/systemlib/ppm_decode.h
+++ b/src/modules/systemlib/ppm_decode.h
@@ -57,6 +57,7 @@ __BEGIN_DECLS
* PPM decoder state
*/
__EXPORT extern uint16_t ppm_buffer[PPM_MAX_CHANNELS]; /**< decoded PPM channel values */
+__EXPORT extern uint16_t ppm_frame_length; /**< length of the decoded PPM frame (includes gap) */
__EXPORT extern unsigned ppm_decoded_channels; /**< count of decoded channels */
__EXPORT extern hrt_abstime ppm_last_valid_decode; /**< timestamp of the last valid decode */
diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk
index 5d5fe50d3..68a080c77 100644
--- a/src/systemcmds/tests/module.mk
+++ b/src/systemcmds/tests/module.mk
@@ -27,4 +27,5 @@ SRCS = test_adc.c \
test_file.c \
tests_main.c \
test_param.c \
- test_ppm_loopback.c
+ test_ppm_loopback.c \
+ test_rc.c
diff --git a/src/systemcmds/tests/test_rc.c b/src/systemcmds/tests/test_rc.c
new file mode 100644
index 000000000..72619fc8b
--- /dev/null
+++ b/src/systemcmds/tests/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;
+}
diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c
index f6415b72f..096106ff3 100644
--- a/src/systemcmds/tests/test_sensors.c
+++ b/src/systemcmds/tests/test_sensors.c
@@ -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,26 +174,40 @@ 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);
+
+ return OK;
+}
+
+static int
+gyro(int argc, char *argv[])
+{
+ printf("\tGYRO: test start\n");
+ fflush(stdout);
+
+ int fd;
+ struct gyro_report buf;
+ int ret;
+
+ fd = open("/dev/gyro", O_RDONLY);
if (fd < 0) {
- printf("\tMPU6K GYRO: open fail, run <l3gd20 start> or <mpu6000 start> first.\n");
+ printf("\tGYRO: open fail, run <l3gd20 start> or <mpu6000 start> first.\n");
return ERROR;
}
@@ -200,37 +215,37 @@ mpu6k(int argc, char *argv[])
usleep(5000);
/* read data - expect samples */
- ret = read(fd, &gyro_buf, sizeof(gyro_buf));
+ ret = read(fd, &buf, sizeof(buf));
- if (ret != sizeof(gyro_buf)) {
- printf("\tMPU6K GYRO: read fail (%d)\n", ret);
+ if (ret != sizeof(buf)) {
+ printf("\tGYRO: 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);
+ printf("\tGYRO 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: MPU6K GYRO passed all tests successfully\n");
+ printf("\tOK: GYRO passed all tests successfully\n");
close(fd);
return OK;
}
static int
-gyro(int argc, char *argv[])
+gyro1(int argc, char *argv[])
{
- printf("\tGYRO: test start\n");
+ printf("\tGYRO1: test start\n");
fflush(stdout);
int fd;
struct gyro_report buf;
int ret;
- fd = open("/dev/gyro", O_RDONLY);
+ fd = open("/dev/gyro1", O_RDONLY);
if (fd < 0) {
- printf("\tGYRO: open fail, run <l3gd20 start> or <mpu6000 start> first.\n");
+ printf("\tGYRO1: open fail, run <l3gd20 start> or <mpu6000 start> first.\n");
return ERROR;
}
@@ -241,15 +256,15 @@ gyro(int argc, char *argv[])
ret = read(fd, &buf, sizeof(buf));
if (ret != sizeof(buf)) {
- printf("\tGYRO: read fail (%d)\n", ret);
+ printf("\tGYRO1: read fail (%d)\n", ret);
return ERROR;
} else {
- printf("\tGYRO rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)buf.x, (double)buf.y, (double)buf.z);
+ 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: GYRO passed all tests successfully\n");
+ printf("\tOK: GYRO1 passed all tests successfully\n");
close(fd);
return OK;
diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h
index 5cbc5ad88..a57d04be3 100644
--- a/src/systemcmds/tests/tests.h
+++ b/src/systemcmds/tests/tests.h
@@ -108,6 +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_rc(int argc, char *argv[]);
__END_DECLS
diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c
index cd998dd18..1088a4407 100644
--- a/src/systemcmds/tests/tests_main.c
+++ b/src/systemcmds/tests/tests_main.c
@@ -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},
{NULL, NULL, 0}
};
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-29 16:52:29 +0000