diff options
27 files changed, 1023 insertions, 325 deletions
diff --git a/ROMFS/px4fmu_common/init.d/102_3dr_skywalker b/ROMFS/px4fmu_common/init.d/102_3dr_skywalker new file mode 100644 index 000000000..e5d21c321 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/102_3dr_skywalker @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/12-13_hex b/ROMFS/px4fmu_common/init.d/12-13_hex index f83f6cfd0..a7578bcaf 100644 --- a/ROMFS/px4fmu_common/init.d/12-13_hex +++ b/ROMFS/px4fmu_common/init.d/12-13_hex @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 5f52969d1..fc05d0b84 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -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 ] diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index ec8dde499..7ab63953f 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -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 diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index ae2a645f7..269ec238e 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -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"); diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c index 09838d02f..2527e4c14 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c @@ -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: diff --git a/src/drivers/device/spi.cpp b/src/drivers/device/spi.cpp index fa6b78d64..fed6c312c 100644 --- a/src/drivers/device/spi.cpp +++ b/src/drivers/device/spi.cpp @@ -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 diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h index 9103dca2e..299575dc5 100644 --- a/src/drivers/device/spi.h +++ b/src/drivers/device/spi.h @@ -102,6 +102,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. */ enum LockMode { diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index 37af26d52..f60964c2b 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -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 */
\ No newline at end of file diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 8f5674823..103b26ac5 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -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 @@ -219,6 +222,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; } @@ -700,8 +708,18 @@ L3GD20::stop() } 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)); diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 60601e22c..47109b67d 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -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 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_A 0x26 +#define WHO_I_AM 0x49 + +#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_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(); @@ -271,6 +301,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. * @@ -534,8 +569,24 @@ out: } 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'"); } diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 70359110e..6bfa583fb 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -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; diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 938786d3f..87788824a 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -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: diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 0441566e9..aab532514 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -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; - 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]; + 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; } - } - /* - * 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++; + 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; } - 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]; - } - pwm->channel_count = _max_actuators; - break; - } + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - 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; + for (unsigned i = 0; i < _max_actuators; i++) { + pwm->values[i] = _failsafe_pwm[i]; + } + + pwm->channel_count = _max_actuators; 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]; + + 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; + break; } - } - /* - * 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++; + 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; } - 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]; - } - pwm->channel_count = _max_actuators; - break; - } + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - 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; + for (unsigned i = 0; i < _max_actuators; i++) { + pwm->values[i] = _disarmed_pwm[i]; + } + + pwm->channel_count = _max_actuators; 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]; + + 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; + 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; } - 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]; - } - pwm->channel_count = _max_actuators; - arg = (unsigned long)&pwm; - break; - } + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - 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; + for (unsigned i = 0; i < _max_actuators; i++) { + pwm->values[i] = _min_pwm[i]; + } + + pwm->channel_count = _max_actuators; + arg = (unsigned long)&pwm; 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]; + + 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; + 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; } - 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,10 +1108,81 @@ 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; } @@ -1305,14 +1446,30 @@ fmu_stop(void) } 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); } diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c index a7f9a6acf..5845429db 100644 --- a/src/modules/mavlink/missionlib.c +++ b/src/modules/mavlink/missionlib.c @@ -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) { diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index ff9eecd74..cd9bd197b 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -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(); diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c index 95335f038..cdb54a80a 100644 --- a/src/modules/px4iofirmware/safety.c +++ b/src/modules/px4iofirmware/safety.c @@ -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); } @@ -164,16 +160,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) { /* indicate that a serious initialisation error occured */ @@ -192,4 +178,4 @@ failsafe_blink(void *arg) } LED_AMBER(failsafe); -}
\ No newline at end of file +} diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 898c04cd5..7c23f38c2 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -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"); diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 53c0e6b4f..cccd1b8ec 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -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 diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/test_file.c index 7f3a654bf..798724cf1 100644 --- a/src/systemcmds/tests/tests_file.c +++ b/src/systemcmds/tests/test_file.c @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file tests_file.c + * @file test_file.c * * File write test. */ diff --git a/src/systemcmds/tests/tests_param.c b/src/systemcmds/tests/test_param.c index 13f17bc43..318d2505b 100644 --- a/src/systemcmds/tests/tests_param.c +++ b/src/systemcmds/tests/test_param.c @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file tests_param.c + * @file test_param.c * * Tests related to the parameter system. */ diff --git a/src/systemcmds/tests/test_ppm_loopback.c b/src/systemcmds/tests/test_ppm_loopback.c new file mode 100644 index 000000000..b9041b013 --- /dev/null +++ b/src/systemcmds/tests/test_ppm_loopback.c @@ -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; +} diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c index 14503276c..f6415b72f 100644 --- a/src/systemcmds/tests/test_sensors.c +++ b/src/systemcmds/tests/test_sensors.c @@ -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); + 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)); - // ret = read(fd, buf, sizeof(buf)); + if (ret != sizeof(buf)) { + printf("\tMPU6K: read1 fail (%d)\n", ret); + return ERROR; - // if (ret != sizeof(buf)) { - // printf("\tMPU-6000: read2 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); + } - // } 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]); - // } + 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"); - /* XXX more tests here */ + 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: ACCEL passed all tests successfully\n"); + 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; } diff --git a/src/systemcmds/tests/test_servo.c b/src/systemcmds/tests/test_servo.c index f95760ca8..ef8512bf5 100644 --- a/src/systemcmds/tests/test_servo.c +++ b/src/systemcmds/tests/test_servo.c @@ -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; } diff --git a/src/systemcmds/tests/test_uart_loopback.c b/src/systemcmds/tests/test_uart_loopback.c index 3be152004..f1d41739b 100644 --- a/src/systemcmds/tests/test_uart_loopback.c +++ b/src/systemcmds/tests/test_uart_loopback.c @@ -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); diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index 4a5eaa0c5..06e5d20e6 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -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[]); diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 9b1cfb1fe..789c9d8b6 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -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> @@ -58,14 +59,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}, |