fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'upstream/master' into fw_autoland_att_tecs_navigator

Browse Source 
Conflicts:
	src/modules/mavlink/missionlib.c
	src/systemcmds/tests/module.mk
This commit is contained in:
Thomas Gubler 2013-12-05 10:53:41 +01:00
commit b86161dd45
27 changed files with 1024 additions and 326 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

@ -318,6 +318,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 ]

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");

13
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,7 @@ __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);
#endif
#ifdef CONFIG_STM32_SPI2
@ -99,6 +101,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 +109,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 +117,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:

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 */

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

@ -92,9 +92,12 @@ 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_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_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_100HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4))
#define ADDR_CTRL_REG2 0x21
#define ADDR_CTRL_REG3 0x22
@ -218,6 +221,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.
@ -574,6 +582,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 +662,15 @@ L3GD20::set_samplerate(unsigned frequency)
} else if (frequency <= 200) {
_current_rate = 190;
bits |= RATE_190HZ_LP_25HZ;
bits |= RATE_190HZ_LP_70HZ;
} else if (frequency <= 400) {
_current_rate = 380;
bits |= RATE_380HZ_LP_20HZ;
bits |= RATE_380HZ_LP_100HZ;
} else if (frequency <= 800) {
_current_rate = 760;
bits |= RATE_760HZ_LP_30HZ;
bits |= RATE_760HZ_LP_100HZ;
} else {
return -EINVAL;
}
@ -699,9 +707,19 @@ L3GD20::stop()
hrt_cancel(&_call);
}
void
L3GD20::disable_i2c(void)
{
uint8_t a = read_reg(0x05);
write_reg(0x05, (0x20 | a));
}
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 */
@ -716,7 +734,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);
@ -753,6 +771,7 @@ 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));

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

@ -82,19 +82,24 @@ static const int ERROR = -1;
/* 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 +117,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 +226,11 @@ public:
*/
void print_info();
/**
* dump register values
*/
void print_registers();
protected:
virtual int probe();
@ -270,6 +300,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.
@ -533,9 +568,25 @@ 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);
@ -548,7 +599,7 @@ LSM303D::reset()
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);
accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);
accel_set_onchip_lowpass_filter_bandwidth(0); // this gives 773Hz
mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA);
mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE);
@ -1248,6 +1299,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));
@ -1325,6 +1377,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));
@ -1380,6 +1433,63 @@ 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" },
{ 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);
}
LSM303D_mag::LSM303D_mag(LSM303D *parent) :
CDev("LSM303D_mag", MAG_DEVICE_PATH),
_parent(parent)
@ -1432,6 +1542,7 @@ void start();
void test();
void reset();
void info();
void regdump();
/**
* Start the driver.
@ -1603,6 +1714,21 @@ 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);
}
} // namespace
@ -1634,5 +1760,11 @@ 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();
errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'");
}

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

@ -161,6 +161,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
@ -351,7 +359,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", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED),
_gyro(new MPU6000_gyro(this)),
_product(0),
_call_interval(0),
@ -991,6 +999,9 @@ MPU6000::read_reg(unsigned reg)
cmd[0] = reg | DIR_READ;
// general register transfer at low clock speed
set_frequency(MPU6000_LOW_BUS_SPEED);
transfer(cmd, cmd, sizeof(cmd));
return cmd[1];
@ -1003,6 +1014,9 @@ MPU6000::read_reg16(unsigned reg)
cmd[0] = reg | DIR_READ;
// general register transfer at low clock speed
set_frequency(MPU6000_LOW_BUS_SPEED);
transfer(cmd, cmd, sizeof(cmd));
return (uint16_t)(cmd[1] << 8) | cmd[2];
@ -1016,6 +1030,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,6 +1156,10 @@ 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;

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:

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);
}

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

@ -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) {

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

@ -117,6 +117,13 @@ show_debug_messages(void)
}
}
static void
heartbeat_blink(void)
{
static bool heartbeat = false;
LED_BLUE(heartbeat = !heartbeat);
}
int
user_start(int argc, char *argv[])
{
@ -201,6 +208,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 +224,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();

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");

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

@ -27,6 +27,7 @@ SRCS = test_adc.c \
test_dataman.c \
tests_file.c \
tests_main.c \
tests_param.c
tests_param.c \
test_ppm_loopback.c
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink

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},