diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-01-17 09:50:22 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-01-17 09:50:22 +0100 |
commit | 73546b6645c5715ba0cecf00899632bf861321c9 (patch) | |
tree | d0eb82de70d6cc02ef0e6a3ccfd89525478ab8e5 /src/drivers | |
parent | 14c0fae175452da6e28ffc20b265de621a2430ba (diff) | |
parent | e691bab71a69216273fdc253695ef849671af9a7 (diff) | |
download | px4-firmware-73546b6645c5715ba0cecf00899632bf861321c9.tar.gz px4-firmware-73546b6645c5715ba0cecf00899632bf861321c9.tar.bz2 px4-firmware-73546b6645c5715ba0cecf00899632bf861321c9.zip |
Merge remote-tracking branch 'upstream/master' into navigator_new
Conflicts:
makefiles/config_px4fmu-v1_backside.mk
src/modules/commander/commander.cpp
src/modules/sdlog2/sdlog2.c
Diffstat (limited to 'src/drivers')
-rw-r--r-- | src/drivers/boards/px4fmu-v2/board_config.h | 6 | ||||
-rw-r--r-- | src/drivers/boards/px4io-v1/board_config.h | 6 | ||||
-rw-r--r-- | src/drivers/boards/px4io-v2/board_config.h | 6 | ||||
-rw-r--r-- | src/drivers/gps/gps.cpp | 194 | ||||
-rw-r--r-- | src/drivers/hil/hil.cpp | 9 | ||||
-rw-r--r-- | src/drivers/px4fmu/fmu.cpp | 11 | ||||
-rw-r--r-- | src/drivers/px4fmu/module.mk | 3 | ||||
-rw-r--r-- | src/drivers/rgbled/rgbled.cpp | 12 |
8 files changed, 154 insertions, 93 deletions
diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 4972e6914..7cfca7656 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -75,7 +75,7 @@ __BEGIN_DECLS /* PX4FMU GPIOs ***********************************************************************************/ /* LEDs */ -#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12) +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12) /* External interrupts */ #define GPIO_EXTI_GYRO_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN0) @@ -87,7 +87,7 @@ __BEGIN_DECLS #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) +#define GPIO_EXTI_MPU_DRDY_OFF (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) @@ -98,7 +98,7 @@ __BEGIN_DECLS #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) +#define GPIO_SPI_CS_MPU_OFF (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_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) diff --git a/src/drivers/boards/px4io-v1/board_config.h b/src/drivers/boards/px4io-v1/board_config.h index c3f39addf..1be4877ba 100644 --- a/src/drivers/boards/px4io-v1/board_config.h +++ b/src/drivers/boards/px4io-v1/board_config.h @@ -58,11 +58,11 @@ /* PX4IO GPIOs **********************************************************************/ /* LEDs */ -#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\ GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14) -#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ +#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\ GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15) -#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ +#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\ GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) /* Safety switch button *************************************************************/ diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h index 8da555211..8152ea4d4 100644 --- a/src/drivers/boards/px4io-v2/board_config.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -74,9 +74,9 @@ /* LEDS **********************************************************************/ -#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14) -#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15) -#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13) +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14) +#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15) +#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13) /* Safety switch button *******************************************************/ diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index fc500a9ec..6b72d560f 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -85,7 +85,7 @@ static const int ERROR = -1; class GPS : public device::CDev { public: - GPS(const char *uart_path); + GPS(const char *uart_path, bool fake_gps); virtual ~GPS(); virtual int init(); @@ -112,6 +112,7 @@ private: struct vehicle_gps_position_s _report; ///< uORB topic for gps position orb_advert_t _report_pub; ///< uORB pub for gps position float _rate; ///< position update rate + bool _fake_gps; ///< fake gps output /** @@ -156,7 +157,7 @@ GPS *g_dev; } -GPS::GPS(const char *uart_path) : +GPS::GPS(const char *uart_path, bool fake_gps) : CDev("gps", GPS_DEVICE_PATH), _task_should_exit(false), _healthy(false), @@ -164,7 +165,8 @@ GPS::GPS(const char *uart_path) : _mode(GPS_DRIVER_MODE_UBX), _Helper(nullptr), _report_pub(-1), - _rate(0.0f) + _rate(0.0f), + _fake_gps(fake_gps) { /* store port name */ strncpy(_port, uart_path, sizeof(_port)); @@ -264,98 +266,133 @@ GPS::task_main() /* loop handling received serial bytes and also configuring in between */ while (!_task_should_exit) { - if (_Helper != nullptr) { - delete(_Helper); - /* set to zero to ensure parser is not used while not instantiated */ - _Helper = nullptr; - } + if (_fake_gps) { + + _report.timestamp_position = hrt_absolute_time(); + _report.lat = (int32_t)47.378301e7f; + _report.lon = (int32_t)8.538777e7f; + _report.alt = (int32_t)400e3f; + _report.timestamp_variance = hrt_absolute_time(); + _report.s_variance_m_s = 10.0f; + _report.p_variance_m = 10.0f; + _report.c_variance_rad = 0.1f; + _report.fix_type = 3; + _report.eph_m = 10.0f; + _report.epv_m = 10.0f; + _report.timestamp_velocity = hrt_absolute_time(); + _report.vel_n_m_s = 0.0f; + _report.vel_e_m_s = 0.0f; + _report.vel_d_m_s = 0.0f; + _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s); + _report.cog_rad = 0.0f; + _report.vel_ned_valid = true; + + //no time and satellite information simulated + + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } - switch (_mode) { - case GPS_DRIVER_MODE_UBX: - _Helper = new UBX(_serial_fd, &_report); - break; + usleep(2e5); - case GPS_DRIVER_MODE_MTK: - _Helper = new MTK(_serial_fd, &_report); - break; + } else { - default: - break; - } + if (_Helper != nullptr) { + delete(_Helper); + /* set to zero to ensure parser is not used while not instantiated */ + _Helper = nullptr; + } - unlock(); + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + _Helper = new UBX(_serial_fd, &_report); + break; + + case GPS_DRIVER_MODE_MTK: + _Helper = new MTK(_serial_fd, &_report); + break; + + default: + break; + } - if (_Helper->configure(_baudrate) == 0) { unlock(); - // GPS is obviously detected successfully, reset statistics - _Helper->reset_update_rates(); + if (_Helper->configure(_baudrate) == 0) { + unlock(); - while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) { -// lock(); - /* opportunistic publishing - else invalid data would end up on the bus */ - if (_report_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + // GPS is obviously detected successfully, reset statistics + _Helper->reset_update_rates(); - } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); - } + while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) { + // lock(); + /* opportunistic publishing - else invalid data would end up on the bus */ + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } - last_rate_count++; + last_rate_count++; - /* measure update rate every 5 seconds */ - if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { - _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); - last_rate_measurement = hrt_absolute_time(); - last_rate_count = 0; - _Helper->store_update_rates(); - _Helper->reset_update_rates(); - } + /* measure update rate every 5 seconds */ + if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { + _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); + last_rate_measurement = hrt_absolute_time(); + last_rate_count = 0; + _Helper->store_update_rates(); + _Helper->reset_update_rates(); + } - if (!_healthy) { - char *mode_str = "unknown"; + if (!_healthy) { + char *mode_str = "unknown"; - switch (_mode) { - case GPS_DRIVER_MODE_UBX: - mode_str = "UBX"; - break; + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + mode_str = "UBX"; + break; - case GPS_DRIVER_MODE_MTK: - mode_str = "MTK"; - break; + case GPS_DRIVER_MODE_MTK: + mode_str = "MTK"; + break; - default: - break; + default: + break; + } + + warnx("module found: %s", mode_str); + _healthy = true; } + } - warnx("module found: %s", mode_str); - _healthy = true; + if (_healthy) { + warnx("module lost"); + _healthy = false; + _rate = 0.0f; } - } - if (_healthy) { - warnx("module lost"); - _healthy = false; - _rate = 0.0f; + lock(); } lock(); - } - - lock(); - /* select next mode */ - switch (_mode) { - case GPS_DRIVER_MODE_UBX: - _mode = GPS_DRIVER_MODE_MTK; - break; + /* select next mode */ + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + _mode = GPS_DRIVER_MODE_MTK; + break; - case GPS_DRIVER_MODE_MTK: - _mode = GPS_DRIVER_MODE_UBX; - break; + case GPS_DRIVER_MODE_MTK: + _mode = GPS_DRIVER_MODE_UBX; + break; - default: - break; + default: + break; + } } } @@ -417,7 +454,7 @@ namespace gps GPS *g_dev; -void start(const char *uart_path); +void start(const char *uart_path, bool fake_gps); void stop(); void test(); void reset(); @@ -427,7 +464,7 @@ void info(); * Start the driver. */ void -start(const char *uart_path) +start(const char *uart_path, bool fake_gps) { int fd; @@ -435,7 +472,7 @@ start(const char *uart_path) errx(1, "already started"); /* create the driver */ - g_dev = new GPS(uart_path); + g_dev = new GPS(uart_path, fake_gps); if (g_dev == nullptr) goto fail; @@ -527,6 +564,7 @@ gps_main(int argc, char *argv[]) /* set to default */ char *device_name = GPS_DEFAULT_UART_PORT; + bool fake_gps = false; /* * Start/load the driver. @@ -542,7 +580,13 @@ gps_main(int argc, char *argv[]) } } - gps::start(device_name); + /* Detect fake gps option */ + for (int i = 2; i < argc; i++) { + if (!strcmp(argv[i], "-f")) + fake_gps = true; + } + + gps::start(device_name, fake_gps); } if (!strcmp(argv[1], "stop")) @@ -567,5 +611,5 @@ gps_main(int argc, char *argv[]) gps::info(); out: - errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n]"); + errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n][-f]"); } diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index c1d73dd87..0a047f38f 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 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 @@ -193,9 +193,10 @@ HIL::~HIL() } while (_task != -1); } - /* clean up the alternate device node */ - if (_primary_pwm_device) - unregister_driver(PWM_OUTPUT_DEVICE_PATH); + // XXX already claimed with CDEV + // /* clean up the alternate device node */ + // if (_primary_pwm_device) + // unregister_driver(PWM_OUTPUT_DEVICE_PATH); g_hil = nullptr; } diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index b28d318d7..4b1b0ed0b 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1108,10 +1108,12 @@ PX4FMU::sensor_reset(int ms) stm32_configgpio(GPIO_SPI_CS_GYRO_OFF); stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF); stm32_configgpio(GPIO_SPI_CS_BARO_OFF); + stm32_configgpio(GPIO_SPI_CS_MPU_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_gpiowrite(GPIO_SPI_CS_MPU_OFF, 0); stm32_configgpio(GPIO_SPI1_SCK_OFF); stm32_configgpio(GPIO_SPI1_MISO_OFF); @@ -1124,10 +1126,12 @@ PX4FMU::sensor_reset(int ms) stm32_configgpio(GPIO_GYRO_DRDY_OFF); stm32_configgpio(GPIO_MAG_DRDY_OFF); stm32_configgpio(GPIO_ACCEL_DRDY_OFF); + stm32_configgpio(GPIO_EXTI_MPU_DRDY_OFF); stm32_gpiowrite(GPIO_GYRO_DRDY_OFF, 0); stm32_gpiowrite(GPIO_MAG_DRDY_OFF, 0); stm32_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0); + stm32_gpiowrite(GPIO_EXTI_MPU_DRDY_OFF, 0); /* set the sensor rail off */ stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); @@ -1160,6 +1164,13 @@ PX4FMU::sensor_reset(int ms) stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + + // // XXX bring up the EXTI pins again + // stm32_configgpio(GPIO_GYRO_DRDY); + // stm32_configgpio(GPIO_MAG_DRDY); + // stm32_configgpio(GPIO_ACCEL_DRDY); + // stm32_configgpio(GPIO_EXTI_MPU_DRDY); + #endif #endif } diff --git a/src/drivers/px4fmu/module.mk b/src/drivers/px4fmu/module.mk index d918abd57..05bc7a5b3 100644 --- a/src/drivers/px4fmu/module.mk +++ b/src/drivers/px4fmu/module.mk @@ -3,5 +3,4 @@ # MODULE_COMMAND = fmu -SRCS = fmu.cpp \ - ../../modules/systemlib/pwm_limit/pwm_limit.c +SRCS = fmu.cpp diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 727c86e02..4f58891ed 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -559,7 +559,7 @@ RGBLED::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b) void rgbled_usage() { - warnx("missing command: try 'start', 'test', 'info', 'off', 'rgb 30 40 50'"); + warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'rgb 30 40 50'"); warnx("options:"); warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); warnx(" -a addr (0x%x)", ADDR); @@ -643,7 +643,7 @@ rgbled_main(int argc, char *argv[]) if (g_rgbled == nullptr) { warnx("not started"); rgbled_usage(); - exit(0); + exit(1); } if (!strcmp(verb, "test")) { @@ -669,7 +669,7 @@ rgbled_main(int argc, char *argv[]) exit(0); } - if (!strcmp(verb, "off")) { + if (!strcmp(verb, "off") || !strcmp(verb, "stop")) { fd = open(RGBLED_DEVICE_PATH, 0); if (fd == -1) { @@ -681,6 +681,12 @@ rgbled_main(int argc, char *argv[]) exit(ret); } + if (!strcmp(verb, "stop")) { + delete g_rgbled; + g_rgbled = nullptr; + exit(0); + } + if (!strcmp(verb, "rgb")) { if (argc < 5) { errx(1, "Usage: rgbled rgb <red> <green> <blue>"); |