diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-12-23 18:16:28 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-12-23 18:16:28 +0400 |
commit | 5a2da77758867914e3e8b55267df56a6f95913ed (patch) | |
tree | 1c016e8f708ab68a9c7762eae436645dcacedae0 | |
parent | c9f785ff7f64960b79129e4e79fd0db4863192ae (diff) | |
parent | 1279b5fbf3838811a08845e41b1e1244fb85d026 (diff) | |
download | px4-firmware-5a2da77758867914e3e8b55267df56a6f95913ed.tar.gz px4-firmware-5a2da77758867914e3e8b55267df56a6f95913ed.tar.bz2 px4-firmware-5a2da77758867914e3e8b55267df56a6f95913ed.zip |
Merge branch 'master' into mathlib_new
-rw-r--r-- | ROMFS/px4fmu_common/init.d/rc.sensors | 2 | ||||
-rw-r--r--[-rwxr-xr-x] | ROMFS/px4fmu_common/init.d/rcS | 0 | ||||
-rw-r--r-- | ROMFS/px4fmu_logging/init.d/rcS | 88 | ||||
-rw-r--r--[-rwxr-xr-x] | ROMFS/px4fmu_test/init.d/rcS | 8 | ||||
-rw-r--r-- | makefiles/config_px4fmu-v2_logging.mk | 157 | ||||
-rw-r--r-- | src/drivers/drv_rc_input.h | 2 | ||||
-rw-r--r-- | src/drivers/px4io/px4io.cpp | 41 | ||||
-rw-r--r-- | src/drivers/stm32/drv_hrt.c | 45 | ||||
-rw-r--r-- | src/modules/commander/commander.cpp | 4 | ||||
-rw-r--r-- | src/modules/sensors/sensor_params.c | 5 | ||||
-rw-r--r-- | src/modules/sensors/sensors.cpp | 28 | ||||
-rw-r--r-- | src/modules/systemlib/perf_counter.c | 3 | ||||
-rw-r--r-- | src/systemcmds/tests/module.mk | 3 | ||||
-rw-r--r-- | src/systemcmds/tests/test_rc.c | 146 | ||||
-rw-r--r-- | src/systemcmds/tests/test_sensors.c | 67 | ||||
-rw-r--r-- | src/systemcmds/tests/tests.h | 1 | ||||
-rw-r--r-- | src/systemcmds/tests/tests_main.c | 1 |
17 files changed, 558 insertions, 43 deletions
diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 7f73acf85..070a4e7e3 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -23,7 +23,7 @@ fi if l3gd20 start then - echo "using L3GD20(H)" + echo "using L3GD20(H)" fi if lsm303d start diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f551c1aa8..f551c1aa8 100755..100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS diff --git a/ROMFS/px4fmu_logging/init.d/rcS b/ROMFS/px4fmu_logging/init.d/rcS new file mode 100644 index 000000000..7b8856719 --- /dev/null +++ b/ROMFS/px4fmu_logging/init.d/rcS @@ -0,0 +1,88 @@ +#!nsh +# +# PX4FMU startup script for logging purposes +# + +# +# Try to mount the microSD card. +# +echo "[init] looking for microSD..." +if mount -t vfat /dev/mmcsd0 /fs/microsd +then + echo "[init] card mounted at /fs/microsd" + # Start playing the startup tune + tone_alarm start +else + echo "[init] no microSD card found" + # Play SOS + tone_alarm error +fi + +uorb start + +# +# Start sensor drivers here. +# + +ms5611 start +adc start + +# mag might be external +if hmc5883 start +then + echo "using HMC5883" +fi + +if mpu6000 start +then + echo "using MPU6000" +fi + +if l3gd20 start +then + echo "using L3GD20(H)" +fi + +if lsm303d start +then + set BOARD fmuv2 +else + set BOARD fmuv1 +fi + +# Start airspeed sensors +if meas_airspeed start +then + echo "using MEAS airspeed sensor" +else + if ets_airspeed start + then + echo "using ETS airspeed sensor (bus 3)" + else + if ets_airspeed start -b 1 + then + echo "Using ETS airspeed sensor (bus 1)" + fi + fi +fi + +# +# Start the sensor collection task. +# IMPORTANT: this also loads param offsets +# ALWAYS start this task before the +# preflight_check. +# +if sensors start +then + echo "SENSORS STARTED" +fi + +sdlog2 start -r 250 -e -b 16 + +if sercon +then + echo "[init] USB interface connected" + + # Try to get an USB console + nshterm /dev/ttyACM0 & +fi
\ No newline at end of file diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS index 7f161b053..6aa1d3d46 100755..100644 --- a/ROMFS/px4fmu_test/init.d/rcS +++ b/ROMFS/px4fmu_test/init.d/rcS @@ -2,3 +2,11 @@ # # PX4FMU startup script for test hackery. # + +if sercon +then + echo "[init] USB interface connected" + + # Try to get an USB console + nshterm /dev/ttyACM0 & +fi
\ No newline at end of file diff --git a/makefiles/config_px4fmu-v2_logging.mk b/makefiles/config_px4fmu-v2_logging.mk new file mode 100644 index 000000000..ed90f6464 --- /dev/null +++ b/makefiles/config_px4fmu-v2_logging.mk @@ -0,0 +1,157 @@ +# +# Makefile for the px4fmu_default configuration +# + +# +# Use the configuration's ROMFS, copy the px4iov2 firmware into +# the ROMFS if it's available +# +ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_logging +ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v2_default.bin + +# +# Board support modules +# +MODULES += drivers/device +MODULES += drivers/stm32 +MODULES += drivers/stm32/adc +MODULES += drivers/stm32/tone_alarm +MODULES += drivers/led +MODULES += drivers/px4fmu +MODULES += drivers/px4io +MODULES += drivers/boards/px4fmu-v2 +MODULES += drivers/rgbled +MODULES += drivers/mpu6000 +MODULES += drivers/lsm303d +MODULES += drivers/l3gd20 +MODULES += drivers/hmc5883 +MODULES += drivers/ms5611 +MODULES += drivers/mb12xx +MODULES += drivers/gps +MODULES += drivers/hil +MODULES += drivers/hott/hott_telemetry +MODULES += drivers/hott/hott_sensors +MODULES += drivers/blinkm +MODULES += drivers/roboclaw +MODULES += drivers/airspeed +MODULES += drivers/ets_airspeed +MODULES += drivers/meas_airspeed +MODULES += modules/sensors + +# Needs to be burned to the ground and re-written; for now, +# just don't build it. +#MODULES += drivers/mkblctrl + +# +# System commands +# +MODULES += systemcmds/ramtron +MODULES += systemcmds/bl_update +MODULES += systemcmds/boardinfo +MODULES += systemcmds/mixer +MODULES += systemcmds/param +MODULES += systemcmds/perf +MODULES += systemcmds/preflight_check +MODULES += systemcmds/pwm +MODULES += systemcmds/esc_calib +MODULES += systemcmds/reboot +MODULES += systemcmds/top +MODULES += systemcmds/tests +MODULES += systemcmds/config +MODULES += systemcmds/nshterm + +# +# General system control +# +MODULES += modules/commander +MODULES += modules/navigator +MODULES += modules/mavlink +MODULES += modules/mavlink_onboard + +# +# Estimation modules (EKF/ SO3 / other filters) +# +MODULES += modules/attitude_estimator_ekf +MODULES += modules/attitude_estimator_so3 +MODULES += modules/att_pos_estimator_ekf +MODULES += modules/position_estimator_inav +MODULES += examples/flow_position_estimator + +# +# Vehicle Control +# +#MODULES += modules/segway # XXX Needs GCC 4.7 fix +MODULES += modules/fw_pos_control_l1 +MODULES += modules/fw_att_control +MODULES += modules/multirotor_att_control +MODULES += modules/multirotor_pos_control + +# +# Logging +# +MODULES += modules/sdlog2 + +# +# Unit tests +# +#MODULES += modules/unit_test +#MODULES += modules/commander/commander_tests + +# +# Library modules +# +MODULES += modules/systemlib +MODULES += modules/systemlib/mixer +MODULES += modules/controllib +MODULES += modules/uORB + +# +# Libraries +# +LIBRARIES += lib/mathlib/CMSIS +MODULES += lib/mathlib +MODULES += lib/mathlib/math/filter +MODULES += lib/ecl +MODULES += lib/external_lgpl +MODULES += lib/geo +MODULES += lib/conversion + +# +# Demo apps +# +#MODULES += examples/math_demo +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/hello_sky +MODULES += examples/px4_simple_app + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/daemon +#MODULES += examples/px4_daemon_app + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/debug_values +#MODULES += examples/px4_mavlink_debug + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control +#MODULES += examples/fixedwing_control + +# Hardware test +#MODULES += examples/hwtest + +# +# Transitional support - add commands from the NuttX export archive. +# +# In general, these should move to modules over time. +# +# Each entry here is <command>.<priority>.<stacksize>.<entrypoint> but we use a helper macro +# to make the table a bit more readable. +# +define _B + $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) +endef + +# command priority stack entrypoint +BUILTIN_COMMANDS := \ + $(call _B, sercon, , 2048, sercon_main ) \ + $(call _B, serdis, , 2048, serdis_main ) diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 86e5a149a..7b18b5b15 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 diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index db882e623..b80844c5b 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, ®s[prolog + 9], channel_count - 9); @@ -2150,7 +2182,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..36226c941 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -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; + + 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; + } - ppm_decoded_channels = i; - ppm_last_valid_decode = hrt_absolute_time(); + } 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 */ diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index dfd4d2f73..0357542f0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1598,8 +1598,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 { diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 587b81588..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 @@ -226,3 +227,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 */ diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 5baab4a5d..eea9438f7 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -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"); @@ -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) 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/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 20182c5d8..4fbd296d2 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -28,4 +28,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 e2105f8ee..6420d3798 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[]); extern int test_mathlib(int argc, char *argv[]); __END_DECLS diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index fd026d70e..5fab8f82e 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}, {"mathlib", test_mathlib, 0}, {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {NULL, NULL, 0} |