forked from Archive/PX4-Autopilot
Merge branch 'master' into mathlib_new
This commit is contained in:
Anton Babushkin
commit
5a2da77758
|
|
@ -23,7 +23,7 @@ fi
|
|||
|
||||
if l3gd20 start
|
||||
then
|
||||
echo "using L3GD20(H)"
|
||||
echo "using L3GD20(H)"
|
||||
fi
|
||||
|
||||
if lsm303d start
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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 )
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
|
|||
|
|
@ -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 {
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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,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
|
||||
|
|
|
|||
|
|
@ -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}
|
||||
|
|
|
|||
Loading…
Reference in New Issue