diff --git a/src/main/config/config.h b/src/main/config/config.h index b45a7148e5..03abdd6e2d 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -47,10 +47,9 @@ typedef enum { FEATURE_TRANSPONDER = 1 << 21, FEATURE_AIRMODE = 1 << 22, FEATURE_SUPEREXPO_RATES = 1 << 23, - FEATURE_OSD = 1 << 24, - FEATURE_VTX = 1 << 25, - FEATURE_RX_NRF24 = 1 << 26, - FEATURE_SOFTSPI = 1 << 27, + FEATURE_VTX = 1 << 24, + FEATURE_RX_NRF24 = 1 << 25, + FEATURE_SOFTSPI = 1 << 26, } features_e; typedef enum { diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c index 215a74cead..36caedddf7 100644 --- a/src/main/drivers/accgyro_mpu6050.c +++ b/src/main/drivers/accgyro_mpu6050.c @@ -1,4 +1,4 @@ -/* +/* * This file is part of Cleanflight. * * Cleanflight is free software: you can redistribute it and/or modify @@ -20,11 +20,10 @@ #include #include "platform.h" + #include "build/build_config.h" - #include "build/debug.h" - #include "common/maths.h" #include "nvic.h" diff --git a/src/main/drivers/accgyro_mpu6500.c b/src/main/drivers/accgyro_mpu6500.c index 984f168f1e..47deae1b8c 100644 --- a/src/main/drivers/accgyro_mpu6500.c +++ b/src/main/drivers/accgyro_mpu6500.c @@ -93,10 +93,15 @@ void mpu6500GyroInit(uint8_t lpf) mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0); delay(100); mpuConfiguration.write(MPU_RA_PWR_MGMT_1, INV_CLK_PLL); + delay(15); mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); + delay(15); mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3); + delay(15); mpuConfiguration.write(MPU_RA_CONFIG, lpf); + delay(15); mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxCalculateDivider()); // Get Divider + delay(100); // Data ready interrupt configuration #ifdef USE_MPU9250_MAG @@ -104,9 +109,7 @@ void mpu6500GyroInit(uint8_t lpf) #else mpuConfiguration.write(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN #endif - #ifdef USE_MPU_DATA_READY_SIGNAL mpuConfiguration.write(MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable #endif - } diff --git a/src/main/drivers/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro_spi_mpu6000.c index fc00292503..925d0ea7b8 100644 --- a/src/main/drivers/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro_spi_mpu6000.c @@ -235,7 +235,7 @@ static void mpu6000AccAndGyroInit(void) // Accel Sample Rate 1kHz // Gyroscope Output Rate = 1kHz when the DLPF is enabled mpu6000WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxCalculateDivider()); - delayMicroseconds(1); + delayMicroseconds(15); // Gyro +/- 1000 DPS Full Scale mpu6000WriteRegister(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); diff --git a/src/main/drivers/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro_spi_mpu6500.c index 32ae6a1ce8..afc69fbf16 100755 --- a/src/main/drivers/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro_spi_mpu6500.c @@ -17,7 +17,6 @@ #include #include -#include #include "platform.h" diff --git a/src/main/drivers/accgyro_spi_mpu9250.c b/src/main/drivers/accgyro_spi_mpu9250.c index c1fa690adb..d448caeb8a 100644 --- a/src/main/drivers/accgyro_spi_mpu9250.c +++ b/src/main/drivers/accgyro_spi_mpu9250.c @@ -27,7 +27,8 @@ #include #include "platform.h" -#include "light_led.h" + +#include "build/debug.h" #include "common/axis.h" #include "common/maths.h" @@ -38,8 +39,7 @@ #include "exti.h" #include "bus_spi.h" #include "gyro_sync.h" -#include "build/debug.h" - +#include "light_led.h" #include "sensor.h" #include "accgyro.h" @@ -168,7 +168,7 @@ static void mpu9250AccAndGyroInit(uint8_t lpf) { verifympu9250WriteRegister(MPU_RA_CONFIG, 0); //8KHz, 250DLPF } - verifympu9250WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops + verifympu9250WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxCalculateDivider()); // Get Divider Drops verifympu9250WriteRegister(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3); verifympu9250WriteRegister(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 4d21eac84c..36d438198d 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -199,7 +199,7 @@ static const char * const featureNames[] = { "SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM", "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "ONESHOT125", "BLACKBOX", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE", - "SUPEREXPO", "OSD", "VTX", "RX_NRF24", "SOFTSPI", NULL + "SUPEREXPO", "VTX", "RX_NRF24", "SOFTSPI", NULL }; // sync this with rxFailsafeChannelMode_e @@ -218,7 +218,7 @@ static const char * const sensorTypeNames[] = { #define SENSOR_NAMES_MASK (SENSOR_GYRO | SENSOR_ACC | SENSOR_BARO | SENSOR_MAG | SENSOR_SONAR) // sync with gyroSensor_e -static const char * const gyroNames[] = { "", "None", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "FAKE"}; +static const char * const gyroNames[] = { "", "None", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "MPU9250", "FAKE"}; // sync with accelerationSensor_e static const char * const accNames[] = { "None", "", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "FAKE"}; // sync with baroSensor_e @@ -415,7 +415,7 @@ static const char * const lookupTableNRF24RX[] = { "CX10", "CX10A", "H8_3D", - "REF" + "INAV" }; #endif diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 6a3b4b9fce..4c051b680d 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1050,7 +1050,7 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_RX_CONFIG: - headSerialReply(17); + headSerialReply(21); serialize8(masterConfig.rxConfig.serialrx_provider); serialize16(masterConfig.rxConfig.maxcheck); serialize16(masterConfig.rxConfig.midrc); @@ -1058,6 +1058,9 @@ static bool processOutCommand(uint8_t cmdMSP) serialize8(masterConfig.rxConfig.spektrum_sat_bind); serialize16(masterConfig.rxConfig.rx_min_usec); serialize16(masterConfig.rxConfig.rx_max_usec); + serialize8(0); // for compatibillity with betaflight + serialize8(0); // for compatibillity with betaflight + serialize16(0); // for compatibillity with betaflight serialize8(masterConfig.rxConfig.nrf24rx_protocol); serialize32(masterConfig.rxConfig.nrf24rx_id); break; @@ -1589,9 +1592,15 @@ static bool processInCommand(void) masterConfig.rxConfig.rx_max_usec = read16(); } if (currentPort->dataSize > 12) { + // for compatibility with betaflight + read8(); + read8(); + read16(); + } + if (currentPort->dataSize > 16) { masterConfig.rxConfig.nrf24rx_protocol = read8(); } - if (currentPort->dataSize > 13) { + if (currentPort->dataSize > 17) { masterConfig.rxConfig.nrf24rx_id = read32(); } break; diff --git a/src/main/rx/nrf24_inav.c b/src/main/rx/nrf24_inav.c index e9831cfa99..157afbaaec 100644 --- a/src/main/rx/nrf24_inav.c +++ b/src/main/rx/nrf24_inav.c @@ -230,7 +230,7 @@ static void inavNrf24Setup(nrf24_protocol_t protocol, const uint32_t *nrf24rx_id UNUSED(protocol); NRF24L01_Initialize(BV(NRF24L01_00_CONFIG_EN_CRC) | BV( NRF24L01_00_CONFIG_CRCO)); // sets PWR_UP, EN_CRC, CRCO - 2 byte CRC - NRF24L01_Setup(); + NRF24L01_SetupBasic(); nrf24rxIdPtr = (uint32_t*)nrf24rx_id; if (nrf24rx_id == NULL || *nrf24rx_id == 0) { diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index e0559761ec..9f74e23e26 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -26,6 +26,7 @@ typedef enum { GYRO_L3GD20, GYRO_MPU6000, GYRO_MPU6500, + GYRO_MPU9250, GYRO_FAKE } gyroSensor_e; diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 971a797132..60ea7d818e 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -22,7 +22,6 @@ #include "build/build_config.h" - #include "common/axis.h" #include "drivers/io.h" @@ -46,6 +45,7 @@ #include "drivers/bus_spi.h" #include "drivers/accgyro_spi_mpu6000.h" #include "drivers/accgyro_spi_mpu6500.h" +#include "drivers/accgyro_spi_mpu9250.h" #include "drivers/gyro_sync.h" #include "drivers/barometer.h" @@ -296,6 +296,20 @@ bool detectGyro(void) #endif ; // fallthrough + case GYRO_MPU9250: +#ifdef USE_GYRO_SPI_MPU9250 + + if (mpu9250SpiGyroDetect(&gyro)) + { + gyroHardware = GYRO_MPU9250; +#ifdef GYRO_MPU9250_ALIGN + gyroAlign = GYRO_MPU9250_ALIGN; +#endif + + break; + } +#endif + ; // fallthrough case GYRO_FAKE: #ifdef USE_FAKE_GYRO if (fakeGyroDetect(&gyro)) { diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index c71a29b76a..3139fbb302 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -64,7 +64,7 @@ #define MAG #define USE_MAG_HMC5883 #define USE_MAG_AK8975 -#define USE_MAG_MAG3110 +//#define USE_MAG_MAG3110 #define USE_VCP #define USE_UART1 @@ -85,7 +85,7 @@ //#define USE_RX_V202 #define USE_RX_CX10 //#define USE_RX_H8_3D -#define USE_RX_REF +#define USE_RX_INAV #define NRF24_DEFAULT_PROTOCOL NRF24RX_SYMA_X5C //#define NRF24_DEFAULT_PROTOCOL NRF24RX_V202_1M //#define NRF24_DEFAULT_PROTOCOL NRF24RX_H8_3D @@ -190,7 +190,7 @@ #undef SPEKTRUM_BIND #else -#define TARGET_MOTOR_COUNT 6 +#define TARGET_MOTOR_COUNT 4 #define DISABLE_UNCOMMON_MIXERS #endif //OPBL diff --git a/src/main/target/CJMCU/target.h b/src/main/target/CJMCU/target.h index 53568cd0ff..b1052e9e02 100644 --- a/src/main/target/CJMCU/target.h +++ b/src/main/target/CJMCU/target.h @@ -70,7 +70,7 @@ #define USE_RX_NRF24 #define USE_RX_CX10 #define USE_RX_H8_3D -#define USE_RX_REF +#define USE_RX_INAV //#define USE_RX_SYMA //#define USE_RX_V202 //#define NRF24_DEFAULT_PROTOCOL NRF24RX_SYMA_X5C diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index c346b5173b..7b802adbde 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -142,7 +142,7 @@ #define USE_RX_CX10 #define USE_RX_H8_3D -#define USE_RX_REF +#define USE_RX_INAV //#define USE_RX_SYMA //#define USE_RX_V202 //#define NRF24_DEFAULT_PROTOCOL NRF24RX_SYMA_X5C