From e6b98b2ab8ec83c0fb00458dabeb9c9a408d29bf Mon Sep 17 00:00:00 2001 From: Dennis Shtatnov Date: Mon, 22 Aug 2016 00:21:29 -0400 Subject: [PATCH] Tweak configs for CF2 Fix build error Capitalization mistake for headers Non-Mac compiler issue Baudrate for crazyflie nrf and fix code style Save space Cleanup mpu9250 driver --- ROMFS/px4fmu_common/init.d/rcS | 26 ++-- cmake/configs/nuttx_crazyflie_default.cmake | 6 +- src/drivers/boards/crazyflie/board_config.h | 22 +-- src/drivers/boards/crazyflie/crazyflie_init.c | 31 ++-- src/drivers/crazyflie/crazyflie.cpp | 18 +-- src/drivers/lps25h/lps25h.cpp | 3 +- src/drivers/mpu9250/mag.cpp | 108 +++----------- src/drivers/mpu9250/mag.h | 33 +++++ src/drivers/mpu9250/mag_i2c.cpp | 14 +- src/drivers/mpu9250/main.cpp | 7 +- src/drivers/mpu9250/mpu9250.cpp | 40 ++++-- src/drivers/mpu9250/mpu9250.h | 58 ++++---- src/drivers/mpu9250/mpu9250_spi.cpp | 3 +- src/modules/mavlink/mavlink_main.cpp | 7 +- src/modules/screen/CMakeLists.txt | 43 ------ src/modules/screen/screen.c | 134 ------------------ 16 files changed, 172 insertions(+), 381 deletions(-) delete mode 100644 src/modules/screen/CMakeLists.txt delete mode 100644 src/modules/screen/screen.c diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 8adcb853a2..2b0f3ce9d9 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -628,8 +628,23 @@ then px4flow start & fi - # Start USB shell if no microSD present, MAVLink else + + set DEBUG_MODE no + if [ $LOG_FILE == /dev/null ] + then + set DEBUG_MODE yes + fi + + + if ver hwcmp CRAZYFLIE + then + set DEBUG_MODE no + fi + + + # Start USB shell if no microSD present, MAVLink else + if [ $DEBUG_MODE == yes ] then # Try to get an USB console nshterm /dev/ttyACM0 & @@ -681,15 +696,6 @@ then ardrone_interface start -d /dev/ttyS1 fi - - # - # Start Crazyflie driver - # - if ver hwcmp CRAZYFLIE - then - crazyflie start - fi - # # Fixed wing setup # diff --git a/cmake/configs/nuttx_crazyflie_default.cmake b/cmake/configs/nuttx_crazyflie_default.cmake index 844260c4cf..6356b9700c 100644 --- a/cmake/configs/nuttx_crazyflie_default.cmake +++ b/cmake/configs/nuttx_crazyflie_default.cmake @@ -9,13 +9,11 @@ set(config_module_list drivers/device drivers/stm32 drivers/led + drivers/px4fmu drivers/boards/crazyflie - drivers/crazyflie drivers/mpu9250 - #drivers/ak8963 drivers/lps25h drivers/gps - # drivers/pwm_out_sim modules/sensors # @@ -70,7 +68,6 @@ set(config_module_list # Logging # modules/sdlog2 - modules/screen # # Library modules @@ -84,7 +81,6 @@ set(config_module_list # # Libraries # - #lib/mathlib/CMSIS lib/controllib lib/mathlib lib/mathlib/math/filter diff --git a/src/drivers/boards/crazyflie/board_config.h b/src/drivers/boards/crazyflie/board_config.h index dfc59d456e..bba6a77097 100644 --- a/src/drivers/boards/crazyflie/board_config.h +++ b/src/drivers/boards/crazyflie/board_config.h @@ -112,17 +112,17 @@ __BEGIN_DECLS #define ADC_BATTERY_CURRENT_CHANNEL ((uint8_t)(-1)) #define ADC_AIRSPEED_VOLTAGE_CHANNEL ((uint8_t)(-1)) - /* PWM - * - * Four PWM motor outputs are configured. - * - * Pins: - * - * CH1 : PA1 : TIM2_CH2 - * CH2 : PB11 : TIM2_CH4 - * CH3 : PA15 : TIM2_CH1 - * CH4 : PB9 : TIM4_CH4 - */ +/* PWM +* +* Four PWM motor outputs are configured. +* +* Pins: +* +* CH1 : PA1 : TIM2_CH2 +* CH2 : PB11 : TIM2_CH4 +* CH3 : PA15 : TIM2_CH1 +* CH4 : PB9 : TIM4_CH4 +*/ #define GPIO_TIM2_CH2OUT GPIO_TIM2_CH2OUT_1 #define GPIO_TIM2_CH4OUT GPIO_TIM2_CH4OUT_2 diff --git a/src/drivers/boards/crazyflie/crazyflie_init.c b/src/drivers/boards/crazyflie/crazyflie_init.c index 966862f6b6..64fb9f3c90 100644 --- a/src/drivers/boards/crazyflie/crazyflie_init.c +++ b/src/drivers/boards/crazyflie/crazyflie_init.c @@ -102,6 +102,17 @@ * Public Functions ****************************************************************************/ +/************************************************************************************ + * Name: board_peripheral_reset + * + * Description: + * + ************************************************************************************/ +__EXPORT void board_peripheral_reset(int ms) +{ + +} + /************************************************************************************ * Name: stm32_boardinitialize * @@ -129,20 +140,6 @@ stm32_boardinitialize(void) #include -#if 0 -#ifdef __cplusplus -__EXPORT int matherr(struct __exception *e) -{ - return 1; -} -#else -__EXPORT int matherr(struct exception *e) -{ - return 1; -} -#endif -#endif - __EXPORT int nsh_archinitialize(void) { int result; @@ -177,13 +174,9 @@ __EXPORT int nsh_archinitialize(void) result = board_i2c_initialize(); if (result != OK) { -// up_ledon(LED_AMBER); + led_on(1); return -ENODEV; } - - - // TODO: Initialize i2c buses right here - return OK; } diff --git a/src/drivers/crazyflie/crazyflie.cpp b/src/drivers/crazyflie/crazyflie.cpp index e0ba77a436..6b3c637e45 100644 --- a/src/drivers/crazyflie/crazyflie.cpp +++ b/src/drivers/crazyflie/crazyflie.cpp @@ -428,7 +428,7 @@ Crazyflie::cycle() /* the PWM limit call takes care of out of band errors, NaN and constrains */ pwm_limit_calc(_servo_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm, - outputs, pwm_limited, &_pwm_limit); + outputs, pwm_limited, &_pwm_limit); /* output to the servos */ for (size_t i = 0; i < num_outputs; i++) { @@ -499,9 +499,9 @@ void Crazyflie::work_stop() int Crazyflie::control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input) + uint8_t control_group, + uint8_t control_index, + float &input) { const actuator_controls_s *controls = (actuator_controls_s *)handle; @@ -518,7 +518,7 @@ Crazyflie::control_callback(uintptr_t handle, /* motor spinup phase - lock throttle to zero */ if (_pwm_limit.state == PWM_LIMIT_STATE_RAMP) { if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && - control_index == actuator_controls_s::INDEX_THROTTLE) { + control_index == actuator_controls_s::INDEX_THROTTLE) { /* limit the throttle output to zero during motor spinup, * as the motors cannot follow any demand yet */ @@ -529,7 +529,7 @@ Crazyflie::control_callback(uintptr_t handle, /* throttle not arming - mark throttle input as invalid */ if (arm_nothrottle()) { if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && - control_index == actuator_controls_s::INDEX_THROTTLE) { + control_index == actuator_controls_s::INDEX_THROTTLE) { /* set the throttle to an invalid value */ input = NAN_VALUE; } @@ -770,7 +770,7 @@ Crazyflie::ioctl(file *filp, int cmd, unsigned long arg) mixer_simple_s *mixinfo = (mixer_simple_s *)arg; SimpleMixer *mixer = new SimpleMixer(control_callback, - (uintptr_t)_controls, mixinfo); + (uintptr_t)_controls, mixinfo); if (mixer->check()) { delete mixer; @@ -1077,8 +1077,8 @@ crazyflie_main(int argc, char *argv[]) if (!strcmp(verb, "start")) { if (crazyflie_start() == OK) { errx(0, "Crazyflie driver started"); - } - else { + + } else { errx(1, "failed to start the Crazyflie driver"); } diff --git a/src/drivers/lps25h/lps25h.cpp b/src/drivers/lps25h/lps25h.cpp index d66b0e8a4b..8a1c7cd39f 100644 --- a/src/drivers/lps25h/lps25h.cpp +++ b/src/drivers/lps25h/lps25h.cpp @@ -830,7 +830,7 @@ LPS25H::collect() } else { _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &new_report, - &_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX); + &_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX); if (_baro_topic == nullptr) { DEVICE_DEBUG("ADVERT FAIL"); @@ -1306,6 +1306,7 @@ lps25h_main(int argc, char *argv[]) while ((ch = getopt(argc, argv, "XIS:CT")) != EOF) { switch (ch) { #if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC) + case 'I': busid = LPS25H_BUS_I2C_INTERNAL; break; diff --git a/src/drivers/mpu9250/mag.cpp b/src/drivers/mpu9250/mag.cpp index fec1df32e5..ca22481252 100644 --- a/src/drivers/mpu9250/mag.cpp +++ b/src/drivers/mpu9250/mag.cpp @@ -68,87 +68,6 @@ #include "mpu9250.h" -/* in 16-bit sampling mode the mag resolution is 1.5 milli Gauss per bit */ - -#define MPU9250_MAG_RANGE_GA 1.5e-3f; - -/* we are using the continuous fixed sampling rate of 100Hz */ - -#define MPU9250_AK8963_SAMPLE_RATE 100 - - -/* mpu9250 master i2c bus specific register address and bit definitions */ - -#define MPUREG_I2C_MST_STATUS 0x36 - -#define BIT_I2C_READ_FLAG 0x80 - -#define MPUREG_I2C_MST_CTRL 0x24 -#define MPUREG_I2C_SLV0_ADDR 0x25 -#define MPUREG_I2C_SLV0_REG 0x26 -#define MPUREG_I2C_SLV0_CTRL 0x27 - -#define MPUREG_I2C_SLV4_ADDR 0x31 -#define MPUREG_I2C_SLV4_REG 0x32 -#define MPUREG_I2C_SLV4_DO 0x33 -#define MPUREG_I2C_SLV4_CTRL 0x34 -#define MPUREG_I2C_SLV4_DI 0x35 - -#define MPUREG_EXT_SENS_DATA_00 0x49 - -#define MPUREG_I2C_SLV0_D0 0x63 -#define MPUREG_I2C_MST_DELAY_CTRL 0x67 -#define MPUREG_USER_CTRL 0x6A - -#define BIT_I2C_SLV0_NACK 0x01 -#define BIT_I2C_FIFO_EN 0x40 -#define BIT_I2C_MST_EN 0x20 -#define BIT_I2C_IF_DIS 0x10 -#define BIT_FIFO_RST 0x04 -#define BIT_I2C_MST_RST 0x02 -#define BIT_SIG_COND_RST 0x01 - -#define BIT_I2C_SLV0_EN 0x80 -#define BIT_I2C_SLV0_BYTE_SW 0x40 -#define BIT_I2C_SLV0_REG_DIS 0x20 -#define BIT_I2C_SLV0_REG_GRP 0x10 - -#define BIT_I2C_MST_MULT_MST_EN 0x80 -#define BIT_I2C_MST_WAIT_FOR_ES 0x40 -#define BIT_I2C_MST_SLV_3_FIFO_EN 0x20 -#define BIT_I2C_MST_P_NSR 0x10 -#define BITS_I2C_MST_CLOCK_258HZ 0x08 -#define BITS_I2C_MST_CLOCK_400HZ 0x0D - -#define BIT_I2C_SLV0_DLY_EN 0x01 -#define BIT_I2C_SLV1_DLY_EN 0x02 -#define BIT_I2C_SLV2_DLY_EN 0x04 -#define BIT_I2C_SLV3_DLY_EN 0x08 - - -/* ak8963 register address and bit definitions */ - -#define AK8963_I2C_ADDR 0x0C -#define AK8963_DEVICE_ID 0x48 - -#define AK8963REG_WIA 0x00 -#define AK8963REG_ST1 0x02 -#define AK8963REG_HXL 0x03 -#define AK8963REG_ASAX 0x10 -#define AK8963REG_CNTL1 0x0A -#define AK8963REG_CNTL2 0x0B - -#define AK8963_SINGLE_MEAS_MODE 0x01 -#define AK8963_CONTINUOUS_MODE1 0x02 -#define AK8963_CONTINUOUS_MODE2 0x06 -#define AK8963_POWERDOWN_MODE 0x00 -#define AK8963_SELFTEST_MODE 0x08 -#define AK8963_FUZE_MODE 0x0F -#define AK8963_16BIT_ADC 0x10 -#define AK8963_14BIT_ADC 0x00 -#define AK8963_RESET 0x01 - - // If interface is non-null, then it will used for interacting with the device. // Otherwise, it will passthrough the parent MPU9250 MPU9250_mag::MPU9250_mag(MPU9250 *parent, device::Device *interface, const char *path) : @@ -255,7 +174,8 @@ void MPU9250_mag::measure() { struct ak8963_regs data; - if(OK == _interface->read(AK8963REG_ST1, &data, sizeof(struct ak8963_regs))){ + + if (OK == _interface->read(AK8963REG_ST1, &data, sizeof(struct ak8963_regs))) { _measure(data); } } @@ -536,12 +456,14 @@ uint8_t MPU9250_mag::read_reg(unsigned int reg) { uint8_t buf; + if (_interface == nullptr) { passthrough_read(reg, &buf, 0x01); - } - else { + + } else { _interface->read(reg, &buf, 1); } + return buf; } @@ -580,8 +502,8 @@ MPU9250_mag::write_reg(unsigned reg, uint8_t value) // general register transfer at low clock speed if (_interface == nullptr) { passthrough_write(reg, value); - } - else { + + } else { _interface->write(MPU9250_LOW_SPEED_OP(reg), &value, 1); } } @@ -603,12 +525,14 @@ MPU9250_mag::ak8963_read_adjustments(void) write_reg(AK8963REG_CNTL1, AK8963_FUZE_MODE | AK8963_16BIT_ADC); usleep(50); + if (_interface != nullptr) { _interface->read(AK8963REG_ASAX, response, 3); - } - else { + + } else { passthrough_read(AK8963REG_ASAX, response, 3); } + write_reg(AK8963REG_CNTL1, AK8963_POWERDOWN_MODE); for (int i = 0; i < 3; i++) { @@ -633,12 +557,12 @@ MPU9250_mag::ak8963_setup(void) int retries = 10; /* Configures the parent to act in master mode */ - if(_interface == nullptr) { + if (_interface == nullptr) { uint8_t user_ctrl = _parent->read_reg(MPUREG_USER_CTRL); _parent->write_checked_reg(MPUREG_USER_CTRL, user_ctrl | BIT_I2C_MST_EN); _parent->write_reg(MPUREG_I2C_MST_CTRL, BIT_I2C_MST_P_NSR | BIT_I2C_MST_WAIT_FOR_ES | BITS_I2C_MST_CLOCK_400HZ); - } - else { + + } else { // uint8_t user_ctrl = _parent->read_reg(MPUREG_USER_CTRL); // // Passthrough mode @@ -659,7 +583,7 @@ MPU9250_mag::ak8963_setup(void) write_reg(AK8963REG_CNTL1, AK8963_CONTINUOUS_MODE2 | AK8963_16BIT_ADC); - if(_interface == NULL) { + if (_interface == NULL) { /* Configure mpu to internally read ak8963 data */ set_passthrough(AK8963REG_ST1, sizeof(struct ak8963_regs)); } diff --git a/src/drivers/mpu9250/mag.h b/src/drivers/mpu9250/mag.h index 7f5e66400c..892d4c0e40 100644 --- a/src/drivers/mpu9250/mag.h +++ b/src/drivers/mpu9250/mag.h @@ -33,6 +33,39 @@ #pragma once + +/* in 16-bit sampling mode the mag resolution is 1.5 milli Gauss per bit */ + +#define MPU9250_MAG_RANGE_GA 1.5e-3f; + +/* we are using the continuous fixed sampling rate of 100Hz */ + +#define MPU9250_AK8963_SAMPLE_RATE 100 + +/* ak8963 register address and bit definitions */ + +#define AK8963_I2C_ADDR 0x0C +#define AK8963_DEVICE_ID 0x48 + +#define AK8963REG_WIA 0x00 +#define AK8963REG_ST1 0x02 +#define AK8963REG_HXL 0x03 +#define AK8963REG_ASAX 0x10 +#define AK8963REG_CNTL1 0x0A +#define AK8963REG_CNTL2 0x0B + +#define AK8963_SINGLE_MEAS_MODE 0x01 +#define AK8963_CONTINUOUS_MODE1 0x02 +#define AK8963_CONTINUOUS_MODE2 0x06 +#define AK8963_POWERDOWN_MODE 0x00 +#define AK8963_SELFTEST_MODE 0x08 +#define AK8963_FUZE_MODE 0x0F +#define AK8963_16BIT_ADC 0x10 +#define AK8963_14BIT_ADC 0x00 +#define AK8963_RESET 0x01 + + + class MPU9250; #pragma pack(push, 1) diff --git a/src/drivers/mpu9250/mag_i2c.cpp b/src/drivers/mpu9250/mag_i2c.cpp index b7bb414c84..39a08aea4a 100644 --- a/src/drivers/mpu9250/mag_i2c.cpp +++ b/src/drivers/mpu9250/mag_i2c.cpp @@ -55,20 +55,12 @@ #include #include -#include "MPU9250.h" +#include "mpu9250.h" #include "mag.h" #include "board_config.h" -// TODO: This is redundant with mag.cpp -#define AK8963_I2C_ADDR 0x0C -#define ADDR_ID 0x00 - -#define ID_WHO_AM_I 0x48 - - - #ifdef USE_I2C device::Device *AK8963_I2C_interface(int bus, bool external_bus); @@ -163,9 +155,9 @@ int AK8963_I2C::probe() { uint8_t whoami = 0; - uint8_t expected = ID_WHO_AM_I; + uint8_t expected = AK8963_DEVICE_ID; - if (read(ADDR_ID, &whoami, 1)) { + if (read(AK8963REG_WIA, &whoami, 1)) { return -EIO; } diff --git a/src/drivers/mpu9250/main.cpp b/src/drivers/mpu9250/main.cpp index 94f9f850b5..c7c5059632 100644 --- a/src/drivers/mpu9250/main.cpp +++ b/src/drivers/mpu9250/main.cpp @@ -69,8 +69,6 @@ #include #include -#include "mag.h" -#include "gyro.h" #include "mpu9250.h" #define MPU_DEVICE_PATH_ACCEL "/dev/mpu9250_accel" @@ -125,7 +123,7 @@ struct mpu9250_bus_option { { MPU9250_BUS_SPI_INTERNAL, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, MPU_DEVICE_PATH_MAG, &MPU9250_SPI_interface, true, PX4_SPI_BUS_SENSORS, NULL }, #endif #if defined(PX4_SPI_BUS_EXT) - { MPU9250_BUS_SPI_EXTERNAL, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, MPU_DEVICE_PATH_MAG_EXT &MPU9250_SPI_interface, true, PX4_SPI_BUS_EXT, NULL }, + { MPU9250_BUS_SPI_EXTERNAL, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, MPU_DEVICE_PATH_MAG_EXT, &MPU9250_SPI_interface, true, PX4_SPI_BUS_EXT, NULL }, #endif }; @@ -189,9 +187,11 @@ start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, bool external) #ifdef USE_I2C /* For i2c interfaces, connect to the magnetomer directly */ bool is_i2c = bus.busid == MPU9250_BUS_I2C_INTERNAL || bus.busid == MPU9250_BUS_I2C_EXTERNAL; + if (is_i2c) { mag_interface = AK8963_I2C_interface(bus.busnum, external); } + #endif bus.dev = new MPU9250(interface, mag_interface, bus.accelpath, bus.gyropath, bus.magpath, rotation); @@ -494,7 +494,6 @@ mpu9250_main(int argc, char *argv[]) int ch; bool external = false; enum Rotation rotation = ROTATION_NONE; - //int accel_range = 8; /* jump over start/off/etc and look at options first */ while ((ch = getopt(argc, argv, "XISsR:")) != EOF) { diff --git a/src/drivers/mpu9250/mpu9250.cpp b/src/drivers/mpu9250/mpu9250.cpp index 7e5eeb71d6..b78fb35a00 100644 --- a/src/drivers/mpu9250/mpu9250.cpp +++ b/src/drivers/mpu9250/mpu9250.cpp @@ -109,7 +109,8 @@ const uint8_t MPU9250::_checked_registers[MPU9250_NUM_CHECKED_REGISTERS] = { MPU }; -MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const char *path_accel, const char *path_gyro, const char *path_mag, +MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const char *path_accel, + const char *path_gyro, const char *path_mag, enum Rotation rotation) : CDev("MPU9250", path_accel), _interface(interface), @@ -122,7 +123,7 @@ MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const #else _use_hrt(true), #endif - _call{}, + _call {}, _call_interval(0), _accel_reports(nullptr), _accel_scale{}, @@ -195,7 +196,9 @@ MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const _gyro_scale.z_scale = 1.0f; memset(&_call, 0, sizeof(_call)); +#if defined(USE_I2C) memset(&_work, 0, sizeof(_work)); +#endif } MPU9250::~MPU9250() @@ -305,11 +308,14 @@ MPU9250::init() return ret; } +#ifdef USE_I2C if (!_mag->is_passthrough() && _mag->_interface->init() != OK) { warnx("failed to setup ak8963 interface"); } +#endif + /* do CDev init for the mag device node, keep it optional */ ret = _mag->init(); @@ -390,7 +396,15 @@ int MPU9250::reset() // INT CFG => Interrupt on Data Ready write_checked_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready usleep(1000); - write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR | (_mag->is_passthrough()? 0 : BIT_INT_BYPASS_EN)); // INT: Clear on any read, also use i2c bypass is master mode isn't needed + +#ifdef USE_I2C + bool bypass = !_mag->is_passthrough(); +#else + bool bypass = false; +#endif + write_checked_reg(MPUREG_INT_PIN_CFG, + BIT_INT_ANYRD_2CLEAR | (bypass ? BIT_INT_BYPASS_EN : + 0)); // INT: Clear on any read, also use i2c bypass is master mode isn't needed usleep(1000); write_checked_reg(MPUREG_ACCEL_CONFIG2, BITS_ACCEL_CONFIG2_41HZ); @@ -1018,7 +1032,7 @@ MPU9250::write_checked_reg(unsigned reg, uint8_t value) if (reg == _checked_registers[i]) { _checked_values[i] = value; _checked_bad[i] = value; - break; // TODO: Assumes no duplicates + break; } } } @@ -1075,6 +1089,7 @@ MPU9250::start() 1000, _call_interval - MPU9250_TIMER_REDUCTION, (hrt_callout)&MPU9250::measure_trampoline, this);; + } else { #ifdef USE_I2C /* schedule a cycle to start things */ @@ -1267,15 +1282,12 @@ MPU9250::measure() /* start measuring */ perf_begin(_sample_perf); - // TODO: Don't fetch magnetometer data if it isn't being managed - // In that case, trigger a magnetometer read - /* * Fetch the full set of measurements from the MPU9250 in one pass. */ if (OK != _interface->read(MPU9250_SET_SPEED(MPUREG_INT_STATUS, MPU9250_HIGH_BUS_SPEED), - (uint8_t *)&mpu_report, - sizeof(mpu_report))) { + (uint8_t *)&mpu_report, + sizeof(mpu_report))) { return; } @@ -1285,13 +1297,19 @@ MPU9250::measure() return; } +#ifdef USE_I2C + if (_mag->is_passthrough()) { +#endif _mag->_measure(mpu_report.mag); - } - else { +#ifdef USE_I2C + + } else { _mag->measure(); } +#endif + /* * Convert from big to little endian */ diff --git a/src/drivers/mpu9250/mpu9250.h b/src/drivers/mpu9250/mpu9250.h index d17981d743..82d3fbf093 100644 --- a/src/drivers/mpu9250/mpu9250.h +++ b/src/drivers/mpu9250/mpu9250.h @@ -31,37 +31,16 @@ * ****************************************************************************/ - -#pragma once - - - -// TODO: All these includes are from main.cpp -#include - -#include #include -#include -#include -#include -#include -#include -#include #include -#include #include -// TODO: These three are new -#include #include -#include #include #include -#include -#include #include #include #include @@ -79,8 +58,6 @@ # define USE_I2C #endif -#define DIR_READ 0x80 -#define DIR_WRITE 0x00 // MPU 9250 registers #define MPUREG_WHOAMI 0x75 @@ -143,7 +120,7 @@ #define MPUREG_FIFO_COUNTL 0x73 #define MPUREG_FIFO_R_W 0x74 - // Configuration bits MPU 9250 +// Configuration bits MPU 9250 #define BIT_SLEEP 0x40 #define BIT_H_RESET 0x80 #define MPU_CLK_SEL_AUTO 0x01 @@ -173,6 +150,33 @@ #define BIT_INT_ANYRD_2CLEAR 0x10 #define BIT_INT_BYPASS_EN 0x02 +#define BIT_I2C_READ_FLAG 0x80 + +#define BIT_I2C_SLV0_NACK 0x01 +#define BIT_I2C_FIFO_EN 0x40 +#define BIT_I2C_MST_EN 0x20 +#define BIT_I2C_IF_DIS 0x10 +#define BIT_FIFO_RST 0x04 +#define BIT_I2C_MST_RST 0x02 +#define BIT_SIG_COND_RST 0x01 + +#define BIT_I2C_SLV0_EN 0x80 +#define BIT_I2C_SLV0_BYTE_SW 0x40 +#define BIT_I2C_SLV0_REG_DIS 0x20 +#define BIT_I2C_SLV0_REG_GRP 0x10 + +#define BIT_I2C_MST_MULT_MST_EN 0x80 +#define BIT_I2C_MST_WAIT_FOR_ES 0x40 +#define BIT_I2C_MST_SLV_3_FIFO_EN 0x20 +#define BIT_I2C_MST_P_NSR 0x10 +#define BITS_I2C_MST_CLOCK_258HZ 0x08 +#define BITS_I2C_MST_CLOCK_400HZ 0x0D + +#define BIT_I2C_SLV0_DLY_EN 0x01 +#define BIT_I2C_SLV1_DLY_EN 0x02 +#define BIT_I2C_SLV2_DLY_EN 0x04 +#define BIT_I2C_SLV3_DLY_EN 0x08 + #define MPU_WHOAMI_9250 0x71 #define MPU9250_ACCEL_DEFAULT_RATE 1000 @@ -209,7 +213,6 @@ struct MPUReport { }; #pragma pack(pop) -// TODO: Big enough? #define MPU_MAX_WRITE_BUFFER_SIZE (2) @@ -244,7 +247,8 @@ class MPU9250_gyro; class MPU9250 : public device::CDev { public: - MPU9250(device::Device *interface, device::Device *mag_interface, const char *path_accel, const char *path_gyro, const char *path_mag, + MPU9250(device::Device *interface, device::Device *mag_interface, const char *path_accel, const char *path_gyro, + const char *path_mag, enum Rotation rotation); virtual ~MPU9250(); @@ -375,7 +379,7 @@ private: /** * When the I2C interfase is on * Perform a poll cycle; collect from the previous measurement - * and start a new one. + * and start a new one. * * This is the heart of the measurement state machine. This function * alternately starts a measurement, or collects the data from the diff --git a/src/drivers/mpu9250/mpu9250_spi.cpp b/src/drivers/mpu9250/mpu9250_spi.cpp index b33ca32bf4..5ce17e380c 100644 --- a/src/drivers/mpu9250/mpu9250_spi.cpp +++ b/src/drivers/mpu9250/mpu9250_spi.cpp @@ -58,7 +58,7 @@ #include #include -#include "MPU9250.h" +#include "mpu9250.h" #include #define DIR_READ 0x80 @@ -118,6 +118,7 @@ MPU9250_SPI_interface(int bus, bool external_bus) #else errx(0, "External SPI not available"); #endif + } else { cs = (spi_dev_e) PX4_SPIDEV_MPU; } diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 715d9394ad..189e617812 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -642,6 +642,10 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name) #define B921600 921600 #endif +#ifndef B1000000 + #define B1000000 1000000 +#endif + /* process baud rate */ int speed; @@ -2630,6 +2634,3 @@ int mavlink_main(int argc, char *argv[]) return 0; } - - - diff --git a/src/modules/screen/CMakeLists.txt b/src/modules/screen/CMakeLists.txt deleted file mode 100644 index 6f8228271f..0000000000 --- a/src/modules/screen/CMakeLists.txt +++ /dev/null @@ -1,43 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015 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. -# -############################################################################ -px4_add_module( - MODULE modules__screen - MAIN screen - COMPILE_FLAGS - -Os - SRCS - screen.c - DEPENDS - platforms__common - ) -# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/screen/screen.c b/src/modules/screen/screen.c deleted file mode 100644 index 214801956e..0000000000 --- a/src/modules/screen/screen.c +++ /dev/null @@ -1,134 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin - * - * 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 screen.c - * - * Using screen to read serial ports. - * - * @author Tim Dyer <> - */ - -#include -#include -#include -#include -#include -#include -#include - -__EXPORT int screen_main(int argc, char *argv[]); - -int -open_uart(const char *device); - - -int screen_main(int argc, char *argv[]) -{ - int uart = open_uart("/dev/ttyS2"); - - //static const int timeout_ms = 1000; - - for (;;) - { - char c; - ssize_t size = read(uart, &c, 1); - if (size) - { - printf("%c", c); - } - - /* - struct pollfd fds; - fds.fd = uart; - fds.events = POLLIN; - - char c; - - if (poll(&fds, 1, timeout_ms) > 0) { - read(uart, &c, 1); - printf("%c", c); - } else { - warnx("UART timeout on TX/RX port"); - //return ERROR; - } - */ - } - - return OK; -} - -int -open_uart(const char *device) -{ - /* baud rate */ - static const speed_t speed = B1000000; - - /* open uart */ - const int uart = open(device, O_RDWR | O_NOCTTY); - - if (uart < 0) { - err(1, "ERR: opening %s", device); - } - - /* Back up the original uart configuration to restore it after exit */ - int termios_state; - struct termios uart_config_original; - - if ((termios_state = tcgetattr(uart, &uart_config_original)) < 0) { - close(uart); - err(1, "ERR: %s: %d", device, termios_state); - } - - /* Fill the struct for the new configuration */ - struct termios uart_config; - tcgetattr(uart, &uart_config); - - /* Clear ONLCR flag (which appends a CR for every LF) */ - uart_config.c_oflag &= ~ONLCR; - - /* Set baud rate */ - if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - close(uart); - err(1, "ERR: %s: %d (cfsetispeed, cfsetospeed)", - device, termios_state); - } - - if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { - close(uart); - err(1, "ERR: %s (tcsetattr)", device); - } - - return uart; -}