dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'master' into vector_control2

This commit is contained in:
Anton Babushkin 2013-12-13 17:26:07 +04:00
parent bae2171edb c46bd8b041
commit 6705e9518b
40 changed files with 1804 additions and 443 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

89
ROMFS/px4fmu_common/init.d/102_3dr_skywalker Normal file
View File

@ -0,0 +1,89 @@
#!nsh
echo "[init] PX4FMU v1, v2 with or without IO on 3DR SkyWalker"
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
param set FW_P_D 0
param set FW_P_I 0
param set FW_P_IMAX 15
param set FW_P_LIM_MAX 50
param set FW_P_LIM_MIN -50
param set FW_P_P 60
param set FW_P_RMAX_NEG 0
param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 1.1
param set FW_R_D 0
param set FW_R_I 5
param set FW_R_IMAX 20
param set FW_R_P 100
param set FW_R_RMAX 100
param set FW_THR_CRUISE 0.65
param set FW_THR_MAX 1
param set FW_THR_MIN 0
param set FW_T_SINK_MAX 5.0
param set FW_T_SINK_MIN 4.0
param set FW_Y_ROLLFF 1.1
param set FW_L1_PERIOD 16
param set RC_SCALE_ROLL 1.0
param set RC_SCALE_PITCH 1.0
param set SYS_AUTOCONFIG 0
param save
fi
#
# Force some key parameters to sane values
# MAV_TYPE 1 = fixed wing
#
param set MAV_TYPE 1
set EXIT_ON_END no
#
# Start and configure PX4IO or FMU interface
#
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
sh /etc/init.d/rc.io
# Limit to 100 Hz updates and (implicit) 50 Hz PWM
px4io limit 100
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
fmu mode_pwm
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
fi
pwm disarmed -c 3 -p 1056
#
# Load mixer and start controllers (depends on px4io)
#
if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ]
then
echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix"
mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AETR.mix
else
echo "Using /etc/mixers/FMU_Q.mix"
mixer load /dev/pwm_output /etc/mixers/FMU_AETR.mix
fi
#
# Start common fixedwing apps
#
sh /etc/init.d/rc.fixedwing
if [ $EXIT_ON_END == yes ]
then
exit
fi

2
ROMFS/px4fmu_common/init.d/12-13_hex
View File

@ -78,7 +78,7 @@ mixer load /dev/pwm_output /etc/mixers/FMU_hex_x.mix
#
# Set PWM output frequency to 400 Hz
#
pwm rate -c 123456 -r 400
pwm rate -a -r 400
#
# Set disarmed, min and max PWM signals

6
ROMFS/px4fmu_common/init.d/rcS
View File

@ -313,6 +313,12 @@ then
sh /etc/init.d/101_hk_bixler
set MODE custom
fi
if param compare SYS_AUTOSTART 102
then
sh /etc/init.d/102_3dr_skywalker
set MODE custom
fi
# Start any custom extensions that might be missing
if [ -f /fs/microsd/etc/rc.local ]

5
nuttx-configs/px4fmu-v2/nsh/defconfig
View File

@ -299,9 +299,10 @@ CONFIG_STM32_USART=y
# CONFIG_USART2_RS485 is not set
CONFIG_USART2_RXDMA=y
# CONFIG_USART3_RS485 is not set
# CONFIG_USART3_RXDMA is not set
CONFIG_USART3_RXDMA=y
# CONFIG_UART4_RS485 is not set
# CONFIG_UART4_RXDMA is not set
CONFIG_UART4_RXDMA=y
# CONFIG_UART5_RXDMA is not set
# CONFIG_USART6_RS485 is not set
# CONFIG_USART6_RXDMA is not set
# CONFIG_UART7_RS485 is not set

2
src/drivers/airspeed/airspeed.h
View File

@ -119,7 +119,7 @@ protected:
virtual int collect() = 0;
work_s _work;
float _max_differential_pressure_pa;
float _max_differential_pressure_pa;
bool _sensor_ok;
int _measure_ticks;
bool _collect_phase;

28
src/drivers/boards/px4fmu-v2/board_config.h
View File

@ -79,17 +79,37 @@ __BEGIN_DECLS
#define GPIO_EXTI_GYRO_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN0)
#define GPIO_EXTI_MAG_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1)
#define GPIO_EXTI_ACCEL_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN4)
#define GPIO_EXTI_MPU_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15)
/* Data ready pins off */
#define GPIO_GYRO_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN0)
#define GPIO_MAG_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN1)
#define GPIO_ACCEL_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN4)
#define GPIO_EXTI_MPU_DRDY (GPIO_INPUT|GPIO_PULLDOWN|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15)
/* SPI1 off */
#define GPIO_SPI1_SCK_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN5)
#define GPIO_SPI1_MISO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN6)
#define GPIO_SPI1_MOSI_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN7)
/* SPI1 chip selects off */
#define GPIO_SPI_CS_GYRO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN13)
#define GPIO_SPI_CS_ACCEL_MAG_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN15)
#define GPIO_SPI_CS_BARO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTD|GPIO_PIN7)
#define GPIO_SPI_CS_MPU (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN2)
/* SPI chip selects */
#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
#define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
#define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7)
#define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10)
#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
#define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
#define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7)
#define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10)
#define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2)
/* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */
#define PX4_SPIDEV_GYRO 1
#define PX4_SPIDEV_ACCEL_MAG 2
#define PX4_SPIDEV_BARO 3
#define PX4_SPIDEV_MPU 4
/* I2C busses */
#define PX4_I2C_BUS_EXPANSION 1

7
src/drivers/boards/px4fmu-v2/px4fmu2_init.c
View File

@ -215,9 +215,9 @@ __EXPORT int nsh_archinitialize(void)
stm32_configgpio(GPIO_ADC1_IN2); /* BATT_VOLTAGE_SENS */
stm32_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */
stm32_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */
stm32_configgpio(GPIO_ADC1_IN10); /* unrouted */
stm32_configgpio(GPIO_ADC1_IN11); /* unrouted */
stm32_configgpio(GPIO_ADC1_IN12); /* unrouted */
// stm32_configgpio(GPIO_ADC1_IN10); /* used by VBUS valid */
// stm32_configgpio(GPIO_ADC1_IN11); /* unused */
// stm32_configgpio(GPIO_ADC1_IN12); /* used by MPU6000 CS */
stm32_configgpio(GPIO_ADC1_IN13); /* FMU_AUX_ADC_1 */
stm32_configgpio(GPIO_ADC1_IN14); /* FMU_AUX_ADC_2 */
stm32_configgpio(GPIO_ADC1_IN15); /* PRESSURE_SENS */
@ -279,6 +279,7 @@ __EXPORT int nsh_archinitialize(void)
SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false);
SPI_SELECT(spi1, PX4_SPIDEV_ACCEL_MAG, false);
SPI_SELECT(spi1, PX4_SPIDEV_BARO, false);
SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
up_udelay(20);
message("[boot] Successfully initialized SPI port 1\n");

18
src/drivers/boards/px4fmu-v2/px4fmu_spi.c
View File

@ -73,6 +73,7 @@ __EXPORT void weak_function stm32_spiinitialize(void)
stm32_configgpio(GPIO_SPI_CS_GYRO);
stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG);
stm32_configgpio(GPIO_SPI_CS_BARO);
stm32_configgpio(GPIO_SPI_CS_MPU);
/* De-activate all peripherals,
* required for some peripheral
@ -81,6 +82,12 @@ __EXPORT void weak_function stm32_spiinitialize(void)
stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
stm32_configgpio(GPIO_EXTI_GYRO_DRDY);
stm32_configgpio(GPIO_EXTI_MAG_DRDY);
stm32_configgpio(GPIO_EXTI_ACCEL_DRDY);
stm32_configgpio(GPIO_EXTI_MPU_DRDY);
#endif
#ifdef CONFIG_STM32_SPI2
@ -99,6 +106,7 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
break;
case PX4_SPIDEV_ACCEL_MAG:
@ -106,6 +114,7 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected);
stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
break;
case PX4_SPIDEV_BARO:
@ -113,6 +122,15 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected);
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
break;
case PX4_SPIDEV_MPU:
/* Making sure the other peripherals are not selected */
stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected);
break;
default:

36
src/drivers/device/cdev.cpp
View File

@ -108,6 +108,42 @@ CDev::~CDev()
unregister_driver(_devname);
}
int
CDev::register_class_devname(const char *class_devname)
{
if (class_devname == nullptr) {
return -EINVAL;
}
int class_instance = 0;
int ret = -ENOSPC;
while (class_instance < 4) {
if (class_instance == 0) {
ret = register_driver(class_devname, &fops, 0666, (void *)this);
if (ret == OK) break;
} else {
char name[32];
snprintf(name, sizeof(name), "%s%u", class_devname, class_instance);
ret = register_driver(name, &fops, 0666, (void *)this);
if (ret == OK) break;
}
class_instance++;
}
if (class_instance == 4)
return ret;
return class_instance;
}
int
CDev::unregister_class_devname(const char *class_devname, unsigned class_instance)
{
if (class_instance > 0) {
char name[32];
snprintf(name, sizeof(name), "%s%u", class_devname, class_instance);
return unregister_driver(name);
}
return unregister_driver(class_devname);
}
int
CDev::init()
{

22
src/drivers/device/device.h
View File

@ -396,6 +396,25 @@ protected:
*/
virtual int close_last(struct file *filp);
/**
* Register a class device name, automatically adding device
* class instance suffix if need be.
*
* @param class_devname Device class name
* @return class_instamce Class instance created, or -errno on failure
*/
virtual int register_class_devname(const char *class_devname);
/**
* Register a class device name, automatically adding device
* class instance suffix if need be.
*
* @param class_devname Device class name
* @param class_instance Device class instance from register_class_devname()
* @return OK on success, -errno otherwise
*/
virtual int unregister_class_devname(const char *class_devname, unsigned class_instance);
private:
static const unsigned _max_pollwaiters = 8;
@ -488,4 +507,7 @@ private:
} // namespace device
// class instance for primary driver of each class
#define CLASS_DEVICE_PRIMARY 0
#endif /* _DEVICE_DEVICE_H */

6
src/drivers/device/spi.cpp
View File

@ -181,4 +181,10 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
return OK;
}
void
SPI::set_frequency(uint32_t frequency)
{
_frequency = frequency;
}
} // namespace device

11
src/drivers/device/spi.h
View File

@ -101,6 +101,17 @@ protected:
*/
int transfer(uint8_t *send, uint8_t *recv, unsigned len);
/**
* Set the SPI bus frequency
* This is used to change frequency on the fly. Some sensors
* (such as the MPU6000) need a lower frequency for setup
* registers and can handle higher frequency for sensor
* value registers
*
* @param frequency Frequency to set (Hz)
*/
void set_frequency(uint32_t frequency);
/**
* Locking modes supported by the driver.
*/

4
src/drivers/drv_gpio.h
View File

@ -46,7 +46,7 @@
/*
* PX4FMU GPIO numbers.
*
* For shared pins, alternate function 1 selects the non-GPIO mode
* For shared pins, alternate function 1 selects the non-GPIO mode
* (USART2, CAN2, etc.)
*/
# define GPIO_EXT_1 (1<<0) /**< high-power GPIO 1 */
@ -144,4 +144,6 @@
/** read all the GPIOs and return their values in *(uint32_t *)arg */
#define GPIO_GET GPIOC(12)
#define GPIO_SENSOR_RAIL_RESET GPIOC(13)
#endif /* _DRV_GPIO_H */

14
src/drivers/drv_hrt.h
View File

@ -141,6 +141,20 @@ __EXPORT extern bool hrt_called(struct hrt_call *entry);
*/
__EXPORT extern void hrt_cancel(struct hrt_call *entry);
/*
* initialise a hrt_call structure
*/
__EXPORT extern void hrt_call_init(struct hrt_call *entry);
/*
* delay a hrt_call_every() periodic call by the given number of
* microseconds. This should be called from within the callout to
* cause the callout to be re-scheduled for a later time. The periodic
* callouts will then continue from that new base time at the
* previously specified period.
*/
__EXPORT extern void hrt_call_delay(struct hrt_call *entry, hrt_abstime delay);
/*
* Initialise the HRT.
*/

42
src/drivers/hmc5883/hmc5883.cpp
View File

@ -77,6 +77,7 @@
*/
#define HMC5883L_ADDRESS PX4_I2C_OBDEV_HMC5883
#define HMC5883L_DEVICE_PATH "/dev/hmc5883"
/* Max measurement rate is 160Hz, however with 160 it will be set to 166 Hz, therefore workaround using 150 */
#define HMC5883_CONVERSION_INTERVAL (1000000 / 150) /* microseconds */
@ -154,6 +155,7 @@ private:
float _range_scale;
float _range_ga;
bool _collect_phase;
int _class_instance;
orb_advert_t _mag_topic;
@ -315,12 +317,13 @@ extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]);
HMC5883::HMC5883(int bus) :
I2C("HMC5883", MAG_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000),
I2C("HMC5883", HMC5883L_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000),
_measure_ticks(0),
_reports(nullptr),
_range_scale(0), /* default range scale from counts to gauss */
_range_ga(1.3f),
_mag_topic(-1),
_class_instance(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")),
_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")),
@ -351,6 +354,9 @@ HMC5883::~HMC5883()
if (_reports != nullptr)
delete _reports;
if (_class_instance != -1)
unregister_class_devname(MAG_DEVICE_PATH, _class_instance);
// free perf counters
perf_free(_sample_perf);
perf_free(_comms_errors);
@ -374,13 +380,17 @@ HMC5883::init()
/* reset the device configuration */
reset();
/* get a publish handle on the mag topic */
struct mag_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report);
_class_instance = register_class_devname(MAG_DEVICE_PATH);
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* get a publish handle on the mag topic if we are
* the primary mag */
struct mag_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report);
if (_mag_topic < 0)
debug("failed to create sensor_mag object");
if (_mag_topic < 0)
debug("failed to create sensor_mag object");
}
ret = OK;
/* sensor is ok, but not calibrated */
@ -875,8 +885,10 @@ HMC5883::collect()
}
#endif
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
if (_mag_topic != -1) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
}
/* post a report to the ring */
if (_reports->force(&new_report)) {
@ -1256,7 +1268,7 @@ start()
goto fail;
/* set the poll rate to default, starts automatic data collection */
fd = open(MAG_DEVICE_PATH, O_RDONLY);
fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
if (fd < 0)
goto fail;
@ -1288,10 +1300,10 @@ test()
ssize_t sz;
int ret;
int fd = open(MAG_DEVICE_PATH, O_RDONLY);
int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
if (fd < 0)
err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH);
err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH);
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));
@ -1388,10 +1400,10 @@ int calibrate()
{
int ret;
int fd = open(MAG_DEVICE_PATH, O_RDONLY);
int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
if (fd < 0)
err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH);
err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH);
if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) {
warnx("failed to enable sensor calibration mode");
@ -1413,7 +1425,7 @@ int calibrate()
void
reset()
{
int fd = open(MAG_DEVICE_PATH, O_RDONLY);
int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
if (fd < 0)
err(1, "failed ");

115
src/drivers/l3gd20/l3gd20.cpp
View File

@ -66,6 +66,8 @@
#include <board_config.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#define L3GD20_DEVICE_PATH "/dev/l3gd20"
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
@ -92,9 +94,17 @@ static const int ERROR = -1;
#define REG1_RATE_LP_MASK 0xF0 /* Mask to guard partial register update */
/* keep lowpass low to avoid noise issues */
#define RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4))
#define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4))
#define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (0<<5) | (1<<4))
#define RATE_190HZ_LP_50HZ ((0<<7) | (1<<6) | (1<<5) | (0<<4))
#define RATE_190HZ_LP_70HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4))
#define RATE_380HZ_LP_20HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4))
#define RATE_380HZ_LP_25HZ ((1<<7) | (0<<6) | (0<<5) | (1<<4))
#define RATE_380HZ_LP_50HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4))
#define RATE_380HZ_LP_100HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4))
#define RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (0<<5) | (0<<4))
#define RATE_760HZ_LP_35HZ ((1<<7) | (1<<6) | (0<<5) | (1<<4))
#define RATE_760HZ_LP_50HZ ((1<<7) | (1<<6) | (1<<5) | (0<<4))
#define RATE_760HZ_LP_100HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4))
#define ADDR_CTRL_REG2 0x21
#define ADDR_CTRL_REG3 0x22
@ -191,6 +201,7 @@ private:
float _gyro_range_scale;
float _gyro_range_rad_s;
orb_advert_t _gyro_topic;
int _class_instance;
unsigned _current_rate;
unsigned _orientation;
@ -198,6 +209,8 @@ private:
unsigned _read;
perf_counter_t _sample_perf;
perf_counter_t _reschedules;
perf_counter_t _errors;
math::LowPassFilter2p _gyro_filter_x;
math::LowPassFilter2p _gyro_filter_y;
@ -218,6 +231,11 @@ private:
*/
void reset();
/**
* disable I2C on the chip
*/
void disable_i2c();
/**
* Static trampoline from the hrt_call context; because we don't have a
* generic hrt wrapper yet.
@ -304,10 +322,13 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
_gyro_range_scale(0.0f),
_gyro_range_rad_s(0.0f),
_gyro_topic(-1),
_class_instance(-1),
_current_rate(0),
_orientation(SENSOR_BOARD_ROTATION_270_DEG),
_read(0),
_sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")),
_reschedules(perf_alloc(PC_COUNT, "l3gd20_reschedules")),
_errors(perf_alloc(PC_COUNT, "l3gd20_errors")),
_gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
_gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
_gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ)
@ -333,8 +354,13 @@ L3GD20::~L3GD20()
if (_reports != nullptr)
delete _reports;
if (_class_instance != -1)
unregister_class_devname(GYRO_DEVICE_PATH, _class_instance);
/* delete the perf counter */
perf_free(_sample_perf);
perf_free(_reschedules);
perf_free(_errors);
}
int
@ -352,10 +378,13 @@ L3GD20::init()
if (_reports == nullptr)
goto out;
/* advertise sensor topic */
struct gyro_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report);
_class_instance = register_class_devname(GYRO_DEVICE_PATH);
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* advertise sensor topic */
struct gyro_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report);
}
reset();
@ -574,6 +603,7 @@ L3GD20::read_reg(unsigned reg)
uint8_t cmd[2];
cmd[0] = reg | DIR_READ;
cmd[1] = 0;
transfer(cmd, cmd, sizeof(cmd));
@ -653,16 +683,15 @@ L3GD20::set_samplerate(unsigned frequency)
} else if (frequency <= 200) {
_current_rate = 190;
bits |= RATE_190HZ_LP_25HZ;
bits |= RATE_190HZ_LP_50HZ;
} else if (frequency <= 400) {
_current_rate = 380;
bits |= RATE_380HZ_LP_20HZ;
bits |= RATE_380HZ_LP_50HZ;
} else if (frequency <= 800) {
_current_rate = 760;
bits |= RATE_760HZ_LP_30HZ;
bits |= RATE_760HZ_LP_50HZ;
} else {
return -EINVAL;
}
@ -699,13 +728,31 @@ L3GD20::stop()
hrt_cancel(&_call);
}
void
L3GD20::disable_i2c(void)
{
uint8_t retries = 10;
while (retries--) {
// add retries
uint8_t a = read_reg(0x05);
write_reg(0x05, (0x20 | a));
if (read_reg(0x05) == (a | 0x20)) {
return;
}
}
debug("FAILED TO DISABLE I2C");
}
void
L3GD20::reset()
{
// ensure the chip doesn't interpret any other bus traffic as I2C
disable_i2c();
/* set default configuration */
write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */
write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */
write_reg(ADDR_CTRL_REG3, 0x08); /* DRDY enable */
write_reg(ADDR_CTRL_REG4, REG4_BDU);
write_reg(ADDR_CTRL_REG5, 0);
@ -716,7 +763,7 @@ L3GD20::reset()
* callback fast enough to not miss data. */
write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE);
set_samplerate(L3GD20_DEFAULT_RATE);
set_samplerate(0); // 760Hz
set_range(L3GD20_DEFAULT_RANGE_DPS);
set_driver_lowpass_filter(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ);
@ -732,9 +779,26 @@ L3GD20::measure_trampoline(void *arg)
dev->measure();
}
#ifdef GPIO_EXTI_GYRO_DRDY
# define L3GD20_USE_DRDY 1
#else
# define L3GD20_USE_DRDY 0
#endif
void
L3GD20::measure()
{
#if L3GD20_USE_DRDY
// if the gyro doesn't have any data ready then re-schedule
// for 100 microseconds later. This ensures we don't double
// read a value and then miss the next value
if (stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) {
perf_count(_reschedules);
hrt_call_delay(&_call, 100);
return;
}
#endif
/* status register and data as read back from the device */
#pragma pack(push, 1)
struct {
@ -753,9 +817,20 @@ L3GD20::measure()
perf_begin(_sample_perf);
/* fetch data from the sensor */
memset(&raw_report, 0, sizeof(raw_report));
raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT;
transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report));
#if L3GD20_USE_DRDY
if ((raw_report.status & 0xF) != 0xF) {
/*
we waited for DRDY, but did not see DRDY on all axes
when we captured. That means a transfer error of some sort
*/
perf_count(_errors);
return;
}
#endif
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
@ -833,6 +908,8 @@ L3GD20::print_info()
{
printf("gyro reads: %u\n", _read);
perf_print_counter(_sample_perf);
perf_print_counter(_reschedules);
perf_print_counter(_errors);
_reports->print_info("report queue");
}
@ -883,7 +960,7 @@ start()
errx(0, "already started");
/* create the driver */
g_dev = new L3GD20(1 /* XXX magic number */, GYRO_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO);
g_dev = new L3GD20(1 /* SPI bus 1 */, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO);
if (g_dev == nullptr)
goto fail;
@ -892,7 +969,7 @@ start()
goto fail;
/* set the poll rate to default, starts automatic data collection */
fd = open(GYRO_DEVICE_PATH, O_RDONLY);
fd = open(L3GD20_DEVICE_PATH, O_RDONLY);
if (fd < 0)
goto fail;
@ -900,6 +977,8 @@ start()
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
goto fail;
close(fd);
exit(0);
fail:
@ -924,10 +1003,10 @@ test()
ssize_t sz;
/* get the driver */
fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY);
fd_gyro = open(L3GD20_DEVICE_PATH, O_RDONLY);
if (fd_gyro < 0)
err(1, "%s open failed", GYRO_DEVICE_PATH);
err(1, "%s open failed", L3GD20_DEVICE_PATH);
/* reset to manual polling */
if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
@ -948,6 +1027,8 @@ test()
warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s,
(int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f));
close(fd_gyro);
/* XXX add poll-rate tests here too */
reset();
@ -960,7 +1041,7 @@ test()
void
reset()
{
int fd = open(GYRO_DEVICE_PATH, O_RDONLY);
int fd = open(L3GD20_DEVICE_PATH, O_RDONLY);
if (fd < 0)
err(1, "failed ");
@ -971,6 +1052,8 @@ reset()
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
err(1, "accel pollrate reset failed");
close(fd);
exit(0);
}

473
src/drivers/lsm303d/lsm303d.cpp
View File

@ -39,6 +39,7 @@
#include <nuttx/config.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <stdint.h>
#include <stdbool.h>
#include <stddef.h>
@ -63,6 +64,7 @@
#include <drivers/drv_accel.h>
#include <drivers/drv_mag.h>
#include <drivers/device/ringbuffer.h>
#include <drivers/drv_tone_alarm.h>
#include <board_config.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
@ -78,23 +80,29 @@ static const int ERROR = -1;
#define DIR_WRITE (0<<7)
#define ADDR_INCREMENT (1<<6)
#define LSM303D_DEVICE_PATH_ACCEL "/dev/lsm303d_accel"
#define LSM303D_DEVICE_PATH_MAG "/dev/lsm303d_mag"
/* register addresses: A: accel, M: mag, T: temp */
#define ADDR_WHO_AM_I 0x0F
#define WHO_I_AM 0x49
#define WHO_I_AM 0x49
#define ADDR_OUT_L_T 0x05
#define ADDR_OUT_H_T 0x06
#define ADDR_STATUS_M 0x07
#define ADDR_OUT_X_L_M 0x08
#define ADDR_OUT_X_H_M 0x09
#define ADDR_OUT_Y_L_M 0x0A
#define ADDR_OUT_Y_H_M 0x0B
#define ADDR_OUT_Z_L_M 0x0C
#define ADDR_OUT_Z_H_M 0x0D
#define ADDR_OUT_TEMP_L 0x05
#define ADDR_OUT_TEMP_H 0x06
#define ADDR_STATUS_M 0x07
#define ADDR_OUT_X_L_M 0x08
#define ADDR_OUT_X_H_M 0x09
#define ADDR_OUT_Y_L_M 0x0A
#define ADDR_OUT_Y_H_M 0x0B
#define ADDR_OUT_Z_L_M 0x0C
#define ADDR_OUT_Z_H_M 0x0D
#define ADDR_INT_CTRL_M 0x12
#define ADDR_INT_SRC_M 0x13
#define ADDR_REFERENCE_X 0x1c
#define ADDR_REFERENCE_Y 0x1d
#define ADDR_REFERENCE_Z 0x1e
#define ADDR_OUT_TEMP_A 0x26
#define ADDR_STATUS_A 0x27
#define ADDR_OUT_X_L_A 0x28
#define ADDR_OUT_X_H_A 0x29
@ -112,6 +120,26 @@ static const int ERROR = -1;
#define ADDR_CTRL_REG6 0x25
#define ADDR_CTRL_REG7 0x26
#define ADDR_FIFO_CTRL 0x2e
#define ADDR_FIFO_SRC 0x2f
#define ADDR_IG_CFG1 0x30
#define ADDR_IG_SRC1 0x31
#define ADDR_IG_THS1 0x32
#define ADDR_IG_DUR1 0x33
#define ADDR_IG_CFG2 0x34
#define ADDR_IG_SRC2 0x35
#define ADDR_IG_THS2 0x36
#define ADDR_IG_DUR2 0x37
#define ADDR_CLICK_CFG 0x38
#define ADDR_CLICK_SRC 0x39
#define ADDR_CLICK_THS 0x3a
#define ADDR_TIME_LIMIT 0x3b
#define ADDR_TIME_LATENCY 0x3c
#define ADDR_TIME_WINDOW 0x3d
#define ADDR_ACT_THS 0x3e
#define ADDR_ACT_DUR 0x3f
#define REG1_RATE_BITS_A ((1<<7) | (1<<6) | (1<<5) | (1<<4))
#define REG1_POWERDOWN_A ((0<<7) | (0<<6) | (0<<5) | (0<<4))
#define REG1_RATE_3_125HZ_A ((0<<7) | (0<<6) | (0<<5) | (1<<4))
@ -201,6 +229,21 @@ public:
*/
void print_info();
/**
* dump register values
*/
void print_registers();
/**
* toggle logging
*/
void toggle_logging();
/**
* check for extreme accel values
*/
void check_extremes(const accel_report *arb);
protected:
virtual int probe();
@ -234,7 +277,7 @@ private:
unsigned _mag_samplerate;
orb_advert_t _accel_topic;
orb_advert_t _mag_topic;
int _class_instance;
unsigned _accel_read;
unsigned _mag_read;
@ -243,6 +286,8 @@ private:
perf_counter_t _mag_sample_perf;
perf_counter_t _reg7_resets;
perf_counter_t _reg1_resets;
perf_counter_t _extreme_values;
perf_counter_t _accel_reschedules;
math::LowPassFilter2p _accel_filter_x;
math::LowPassFilter2p _accel_filter_y;
@ -253,6 +298,15 @@ private:
uint8_t _reg7_expected;
uint8_t _reg1_expected;
// accel logging
int _accel_log_fd;
bool _accel_logging_enabled;
uint64_t _last_extreme_us;
uint64_t _last_log_us;
uint64_t _last_log_sync_us;
uint64_t _last_log_reg_us;
uint64_t _last_log_alarm_us;
/**
* Start automatic measurement.
*/
@ -270,6 +324,11 @@ private:
*/
void reset();
/**
* disable I2C on the chip
*/
void disable_i2c();
/**
* Static trampoline from the hrt_call context; because we don't have a
* generic hrt wrapper yet.
@ -408,6 +467,8 @@ public:
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
virtual int init();
protected:
friend class LSM303D;
@ -415,6 +476,9 @@ protected:
private:
LSM303D *_parent;
orb_advert_t _mag_topic;
int _mag_class_instance;
void measure();
void measure_trampoline(void *arg);
@ -436,18 +500,26 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
_mag_range_scale(0.0f),
_mag_samplerate(0),
_accel_topic(-1),
_mag_topic(-1),
_class_instance(-1),
_accel_read(0),
_mag_read(0),
_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
_mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")),
_reg1_resets(perf_alloc(PC_COUNT, "lsm303d_reg1_resets")),
_reg7_resets(perf_alloc(PC_COUNT, "lsm303d_reg7_resets")),
_extreme_values(perf_alloc(PC_COUNT, "lsm303d_extremes")),
_accel_reschedules(perf_alloc(PC_COUNT, "lsm303d_accel_resched")),
_accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_reg1_expected(0),
_reg7_expected(0)
_reg7_expected(0),
_accel_log_fd(-1),
_accel_logging_enabled(false),
_last_log_us(0),
_last_log_sync_us(0),
_last_log_reg_us(0),
_last_log_alarm_us(0)
{
// enable debug() calls
_debug_enabled = true;
@ -479,11 +551,17 @@ LSM303D::~LSM303D()
if (_mag_reports != nullptr)
delete _mag_reports;
if (_class_instance != -1)
unregister_class_devname(ACCEL_DEVICE_PATH, _class_instance);
delete _mag;
/* delete the perf counter */
perf_free(_accel_sample_perf);
perf_free(_mag_sample_perf);
perf_free(_reg1_resets);
perf_free(_reg7_resets);
perf_free(_extreme_values);
}
int
@ -505,10 +583,6 @@ LSM303D::init()
goto out;
/* advertise accel topic */
struct accel_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report);
_mag_reports = new RingBuffer(2, sizeof(mag_report));
if (_mag_reports == nullptr)
@ -516,26 +590,45 @@ LSM303D::init()
reset();
/* advertise mag topic */
struct mag_report zero_mag_report;
memset(&zero_mag_report, 0, sizeof(zero_mag_report));
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_mag_report);
/* do CDev init for the mag device node, keep it optional */
mag_ret = _mag->init();
if (mag_ret != OK) {
_mag_topic = -1;
/* do CDev init for the mag device node */
ret = _mag->init();
if (ret != OK) {
warnx("MAG init failed");
goto out;
}
_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
if (_class_instance == CLASS_DEVICE_PRIMARY) {
// we are the primary accel device, so advertise to
// the ORB
struct accel_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report);
}
ret = OK;
out:
return ret;
}
void
LSM303D::disable_i2c(void)
{
uint8_t a = read_reg(0x02);
write_reg(0x02, (0x10 | a));
a = read_reg(0x02);
write_reg(0x02, (0xF7 & a));
a = read_reg(0x15);
write_reg(0x15, (0x80 | a));
a = read_reg(0x02);
write_reg(0x02, (0xE7 & a));
}
void
LSM303D::reset()
{
// ensure the chip doesn't interpret any other bus traffic as I2C
disable_i2c();
/* enable accel*/
_reg1_expected = REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A;
write_reg(ADDR_CTRL_REG1, _reg1_expected);
@ -544,10 +637,17 @@ LSM303D::reset()
_reg7_expected = REG7_CONT_MODE_M;
write_reg(ADDR_CTRL_REG7, _reg7_expected);
write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
write_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1
write_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2
accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G);
accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE);
accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ);
// we setup the anti-alias on-chip filter as 50Hz. We believe
// this operates in the analog domain, and is critical for
// anti-aliasing. The 2 pole software filter is designed to
// operate in conjunction with this on-chip filter
accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);
mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA);
@ -572,6 +672,122 @@ LSM303D::probe()
return -EIO;
}
#define ACCEL_LOGFILE "/fs/microsd/lsm303d.log"
/**
check for extreme accelerometer values and log to a file on the SD card
*/
void
LSM303D::check_extremes(const accel_report *arb)
{
const float extreme_threshold = 30;
static bool boot_ok = false;
bool is_extreme = (fabsf(arb->x) > extreme_threshold &&
fabsf(arb->y) > extreme_threshold &&
fabsf(arb->z) > extreme_threshold);
if (is_extreme) {
perf_count(_extreme_values);
// force accel logging on if we see extreme values
_accel_logging_enabled = true;
} else {
boot_ok = true;
}
if (! _accel_logging_enabled) {
// logging has been disabled by user, close
if (_accel_log_fd != -1) {
::close(_accel_log_fd);
_accel_log_fd = -1;
}
return;
}
if (_accel_log_fd == -1) {
// keep last 10 logs
::unlink(ACCEL_LOGFILE ".9");
for (uint8_t i=8; i>0; i--) {
uint8_t len = strlen(ACCEL_LOGFILE)+3;
char log1[len], log2[len];
snprintf(log1, sizeof(log1), "%s.%u", ACCEL_LOGFILE, (unsigned)i);
snprintf(log2, sizeof(log2), "%s.%u", ACCEL_LOGFILE, (unsigned)(i+1));
::rename(log1, log2);
}
::rename(ACCEL_LOGFILE, ACCEL_LOGFILE ".1");
// open the new logfile
_accel_log_fd = ::open(ACCEL_LOGFILE, O_WRONLY|O_CREAT|O_TRUNC, 0666);
if (_accel_log_fd == -1) {
return;
}
}
uint64_t now = hrt_absolute_time();
// log accels at 1Hz
if (_last_log_us == 0 ||
now - _last_log_us > 1000*1000) {
_last_log_us = now;
::dprintf(_accel_log_fd, "ARB %llu %.3f %.3f %.3f %d %d %d boot_ok=%u\r\n",
(unsigned long long)arb->timestamp,
arb->x, arb->y, arb->z,
(int)arb->x_raw,
(int)arb->y_raw,
(int)arb->z_raw,
(unsigned)boot_ok);
}
const uint8_t reglist[] = { ADDR_WHO_AM_I, 0x02, 0x15, ADDR_STATUS_A, ADDR_STATUS_M, ADDR_CTRL_REG0, ADDR_CTRL_REG1,
ADDR_CTRL_REG2, ADDR_CTRL_REG3, ADDR_CTRL_REG4, ADDR_CTRL_REG5, ADDR_CTRL_REG6,
ADDR_CTRL_REG7, ADDR_OUT_TEMP_L, ADDR_OUT_TEMP_H, ADDR_INT_CTRL_M, ADDR_INT_SRC_M,
ADDR_REFERENCE_X, ADDR_REFERENCE_Y, ADDR_REFERENCE_Z, ADDR_OUT_X_L_A, ADDR_OUT_X_H_A,
ADDR_OUT_Y_L_A, ADDR_OUT_Y_H_A, ADDR_OUT_Z_L_A, ADDR_OUT_Z_H_A, ADDR_FIFO_CTRL,
ADDR_FIFO_SRC, ADDR_IG_CFG1, ADDR_IG_SRC1, ADDR_IG_THS1, ADDR_IG_DUR1, ADDR_IG_CFG2,
ADDR_IG_SRC2, ADDR_IG_THS2, ADDR_IG_DUR2, ADDR_CLICK_CFG, ADDR_CLICK_SRC,
ADDR_CLICK_THS, ADDR_TIME_LIMIT, ADDR_TIME_LATENCY, ADDR_TIME_WINDOW,
ADDR_ACT_THS, ADDR_ACT_DUR,
ADDR_OUT_X_L_M, ADDR_OUT_X_H_M,
ADDR_OUT_Y_L_M, ADDR_OUT_Y_H_M, ADDR_OUT_Z_L_M, ADDR_OUT_Z_H_M, 0x02, 0x15, ADDR_WHO_AM_I};
uint8_t regval[sizeof(reglist)];
for (uint8_t i=0; i<sizeof(reglist); i++) {
regval[i] = read_reg(reglist[i]);
}
// log registers at 10Hz when we have extreme values, or 0.5 Hz without
if (_last_log_reg_us == 0 ||
(is_extreme && (now - _last_log_reg_us > 250*1000)) ||
(now - _last_log_reg_us > 10*1000*1000)) {
_last_log_reg_us = now;
::dprintf(_accel_log_fd, "XREG %llu", (unsigned long long)hrt_absolute_time());
for (uint8_t i=0; i<sizeof(reglist); i++) {
::dprintf(_accel_log_fd, " %02x:%02x", (unsigned)reglist[i], (unsigned)regval[i]);
}
::dprintf(_accel_log_fd, "\n");
}
// fsync at 0.1Hz
if (now - _last_log_sync_us > 10*1000*1000) {
_last_log_sync_us = now;
::fsync(_accel_log_fd);
}
// play alarm every 10s if we have had an extreme value
if (perf_event_count(_extreme_values) != 0 &&
(now - _last_log_alarm_us > 10*1000*1000)) {
_last_log_alarm_us = now;
int tfd = ::open(TONEALARM_DEVICE_PATH, 0);
if (tfd != -1) {
uint8_t tone = 3;
if (!is_extreme) {
tone = 3;
} else if (boot_ok) {
tone = 4;
} else {
tone = 5;
}
::ioctl(tfd, TONE_SET_ALARM, tone);
::close(tfd);
}
}
}
ssize_t
LSM303D::read(struct file *filp, char *buffer, size_t buflen)
{
@ -590,6 +806,7 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen)
*/
while (count--) {
if (_accel_reports->get(arb)) {
check_extremes(arb);
ret += sizeof(*arb);
arb++;
}
@ -952,6 +1169,7 @@ LSM303D::read_reg(unsigned reg)
uint8_t cmd[2];
cmd[0] = reg | DIR_READ;
cmd[1] = 0;
transfer(cmd, cmd, sizeof(cmd));
@ -1224,6 +1442,14 @@ LSM303D::mag_measure_trampoline(void *arg)
void
LSM303D::measure()
{
// if the accel doesn't have any data ready then re-schedule
// for 100 microseconds later. This ensures we don't double
// read a value and then miss the next value
if (stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) {
perf_count(_accel_reschedules);
hrt_call_delay(&_accel_call, 100);
return;
}
if (read_reg(ADDR_CTRL_REG1) != _reg1_expected) {
perf_count(_reg1_resets);
reset();
@ -1248,6 +1474,7 @@ LSM303D::measure()
perf_begin(_accel_sample_perf);
/* fetch data from the sensor */
memset(&raw_accel_report, 0, sizeof(raw_accel_report));
raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT;
transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report));
@ -1290,8 +1517,10 @@ LSM303D::measure()
/* notify anyone waiting for data */
poll_notify(POLLIN);
/* publish for subscribers */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
if (_accel_topic != -1) {
/* publish for subscribers */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
}
_accel_read++;
@ -1325,6 +1554,7 @@ LSM303D::mag_measure()
perf_begin(_mag_sample_perf);
/* fetch data from the sensor */
memset(&raw_mag_report, 0, sizeof(raw_mag_report));
raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT;
transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report));
@ -1361,8 +1591,10 @@ LSM303D::mag_measure()
/* notify anyone waiting for data */
poll_notify(POLLIN);
/* publish for subscribers */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
if (_mag->_mag_topic != -1) {
/* publish for subscribers */
orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
}
_mag_read++;
@ -1380,14 +1612,111 @@ LSM303D::print_info()
_mag_reports->print_info("mag reports");
}
void
LSM303D::print_registers()
{
const struct {
uint8_t reg;
const char *name;
} regmap[] = {
{ ADDR_WHO_AM_I, "WHO_AM_I" },
{ 0x02, "I2C_CONTROL1" },
{ 0x15, "I2C_CONTROL2" },
{ ADDR_STATUS_A, "STATUS_A" },
{ ADDR_STATUS_M, "STATUS_M" },
{ ADDR_CTRL_REG0, "CTRL_REG0" },
{ ADDR_CTRL_REG1, "CTRL_REG1" },
{ ADDR_CTRL_REG2, "CTRL_REG2" },
{ ADDR_CTRL_REG3, "CTRL_REG3" },
{ ADDR_CTRL_REG4, "CTRL_REG4" },
{ ADDR_CTRL_REG5, "CTRL_REG5" },
{ ADDR_CTRL_REG6, "CTRL_REG6" },
{ ADDR_CTRL_REG7, "CTRL_REG7" },
{ ADDR_OUT_TEMP_L, "TEMP_L" },
{ ADDR_OUT_TEMP_H, "TEMP_H" },
{ ADDR_INT_CTRL_M, "INT_CTRL_M" },
{ ADDR_INT_SRC_M, "INT_SRC_M" },
{ ADDR_REFERENCE_X, "REFERENCE_X" },
{ ADDR_REFERENCE_Y, "REFERENCE_Y" },
{ ADDR_REFERENCE_Z, "REFERENCE_Z" },
{ ADDR_OUT_X_L_A, "ACCEL_XL" },
{ ADDR_OUT_X_H_A, "ACCEL_XH" },
{ ADDR_OUT_Y_L_A, "ACCEL_YL" },
{ ADDR_OUT_Y_H_A, "ACCEL_YH" },
{ ADDR_OUT_Z_L_A, "ACCEL_ZL" },
{ ADDR_OUT_Z_H_A, "ACCEL_ZH" },
{ ADDR_FIFO_CTRL, "FIFO_CTRL" },
{ ADDR_FIFO_SRC, "FIFO_SRC" },
{ ADDR_IG_CFG1, "IG_CFG1" },
{ ADDR_IG_SRC1, "IG_SRC1" },
{ ADDR_IG_THS1, "IG_THS1" },
{ ADDR_IG_DUR1, "IG_DUR1" },
{ ADDR_IG_CFG2, "IG_CFG2" },
{ ADDR_IG_SRC2, "IG_SRC2" },
{ ADDR_IG_THS2, "IG_THS2" },
{ ADDR_IG_DUR2, "IG_DUR2" },
{ ADDR_CLICK_CFG, "CLICK_CFG" },
{ ADDR_CLICK_SRC, "CLICK_SRC" },
{ ADDR_CLICK_THS, "CLICK_THS" },
{ ADDR_TIME_LIMIT, "TIME_LIMIT" },
{ ADDR_TIME_LATENCY,"TIME_LATENCY" },
{ ADDR_TIME_WINDOW, "TIME_WINDOW" },
{ ADDR_ACT_THS, "ACT_THS" },
{ ADDR_ACT_DUR, "ACT_DUR" }
};
for (uint8_t i=0; i<sizeof(regmap)/sizeof(regmap[0]); i++) {
printf("0x%02x %s\n", read_reg(regmap[i].reg), regmap[i].name);
}
printf("_reg1_expected=0x%02x\n", _reg1_expected);
printf("_reg7_expected=0x%02x\n", _reg7_expected);
}
void
LSM303D::toggle_logging()
{
if (! _accel_logging_enabled) {
_accel_logging_enabled = true;
printf("Started logging to %s\n", ACCEL_LOGFILE);
} else {
_accel_logging_enabled = false;
printf("Stopped logging\n");
}
}
LSM303D_mag::LSM303D_mag(LSM303D *parent) :
CDev("LSM303D_mag", MAG_DEVICE_PATH),
_parent(parent)
CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG),
_parent(parent),
_mag_topic(-1),
_mag_class_instance(-1)
{
}
LSM303D_mag::~LSM303D_mag()
{
if (_mag_class_instance != -1)
unregister_class_devname(MAG_DEVICE_PATH, _mag_class_instance);
}
int
LSM303D_mag::init()
{
int ret;
ret = CDev::init();
if (ret != OK)
goto out;
_mag_class_instance = register_class_devname(MAG_DEVICE_PATH);
if (_mag_class_instance == CLASS_DEVICE_PRIMARY) {
// we are the primary mag device, so advertise to
// the ORB
struct mag_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report);
}
out:
return ret;
}
void
@ -1432,6 +1761,8 @@ void start();
void test();
void reset();
void info();
void regdump();
void logging();
/**
* Start the driver.
@ -1445,7 +1776,7 @@ start()
errx(0, "already started");
/* create the driver */
g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG);
g_dev = new LSM303D(1 /* SPI dev 1 */, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG);
if (g_dev == nullptr) {
warnx("failed instantiating LSM303D obj");
@ -1456,7 +1787,7 @@ start()
goto fail;
/* set the poll rate to default, starts automatic data collection */
fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY);
if (fd < 0)
goto fail;
@ -1464,7 +1795,7 @@ start()
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
goto fail;
fd_mag = open(MAG_DEVICE_PATH, O_RDONLY);
fd_mag = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY);
/* don't fail if open cannot be opened */
if (0 <= fd_mag) {
@ -1473,6 +1804,8 @@ start()
}
}
close(fd);
close(fd_mag);
exit(0);
fail:
@ -1499,10 +1832,10 @@ test()
int ret;
/* get the driver */
fd_accel = open(ACCEL_DEVICE_PATH, O_RDONLY);
fd_accel = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY);
if (fd_accel < 0)
err(1, "%s open failed", ACCEL_DEVICE_PATH);
err(1, "%s open failed", LSM303D_DEVICE_PATH_ACCEL);
/* do a simple demand read */
sz = read(fd_accel, &accel_report, sizeof(accel_report));
@ -1528,10 +1861,10 @@ test()
struct mag_report m_report;
/* get the driver */
fd_mag = open(MAG_DEVICE_PATH, O_RDONLY);
fd_mag = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY);
if (fd_mag < 0)
err(1, "%s open failed", MAG_DEVICE_PATH);
err(1, "%s open failed", LSM303D_DEVICE_PATH_MAG);
/* check if mag is onboard or external */
if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0)
@ -1554,6 +1887,9 @@ test()
/* XXX add poll-rate tests here too */
close(fd_accel);
close(fd_mag);
reset();
errx(0, "PASS");
}
@ -1564,7 +1900,7 @@ test()
void
reset()
{
int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
int fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY);
if (fd < 0)
err(1, "failed ");
@ -1575,7 +1911,9 @@ reset()
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
err(1, "accel pollrate reset failed");
fd = open(MAG_DEVICE_PATH, O_RDONLY);
close(fd);
fd = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY);
if (fd < 0) {
warnx("mag could not be opened, external mag might be used");
@ -1585,6 +1923,8 @@ reset()
err(1, "mag pollrate reset failed");
}
close(fd);
exit(0);
}
@ -1603,6 +1943,35 @@ info()
exit(0);
}
/**
* dump registers from device
*/
void
regdump()
{
if (g_dev == nullptr)
errx(1, "driver not running\n");
printf("regdump @ %p\n", g_dev);
g_dev->print_registers();
exit(0);
}
/**
* toggle logging
*/
void
logging()
{
if (g_dev == nullptr)
errx(1, "driver not running\n");
g_dev->toggle_logging();
exit(0);
}
} // namespace
@ -1634,5 +2003,17 @@ lsm303d_main(int argc, char *argv[])
if (!strcmp(argv[1], "info"))
lsm303d::info();
errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
/*
* dump device registers
*/
if (!strcmp(argv[1], "regdump"))
lsm303d::regdump();
/*
* dump device registers
*/
if (!strcmp(argv[1], "logging"))
lsm303d::logging();
errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'logging' or 'regdump'");
}

165
src/drivers/mpu6000/mpu6000.cpp
View File

@ -75,6 +75,9 @@
#define DIR_READ 0x80
#define DIR_WRITE 0x00
#define MPU_DEVICE_PATH_ACCEL "/dev/mpu6000_accel"
#define MPU_DEVICE_PATH_GYRO "/dev/mpu6000_gyro"
// MPU 6000 registers
#define MPUREG_WHOAMI 0x75
#define MPUREG_SMPLRT_DIV 0x19
@ -161,6 +164,14 @@
#define MPU6000_ONE_G 9.80665f
/*
the MPU6000 can only handle high SPI bus speeds on the sensor and
interrupt status registers. All other registers have a maximum 1MHz
SPI speed
*/
#define MPU6000_LOW_BUS_SPEED 1000*1000
#define MPU6000_HIGH_BUS_SPEED 10*1000*1000
class MPU6000_gyro;
class MPU6000 : public device::SPI
@ -200,17 +211,19 @@ private:
float _accel_range_scale;
float _accel_range_m_s2;
orb_advert_t _accel_topic;
int _accel_class_instance;
RingBuffer *_gyro_reports;
struct gyro_scale _gyro_scale;
float _gyro_range_scale;
float _gyro_range_rad_s;
orb_advert_t _gyro_topic;
unsigned _reads;
unsigned _sample_rate;
perf_counter_t _accel_reads;
perf_counter_t _gyro_reads;
perf_counter_t _sample_perf;
perf_counter_t _bad_transfers;
math::LowPassFilter2p _accel_filter_x;
math::LowPassFilter2p _accel_filter_y;
@ -338,12 +351,17 @@ public:
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
virtual int init();
protected:
friend class MPU6000;
void parent_poll_notify();
private:
MPU6000 *_parent;
orb_advert_t _gyro_topic;
int _gyro_class_instance;
};
@ -351,7 +369,7 @@ private:
extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); }
MPU6000::MPU6000(int bus, spi_dev_e device) :
SPI("MPU6000", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 10000000),
SPI("MPU6000", MPU_DEVICE_PATH_ACCEL, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED),
_gyro(new MPU6000_gyro(this)),
_product(0),
_call_interval(0),
@ -359,13 +377,15 @@ MPU6000::MPU6000(int bus, spi_dev_e device) :
_accel_range_scale(0.0f),
_accel_range_m_s2(0.0f),
_accel_topic(-1),
_accel_class_instance(-1),
_gyro_reports(nullptr),
_gyro_range_scale(0.0f),
_gyro_range_rad_s(0.0f),
_gyro_topic(-1),
_reads(0),
_sample_rate(1000),
_accel_reads(perf_alloc(PC_COUNT, "mpu6000_accel_read")),
_gyro_reads(perf_alloc(PC_COUNT, "mpu6000_gyro_read")),
_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")),
_bad_transfers(perf_alloc(PC_COUNT, "mpu6000_bad_transfers")),
_accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
@ -409,8 +429,14 @@ MPU6000::~MPU6000()
if (_gyro_reports != nullptr)
delete _gyro_reports;
if (_accel_class_instance != -1)
unregister_class_devname(ACCEL_DEVICE_PATH, _accel_class_instance);
/* delete the perf counter */
perf_free(_sample_perf);
perf_free(_accel_reads);
perf_free(_gyro_reads);
perf_free(_bad_transfers);
}
int
@ -455,24 +481,23 @@ MPU6000::init()
_gyro_scale.z_scale = 1.0f;
/* do CDev init for the gyro device node, keep it optional */
gyro_ret = _gyro->init();
ret = _gyro->init();
/* if probe/setup failed, bail now */
if (ret != OK) {
debug("gyro init failed");
return ret;
}
/* fetch an initial set of measurements for advertisement */
measure();
if (gyro_ret != OK) {
_gyro_topic = -1;
} else {
gyro_report gr;
_gyro_reports->get(&gr);
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr);
}
/* advertise accel topic */
accel_report ar;
_accel_reports->get(&ar);
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar);
_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
if (_accel_class_instance == CLASS_DEVICE_PRIMARY) {
/* advertise accel topic */
accel_report ar;
_accel_reports->get(&ar);
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar);
}
out:
return ret;
@ -652,6 +677,8 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen)
if (_accel_reports->empty())
return -EAGAIN;
perf_count(_accel_reads);
/* copy reports out of our buffer to the caller */
accel_report *arp = reinterpret_cast<accel_report *>(buffer);
int transferred = 0;
@ -669,12 +696,12 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen)
int
MPU6000::self_test()
{
if (_reads == 0) {
if (perf_event_count(_sample_perf) == 0) {
measure();
}
/* return 0 on success, 1 else */
return (_reads > 0) ? 0 : 1;
return (perf_event_count(_sample_perf) > 0) ? 0 : 1;
}
int
@ -746,6 +773,8 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen)
if (_gyro_reports->empty())
return -EAGAIN;
perf_count(_gyro_reads);
/* copy reports out of our buffer to the caller */
gyro_report *grp = reinterpret_cast<gyro_report *>(buffer);
int transferred = 0;
@ -987,9 +1016,10 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
uint8_t
MPU6000::read_reg(unsigned reg)
{
uint8_t cmd[2];
uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0};
cmd[0] = reg | DIR_READ;
// general register transfer at low clock speed
set_frequency(MPU6000_LOW_BUS_SPEED);
transfer(cmd, cmd, sizeof(cmd));
@ -999,9 +1029,10 @@ MPU6000::read_reg(unsigned reg)
uint16_t
MPU6000::read_reg16(unsigned reg)
{
uint8_t cmd[3];
uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 };
cmd[0] = reg | DIR_READ;
// general register transfer at low clock speed
set_frequency(MPU6000_LOW_BUS_SPEED);
transfer(cmd, cmd, sizeof(cmd));
@ -1016,6 +1047,9 @@ MPU6000::write_reg(unsigned reg, uint8_t value)
cmd[0] = reg | DIR_WRITE;
cmd[1] = value;
// general register transfer at low clock speed
set_frequency(MPU6000_LOW_BUS_SPEED);
transfer(cmd, nullptr, sizeof(cmd));
}
@ -1139,12 +1173,13 @@ MPU6000::measure()
* Fetch the full set of measurements from the MPU6000 in one pass.
*/
mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS;
// sensor transfer at high clock speed
set_frequency(MPU6000_HIGH_BUS_SPEED);
if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report)))
return;
/* count measurement */
_reads++;
/*
* Convert from big to little endian
*/
@ -1159,6 +1194,20 @@ MPU6000::measure()
report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y);
report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z);
if (report.accel_x == 0 &&
report.accel_y == 0 &&
report.accel_z == 0 &&
report.temp == 0 &&
report.gyro_x == 0 &&
report.gyro_y == 0 &&
report.gyro_z == 0) {
// all zero data - probably a SPI bus error
perf_count(_bad_transfers);
perf_end(_sample_perf);
return;
}
/*
* Swap axes and negate y
*/
@ -1249,10 +1298,11 @@ MPU6000::measure()
poll_notify(POLLIN);
_gyro->parent_poll_notify();
/* and publish for subscribers */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
if (_gyro_topic != -1) {
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb);
if (_accel_topic != -1) {
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
}
if (_gyro->_gyro_topic != -1) {
orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
}
/* stop measuring */
@ -1263,19 +1313,48 @@ void
MPU6000::print_info()
{
perf_print_counter(_sample_perf);
printf("reads: %u\n", _reads);
perf_print_counter(_accel_reads);
perf_print_counter(_gyro_reads);
_accel_reports->print_info("accel queue");
_gyro_reports->print_info("gyro queue");
}
MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) :
CDev("MPU6000_gyro", GYRO_DEVICE_PATH),
_parent(parent)
CDev("MPU6000_gyro", MPU_DEVICE_PATH_GYRO),
_parent(parent),
_gyro_class_instance(-1)
{
}
MPU6000_gyro::~MPU6000_gyro()
{
if (_gyro_class_instance != -1)
unregister_class_devname(GYRO_DEVICE_PATH, _gyro_class_instance);
}
int
MPU6000_gyro::init()
{
int ret;
// do base class init
ret = CDev::init();
/* if probe/setup failed, bail now */
if (ret != OK) {
debug("gyro init failed");
return ret;
}
_gyro_class_instance = register_class_devname(GYRO_DEVICE_PATH);
if (_gyro_class_instance == CLASS_DEVICE_PRIMARY) {
gyro_report gr;
memset(&gr, 0, sizeof(gr));
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr);
}
out:
return ret;
}
void
@ -1331,7 +1410,7 @@ start()
goto fail;
/* set the poll rate to default, starts automatic data collection */
fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY);
if (fd < 0)
goto fail;
@ -1339,6 +1418,8 @@ start()
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
goto fail;
close(fd);
exit(0);
fail:
@ -1363,17 +1444,17 @@ test()
ssize_t sz;
/* get the driver */
int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY);
if (fd < 0)
err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)",
ACCEL_DEVICE_PATH);
MPU_DEVICE_PATH_ACCEL);
/* get the driver */
int fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY);
int fd_gyro = open(MPU_DEVICE_PATH_GYRO, O_RDONLY);
if (fd_gyro < 0)
err(1, "%s open failed", GYRO_DEVICE_PATH);
err(1, "%s open failed", MPU_DEVICE_PATH_GYRO);
/* reset to manual polling */
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
@ -1431,7 +1512,7 @@ test()
void
reset()
{
int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY);
if (fd < 0)
err(1, "failed ");
@ -1442,6 +1523,8 @@ reset()
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
err(1, "driver poll restart failed");
close(fd);
exit(0);
}

7
src/drivers/ms5611/ms5611.cpp
View File

@ -420,8 +420,11 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
return _reports->size();
case SENSORIOCRESET:
/* XXX implement this */
return -EINVAL;
/*
* Since we are initialized, we do not need to do anything, since the
* PROM is correctly read and the part does not need to be configured.
*/
return OK;
case BAROIOCSMSLPRESSURE:

24
src/drivers/ms5611/ms5611_spi.cpp
View File

@ -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, 2000000),
SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 6*1000*1000),
_prom(prom_buf)
{
}
@ -134,7 +134,6 @@ int
MS5611_SPI::init()
{
int ret;
irqstate_t flags;
ret = SPI::init();
if (ret != OK) {
@ -167,10 +166,9 @@ MS5611_SPI::read(unsigned offset, void *data, unsigned count)
uint8_t b[4];
uint32_t w;
} *cvt = (_cvt *)data;
uint8_t buf[4];
uint8_t buf[4] = { 0 | DIR_WRITE, 0, 0, 0 };
/* read the most recent measurement */
buf[0] = 0 | DIR_WRITE;
int ret = _transfer(&buf[0], &buf[0], sizeof(buf));
if (ret == OK) {
@ -238,21 +236,31 @@ MS5611_SPI::_read_prom()
usleep(3000);
/* read and convert PROM words */
bool all_zero = true;
for (int i = 0; i < 8; i++) {
uint8_t cmd = (ADDR_PROM_SETUP + (i * 2));
_prom.c[i] = _reg16(cmd);
if (_prom.c[i] != 0)
all_zero = false;
//debug("prom[%u]=0x%x", (unsigned)i, (unsigned)_prom.c[i]);
}
/* calculate CRC and return success/failure accordingly */
return ms5611::crc4(&_prom.c[0]) ? OK : -EIO;
int ret = ms5611::crc4(&_prom.c[0]) ? OK : -EIO;
if (ret != OK) {
debug("crc failed");
}
if (all_zero) {
debug("prom all zero");
ret = -EIO;
}
return ret;
}
uint16_t
MS5611_SPI::_reg16(unsigned reg)
{
uint8_t cmd[3];
cmd[0] = reg | DIR_READ;
uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 };
_transfer(cmd, cmd, sizeof(cmd));

484
src/drivers/px4fmu/fmu.cpp
View File

@ -164,6 +164,7 @@ private:
static const unsigned _ngpio;
void gpio_reset(void);
void sensor_reset(int ms);
void gpio_set_function(uint32_t gpios, int function);
void gpio_write(uint32_t gpios, int function);
uint32_t gpio_read(void);
@ -226,10 +227,10 @@ PX4FMU::PX4FMU() :
_armed(false),
_pwm_on(false),
_mixers(nullptr),
_failsafe_pwm({0}),
_disarmed_pwm({0}),
_num_failsafe_set(0),
_num_disarmed_set(0)
_failsafe_pwm( {0}),
_disarmed_pwm( {0}),
_num_failsafe_set(0),
_num_disarmed_set(0)
{
for (unsigned i = 0; i < _max_actuators; i++) {
_min_pwm[i] = PWM_DEFAULT_MIN;
@ -293,11 +294,11 @@ PX4FMU::init()
/* start the IO interface task */
_task = task_spawn_cmd("fmuservo",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2048,
(main_t)&PX4FMU::task_main_trampoline,
nullptr);
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2048,
(main_t)&PX4FMU::task_main_trampoline,
nullptr);
if (_task < 0) {
debug("task start failed: %d", errno);
@ -326,7 +327,7 @@ PX4FMU::set_mode(Mode mode)
switch (mode) {
case MODE_2PWM: // v1 multi-port with flow control lines as PWM
debug("MODE_2PWM");
/* default output rates */
_pwm_default_rate = 50;
_pwm_alt_rate = 50;
@ -340,7 +341,7 @@ PX4FMU::set_mode(Mode mode)
case MODE_4PWM: // v1 multi-port as 4 PWM outs
debug("MODE_4PWM");
/* default output rates */
_pwm_default_rate = 50;
_pwm_alt_rate = 50;
@ -354,7 +355,7 @@ PX4FMU::set_mode(Mode mode)
case MODE_6PWM: // v2 PWMs as 6 PWM outs
debug("MODE_6PWM");
/* default output rates */
_pwm_default_rate = 50;
_pwm_alt_rate = 50;
@ -396,6 +397,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
// get the channel mask for this rate group
uint32_t mask = up_pwm_servo_get_rate_group(group);
if (mask == 0)
continue;
@ -409,6 +411,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
// not a legal map, bail
return -EINVAL;
}
} else {
// set it - errors here are unexpected
if (alt != 0) {
@ -416,6 +419,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
warn("rate group set alt failed");
return -EINVAL;
}
} else {
if (up_pwm_servo_set_rate_group_update(group, _pwm_default_rate) != OK) {
warn("rate group set default failed");
@ -425,6 +429,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
}
}
}
_pwm_alt_rate_channels = rate_map;
_pwm_default_rate = default_rate;
_pwm_alt_rate = alt_rate;
@ -471,7 +476,7 @@ PX4FMU::task_main()
memset(&controls_effective, 0, sizeof(controls_effective));
/* advertise the effective control inputs */
_t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1),
&controls_effective);
&controls_effective);
pollfd fds[2];
fds[0].fd = _t_actuators;
@ -503,6 +508,7 @@ PX4FMU::task_main()
* We always mix at max rate; some channels may update slower.
*/
unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate;
if (_current_update_rate != max_rate) {
_current_update_rate = max_rate;
int update_rate_in_ms = int(1000 / _current_update_rate);
@ -511,6 +517,7 @@ PX4FMU::task_main()
if (update_rate_in_ms < 2) {
update_rate_in_ms = 2;
}
/* reject slower than 10 Hz updates */
if (update_rate_in_ms > 100) {
update_rate_in_ms = 100;
@ -532,6 +539,7 @@ PX4FMU::task_main()
log("poll error %d", errno);
usleep(1000000);
continue;
} else if (ret == 0) {
/* timeout: no control data, switch to failsafe values */
// warnx("no PWM: failsafe");
@ -553,12 +561,15 @@ PX4FMU::task_main()
case MODE_2PWM:
num_outputs = 2;
break;
case MODE_4PWM:
num_outputs = 4;
break;
case MODE_6PWM:
num_outputs = 6;
break;
default:
num_outputs = 0;
break;
@ -572,9 +583,9 @@ PX4FMU::task_main()
for (unsigned i = 0; i < num_outputs; i++) {
/* last resort: catch NaN, INF and out-of-band errors */
if (i >= outputs.noutputs ||
!isfinite(outputs.output[i]) ||
outputs.output[i] < -1.0f ||
outputs.output[i] > 1.0f) {
!isfinite(outputs.output[i]) ||
outputs.output[i] < -1.0f ||
outputs.output[i] > 1.0f) {
/*
* Value is NaN, INF or out of band - set to the minimum value.
* This will be clearly visible on the servo status and will limit the risk of accidentally
@ -592,6 +603,7 @@ PX4FMU::task_main()
for (unsigned i = 0; i < num_outputs; i++) {
controls_effective.control_effective[i] = (float)pwm_limited[i];
}
orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
/* output to the servos */
@ -613,11 +625,13 @@ PX4FMU::task_main()
/* update the armed status and check that we're not locked down */
bool set_armed = aa.armed && !aa.lockdown;
if (_armed != set_armed)
_armed = set_armed;
/* update PWM status if armed or if disarmed PWM values are set */
bool pwm_on = (aa.armed || _num_disarmed_set > 0);
if (_pwm_on != pwm_on) {
_pwm_on = pwm_on;
up_pwm_servo_arm(pwm_on);
@ -626,25 +640,31 @@ PX4FMU::task_main()
}
#ifdef HRT_PPM_CHANNEL
// see if we have new PPM input data
if (ppm_last_valid_decode != rc_in.timestamp) {
// we have a new PPM frame. Publish it.
rc_in.channel_count = ppm_decoded_channels;
if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) {
rc_in.channel_count = RC_INPUT_MAX_CHANNELS;
}
for (uint8_t i=0; i<rc_in.channel_count; i++) {
for (uint8_t i = 0; i < rc_in.channel_count; i++) {
rc_in.values[i] = ppm_buffer[i];
}
rc_in.timestamp = ppm_last_valid_decode;
/* lazily advertise on first publication */
if (to_input_rc == 0) {
to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_in);
} else {
} else {
orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in);
}
}
#endif
}
@ -753,142 +773,176 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
case PWM_SERVO_SET_FAILSAFE_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
/* discard if too many values are sent */
if (pwm->channel_count > _max_actuators) {
ret = -EINVAL;
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > _max_actuators) {
ret = -EINVAL;
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
_failsafe_pwm[i] = PWM_HIGHEST_MAX;
} else if (pwm->values[i] < PWM_LOWEST_MIN) {
_failsafe_pwm[i] = PWM_LOWEST_MIN;
} else {
_failsafe_pwm[i] = pwm->values[i];
}
}
/*
* update the counter
* this is needed to decide if disarmed PWM output should be turned on or not
*/
_num_failsafe_set = 0;
for (unsigned i = 0; i < _max_actuators; i++) {
if (_failsafe_pwm[i] > 0)
_num_failsafe_set++;
}
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
_failsafe_pwm[i] = PWM_HIGHEST_MAX;
} else if (pwm->values[i] < PWM_LOWEST_MIN) {
_failsafe_pwm[i] = PWM_LOWEST_MIN;
} else {
_failsafe_pwm[i] = pwm->values[i];
}
}
/*
* update the counter
* this is needed to decide if disarmed PWM output should be turned on or not
*/
_num_failsafe_set = 0;
for (unsigned i = 0; i < _max_actuators; i++) {
if (_failsafe_pwm[i] > 0)
_num_failsafe_set++;
}
break;
}
case PWM_SERVO_GET_FAILSAFE_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _failsafe_pwm[i];
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _failsafe_pwm[i];
}
pwm->channel_count = _max_actuators;
break;
}
pwm->channel_count = _max_actuators;
break;
}
case PWM_SERVO_SET_DISARMED_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
/* discard if too many values are sent */
if (pwm->channel_count > _max_actuators) {
ret = -EINVAL;
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > _max_actuators) {
ret = -EINVAL;
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
_disarmed_pwm[i] = PWM_HIGHEST_MAX;
} else if (pwm->values[i] < PWM_LOWEST_MIN) {
_disarmed_pwm[i] = PWM_LOWEST_MIN;
} else {
_disarmed_pwm[i] = pwm->values[i];
}
}
/*
* update the counter
* this is needed to decide if disarmed PWM output should be turned on or not
*/
_num_disarmed_set = 0;
for (unsigned i = 0; i < _max_actuators; i++) {
if (_disarmed_pwm[i] > 0)
_num_disarmed_set++;
}
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
_disarmed_pwm[i] = PWM_HIGHEST_MAX;
} else if (pwm->values[i] < PWM_LOWEST_MIN) {
_disarmed_pwm[i] = PWM_LOWEST_MIN;
} else {
_disarmed_pwm[i] = pwm->values[i];
}
}
/*
* update the counter
* this is needed to decide if disarmed PWM output should be turned on or not
*/
_num_disarmed_set = 0;
for (unsigned i = 0; i < _max_actuators; i++) {
if (_disarmed_pwm[i] > 0)
_num_disarmed_set++;
}
break;
}
case PWM_SERVO_GET_DISARMED_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _disarmed_pwm[i];
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _disarmed_pwm[i];
}
pwm->channel_count = _max_actuators;
break;
}
pwm->channel_count = _max_actuators;
break;
}
case PWM_SERVO_SET_MIN_PWM: {
struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
/* discard if too many values are sent */
if (pwm->channel_count > _max_actuators) {
ret = -EINVAL;
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > _max_actuators) {
ret = -EINVAL;
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] > PWM_HIGHEST_MIN) {
_min_pwm[i] = PWM_HIGHEST_MIN;
} else if (pwm->values[i] < PWM_LOWEST_MIN) {
_min_pwm[i] = PWM_LOWEST_MIN;
} else {
_min_pwm[i] = pwm->values[i];
}
}
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] > PWM_HIGHEST_MIN) {
_min_pwm[i] = PWM_HIGHEST_MIN;
} else if (pwm->values[i] < PWM_LOWEST_MIN) {
_min_pwm[i] = PWM_LOWEST_MIN;
} else {
_min_pwm[i] = pwm->values[i];
}
}
break;
}
case PWM_SERVO_GET_MIN_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _min_pwm[i];
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _min_pwm[i];
}
pwm->channel_count = _max_actuators;
arg = (unsigned long)&pwm;
break;
}
pwm->channel_count = _max_actuators;
arg = (unsigned long)&pwm;
break;
}
case PWM_SERVO_SET_MAX_PWM: {
struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
/* discard if too many values are sent */
if (pwm->channel_count > _max_actuators) {
ret = -EINVAL;
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > _max_actuators) {
ret = -EINVAL;
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] < PWM_LOWEST_MAX) {
_max_pwm[i] = PWM_LOWEST_MAX;
} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
_max_pwm[i] = PWM_HIGHEST_MAX;
} else {
_max_pwm[i] = pwm->values[i];
}
}
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] < PWM_LOWEST_MAX) {
_max_pwm[i] = PWM_LOWEST_MAX;
} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
_max_pwm[i] = PWM_HIGHEST_MAX;
} else {
_max_pwm[i] = pwm->values[i];
}
}
break;
}
case PWM_SERVO_GET_MAX_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _max_pwm[i];
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _max_pwm[i];
}
pwm->channel_count = _max_actuators;
arg = (unsigned long)&pwm;
break;
}
pwm->channel_count = _max_actuators;
arg = (unsigned long)&pwm;
break;
}
case PWM_SERVO_SET(5):
case PWM_SERVO_SET(4):
@ -910,6 +964,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
case PWM_SERVO_SET(0):
if (arg <= 2100) {
up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg);
} else {
ret = -EINVAL;
}
@ -946,18 +1001,21 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
break;
case PWM_SERVO_GET_COUNT:
case PWM_SERVO_GET_COUNT:
case MIXERIOCGETOUTPUTCOUNT:
switch (_mode) {
case MODE_6PWM:
*(unsigned *)arg = 6;
break;
case MODE_4PWM:
*(unsigned *)arg = 4;
break;
case MODE_2PWM:
*(unsigned *)arg = 2;
break;
default:
ret = -EINVAL;
break;
@ -1015,6 +1073,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
ret = -EINVAL;
}
}
break;
}
@ -1049,9 +1108,80 @@ PX4FMU::write(file *filp, const char *buffer, size_t len)
for (uint8_t i = 0; i < count; i++) {
up_pwm_servo_set(i, values[i]);
}
return count * 2;
}
void
PX4FMU::sensor_reset(int ms)
{
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
if (ms < 1) {
ms = 1;
}
/* disable SPI bus */
stm32_configgpio(GPIO_SPI_CS_GYRO_OFF);
stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF);
stm32_configgpio(GPIO_SPI_CS_BARO_OFF);
stm32_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0);
stm32_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0);
stm32_configgpio(GPIO_SPI1_SCK_OFF);
stm32_configgpio(GPIO_SPI1_MISO_OFF);
stm32_configgpio(GPIO_SPI1_MOSI_OFF);
stm32_gpiowrite(GPIO_SPI1_SCK_OFF, 0);
stm32_gpiowrite(GPIO_SPI1_MISO_OFF, 0);
stm32_gpiowrite(GPIO_SPI1_MOSI_OFF, 0);
stm32_configgpio(GPIO_GYRO_DRDY_OFF);
stm32_configgpio(GPIO_MAG_DRDY_OFF);
stm32_configgpio(GPIO_ACCEL_DRDY_OFF);
stm32_gpiowrite(GPIO_GYRO_DRDY_OFF, 0);
stm32_gpiowrite(GPIO_MAG_DRDY_OFF, 0);
stm32_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0);
/* set the sensor rail off */
stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN);
stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0);
/* wait for the sensor rail to reach GND */
usleep(ms * 1000);
warnx("reset done, %d ms", ms);
/* re-enable power */
/* switch the sensor rail back on */
stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1);
/* wait a bit before starting SPI, different times didn't influence results */
usleep(100);
/* reconfigure the SPI pins */
#ifdef CONFIG_STM32_SPI1
stm32_configgpio(GPIO_SPI_CS_GYRO);
stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG);
stm32_configgpio(GPIO_SPI_CS_BARO);
stm32_configgpio(GPIO_SPI_CS_MPU);
/* De-activate all peripherals,
* required for some peripheral
* state machines
*/
stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
#endif
#endif
}
void
PX4FMU::gpio_reset(void)
{
@ -1062,6 +1192,7 @@ PX4FMU::gpio_reset(void)
for (unsigned i = 0; i < _ngpio; i++) {
if (_gpio_tab[i].input != 0) {
stm32_configgpio(_gpio_tab[i].input);
} else if (_gpio_tab[i].output != 0) {
stm32_configgpio(_gpio_tab[i].output);
}
@ -1078,6 +1209,7 @@ void
PX4FMU::gpio_set_function(uint32_t gpios, int function)
{
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
/*
* GPIOs 0 and 1 must have the same direction as they are buffered
* by a shared 2-port driver. Any attempt to set either sets both.
@ -1089,6 +1221,7 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function)
if (GPIO_SET_OUTPUT == function)
stm32_gpiowrite(GPIO_GPIO_DIR, 1);
}
#endif
/* configure selected GPIOs as required */
@ -1113,9 +1246,11 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function)
}
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
/* flip buffer to input mode if required */
if ((GPIO_SET_INPUT == function) && (gpios & 3))
stm32_gpiowrite(GPIO_GPIO_DIR, 0);
#endif
}
@ -1154,6 +1289,10 @@ PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg)
gpio_reset();
break;
case GPIO_SENSOR_RAIL_RESET:
sensor_reset(arg);
break;
case GPIO_SET_OUTPUT:
case GPIO_SET_INPUT:
case GPIO_SET_ALT_1:
@ -1227,8 +1366,9 @@ fmu_new_mode(PortMode new_mode)
#endif
break;
/* mixed modes supported on v1 board only */
/* mixed modes supported on v1 board only */
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
case PORT_FULL_SERIAL:
/* set all multi-GPIOs to serial mode */
gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4;
@ -1251,6 +1391,7 @@ fmu_new_mode(PortMode new_mode)
servo_mode = PX4FMU::MODE_2PWM;
break;
#endif
default:
return -1;
}
@ -1304,15 +1445,31 @@ fmu_stop(void)
return ret;
}
void
sensor_reset(int ms)
{
int fd;
int ret;
fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
if (fd < 0)
errx(1, "open fail");
if (ioctl(fd, GPIO_SENSOR_RAIL_RESET, ms) < 0)
err(1, "servo arm failed");
}
void
test(void)
{
int fd;
unsigned servo_count = 0;
unsigned servo_count = 0;
unsigned pwm_value = 1000;
int direction = 1;
int ret;
fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
if (fd < 0)
@ -1320,9 +1477,9 @@ test(void)
if (ioctl(fd, PWM_SERVO_ARM, 0) < 0) err(1, "servo arm failed");
if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) != 0) {
err(1, "Unable to get servo count\n");
}
if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) != 0) {
err(1, "Unable to get servo count\n");
}
warnx("Testing %u servos", (unsigned)servo_count);
@ -1335,32 +1492,38 @@ test(void)
for (;;) {
/* sweep all servos between 1000..2000 */
servo_position_t servos[servo_count];
for (unsigned i = 0; i < servo_count; i++)
servos[i] = pwm_value;
if (direction == 1) {
// use ioctl interface for one direction
for (unsigned i=0; i < servo_count; i++) {
if (ioctl(fd, PWM_SERVO_SET(i), servos[i]) < 0) {
err(1, "servo %u set failed", i);
}
}
} else {
// and use write interface for the other direction
ret = write(fd, servos, sizeof(servos));
if (ret != (int)sizeof(servos))
err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret);
}
if (direction == 1) {
// use ioctl interface for one direction
for (unsigned i = 0; i < servo_count; i++) {
if (ioctl(fd, PWM_SERVO_SET(i), servos[i]) < 0) {
err(1, "servo %u set failed", i);
}
}
} else {
// and use write interface for the other direction
ret = write(fd, servos, sizeof(servos));
if (ret != (int)sizeof(servos))
err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret);
}
if (direction > 0) {
if (pwm_value < 2000) {
pwm_value++;
} else {
direction = -1;
}
} else {
if (pwm_value > 1000) {
pwm_value--;
} else {
direction = 1;
}
@ -1372,6 +1535,7 @@ test(void)
if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value))
err(1, "error reading PWM servo %d", i);
if (value != servos[i])
errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]);
}
@ -1379,12 +1543,14 @@ test(void)
/* Check if user wants to quit */
char c;
ret = poll(&fds, 1, 0);
if (ret > 0) {
read(0, &c, 1);
if (c == 0x03 || c == 0x63 || c == 'q') {
warnx("User abort\n");
break;
break;
}
}
}
@ -1457,6 +1623,7 @@ fmu_main(int argc, char *argv[])
new_mode = PORT_FULL_PWM;
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
} else if (!strcmp(verb, "mode_serial")) {
new_mode = PORT_FULL_SERIAL;
@ -1489,11 +1656,24 @@ fmu_main(int argc, char *argv[])
if (!strcmp(verb, "fake"))
fake(argc - 1, argv + 1);
if (!strcmp(verb, "sensor_reset")) {
if (argc > 2) {
int reset_time = strtol(argv[2], 0, 0);
sensor_reset(reset_time);
} else {
sensor_reset(0);
warnx("resettet default time");
}
exit(0);
}
fprintf(stderr, "FMU: unrecognised command, try:\n");
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test\n");
#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
fprintf(stderr, " mode_gpio, mode_pwm, test\n");
fprintf(stderr, " mode_gpio, mode_pwm, test, sensor_reset [milliseconds]\n");
#endif
exit(1);
}

112
src/drivers/px4io/px4io.cpp
View File

@ -54,6 +54,7 @@
#include <unistd.h>
#include <fcntl.h>
#include <math.h>
#include <crc32.h>
#include <arch/board/board.h>
@ -95,6 +96,8 @@ extern device::Device *PX4IO_serial_interface() weak_function;
#define PX4IO_SET_DEBUG _IOC(0xff00, 0)
#define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1)
#define PX4IO_REBOOT_BOOTLOADER _IOC(0xff00, 2)
#define PX4IO_CHECK_CRC _IOC(0xff00, 3)
#define UPDATE_INTERVAL_MIN 2 // 2 ms -> 500 Hz
#define ORB_CHECK_INTERVAL 200000 // 200 ms -> 5 Hz
@ -1659,11 +1662,13 @@ void
PX4IO::print_status()
{
/* basic configuration */
printf("protocol %u hardware %u bootloader %u buffer %uB\n",
printf("protocol %u hardware %u bootloader %u buffer %uB crc 0x%04x%04x\n",
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION),
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION),
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION),
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER));
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC+1));
printf("%u controls %u actuators %u R/C inputs %u analog inputs %u relays\n",
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT),
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT),
@ -2146,6 +2151,29 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG, arg);
break;
case PX4IO_REBOOT_BOOTLOADER:
if (system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
return -EINVAL;
/* reboot into bootloader - arg must be PX4IO_REBOOT_BL_MAGIC */
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_REBOOT_BL, arg);
// we don't expect a reply from this operation
ret = OK;
break;
case PX4IO_CHECK_CRC: {
/* check IO firmware CRC against passed value */
uint32_t io_crc = 0;
ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC, (uint16_t *)&io_crc, 2);
if (ret != OK)
return ret;
if (io_crc != arg) {
debug("crc mismatch 0x%08x 0x%08x", (unsigned)io_crc, arg);
return -EINVAL;
}
break;
}
case PX4IO_INAIR_RESTART_ENABLE:
/* set/clear the 'in-air restart' bit */
@ -2530,7 +2558,7 @@ px4io_main(int argc, char *argv[])
}
PX4IO_Uploader *up;
const char *fn[3];
const char *fn[4];
/* work out what we're uploading... */
if (argc > 2) {
@ -2673,6 +2701,82 @@ 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
*/
if (argc <= 2) {
printf("usage: px4io checkcrc filename\n");
exit(1);
}
if (g_dev == nullptr) {
printf("px4io is not started\n");
exit(1);
}
int fd = open(argv[2], O_RDONLY);
if (fd == -1) {
printf("open of %s failed - %d\n", argv[2], errno);
exit(1);
}
const uint32_t app_size_max = 0xf000;
uint32_t fw_crc = 0;
uint32_t nbytes = 0;
while (true) {
uint8_t buf[16];
int n = read(fd, buf, sizeof(buf));
if (n <= 0) break;
fw_crc = crc32part(buf, n, fw_crc);
nbytes += n;
}
close(fd);
while (nbytes < app_size_max) {
uint8_t b = 0xff;
fw_crc = crc32part(&b, 1, fw_crc);
nbytes++;
}
int ret = g_dev->ioctl(nullptr, PX4IO_CHECK_CRC, fw_crc);
if (ret != OK) {
printf("check CRC failed - %d\n", ret);
exit(1);
}
printf("CRCs match\n");
exit(0);
}
if (!strcmp(argv[1], "rx_dsm") ||
!strcmp(argv[1], "rx_dsm_10bit") ||
!strcmp(argv[1], "rx_dsm_11bit") ||
@ -2690,5 +2794,5 @@ px4io_main(int argc, char *argv[])
bind(argc, argv);
out:
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind' or 'update'");
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind', 'checkcrc', 'forceupdate' or 'update'");
}

5
src/drivers/px4io/px4io_uploader.cpp
View File

@ -274,7 +274,10 @@ PX4IO_Uploader::drain()
int ret;
do {
ret = recv(c, 1000);
// the small recv timeout here is to allow for fast
// drain when rebooting the io board for a forced
// update of the fw without using the safety switch
ret = recv(c, 40);
#ifdef UDEBUG
if (ret == OK) {

25
src/drivers/stm32/drv_hrt.c
View File

@ -733,6 +733,13 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte
irqstate_t flags = irqsave();
/* if the entry is currently queued, remove it */
/* note that we are using a potentially uninitialised
entry->link here, but it is safe as sq_rem() doesn't
dereference the passed node unless it is found in the
list. So we potentially waste a bit of time searching the
queue for the uninitialised entry->link but we don't do
anything actually unsafe.
*/
if (entry->deadline != 0)
sq_rem(&entry->link, &callout_queue);
@ -839,7 +846,12 @@ hrt_call_invoke(void)
/* if the callout has a non-zero period, it has to be re-entered */
if (call->period != 0) {
call->deadline = deadline + call->period;
// re-check call->deadline to allow for
// callouts to re-schedule themselves
// using hrt_call_delay()
if (call->deadline <= now) {
call->deadline = deadline + call->period;
}
hrt_call_enter(call);
}
}
@ -906,5 +918,16 @@ hrt_latency_update(void)
latency_counters[index]++;
}
void
hrt_call_init(struct hrt_call *entry)
{
memset(entry, 0, sizeof(*entry));
}
void
hrt_call_delay(struct hrt_call *entry, hrt_abstime delay)
{
entry->deadline = hrt_absolute_time() + delay;
}
#endif /* HRT_TIMER */

8
src/lib/mathlib/math/filter/LowPassFilter2p.cpp
View File

@ -46,6 +46,10 @@ namespace math
void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq)
{
_cutoff_freq = cutoff_freq;
if (_cutoff_freq <= 0.0f) {
// no filtering
return;
}
float fr = sample_freq/_cutoff_freq;
float ohm = tanf(M_PI_F/fr);
float c = 1.0f+2.0f*cosf(M_PI_F/4.0f)*ohm + ohm*ohm;
@ -58,6 +62,10 @@ void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq)
float LowPassFilter2p::apply(float sample)
{
if (_cutoff_freq <= 0.0f) {
// no filtering
return sample;
}
// do the filtering
float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
if (isnan(delay_element_0) || isinf(delay_element_0)) {

10
src/modules/mavlink/missionlib.c
View File

@ -276,7 +276,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
next_setpoint_index = index + 1;
}
while (next_setpoint_index < wpm->size - 1) {
while (next_setpoint_index < wpm->size) {
if (wpm->waypoints[next_setpoint_index].frame == (int)MAV_FRAME_GLOBAL && (wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT ||
wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
@ -301,7 +301,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
sp.lon = wpm->waypoints[last_setpoint_index].y * 1e7f;
sp.altitude = wpm->waypoints[last_setpoint_index].z;
sp.altitude_is_relative = false;
sp.yaw = (wpm->waypoints[last_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F;
sp.yaw = _wrap_pi(wpm->waypoints[last_setpoint_index].param4 / 180.0f * M_PI_F);
set_special_fields(wpm->waypoints[last_setpoint_index].param1,
wpm->waypoints[last_setpoint_index].param2,
wpm->waypoints[last_setpoint_index].param3,
@ -317,7 +317,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
sp.lon = wpm->waypoints[next_setpoint_index].y * 1e7f;
sp.altitude = wpm->waypoints[next_setpoint_index].z;
sp.altitude_is_relative = false;
sp.yaw = (wpm->waypoints[next_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F;
sp.yaw = _wrap_pi(wpm->waypoints[next_setpoint_index].param4 / 180.0f * M_PI_F);
set_special_fields(wpm->waypoints[next_setpoint_index].param1,
wpm->waypoints[next_setpoint_index].param2,
wpm->waypoints[next_setpoint_index].param3,
@ -343,7 +343,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
sp.lon = param6_lon_y * 1e7f;
sp.altitude = param7_alt_z;
sp.altitude_is_relative = true;
sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F;
sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
set_special_fields(param1, param2, param3, param4, command, &sp);
/* Initialize publication if necessary */
@ -364,7 +364,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
sp.x = param5_lat_x;
sp.y = param6_lon_y;
sp.z = param7_alt_z;
sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F;
sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
/* Initialize publication if necessary */
if (local_position_setpoint_pub < 0) {

5
src/modules/px4iofirmware/protocol.h
View File

@ -186,6 +186,11 @@ enum { /* DSM bind states */
/* 8 */
#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */
#define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */
#define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */
#define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */
/* autopilot control values, -10000..10000 */
#define PX4IO_PAGE_CONTROLS 51 /* 0..CONFIG_CONTROL_COUNT */

33
src/modules/px4iofirmware/px4io.c
View File

@ -45,6 +45,7 @@
#include <string.h>
#include <poll.h>
#include <signal.h>
#include <crc32.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_hrt.h>
@ -117,6 +118,29 @@ show_debug_messages(void)
}
}
static void
heartbeat_blink(void)
{
static bool heartbeat = false;
LED_BLUE(heartbeat = !heartbeat);
}
static void
calculate_fw_crc(void)
{
#define APP_SIZE_MAX 0xf000
#define APP_LOAD_ADDRESS 0x08001000
// compute CRC of the current firmware
uint32_t sum = 0;
for (unsigned p = 0; p < APP_SIZE_MAX; p += 4) {
uint32_t bytes = *(uint32_t *)(p + APP_LOAD_ADDRESS);
sum = crc32part((uint8_t *)&bytes, sizeof(bytes), sum);
}
r_page_setup[PX4IO_P_SETUP_CRC] = sum & 0xFFFF;
r_page_setup[PX4IO_P_SETUP_CRC+1] = sum >> 16;
}
int
user_start(int argc, char *argv[])
{
@ -129,6 +153,9 @@ user_start(int argc, char *argv[])
/* configure the high-resolution time/callout interface */
hrt_init();
/* calculate our fw CRC so FMU can decide if we need to update */
calculate_fw_crc();
/*
* Poll at 1ms intervals for received bytes that have not triggered
* a DMA event.
@ -201,6 +228,7 @@ user_start(int argc, char *argv[])
*/
uint64_t last_debug_time = 0;
uint64_t last_heartbeat_time = 0;
for (;;) {
/* track the rate at which the loop is running */
@ -216,6 +244,11 @@ user_start(int argc, char *argv[])
controls_tick();
perf_end(controls_perf);
if ((hrt_absolute_time() - last_heartbeat_time) > 250*1000) {
last_heartbeat_time = hrt_absolute_time();
heartbeat_blink();
}
#if 0
/* check for debug activity */
show_debug_messages();

27
src/modules/px4iofirmware/registers.c
View File

@ -45,6 +45,8 @@
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
#include <systemlib/systemlib.h>
#include <stm32_pwr.h>
#include "px4io.h"
#include "protocol.h"
@ -154,6 +156,8 @@ volatile uint16_t r_page_setup[] =
[PX4IO_P_SETUP_VBATT_SCALE] = 10000,
#endif
[PX4IO_P_SETUP_SET_DEBUG] = 0,
[PX4IO_P_SETUP_REBOOT_BL] = 0,
[PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0,
};
#define PX4IO_P_SETUP_FEATURES_VALID (0)
@ -501,6 +505,29 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]);
break;
case PX4IO_P_SETUP_REBOOT_BL:
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ||
(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
// don't allow reboot while armed
break;
}
// check the magic value
if (value != PX4IO_REBOOT_BL_MAGIC)
break;
// note that we don't set BL_WAIT_MAGIC in
// BKP_DR1 as that is not necessary given the
// timing of the forceupdate command. The
// bootloader on px4io waits for enough time
// anyway, and this method works with older
// bootloader versions (tested with both
// revision 3 and revision 4).
up_systemreset();
break;
case PX4IO_P_SETUP_DSM:
dsm_bind(value & 0x0f, (value >> 4) & 7);
break;

16
src/modules/px4iofirmware/safety.c
View File

@ -77,7 +77,6 @@ static unsigned blink_counter = 0;
static bool safety_button_pressed;
static void safety_check_button(void *arg);
static void heartbeat_blink(void *arg);
static void failsafe_blink(void *arg);
void
@ -86,9 +85,6 @@ safety_init(void)
/* arrange for the button handler to be called at 10Hz */
hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL);
/* arrange for the heartbeat handler to be called at 4Hz */
hrt_call_every(&heartbeat_call, 1000, 250000, heartbeat_blink, NULL);
/* arrange for the failsafe blinker to be called at 8Hz */
hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL);
}
@ -163,16 +159,6 @@ safety_check_button(void *arg)
}
}
static void
heartbeat_blink(void *arg)
{
static bool heartbeat = false;
/* XXX add flags here that need to be frobbed by various loops */
LED_BLUE(heartbeat = !heartbeat);
}
static void
failsafe_blink(void *arg)
{
@ -192,4 +178,4 @@ failsafe_blink(void *arg)
}
LED_AMBER(failsafe);
}
}

2
src/systemcmds/pwm/pwm.c
View File

@ -210,7 +210,7 @@ pwm_main(int argc, char *argv[])
err(1, "PWM_SERVO_GET_COUNT");
if (!strcmp(argv[1], "arm")) {
/* tell IO that its ok to disable its safety with the switch */
/* tell safety that its ok to disable it with the switch */
ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
if (ret != OK)
err(1, "PWM_SERVO_SET_ARM_OK");

5
src/systemcmds/tests/module.mk
View File

@ -24,6 +24,7 @@ SRCS = test_adc.c \
test_uart_loopback.c \
test_uart_send.c \
test_mixer.cpp \
tests_file.c \
test_file.c \
tests_main.c \
tests_param.c
test_param.c \
test_ppm_loopback.c

2
src/systemcmds/tests/tests_file.c → src/systemcmds/tests/test_file.c
View File

@ -32,7 +32,7 @@
****************************************************************************/
/**
* @file tests_file.c
* @file test_file.c
*
* File write test.
*/

2
src/systemcmds/tests/tests_param.c → src/systemcmds/tests/test_param.c
View File

@ -32,7 +32,7 @@
****************************************************************************/
/**
* @file tests_param.c
* @file test_param.c
*
* Tests related to the parameter system.
*/

178
src/systemcmds/tests/test_ppm_loopback.c Normal file
View File

@ -0,0 +1,178 @@
/****************************************************************************
*
* 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 <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 <uORB/topics/rc_channels.h>
#include <systemlib/err.h>
#include "tests.h"
#include <math.h>
#include <float.h>
int test_ppm_loopback(int argc, char *argv[])
{
int _rc_sub = orb_subscribe(ORB_ID(input_rc));
int servo_fd, result;
servo_position_t data[PWM_OUTPUT_MAX_CHANNELS];
servo_position_t pos;
servo_fd = open(PWM_OUTPUT_DEVICE_PATH, O_RDWR);
if (servo_fd < 0) {
printf("failed opening /dev/pwm_servo\n");
}
printf("Servo readback, pairs of values should match defaults\n");
unsigned servo_count;
result = ioctl(servo_fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
if (result != OK) {
warnx("PWM_SERVO_GET_COUNT");
return ERROR;
}
for (unsigned i = 0; i < servo_count; i++) {
result = ioctl(servo_fd, PWM_SERVO_GET(i), (unsigned long)&pos);
if (result < 0) {
printf("failed reading channel %u\n", i);
}
//printf("%u: %u %u\n", i, pos, data[i]);
}
// /* tell safety that its ok to disable it with the switch */
// result = ioctl(servo_fd, PWM_SERVO_SET_ARM_OK, 0);
// if (result != OK)
// warnx("FAIL: PWM_SERVO_SET_ARM_OK");
// tell output device that the system is armed (it will output values if safety is off)
// result = ioctl(servo_fd, PWM_SERVO_ARM, 0);
// if (result != OK)
// warnx("FAIL: PWM_SERVO_ARM");
int pwm_values[] = {1200, 1300, 1900, 1700, 1500, 1250, 1800, 1400};
// for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) {
// result = ioctl(servo_fd, PWM_SERVO_SET(i), pwm_values[i]);
// if (result) {
// (void)close(servo_fd);
// return ERROR;
// } else {
// warnx("channel %d set to %d", i, pwm_values[i]);
// }
// }
warnx("servo count: %d", servo_count);
struct pwm_output_values pwm_out = {.values = {0}, .channel_count = 0};
for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) {
pwm_out.values[i] = pwm_values[i];
//warnx("channel %d: disarmed PWM: %d", i+1, pwm_values[i]);
pwm_out.channel_count++;
}
result = ioctl(servo_fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_out);
/* give driver 10 ms to propagate */
/* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */
struct rc_input_values rc_input;
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);
if (rc_updated) {
orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
// int ppm_fd = open(RC_INPUT_DEVICE_PATH, O_RDONLY);
// struct rc_input_values rc;
// result = read(ppm_fd, &rc, sizeof(rc));
// if (result != sizeof(rc)) {
// warnx("Error reading RC output");
// (void)close(servo_fd);
// (void)close(ppm_fd);
// return ERROR;
// }
/* go and check values */
for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) {
if (fabsf(rc_input.values[i] - pwm_values[i]) > 10) {
warnx("comparison fail: RC: %d, expected: %d", rc_input.values[i], pwm_values[i]);
(void)close(servo_fd);
return ERROR;
}
}
} else {
warnx("failed reading RC input data");
return ERROR;
}
warnx("PPM LOOPBACK TEST PASSED SUCCESSFULLY!");
return 0;
}

98
src/systemcmds/tests/test_sensors.c
View File

@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* 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
@ -49,6 +48,8 @@
#include <fcntl.h>
#include <errno.h>
#include <debug.h>
#include <math.h>
#include <systemlib/err.h>
#include <arch/board/board.h>
@ -77,6 +78,7 @@ 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[]);
/****************************************************************************
* Private Data
@ -91,6 +93,7 @@ struct {
{"gyro", "/dev/gyro", gyro},
{"mag", "/dev/mag", mag},
{"baro", "/dev/baro", baro},
{"mpu6k", "/dev/mpu6k", mpu6k},
{NULL, NULL, NULL}
};
@ -133,23 +136,83 @@ accel(int argc, char *argv[])
printf("\tACCEL accel: x:%8.4f\ty:%8.4f\tz:%8.4f m/s^2\n", (double)buf.x, (double)buf.y, (double)buf.z);
}
// /* wait at least 10ms, sensor should have data after no more than 2ms */
// usleep(100000);
// ret = read(fd, buf, sizeof(buf));
// if (ret != sizeof(buf)) {
// printf("\tMPU-6000: read2 fail (%d)\n", ret);
// return ERROR;
// } else {
// printf("\tMPU-6000 values: acc: x:%d\ty:%d\tz:%d\tgyro: r:%d\tp:%d\ty:%d\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]);
// }
/* XXX more tests here */
if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) {
warnx("MPU6K acceleration values out of range!");
return ERROR;
}
/* Let user know everything is ok */
printf("\tOK: ACCEL passed all tests successfully\n");
close(fd);
return OK;
}
static int
mpu6k(int argc, char *argv[])
{
printf("\tMPU6K: 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);
if (fd < 0) {
printf("\tMPU6K: open fail, run <mpu6000 start> first.\n");
return ERROR;
}
/* wait at least 100ms, sensor should have data after no more than 20ms */
usleep(100000);
/* read data - expect samples */
ret = read(fd, &buf, sizeof(buf));
if (ret != sizeof(buf)) {
printf("\tMPU6K: 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);
}
if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) {
warnx("MPU6K acceleration values out of range!");
return ERROR;
}
/* Let user know everything is ok */
printf("\tOK: MPU6K ACCEL 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;
}
@ -187,6 +250,7 @@ gyro(int argc, char *argv[])
/* Let user know everything is ok */
printf("\tOK: GYRO passed all tests successfully\n");
close(fd);
return OK;
}
@ -224,6 +288,7 @@ mag(int argc, char *argv[])
/* Let user know everything is ok */
printf("\tOK: MAG passed all tests successfully\n");
close(fd);
return OK;
}
@ -261,6 +326,7 @@ baro(int argc, char *argv[])
/* Let user know everything is ok */
printf("\tOK: BARO passed all tests successfully\n");
close(fd);
return OK;
}

66
src/systemcmds/tests/test_servo.c
View File

@ -1,7 +1,6 @@
/****************************************************************************
* px4/sensors/test_hrt.c
*
* 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
@ -13,7 +12,7 @@
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name NuttX nor the names of its contributors may be
* 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.
*
@ -32,9 +31,11 @@
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
/**
* @file test_servo.c
* Tests the servo outputs
*
*/
#include <nuttx/config.h>
@ -55,39 +56,6 @@
#include "tests.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Private Types
****************************************************************************/
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
/****************************************************************************
* Private Data
****************************************************************************/
/****************************************************************************
* Public Data
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: test_servo
****************************************************************************/
int test_servo(int argc, char *argv[])
{
int fd, result;
@ -110,7 +78,14 @@ int test_servo(int argc, char *argv[])
printf("Servo readback, pairs of values should match defaults\n");
for (unsigned i = 0; i < PWM_OUTPUT_MAX_CHANNELS; i++) {
unsigned servo_count;
result = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
if (result != OK) {
warnx("PWM_SERVO_GET_COUNT");
return ERROR;
}
for (unsigned i = 0; i < servo_count; i++) {
result = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&pos);
if (result < 0) {
@ -122,11 +97,20 @@ int test_servo(int argc, char *argv[])
}
printf("Servos arming at default values\n");
/* tell safety that its ok to disable it with the switch */
result = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
if (result != OK)
warnx("FAIL: PWM_SERVO_SET_ARM_OK");
/* tell output device that the system is armed (it will output values if safety is off) */
result = ioctl(fd, PWM_SERVO_ARM, 0);
if (result != OK)
warnx("FAIL: PWM_SERVO_ARM");
usleep(5000000);
printf("Advancing channel 0 to 1500\n");
result = ioctl(fd, PWM_SERVO_SET(0), 1500);
printf("Advancing channel 1 to 1800\n");
result = ioctl(fd, PWM_SERVO_SET(1), 1800);
out:
return 0;
}

70
src/systemcmds/tests/test_uart_loopback.c
View File

@ -1,8 +1,6 @@
/****************************************************************************
* px4/sensors/test_gpio.c
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* 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
@ -14,7 +12,7 @@
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name NuttX nor the names of its contributors may be
* 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.
*
@ -33,9 +31,11 @@
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
/**
* @file test_uart_loopback.c
* Tests the uart outputs
*
*/
#include <nuttx/config.h>
@ -55,40 +55,6 @@
#include <math.h>
#include <float.h>
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Private Types
****************************************************************************/
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
/****************************************************************************
* Private Data
****************************************************************************/
/****************************************************************************
* Public Data
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: test_led
****************************************************************************/
int test_uart_loopback(int argc, char *argv[])
{
@ -97,11 +63,11 @@ int test_uart_loopback(int argc, char *argv[])
int uart5_nwrite = 0;
int uart2_nwrite = 0;
int uart1 = open("/dev/ttyS0", O_RDWR | O_NOCTTY); //
/* opening stdout */
int stdout_fd = 1;
/* assuming NuttShell is on UART1 (/dev/ttyS0) */
int uart2 = open("/dev/ttyS1", O_RDWR | O_NONBLOCK | O_NOCTTY); //
int uart5 = open("/dev/ttyS2", O_RDWR | O_NONBLOCK | O_NOCTTY); //
int uart2 = open("/dev/ttyS1", O_RDWR | O_NONBLOCK | O_NOCTTY);
int uart5 = open("/dev/ttyS2", O_RDWR | O_NONBLOCK | O_NOCTTY);
if (uart2 < 0) {
printf("ERROR opening UART2, aborting..\n");
@ -113,7 +79,7 @@ int test_uart_loopback(int argc, char *argv[])
exit(uart5);
}
uint8_t sample_uart1[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', '\n'};
uint8_t sample_stdout_fd[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', '\n'};
uint8_t sample_uart2[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', 0};
uint8_t sample_uart5[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', 0};
@ -121,7 +87,7 @@ int test_uart_loopback(int argc, char *argv[])
for (i = 0; i < 1000; i++) {
// printf("TEST #%d\n",i);
write(uart1, sample_uart1, sizeof(sample_uart1));
write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd));
/* uart2 -> uart5 */
r = write(uart2, sample_uart2, sizeof(sample_uart2));
@ -130,7 +96,7 @@ int test_uart_loopback(int argc, char *argv[])
uart2_nwrite += r;
// printf("TEST #%d\n",i);
write(uart1, sample_uart1, sizeof(sample_uart1));
write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd));
/* uart2 -> uart5 */
r = write(uart5, sample_uart5, sizeof(sample_uart5));
@ -139,7 +105,7 @@ int test_uart_loopback(int argc, char *argv[])
uart5_nwrite += r;
// printf("TEST #%d\n",i);
write(uart1, sample_uart1, sizeof(sample_uart1));
write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd));
/* try to read back values */
do {
@ -150,7 +116,7 @@ int test_uart_loopback(int argc, char *argv[])
} while (r > 0);
// printf("TEST #%d\n",i);
write(uart1, sample_uart1, sizeof(sample_uart1));
write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd));
do {
r = read(uart2, sample_uart5, sizeof(sample_uart5));
@ -160,7 +126,7 @@ int test_uart_loopback(int argc, char *argv[])
} while (r > 0);
// printf("TEST #%d\n",i);
// write(uart1, sample_uart1, sizeof(sample_uart5));
// write(stdout_fd, sample_stdout_fd, sizeof(sample_uart5));
}
for (i = 0; i < 200000; i++) {
@ -181,7 +147,7 @@ int test_uart_loopback(int argc, char *argv[])
}
close(uart1);
close(stdout_fd);
close(uart2);
close(uart5);

9
src/systemcmds/tests/tests.h
View File

@ -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
@ -34,6 +34,12 @@
#ifndef __APPS_PX4_TESTS_H
#define __APPS_PX4_TESTS_H
/**
* @file tests.h
* Tests declaration file.
*
*/
/****************************************************************************
* Included Files
****************************************************************************/
@ -88,6 +94,7 @@ extern int test_int(int argc, char *argv[]);
extern int test_float(int argc, char *argv[]);
extern int test_ppm(int argc, char *argv[]);
extern int test_servo(int argc, char *argv[]);
extern int test_ppm_loopback(int argc, char *argv[]);
extern int test_uart_loopback(int argc, char *argv[]);
extern int test_uart_baudchange(int argc, char *argv[]);
extern int test_cpuload(int argc, char *argv[]);

14
src/systemcmds/tests/tests_main.c
View File

@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @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
@ -35,6 +34,8 @@
/**
* @file tests_main.c
* Tests main file, loads individual tests.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include <nuttx/config.h>
@ -57,14 +58,6 @@
#include "tests.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Private Types
****************************************************************************/
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
@ -94,6 +87,7 @@ const struct {
{"hrt", test_hrt, OPT_NOJIGTEST | OPT_NOALLTEST},
{"ppm", test_ppm, OPT_NOJIGTEST | OPT_NOALLTEST},
{"servo", test_servo, OPT_NOJIGTEST | OPT_NOALLTEST},
{"ppm_loopback", test_ppm_loopback, OPT_NOALLTEST},
{"adc", test_adc, OPT_NOJIGTEST},
{"jig_voltages", test_jig_voltages, OPT_NOALLTEST},
{"uart_loopback", test_uart_loopback, OPT_NOJIGTEST | OPT_NOALLTEST},