1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 00:35:34 +03:00

Initial cut on BrainFPV RADIX

This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2018-03-23 20:50:52 +10:00
parent 2aac86663d
commit a2ba4c1d91
9 changed files with 530 additions and 1 deletions

View file

@ -0,0 +1,277 @@
/*
* This file is part of INAV.
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
* You can obtain one at http://mozilla.org/MPL/2.0/.
*
* Alternatively, the contents of this file may be used under the terms
* of the GNU General Public License Version 3, as described below:
*
* This file is free software: you may copy, redistribute and/or modify
* it under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or (at your
* option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
* Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*/
#include <stdbool.h>
#include <stdint.h>
#include "platform.h"
#include "build/debug.h"
#include "common/axis.h"
#include "common/maths.h"
#include "common/utils.h"
#include "drivers/system.h"
#include "drivers/time.h"
#include "drivers/io.h"
#include "drivers/exti.h"
#include "drivers/bus.h"
#include "drivers/sensor.h"
#include "drivers/accgyro/accgyro.h"
#include "drivers/accgyro/accgyro_bmi160.h"
#if defined(USE_GYRO_BMI160) || defined(USE_ACC_BMI160)
/* BMI160 Registers */
#define BMI160_REG_CHIPID 0x00
#define BMI160_REG_PMU_STAT 0x03
#define BMI160_REG_GYR_DATA_X_LSB 0x0C
#define BMI160_REG_ACC_DATA_X_LSB 0x12
#define BMI160_REG_STATUS 0x1B
#define BMI160_REG_TEMPERATURE_0 0x20
#define BMI160_REG_ACC_CONF 0x40
#define BMI160_REG_ACC_RANGE 0x41
#define BMI160_REG_GYR_CONF 0x42
#define BMI160_REG_GYR_RANGE 0x43
#define BMI160_REG_INT_EN1 0x51
#define BMI160_REG_INT_OUT_CTRL 0x53
#define BMI160_REG_INT_MAP1 0x56
#define BMI160_REG_FOC_CONF 0x69
#define BMI160_REG_CONF 0x6A
#define BMI160_REG_OFFSET_0 0x77
#define BMI160_REG_CMD 0x7E
/* Register values */
#define BMI160_PMU_CMD_PMU_ACC_NORMAL 0x11
#define BMI160_PMU_CMD_PMU_GYR_NORMAL 0x15
#define BMI160_INT_EN1_DRDY 0x10
#define BMI160_INT_OUT_CTRL_INT1_CONFIG 0x0A
#define BMI160_REG_INT_MAP1_INT1_DRDY 0x80
#define BMI160_CMD_START_FOC 0x03
#define BMI160_CMD_PROG_NVM 0xA0
#define BMI160_REG_STATUS_NVM_RDY 0x10
#define BMI160_REG_STATUS_FOC_RDY 0x08
#define BMI160_REG_CONF_NVM_PROG_EN 0x02
#define BMI160_BWP_NORMAL 0x20
#define BMI160_BWP_OSR2 0x10
#define BMI160_BWP_OSR4 0x00
#define BMI160_ODR_400_Hz 0x0A
#define BMI160_ODR_800_Hz 0x0B
#define BMI160_ODR_1600_Hz 0x0C
#define BMI160_ODR_3200_Hz 0x0D
#define BMI160_RANGE_2G 0x03
#define BMI160_RANGE_4G 0x05
#define BMI160_RANGE_8G 0x08
#define BMI160_RANGE_16G 0x0C
#define BMI160_RANGE_125DPS 0x04
#define BMI160_RANGE_250DPS 0x03
#define BMI160_RANGE_500DPS 0x02
#define BMI160_RANGE_1000DPS 0x01
#define BMI160_RANGE_2000DPS 0x00
typedef struct __attribute__ ((__packed__)) bmi160ContextData_s {
uint16_t chipMagicNumber;
uint8_t lastReadStatus;
uint8_t __padding;
uint8_t gyroRaw[6];
uint8_t accRaw[6];
} bmi160ContextData_t;
STATIC_ASSERT(sizeof(bmi160ContextData_t) < BUS_SCRATCHPAD_MEMORY_SIZE, busDevice_scratchpad_memory_too_small);
static const gyroFilterAndRateConfig_t gyroConfigs[] = {
{ GYRO_LPF_256HZ, 3200, { BMI160_BWP_NORMAL | BMI160_ODR_3200_Hz} },
{ GYRO_LPF_256HZ, 1600, { BMI160_BWP_NORMAL | BMI160_ODR_1600_Hz} },
{ GYRO_LPF_256HZ, 800, { BMI160_BWP_NORMAL | BMI160_ODR_800_Hz } },
{ GYRO_LPF_188HZ, 800, { BMI160_BWP_OSR2 | BMI160_ODR_800_Hz } }, // ODR = 800 Hz, LPF = 128 Hz
{ GYRO_LPF_188HZ, 400, { BMI160_BWP_NORMAL | BMI160_ODR_400_Hz } }, // ODR = 400 Hz, LPF = 137 Hz
{ GYRO_LPF_98HZ, 800, { BMI160_BWP_OSR4 | BMI160_ODR_800_Hz } }, // ODR = 800 Hz, LPF = 63 Hz
{ GYRO_LPF_98HZ, 400, { BMI160_BWP_OSR2 | BMI160_ODR_400_Hz } }, // ODR = 400 Hz, LPF = 68 Hz
{ GYRO_LPF_42HZ, 800, { BMI160_BWP_OSR4 | BMI160_ODR_800_Hz } }, // ODR = 800 Hz, LPF = 63 Hz
{ GYRO_LPF_42HZ, 400, { BMI160_BWP_OSR4 | BMI160_ODR_400_Hz } }, // ODR = 400 Hz, LPF = 34 Hz
};
static void bmi160AccAndGyroInit(gyroDev_t *gyro)
{
uint8_t value;
gyroIntExtiInit(gyro);
busSetSpeed(gyro->busDev, BUS_SPEED_INITIALIZATION);
// Normal power mode, can take up to 80+3.8ms
busWrite(gyro->busDev, BMI160_REG_CMD, BMI160_PMU_CMD_PMU_GYR_NORMAL);
delay(100);
busWrite(gyro->busDev, BMI160_REG_CMD, BMI160_PMU_CMD_PMU_ACC_NORMAL);
delay(5);
// Verify that normal power mode was entered
busRead(gyro->busDev, BMI160_REG_PMU_STAT, &value);
if ((value & 0x3C) != 0x14) {
failureMode(FAILURE_GYRO_INIT_FAILED);
}
// Set ranges and ODR
busWrite(gyro->busDev, BMI160_REG_ACC_CONF, BMI160_BWP_OSR4 | BMI160_ODR_1600_Hz);
delay(1);
// Figure out suitable filter configuration
const gyroFilterAndRateConfig_t * config = chooseGyroConfig(gyro->lpf, 1000000 / gyro->requestedSampleIntervalUs, &gyroConfigs[0], ARRAYLEN(gyroConfigs));
gyro->sampleRateIntervalUs = 1000000 / config->gyroRateHz;
busWrite(gyro->busDev, BMI160_REG_GYR_CONF, config->gyroConfigValues[0]);
delay(1);
busWrite(gyro->busDev, BMI160_REG_ACC_RANGE, BMI160_RANGE_8G);
delay(1);
busWrite(gyro->busDev, BMI160_REG_GYR_RANGE, BMI160_RANGE_2000DPS);
delay(1);
// Enable offset compensation
// uint8_t val = spiBusReadRegister(bus, BMI160_REG_OFFSET_0);
// busWrite(gyro->busDev, BMI160_REG_OFFSET_0, val | 0xC0);
// Enable data ready interrupt
busWrite(gyro->busDev, BMI160_REG_INT_EN1, BMI160_INT_EN1_DRDY);
delay(1);
// Enable INT1 pin
busWrite(gyro->busDev, BMI160_REG_INT_OUT_CTRL, BMI160_INT_OUT_CTRL_INT1_CONFIG);
delay(1);
// Map data ready interrupt to INT1 pin
busWrite(gyro->busDev, BMI160_REG_INT_MAP1, BMI160_REG_INT_MAP1_INT1_DRDY);
delay(1);
busSetSpeed(gyro->busDev, BUS_SPEED_FAST);
}
bool bmi160GyroReadScratchpad(gyroDev_t *gyro)
{
bmi160ContextData_t * ctx = busDeviceGetScratchpadMemory(gyro->busDev);
ctx->lastReadStatus = busReadBuf(gyro->busDev, BMI160_REG_GYR_DATA_X_LSB, ctx->gyroRaw, 6 + 6);
if (ctx->lastReadStatus) {
gyro->gyroADCRaw[X] = (int16_t)((ctx->gyroRaw[1] << 8) | ctx->gyroRaw[0]);
gyro->gyroADCRaw[Y] = (int16_t)((ctx->gyroRaw[3] << 8) | ctx->gyroRaw[2]);
gyro->gyroADCRaw[Z] = (int16_t)((ctx->gyroRaw[5] << 8) | ctx->gyroRaw[4]);
return true;
}
return false;
}
bool bmi160AccReadScratchpad(accDev_t *acc)
{
bmi160ContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev);
if (ctx->lastReadStatus) {
acc->ADCRaw[X] = (int16_t)((ctx->accRaw[1] << 8) | ctx->accRaw[0]);
acc->ADCRaw[Y] = (int16_t)((ctx->accRaw[3] << 8) | ctx->accRaw[2]);
acc->ADCRaw[Z] = (int16_t)((ctx->accRaw[5] << 8) | ctx->accRaw[4]);
return true;
}
return false;
}
static void bmi160AccInit(accDev_t *acc)
{
acc->acc_1G = 4096;
}
bool bmi160AccDetect(accDev_t *acc)
{
acc->busDev = busDeviceOpen(BUSTYPE_ANY, DEVHW_BMI160, acc->imuSensorToUse);
if (acc->busDev == NULL) {
return false;
}
bmi160ContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev);
if (ctx->chipMagicNumber != 0xB160) {
return false;
}
acc->initFn = bmi160AccInit;
acc->readFn = bmi160AccReadScratchpad;
return true;
}
static bool deviceDetect(busDevice_t * busDev)
{
uint8_t attempts;
busSetSpeed(busDev, BUS_SPEED_INITIALIZATION);
for (attempts = 0; attempts < 5; attempts++) {
uint8_t chipId;
delay(100);
busRead(busDev, BMI160_REG_CHIPID, &chipId);
if (chipId == 0xD1) {
return true;
}
}
return false;
}
bool bmi160GyroDetect(gyroDev_t *gyro)
{
gyro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_BMI160, gyro->imuSensorToUse, OWNER_MPU);
if (gyro->busDev == NULL) {
return false;
}
if (!deviceDetect(gyro->busDev)) {
busDeviceDeInit(gyro->busDev);
return false;
}
// Magic number for ACC detection to indicate that we have detected BMI160 gyro
bmi160ContextData_t * ctx = busDeviceGetScratchpadMemory(gyro->busDev);
ctx->chipMagicNumber = 0xB160;
gyro->initFn = bmi160AccAndGyroInit;
gyro->readFn = bmi160GyroReadScratchpad;
gyro->intStatusFn = gyroCheckDataReady;
gyro->temperatureFn = NULL;
gyro->scale = 1.0f / 16.4f; // 16.4 dps/lsb scalefactor
return true;
}
#endif

View file

@ -0,0 +1,30 @@
/*
* This file is part of INAV.
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
* You can obtain one at http://mozilla.org/MPL/2.0/.
*
* Alternatively, the contents of this file may be used under the terms
* of the GNU General Public License Version 3, as described below:
*
* This file is free software: you may copy, redistribute and/or modify
* it under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or (at your
* option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
* Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*/
#pragma once
#include "drivers/sensor.h"
bool bmi160AccDetect(accDev_t *acc);
bool bmi160GyroDetect(gyroDev_t *gyro);

View file

@ -46,6 +46,7 @@
#include "drivers/accgyro/accgyro_adxl345.h"
#include "drivers/accgyro/accgyro_mma845x.h"
#include "drivers/accgyro/accgyro_bma280.h"
#include "drivers/accgyro/accgyro_bmi160.h"
#include "drivers/accgyro/accgyro_fake.h"
#include "drivers/logging.h"
#include "drivers/sensor.h"
@ -246,6 +247,22 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
FALLTHROUGH;
#endif
#if defined(USE_ACC_BMI160)
case ACC_BMI160:
if (bmi160AccDetect(dev)) {
#ifdef ACC_BMI160_ALIGN
dev->accAlign = ACC_BMI160_ALIGN;
#endif
accHardware = ACC_BMI160;
break;
}
/* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
if (accHardwareToUse != ACC_AUTODETECT) {
break;
}
FALLTHROUGH;
#endif
#ifdef USE_FAKE_ACC
case ACC_FAKE:
if (fakeAccDetect(dev)) {

View file

@ -39,7 +39,8 @@ typedef enum {
ACC_MPU6000 = 7,
ACC_MPU6500 = 8,
ACC_MPU9250 = 9,
ACC_FAKE = 10,
ACC_BMI160 = 10,
ACC_FAKE = 11,
ACC_MAX = ACC_FAKE
} accelerationSensor_e;

View file

@ -47,6 +47,7 @@
#include "drivers/accgyro/accgyro_adxl345.h"
#include "drivers/accgyro/accgyro_mma845x.h"
#include "drivers/accgyro/accgyro_bma280.h"
#include "drivers/accgyro/accgyro_bmi160.h"
#include "drivers/accgyro/accgyro_fake.h"
#include "drivers/io.h"
#include "drivers/logging.h"
@ -201,6 +202,18 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
FALLTHROUGH;
#endif
#ifdef USE_GYRO_BMI160
case GYRO_BMI160:
if (bmi160GyroDetect(dev)) {
gyroHardware = GYRO_BMI160;
#ifdef GYRO_BMI160_ALIGN
dev->gyroAlign = GYRO_BMI160_ALIGN;
#endif
break;
}
FALLTHROUGH;
#endif
#ifdef USE_FAKE_GYRO
case GYRO_FAKE:
if (fakeGyroDetect(dev)) {

View file

@ -34,6 +34,7 @@ typedef enum {
GYRO_MPU6000,
GYRO_MPU6500,
GYRO_MPU9250,
GYRO_BMI160,
GYRO_FAKE
} gyroSensor_e;

View file

@ -0,0 +1,35 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdint.h>
#include <platform.h>
#include "drivers/io.h"
#include "drivers/bus.h"
#include "drivers/pwm_mapping.h"
#include "drivers/timer.h"
/* TIMERS */
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
{ TIM12, IO_TAG(PB14), TIM_Channel_1, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM12, TIM_USE_PPM },
{ TIM5, IO_TAG(PA2), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM5, TIM_USE_MC_MOTOR }, // S3_OUT
{ TIM5, IO_TAG(PA3), TIM_Channel_4, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM5, TIM_USE_MC_MOTOR }, // S4_OUT
{ TIM1, IO_TAG(PA10), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM1, TIM_USE_MC_MOTOR }, // S5_OUT
{ TIM2, IO_TAG(PA15), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM2, TIM_USE_MC_MOTOR }, // S6_OUT
{ TIM8, IO_TAG(PC8), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_MC_MOTOR }, // S5_OUT
{ TIM3, IO_TAG(PB0), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR }, // S6_OUT
};

View file

@ -0,0 +1,144 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#define TARGET_BOARD_IDENTIFIER "RADIX"
#define USBD_PRODUCT_STRING "BrainFPV RADIX"
#define LED0 PA4
#define LED0_INVERTED
#define BEEPER NONE
#define USE_EXTI
#define BMI160_SPI_BUS BUS_SPI3
#define BMI160_CS_PIN PB4
#define GYRO_EXTI_PIN PC13
#define USE_GYRO
#define USE_GYRO_BMI160
#define GYRO_BMI160_ALIGN CW0_DEG
#define USE_ACC
#define USE_ACC_BMI160
#define ACC_BMI160_ALIGN CW0_DEG
// #define USE_MAG
// #define MAG_I2C_BUS BUS_I2C1
// #define MAG_HMC5883_ALIGN CW90_DEG
// #define USE_MAG_HMC5883
// #define USE_MAG_QMC5883
// #define USE_MAG_IST8310
// #define USE_MAG_MAG3110
#define USE_BARO
#define USE_BARO_BMP280
#define BMP280_SPI_BUS BUS_SPI3
#define BMP280_CS_PIN PB8
//#define USE_PITOT_MS4525
//#define PITOT_I2C_BUS BUS_I2C2
// #define USE_OPTICAL_FLOW
// #define USE_OPFLOW_CXOF
// #define USE_RANGEFINDER
// #define USE_RANGEFINDER_UIB
// #define USE_RANGEFINDER_VL53L0X
// #define VL53L0X_I2C_BUS BUS_I2C2
#define USE_VCP
#define VBUS_SENSING_PIN PA9
#define VBUS_SENSING_ENABLED
#define USE_UART1
#define UART1_RX_PIN PB7
#define UART1_TX_PIN PB6
#define USE_UART3
#define UART3_RX_PIN PC5
#define UART3_TX_PIN PB10
#define USE_UART4
#define UART4_RX_PIN PA1
#define UART4_TX_PIN PA0
#define USE_UART6
#define UART6_RX_PIN PC7
#define UART6_TX_PIN PC6
#define SERIAL_PORT_COUNT 5 //VCP, USART1, USART3, USART4, USART6
#define USE_SPI
#define USE_SPI_DEVICE_1
#define SPI1_SCK_PIN PA5
#define SPI1_MISO_PIN PA6
#define SPI1_MOSI_PIN PA7
#define USE_SPI_DEVICE_3
#define SPI3_SCK_PIN PB3
#define SPI3_MISO_PIN PC11
#define SPI3_MOSI_PIN PC12
// #define USE_I2C
// #define USE_I2C_DEVICE_1
// #define I2C1_SCL PB8
// #define I2C1_SDA PB9
#define USE_ADC
#define ADC_CHANNEL_1_PIN PC0
#define ADC_CHANNEL_2_PIN PC1
#define ADC_CHANNEL_3_PIN PC3
#define VBAT_ADC_CHANNEL ADC_CHN_1
#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2
#define RSSI_ADC_CHANNEL ADC_CHN_3
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
#define USE_SDCARD
#define SDCARD_DETECT_INVERTED
#define SDCARD_SPI_INSTANCE SPI2
#define SDCARD_DETECT_PIN PB13
#define SDCARD_SPI_CS_PIN PB15
#define SDCARD_DMA_CHANNEL_TX DMA2_Stream3
#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF3
#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA2
#define SDCARD_DMA_CHANNEL DMA_Channel_3
#define SENSORS_SET (SENSOR_ACC|SENSOR_BARO)
#define DEFAULT_RX_TYPE RX_TYPE_SERIAL
#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX)
#define USE_SPEKTRUM_BIND
#define BIND_PIN PB14
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
#define MAX_PWM_OUTPUT_PORTS 6
#define TARGET_IO_PORTA 0xffff
#define TARGET_IO_PORTB 0xffff
#define TARGET_IO_PORTC 0xffff
#define TARGET_IO_PORTD (BIT(2))
#define USABLE_TIMER_CHANNEL_COUNT 7
#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(5) | TIM_N(12) )

View file

@ -0,0 +1,11 @@
F446_TARGETS += $(TARGET)
FEATURES += VCP SDCARD
HSE_VALUE = 16000000
TARGET_SRC = \
drivers/accgyro/accgyro_bmi160.c \
drivers/barometer/barometer_bmp280.c \
drivers/compass/compass_hmc5883l.c \
drivers/compass/compass_qmc5883l.c \
drivers/compass/compass_ist8310.c \
drivers/compass/compass_mag3110.c