1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-23 16:25:31 +03:00

Merge branch 'master' into task_dispatch

This commit is contained in:
blckmn 2017-01-10 09:39:59 +11:00
commit 1e75f90c52
87 changed files with 11222 additions and 1000 deletions

View file

@ -55,6 +55,10 @@
#define FLASH_PAGE_SIZE ((uint32_t)0x40000)
#endif
#if defined(STM32F722xx)
#define FLASH_PAGE_SIZE ((uint32_t)0x20000)
#endif
#if defined(STM32F40_41xxx)
#define FLASH_PAGE_SIZE ((uint32_t)0x20000)
#endif
@ -77,13 +81,15 @@
#if defined(FLASH_SIZE)
#if defined(STM32F40_41xxx)
#define FLASH_PAGE_COUNT 4 // just to make calculations work
#define FLASH_PAGE_COUNT 4
#elif defined (STM32F411xE)
#define FLASH_PAGE_COUNT 3 // just to make calculations work
#define FLASH_PAGE_COUNT 3
#elif defined (STM32F722xx)
#define FLASH_PAGE_COUNT 3
#elif defined (STM32F745xx)
#define FLASH_PAGE_COUNT 4 // just to make calculations work
#define FLASH_PAGE_COUNT 4
#elif defined (STM32F746xx)
#define FLASH_PAGE_COUNT 4 // just to make calculations work
#define FLASH_PAGE_COUNT 4
#else
#define FLASH_PAGE_COUNT ((FLASH_SIZE * 0x400) / FLASH_PAGE_SIZE)
#endif
@ -142,6 +148,9 @@ bool isEEPROMContentValid(void)
if (temp->size != sizeof(master_t) || temp->magic_be != 0xBE || temp->magic_ef != 0xEF)
return false;
if (strncasecmp(temp->boardIdentifier, TARGET_BOARD_IDENTIFIER, sizeof(TARGET_BOARD_IDENTIFIER)))
return false;
// verify integrity of temporary copy
checksum = calculateChecksum((const uint8_t *) temp, sizeof(master_t));
if (checksum != 0)

View file

@ -17,10 +17,9 @@
#pragma once
#define EEPROM_CONF_VERSION 148
#define EEPROM_CONF_VERSION 150
void initEEPROM(void);
void writeEEPROM();
void readEEPROM(void);
bool isEEPROMContentValid(void);

View file

@ -86,6 +86,7 @@
#define failsafeConfig(x) (&masterConfig.failsafeConfig)
#define serialConfig(x) (&masterConfig.serialConfig)
#define telemetryConfig(x) (&masterConfig.telemetryConfig)
#define ibusTelemetryConfig(x) (&masterConfig.telemetryConfig)
#define ppmConfig(x) (&masterConfig.ppmConfig)
#define pwmConfig(x) (&masterConfig.pwmConfig)
#define adcConfig(x) (&masterConfig.adcConfig)
@ -136,6 +137,7 @@ typedef struct master_s {
pidConfig_t pidConfig;
uint8_t debug_mode; // Processing denominator for PID controller vs gyro sampling rate
uint8_t task_statistics;
gyroConfig_t gyroConfig;
compassConfig_t compassConfig;
@ -241,6 +243,7 @@ typedef struct master_s {
uint32_t preferred_beeper_off_flags;
char name[MAX_NAME_LENGTH + 1];
char boardIdentifier[sizeof(TARGET_BOARD_IDENTIFIER)];
uint8_t magic_ef; // magic number, should be 0xEF
uint8_t chk; // XOR checksum

View file

@ -26,6 +26,10 @@
#define MPU_I2C_INSTANCE I2C_DEVICE
#endif
#if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689)
#define GYRO_SUPPORTS_32KHZ
#endif
#define GYRO_LPF_256HZ 0
#define GYRO_LPF_188HZ 1
#define GYRO_LPF_98HZ 2
@ -35,15 +39,24 @@
#define GYRO_LPF_5HZ 6
#define GYRO_LPF_NONE 7
typedef enum {
GYRO_RATE_1_kHz,
GYRO_RATE_8_kHz,
GYRO_RATE_32_kHz,
} gyroRateKHz_e;
typedef struct gyroDev_s {
sensorGyroInitFuncPtr init; // initialize function
sensorGyroReadFuncPtr read; // read 3 axis data function
sensorGyroReadDataFuncPtr temperature; // read temperature if available
sensorGyroInterruptStatusFuncPtr intStatus;
sensorGyroUpdateFuncPtr update;
extiCallbackRec_t exti;
float scale; // scalefactor
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
uint16_t lpf;
volatile int16_t gyroADCRaw[XYZ_AXIS_COUNT];
uint8_t lpf;
gyroRateKHz_e gyroRateKHz;
uint8_t mpuDividerDrops;
volatile bool dataReady;
sensor_align_e gyroAlign;
mpuDetectionResult_t mpuDetectionResult;

View file

@ -22,6 +22,7 @@
#include "platform.h"
#include "build/atomic.h"
#include "build/build_config.h"
#include "build/debug.h"
@ -46,7 +47,6 @@
#include "accgyro_spi_mpu9250.h"
#include "accgyro_mpu.h"
//#define DEBUG_MPU_DATA_READY_INTERRUPT
mpuResetFuncPtr mpuReset;
@ -220,15 +220,20 @@ static void mpu6050FindRevision(gyroDev_t *gyro)
#if defined(MPU_INT_EXTI)
static void mpuIntExtiHandler(extiCallbackRec_t *cb)
{
#ifdef DEBUG_MPU_DATA_READY_INTERRUPT
static uint32_t lastCalledAtUs = 0;
const uint32_t nowUs = micros();
debug[0] = (uint16_t)(nowUs - lastCalledAtUs);
lastCalledAtUs = nowUs;
#endif
gyroDev_t *gyro = container_of(cb, gyroDev_t, exti);
gyro->dataReady = true;
if (gyro->update) {
gyro->update(gyro);
}
#ifdef DEBUG_MPU_DATA_READY_INTERRUPT
static uint32_t lastCalledAt = 0;
uint32_t now = micros();
uint32_t callDelta = now - lastCalledAt;
debug[0] = callDelta;
lastCalledAt = now;
const uint32_t now2Us = micros();
debug[1] = (uint16_t)(now2Us - nowUs);
#endif
}
#endif
@ -296,6 +301,13 @@ bool mpuAccRead(accDev_t *acc)
return true;
}
void mpuGyroSetIsrUpdate(gyroDev_t *gyro, sensorGyroUpdateFuncPtr updateFn)
{
ATOMIC_BLOCK(NVIC_PRIO_MPU_INT_EXTI) {
gyro->update = updateFn;
}
}
bool mpuGyroRead(gyroDev_t *gyro)
{
uint8_t data[6];

View file

@ -18,6 +18,13 @@
#pragma once
#include "exti.h"
#include "sensor.h"
//#define DEBUG_MPU_DATA_READY_INTERRUPT
#if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689)
#define GYRO_USES_SPI
#endif
// MPU6050
#define MPU_RA_WHO_AM_I 0x75
@ -190,3 +197,5 @@ bool mpuAccRead(struct accDev_s *acc);
bool mpuGyroRead(struct gyroDev_s *gyro);
mpuDetectionResult_t *mpuDetect(struct gyroDev_s *gyro);
bool mpuCheckDataReady(struct gyroDev_s *gyro);
void mpuGyroSetIsrUpdate(struct gyroDev_s *gyro, sensorGyroUpdateFuncPtr updateFn);

View file

@ -82,7 +82,7 @@ static void mpu6050GyroInit(gyroDev_t *gyro)
gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
delay(100);
gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure
gyro->mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec

View file

@ -63,13 +63,14 @@ void mpu6500GyroInit(gyroDev_t *gyro)
delay(100);
gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
delay(15);
gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED);
gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, raGyroConfigData);
delay(15);
gyro->mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
delay(15);
gyro->mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf);
delay(15);
gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops
gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops
delay(100);
// Data ready interrupt configuration

View file

@ -138,13 +138,14 @@ void icm20689GyroInit(gyroDev_t *gyro)
// delay(100);
gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
delay(15);
gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED);
gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, raGyroConfigData);
delay(15);
gyro->mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
delay(15);
gyro->mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf);
delay(15);
gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops
gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops
delay(100);
// Data ready interrupt configuration

View file

@ -45,7 +45,7 @@
#include "accgyro_spi_mpu6000.h"
static void mpu6000AccAndGyroInit(void);
static void mpu6000AccAndGyroInit(gyroDev_t *gyro);
static bool mpuSpi6000InitDone = false;
@ -128,7 +128,7 @@ void mpu6000SpiGyroInit(gyroDev_t *gyro)
{
mpuGyroInit(gyro);
mpu6000AccAndGyroInit();
mpu6000AccAndGyroInit(gyro);
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
@ -201,7 +201,7 @@ bool mpu6000SpiDetect(void)
return false;
}
static void mpu6000AccAndGyroInit(void)
static void mpu6000AccAndGyroInit(gyroDev_t *gyro)
{
if (mpuSpi6000InitDone) {
return;
@ -229,7 +229,7 @@ static void mpu6000AccAndGyroInit(void)
// Accel Sample Rate 1kHz
// Gyroscope Output Rate = 1kHz when the DLPF is enabled
mpu6000WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops());
mpu6000WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro));
delayMicroseconds(15);
// Gyro +/- 1000 DPS Full Scale

View file

@ -46,7 +46,7 @@
#include "accgyro_mpu.h"
#include "accgyro_spi_mpu9250.h"
static void mpu9250AccAndGyroInit(uint8_t lpf);
static void mpu9250AccAndGyroInit(gyroDev_t *gyro);
static bool mpuSpi9250InitDone = false;
@ -100,7 +100,7 @@ void mpu9250SpiGyroInit(gyroDev_t *gyro)
{
mpuGyroInit(gyro);
mpu9250AccAndGyroInit(gyro->lpf);
mpu9250AccAndGyroInit(gyro);
spiResetErrorCounter(MPU9250_SPI_INSTANCE);
@ -140,7 +140,7 @@ bool verifympu9250WriteRegister(uint8_t reg, uint8_t data)
return false;
}
static void mpu9250AccAndGyroInit(uint8_t lpf) {
static void mpu9250AccAndGyroInit(gyroDev_t *gyro) {
if (mpuSpi9250InitDone) {
return;
@ -153,17 +153,19 @@ static void mpu9250AccAndGyroInit(uint8_t lpf) {
verifympu9250WriteRegister(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
verifympu9250WriteRegister(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3 | FCB_DISABLED); //Fchoice_b defaults to 00 which makes fchoice 11
//Fchoice_b defaults to 00 which makes fchoice 11
const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED);
verifympu9250WriteRegister(MPU_RA_GYRO_CONFIG, raGyroConfigData);
if (lpf == 4) {
if (gyro->lpf == 4) {
verifympu9250WriteRegister(MPU_RA_CONFIG, 1); //1KHz, 184DLPF
} else if (lpf < 4) {
} else if (gyro->lpf < 4) {
verifympu9250WriteRegister(MPU_RA_CONFIG, 7); //8KHz, 3600DLPF
} else if (lpf > 4) {
} else if (gyro->lpf > 4) {
verifympu9250WriteRegister(MPU_RA_CONFIG, 0); //8KHz, 250DLPF
}
verifympu9250WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops
verifympu9250WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro));
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

View file

@ -51,6 +51,10 @@ typedef struct adcDevice_s {
#else
DMA_Channel_TypeDef* DMAy_Channelx;
#endif
#if defined(STM32F7)
ADC_HandleTypeDef ADCHandle;
DMA_HandleTypeDef DmaHandle;
#endif
} adcDevice_t;
extern const adcDevice_t adcHardware[];

View file

@ -85,9 +85,6 @@ ADCDevice adcDeviceByInstance(ADC_TypeDef *instance)
void adcInit(adcConfig_t *config)
{
DMA_HandleTypeDef DmaHandle;
ADC_HandleTypeDef ADCHandle;
uint8_t i;
uint8_t configuredAdcChannels = 0;
@ -136,47 +133,47 @@ void adcInit(adcConfig_t *config)
RCC_ClockCmd(adc.rccADC, ENABLE);
dmaInit(dmaGetIdentifier(adc.DMAy_Streamx), OWNER_ADC, 0);
ADCHandle.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV8;
ADCHandle.Init.ContinuousConvMode = ENABLE;
ADCHandle.Init.Resolution = ADC_RESOLUTION_12B;
ADCHandle.Init.ExternalTrigConv = ADC_EXTERNALTRIGCONV_T1_CC1;
ADCHandle.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
ADCHandle.Init.DataAlign = ADC_DATAALIGN_RIGHT;
ADCHandle.Init.NbrOfConversion = configuredAdcChannels;
ADCHandle.Init.ScanConvMode = configuredAdcChannels > 1 ? ENABLE : DISABLE; // 1=scan more that one channel in group
ADCHandle.Init.DiscontinuousConvMode = DISABLE;
ADCHandle.Init.NbrOfDiscConversion = 0;
ADCHandle.Init.DMAContinuousRequests = ENABLE;
ADCHandle.Init.EOCSelection = DISABLE;
ADCHandle.Instance = adc.ADCx;
adc.ADCHandle.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV8;
adc.ADCHandle.Init.ContinuousConvMode = ENABLE;
adc.ADCHandle.Init.Resolution = ADC_RESOLUTION_12B;
adc.ADCHandle.Init.ExternalTrigConv = ADC_EXTERNALTRIGCONV_T1_CC1;
adc.ADCHandle.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
adc.ADCHandle.Init.DataAlign = ADC_DATAALIGN_RIGHT;
adc.ADCHandle.Init.NbrOfConversion = configuredAdcChannels;
adc.ADCHandle.Init.ScanConvMode = configuredAdcChannels > 1 ? ENABLE : DISABLE; // 1=scan more that one channel in group
adc.ADCHandle.Init.DiscontinuousConvMode = DISABLE;
adc.ADCHandle.Init.NbrOfDiscConversion = 0;
adc.ADCHandle.Init.DMAContinuousRequests = ENABLE;
adc.ADCHandle.Init.EOCSelection = DISABLE;
adc.ADCHandle.Instance = adc.ADCx;
/*##-1- Configure the ADC peripheral #######################################*/
if (HAL_ADC_Init(&ADCHandle) != HAL_OK)
if (HAL_ADC_Init(&adc.ADCHandle) != HAL_OK)
{
/* Initialization Error */
}
DmaHandle.Init.Channel = adc.channel;
DmaHandle.Init.Direction = DMA_PERIPH_TO_MEMORY;
DmaHandle.Init.PeriphInc = DMA_PINC_DISABLE;
DmaHandle.Init.MemInc = configuredAdcChannels > 1 ? DMA_MINC_ENABLE : DMA_MINC_DISABLE;
DmaHandle.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD;
DmaHandle.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD;
DmaHandle.Init.Mode = DMA_CIRCULAR;
DmaHandle.Init.Priority = DMA_PRIORITY_HIGH;
DmaHandle.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
DmaHandle.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_FULL;
DmaHandle.Init.MemBurst = DMA_MBURST_SINGLE;
DmaHandle.Init.PeriphBurst = DMA_PBURST_SINGLE;
DmaHandle.Instance = adc.DMAy_Streamx;
adc.DmaHandle.Init.Channel = adc.channel;
adc.DmaHandle.Init.Direction = DMA_PERIPH_TO_MEMORY;
adc.DmaHandle.Init.PeriphInc = DMA_PINC_DISABLE;
adc.DmaHandle.Init.MemInc = configuredAdcChannels > 1 ? DMA_MINC_ENABLE : DMA_MINC_DISABLE;
adc.DmaHandle.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD;
adc.DmaHandle.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD;
adc.DmaHandle.Init.Mode = DMA_CIRCULAR;
adc.DmaHandle.Init.Priority = DMA_PRIORITY_HIGH;
adc.DmaHandle.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
adc.DmaHandle.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_FULL;
adc.DmaHandle.Init.MemBurst = DMA_MBURST_SINGLE;
adc.DmaHandle.Init.PeriphBurst = DMA_PBURST_SINGLE;
adc.DmaHandle.Instance = adc.DMAy_Streamx;
/*##-2- Initialize the DMA stream ##########################################*/
if (HAL_DMA_Init(&DmaHandle) != HAL_OK)
if (HAL_DMA_Init(&adc.DmaHandle) != HAL_OK)
{
/* Initialization Error */
}
__HAL_LINKDMA(&ADCHandle, DMA_Handle, DmaHandle);
__HAL_LINKDMA(&adc.ADCHandle, DMA_Handle, adc.DmaHandle);
uint8_t rank = 1;
for (i = 0; i < ADC_CHANNEL_COUNT; i++) {
@ -190,14 +187,15 @@ void adcInit(adcConfig_t *config)
sConfig.Offset = 0;
/*##-3- Configure ADC regular channel ######################################*/
if (HAL_ADC_ConfigChannel(&ADCHandle, &sConfig) != HAL_OK)
if (HAL_ADC_ConfigChannel(&adc.ADCHandle, &sConfig) != HAL_OK)
{
/* Channel Configuration Error */
}
}
HAL_CLEANINVALIDATECACHE((uint32_t*)&adcValues, configuredAdcChannels);
/*##-4- Start the conversion process #######################################*/
if(HAL_ADC_Start_DMA(&ADCHandle, (uint32_t*)&adcValues, configuredAdcChannels) != HAL_OK)
if(HAL_ADC_Start_DMA(&adc.ADCHandle, (uint32_t*)&adcValues, configuredAdcChannels) != HAL_OK)
{
/* Start Conversation Error */
}

View file

@ -33,7 +33,9 @@ typedef enum I2CDevice {
I2CDEV_1 = 0,
I2CDEV_2,
I2CDEV_3,
#ifdef USE_I2C4
I2CDEV_4,
#endif
I2CDEV_COUNT
} I2CDevice;

View file

@ -28,7 +28,7 @@
#include "io_impl.h"
#include "rcc.h"
#ifndef SOFT_I2C
#if !defined(SOFT_I2C) && defined(USE_I2C)
#define CLOCKSPEED 800000 // i2c clockspeed 400kHz default (conform specs), 800kHz and 1200kHz (Betaflight default)
@ -62,18 +62,22 @@ static void i2cUnstick(IO_t scl, IO_t sda);
#define I2C3_SDA PB4
#endif
#if defined(USE_I2C4)
#ifndef I2C4_SCL
#define I2C4_SCL PD12
#endif
#ifndef I2C4_SDA
#define I2C4_SDA PD13
#endif
#endif
static i2cDevice_t i2cHardwareMap[] = {
{ .dev = I2C1, .scl = IO_TAG(I2C1_SCL), .sda = IO_TAG(I2C1_SDA), .rcc = RCC_APB1(I2C1), .overClock = I2C1_OVERCLOCK, .ev_irq = I2C1_EV_IRQn, .er_irq = I2C1_ER_IRQn, .af = GPIO_AF4_I2C1 },
{ .dev = I2C2, .scl = IO_TAG(I2C2_SCL), .sda = IO_TAG(I2C2_SDA), .rcc = RCC_APB1(I2C2), .overClock = I2C2_OVERCLOCK, .ev_irq = I2C2_EV_IRQn, .er_irq = I2C2_ER_IRQn, .af = GPIO_AF4_I2C2 },
{ .dev = I2C3, .scl = IO_TAG(I2C3_SCL), .sda = IO_TAG(I2C3_SDA), .rcc = RCC_APB1(I2C3), .overClock = I2C2_OVERCLOCK, .ev_irq = I2C3_EV_IRQn, .er_irq = I2C3_ER_IRQn, .af = GPIO_AF4_I2C3 },
#if defined(USE_I2C4)
{ .dev = I2C4, .scl = IO_TAG(I2C4_SCL), .sda = IO_TAG(I2C4_SDA), .rcc = RCC_APB1(I2C4), .overClock = I2C2_OVERCLOCK, .ev_irq = I2C4_EV_IRQn, .er_irq = I2C4_ER_IRQn, .af = GPIO_AF4_I2C4 }
#endif
};
@ -112,6 +116,7 @@ void I2C3_EV_IRQHandler(void)
HAL_I2C_EV_IRQHandler(&i2cHandle[I2CDEV_3].Handle);
}
#ifdef USE_I2C4
void I2C4_ER_IRQHandler(void)
{
HAL_I2C_ER_IRQHandler(&i2cHandle[I2CDEV_4].Handle);
@ -121,6 +126,7 @@ void I2C4_EV_IRQHandler(void)
{
HAL_I2C_EV_IRQHandler(&i2cHandle[I2CDEV_4].Handle);
}
#endif
static volatile uint16_t i2cErrorCount = 0;
@ -192,9 +198,11 @@ void i2cInit(I2CDevice device)
case I2CDEV_3:
__HAL_RCC_I2C3_CLK_ENABLE();
break;
#ifdef USE_I2C4
case I2CDEV_4:
__HAL_RCC_I2C4_CLK_ENABLE();
break;
#endif
default:
break;
}

View file

@ -25,8 +25,12 @@
#include "display_ug2864hsweg01.h"
#ifndef OLED_I2C_INSTANCE
#if !defined(OLED_I2C_INSTANCE)
#if defined(I2C_DEVICE)
#define OLED_I2C_INSTANCE I2C_DEVICE
#else
#define OLED_I2C_INSTANCE I2C_NONE
#endif
#endif
#define INVERSE_CHAR_FORMAT 0x7f // 0b01111111
@ -250,7 +254,6 @@ void i2c_OLED_send_string(const char *string)
/**
* according to http://www.adafruit.com/datasheets/UG-2864HSWEG01.pdf Chapter 4.4 Page 15
*/
#if 1
bool ug2864hsweg01InitI2C(void)
{
@ -286,42 +289,3 @@ bool ug2864hsweg01InitI2C(void)
return true;
}
#else
void ug2864hsweg01InitI2C(void)
{
i2c_OLED_send_cmd(0xae); //display off
i2c_OLED_send_cmd(0xa4); //SET All pixels OFF
// i2c_OLED_send_cmd(0xa5); //SET ALL pixels ON
delay(50);
// i2c_OLED_send_cmd(0x8D); // charge pump
// i2c_OLED_send_cmd(0x14); // enable
i2c_OLED_send_cmd(0x20); //Set Memory Addressing Mode
i2c_OLED_send_cmd(0x02); //Set Memory Addressing Mode to Page addressing mode(RESET)
// i2c_OLED_send_cmd(0xa0); //colum address 0 mapped to SEG0 (POR)*** wires at bottom
i2c_OLED_send_cmd(0xa1); //colum address 127 mapped to SEG0 (POR) ** wires at top of board
// i2c_OLED_send_cmd(0xC0); // Scan from Right to Left (POR) *** wires at bottom
i2c_OLED_send_cmd(0xC8); // Scan from Left to Right ** wires at top
i2c_OLED_send_cmd(0xa6); // Set WHITE chars on BLACK backround
// i2c_OLED_send_cmd(0xa7); // Set BLACK chars on WHITE backround
i2c_OLED_send_cmd(0x81); // Setup CONTRAST CONTROL, following byte is the contrast Value
i2c_OLED_send_cmd(0xaf); // contrast value between 1 ( == dull) to 256 ( == bright)
// i2c_OLED_send_cmd(0xd3); // Display Offset :
// i2c_OLED_send_cmd(0x0); // 0
// delay(20);
// i2c_OLED_send_cmd(0x40); // Display start line [0;63] -> [0x40;0x7f]
// delay(20);
#ifdef DISPLAY_FONT_DSIZE
i2c_OLED_send_cmd(0xd6); // zoom
i2c_OLED_send_cmd(0x01);// on
#else
// i2c_OLED_send_cmd(0xd6); // zoom
// i2c_OLED_send_cmd(0x00); // off
#endif
delay(20);
i2c_OLED_send_cmd(0xaf); //display on
delay(20);
i2c_OLED_clear_display();
}
#endif

View file

@ -104,4 +104,14 @@ dmaIdentifier_e dmaGetIdentifier(const DMA_Channel_TypeDef* channel)
}
}
return 0;
}
dmaChannelDescriptor_t* getDmaDescriptor(const DMA_Channel_TypeDef* channel)
{
for (int i = 0; i < DMA_MAX_DESCRIPTORS; i++) {
if (dmaDescriptors[i].channel == channel) {
return &dmaDescriptors[i];
}
}
return NULL;
}

View file

@ -38,11 +38,12 @@ typedef struct dmaChannelDescriptor_s {
uint8_t resourceIndex;
} dmaChannelDescriptor_t;
#if defined(STM32F4) || defined(STM32F7)
#if defined(STM32F7)
#define HAL_CLEANINVALIDATECACHE(addr, size) (SCB_CleanInvalidateDCache_by_Addr((uint32_t*)((uint32_t)addr & ~0x1f), ((uint32_t)(addr + size + 0x1f) & ~0x1f) - ((uint32_t)addr & ~0x1f)))
#define HAL_CLEANCACHE(addr, size) (SCB_CleanDCache_by_Addr((uint32_t*)((uint32_t)addr & ~0x1f), ((uint32_t)(addr + size + 0x1f) & ~0x1f) - ((uint32_t)addr & ~0x1f)))
#endif
#if defined(STM32F4) || defined(STM32F7)
uint32_t dmaFlag_IT_TCIF(const DMA_Stream_TypeDef *stream);
typedef enum {
@ -87,6 +88,7 @@ typedef enum {
#define DMA_IT_FEIF ((uint32_t)0x00000001)
dmaIdentifier_e dmaGetIdentifier(const DMA_Stream_TypeDef* stream);
dmaChannelDescriptor_t* getDmaDescriptor(const DMA_Stream_TypeDef* stream);
#else
@ -127,7 +129,7 @@ typedef enum {
#define DMA_IT_TEIF ((uint32_t)0x00000008)
dmaIdentifier_e dmaGetIdentifier(const DMA_Channel_TypeDef* channel);
dmaChannelDescriptor_t* getDmaDescriptor(const DMA_Channel_TypeDef* channel);
#endif
void dmaInit(dmaIdentifier_e identifier, resourceOwner_e owner, uint8_t resourceIndex);

View file

@ -123,4 +123,14 @@ dmaIdentifier_e dmaGetIdentifier(const DMA_Stream_TypeDef* stream)
}
}
return 0;
}
}
dmaChannelDescriptor_t* getDmaDescriptor(const DMA_Stream_TypeDef* stream)
{
for (int i = 0; i < DMA_MAX_DESCRIPTORS; i++) {
if (dmaDescriptors[i].stream == stream) {
return &dmaDescriptors[i];
}
}
return NULL;
}

View file

@ -14,7 +14,6 @@
#include "accgyro.h"
#include "gyro_sync.h"
static uint8_t mpuDividerDrops;
bool gyroSyncCheckUpdate(gyroDev_t *gyro)
{
@ -23,24 +22,37 @@ bool gyroSyncCheckUpdate(gyroDev_t *gyro)
return gyro->intStatus(gyro);
}
uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator)
uint32_t gyroSetSampleRate(gyroDev_t *gyro, uint8_t lpf, uint8_t gyroSyncDenominator, bool gyro_use_32khz)
{
int gyroSamplePeriod;
#ifndef GYRO_SUPPORTS_32KHZ
if (gyro_use_32khz) {
gyro_use_32khz = false;
}
#endif
float gyroSamplePeriod;
if (lpf == GYRO_LPF_256HZ || lpf == GYRO_LPF_NONE) {
gyroSamplePeriod = 125;
if (gyro_use_32khz) {
gyro->gyroRateKHz = GYRO_RATE_32_kHz;
gyroSamplePeriod = 31.5f;
} else {
gyro->gyroRateKHz = GYRO_RATE_8_kHz;
gyroSamplePeriod = 125.0f;
}
} else {
gyroSamplePeriod = 1000;
gyro->gyroRateKHz = GYRO_RATE_1_kHz;
gyroSamplePeriod = 1000.0f;
gyroSyncDenominator = 1; // Always full Sampling 1khz
}
// calculate gyro divider and targetLooptime (expected cycleTime)
mpuDividerDrops = gyroSyncDenominator - 1;
const uint32_t targetLooptime = gyroSyncDenominator * gyroSamplePeriod;
gyro->mpuDividerDrops = gyroSyncDenominator - 1;
const uint32_t targetLooptime = (uint32_t)(gyroSyncDenominator * gyroSamplePeriod);
return targetLooptime;
}
uint8_t gyroMPU6xxxGetDividerDrops(void)
uint8_t gyroMPU6xxxGetDividerDrops(const gyroDev_t *gyro)
{
return mpuDividerDrops;
return gyro->mpuDividerDrops;
}

View file

@ -5,8 +5,8 @@
* Author: borisb
*/
struct gyroDev_s;
#include "drivers/accgyro.h"
bool gyroSyncCheckUpdate(struct gyroDev_s *gyro);
uint8_t gyroMPU6xxxGetDividerDrops(void);
uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator);
bool gyroSyncCheckUpdate(gyroDev_t *gyro);
uint8_t gyroMPU6xxxGetDividerDrops(const gyroDev_t *gyro);
uint32_t gyroSetSampleRate(gyroDev_t *gyro, uint8_t lpf, uint8_t gyroSyncDenominator, bool gyro_use_32khz);

View file

@ -6,7 +6,7 @@
#define NVIC_PRIO_MAX NVIC_BUILD_PRIORITY(0, 1)
#define NVIC_PRIO_TIMER NVIC_BUILD_PRIORITY(1, 1)
#define NVIC_PRIO_BARO_EXTI NVIC_BUILD_PRIORITY(0x0f, 0x0f)
#define NVIC_PRIO_SONAR_EXTI NVIC_BUILD_PRIORITY(2, 0) // maybe increate slightly
#define NVIC_PRIO_SONAR_EXTI NVIC_BUILD_PRIORITY(2, 0) // maybe increase slightly
#define NVIC_PRIO_TRANSPONDER_DMA NVIC_BUILD_PRIORITY(3, 0)
#define NVIC_PRIO_MPU_INT_EXTI NVIC_BUILD_PRIORITY(0x0f, 0x0f)
#define NVIC_PRIO_MAG_INT_EXTI NVIC_BUILD_PRIORITY(0x0f, 0x0f)

View file

@ -223,6 +223,8 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot
idlePulse = 0;
break;
#ifdef USE_DSHOT
case PWM_TYPE_DSHOT1200:
case PWM_TYPE_DSHOT900:
case PWM_TYPE_DSHOT600:
case PWM_TYPE_DSHOT300:
case PWM_TYPE_DSHOT150:
@ -284,6 +286,25 @@ pwmOutputPort_t *pwmGetMotors(void)
return motors;
}
#ifdef USE_DSHOT
uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType)
{
switch (pwmProtocolType) {
case(PWM_TYPE_DSHOT1200):
return MOTOR_DSHOT1200_MHZ * 1000000;
case(PWM_TYPE_DSHOT900):
return MOTOR_DSHOT900_MHZ * 1000000;
case(PWM_TYPE_DSHOT600):
return MOTOR_DSHOT600_MHZ * 1000000;
case(PWM_TYPE_DSHOT300):
return MOTOR_DSHOT300_MHZ * 1000000;
default:
case(PWM_TYPE_DSHOT150):
return MOTOR_DSHOT150_MHZ * 1000000;
}
}
#endif
#ifdef USE_SERVOS
void pwmWriteServo(uint8_t index, uint16_t value)
{

View file

@ -20,6 +20,7 @@
#include "io/motors.h"
#include "io/servos.h"
#include "drivers/timer.h"
#include "drivers/dma.h"
typedef enum {
PWM_TYPE_STANDARD = 0,
@ -27,14 +28,30 @@ typedef enum {
PWM_TYPE_ONESHOT42,
PWM_TYPE_MULTISHOT,
PWM_TYPE_BRUSHED,
PWM_TYPE_DSHOT600,
PWM_TYPE_DSHOT300,
PWM_TYPE_DSHOT150,
PWM_TYPE_DSHOT300,
PWM_TYPE_DSHOT600,
PWM_TYPE_DSHOT900,
PWM_TYPE_DSHOT1200,
PWM_TYPE_MAX
} motorPwmProtocolTypes_e;
#define PWM_TIMER_MHZ 1
#ifdef USE_DSHOT
#define MAX_DMA_TIMERS 8
#define MOTOR_DSHOT1200_MHZ 24
#define MOTOR_DSHOT900_MHZ 18
#define MOTOR_DSHOT600_MHZ 12
#define MOTOR_DSHOT300_MHZ 6
#define MOTOR_DSHOT150_MHZ 3
#define MOTOR_BIT_0 7
#define MOTOR_BIT_1 14
#define MOTOR_BITLENGTH 19
#endif
#if defined(STM32F40_41xxx) // must be multiples of timer clock
#define ONESHOT125_TIMER_MHZ 12
#define ONESHOT42_TIMER_MHZ 21
@ -70,6 +87,7 @@ typedef struct {
#else
uint8_t dmaBuffer[MOTOR_DMA_BUFFER_SIZE];
#endif
dmaChannelDescriptor_t* dmaDescriptor;
#if defined(STM32F7)
TIM_HandleTypeDef TimHandle;
DMA_HandleTypeDef hdma_tim;
@ -98,6 +116,7 @@ void servoInit(const servoConfig_t *servoConfig);
void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse);
#ifdef USE_DSHOT
uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType);
void pwmWriteDigital(uint8_t index, uint16_t value);
void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType);
void pwmCompleteDigitalMotorUpdate(uint8_t motorCount);

View file

@ -32,16 +32,6 @@
#ifdef USE_DSHOT
#define MAX_DMA_TIMERS 8
#define MOTOR_DSHOT600_MHZ 24
#define MOTOR_DSHOT300_MHZ 12
#define MOTOR_DSHOT150_MHZ 6
#define MOTOR_BIT_0 14
#define MOTOR_BIT_1 29
#define MOTOR_BITLENGTH 39
static uint8_t dmaMotorTimerCount = 0;
static motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS];
static motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS];
@ -94,7 +84,10 @@ void pwmWriteDigital(uint8_t index, uint16_t value)
packet <<= 1;
}
DMA_Cmd(motor->timerHardware->dmaChannel, DISABLE);
TIM_DMACmd(motor->timerHardware->tim, motor->timerDmaSource, DISABLE);
DMA_SetCurrDataCounter(motor->timerHardware->dmaChannel, MOTOR_DMA_BUFFER_SIZE);
DMA_CLEAR_FLAG(motor->dmaDescriptor, DMA_IT_TCIF);
DMA_Cmd(motor->timerHardware->dmaChannel, ENABLE);
}
@ -112,16 +105,6 @@ void pwmCompleteDigitalMotorUpdate(uint8_t motorCount)
}
}
static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor)
{
if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) {
motorDmaOutput_t * const motor = &dmaMotors[descriptor->userParam];
DMA_Cmd(descriptor->channel, DISABLE);
TIM_DMACmd(motor->timerHardware->tim, motor->timerDmaSource, DISABLE);
DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF);
}
}
void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType)
{
TIM_OCInitTypeDef TIM_OCInitStructure;
@ -146,20 +129,7 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t
RCC_ClockCmd(timerRCC(timer), ENABLE);
TIM_Cmd(timer, DISABLE);
uint32_t hz;
switch (pwmProtocolType) {
case(PWM_TYPE_DSHOT600):
hz = MOTOR_DSHOT600_MHZ * 1000000;
break;
case(PWM_TYPE_DSHOT300):
hz = MOTOR_DSHOT300_MHZ * 1000000;
break;
default:
case(PWM_TYPE_DSHOT150):
hz = MOTOR_DSHOT150_MHZ * 1000000;
}
TIM_TimeBaseStructure.TIM_Prescaler = (uint16_t)((SystemCoreClock / timerClockDivisor(timer) / hz) - 1);
TIM_TimeBaseStructure.TIM_Prescaler = (uint16_t)((SystemCoreClock / timerClockDivisor(timer) / getDshotHz(pwmProtocolType)) - 1);
TIM_TimeBaseStructure.TIM_Period = MOTOR_BITLENGTH;
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
@ -201,9 +171,8 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t
}
dmaInit(timerHardware->dmaIrqHandler, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
dmaSetHandler(timerHardware->dmaIrqHandler, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(1, 2), motorIndex);
motor->dmaDescriptor = getDmaDescriptor(channel);
DMA_Cmd(channel, DISABLE);
DMA_DeInit(channel);
DMA_StructInit(&DMA_InitStructure);
DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)timerChCCR(timerHardware);
@ -219,8 +188,6 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t
DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
DMA_Init(channel, &DMA_InitStructure);
DMA_ITConfig(channel, DMA_IT_TC, ENABLE);
}
#endif

View file

@ -31,16 +31,6 @@
#ifdef USE_DSHOT
#define MAX_DMA_TIMERS 8
#define MOTOR_DSHOT600_MHZ 12
#define MOTOR_DSHOT300_MHZ 6
#define MOTOR_DSHOT150_MHZ 3
#define MOTOR_BIT_0 7
#define MOTOR_BIT_1 14
#define MOTOR_BITLENGTH 19
static uint8_t dmaMotorTimerCount = 0;
static motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS];
static motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS];
@ -92,7 +82,9 @@ void pwmWriteDigital(uint8_t index, uint16_t value)
packet <<= 1;
}
TIM_DMACmd(motor->timerHardware->tim, motor->timerDmaSource, DISABLE);
DMA_SetCurrDataCounter(motor->timerHardware->dmaStream, MOTOR_DMA_BUFFER_SIZE);
DMA_CLEAR_FLAG(motor->dmaDescriptor, DMA_IT_TCIF);
DMA_Cmd(motor->timerHardware->dmaStream, ENABLE);
}
@ -110,16 +102,6 @@ void pwmCompleteDigitalMotorUpdate(uint8_t motorCount)
}
}
static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor)
{
if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) {
motorDmaOutput_t * const motor = &dmaMotors[descriptor->userParam];
DMA_Cmd(descriptor->stream, DISABLE);
TIM_DMACmd(motor->timerHardware->tim, motor->timerDmaSource, DISABLE);
DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF);
}
}
void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType)
{
TIM_OCInitTypeDef TIM_OCInitStructure;
@ -144,20 +126,7 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t
RCC_ClockCmd(timerRCC(timer), ENABLE);
TIM_Cmd(timer, DISABLE);
uint32_t hz;
switch (pwmProtocolType) {
case(PWM_TYPE_DSHOT600):
hz = MOTOR_DSHOT600_MHZ * 1000000;
break;
case(PWM_TYPE_DSHOT300):
hz = MOTOR_DSHOT300_MHZ * 1000000;
break;
default:
case(PWM_TYPE_DSHOT150):
hz = MOTOR_DSHOT150_MHZ * 1000000;
}
TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / timerClockDivisor(timer) / hz) - 1;
TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / timerClockDivisor(timer) / getDshotHz(pwmProtocolType)) - 1;
TIM_TimeBaseStructure.TIM_Period = MOTOR_BITLENGTH;
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
@ -199,7 +168,7 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t
}
dmaInit(timerHardware->dmaIrqHandler, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
dmaSetHandler(timerHardware->dmaIrqHandler, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(1, 2), motorIndex);
motor->dmaDescriptor = getDmaDescriptor(stream);
DMA_Cmd(stream, DISABLE);
DMA_DeInit(stream);
@ -222,9 +191,6 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t
DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
DMA_Init(stream, &DMA_InitStructure);
DMA_ITConfig(stream, DMA_IT_TC, ENABLE);
DMA_ClearITPendingBit(stream, dmaFlag_IT_TCIF(timerHardware->dmaStream));
}
#endif

View file

@ -30,16 +30,6 @@
#ifdef USE_DSHOT
#define MAX_DMA_TIMERS 8
#define MOTOR_DSHOT600_MHZ 12
#define MOTOR_DSHOT300_MHZ 6
#define MOTOR_DSHOT150_MHZ 3
#define MOTOR_BIT_0 7
#define MOTOR_BIT_1 14
#define MOTOR_BITLENGTH 19
static uint8_t dmaMotorTimerCount = 0;
static motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS];
static motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS];
@ -62,7 +52,6 @@ uint8_t getTimerIndex(TIM_TypeDef *timer)
void pwmWriteDigital(uint8_t index, uint16_t value)
{
if (!pwmMotorsEnabled) {
return;
}
@ -92,6 +81,9 @@ void pwmWriteDigital(uint8_t index, uint16_t value)
packet <<= 1;
}
/* may not be required */
HAL_DMA_IRQHandler(motor->TimHandle.hdma[motor->timerDmaSource]);
if(HAL_TIM_PWM_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, MOTOR_DMA_BUFFER_SIZE) != HAL_OK)
{
/* Starting PWM generation Error */
@ -102,34 +94,8 @@ void pwmWriteDigital(uint8_t index, uint16_t value)
void pwmCompleteDigitalMotorUpdate(uint8_t motorCount)
{
UNUSED(motorCount);
if (!pwmMotorsEnabled) {
return;
}
for (uint8_t i = 0; i < dmaMotorTimerCount; i++) {
//TIM_SetCounter(dmaMotorTimers[i].timer, 0);
//TIM_DMACmd(dmaMotorTimers[i].timer, dmaMotorTimers[i].timerDmaSources, ENABLE);
}
}
static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor)
{
motorDmaOutput_t * const motor = &dmaMotors[descriptor->userParam];
HAL_DMA_IRQHandler(motor->TimHandle.hdma[motor->timerDmaSource]);
}
/*static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor)
{
if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) {
motorDmaOutput_t * const motor = &dmaMotors[descriptor->userParam];
DMA_Cmd(descriptor->stream, DISABLE);
TIM_DMACmd(motor->timerHardware->tim, motor->timerDmaSource, DISABLE);
DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF);
}
}*/
void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType)
{
motorDmaOutput_t * const motor = &dmaMotors[motorIndex];
@ -149,21 +115,8 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t
if (configureTimer) {
RCC_ClockCmd(timerRCC(timer), ENABLE);
uint32_t hz;
switch (pwmProtocolType) {
case(PWM_TYPE_DSHOT600):
hz = MOTOR_DSHOT600_MHZ * 1000000;
break;
case(PWM_TYPE_DSHOT300):
hz = MOTOR_DSHOT300_MHZ * 1000000;
break;
default:
case(PWM_TYPE_DSHOT150):
hz = MOTOR_DSHOT150_MHZ * 1000000;
}
motor->TimHandle.Instance = timerHardware->tim;
motor->TimHandle.Init.Prescaler = (SystemCoreClock / timerClockDivisor(timer) / hz) - 1;;
motor->TimHandle.Init.Prescaler = (SystemCoreClock / timerClockDivisor(timer) / getDshotHz(pwmProtocolType)) - 1;;
motor->TimHandle.Init.Period = MOTOR_BITLENGTH;
motor->TimHandle.Init.RepetitionCounter = 0;
motor->TimHandle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
@ -180,21 +133,7 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t
motor->TimHandle = dmaMotors[timerIndex].TimHandle;
}
switch (timerHardware->channel) {
case TIM_CHANNEL_1:
motor->timerDmaSource = TIM_DMA_ID_CC1;
break;
case TIM_CHANNEL_2:
motor->timerDmaSource = TIM_DMA_ID_CC2;
break;
case TIM_CHANNEL_3:
motor->timerDmaSource = TIM_DMA_ID_CC3;
break;
case TIM_CHANNEL_4:
motor->timerDmaSource = TIM_DMA_ID_CC4;
break;
}
motor->timerDmaSource = timerDmaSource(timerHardware->channel);
dmaMotorTimers[timerIndex].timerDmaSources |= motor->timerDmaSource;
/* Set the parameters to be configured */
@ -223,7 +162,6 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t
__HAL_LINKDMA(&motor->TimHandle, hdma[motor->timerDmaSource], motor->hdma_tim);
dmaInit(timerHardware->dmaIrqHandler, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
dmaSetHandler(timerHardware->dmaIrqHandler, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(1, 2), motorIndex);
/* Initialize TIMx DMA handle */
if(HAL_DMA_Init(motor->TimHandle.hdma[motor->timerDmaSource]) != HAL_OK)

View file

@ -38,5 +38,6 @@ typedef bool (*sensorAccReadFuncPtr)(struct accDev_s *acc);
struct gyroDev_s;
typedef void (*sensorGyroInitFuncPtr)(struct gyroDev_s *gyro);
typedef bool (*sensorGyroReadFuncPtr)(struct gyroDev_s *gyro);
typedef bool (*sensorGyroUpdateFuncPtr)(struct gyroDev_s *gyro);
typedef bool (*sensorGyroReadDataFuncPtr)(struct gyroDev_s *gyro, int16_t *data);
typedef bool (*sensorGyroInterruptStatusFuncPtr)(struct gyroDev_s *gyro);

View file

@ -112,17 +112,21 @@
#ifdef REMAP_TIM16_DMA
#define DEF_TIM_DMA__TIM16_CH1 DMA1_CH6
#define DEF_TIM_DMA__TIM16_CH1N DMA1_CH6
#define DEF_TIM_DMA__TIM16_UP DMA1_CH6
#else
#define DEF_TIM_DMA__TIM16_CH1 DMA1_CH3
#define DEF_TIM_DMA__TIM16_CH1N DMA1_CH3
#define DEF_TIM_DMA__TIM16_UP DMA1_CH3
#endif
#ifdef REMAP_TIM17_DMA
#define DEF_TIM_DMA__TIM17_CH1 DMA1_CH7
#define DEF_TIM_DMA__TIM17_CH1N DMA1_CH7
#define DEF_TIM_DMA__TIM17_UP DMA1_CH7
#else
#define DEF_TIM_DMA__TIM17_CH1 DMA1_CH1
#define DEF_TIM_DMA__TIM17_CH1N DMA1_CH1
#define DEF_TIM_DMA__TIM17_UP DMA1_CH1
#endif
@ -199,6 +203,8 @@
#define GPIO_AF__PB9_TIM4_CH4 GPIO_AF_2
#define GPIO_AF__PB15_TIM15_CH1N GPIO_AF_2
#define GPIO_AF__PB5_TIM8_CH3N GPIO_AF_3
#define GPIO_AF__PB0_TIM8_CH2N GPIO_AF_4
#define GPIO_AF__PB1_TIM8_CH3N GPIO_AF_4
#define GPIO_AF__PB3_TIM8_CH1N GPIO_AF_4

View file

@ -181,7 +181,7 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->levelSensitivity = 100; // 100 degrees at full stick
pidProfile->setpointRelaxRatio = 30;
pidProfile->dtermSetpointWeight = 200;
pidProfile->yawRateAccelLimit = 20.0f;
pidProfile->yawRateAccelLimit = 10.0f;
pidProfile->rateAccelLimit = 0.0f;
pidProfile->itermThrottleThreshold = 350;
}
@ -439,10 +439,11 @@ void resetBatteryConfig(batteryConfig_t *batteryConfig)
void resetSerialConfig(serialConfig_t *serialConfig)
{
uint8_t index;
memset(serialConfig, 0, sizeof(serialConfig_t));
serialConfig->serial_update_rate_hz = 100;
serialConfig->reboot_character = 'R';
for (index = 0; index < SERIAL_PORT_COUNT; index++) {
for (int index = 0; index < SERIAL_PORT_COUNT; index++) {
serialConfig->portConfigs[index].identifier = serialPortIdentifiers[index];
serialConfig->portConfigs[index].msp_baudrateIndex = BAUD_115200;
serialConfig->portConfigs[index].gps_baudrateIndex = BAUD_57600;
@ -451,13 +452,10 @@ void resetSerialConfig(serialConfig_t *serialConfig)
}
serialConfig->portConfigs[0].functionMask = FUNCTION_MSP;
#if defined(USE_VCP)
// This allows MSP connection via USART & VCP so the board can be reconfigured.
serialConfig->portConfigs[1].functionMask = FUNCTION_MSP;
#endif
serialConfig->reboot_character = 'R';
}
void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig)
@ -620,6 +618,7 @@ void createDefaultConfig(master_t *config)
config->gyroConfig.gyro_soft_notch_cutoff_2 = 100;
config->debug_mode = DEBUG_MODE;
config->task_statistics = true;
resetAccelerometerTrims(&config->accelerometerConfig.accZero);
@ -692,6 +691,7 @@ void createDefaultConfig(master_t *config)
config->rxConfig.rssi_scale = RSSI_SCALE_DEFAULT;
config->rxConfig.rssi_ppm_invert = 0;
config->rxConfig.rcInterpolation = RC_SMOOTHING_AUTO;
config->rxConfig.rcInterpolationChannels = 0;
config->rxConfig.rcInterpolationInterval = 19;
config->rxConfig.fpvCamAngleDegrees = 0;
config->rxConfig.max_aux_channel = MAX_AUX_CHANNELS;
@ -837,6 +837,9 @@ void createDefaultConfig(master_t *config)
resetStatusLedConfig(&config->statusLedConfig);
/* merely to force a reset if the person inadvertently flashes the wrong target */
strncpy(config->boardIdentifier, TARGET_BOARD_IDENTIFIER, sizeof(TARGET_BOARD_IDENTIFIER));
#if defined(TARGET_CONFIG)
targetConfiguration(config);
#endif
@ -1043,6 +1046,25 @@ void validateAndFixGyroConfig(void)
float samplingTime = 0.000125f;
if (gyroConfig()->gyro_use_32khz) {
#ifdef GYRO_SUPPORTS_32KHZ
samplingTime = 0.00003125;
// F1 and F3 can't handle high pid speed.
#if defined(STM32F1)
pidConfig()->pid_process_denom = constrain(pidConfig()->pid_process_denom, 16, 16);
#endif
#if defined(STM32F3)
pidConfig()->pid_process_denom = constrain(pidConfig()->pid_process_denom, 4, 16);
#endif
#else
gyroConfig()->gyro_use_32khz = false;
#endif
}
#if !defined(GYRO_USES_SPI) || !defined(USE_MPU_DATA_READY_SIGNAL)
gyroConfig()->gyro_isr_update = false;
#endif
if (gyroConfig()->gyro_lpf != GYRO_LPF_256HZ && gyroConfig()->gyro_lpf != GYRO_LPF_NONE) {
pidConfig()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
gyroConfig()->gyro_sync_denom = 1;
@ -1063,9 +1085,12 @@ void validateAndFixGyroConfig(void)
motorUpdateRestriction = 0.0001f;
break;
case (PWM_TYPE_DSHOT150):
motorUpdateRestriction = 0.000125f;
motorUpdateRestriction = 0.000250f;
break;
case (PWM_TYPE_DSHOT300):
motorUpdateRestriction = 0.0001f;
break;
case (PWM_TYPE_DSHOT600):
motorUpdateRestriction = 0.0000625f;
break;
default:
@ -1076,7 +1101,7 @@ void validateAndFixGyroConfig(void)
pidConfig()->pid_process_denom = motorUpdateRestriction / (samplingTime * gyroConfig()->gyro_sync_denom);
// Prevent overriding the max rate of motors
if(motorConfig()->useUnsyncedPwm) {
if(motorConfig()->useUnsyncedPwm && (motorConfig()->motorPwmProtocol <= PWM_TYPE_BRUSHED)) {
uint32_t maxEscRate = lrintf(1.0f / motorUpdateRestriction);
if(motorConfig()->motorPwmRate > maxEscRate)

View file

@ -570,4 +570,3 @@ void init(void)
fcTasksInit();
systemState |= SYSTEM_STATE_READY;
}

View file

@ -82,8 +82,6 @@ enum {
#define AIRMODE_THOTTLE_THRESHOLD 1350 // Make configurable in the future. ~35% throttle should be fine
uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
int16_t magHold;
int16_t headFreeModeHold;
@ -94,15 +92,10 @@ static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the m
static float throttlePIDAttenuation;
uint16_t filteredCycleTime;
bool isRXDataNew;
static bool armingCalibrationWasInitialised;
static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
float getThrottlePIDAttenuation(void) {
return throttlePIDAttenuation;
}
float getSetpointRate(int axis) {
return setpointRate[axis];
}
@ -157,7 +150,7 @@ void calculateSetpointRate(int axis, int16_t rc) {
if (rcExpo) {
float expof = rcExpo / 100.0f;
rcCommandf = rcCommandf * power3(rcDeflection[axis]) * expof + rcCommandf * (1-expof);
rcCommandf = rcCommandf * power3(rcDeflectionAbs[axis]) * expof + rcCommandf * (1-expof);
}
angleRate = 200.0f * rcRate * rcCommandf;
@ -216,6 +209,7 @@ void processRcCommand(void)
static int16_t deltaRC[4] = { 0, 0, 0, 0 };
static int16_t factor, rcInterpolationFactor;
static uint16_t currentRxRefreshRate;
const uint8_t interpolationChannels = rxConfig()->rcInterpolationChannels + 1;
uint16_t rxRefreshRate;
bool readyToCalculateRate = false;
@ -247,7 +241,7 @@ void processRcCommand(void)
debug[3] = rxRefreshRate;
}
for (int channel=0; channel < 4; channel++) {
for (int channel=ROLL; channel <= interpolationChannels; channel++) {
deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
lastCommand[channel] = rcCommand[channel];
}
@ -259,7 +253,8 @@ void processRcCommand(void)
// Interpolate steps of rcCommand
if (factor > 0) {
for (int channel=0; channel < 4; channel++) rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
for (int channel=ROLL; channel <= interpolationChannels; channel++)
rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
} else {
factor = 0;
}
@ -689,46 +684,39 @@ void processRx(timeUs_t currentTimeUs)
#endif
}
void subTaskPidController(void)
static void subTaskPidController(void)
{
uint32_t startTime;
if (debugMode == DEBUG_PIDLOOP || debugMode == DEBUG_SCHEDULER) {startTime = micros();}
if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
// PID - note this is function pointer set by setPIDController()
pidController(
&currentProfile->pidProfile,
&accelerometerConfig()->accelerometerTrims
);
if (debugMode == DEBUG_PIDLOOP || debugMode == DEBUG_SCHEDULER) {debug[1] = micros() - startTime;}
pidController(&currentProfile->pidProfile, &accelerometerConfig()->accelerometerTrims, throttlePIDAttenuation);
DEBUG_SET(DEBUG_PIDLOOP, 1, micros() - startTime);
}
void subTaskMainSubprocesses(void)
static void subTaskMainSubprocesses(timeUs_t currentTimeUs)
{
uint32_t startTime;
if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
const uint32_t startTime = micros();
// Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
if (gyro.dev.temperature) {
// Read out gyro temperature if used for telemmetry
if (feature(FEATURE_TELEMETRY) && gyro.dev.temperature) {
gyro.dev.temperature(&gyro.dev, &telemTemperature1);
}
#ifdef MAG
if (sensors(SENSOR_MAG)) {
updateMagHold();
}
#endif
#ifdef GTUNE
updateGtuneState();
if (sensors(SENSOR_MAG)) {
updateMagHold();
}
#endif
#if defined(BARO) || defined(SONAR)
// updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
updateRcCommands();
if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) {
if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) {
applyAltHold(&masterConfig.airplaneConfig);
}
// updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
updateRcCommands();
if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) {
if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) {
applyAltHold(&masterConfig.airplaneConfig);
}
}
#endif
// If we're armed, at minimum throttle, and we do arming via the
@ -768,25 +756,28 @@ void subTaskMainSubprocesses(void)
#ifdef BLACKBOX
if (!cliMode && feature(FEATURE_BLACKBOX)) {
handleBlackbox(startTime);
handleBlackbox(currentTimeUs);
}
#endif
#ifdef TRANSPONDER
transponderUpdate(startTime);
transponderUpdate(currentTimeUs);
#endif
DEBUG_SET(DEBUG_PIDLOOP, 2, micros() - startTime);
}
void subTaskMotorUpdate(void)
static void subTaskMotorUpdate(void)
{
const uint32_t startTime = micros();
uint32_t startTime;
if (debugMode == DEBUG_CYCLETIME) {
startTime = micros();
static uint32_t previousMotorUpdateTime;
const uint32_t currentDeltaTime = startTime - previousMotorUpdateTime;
debug[2] = currentDeltaTime;
debug[3] = currentDeltaTime - targetPidLooptime;
previousMotorUpdateTime = startTime;
} else if (debugMode == DEBUG_PIDLOOP) {
startTime = micros();
}
mixTable(&currentProfile->pidProfile);
@ -818,20 +809,16 @@ uint8_t setPidUpdateCountDown(void)
// Function for loop trigger
void taskMainPidLoop(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
static bool runTaskMainSubprocesses;
static uint8_t pidUpdateCountdown;
cycleTime = getTaskDeltaTime(TASK_SELF);
if (debugMode == DEBUG_CYCLETIME) {
debug[0] = cycleTime;
debug[0] = getTaskDeltaTime(TASK_SELF);
debug[1] = averageSystemLoadPercent;
}
if (runTaskMainSubprocesses) {
subTaskMainSubprocesses();
subTaskMainSubprocesses(currentTimeUs);
runTaskMainSubprocesses = false;
}
@ -841,9 +828,9 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
// 2 - subTaskMainSubprocesses()
// 3 - subTaskMotorUpdate()
uint32_t startTime;
if (debugMode == DEBUG_PIDLOOP || debugMode == DEBUG_SCHEDULER) {startTime = micros();}
if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
gyroUpdate();
if (debugMode == DEBUG_PIDLOOP || debugMode == DEBUG_SCHEDULER) {debug[0] = micros() - startTime;}
DEBUG_SET(DEBUG_PIDLOOP, 0, micros() - startTime);
if (pidUpdateCountdown) {
pidUpdateCountdown--;

View file

@ -34,7 +34,6 @@ void updateLEDs(void);
void updateRcCommands(void);
void taskMainPidLoop(timeUs_t currentTimeUs);
float getThrottlePIDAttenuation(void);
float getSetpointRate(int axis);
float getRcDeflection(int axis);
float getRcDeflectionAbs(int axis);

View file

@ -614,7 +614,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break;
case MSP_STATUS_EX:
sbufWriteU16(dst, cycleTime);
sbufWriteU16(dst, getTaskDeltaTime(TASK_GYROPID));
#ifdef USE_I2C
sbufWriteU16(dst, i2cGetErrorCounter());
#else
@ -638,7 +638,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break;
case MSP_STATUS:
sbufWriteU16(dst, cycleTime);
sbufWriteU16(dst, getTaskDeltaTime(TASK_GYROPID));
#ifdef USE_I2C
sbufWriteU16(dst, i2cGetErrorCounter());
#else
@ -1140,6 +1140,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
sbufWriteU8(dst, motorConfig()->motorPwmProtocol);
sbufWriteU16(dst, motorConfig()->motorPwmRate);
sbufWriteU16(dst, (uint16_t)(motorConfig()->digitalIdleOffsetPercent * 100));
sbufWriteU8(dst, gyroConfig()->gyro_use_32khz);
//!!TODO gyro_isr_update to be added pending decision
//sbufWriteU8(dst, gyroConfig()->gyro_isr_update);
break;
case MSP_FILTER_CONFIG :
@ -1483,6 +1486,14 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
if (dataSize > 7) {
motorConfig()->digitalIdleOffsetPercent = sbufReadU16(src) / 100.0f;
}
if (sbufBytesRemaining(src)) {
gyroConfig()->gyro_use_32khz = sbufReadU8(src);
}
//!!TODO gyro_isr_update to be added pending decision
/*if (sbufBytesRemaining(src)) {
gyroConfig()->gyro_isr_update = sbufReadU8(src);
}*/
validateAndFixGyroConfig();
break;
case MSP_SET_FILTER_CONFIG:

View file

@ -231,6 +231,7 @@ void fcTasksInit(void)
setTaskEnabled(TASK_ATTITUDE, sensors(SENSOR_ACC));
setTaskEnabled(TASK_SERIAL, true);
rescheduleTask(TASK_SERIAL, TASK_PERIOD_HZ(serialConfig()->serial_update_rate_hz));
setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_CURRENT_METER));
setTaskEnabled(TASK_RX, true);
@ -310,7 +311,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
.taskName = "SYSTEM",
.taskFunc = taskSystem,
.desiredPeriod = TASK_PERIOD_HZ(10), // 10Hz, every 100 ms
.staticPriority = TASK_PRIORITY_HIGH,
.staticPriority = TASK_PRIORITY_MEDIUM_HIGH,
},
[TASK_GYROPID] = {

View file

@ -615,10 +615,12 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
newValue = constrain((int)pidProfile->dtermSetpointWeight + delta, 0, 254); // FIXME magic numbers repeated in serial_cli.c
pidProfile->dtermSetpointWeight = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT, newValue);
break;
case ADJUSTMENT_D_SETPOINT_TRANSITION:
newValue = constrain((int)pidProfile->setpointRelaxRatio + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c
pidProfile->setpointRelaxRatio = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT_TRANSITION, newValue);
break;
default:
break;
};

View file

@ -105,8 +105,6 @@ uint8_t cliMode = 0;
#include "telemetry/frsky.h"
#include "telemetry/telemetry.h"
extern uint16_t cycleTime; // FIXME dependency on mw.c
static serialPort_t *cliPort;
static bufWriter_t *cliWriter;
static uint8_t cliWriteBuffer[sizeof(*cliWriter) + 128];
@ -335,7 +333,7 @@ static const char * const lookupTableSuperExpoYaw[] = {
static const char * const lookupTablePwmProtocol[] = {
"OFF", "ONESHOT125", "ONESHOT42", "MULTISHOT", "BRUSHED",
#ifdef USE_DSHOT
"DSHOT600", "DSHOT300", "DSHOT150"
"DSHOT150", "DSHOT300", "DSHOT600", "DSHOT900", "DSHOT1200",
#endif
};
@ -343,6 +341,10 @@ static const char * const lookupTableRcInterpolation[] = {
"OFF", "PRESET", "AUTO", "MANUAL"
};
static const char * const lookupTableRcInterpolationChannels[] = {
"RP", "RPY", "RPYT"
};
static const char * const lookupTableLowpassType[] = {
"PT1", "BIQUAD", "FIR"
};
@ -390,6 +392,7 @@ typedef enum {
TABLE_SUPEREXPO_YAW,
TABLE_MOTOR_PWM_PROTOCOL,
TABLE_RC_INTERPOLATION,
TABLE_RC_INTERPOLATION_CHANNELS,
TABLE_LOWPASS_TYPE,
TABLE_FAILSAFE,
#ifdef OSD
@ -431,6 +434,7 @@ static const lookupTableEntry_t lookupTables[] = {
{ lookupTableSuperExpoYaw, sizeof(lookupTableSuperExpoYaw) / sizeof(char *) },
{ lookupTablePwmProtocol, sizeof(lookupTablePwmProtocol) / sizeof(char *) },
{ lookupTableRcInterpolation, sizeof(lookupTableRcInterpolation) / sizeof(char *) },
{ lookupTableRcInterpolationChannels, sizeof(lookupTableRcInterpolationChannels) / sizeof(char *) },
{ lookupTableLowpassType, sizeof(lookupTableLowpassType) / sizeof(char *) },
{ lookupTableFailsafe, sizeof(lookupTableFailsafe) / sizeof(char *) },
#ifdef OSD
@ -486,12 +490,16 @@ typedef struct {
} clivalue_t;
const clivalue_t valueTable[] = {
#ifndef SKIP_TASK_STATISTICS
{ "task_statistics", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.task_statistics, .config.lookup = { TABLE_OFF_ON } },
#endif
{ "mid_rc", VAR_UINT16 | MASTER_VALUE, &rxConfig()->midrc, .config.minmax = { 1200, 1700 } },
{ "min_check", VAR_UINT16 | MASTER_VALUE, &rxConfig()->mincheck, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "max_check", VAR_UINT16 | MASTER_VALUE, &rxConfig()->maxcheck, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "rssi_channel", VAR_INT8 | MASTER_VALUE, &rxConfig()->rssi_channel, .config.minmax = { 0, MAX_SUPPORTED_RC_CHANNEL_COUNT } },
{ "rssi_scale", VAR_UINT8 | MASTER_VALUE, &rxConfig()->rssi_scale, .config.minmax = { RSSI_SCALE_MIN, RSSI_SCALE_MAX } },
{ "rc_interpolation", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &rxConfig()->rcInterpolation, .config.lookup = { TABLE_RC_INTERPOLATION } },
{ "rc_interpolation_channels", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &rxConfig()->rcInterpolationChannels, .config.lookup = { TABLE_RC_INTERPOLATION_CHANNELS } },
{ "rc_interpolation_interval", VAR_UINT8 | MASTER_VALUE, &rxConfig()->rcInterpolationInterval, .config.minmax = { 1, 50 } },
{ "rssi_ppm_invert", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &rxConfig()->rssi_ppm_invert, .config.lookup = { TABLE_OFF_ON } },
#if defined(USE_PWM)
@ -523,7 +531,8 @@ const clivalue_t valueTable[] = {
{ "fixedwing_althold_dir", VAR_INT8 | MASTER_VALUE, &airplaneConfig()->fixedwing_althold_dir, .config.minmax = { -1, 1 } },
{ "reboot_character", VAR_UINT8 | MASTER_VALUE, &serialConfig()->reboot_character, .config.minmax = { 48, 126 } },
{ "reboot_character", VAR_UINT8 | MASTER_VALUE, &serialConfig()->reboot_character, .config.minmax = { 48, 126 } },
{ "serial_update_rate_hz", VAR_UINT16 | MASTER_VALUE, &serialConfig()->serial_update_rate_hz, .config.minmax = { 100, 2000 } },
#ifdef GPS
{ "gps_provider", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gpsConfig()->provider, .config.lookup = { TABLE_GPS_PROVIDER } },
@ -575,6 +584,9 @@ const clivalue_t valueTable[] = {
{ "frsky_vfas_cell_voltage", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &telemetryConfig()->frsky_vfas_cell_voltage, .config.lookup = { TABLE_OFF_ON } },
{ "hott_alarm_sound_interval", VAR_UINT8 | MASTER_VALUE, &telemetryConfig()->hottAlarmSoundInterval, .config.minmax = { 0, 120 } },
{ "pid_values_as_telemetry", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &telemetryConfig()->pidValuesAsTelemetry, .config.lookup = {TABLE_OFF_ON } },
#if defined(TELEMETRY_IBUS)
{ "ibus_report_cell_voltage", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &ibusTelemetryConfig()->report_cell_voltage, .config.lookup = { TABLE_OFF_ON } },
#endif
#endif
{ "battery_capacity", VAR_UINT16 | MASTER_VALUE, &batteryConfig()->batteryCapacity, .config.minmax = { 0, 20000 } },
@ -601,7 +613,13 @@ const clivalue_t valueTable[] = {
{ "align_board_yaw", VAR_INT16 | MASTER_VALUE, &boardAlignment()->yawDegrees, .config.minmax = { -180, 360 } },
{ "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gyroConfig()->gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } },
{ "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &gyroConfig()->gyro_sync_denom, .config.minmax = { 1, 8 } },
{ "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &gyroConfig()->gyro_sync_denom, .config.minmax = { 1, 32 } },
#if defined(GYRO_USES_SPI) && defined(USE_MPU_DATA_READY_SIGNAL)
{ "gyro_isr_update", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gyroConfig()->gyro_isr_update, .config.lookup = { TABLE_OFF_ON } },
#endif
#ifdef GYRO_SUPPORTS_32KHZ
{ "gyro_use_32khz", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gyroConfig()->gyro_use_32khz, .config.lookup = { TABLE_OFF_ON } },
#endif
{ "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gyroConfig()->gyro_soft_lpf_type, .config.lookup = { TABLE_LOWPASS_TYPE } },
{ "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &gyroConfig()->gyro_soft_lpf_hz, .config.minmax = { 0, 255 } },
{ "gyro_notch1_hz", VAR_UINT16 | MASTER_VALUE, &gyroConfig()->gyro_soft_notch_hz_1, .config.minmax = { 0, 1000 } },
@ -3163,33 +3181,22 @@ static void cliStatus(char *cmdline)
{
UNUSED(cmdline);
cliPrintf("System Uptime: %d seconds, Voltage: %d * 0.1V (%dS battery - %s), CPU:%d%%\r\n",
millis() / 1000,
getVbat(),
batteryCellCount,
getBatteryStateString(),
constrain(averageSystemLoadPercent, 0, 100)
);
cliPrintf("System Uptime: %d seconds\r\n", millis() / 1000);
cliPrintf("Voltage: %d * 0.1V (%dS battery - %s)\r\n", getVbat(), batteryCellCount, getBatteryStateString());
cliPrintf("CPU Clock=%dMHz", (SystemCoreClock / 1000000));
#if (FLASH_SIZE > 64) && !defined(CLI_MINIMAL_VERBOSITY)
uint32_t mask;
uint32_t detectedSensorsMask = sensorsMask();
const uint32_t detectedSensorsMask = sensorsMask();
for (uint32_t i = 0; ; i++) {
if (sensorTypeNames[i] == NULL)
if (sensorTypeNames[i] == NULL) {
break;
mask = (1 << i);
}
const uint32_t mask = (1 << i);
if ((detectedSensorsMask & mask) && (mask & SENSOR_NAMES_MASK)) {
const char *sensorHardware;
uint8_t sensorHardwareIndex = detectedSensors[i];
sensorHardware = sensorHardwareNames[i][sensorHardwareIndex];
const uint8_t sensorHardwareIndex = detectedSensors[i];
const char *sensorHardware = sensorHardwareNames[i][sensorHardwareIndex];
cliPrintf(", %s=%s", sensorTypeNames[i], sensorHardware);
if (mask == SENSOR_ACC && acc.dev.revisionCode) {
cliPrintf(".%c", acc.dev.revisionCode);
}
@ -3198,10 +3205,14 @@ static void cliStatus(char *cmdline)
#endif
cliPrint("\r\n");
#ifdef USE_SDCARD
cliSdInfo(NULL);
#endif
#ifdef USE_I2C
uint16_t i2cErrorCounter = i2cGetErrorCounter();
const uint16_t i2cErrorCounter = i2cGetErrorCounter();
#else
uint16_t i2cErrorCounter = 0;
const uint16_t i2cErrorCounter = 0;
#endif
#ifdef STACK_CHECK
@ -3209,11 +3220,14 @@ static void cliStatus(char *cmdline)
#endif
cliPrintf("Stack size: %d, Stack address: 0x%x\r\n", stackTotalSize(), stackHighMem());
cliPrintf("Cycle Time: %d, I2C Errors: %d, config size: %d\r\n", cycleTime, i2cErrorCounter, sizeof(master_t));
cliPrintf("I2C Errors: %d, config size: %d\r\n", i2cErrorCounter, sizeof(master_t));
const int gyroRate = getTaskDeltaTime(TASK_GYROPID) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_GYROPID)));
const int rxRate = getTaskDeltaTime(TASK_RX) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_RX)));
const int systemRate = getTaskDeltaTime(TASK_SYSTEM) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_SYSTEM)));
cliPrintf("CPU:%d%%, cycle time: %d, GYRO rate: %d, RX rate: %d, System rate: %d\r\n",
constrain(averageSystemLoadPercent, 0, 100), getTaskDeltaTime(TASK_GYROPID), gyroRate, rxRate, systemRate);
#ifdef USE_SDCARD
cliSdInfo(NULL);
#endif
}
#ifndef SKIP_TASK_STATISTICS
@ -3224,7 +3238,11 @@ static void cliTasks(char *cmdline)
int averageLoadSum = 0;
#ifndef CLI_MINIMAL_VERBOSITY
cliPrintf("Task list rate/hz max/us avg/us maxload avgload total/ms\r\n");
if (masterConfig.task_statistics) {
cliPrintf("Task list rate/hz max/us avg/us maxload avgload total/ms\r\n");
} else {
cliPrintf("Task list rate/hz\r\n");
}
#endif
for (cfTaskId_e taskId = 0; taskId < TASK_COUNT; taskId++) {
cfTaskInfo_t taskInfo;
@ -3233,7 +3251,7 @@ static void cliTasks(char *cmdline)
int taskFrequency;
int subTaskFrequency;
if (taskId == TASK_GYROPID) {
subTaskFrequency = (int)(1000000.0f / ((float)cycleTime));
subTaskFrequency = taskInfo.latestDeltaTime == 0 ? 0 : (int)(1000000.0f / ((float)taskInfo.latestDeltaTime));
taskFrequency = subTaskFrequency / pidConfig()->pid_process_denom;
if (pidConfig()->pid_process_denom > 1) {
cliPrintf("%02d - (%13s) ", taskId, taskInfo.taskName);
@ -3242,27 +3260,33 @@ static void cliTasks(char *cmdline)
cliPrintf("%02d - (%9s/%3s) ", taskId, taskInfo.subTaskName, taskInfo.taskName);
}
} else {
taskFrequency = (int)(1000000.0f / ((float)taskInfo.latestDeltaTime));
taskFrequency = taskInfo.latestDeltaTime == 0 ? 0 : (int)(1000000.0f / ((float)taskInfo.latestDeltaTime));
cliPrintf("%02d - (%13s) ", taskId, taskInfo.taskName);
}
const int maxLoad = (taskInfo.maxExecutionTime * taskFrequency + 5000) / 1000;
const int averageLoad = (taskInfo.averageExecutionTime * taskFrequency + 5000) / 1000;
const int maxLoad = taskInfo.maxExecutionTime == 0 ? 0 :(taskInfo.maxExecutionTime * taskFrequency + 5000) / 1000;
const int averageLoad = taskInfo.averageExecutionTime == 0 ? 0 : (taskInfo.averageExecutionTime * taskFrequency + 5000) / 1000;
if (taskId != TASK_SERIAL) {
maxLoadSum += maxLoad;
averageLoadSum += averageLoad;
}
cliPrintf("%6d %7d %7d %4d.%1d%% %4d.%1d%% %9d\r\n",
taskFrequency, taskInfo.maxExecutionTime, taskInfo.averageExecutionTime,
maxLoad/10, maxLoad%10, averageLoad/10, averageLoad%10, taskInfo.totalExecutionTime / 1000);
if (masterConfig.task_statistics) {
cliPrintf("%6d %7d %7d %4d.%1d%% %4d.%1d%% %9d\r\n",
taskFrequency, taskInfo.maxExecutionTime, taskInfo.averageExecutionTime,
maxLoad/10, maxLoad%10, averageLoad/10, averageLoad%10, taskInfo.totalExecutionTime / 1000);
} else {
cliPrintf("%6d\r\n", taskFrequency);
}
if (taskId == TASK_GYROPID && pidConfig()->pid_process_denom > 1) {
cliPrintf(" - (%13s) %6d\r\n", taskInfo.subTaskName, subTaskFrequency);
}
}
}
cfCheckFuncInfo_t checkFuncInfo;
getCheckFuncInfo(&checkFuncInfo);
cliPrintf("RX Check Function %17d %7d %25d\r\n", checkFuncInfo.maxExecutionTime, checkFuncInfo.averageExecutionTime, checkFuncInfo.totalExecutionTime / 1000);
cliPrintf("Total (excluding SERIAL) %23d.%1d%% %4d.%1d%%\r\n", maxLoadSum/10, maxLoadSum%10, averageLoadSum/10, averageLoadSum%10);
if (masterConfig.task_statistics) {
cfCheckFuncInfo_t checkFuncInfo;
getCheckFuncInfo(&checkFuncInfo);
cliPrintf("RX Check Function %17d %7d %25d\r\n", checkFuncInfo.maxExecutionTime, checkFuncInfo.averageExecutionTime, checkFuncInfo.totalExecutionTime / 1000);
cliPrintf("Total (excluding SERIAL) %23d.%1d%% %4d.%1d%%\r\n", maxLoadSum/10, maxLoadSum%10, averageLoadSum/10, averageLoadSum%10);
}
}
#endif
@ -3698,9 +3722,12 @@ static void cliHelp(char *cmdline);
const clicmd_t cmdTable[] = {
CLI_COMMAND_DEF("adjrange", "configure adjustment ranges", NULL, cliAdjustmentRange),
CLI_COMMAND_DEF("aux", "configure modes", NULL, cliAux),
#ifdef BEEPER
CLI_COMMAND_DEF("beeper", "turn on/off beeper", "list\r\n"
"\t<+|->[name]", cliBeeper),
#endif
#ifdef LED_STRIP
CLI_COMMAND_DEF("color", "configure colors", NULL, cliColor),
CLI_COMMAND_DEF("mode_color", "configure mode and special colors", NULL, cliModeColor),
#endif
CLI_COMMAND_DEF("defaults", "reset to defaults and reboot", NULL, cliDefaults),
CLI_COMMAND_DEF("dfu", "DFU mode on reboot", NULL, cliDfu),
@ -3708,6 +3735,9 @@ const clicmd_t cmdTable[] = {
"[master|profile|rates|all] {showdefaults}", cliDiff),
CLI_COMMAND_DEF("dump", "dump configuration",
"[master|profile|rates|all] {showdefaults}", cliDump),
#ifdef USE_ESCSERIAL
CLI_COMMAND_DEF("escprog", "passthrough esc to serial", "<mode [sk/bl/ki/cc]> <index>", cliEscPassthrough),
#endif
CLI_COMMAND_DEF("exit", NULL, NULL, cliExit),
CLI_COMMAND_DEF("feature", "configure features",
"list\r\n"
@ -3720,13 +3750,9 @@ const clicmd_t cmdTable[] = {
CLI_COMMAND_DEF("flash_write", NULL, "<address> <message>", cliFlashWrite),
#endif
#endif
CLI_COMMAND_DEF("get", "get variable value",
"[name]", cliGet),
CLI_COMMAND_DEF("get", "get variable value", "[name]", cliGet),
#ifdef GPS
CLI_COMMAND_DEF("gpspassthrough", "passthrough gps to serial", NULL, cliGpsPassthrough),
#endif
#ifdef USE_ESCSERIAL
CLI_COMMAND_DEF("escprog", "passthrough esc to serial", "<mode [sk/bl/ki/cc]> <index>", cliEscPassthrough),
#endif
CLI_COMMAND_DEF("help", NULL, NULL, cliHelp),
#ifdef LED_STRIP
@ -3735,16 +3761,19 @@ const clicmd_t cmdTable[] = {
CLI_COMMAND_DEF("map", "configure rc channel order",
"[<map>]", cliMap),
#ifndef USE_QUAD_MIXER_ONLY
CLI_COMMAND_DEF("mixer", "configure mixer",
"list\r\n"
CLI_COMMAND_DEF("mixer", "configure mixer", "list\r\n"
"\t<name>", cliMixer),
#endif
CLI_COMMAND_DEF("mmix", "custom motor mixer", NULL, cliMotorMix),
#ifdef LED_STRIP
CLI_COMMAND_DEF("mode_color", "configure mode and special colors", NULL, cliModeColor),
#endif
CLI_COMMAND_DEF("motor", "get/set motor",
"<index> [<value>]", cliMotor),
CLI_COMMAND_DEF("name", "name of craft", NULL, cliName),
#if (FLASH_SIZE > 128)
CLI_COMMAND_DEF("play_sound", NULL,
"[<index>]\r\n", cliPlaySound),
"[<index>]", cliPlaySound),
#endif
CLI_COMMAND_DEF("profile", "change profile",
"[<index>]", cliProfile),
@ -3752,43 +3781,34 @@ const clicmd_t cmdTable[] = {
#if (FLASH_SIZE > 64)
CLI_COMMAND_DEF("resource", "view currently used resources", NULL, cliResource),
#endif
CLI_COMMAND_DEF("rxrange", "configure rx channel ranges", NULL, cliRxRange),
CLI_COMMAND_DEF("rxfail", "show/set rx failsafe settings", NULL, cliRxFail),
CLI_COMMAND_DEF("rxrange", "configure rx channel ranges", NULL, cliRxRange),
CLI_COMMAND_DEF("save", "save and reboot", NULL, cliSave),
#ifdef USE_SDCARD
CLI_COMMAND_DEF("sd_info", "sdcard info", NULL, cliSdInfo),
#endif
CLI_COMMAND_DEF("serial", "configure serial ports", NULL, cliSerial),
#ifndef SKIP_SERIAL_PASSTHROUGH
CLI_COMMAND_DEF("serialpassthrough", "passthrough serial data to port",
"<id> [baud] [mode] : passthrough to serial",
cliSerialPassthrough),
CLI_COMMAND_DEF("serialpassthrough", "passthrough serial data to port", "<id> [baud] [mode] : passthrough to serial", cliSerialPassthrough),
#endif
#ifdef USE_SERVOS
CLI_COMMAND_DEF("servo", "configure servos", NULL, cliServo),
#endif
CLI_COMMAND_DEF("set", "change setting",
"[<name>=<value>]", cliSet),
CLI_COMMAND_DEF("set", "change setting", "[<name>=<value>]", cliSet),
#ifdef USE_SERVOS
CLI_COMMAND_DEF("smix", "servo mixer",
"<rule> <servo> <source> <rate> <speed> <min> <max> <box>\r\n"
CLI_COMMAND_DEF("smix", "servo mixer", "<rule> <servo> <source> <rate> <speed> <min> <max> <box>\r\n"
"\treset\r\n"
"\tload <mixer>\r\n"
"\treverse <servo> <source> r|n", cliServoMix),
#endif
#ifdef USE_SDCARD
CLI_COMMAND_DEF("sd_info", "sdcard info", NULL, cliSdInfo),
#endif
CLI_COMMAND_DEF("status", "show status", NULL, cliStatus),
#ifndef SKIP_TASK_STATISTICS
CLI_COMMAND_DEF("tasks", "show task stats", NULL, cliTasks),
#endif
CLI_COMMAND_DEF("version", "show version", NULL, cliVersion),
#ifdef BEEPER
CLI_COMMAND_DEF("beeper", "turn on/off beeper", "list\r\n"
"\t<+|->[name]", cliBeeper),
#endif
#ifdef VTX
CLI_COMMAND_DEF("vtx", "vtx channels on switch", NULL, cliVtx),
#endif
CLI_COMMAND_DEF("name", "Name of craft", NULL, cliName),
};
#define CMD_COUNT (sizeof(cmdTable) / sizeof(clicmd_t))
@ -3927,6 +3947,8 @@ void cliEnter(serialPort_t *serialPort)
setPrintfSerialPort(cliPort);
cliWriter = bufWriterInit(cliWriteBuffer, sizeof(cliWriteBuffer), (bufWrite_t)serialWriteBufShim, serialPort);
schedulerSetCalulateTaskStatistics(masterConfig.task_statistics);
#ifndef CLI_MINIMAL_VERBOSITY
cliPrint("\r\nEntering CLI Mode, type 'exit' to return, or 'help'\r\n");
#else

View file

@ -247,11 +247,19 @@ uint8_t getMotorCount()
bool isMotorProtocolDshot(void) {
#ifdef USE_DSHOT
if (motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT150 || motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT300 || motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT600)
switch(motorConfig->motorPwmProtocol) {
case PWM_TYPE_DSHOT1200:
case PWM_TYPE_DSHOT900:
case PWM_TYPE_DSHOT600:
case PWM_TYPE_DSHOT300:
case PWM_TYPE_DSHOT150:
return true;
else
default:
return false;
}
#else
return false;
#endif
return false;
}
// Add here scaled ESC outputs for digital protol

View file

@ -157,7 +157,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) {
maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 1000 * dT;
}
float calcHorizonLevelStrength(void) {
static float calcHorizonLevelStrength(void) {
float horizonLevelStrength = 0.0f;
if (horizonTransition > 0.0f) {
const float mostDeflectedPos = MAX(getRcDeflectionAbs(FD_ROLL), getRcDeflectionAbs(FD_PITCH));
@ -167,7 +167,7 @@ float calcHorizonLevelStrength(void) {
return horizonLevelStrength;
}
float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint) {
static float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint) {
// calculate error angle and limit the angle to the max inclination
float errorAngle = pidProfile->levelSensitivity * getRcDeflection(axis);
#ifdef GPS
@ -187,7 +187,7 @@ float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims
return currentPidSetpoint;
}
float accelerationLimit(int axis, float currentPidSetpoint) {
static float accelerationLimit(int axis, float currentPidSetpoint) {
static float previousSetpoint[3];
const float currentVelocity = currentPidSetpoint- previousSetpoint[axis];
@ -200,14 +200,12 @@ float accelerationLimit(int axis, float currentPidSetpoint) {
// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
// Based on 2DOF reference design (matlab)
void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim)
void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float tpaFactor)
{
static float previousRateError[2];
static float previousSetpoint[3];
// ----------PID controller----------
const float tpaFactor = getThrottlePIDAttenuation();
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
float currentPidSetpoint = getSetpointRate(axis);

View file

@ -20,15 +20,15 @@
#include <stdbool.h>
#define PID_CONTROLLER_BETAFLIGHT 1
#define PID_MIXER_SCALING 900.0f
#define PID_MIXER_SCALING 100.0f
#define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter
#define YAW_P_LIMIT_MAX 500 // Maximum value for yaw P limiter
#define PIDSUM_LIMIT 0.5f
// Scaling factors for Pids for better tunable range in configurator for betaflight pid controller. The scaling is based on legacy pid controller or previous float
#define PTERM_SCALE 0.032029f
#define ITERM_SCALE 0.244381f
#define DTERM_SCALE 0.000529f
#define PTERM_SCALE 0.003558774f
#define ITERM_SCALE 0.027153417f
#define DTERM_SCALE 0.000058778f
typedef enum {
PIDROLL,
@ -88,7 +88,7 @@ typedef struct pidConfig_s {
} pidConfig_t;
union rollAndPitchTrims_u;
void pidController(const pidProfile_t *pidProfile, const union rollAndPitchTrims_u *angleTrim);
void pidController(const pidProfile_t *pidProfile, const union rollAndPitchTrims_u *angleTrim, float tpaFactor);
extern float axisPIDf[3];
extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];

View file

@ -27,18 +27,18 @@ typedef enum {
typedef enum {
FUNCTION_NONE = 0,
FUNCTION_MSP = (1 << 0), // 1
FUNCTION_GPS = (1 << 1), // 2
FUNCTION_TELEMETRY_FRSKY = (1 << 2), // 4
FUNCTION_TELEMETRY_HOTT = (1 << 3), // 8
FUNCTION_TELEMETRY_LTM = (1 << 4), // 16
FUNCTION_TELEMETRY_SMARTPORT = (1 << 5), // 32
FUNCTION_RX_SERIAL = (1 << 6), // 64
FUNCTION_BLACKBOX = (1 << 7), // 128
FUNCTION_TELEMETRY_MAVLINK = (1 << 9), // 512
FUNCTION_ESC_SENSOR = (1 << 10),// 1024
FUNCTION_VTX_CONTROL = (1 << 11),// 2048
FUNCTION_MSP = (1 << 0), // 1
FUNCTION_GPS = (1 << 1), // 2
FUNCTION_TELEMETRY_FRSKY = (1 << 2), // 4
FUNCTION_TELEMETRY_HOTT = (1 << 3), // 8
FUNCTION_TELEMETRY_LTM = (1 << 4), // 16
FUNCTION_TELEMETRY_SMARTPORT = (1 << 5), // 32
FUNCTION_RX_SERIAL = (1 << 6), // 64
FUNCTION_BLACKBOX = (1 << 7), // 128
FUNCTION_TELEMETRY_MAVLINK = (1 << 9), // 512
FUNCTION_ESC_SENSOR = (1 << 10), // 1024
FUNCTION_VTX_CONTROL = (1 << 11), // 2048
FUNCTION_TELEMETRY_IBUS = (1 << 12) // 4096
} serialPortFunction_e;
typedef enum {
@ -96,8 +96,8 @@ serialPort_t *findNextSharedSerialPort(uint16_t functionMask, serialPortFunction
// configuration
//
typedef struct serialPortConfig_s {
serialPortIdentifier_e identifier;
uint16_t functionMask;
serialPortIdentifier_e identifier;
uint8_t msp_baudrateIndex;
uint8_t gps_baudrateIndex;
uint8_t blackbox_baudrateIndex;
@ -105,8 +105,9 @@ typedef struct serialPortConfig_s {
} serialPortConfig_t;
typedef struct serialConfig_s {
uint8_t reboot_character; // which byte is used to reboot. Default 'R', could be changed carefully to something else.
serialPortConfig_t portConfigs[SERIAL_PORT_COUNT];
uint16_t serial_update_rate_hz;
uint8_t reboot_character; // which byte is used to reboot. Default 'R', could be changed carefully to something else.
} serialConfig_t;
typedef void serialConsumer(uint8_t);

View file

@ -59,7 +59,7 @@
#define MSP_PROTOCOL_VERSION 0
#define API_VERSION_MAJOR 1 // increment when major changes are made
#define API_VERSION_MINOR 24 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR
#define API_VERSION_MINOR 26 // increment after a release, to set the version for all changes to go into the following release (if no changes to MSP are made between the releases, this can be reverted before the release)
#define API_VERSION_LENGTH 2

View file

@ -17,7 +17,7 @@
#pragma once
#if defined(STM32F745xx) || defined(STM32F746xx)
#if defined(STM32F745xx) || defined(STM32F746xx) || defined(STM32F722xx)
#include "stm32f7xx.h"
#include "stm32f7xx_hal.h"
@ -27,9 +27,9 @@
#define U_ID_2 (*(uint32_t*)0x1ff0f428)
#define STM32F7
#endif
#if defined(STM32F40_41xxx) || defined (STM32F411xE)
#elif defined(STM32F40_41xxx) || defined (STM32F411xE)
#include "stm32f4xx_conf.h"
#include "stm32f4xx_rcc.h"
#include "stm32f4xx_gpio.h"
@ -41,9 +41,8 @@
#define U_ID_2 (*(uint32_t*)0x1fff7a18)
#define STM32F4
#endif
#ifdef STM32F303xC
#elif defined(STM32F303xC)
#include "stm32f30x_conf.h"
#include "stm32f30x_rcc.h"
#include "stm32f30x_gpio.h"
@ -55,9 +54,8 @@
#define U_ID_2 (*(uint32_t*)0x1FFFF7B4)
#define STM32F3
#endif
#ifdef STM32F10X
#elif defined(STM32F10X)
#include "stm32f10x_conf.h"
#include "stm32f10x_gpio.h"
@ -69,7 +67,10 @@
#define U_ID_2 (*(uint32_t*)0x1FFFF7F0)
#define STM32F1
#endif // STM32F10X
#else // STM32F10X
#error "Invalid chipset specified. Update platform.h"
#endif
#include "target/common.h"
#include "target.h"

View file

@ -126,6 +126,7 @@ typedef struct rxConfig_s {
uint16_t mincheck; // minimum rc end
uint16_t maxcheck; // maximum rc end
uint8_t rcInterpolation;
uint8_t rcInterpolationChannels;
uint8_t rcInterpolationInterval;
uint8_t fpvCamAngleDegrees; // Camera angle to be scaled into rc commands
uint8_t max_aux_channel;

View file

@ -44,6 +44,7 @@ static cfTask_t *currentTask = NULL;
static uint32_t totalWaitingTasks;
static uint32_t totalWaitingTasksSamples;
static bool calculateTaskStatistics;
uint16_t averageSystemLoadPercent = 0;
@ -156,8 +157,11 @@ void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t * taskInfo)
void rescheduleTask(cfTaskId_e taskId, uint32_t newPeriodMicros)
{
if (taskId == TASK_SELF || taskId < TASK_COUNT) {
cfTask_t *task = taskId == TASK_SELF ? currentTask : &cfTasks[taskId];
if (taskId == TASK_SELF) {
cfTask_t *task = currentTask;
task->desiredPeriod = MAX(SCHEDULER_DELAY_LIMIT, newPeriodMicros); // Limit delay to 100us (10 kHz) to prevent scheduler clogging
} else if (taskId < TASK_COUNT) {
cfTask_t *task = &cfTasks[taskId];
task->desiredPeriod = MAX(SCHEDULER_DELAY_LIMIT, newPeriodMicros); // Limit delay to 100us (10 kHz) to prevent scheduler clogging
}
}
@ -185,8 +189,31 @@ uint32_t getTaskDeltaTime(cfTaskId_e taskId)
}
}
void schedulerSetCalulateTaskStatistics(bool calculateTaskStatisticsToUse)
{
calculateTaskStatistics = calculateTaskStatisticsToUse;
}
void schedulerResetTaskStatistics(cfTaskId_e taskId)
{
#ifdef SKIP_TASK_STATISTICS
UNUSED(taskId);
#else
if (taskId == TASK_SELF) {
currentTask->movingSumExecutionTime = 0;
currentTask->totalExecutionTime = 0;
currentTask->maxExecutionTime = 0;
} else if (taskId < TASK_COUNT) {
cfTasks[taskId].movingSumExecutionTime = 0;
cfTasks[taskId].totalExecutionTime = 0;
cfTasks[taskId].totalExecutionTime = 0;
}
#endif
}
void schedulerInit(void)
{
calculateTaskStatistics = true;
queueClear();
queueAdd(&cfTasks[TASK_SYSTEM]);
}
@ -217,24 +244,28 @@ void scheduler(void)
uint16_t waitingTasks = 0;
for (cfTask_t *task = queueFirst(); task != NULL; task = queueNext()) {
// Task has checkFunc - event driven
if (task->checkFunc != NULL) {
if (task->checkFunc) {
#if defined(SCHEDULER_DEBUG)
const timeUs_t currentTimeBeforeCheckFuncCall = micros();
#else
const timeUs_t currentTimeBeforeCheckFuncCall = currentTimeUs;
#endif
// Increase priority for event driven tasks
if (task->dynamicPriority > 0) {
task->taskAgeCycles = 1 + ((currentTimeUs - task->lastSignaledAt) / task->desiredPeriod);
task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles;
waitingTasks++;
} else if (task->checkFunc(currentTimeBeforeCheckFuncCall, currentTimeBeforeCheckFuncCall - task->lastExecutedAt)) {
#if defined(SCHEDULER_DEBUG) || !defined(SKIP_TASK_STATISTICS)
const uint32_t checkFuncExecutionTime = micros() - currentTimeBeforeCheckFuncCall;
#endif
#if defined(SCHEDULER_DEBUG)
DEBUG_SET(DEBUG_SCHEDULER, 3, checkFuncExecutionTime);
DEBUG_SET(DEBUG_SCHEDULER, 3, micros() - currentTimeBeforeCheckFuncCall);
#endif
#ifndef SKIP_TASK_STATISTICS
checkFuncMovingSumExecutionTime += checkFuncExecutionTime - checkFuncMovingSumExecutionTime / MOVING_SUM_COUNT;
checkFuncTotalExecutionTime += checkFuncExecutionTime; // time consumed by scheduler + task
checkFuncMaxExecutionTime = MAX(checkFuncMaxExecutionTime, checkFuncExecutionTime);
if (calculateTaskStatistics) {
const uint32_t checkFuncExecutionTime = micros() - currentTimeBeforeCheckFuncCall;
checkFuncMovingSumExecutionTime += checkFuncExecutionTime - checkFuncMovingSumExecutionTime / MOVING_SUM_COUNT;
checkFuncTotalExecutionTime += checkFuncExecutionTime; // time consumed by scheduler + task
checkFuncMaxExecutionTime = MAX(checkFuncMaxExecutionTime, checkFuncExecutionTime);
}
#endif
task->lastSignaledAt = currentTimeBeforeCheckFuncCall;
task->taskAgeCycles = 1;
@ -270,21 +301,27 @@ void scheduler(void)
currentTask = selectedTask;
if (selectedTask != NULL) {
if (selectedTask) {
// Found a task that should be run
selectedTask->taskLatestDeltaTime = currentTimeUs - selectedTask->lastExecutedAt;
selectedTask->lastExecutedAt = currentTimeUs;
selectedTask->dynamicPriority = 0;
// Execute task
const timeUs_t currentTimeBeforeTaskCall = micros();
selectedTask->taskFunc(currentTimeBeforeTaskCall);
#ifdef SKIP_TASK_STATISTICS
selectedTask->taskFunc(currentTimeUs);
#else
if (calculateTaskStatistics) {
const timeUs_t currentTimeBeforeTaskCall = micros();
selectedTask->taskFunc(currentTimeBeforeTaskCall);
const timeUs_t taskExecutionTime = micros() - currentTimeBeforeTaskCall;
selectedTask->movingSumExecutionTime += taskExecutionTime - selectedTask->movingSumExecutionTime / MOVING_SUM_COUNT;
selectedTask->totalExecutionTime += taskExecutionTime; // time consumed by scheduler + task
selectedTask->maxExecutionTime = MAX(selectedTask->maxExecutionTime, taskExecutionTime);
} else {
selectedTask->taskFunc(currentTimeUs);
}
#ifndef SKIP_TASK_STATISTICS
const timeUs_t taskExecutionTime = micros() - currentTimeBeforeTaskCall;
selectedTask->movingSumExecutionTime += taskExecutionTime - selectedTask->movingSumExecutionTime / MOVING_SUM_COUNT;
selectedTask->totalExecutionTime += taskExecutionTime; // time consumed by scheduler + task
selectedTask->maxExecutionTime = MAX(selectedTask->maxExecutionTime, taskExecutionTime);
#endif
#if defined(SCHEDULER_DEBUG)
DEBUG_SET(DEBUG_SCHEDULER, 2, micros() - currentTimeUs - taskExecutionTime); // time spent in scheduler

View file

@ -23,6 +23,7 @@ typedef enum {
TASK_PRIORITY_IDLE = 0, // Disables dynamic scheduling, task is executed only if no other task is active this cycle
TASK_PRIORITY_LOW = 1,
TASK_PRIORITY_MEDIUM = 3,
TASK_PRIORITY_MEDIUM_HIGH = 4,
TASK_PRIORITY_HIGH = 5,
TASK_PRIORITY_REALTIME = 6,
TASK_PRIORITY_MAX = 255
@ -145,6 +146,8 @@ void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t *taskInfo);
void rescheduleTask(cfTaskId_e taskId, uint32_t newPeriodMicros);
void setTaskEnabled(cfTaskId_e taskId, bool newEnabledState);
uint32_t getTaskDeltaTime(cfTaskId_e taskId);
void schedulerSetCalulateTaskStatistics(bool calculateTaskStatistics);
void schedulerResetTaskStatistics(cfTaskId_e taskId);
void schedulerInit(void);
void scheduler(void);

View file

@ -94,7 +94,7 @@ typedef enum {
#define ESC_BOOTTIME 5000 // 5 seconds
#define ESC_REQUEST_TIMEOUT 100 // 100 ms (data transfer takes only 900us)
static bool tlmFrameDone = false;
static volatile bool tlmFramePending = false;
static uint8_t tlm[ESC_SENSOR_BUFFSIZE] = { 0, };
static uint8_t tlmFramePosition = 0;
@ -159,15 +159,16 @@ static void escSensorDataReceive(uint16_t c)
// KISS ESC sends some data during startup, ignore this for now (maybe future use)
// startup data could be firmware version and serialnumber
if (escSensorTriggerState == ESC_SENSOR_TRIGGER_STARTUP || tlmFrameDone) {
if (!tlmFramePending) {
return;
}
tlm[tlmFramePosition] = (uint8_t)c;
if (tlmFramePosition == ESC_SENSOR_BUFFSIZE - 1) {
tlmFrameDone = true;
tlmFramePosition = 0;
tlmFramePending = false;
} else {
tlmFramePosition++;
}
@ -213,7 +214,7 @@ static uint8_t get_crc8(uint8_t *Buf, uint8_t BufLen)
static uint8_t decodeEscFrame(void)
{
if (!tlmFrameDone) {
if (tlmFramePending) {
return ESC_SENSOR_FRAME_PENDING;
}
@ -236,8 +237,6 @@ static uint8_t decodeEscFrame(void)
frameStatus = ESC_SENSOR_FRAME_FAILED;
}
tlmFrameDone = false;
return frameStatus;
}
@ -277,6 +276,7 @@ void escSensorProcess(timeUs_t currentTimeUs)
case ESC_SENSOR_TRIGGER_READY:
escTriggerTimestamp = currentTimeMs;
tlmFramePending = true;
motorDmaOutput_t * const motor = getMotorDmaOutput(escSensorMotor);
motor->requestTelemetry = true;
escSensorTriggerState = ESC_SENSOR_TRIGGER_PENDING;

View file

@ -54,6 +54,8 @@
#include "io/beeper.h"
#include "io/statusindicator.h"
#include "scheduler/scheduler.h"
#include "sensors/sensors.h"
#include "sensors/boardalignment.h"
#include "sensors/gyro.h"
@ -240,7 +242,8 @@ bool gyroInit(const gyroConfig_t *gyroConfigToUse)
if (!gyroDetect(&gyro.dev)) {
return false;
}
gyro.targetLooptime = gyroSetSampleRate(gyroConfig->gyro_lpf, gyroConfig->gyro_sync_denom); // Set gyro sample rate before initialisation
// Must set gyro sample rate before initialisation
gyro.targetLooptime = gyroSetSampleRate(&gyro.dev, gyroConfig->gyro_lpf, gyroConfig->gyro_sync_denom, gyroConfig->gyro_use_32khz);
gyro.dev.lpf = gyroConfig->gyro_lpf;
gyro.dev.init(&gyro.dev);
gyroInitFilters();
@ -358,19 +361,55 @@ static void performGyroCalibration(uint8_t gyroMovementCalibrationThreshold)
}
if (isOnFinalGyroCalibrationCycle()) {
schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics
beeper(BEEPER_GYRO_CALIBRATED);
}
calibratingG--;
}
#if defined(GYRO_USES_SPI) && defined(USE_MPU_DATA_READY_SIGNAL)
static bool gyroUpdateISR(gyroDev_t* gyroDev)
{
if (!gyroDev->dataReady || !gyroDev->read(gyroDev)) {
return false;
}
#ifdef DEBUG_MPU_DATA_READY_INTERRUPT
debug[2] = (uint16_t)(micros() & 0xffff);
#endif
gyroDev->dataReady = false;
// move gyro data into 32-bit variables to avoid overflows in calculations
gyroADC[X] = gyroDev->gyroADCRaw[X];
gyroADC[Y] = gyroDev->gyroADCRaw[Y];
gyroADC[Z] = gyroDev->gyroADCRaw[Z];
alignSensors(gyroADC, gyroDev->gyroAlign);
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroADC[axis] -= gyroZero[axis];
// scale gyro output to degrees per second
float gyroADCf = (float)gyroADC[axis] * gyroDev->scale;
gyroADCf = softLpfFilterApplyFn(softLpfFilter[axis], gyroADCf);
gyroADCf = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf);
gyroADCf = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf);
gyro.gyroADCf[axis] = gyroADCf;
}
return true;
}
#endif
void gyroUpdate(void)
{
// range: +/- 8192; +/- 2000 deg/sec
if (gyro.dev.update) {
// if the gyro update function is set then return, since the gyro is read in gyroUpdateISR
return;
}
if (!gyro.dev.read(&gyro.dev)) {
return;
}
gyro.dev.dataReady = false;
// move gyro data into 32-bit variables to avoid overflows in calculations
gyroADC[X] = gyro.dev.gyroADCRaw[X];
gyroADC[Y] = gyro.dev.gyroADCRaw[Y];
gyroADC[Z] = gyro.dev.gyroADCRaw[Z];
@ -378,27 +417,40 @@ void gyroUpdate(void)
alignSensors(gyroADC, gyro.dev.gyroAlign);
const bool calibrationComplete = isGyroCalibrationComplete();
if (!calibrationComplete) {
if (calibrationComplete) {
#if defined(GYRO_USES_SPI) && defined(USE_MPU_DATA_READY_SIGNAL)
// SPI-based gyro so can read and update in ISR
if (gyroConfig->gyro_isr_update) {
mpuGyroSetIsrUpdate(&gyro.dev, gyroUpdateISR);
return;
}
#endif
#ifdef DEBUG_MPU_DATA_READY_INTERRUPT
debug[3] = (uint16_t)(micros() & 0xffff);
#endif
} else {
performGyroCalibration(gyroConfig->gyroMovementCalibrationThreshold);
}
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroADC[axis] -= gyroZero[axis];
// scale gyro output to degrees per second
gyro.gyroADCf[axis] = (float)gyroADC[axis] * gyro.dev.scale;
float gyroADCf = (float)gyroADC[axis] * gyro.dev.scale;
DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyro.gyroADCf[axis]));
// Apply LPF
DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf));
gyroADCf = softLpfFilterApplyFn(softLpfFilter[axis], gyroADCf);
gyro.gyroADCf[axis] = softLpfFilterApplyFn(softLpfFilter[axis], gyro.gyroADCf[axis]);
// Apply Notch filtering
DEBUG_SET(DEBUG_NOTCH, axis, lrintf(gyroADCf));
gyroADCf = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf);
gyroADCf = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf);
gyro.gyroADCf[axis] = gyroADCf;
}
DEBUG_SET(DEBUG_NOTCH, axis, lrintf(gyro.gyroADCf[axis]));
gyro.gyroADCf[axis] = notchFilter1ApplyFn(notchFilter1[axis], gyro.gyroADCf[axis]);
gyro.gyroADCf[axis] = notchFilter2ApplyFn(notchFilter2[axis], gyro.gyroADCf[axis]);
if (!calibrationComplete) {
gyroADC[axis] = lrintf(gyro.gyroADCf[axis] / gyro.dev.scale);
}
if (!calibrationComplete) {
gyroADC[X] = lrintf(gyro.gyroADCf[X] / gyro.dev.scale);
gyroADC[Y] = lrintf(gyro.gyroADCf[Y] / gyro.dev.scale);
gyroADC[Z] = lrintf(gyro.gyroADCf[Z] / gyro.dev.scale);
}
}

View file

@ -51,6 +51,8 @@ typedef struct gyroConfig_s {
uint8_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
uint8_t gyro_soft_lpf_type;
uint8_t gyro_soft_lpf_hz;
bool gyro_isr_update;
bool gyro_use_32khz;
uint16_t gyro_soft_notch_hz_1;
uint16_t gyro_soft_notch_cutoff_1;
uint16_t gyro_soft_notch_hz_2;

View file

@ -25,6 +25,7 @@ typedef enum {
SENSOR_INDEX_COUNT
} sensorIndex_e;
extern int16_t telemTemperature1; //FIXME move to temp sensor...?
extern uint8_t detectedSensors[SENSOR_INDEX_COUNT];
typedef struct int16_flightDynamicsTrims_s {

View file

@ -0,0 +1,551 @@
/**
******************************************************************************
* @file startup_stm32f722xx.s
* @author MCD Application Team
* @version nil - pending release
* @date
* @brief STM32F722xx Devices vector table for GCC toolchain based application.
* This module performs:
* - Set the initial SP
* - Set the initial PC == Reset_Handler,
* - Set the vector table entries with the exceptions ISR address
* - Branches to main in the C library (which eventually
* calls main()).
* After Reset the Cortex-M7 processor is in Thread mode,
* priority is Privileged, and the Stack is set to Main.
******************************************************************************
* @attention
*
* <h2><center>&copy; COPYRIGHT 2016 STMicroelectronics</center></h2>
*
* 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 of STMicroelectronics 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 HOLDER 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.
*
******************************************************************************
*/
.syntax unified
.cpu cortex-m7
.fpu softvfp
.thumb
.global g_pfnVectors
.global Default_Handler
/* start address for the initialization values of the .data section.
defined in linker script */
.word _sidata
/* start address for the .data section. defined in linker script */
.word _sdata
/* end address for the .data section. defined in linker script */
.word _edata
/* start address for the .bss section. defined in linker script */
.word _sbss
/* end address for the .bss section. defined in linker script */
.word _ebss
/* stack used for SystemInit_ExtMemCtl; always internal RAM used */
/**
* @brief This is the code that gets called when the processor first
* starts execution following a reset event. Only the absolutely
* necessary set is performed, after which the application
* supplied main() routine is called.
* @param None
* @retval : None
*/
.section .text.Reset_Handler
.weak Reset_Handler
.type Reset_Handler, %function
Reset_Handler:
/* Copy the data segment initializers from flash to SRAM */
movs r1, #0
b LoopCopyDataInit
CopyDataInit:
ldr r3, =_sidata
ldr r3, [r3, r1]
str r3, [r0, r1]
adds r1, r1, #4
LoopCopyDataInit:
ldr r0, =_sdata
ldr r3, =_edata
adds r2, r0, r1
cmp r2, r3
bcc CopyDataInit
ldr r2, =_sbss
b LoopFillZerobss
/* Zero fill the bss segment. */
FillZerobss:
movs r3, #0
str r3, [r2], #4
LoopFillZerobss:
ldr r3, = _ebss
cmp r2, r3
bcc FillZerobss
/*FPU settings*/
ldr r0, =0xE000ED88 /* Enable CP10,CP11 */
ldr r1,[r0]
orr r1,r1,#(0xF << 20)
str r1,[r0]
/* Call the clock system initialization function.*/
bl SystemInit
/* Call the application's entry point.*/
bl main
bx lr
LoopForever:
b LoopForever
.size Reset_Handler, .-Reset_Handler
/**
* @brief This is the code that gets called when the processor receives an
* unexpected interrupt. This simply enters an infinite loop, preserving
* the system state for examination by a debugger.
* @param None
* @retval None
*/
.section .text.Default_Handler,"ax",%progbits
Default_Handler:
Infinite_Loop:
b Infinite_Loop
.size Default_Handler, .-Default_Handler
/******************************************************************************
*
* The minimal vector table for a Cortex M7. Note that the proper constructs
* must be placed on this to ensure that it ends up at physical address
* 0x0000.0000.
*
*******************************************************************************/
.section .isr_vector,"a",%progbits
.type g_pfnVectors, %object
.size g_pfnVectors, .-g_pfnVectors
g_pfnVectors:
.word _estack
.word Reset_Handler
.word NMI_Handler
.word HardFault_Handler
.word MemManage_Handler
.word BusFault_Handler
.word UsageFault_Handler
.word 0
.word 0
.word 0
.word 0
.word SVC_Handler
.word DebugMon_Handler
.word 0
.word PendSV_Handler
.word SysTick_Handler
/* External Interrupts */
.word WWDG_IRQHandler /* Window WatchDog */
.word PVD_IRQHandler /* PVD through EXTI Line detection */
.word TAMP_STAMP_IRQHandler /* Tamper and TimeStamps through the EXTI line */
.word RTC_WKUP_IRQHandler /* RTC Wakeup through the EXTI line */
.word FLASH_IRQHandler /* FLASH */
.word RCC_IRQHandler /* RCC */
.word EXTI0_IRQHandler /* EXTI Line0 */
.word EXTI1_IRQHandler /* EXTI Line1 */
.word EXTI2_IRQHandler /* EXTI Line2 */
.word EXTI3_IRQHandler /* EXTI Line3 */
.word EXTI4_IRQHandler /* EXTI Line4 */
.word DMA1_Stream0_IRQHandler /* DMA1 Stream 0 */
.word DMA1_Stream1_IRQHandler /* DMA1 Stream 1 */
.word DMA1_Stream2_IRQHandler /* DMA1 Stream 2 */
.word DMA1_Stream3_IRQHandler /* DMA1 Stream 3 */
.word DMA1_Stream4_IRQHandler /* DMA1 Stream 4 */
.word DMA1_Stream5_IRQHandler /* DMA1 Stream 5 */
.word DMA1_Stream6_IRQHandler /* DMA1 Stream 6 */
.word ADC_IRQHandler /* ADC1, ADC2 and ADC3s */
.word CAN1_TX_IRQHandler /* CAN1 TX */
.word CAN1_RX0_IRQHandler /* CAN1 RX0 */
.word CAN1_RX1_IRQHandler /* CAN1 RX1 */
.word CAN1_SCE_IRQHandler /* CAN1 SCE */
.word EXTI9_5_IRQHandler /* External Line[9:5]s */
.word TIM1_BRK_TIM9_IRQHandler /* TIM1 Break and TIM9 */
.word TIM1_UP_TIM10_IRQHandler /* TIM1 Update and TIM10 */
.word TIM1_TRG_COM_TIM11_IRQHandler /* TIM1 Trigger and Commutation and TIM11 */
.word TIM1_CC_IRQHandler /* TIM1 Capture Compare */
.word TIM2_IRQHandler /* TIM2 */
.word TIM3_IRQHandler /* TIM3 */
.word TIM4_IRQHandler /* TIM4 */
.word I2C1_EV_IRQHandler /* I2C1 Event */
.word I2C1_ER_IRQHandler /* I2C1 Error */
.word I2C2_EV_IRQHandler /* I2C2 Event */
.word I2C2_ER_IRQHandler /* I2C2 Error */
.word SPI1_IRQHandler /* SPI1 */
.word SPI2_IRQHandler /* SPI2 */
.word USART1_IRQHandler /* USART1 */
.word USART2_IRQHandler /* USART2 */
.word USART3_IRQHandler /* USART3 */
.word EXTI15_10_IRQHandler /* External Line[15:10]s */
.word RTC_Alarm_IRQHandler /* RTC Alarm (A and B) through EXTI Line */
.word OTG_FS_WKUP_IRQHandler /* USB OTG FS Wakeup through EXTI line */
.word TIM8_BRK_TIM12_IRQHandler /* TIM8 Break and TIM12 */
.word TIM8_UP_TIM13_IRQHandler /* TIM8 Update and TIM13 */
.word TIM8_TRG_COM_TIM14_IRQHandler /* TIM8 Trigger and Commutation and TIM14 */
.word TIM8_CC_IRQHandler /* TIM8 Capture Compare */
.word DMA1_Stream7_IRQHandler /* DMA1 Stream7 */
.word FMC_IRQHandler /* FMC */
.word SDMMC1_IRQHandler /* SDMMC1 */
.word TIM5_IRQHandler /* TIM5 */
.word SPI3_IRQHandler /* SPI3 */
.word UART4_IRQHandler /* UART4 */
.word UART5_IRQHandler /* UART5 */
.word TIM6_DAC_IRQHandler /* TIM6 and DAC1&2 underrun errors */
.word TIM7_IRQHandler /* TIM7 */
.word DMA2_Stream0_IRQHandler /* DMA2 Stream 0 */
.word DMA2_Stream1_IRQHandler /* DMA2 Stream 1 */
.word DMA2_Stream2_IRQHandler /* DMA2 Stream 2 */
.word DMA2_Stream3_IRQHandler /* DMA2 Stream 3 */
.word DMA2_Stream4_IRQHandler /* DMA2 Stream 4 */
.word ETH_IRQHandler /* Ethernet */
.word ETH_WKUP_IRQHandler /* Ethernet Wakeup through EXTI line */
.word 0 /* CAN2 TX */
.word 0 /* CAN2 RX0 */
.word 0 /* CAN2 RX1 */
.word 0 /* CAN2 SCE */
.word OTG_FS_IRQHandler /* USB OTG FS */
.word DMA2_Stream5_IRQHandler /* DMA2 Stream 5 */
.word DMA2_Stream6_IRQHandler /* DMA2 Stream 6 */
.word DMA2_Stream7_IRQHandler /* DMA2 Stream 7 */
.word USART6_IRQHandler /* USART6 */
.word I2C3_EV_IRQHandler /* I2C3 event */
.word I2C3_ER_IRQHandler /* I2C3 error */
.word OTG_HS_EP1_OUT_IRQHandler /* USB OTG HS End Point 1 Out */
.word OTG_HS_EP1_IN_IRQHandler /* USB OTG HS End Point 1 In */
.word OTG_HS_WKUP_IRQHandler /* USB OTG HS Wakeup through EXTI */
.word OTG_HS_IRQHandler /* USB OTG HS */
.word DCMI_IRQHandler /* DCMI */
.word 0 /* Reserved */
.word RNG_IRQHandler /* Rng */
.word FPU_IRQHandler /* FPU */
.word 0 /* UART7 */
.word 0 /* UART8 */
.word 0 /* SPI4 */
.word 0 /* SPI5 */
.word 0 /* SPI6 */
.word SAI1_IRQHandler /* SAI1 */
.word 0 /* Reserved */
.word 0 /* Reserved */
.word DMA2D_IRQHandler /* DMA2D */
.word SAI2_IRQHandler /* SAI2 */
.word QUADSPI_IRQHandler /* QUADSPI */
.word 0 /* LPTIM1 */
.word CEC_IRQHandler /* HDMI_CEC */
.word 0 /* I2C4 Event */
.word 0 /* I2C4 Error */
.word SPDIF_RX_IRQHandler /* SPDIF_RX */
/*******************************************************************************
*
* Provide weak aliases for each Exception handler to the Default_Handler.
* As they are weak aliases, any function with the same name will override
* this definition.
*
*******************************************************************************/
.weak NMI_Handler
.thumb_set NMI_Handler,Default_Handler
.weak HardFault_Handler
.thumb_set HardFault_Handler,Default_Handler
.weak MemManage_Handler
.thumb_set MemManage_Handler,Default_Handler
.weak BusFault_Handler
.thumb_set BusFault_Handler,Default_Handler
.weak UsageFault_Handler
.thumb_set UsageFault_Handler,Default_Handler
.weak SVC_Handler
.thumb_set SVC_Handler,Default_Handler
.weak DebugMon_Handler
.thumb_set DebugMon_Handler,Default_Handler
.weak PendSV_Handler
.thumb_set PendSV_Handler,Default_Handler
.weak SysTick_Handler
.thumb_set SysTick_Handler,Default_Handler
.weak WWDG_IRQHandler
.thumb_set WWDG_IRQHandler,Default_Handler
.weak PVD_IRQHandler
.thumb_set PVD_IRQHandler,Default_Handler
.weak TAMP_STAMP_IRQHandler
.thumb_set TAMP_STAMP_IRQHandler,Default_Handler
.weak RTC_WKUP_IRQHandler
.thumb_set RTC_WKUP_IRQHandler,Default_Handler
.weak FLASH_IRQHandler
.thumb_set FLASH_IRQHandler,Default_Handler
.weak RCC_IRQHandler
.thumb_set RCC_IRQHandler,Default_Handler
.weak EXTI0_IRQHandler
.thumb_set EXTI0_IRQHandler,Default_Handler
.weak EXTI1_IRQHandler
.thumb_set EXTI1_IRQHandler,Default_Handler
.weak EXTI2_IRQHandler
.thumb_set EXTI2_IRQHandler,Default_Handler
.weak EXTI3_IRQHandler
.thumb_set EXTI3_IRQHandler,Default_Handler
.weak EXTI4_IRQHandler
.thumb_set EXTI4_IRQHandler,Default_Handler
.weak DMA1_Stream0_IRQHandler
.thumb_set DMA1_Stream0_IRQHandler,Default_Handler
.weak DMA1_Stream1_IRQHandler
.thumb_set DMA1_Stream1_IRQHandler,Default_Handler
.weak DMA1_Stream2_IRQHandler
.thumb_set DMA1_Stream2_IRQHandler,Default_Handler
.weak DMA1_Stream3_IRQHandler
.thumb_set DMA1_Stream3_IRQHandler,Default_Handler
.weak DMA1_Stream4_IRQHandler
.thumb_set DMA1_Stream4_IRQHandler,Default_Handler
.weak DMA1_Stream5_IRQHandler
.thumb_set DMA1_Stream5_IRQHandler,Default_Handler
.weak DMA1_Stream6_IRQHandler
.thumb_set DMA1_Stream6_IRQHandler,Default_Handler
.weak ADC_IRQHandler
.thumb_set ADC_IRQHandler,Default_Handler
.weak CAN1_TX_IRQHandler
.thumb_set CAN1_TX_IRQHandler,Default_Handler
.weak CAN1_RX0_IRQHandler
.thumb_set CAN1_RX0_IRQHandler,Default_Handler
.weak CAN1_RX1_IRQHandler
.thumb_set CAN1_RX1_IRQHandler,Default_Handler
.weak CAN1_SCE_IRQHandler
.thumb_set CAN1_SCE_IRQHandler,Default_Handler
.weak EXTI9_5_IRQHandler
.thumb_set EXTI9_5_IRQHandler,Default_Handler
.weak TIM1_BRK_TIM9_IRQHandler
.thumb_set TIM1_BRK_TIM9_IRQHandler,Default_Handler
.weak TIM1_UP_TIM10_IRQHandler
.thumb_set TIM1_UP_TIM10_IRQHandler,Default_Handler
.weak TIM1_TRG_COM_TIM11_IRQHandler
.thumb_set TIM1_TRG_COM_TIM11_IRQHandler,Default_Handler
.weak TIM1_CC_IRQHandler
.thumb_set TIM1_CC_IRQHandler,Default_Handler
.weak TIM2_IRQHandler
.thumb_set TIM2_IRQHandler,Default_Handler
.weak TIM3_IRQHandler
.thumb_set TIM3_IRQHandler,Default_Handler
.weak TIM4_IRQHandler
.thumb_set TIM4_IRQHandler,Default_Handler
.weak I2C1_EV_IRQHandler
.thumb_set I2C1_EV_IRQHandler,Default_Handler
.weak I2C1_ER_IRQHandler
.thumb_set I2C1_ER_IRQHandler,Default_Handler
.weak I2C2_EV_IRQHandler
.thumb_set I2C2_EV_IRQHandler,Default_Handler
.weak I2C2_ER_IRQHandler
.thumb_set I2C2_ER_IRQHandler,Default_Handler
.weak SPI1_IRQHandler
.thumb_set SPI1_IRQHandler,Default_Handler
.weak SPI2_IRQHandler
.thumb_set SPI2_IRQHandler,Default_Handler
.weak USART1_IRQHandler
.thumb_set USART1_IRQHandler,Default_Handler
.weak USART2_IRQHandler
.thumb_set USART2_IRQHandler,Default_Handler
.weak USART3_IRQHandler
.thumb_set USART3_IRQHandler,Default_Handler
.weak EXTI15_10_IRQHandler
.thumb_set EXTI15_10_IRQHandler,Default_Handler
.weak RTC_Alarm_IRQHandler
.thumb_set RTC_Alarm_IRQHandler,Default_Handler
.weak OTG_FS_WKUP_IRQHandler
.thumb_set OTG_FS_WKUP_IRQHandler,Default_Handler
.weak TIM8_BRK_TIM12_IRQHandler
.thumb_set TIM8_BRK_TIM12_IRQHandler,Default_Handler
.weak TIM8_UP_TIM13_IRQHandler
.thumb_set TIM8_UP_TIM13_IRQHandler,Default_Handler
.weak TIM8_TRG_COM_TIM14_IRQHandler
.thumb_set TIM8_TRG_COM_TIM14_IRQHandler,Default_Handler
.weak TIM8_CC_IRQHandler
.thumb_set TIM8_CC_IRQHandler,Default_Handler
.weak DMA1_Stream7_IRQHandler
.thumb_set DMA1_Stream7_IRQHandler,Default_Handler
.weak FMC_IRQHandler
.thumb_set FMC_IRQHandler,Default_Handler
.weak SDMMC1_IRQHandler
.thumb_set SDMMC1_IRQHandler,Default_Handler
.weak TIM5_IRQHandler
.thumb_set TIM5_IRQHandler,Default_Handler
.weak SPI3_IRQHandler
.thumb_set SPI3_IRQHandler,Default_Handler
.weak UART4_IRQHandler
.thumb_set UART4_IRQHandler,Default_Handler
.weak UART5_IRQHandler
.thumb_set UART5_IRQHandler,Default_Handler
.weak TIM6_DAC_IRQHandler
.thumb_set TIM6_DAC_IRQHandler,Default_Handler
.weak TIM7_IRQHandler
.thumb_set TIM7_IRQHandler,Default_Handler
.weak DMA2_Stream0_IRQHandler
.thumb_set DMA2_Stream0_IRQHandler,Default_Handler
.weak DMA2_Stream1_IRQHandler
.thumb_set DMA2_Stream1_IRQHandler,Default_Handler
.weak DMA2_Stream2_IRQHandler
.thumb_set DMA2_Stream2_IRQHandler,Default_Handler
.weak DMA2_Stream3_IRQHandler
.thumb_set DMA2_Stream3_IRQHandler,Default_Handler
.weak DMA2_Stream4_IRQHandler
.thumb_set DMA2_Stream4_IRQHandler,Default_Handler
.weak ETH_IRQHandler
.thumb_set ETH_IRQHandler,Default_Handler
.weak ETH_WKUP_IRQHandler
.thumb_set ETH_WKUP_IRQHandler,Default_Handler
.weak OTG_FS_IRQHandler
.thumb_set OTG_FS_IRQHandler,Default_Handler
.weak DMA2_Stream5_IRQHandler
.thumb_set DMA2_Stream5_IRQHandler,Default_Handler
.weak DMA2_Stream6_IRQHandler
.thumb_set DMA2_Stream6_IRQHandler,Default_Handler
.weak DMA2_Stream7_IRQHandler
.thumb_set DMA2_Stream7_IRQHandler,Default_Handler
.weak USART6_IRQHandler
.thumb_set USART6_IRQHandler,Default_Handler
.weak I2C3_EV_IRQHandler
.thumb_set I2C3_EV_IRQHandler,Default_Handler
.weak I2C3_ER_IRQHandler
.thumb_set I2C3_ER_IRQHandler,Default_Handler
.weak OTG_HS_EP1_OUT_IRQHandler
.thumb_set OTG_HS_EP1_OUT_IRQHandler,Default_Handler
.weak OTG_HS_EP1_IN_IRQHandler
.thumb_set OTG_HS_EP1_IN_IRQHandler,Default_Handler
.weak OTG_HS_WKUP_IRQHandler
.thumb_set OTG_HS_WKUP_IRQHandler,Default_Handler
.weak OTG_HS_IRQHandler
.thumb_set OTG_HS_IRQHandler,Default_Handler
.weak DCMI_IRQHandler
.thumb_set DCMI_IRQHandler,Default_Handler
.weak RNG_IRQHandler
.thumb_set RNG_IRQHandler,Default_Handler
.weak FPU_IRQHandler
.thumb_set FPU_IRQHandler,Default_Handler
.weak SAI1_IRQHandler
.thumb_set SAI1_IRQHandler,Default_Handler
.weak DMA2D_IRQHandler
.thumb_set DMA2D_IRQHandler,Default_Handler
.weak SAI2_IRQHandler
.thumb_set SAI2_IRQHandler,Default_Handler
.weak QUADSPI_IRQHandler
.thumb_set QUADSPI_IRQHandler,Default_Handler
.weak CEC_IRQHandler
.thumb_set CEC_IRQHandler,Default_Handler
.weak SPDIF_RX_IRQHandler
.thumb_set SPDIF_RX_IRQHandler,Default_Handler
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/

View file

@ -34,16 +34,16 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM, 0, 0 ), // S5_IN
DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 0 ), // S6_IN
DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, 1, 0 ), // S10_OUT 1
DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, 1, 0 ), // S6_OUT 2
DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, 1, 0 ), // S1_OUT 4
DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR | TIM_USE_LED, 1, 0 ), // S2_OUT
DEF_TIM(TIM3, CH2, PB5, TIM_USE_MOTOR, 1, 0 ), // S4_OUT
DEF_TIM(TIM5, CH1, PA0, TIM_USE_MOTOR, 1, 0 ), // S7_OUT
DEF_TIM(TIM4, CH4, PB9, TIM_USE_MOTOR, 1, 0 ), // S5_OUT 3
DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, 1, 0 ), // S10_OUT 1 DMA1_ST7
DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, 1, 0 ), // S6_OUT 2 DMA1_ST1
DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, 1, 0 ), // S2_OUT 3 DMA1_ST4
DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, 1, 1 ), // S1_OUT 4 DMA1_ST7 DMA1_ST6
DEF_TIM(TIM3, CH2, PB5, TIM_USE_MOTOR | TIM_USE_LED, 1, 0 ), // S4_OUT DMA1_ST5
DEF_TIM(TIM5, CH1, PA0, TIM_USE_MOTOR, 1, 0 ), // S7_OUT DMA1_ST2
DEF_TIM(TIM4, CH4, PB9, TIM_USE_MOTOR, 1, 0 ), // S5_OUT
DEF_TIM(TIM9, CH2, PE6, TIM_USE_MOTOR, 1, 0 ), // S3_OUT
DEF_TIM(TIM2, CH2, PB3, TIM_USE_MOTOR, 1, 0 ), // S8_OUT
DEF_TIM(TIM3, CH1, PB4, TIM_USE_MOTOR, 1, 0 ), // S9_OUT
DEF_TIM(TIM2, CH2, PB3, TIM_USE_MOTOR, 1, 0 ), // S8_OUT DMA1_ST6
DEF_TIM(TIM3, CH1, PB4, TIM_USE_MOTOR, 1, 0 ), // S9_OUT DMA1_ST4
};
#else
// STANDARD LAYOUT

View file

@ -131,6 +131,7 @@
#define SDCARD_DMA_CHANNEL DMA_CHANNEL_4
#define USE_I2C
#define USE_I2C4
#define I2C_DEVICE (I2CDEV_4)
//#define I2C_DEVICE_EXT (I2CDEV_2)

View file

@ -28,12 +28,12 @@
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
DEF_TIM(TIM4, CH2, PB7, TIM_USE_PPM, TIMER_INPUT_ENABLED), // PPM IN
DEF_TIM(TIM16,CH1, PA6, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM1
DEF_TIM(TIM1, CH1N,PA7, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED), // PWM2
DEF_TIM(TIM8, CH1N,PA7, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED), // PWM2
DEF_TIM(TIM8, CH2, PB8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM3
DEF_TIM(TIM17,CH1, PB9, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM4
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM5
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM6
DEF_TIM(TIM1, CH2N,PB0, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED), // PWM5
DEF_TIM(TIM8, CH3N,PB1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED), // PWM6
DEF_TIM(TIM2, CH1, PA0, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM7
DEF_TIM(TIM15,CH1, PA2, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM8
DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM8
DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR | TIM_USE_LED, TIMER_OUTPUT_ENABLED), // LED_STRIP --requires resource remap with dshot (remap to motor 5??)--
};

View file

@ -45,6 +45,8 @@
#include "rx/rx.h"
#include "rx/msp.h"
#include "scheduler/scheduler.h"
#include "sensors/boardalignment.h"
#include "sensors/sensors.h"
#include "sensors/battery.h"
@ -269,7 +271,6 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
extern volatile uint8_t CRC8;
extern volatile bool coreProReady;
extern uint16_t cycleTime; // FIXME dependency on mw.c
// this is calculated at startup based on enabled features.
static uint8_t activeBoxIds[CHECKBOX_ITEM_COUNT];
@ -562,7 +563,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
break;
case BST_STATUS:
bstWrite16(cycleTime);
bstWrite16(getTaskDeltaTime(TASK_GYROPID));
#ifdef USE_I2C
bstWrite16(i2cGetErrorCounter());
#else
@ -691,7 +692,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
break;
case BST_LOOP_TIME:
//bstWrite16(masterConfig.looptime);
bstWrite16(cycleTime);
bstWrite16(getTaskDeltaTime(TASK_GYROPID));
break;
case BST_RC_TUNING:
bstWrite8(currentControlRateProfile->rcRate8);
@ -1043,7 +1044,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
break;
case BST_SET_LOOP_TIME:
//masterConfig.looptime = bstRead16();
cycleTime = bstRead16();
bstRead16();
break;
case BST_SET_PID_CONTROLLER:
break;

View file

@ -114,10 +114,10 @@
#define LED_STRIP
#define DEFAULT_FEATURES (FEATURE_RX_PPM | FEATURE_VBAT | FEATURE_FAILSAFE | FEATURE_AIRMODE | FEATURE_LED_STRIP)
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_FAILSAFE | FEATURE_AIRMODE | FEATURE_LED_STRIP)
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
#define SERIALRX_PROVIDER SERIALRX_SBUS
#define SERIALRX_UART SERIAL_PORT_USART3
#define SERIALRX_UART SERIAL_PORT_USART2
#define USE_SERIAL_4WAY_BLHELI_INTERFACE

View file

@ -148,9 +148,9 @@
#define LED_STRIP
//#define SONAR
//#define SONAR_ECHO_PIN PB1
//#define SONAR_TRIGGER_PIN PB0
#define SONAR
#define SONAR_ECHO_PIN PB1
#define SONAR_TRIGGER_PIN PB0
#define DEFAULT_FEATURES FEATURE_BLACKBOX
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM

View file

@ -1,438 +0,0 @@
/**
******************************************************************************
* @file stm32f7xx_hal_conf.h
* @brief HAL configuration file.
******************************************************************************
* @attention
*
* <h2><center>&copy; COPYRIGHT(c) 2016 STMicroelectronics</center></h2>
*
* 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 of STMicroelectronics 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 HOLDER 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.
*
******************************************************************************
*/
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __STM32F7xx_HAL_CONF_H
#define __STM32F7xx_HAL_CONF_H
#ifdef __cplusplus
extern "C" {
#endif
/* Exported types ------------------------------------------------------------*/
/* Exported constants --------------------------------------------------------*/
/* ########################## Module Selection ############################## */
/**
* @brief This is the list of modules to be used in the HAL driver
*/
#define HAL_MODULE_ENABLED
#define HAL_ADC_MODULE_ENABLED
/* #define HAL_CAN_MODULE_ENABLED */
/* #define HAL_CEC_MODULE_ENABLED */
/* #define HAL_CRC_MODULE_ENABLED */
/* #define HAL_CRYP_MODULE_ENABLED */
/* #define HAL_DAC_MODULE_ENABLED */
/* #define HAL_DCMI_MODULE_ENABLED */
/* #define HAL_DMA2D_MODULE_ENABLED */
/* #define HAL_ETH_MODULE_ENABLED */
/* #define HAL_NAND_MODULE_ENABLED */
/* #define HAL_NOR_MODULE_ENABLED */
/* #define HAL_SRAM_MODULE_ENABLED */
/* #define HAL_SDRAM_MODULE_ENABLED */
/* #define HAL_HASH_MODULE_ENABLED */
/* #define HAL_I2S_MODULE_ENABLED */
/* #define HAL_IWDG_MODULE_ENABLED */
/* #define HAL_LPTIM_MODULE_ENABLED */
/* #define HAL_LTDC_MODULE_ENABLED */
/* #define HAL_QSPI_MODULE_ENABLED */
/* #define HAL_RNG_MODULE_ENABLED */
/* #define HAL_RTC_MODULE_ENABLED */
/* #define HAL_SAI_MODULE_ENABLED */
/* #define HAL_SD_MODULE_ENABLED */
/* #define HAL_SPDIFRX_MODULE_ENABLED */
#define HAL_SPI_MODULE_ENABLED
#define HAL_TIM_MODULE_ENABLED
#define HAL_UART_MODULE_ENABLED
#define HAL_USART_MODULE_ENABLED
/* #define HAL_IRDA_MODULE_ENABLED */
/* #define HAL_SMARTCARD_MODULE_ENABLED */
/* #define HAL_WWDG_MODULE_ENABLED */
#define HAL_PCD_MODULE_ENABLED
/* #define HAL_HCD_MODULE_ENABLED */
/* #define HAL_DFSDM_MODULE_ENABLED */
/* #define HAL_DSI_MODULE_ENABLED */
/* #define HAL_JPEG_MODULE_ENABLED */
/* #define HAL_MDIOS_MODULE_ENABLED */
#define HAL_GPIO_MODULE_ENABLED
#define HAL_DMA_MODULE_ENABLED
#define HAL_RCC_MODULE_ENABLED
#define HAL_FLASH_MODULE_ENABLED
#define HAL_PWR_MODULE_ENABLED
#define HAL_I2C_MODULE_ENABLED
#define HAL_CORTEX_MODULE_ENABLED
/* ########################## HSE/HSI Values adaptation ##################### */
/**
* @brief Adjust the value of External High Speed oscillator (HSE) used in your application.
* This value is used by the RCC HAL module to compute the system frequency
* (when HSE is used as system clock source, directly or through the PLL).
*/
#if !defined (HSE_VALUE)
#define HSE_VALUE ((uint32_t)8000000U) /*!< Value of the External oscillator in Hz */
#endif /* HSE_VALUE */
#if !defined (HSE_STARTUP_TIMEOUT)
#define HSE_STARTUP_TIMEOUT ((uint32_t)100U) /*!< Time out for HSE start up, in ms */
#endif /* HSE_STARTUP_TIMEOUT */
/**
* @brief Internal High Speed oscillator (HSI) value.
* This value is used by the RCC HAL module to compute the system frequency
* (when HSI is used as system clock source, directly or through the PLL).
*/
#if !defined (HSI_VALUE)
#define HSI_VALUE ((uint32_t)16000000U) /*!< Value of the Internal oscillator in Hz*/
#endif /* HSI_VALUE */
/**
* @brief Internal Low Speed oscillator (LSI) value.
*/
#if !defined (LSI_VALUE)
#define LSI_VALUE ((uint32_t)32000U) /*!< LSI Typical Value in Hz*/
#endif /* LSI_VALUE */ /*!< Value of the Internal Low Speed oscillator in Hz
The real value may vary depending on the variations
in voltage and temperature. */
/**
* @brief External Low Speed oscillator (LSE) value.
*/
#if !defined (LSE_VALUE)
#define LSE_VALUE ((uint32_t)32768U) /*!< Value of the External Low Speed oscillator in Hz */
#endif /* LSE_VALUE */
#if !defined (LSE_STARTUP_TIMEOUT)
#define LSE_STARTUP_TIMEOUT ((uint32_t)5000U) /*!< Time out for LSE start up, in ms */
#endif /* LSE_STARTUP_TIMEOUT */
/**
* @brief External clock source for I2S peripheral
* This value is used by the I2S HAL module to compute the I2S clock source
* frequency, this source is inserted directly through I2S_CKIN pad.
*/
#if !defined (EXTERNAL_CLOCK_VALUE)
#define EXTERNAL_CLOCK_VALUE ((uint32_t)12288000U) /*!< Value of the Internal oscillator in Hz*/
#endif /* EXTERNAL_CLOCK_VALUE */
/* Tip: To avoid modifying this file each time you need to use different HSE,
=== you can define the HSE value in your toolchain compiler preprocessor. */
/* ########################### System Configuration ######################### */
/**
* @brief This is the HAL system configuration section
*/
#define VDD_VALUE ((uint32_t)3300U) /*!< Value of VDD in mv */
#define TICK_INT_PRIORITY ((uint32_t)0U) /*!< tick interrupt priority */
#define USE_RTOS 0U
#define PREFETCH_ENABLE 0U
#define ART_ACCLERATOR_ENABLE 0U /* To enable instruction cache and prefetch */
/* ########################## Assert Selection ############################## */
/**
* @brief Uncomment the line below to expanse the "assert_param" macro in the
* HAL drivers code
*/
/* #define USE_FULL_ASSERT 1 */
/* ################## Ethernet peripheral configuration ##################### */
/* Section 1 : Ethernet peripheral configuration */
/* MAC ADDRESS: MAC_ADDR0:MAC_ADDR1:MAC_ADDR2:MAC_ADDR3:MAC_ADDR4:MAC_ADDR5 */
#define MAC_ADDR0 2U
#define MAC_ADDR1 0U
#define MAC_ADDR2 0U
#define MAC_ADDR3 0U
#define MAC_ADDR4 0U
#define MAC_ADDR5 0U
/* Definition of the Ethernet driver buffers size and count */
#define ETH_RX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for receive */
#define ETH_TX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for transmit */
#define ETH_RXBUFNB ((uint32_t)4U) /* 4 Rx buffers of size ETH_RX_BUF_SIZE */
#define ETH_TXBUFNB ((uint32_t)4U) /* 4 Tx buffers of size ETH_TX_BUF_SIZE */
/* Section 2: PHY configuration section */
/* DP83848_PHY_ADDRESS Address*/
#define DP83848_PHY_ADDRESS 0x01U
/* PHY Reset delay these values are based on a 1 ms Systick interrupt*/
#define PHY_RESET_DELAY ((uint32_t)0x000000FFU)
/* PHY Configuration delay */
#define PHY_CONFIG_DELAY ((uint32_t)0x00000FFFU)
#define PHY_READ_TO ((uint32_t)0x0000FFFFU)
#define PHY_WRITE_TO ((uint32_t)0x0000FFFFU)
/* Section 3: Common PHY Registers */
#define PHY_BCR ((uint16_t)0x0000U) /*!< Transceiver Basic Control Register */
#define PHY_BSR ((uint16_t)0x0001U) /*!< Transceiver Basic Status Register */
#define PHY_RESET ((uint16_t)0x8000U) /*!< PHY Reset */
#define PHY_LOOPBACK ((uint16_t)0x4000U) /*!< Select loop-back mode */
#define PHY_FULLDUPLEX_100M ((uint16_t)0x2100U) /*!< Set the full-duplex mode at 100 Mb/s */
#define PHY_HALFDUPLEX_100M ((uint16_t)0x2000U) /*!< Set the half-duplex mode at 100 Mb/s */
#define PHY_FULLDUPLEX_10M ((uint16_t)0x0100U) /*!< Set the full-duplex mode at 10 Mb/s */
#define PHY_HALFDUPLEX_10M ((uint16_t)0x0000U) /*!< Set the half-duplex mode at 10 Mb/s */
#define PHY_AUTONEGOTIATION ((uint16_t)0x1000U) /*!< Enable auto-negotiation function */
#define PHY_RESTART_AUTONEGOTIATION ((uint16_t)0x0200U) /*!< Restart auto-negotiation function */
#define PHY_POWERDOWN ((uint16_t)0x0800U) /*!< Select the power down mode */
#define PHY_ISOLATE ((uint16_t)0x0400U) /*!< Isolate PHY from MII */
#define PHY_AUTONEGO_COMPLETE ((uint16_t)0x0020U) /*!< Auto-Negotiation process completed */
#define PHY_LINKED_STATUS ((uint16_t)0x0004U) /*!< Valid link established */
#define PHY_JABBER_DETECTION ((uint16_t)0x0002U) /*!< Jabber condition detected */
/* Section 4: Extended PHY Registers */
#define PHY_SR ((uint16_t)0x10U) /*!< PHY status register Offset */
#define PHY_SPEED_STATUS ((uint16_t)0x0002U) /*!< PHY Speed mask */
#define PHY_DUPLEX_STATUS ((uint16_t)0x0004U) /*!< PHY Duplex mask */
/* ################## SPI peripheral configuration ########################## */
/* CRC FEATURE: Use to activate CRC feature inside HAL SPI Driver
* Activated: CRC code is present inside driver
* Deactivated: CRC code cleaned from driver
*/
#define USE_SPI_CRC 0U
/* Includes ------------------------------------------------------------------*/
/**
* @brief Include module's header file
*/
#ifdef HAL_RCC_MODULE_ENABLED
#include "stm32f7xx_hal_rcc.h"
#endif /* HAL_RCC_MODULE_ENABLED */
#ifdef HAL_GPIO_MODULE_ENABLED
#include "stm32f7xx_hal_gpio.h"
#endif /* HAL_GPIO_MODULE_ENABLED */
#ifdef HAL_DMA_MODULE_ENABLED
#include "stm32f7xx_hal_dma.h"
#endif /* HAL_DMA_MODULE_ENABLED */
#ifdef HAL_CORTEX_MODULE_ENABLED
#include "stm32f7xx_hal_cortex.h"
#endif /* HAL_CORTEX_MODULE_ENABLED */
#ifdef HAL_ADC_MODULE_ENABLED
#include "stm32f7xx_hal_adc.h"
#endif /* HAL_ADC_MODULE_ENABLED */
#ifdef HAL_CAN_MODULE_ENABLED
#include "stm32f7xx_hal_can.h"
#endif /* HAL_CAN_MODULE_ENABLED */
#ifdef HAL_CEC_MODULE_ENABLED
#include "stm32f7xx_hal_cec.h"
#endif /* HAL_CEC_MODULE_ENABLED */
#ifdef HAL_CRC_MODULE_ENABLED
#include "stm32f7xx_hal_crc.h"
#endif /* HAL_CRC_MODULE_ENABLED */
#ifdef HAL_CRYP_MODULE_ENABLED
#include "stm32f7xx_hal_cryp.h"
#endif /* HAL_CRYP_MODULE_ENABLED */
#ifdef HAL_DMA2D_MODULE_ENABLED
#include "stm32f7xx_hal_dma2d.h"
#endif /* HAL_DMA2D_MODULE_ENABLED */
#ifdef HAL_DAC_MODULE_ENABLED
#include "stm32f7xx_hal_dac.h"
#endif /* HAL_DAC_MODULE_ENABLED */
#ifdef HAL_DCMI_MODULE_ENABLED
#include "stm32f7xx_hal_dcmi.h"
#endif /* HAL_DCMI_MODULE_ENABLED */
#ifdef HAL_ETH_MODULE_ENABLED
#include "stm32f7xx_hal_eth.h"
#endif /* HAL_ETH_MODULE_ENABLED */
#ifdef HAL_FLASH_MODULE_ENABLED
#include "stm32f7xx_hal_flash.h"
#endif /* HAL_FLASH_MODULE_ENABLED */
#ifdef HAL_SRAM_MODULE_ENABLED
#include "stm32f7xx_hal_sram.h"
#endif /* HAL_SRAM_MODULE_ENABLED */
#ifdef HAL_NOR_MODULE_ENABLED
#include "stm32f7xx_hal_nor.h"
#endif /* HAL_NOR_MODULE_ENABLED */
#ifdef HAL_NAND_MODULE_ENABLED
#include "stm32f7xx_hal_nand.h"
#endif /* HAL_NAND_MODULE_ENABLED */
#ifdef HAL_SDRAM_MODULE_ENABLED
#include "stm32f7xx_hal_sdram.h"
#endif /* HAL_SDRAM_MODULE_ENABLED */
#ifdef HAL_HASH_MODULE_ENABLED
#include "stm32f7xx_hal_hash.h"
#endif /* HAL_HASH_MODULE_ENABLED */
#ifdef HAL_I2C_MODULE_ENABLED
#include "stm32f7xx_hal_i2c.h"
#endif /* HAL_I2C_MODULE_ENABLED */
#ifdef HAL_I2S_MODULE_ENABLED
#include "stm32f7xx_hal_i2s.h"
#endif /* HAL_I2S_MODULE_ENABLED */
#ifdef HAL_IWDG_MODULE_ENABLED
#include "stm32f7xx_hal_iwdg.h"
#endif /* HAL_IWDG_MODULE_ENABLED */
#ifdef HAL_LPTIM_MODULE_ENABLED
#include "stm32f7xx_hal_lptim.h"
#endif /* HAL_LPTIM_MODULE_ENABLED */
#ifdef HAL_LTDC_MODULE_ENABLED
#include "stm32f7xx_hal_ltdc.h"
#endif /* HAL_LTDC_MODULE_ENABLED */
#ifdef HAL_PWR_MODULE_ENABLED
#include "stm32f7xx_hal_pwr.h"
#endif /* HAL_PWR_MODULE_ENABLED */
#ifdef HAL_QSPI_MODULE_ENABLED
#include "stm32f7xx_hal_qspi.h"
#endif /* HAL_QSPI_MODULE_ENABLED */
#ifdef HAL_RNG_MODULE_ENABLED
#include "stm32f7xx_hal_rng.h"
#endif /* HAL_RNG_MODULE_ENABLED */
#ifdef HAL_RTC_MODULE_ENABLED
#include "stm32f7xx_hal_rtc.h"
#endif /* HAL_RTC_MODULE_ENABLED */
#ifdef HAL_SAI_MODULE_ENABLED
#include "stm32f7xx_hal_sai.h"
#endif /* HAL_SAI_MODULE_ENABLED */
#ifdef HAL_SD_MODULE_ENABLED
#include "stm32f7xx_hal_sd.h"
#endif /* HAL_SD_MODULE_ENABLED */
#ifdef HAL_SPDIFRX_MODULE_ENABLED
#include "stm32f7xx_hal_spdifrx.h"
#endif /* HAL_SPDIFRX_MODULE_ENABLED */
#ifdef HAL_SPI_MODULE_ENABLED
#include "stm32f7xx_hal_spi.h"
#endif /* HAL_SPI_MODULE_ENABLED */
#ifdef HAL_TIM_MODULE_ENABLED
#include "stm32f7xx_hal_tim.h"
#endif /* HAL_TIM_MODULE_ENABLED */
#ifdef HAL_UART_MODULE_ENABLED
#include "stm32f7xx_hal_uart.h"
#endif /* HAL_UART_MODULE_ENABLED */
#ifdef HAL_USART_MODULE_ENABLED
#include "stm32f7xx_hal_usart.h"
#endif /* HAL_USART_MODULE_ENABLED */
#ifdef HAL_IRDA_MODULE_ENABLED
#include "stm32f7xx_hal_irda.h"
#endif /* HAL_IRDA_MODULE_ENABLED */
#ifdef HAL_SMARTCARD_MODULE_ENABLED
#include "stm32f7xx_hal_smartcard.h"
#endif /* HAL_SMARTCARD_MODULE_ENABLED */
#ifdef HAL_WWDG_MODULE_ENABLED
#include "stm32f7xx_hal_wwdg.h"
#endif /* HAL_WWDG_MODULE_ENABLED */
#ifdef HAL_PCD_MODULE_ENABLED
#include "stm32f7xx_hal_pcd.h"
#endif /* HAL_PCD_MODULE_ENABLED */
#ifdef HAL_HCD_MODULE_ENABLED
#include "stm32f7xx_hal_hcd.h"
#endif /* HAL_HCD_MODULE_ENABLED */
#ifdef HAL_DFSDM_MODULE_ENABLED
#include "stm32f7xx_hal_dfsdm.h"
#endif /* HAL_DFSDM_MODULE_ENABLED */
#ifdef HAL_DSI_MODULE_ENABLED
#include "stm32f7xx_hal_dsi.h"
#endif /* HAL_DSI_MODULE_ENABLED */
#ifdef HAL_JPEG_MODULE_ENABLED
#include "stm32f7xx_hal_jpeg.h"
#endif /* HAL_JPEG_MODULE_ENABLED */
#ifdef HAL_MDIOS_MODULE_ENABLED
#include "stm32f7xx_hal_mdios.h"
#endif /* HAL_MDIOS_MODULE_ENABLED */
/* Exported macro ------------------------------------------------------------*/
#ifdef USE_FULL_ASSERT
/**
* @brief The assert_param macro is used for function's parameters check.
* @param expr: If expr is false, it calls assert_failed function
* which reports the name of the source file and the source
* line number of the call that failed.
* If expr is true, it returns no value.
* @retval None
*/
#define assert_param(expr) ((expr) ? (void)0U : assert_failed((uint8_t *)__FILE__, __LINE__))
/* Exported functions ------------------------------------------------------- */
void assert_failed(uint8_t* file, uint32_t line);
#else
#define assert_param(expr) ((void)0U)
#endif /* USE_FULL_ASSERT */
#ifdef __cplusplus
}
#endif
#endif /* __STM32F7xx_HAL_CONF_H */
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/

View file

@ -26,10 +26,10 @@
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
DEF_TIM(TIM2, CH1,PA15, TIM_USE_PPM, TIMER_INPUT_ENABLED), // PPM IN
DEF_TIM(TIM8,CH2N, PB4, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED), // PWM1
DEF_TIM(TIM16,CH1, PB4, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM1
DEF_TIM(TIM17,CH1, PB5, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM2
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM3
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM4
DEF_TIM(TIM8,CH3N, PB1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED), // PWM3
DEF_TIM(TIM8,CH2N, PB0, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED), // PWM4
DEF_TIM(TIM16,CH1, PB8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM5
DEF_TIM(TIM17,CH1, PB9, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM6
DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, TIMER_OUTPUT_ENABLED), // LED_STRIP

View file

@ -48,6 +48,7 @@
#define USE_ESC_SENSOR
#define REMAP_TIM17_DMA
#define REMAP_TIM16_DMA
#define USE_VCP
#define USE_UART1

View file

@ -49,6 +49,7 @@
#define USE_EXTI
#define MAG_INT_EXTI PC14
#define MPU_INT_EXTI PC13
//#define DEBUG_MPU_DATA_READY_INTERRUPT
#define USE_MPU_DATA_READY_SIGNAL
//#define DEBUG_MAG_DATA_READY_INTERRUPT

View file

@ -0,0 +1,37 @@
/*
* 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/dma.h"
#include "drivers/timer.h"
#include "drivers/timer_def.h"
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
DEF_TIM(TIM3, CH2, PC7, TIM_USE_PPM, TIMER_OUTPUT_STANDARD, 0 ),
DEF_TIM(TIM5, CH1, PA0, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0 ),
DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0 ),
DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0 ),
DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 1 ),
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR | TIM_USE_LED, TIMER_OUTPUT_STANDARD, 0 ),
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0 ),
DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0 ),
DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0 ),
};

View file

@ -0,0 +1,140 @@
/*
* 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 "NERO"
#define CONFIG_START_FLASH_ADDRESS (0x08060000)
#define USBD_PRODUCT_STRING "NERO"
#define HW_PIN PB2
#define BOARD_HAS_VOLTAGE_DIVIDER
#define LED0 PB6
#define LED1 PB5
#define LED2 PB4
#define BEEPER PC1
#define BEEPER_INVERTED
// MPU6500 interrupt
#define USE_EXTI
#define MPU_INT_EXTI PB2
#define USE_MPU_DATA_READY_SIGNAL
//#define DEBUG_MPU_DATA_READY_INTERRUPT
#define MPU6500_CS_PIN PC4
#define MPU6500_SPI_INSTANCE SPI1
#define ACC
#define USE_ACC_MPU6500
#define USE_ACC_SPI_MPU6500
#define ACC_MPU6500_ALIGN CW0_DEG
#define GYRO
#define USE_GYRO_MPU6500
#define USE_GYRO_SPI_MPU6500
#define GYRO_MPU6500_ALIGN CW0_DEG
#define USE_SDCARD
#define SDCARD_DETECT_INVERTED
#define SDCARD_DETECT_PIN PD2
#define SDCARD_SPI_INSTANCE SPI3
#define SDCARD_SPI_CS_PIN PA15
// SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init:
#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz
// Divide to under 25MHz for normal operation:
#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz
#define SDCARD_DMA_CHANNEL_TX DMA1_Stream5
#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF1_5
#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1
#define SDCARD_DMA_CHANNEL DMA_CHANNEL_0
#define USE_I2C
#define I2C_DEVICE (I2CDEV_1)
#define USE_VCP
//#define VBUS_SENSING_PIN PA8
//#define VBUS_SENSING_ENABLED
#define USE_UART1
#define UART1_RX_PIN PA10
#define UART1_TX_PIN PA9
#define USE_UART3
#define UART3_RX_PIN PB11
#define UART3_TX_PIN PB10
#define USE_UART6
#define UART6_RX_PIN PC7
#define UART6_TX_PIN PC6
#define SERIAL_PORT_COUNT 4
//#define USE_ESCSERIAL //TODO: make ESC serial F7 compatible
//#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1
#define USE_SPI
#define USE_SPI_DEVICE_1
#define SPI1_NSS_PIN PC4
#define SPI1_SCK_PIN PA5
#define SPI1_MISO_PIN PA6
#define SPI1_MOSI_PIN PA7
#define USE_SPI_DEVICE_2
#define SPI2_NSS_PIN PB12
#define SPI2_SCK_PIN PB13
#define SPI2_MISO_PIN PB14
#define SPI2_MOSI_PIN PB15
#define USE_SPI_DEVICE_3
#define SPI3_NSS_PIN PB3
#define SPI3_SCK_PIN PC10
#define SPI3_MISO_PIN PC11
#define SPI3_MOSI_PIN PC12
#define USE_ADC
#define VBAT_ADC_PIN PC3
//#define USE_ESC_SENSOR
#define LED_STRIP
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
#define DEFAULT_FEATURES FEATURE_BLACKBOX
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
#define SERIALRX_PROVIDER SERIALRX_SBUS
#define SERIALRX_UART SERIAL_PORT_USART6
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
#define SPEKTRUM_BIND
#define BIND_PIN PB11
#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 9
#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(8) | TIM_N(9) )

View file

@ -0,0 +1,8 @@
F7X2RE_TARGETS += $(TARGET)
FEATURES += SDCARD VCP
TARGET_SRC = \
drivers/accgyro_spi_mpu6500.c \
drivers/accgyro_mpu6500.c \
drivers/light_ws2811strip.c \
drivers/light_ws2811strip_hal.c

View file

@ -45,19 +45,21 @@
#define BMP280_SPI_INSTANCE SPI1
#define BMP280_CS_PIN PA13
//#define BARO
//#define USE_BARO_BMP280
//#define USE_BARO_SPI_BMP280
#define BARO
#define USE_BARO_BMP280
#define USE_BARO_SPI_BMP280
//#define MAG // External
//#define USE_MAG_AK8963
//#define USE_MAG_AK8975
//#define USE_MAG_HMC5883
#define MAG // External
#define USE_MAG_AK8963
#define USE_MAG_AK8975
#define USE_MAG_HMC5883
//#define SONAR
//#define SONAR_ECHO_PIN PB1
//#define SONAR_TRIGGER_PIN PB0
#undef GPS
#define USB_IO
#define USB_CABLE_DETECTION
#define USB_DETECT_PIN PB5

View file

@ -109,15 +109,15 @@
#define SERIALRX_PROVIDER SERIALRX_SBUS
#define SERIALRX_UART SERIAL_PORT_USART1
//#define NAV
//#define NAV_AUTO_MAG_DECLINATION
//#define NAV_GPS_GLITCH_DETECTION
//#define NAV_MAX_WAYPOINTS 60
//#define GPS
#define NAV
#define NAV_AUTO_MAG_DECLINATION
#define NAV_GPS_GLITCH_DETECTION
#define NAV_MAX_WAYPOINTS 60
#define GPS
#define BLACKBOX
#define TELEMETRY
#define SERIAL_RX
//#define AUTOTUNE
#define AUTOTUNE
#define USE_SERVOS
#define USE_CLI

View file

View file

@ -31,6 +31,10 @@
#define TARGET_BOARD_IDENTIFIER "SOUL"
#define USBD_PRODUCT_STRING "DemonSoulF4"
#elif defined(PODIUMF4)
#define TARGET_BOARD_IDENTIFIER "PODI"
#define USBD_PRODUCT_STRING "PodiumF4"
#else
#define TARGET_BOARD_IDENTIFIER "REVO"
#define USBD_PRODUCT_STRING "Revolution"
@ -44,6 +48,11 @@
#define USE_ESC_SENSOR
#define LED0 PB5
#if defined(PODIUMF4)
#define LED1 PB4
#define LED2 PB6
#endif
// Disable LED1, conflicts with AirbotF4/Flip32F4/Revolt beeper
#if defined(AIRBOTF4)
#define BEEPER PB4
@ -78,7 +87,7 @@
#define USE_GYRO_SPI_MPU6000
#define ACC_MPU6000_ALIGN CW180_DEG
#elif defined(REVOLT)
#elif defined(REVOLT) || defined(PODIUMF4)
#define USE_ACC_MPU6500
#define USE_ACC_SPI_MPU6500
@ -112,7 +121,7 @@
#define MPU_INT_EXTI PC4
#define USE_MPU_DATA_READY_SIGNAL
#if !defined(AIRBOTF4) && !defined(REVOLT) && !defined(SOULF4)
#if !defined(AIRBOTF4) && !defined(REVOLT) && !defined(SOULF4) && !defined(PODIUMF4)
#define MAG
#define USE_MAG_HMC5883
#define MAG_HMC5883_ALIGN CW90_DEG
@ -129,7 +138,11 @@
#define USE_FLASH_M25P16
#define USE_VCP
#if defined(PODIUMF4)
#define VBUS_SENSING_PIN PA8
#else
#define VBUS_SENSING_PIN PC5
#endif
#define USE_UART1
#define UART1_RX_PIN PA10
@ -163,16 +176,26 @@
#define I2C_DEVICE (I2CDEV_1)
#define USE_ADC
#if !defined(PODIUMF4)
#define CURRENT_METER_ADC_PIN PC1
#define VBAT_ADC_PIN PC2
//#define RSSI_ADC_PIN PA0
#else
#define VBAT_ADC_PIN PC3
#define VBAT_ADC_CHANNEL ADC_Channel_13
#endif
#define LED_STRIP
#define SENSORS_SET (SENSOR_ACC)
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
#if defined(PODIUMF4)
#define SERIALRX_PROVIDER SERIALRX_SBUS
#define SERIALRX_UART SERIAL_PORT_USART6
#define DEFAULT_FEATURES FEATURE_TELEMETRY
#else
#define DEFAULT_FEATURES (FEATURE_BLACKBOX)
#endif
#define SPEKTRUM_BIND
// USART3,

View file

@ -25,13 +25,13 @@
#include "drivers/dma.h"
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
DEF_TIM(TIM4, CH1, PB6, TIM_USE_MOTOR, 1), // PWM1 - PB6
DEF_TIM(TIM4, CH2, PB7, TIM_USE_MOTOR, 1), // PWM2 - PB6
DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, 1), // PWM3 - PB8
DEF_TIM(TIM4, CH4, PB9, TIM_USE_MOTOR, 1), // PWM4 - PB9
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 1), // PWM5 - PB0 - *TIM3_CH3
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 1), // PWM6 - PB1 - *TIM3_CH4
DEF_TIM(TIM2, CH4, PB11, TIM_USE_PPM, 0), // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7)y
DEF_TIM(TIM2, CH4, PB11, TIM_USE_PPM, TIMER_INPUT_ENABLED), // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7)
DEF_TIM(TIM16,CH1N,PB6, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED), // PWM1 - PB6
DEF_TIM(TIM3, CH4, PB7, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM2 - PB6
DEF_TIM(TIM8, CH2, PB8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM3 - PB8
DEF_TIM(TIM17,CH1, PB9, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM4 - PB9
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM5 - PB0 - *TIM3_CH3
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM6 - PB1 - *TIM3_CH4
};

View file

@ -95,6 +95,9 @@
#define SPI3_MISO_PIN PB4
#define SPI3_MOSI_PIN PB5
#define REMAP_TIM16_DMA
#define REMAP_TIM17_DMA
#define USE_MAX7456
#define MAX7456_SPI_INSTANCE SPI3
#define MAX7456_SPI_CS_PIN PA15

View file

@ -168,6 +168,7 @@
#define OSD
#undef GPS
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_BLACKBOX | FEATURE_RSSI_ADC | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_OSD)
@ -193,4 +194,3 @@
#define USABLE_TIMER_CHANNEL_COUNT 12 // 2xPPM, 6xPWM, UART3 RX/TX, LED Strip, IR.
#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(15) | TIM_N(16))

View file

@ -43,7 +43,6 @@
#define STM_FAST_TARGET
#define USE_DSHOT
#define I2C3_OVERCLOCK true
#define GPS
#endif
#ifdef STM32F3
@ -82,6 +81,7 @@
#if (FLASH_SIZE > 64)
#define BLACKBOX
#define GPS
#define TELEMETRY
#define TELEMETRY_FRSKY
#define TELEMETRY_HOTT
@ -99,6 +99,7 @@
#define TELEMETRY_SRXL
#define TELEMETRY_JETIEXBUS
#define TELEMETRY_MAVLINK
#define TELEMETRY_IBUS
#define USE_RX_MSP
#define USE_SERIALRX_JETIEXBUS
#define VTX_CONTROL

View file

@ -0,0 +1,29 @@
/*
*****************************************************************************
**
** File : stm32_flash_f722.ld
**
** Abstract : Linker script for STM32F722RETx Device with
** 512KByte FLASH, 256KByte RAM
**
*****************************************************************************
*/
/* Entry Point */
ENTRY(Reset_Handler)
/* Specify the memory areas */
MEMORY
{
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 384K
FLASH_CONFIG (r) : ORIGIN = 0x08060000, LENGTH = 128K
TCM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K
RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 192K
MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K
}
/* note TCM could be used for stack */
REGION_ALIAS("STACKRAM", TCM)
INCLUDE "stm32_flash.ld"

View file

@ -66,13 +66,22 @@
#include "stm32f7xx.h"
#if !defined (HSE_VALUE)
#define HSE_VALUE ((uint32_t)25000000) /*!< Default value of the External oscillator in Hz */
#define HSE_VALUE ((uint32_t)8000000) /*!< Default value of the External oscillator in Hz */
#endif /* HSE_VALUE */
#if !defined (HSI_VALUE)
#define HSI_VALUE ((uint32_t)16000000) /*!< Value of the Internal oscillator in Hz*/
#endif /* HSI_VALUE */
#define PLL_M 8
#define PLL_N 432
#define PLL_P RCC_PLLP_DIV2 /* 2 */
#define PLL_Q 9
#define PLL_SAIN 384
#define PLL_SAIQ 7
#define PLL_SAIP RCC_PLLSAIP_DIV8
/**
* @}
*/
@ -122,7 +131,7 @@
is no need to call the 2 first functions listed above, since SystemCoreClock
variable is updated automatically.
*/
uint32_t SystemCoreClock = 216000000;
uint32_t SystemCoreClock = (PLL_N / PLL_P) * 1000000;
const uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9};
const uint8_t APBPrescTable[8] = {0, 0, 0, 0, 1, 2, 3, 4};
@ -152,10 +161,10 @@
RCC_OscInitStruct.HSEState = RCC_HSE_ON;
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
RCC_OscInitStruct.PLL.PLLM = 8;
RCC_OscInitStruct.PLL.PLLN = 432;
RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
RCC_OscInitStruct.PLL.PLLQ = 9;
RCC_OscInitStruct.PLL.PLLM = PLL_M;
RCC_OscInitStruct.PLL.PLLN = PLL_N;
RCC_OscInitStruct.PLL.PLLP = PLL_P;
RCC_OscInitStruct.PLL.PLLQ = PLL_Q;
ret = HAL_RCC_OscConfig(&RCC_OscInitStruct);
if(ret != HAL_OK)
@ -172,9 +181,9 @@
/* Select PLLSAI output as USB clock source */
PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_CLK48;
PeriphClkInitStruct.Clk48ClockSelection = RCC_CLK48SOURCE_PLLSAIP;
PeriphClkInitStruct.PLLSAI.PLLSAIN = 384;
PeriphClkInitStruct.PLLSAI.PLLSAIQ = 7;
PeriphClkInitStruct.PLLSAI.PLLSAIP = RCC_PLLSAIP_DIV8;
PeriphClkInitStruct.PLLSAI.PLLSAIN = PLL_SAIN;
PeriphClkInitStruct.PLLSAI.PLLSAIQ = PLL_SAIQ;
PeriphClkInitStruct.PLLSAI.PLLSAIP = PLL_SAIP;
if(HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK)
{
while(1) {};

View file

@ -79,8 +79,6 @@ static portSharing_e frskyPortSharing;
extern batteryConfig_t *batteryConfig;
extern int16_t telemTemperature1; // FIXME dependency on mw.c
#define CYCLETIME 125
#define PROTOCOL_HEADER 0x5E

426
src/main/telemetry/ibus.c Normal file
View file

@ -0,0 +1,426 @@
/*
* 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/>.
*
* FlySky iBus telemetry implementation by CraigJPerry.
* Unit tests and some additions by Unitware
*
* Many thanks to Dave Borthwick's iBus telemetry dongle converter for
* PIC 12F1572 (also distributed under GPLv3) which was referenced to
* clarify the protocol.
*/
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include "platform.h"
#include "common/utils.h"
#if defined(TELEMETRY) && defined(TELEMETRY_IBUS)
#include "config/config_master.h"
#include "common/axis.h"
#include "drivers/system.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "drivers/serial.h"
#include "sensors/sensors.h"
#include "sensors/acceleration.h"
#include "sensors/battery.h"
#include "sensors/barometer.h"
#include "io/serial.h"
#include "fc/rc_controls.h"
#include "scheduler/scheduler.h"
#include "telemetry/telemetry.h"
#include "telemetry/ibus.h"
/*
* iBus Telemetry is a half-duplex serial protocol. It shares 1 line for
* both TX and RX. It runs at a fixed baud rate of 115200. Queries are sent
* every 7ms by the iBus receiver. Multiple sensors can be daisy chained with
* ibus but this is implemented but not tested because i don't have one of the
* sensors to test with
*
* _______
* / \ /---------\
* | STM32 |--UART TX-->[Bi-directional @ 115200 baud]<--| IBUS RX |
* | uC |--UART RX--x[not connected] \---------/
* \_______/
*
*
* The protocol is driven by the iBus receiver, currently either an IA6B or
* IA10. All iBus traffic is little endian. It begins with the iBus rx
* querying for a sensor on the iBus:
*
*
* /---------\
* | IBUS RX | > Hello sensor at address 1, are you there?
* \---------/ [ 0x04, 0x81, 0x7A, 0xFF ]
*
* 0x04 - Packet Length
* 0x81 - bits 7-4 Command (1000 = discover sensor)
* bits 3-0 Address (0001 = address 1)
* 0x7A, 0xFF - Checksum, 0xFFFF - (0x04 + 0x81)
*
*
* Due to the daisy-chaining, this hello also serves to inform the sensor
* of its address (position in the chain). There are 16 possible addresses
* in iBus, however address 0 is reserved for the rx's internally measured
* voltage leaving 0x1 to 0xF remaining.
*
* Having learned it's address, the sensor simply echos the message back:
*
*
* /--------\
* Yes, i'm here, hello! < | Sensor |
* [ 0x04, 0x81, 0x7A, 0xFF ] \--------/
*
* 0x04, 0x81, 0x7A, 0xFF - Echo back received packet
*
*
* On receipt of a response, the iBus rx next requests the sensor's type:
*
*
* /---------\
* | IBUS RX | > Sensor at address 1, what type are you?
* \---------/ [ 0x04, 0x91, 0x6A, 0xFF ]
*
* 0x04 - Packet Length
* 0x91 - bits 7-4 Command (1001 = request sensor type)
* bits 3-0 Address (0001 = address 1)
* 0x6A, 0xFF - Checksum, 0xFFFF - (0x04 + 0x91)
*
*
* To which the sensor responds with its details:
*
*
* /--------\
* Yes, i'm here, hello! < | Sensor |
* [ 0x06 0x91 0x00 0x02 0x66 0xFF ] \--------/
*
* 0x06 - Packet Length
* 0x91 - bits 7-4 Command (1001 = request sensor type)
* bits 3-0 Address (0001 = address 1)
* 0x00 - Measurement type (0 = internal voltage)
* 0x02 - Unknown, always 0x02
* 0x66, 0xFF - Checksum, 0xFFFF - (0x06 + 0x91 + 0x00 + 0x02)
*
*
* The iBus rx continues the discovery process by querying the next
* address. Discovery stops at the first address which does not respond.
*
* The iBus rx then begins a continual loop, requesting measurements from
* each sensor discovered:
*
*
* /---------\
* | IBUS RX | > Sensor at address 1, please send your measurement
* \---------/ [ 0x04, 0xA1, 0x5A, 0xFF ]
*
* 0x04 - Packet Length
* 0xA1 - bits 7-4 Command (1010 = request measurement)
* bits 3-0 Address (0001 = address 1)
* 0x5A, 0xFF - Checksum, 0xFFFF - (0x04 + 0xA1)
*
*
* /--------\
* I'm reading 0 volts < | Sensor |
* [ 0x06 0xA1 0x00 0x00 0x5E 0xFF ] \--------/
*
* 0x06 - Packet Length
* 0xA1 - bits 7-4 Command (1010 = request measurement)
* bits 3-0 Address (0001 = address 1)
* 0x00, 0x00 - The measurement
* 0x58, 0xFF - Checksum, 0xFFFF - (0x06 + 0xA1 + 0x00 + 0x00)
*
*
* Due to the limited telemetry data types possible with ibus, we
* simply send everything which can be represented. Currently this
* is voltage and temperature.
*
*/
/*
PG_REGISTER_WITH_RESET_TEMPLATE(ibusTelemetryConfig_t, ibusTelemetryConfig, PG_IBUS_TELEMETRY_CONFIG, 0);
PG_RESET_TEMPLATE(ibusTelemetryConfig_t, ibusTelemetryConfig,
.report_cell_voltage = false,
);
*/
#define IBUS_TASK_PERIOD_US (500)
#define IBUS_UART_MODE (MODE_RXTX)
#define IBUS_BAUDRATE (115200)
#define IBUS_CYCLE_TIME_MS (8)
#define IBUS_CHECKSUM_SIZE (2)
#define IBUS_MIN_LEN (2 + IBUS_CHECKSUM_SIZE)
#define IBUS_MAX_TX_LEN (6)
#define IBUS_MAX_RX_LEN (4)
#define IBUS_RX_BUF_LEN (IBUS_MAX_RX_LEN)
#define IBUS_TEMPERATURE_OFFSET (400)
typedef uint8_t ibusAddress_t;
typedef enum {
IBUS_COMMAND_DISCOVER_SENSOR = 0x80,
IBUS_COMMAND_SENSOR_TYPE = 0x90,
IBUS_COMMAND_MEASUREMENT = 0xA0
} ibusCommand_e;
typedef enum {
IBUS_SENSOR_TYPE_TEMPERATURE = 0x01,
IBUS_SENSOR_TYPE_RPM = 0x02,
IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE = 0x03
} ibusSensorType_e;
/* Address lookup relative to the sensor base address which is the lowest address seen by the FC
The actual lowest value is likely to change when sensors are daisy chained */
static const uint8_t sensorAddressTypeLookup[] = {
IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE,
IBUS_SENSOR_TYPE_TEMPERATURE,
IBUS_SENSOR_TYPE_RPM
};
static serialPort_t *ibusSerialPort = NULL;
static serialPortConfig_t *ibusSerialPortConfig;
/* The sent bytes will be echoed back since Tx and Rx are wired together, this counter
* will keep track of how many rx chars that shall be discarded */
static uint8_t outboundBytesToIgnoreOnRxCount = 0;
static bool ibusTelemetryEnabled = false;
static portSharing_e ibusPortSharing;
#define INVALID_IBUS_ADDRESS 0
static ibusAddress_t ibusBaseAddress = INVALID_IBUS_ADDRESS;
static uint8_t ibusReceiveBuffer[IBUS_RX_BUF_LEN] = { 0x0 };
static uint16_t calculateChecksum(const uint8_t *ibusPacket, size_t packetLength)
{
uint16_t checksum = 0xFFFF;
for (size_t i = 0; i < packetLength - IBUS_CHECKSUM_SIZE; i++) {
checksum -= ibusPacket[i];
}
return checksum;
}
static bool isChecksumOk(const uint8_t *ibusPacket)
{
uint16_t calculatedChecksum = calculateChecksum(ibusReceiveBuffer, IBUS_RX_BUF_LEN);
// Note that there's a byte order swap to little endian here
return (calculatedChecksum >> 8) == ibusPacket[IBUS_RX_BUF_LEN - 1]
&& (calculatedChecksum & 0xFF) == ibusPacket[IBUS_RX_BUF_LEN - 2];
}
static void transmitIbusPacket(uint8_t *ibusPacket, size_t payloadLength)
{
uint16_t checksum = calculateChecksum(ibusPacket, payloadLength + IBUS_CHECKSUM_SIZE);
for (size_t i = 0; i < payloadLength; i++) {
serialWrite(ibusSerialPort, ibusPacket[i]);
}
serialWrite(ibusSerialPort, checksum & 0xFF);
serialWrite(ibusSerialPort, checksum >> 8);
outboundBytesToIgnoreOnRxCount += payloadLength + IBUS_CHECKSUM_SIZE;
}
static void sendIbusDiscoverSensorReply(ibusAddress_t address)
{
uint8_t sendBuffer[] = { 0x04, IBUS_COMMAND_DISCOVER_SENSOR | address};
transmitIbusPacket(sendBuffer, sizeof(sendBuffer));
}
static void sendIbusSensorType(ibusAddress_t address)
{
uint8_t sendBuffer[] = {0x06,
IBUS_COMMAND_SENSOR_TYPE | address,
sensorAddressTypeLookup[address - ibusBaseAddress],
0x02
};
transmitIbusPacket(sendBuffer, sizeof(sendBuffer));
}
static void sendIbusMeasurement(ibusAddress_t address, uint16_t measurement)
{
uint8_t sendBuffer[] = { 0x06, IBUS_COMMAND_MEASUREMENT | address, measurement & 0xFF, measurement >> 8};
transmitIbusPacket(sendBuffer, sizeof(sendBuffer));
}
static bool isCommand(ibusCommand_e expected, const uint8_t *ibusPacket)
{
return (ibusPacket[1] & 0xF0) == expected;
}
static ibusAddress_t getAddress(const uint8_t *ibusPacket)
{
return (ibusPacket[1] & 0x0F);
}
static void dispatchMeasurementReply(ibusAddress_t address)
{
int value;
switch (sensorAddressTypeLookup[address - ibusBaseAddress]) {
case IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE:
value = getVbat() * 10;
if (ibusTelemetryConfig()->report_cell_voltage) {
value /= batteryCellCount;
}
sendIbusMeasurement(address, value);
break;
case IBUS_SENSOR_TYPE_TEMPERATURE:
#ifdef BARO
value = (baro.baroTemperature + 5) / 10; // +5 to make integer division rounding correct
#else
value = telemTemperature1 * 10;
#endif
sendIbusMeasurement(address, value + IBUS_TEMPERATURE_OFFSET);
break;
case IBUS_SENSOR_TYPE_RPM:
sendIbusMeasurement(address, (uint16_t) rcCommand[THROTTLE]);
break;
}
}
static void autodetectFirstReceivedAddressAsBaseAddress(ibusAddress_t returnAddress)
{
if ((INVALID_IBUS_ADDRESS == ibusBaseAddress) &&
(INVALID_IBUS_ADDRESS != returnAddress)) {
ibusBaseAddress = returnAddress;
}
}
static bool theAddressIsWithinOurRange(ibusAddress_t returnAddress)
{
return (returnAddress >= ibusBaseAddress) &&
(ibusAddress_t)(returnAddress - ibusBaseAddress) < ARRAYLEN(sensorAddressTypeLookup);
}
static void respondToIbusRequest(uint8_t ibusPacket[static IBUS_RX_BUF_LEN])
{
ibusAddress_t returnAddress = getAddress(ibusPacket);
autodetectFirstReceivedAddressAsBaseAddress(returnAddress);
if (theAddressIsWithinOurRange(returnAddress)) {
if (isCommand(IBUS_COMMAND_DISCOVER_SENSOR, ibusPacket)) {
sendIbusDiscoverSensorReply(returnAddress);
} else if (isCommand(IBUS_COMMAND_SENSOR_TYPE, ibusPacket)) {
sendIbusSensorType(returnAddress);
} else if (isCommand(IBUS_COMMAND_MEASUREMENT, ibusPacket)) {
dispatchMeasurementReply(returnAddress);
}
}
}
static void pushOntoTail(uint8_t buffer[IBUS_MIN_LEN], size_t bufferLength, uint8_t value)
{
memmove(buffer, buffer + 1, bufferLength - 1);
ibusReceiveBuffer[bufferLength - 1] = value;
}
void initIbusTelemetry(void)
{
ibusSerialPortConfig = findSerialPortConfig(FUNCTION_TELEMETRY_IBUS);
ibusPortSharing = determinePortSharing(ibusSerialPortConfig, FUNCTION_TELEMETRY_IBUS);
ibusBaseAddress = INVALID_IBUS_ADDRESS;
}
void handleIbusTelemetry(void)
{
if (!ibusTelemetryEnabled) {
return;
}
while (serialRxBytesWaiting(ibusSerialPort) > 0) {
uint8_t c = serialRead(ibusSerialPort);
if (outboundBytesToIgnoreOnRxCount) {
outboundBytesToIgnoreOnRxCount--;
continue;
}
pushOntoTail(ibusReceiveBuffer, IBUS_RX_BUF_LEN, c);
if (isChecksumOk(ibusReceiveBuffer)) {
respondToIbusRequest(ibusReceiveBuffer);
}
}
}
bool checkIbusTelemetryState(void)
{
bool newTelemetryEnabledValue = telemetryDetermineEnabledState(ibusPortSharing);
if (newTelemetryEnabledValue == ibusTelemetryEnabled) {
return false;
}
if (newTelemetryEnabledValue) {
rescheduleTask(TASK_TELEMETRY, IBUS_TASK_PERIOD_US);
configureIbusTelemetryPort();
} else {
freeIbusTelemetryPort();
}
return true;
}
void configureIbusTelemetryPort(void)
{
portOptions_t portOptions;
if (!ibusSerialPortConfig) {
return;
}
portOptions = SERIAL_BIDIR;
ibusSerialPort = openSerialPort(ibusSerialPortConfig->identifier, FUNCTION_TELEMETRY_IBUS, NULL, IBUS_BAUDRATE,
IBUS_UART_MODE, portOptions);
if (!ibusSerialPort) {
return;
}
ibusTelemetryEnabled = true;
outboundBytesToIgnoreOnRxCount = 0;
}
void freeIbusTelemetryPort(void)
{
closeSerialPort(ibusSerialPort);
ibusSerialPort = NULL;
ibusTelemetryEnabled = false;
}
#endif

34
src/main/telemetry/ibus.h Normal file
View file

@ -0,0 +1,34 @@
/*
* 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
/*
typedef struct ibusTelemetryConfig_s {
uint8_t report_cell_voltage; // report vbatt divided with cellcount
} ibusTelemetryConfig_t;
PG_DECLARE(ibusTelemetryConfig_t, ibusTelemetryConfig);
*/
void initIbusTelemetry(void);
void handleIbusTelemetry(void);
bool checkIbusTelemetryState(void);
void configureIbusTelemetryPort(void);
void freeIbusTelemetryPort(void);

View file

@ -48,6 +48,7 @@
#include "telemetry/mavlink.h"
#include "telemetry/crsf.h"
#include "telemetry/srxl.h"
#include "telemetry/ibus.h"
static telemetryConfig_t *telemetryConfig;
@ -82,6 +83,9 @@ void telemetryInit(void)
#ifdef TELEMETRY_SRXL
initSrxlTelemetry();
#endif
#ifdef TELEMETRY_IBUS
initIbusTelemetry();
#endif
telemetryCheckState();
}

View file

@ -46,6 +46,7 @@ typedef struct telemetryConfig_s {
uint8_t frsky_vfas_cell_voltage;
uint8_t hottAlarmSoundInterval;
uint8_t pidValuesAsTelemetry;
uint8_t report_cell_voltage;
} telemetryConfig_t;
void telemetryInit(void);