diff --git a/Makefile b/Makefile index d97613b3e4..b8c891ef62 100644 --- a/Makefile +++ b/Makefile @@ -664,7 +664,7 @@ all: $(VALID_TARGETS) $(VALID_TARGETS): echo "" && \ echo "Building $@" && \ - $(MAKE) -j binary hex TARGET=$@ && \ + $(MAKE) binary hex TARGET=$@ && \ echo "Building $@ succeeded." @@ -712,8 +712,11 @@ st-flash_$(TARGET): $(TARGET_BIN) ## st-flash : flash firmware (.bin) onto flight controller st-flash: st-flash_$(TARGET) -binary: $(TARGET_BIN) -hex: $(TARGET_HEX) +binary: + $(MAKE) -j $(TARGET_BIN) + +hex: + $(MAKE) -j $(TARGET_HEX) unbrick_$(TARGET): $(TARGET_HEX) stty -F $(SERIAL_DEVICE) raw speed 115200 -crtscts cs8 -parenb -cstopb -ixon diff --git a/src/main/config/config.c b/src/main/config/config.c index 2c0c9c17d9..284c59097b 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -659,7 +659,6 @@ static void resetConf(void) masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL; #endif #ifdef ALIENFLIGHTF3 - masterConfig.batteryConfig.vbatscale = 20; masterConfig.mag_hardware = MAG_NONE; // disabled by default #endif masterConfig.rxConfig.serialrx_provider = SERIALRX_SPEKTRUM2048; @@ -687,8 +686,6 @@ static void resetConf(void) #if defined(SINGULARITY) // alternative defaults settings for SINGULARITY target - masterConfig.batteryConfig.vbatscale = 77; - masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL; #endif diff --git a/src/main/drivers/accgyro_lsm303dlhc.c b/src/main/drivers/accgyro_lsm303dlhc.c index 7a7222d4d6..4caed30911 100644 --- a/src/main/drivers/accgyro_lsm303dlhc.c +++ b/src/main/drivers/accgyro_lsm303dlhc.c @@ -26,7 +26,7 @@ #include "common/axis.h" #include "system.h" -#include "gpio.h" +#include "io.h" #include "bus_i2c.h" #include "sensor.h" diff --git a/src/main/drivers/accgyro_lsm303dlhc.h b/src/main/drivers/accgyro_lsm303dlhc.h index 6cc79066cc..1f5c56f68e 100644 --- a/src/main/drivers/accgyro_lsm303dlhc.h +++ b/src/main/drivers/accgyro_lsm303dlhc.h @@ -22,8 +22,7 @@ */ /* LSM303DLHC ACC struct */ -typedef struct -{ +typedef struct { uint8_t Power_Mode; /* Power-down/Normal Mode */ uint8_t AccOutput_DataRate; /* OUT data rate */ uint8_t Axes_Enable; /* Axes enable */ @@ -31,25 +30,23 @@ typedef struct uint8_t BlockData_Update; /* Block Data Update */ uint8_t Endianness; /* Endian Data selection */ uint8_t AccFull_Scale; /* Full Scale selection */ -}LSM303DLHCAcc_InitTypeDef; +} LSM303DLHCAcc_InitTypeDef; /* LSM303DLHC Acc High Pass Filter struct */ -typedef struct -{ +typedef struct { uint8_t HighPassFilter_Mode_Selection; /* Internal filter mode */ uint8_t HighPassFilter_CutOff_Frequency; /* High pass filter cut-off frequency */ uint8_t HighPassFilter_AOI1; /* HPF_enabling/disabling for AOI function on interrupt 1 */ uint8_t HighPassFilter_AOI2; /* HPF_enabling/disabling for AOI function on interrupt 2 */ -}LSM303DLHCAcc_FilterConfigTypeDef; +} LSM303DLHCAcc_FilterConfigTypeDef; /* LSM303DLHC Mag struct */ -typedef struct -{ +typedef struct { uint8_t Temperature_Sensor; /* Temperature sensor enable/disable */ uint8_t MagOutput_DataRate; /* OUT data rate */ uint8_t Working_Mode; /* operating mode */ uint8_t MagFull_Scale; /* Full Scale selection */ -}LSM303DLHCMag_InitTypeDef; +} LSM303DLHCMag_InitTypeDef; /** * @} */ @@ -78,43 +75,11 @@ typedef struct * @brief LSM303DLHC I2C Interface pins */ #define LSM303DLHC_I2C I2C1 -#define LSM303DLHC_I2C_CLK RCC_APB1Periph_I2C1 - -#define LSM303DLHC_I2C_SCK_PIN GPIO_Pin_6 /* PB.06 */ -#define LSM303DLHC_I2C_SCK_GPIO_PORT GPIOB /* GPIOB */ -#define LSM303DLHC_I2C_SCK_GPIO_CLK RCC_AHBPeriph_GPIOB -#define LSM303DLHC_I2C_SCK_SOURCE GPIO_PinSource6 -#define LSM303DLHC_I2C_SCK_AF GPIO_AF_4 - -#define LSM303DLHC_I2C_SDA_PIN GPIO_Pin_7 /* PB.7 */ -#define LSM303DLHC_I2C_SDA_GPIO_PORT GPIOB /* GPIOB */ -#define LSM303DLHC_I2C_SDA_GPIO_CLK RCC_AHBPeriph_GPIOB -#define LSM303DLHC_I2C_SDA_SOURCE GPIO_PinSource7 -#define LSM303DLHC_I2C_SDA_AF GPIO_AF_4 - -#define LSM303DLHC_DRDY_PIN GPIO_Pin_2 /* PE.02 */ -#define LSM303DLHC_DRDY_GPIO_PORT GPIOE /* GPIOE */ -#define LSM303DLHC_DRDY_GPIO_CLK RCC_AHBPeriph_GPIOE -#define LSM303DLHC_DRDY_EXTI_LINE EXTI_Line2 -#define LSM303DLHC_DRDY_EXTI_PORT_SOURCE EXTI_PortSourceGPIOE -#define LSM303DLHC_DRDY_EXTI_PIN_SOURCE EXTI_PinSource2 -#define LSM303DLHC_DRDY_EXTI_IRQn EXTI2_TS_IRQn - -#define LSM303DLHC_I2C_INT1_PIN GPIO_Pin_4 /* PE.04 */ -#define LSM303DLHC_I2C_INT1_GPIO_PORT GPIOE /* GPIOE */ -#define LSM303DLHC_I2C_INT1_GPIO_CLK RCC_AHBPeriph_GPIOE -#define LSM303DLHC_I2C_INT1_EXTI_LINE EXTI_Line4 -#define LSM303DLHC_I2C_INT1_EXTI_PORT_SOURCE EXTI_PortSourceGPIOE -#define LSM303DLHC_I2C_INT1_EXTI_PIN_SOURCE EXTI_PinSource4 -#define LSM303DLHC_I2C_INT1_EXTI_IRQn EXTI4_IRQn - -#define LSM303DLHC_I2C_INT2_PIN GPIO_Pin_5 /* PE.05 */ -#define LSM303DLHC_I2C_INT2_GPIO_PORT GPIOE /* GPIOE */ -#define LSM303DLHC_I2C_INT2_GPIO_CLK RCC_AHBPeriph_GPIOE -#define LSM303DLHC_I2C_INT2_EXTI_LINE EXTI_Line5 -#define LSM303DLHC_I2C_INT2_EXTI_PORT_SOURCE EXTI_PortSourceGPIOE -#define LSM303DLHC_I2C_INT2_EXTI_PIN_SOURCE EXTI_PinSource5ss -#define LSM303DLHC_I2C_INT2_EXTI_IRQn EXTI9_5_IRQn +#define LSM303DLHC_I2C_SCK_PIN PB6 /* PB.06 */ +#define LSM303DLHC_I2C_SDA_PIN PB7 /* PB.7 */ +#define LSM303DLHC_DRDY_PIN PE2 /* PE.02 */ +#define LSM303DLHC_I2C_INT1_PIN PE4 /* PE.04 */ +#define LSM303DLHC_I2C_INT2_PIN PE5 /* PE.05 */ /******************************************************************************/ /*************************** START REGISTER MAPPING **************************/ diff --git a/src/main/drivers/accgyro_mpu.h b/src/main/drivers/accgyro_mpu.h index 4ce6b552de..917c06f1a8 100644 --- a/src/main/drivers/accgyro_mpu.h +++ b/src/main/drivers/accgyro_mpu.h @@ -17,6 +17,8 @@ #pragma once +#include "exti.h" + // MPU6050 #define MPU_RA_WHO_AM_I 0x75 #define MPU_RA_WHO_AM_I_LEGACY 0x00 diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c index d4fa0fb846..3cc3ac8f69 100644 --- a/src/main/drivers/accgyro_mpu6050.c +++ b/src/main/drivers/accgyro_mpu6050.c @@ -38,8 +38,6 @@ #include "accgyro_mpu.h" #include "accgyro_mpu6050.h" -extern uint8_t mpuLowPassFilter; - //#define DEBUG_MPU_DATA_READY_INTERRUPT // MPU6050, Standard address 0x68 diff --git a/src/main/drivers/adc.h b/src/main/drivers/adc.h index f090c8fa8d..55914a3365 100644 --- a/src/main/drivers/adc.h +++ b/src/main/drivers/adc.h @@ -17,6 +17,8 @@ #pragma once +#include "io.h" + typedef enum { ADC_BATTERY = 0, ADC_RSSI = 1, @@ -28,6 +30,7 @@ typedef enum { #define ADC_CHANNEL_COUNT (ADC_CHANNEL_MAX + 1) typedef struct adc_config_s { + ioTag_t tag; uint8_t adcChannel; // ADC1_INxx channel number uint8_t dmaIndex; // index into DMA buffer in case of sparse channels bool enabled; diff --git a/src/main/drivers/adc_stm32f10x.c b/src/main/drivers/adc_stm32f10x.c index a810ac731b..9d94742468 100644 --- a/src/main/drivers/adc_stm32f10x.c +++ b/src/main/drivers/adc_stm32f10x.c @@ -91,60 +91,50 @@ void adcInit(drv_adc_config_t *init) #ifdef VBAT_ADC_PIN if (init->enableVBat) { - IOInit(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), IO_CONFIG(GPIO_Mode_AIN, 0)); - adcConfig[ADC_BATTERY].adcChannel = adcChannelByTag(IO_TAG(VBAT_ADC_PIN)); - adcConfig[ADC_BATTERY].dmaIndex = configuredAdcChannels++; - adcConfig[ADC_BATTERY].enabled = true; - adcConfig[ADC_BATTERY].sampleTime = ADC_SampleTime_239Cycles5; + adcConfig[ADC_BATTERY].tag = IO_TAG(VBAT_ADC_PIN); } #endif #ifdef RSSI_ADC_PIN if (init->enableRSSI) { - IOInit(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), IO_CONFIG(GPIO_Mode_AIN, 0)); - adcConfig[ADC_RSSI].adcChannel = adcChannelByTag(IO_TAG(RSSI_ADC_PIN)); - adcConfig[ADC_RSSI].dmaIndex = configuredAdcChannels++; - adcConfig[ADC_RSSI].enabled = true; - adcConfig[ADC_RSSI].sampleTime = ADC_SampleTime_239Cycles5; + adcConfig[ADC_RSSI].tag = IO_TAG(RSSI_ADC_PIN); } #endif #ifdef EXTERNAL1_ADC_PIN if (init->enableExternal1) { - IOInit(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), IO_CONFIG(GPIO_Mode_AIN, 0)); - adcConfig[ADC_EXTERNAL1].adcChannel = adcChannelByTag(IO_TAG(EXTERNAL1_ADC_PIN)); - adcConfig[ADC_EXTERNAL1].dmaIndex = configuredAdcChannels++; - adcConfig[ADC_EXTERNAL1].enabled = true; - adcConfig[ADC_EXTERNAL1].sampleTime = ADC_SampleTime_239Cycles5; + adcConfig[ADC_EXTERNAL1].tag = IO_TAG(EXTERNAL1_ADC_PIN); } #endif #ifdef CURRENT_METER_ADC_PIN if (init->enableCurrentMeter) { - IOInit(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), IO_CONFIG(GPIO_Mode_AIN, 0)); - adcConfig[ADC_CURRENT].adcChannel = adcChannelByTag(IO_TAG(CURRENT_METER_ADC_PIN)); - adcConfig[ADC_CURRENT].dmaIndex = configuredAdcChannels++; - adcConfig[ADC_CURRENT].enabled = true; - adcConfig[ADC_CURRENT].sampleTime = ADC_SampleTime_239Cycles5; + adcConfig[ADC_CURRENT].tag = IO_TAG(CURRENT_METER_ADC_PIN); } #endif ADCDevice device = adcDeviceByInstance(ADC_INSTANCE); if (device == ADCINVALID) return; - + adcDevice_t adc = adcHardware[device]; + + for (uint8_t i = 0; i < ADC_CHANNEL_COUNT; i++) { + if (!adcConfig[i].tag) + continue; + + IOInit(IOGetByTag(adcConfig[i].tag), OWNER_SYSTEM, RESOURCE_ADC); + IOConfigGPIO(IOGetByTag(adcConfig[i].tag), IO_CONFIG(GPIO_Mode_AIN, 0)); + adcConfig[i].adcChannel = adcChannelByTag(adcConfig[i].tag); + adcConfig[i].dmaIndex = configuredAdcChannels++; + adcConfig[i].sampleTime = ADC_SampleTime_239Cycles5; + adcConfig[i].enabled = true; + } RCC_ADCCLKConfig(RCC_PCLK2_Div8); // 9MHz from 72MHz APB2 clock(HSE), 8MHz from 64MHz (HSI) RCC_ClockCmd(adc.rccADC, ENABLE); RCC_ClockCmd(adc.rccDMA, ENABLE); - // FIXME ADC driver assumes all the GPIO was already placed in 'AIN' mode - DMA_DeInit(adc.DMAy_Channelx); DMA_InitTypeDef DMA_InitStructure; DMA_StructInit(&DMA_InitStructure); diff --git a/src/main/drivers/adc_stm32f30x.c b/src/main/drivers/adc_stm32f30x.c index 8c2ccba5ec..3fc7427b5a 100644 --- a/src/main/drivers/adc_stm32f30x.c +++ b/src/main/drivers/adc_stm32f30x.c @@ -107,53 +107,25 @@ void adcInit(drv_adc_config_t *init) #ifdef VBAT_ADC_PIN if (init->enableVBat) { - IOInit(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); - - adcConfig[ADC_BATTERY].adcChannel = adcChannelByTag(IO_TAG(VBAT_ADC_PIN)); - adcConfig[ADC_BATTERY].dmaIndex = adcChannelCount; - adcConfig[ADC_BATTERY].sampleTime = ADC_SampleTime_601Cycles5; - adcConfig[ADC_BATTERY].enabled = true; - adcChannelCount++; + adcConfig[ADC_BATTERY].tag = IO_TAG(VBAT_ADC_PIN); } #endif #ifdef RSSI_ADC_PIN if (init->enableRSSI) { - IOInit(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); - - adcConfig[ADC_RSSI].adcChannel = adcChannelByTag(IO_TAG(RSSI_ADC_PIN)); - adcConfig[ADC_RSSI].dmaIndex = adcChannelCount; - adcConfig[ADC_RSSI].sampleTime = ADC_SampleTime_601Cycles5; - adcConfig[ADC_RSSI].enabled = true; - adcChannelCount++; + adcConfig[ADC_RSSI].tag = IO_TAG(RSSI_ADC_PIN); } #endif #ifdef CURRENT_METER_ADC_GPIO if (init->enableCurrentMeter) { - IOInit(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); - - adcConfig[ADC_CURRENT].adcChannel = adcChannelByTag(IO_TAG(CURRENT_METER_ADC_PIN)); - adcConfig[ADC_CURRENT].dmaIndex = adcChannelCount; - adcConfig[ADC_CURRENT].sampleTime = ADC_SampleTime_601Cycles5; - adcConfig[ADC_CURRENT].enabled = true; - adcChannelCount++; + adcConfig[ADC_CURRENT].tag = IO_TAG(CURRENT_METER_ADC_PIN); } #endif #ifdef EXTERNAL1_ADC_GPIO if (init->enableExternal1) { - IOInit(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); - - adcConfig[ADC_EXTERNAL1].adcChannel = adcChannelByTag(IO_TAG(EXTERNAL1_ADC_PIN)); - adcConfig[ADC_EXTERNAL1].dmaIndex = adcChannelCount; - adcConfig[ADC_EXTERNAL1].sampleTime = ADC_SampleTime_601Cycles5; - adcConfig[ADC_EXTERNAL1].enabled = true; - adcChannelCount++; + adcConfig[ADC_EXTERNAL1].tag = IO_TAG(EXTERNAL1_ADC_PIN); } #endif @@ -163,6 +135,18 @@ void adcInit(drv_adc_config_t *init) adcDevice_t adc = adcHardware[device]; + for (uint8_t i = 0; i < ADC_CHANNEL_COUNT; i++) { + if (!adcConfig[i].tag) + continue; + + IOInit(IOGetByTag(adcConfig[i].tag), OWNER_SYSTEM, RESOURCE_ADC); + IOConfigGPIO(IOGetByTag(adcConfig[i].tag), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); + adcConfig[i].adcChannel = adcChannelByTag(adcConfig[i].tag); + adcConfig[i].dmaIndex = adcChannelCount++; + adcConfig[i].sampleTime = ADC_SampleTime_601Cycles5; + adcConfig[i].enabled = true; + } + RCC_ADCCLKConfig(RCC_ADC12PLLCLK_Div256); // 72 MHz divided by 256 = 281.25 kHz RCC_ClockCmd(adc.rccADC, ENABLE); RCC_ClockCmd(adc.rccDMA, ENABLE); diff --git a/src/main/drivers/adc_stm32f4xx.c b/src/main/drivers/adc_stm32f4xx.c index e4c086a609..e0b36a5f33 100644 --- a/src/main/drivers/adc_stm32f4xx.c +++ b/src/main/drivers/adc_stm32f4xx.c @@ -100,45 +100,25 @@ void adcInit(drv_adc_config_t *init) #ifdef VBAT_ADC_PIN if (init->enableVBat) { - IOInit(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); - adcConfig[ADC_BATTERY].adcChannel = adcChannelByTag(IO_TAG(VBAT_ADC_PIN)); //VBAT_ADC_CHANNEL; - adcConfig[ADC_BATTERY].dmaIndex = configuredAdcChannels++; - adcConfig[ADC_BATTERY].enabled = true; - adcConfig[ADC_BATTERY].sampleTime = ADC_SampleTime_480Cycles; - } -#endif - -#ifdef EXTERNAL1_ADC_PIN - if (init->enableExternal1) { - IOInit(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); - adcConfig[ADC_EXTERNAL1].adcChannel = adcChannelByTag(IO_TAG(EXTERNAL1_ADC_PIN)); //EXTERNAL1_ADC_CHANNEL; - adcConfig[ADC_EXTERNAL1].dmaIndex = configuredAdcChannels++; - adcConfig[ADC_EXTERNAL1].enabled = true; - adcConfig[ADC_EXTERNAL1].sampleTime = ADC_SampleTime_480Cycles; + adcConfig[ADC_BATTERY].tag = IO_TAG(VBAT_ADC_PIN); //VBAT_ADC_CHANNEL; } #endif #ifdef RSSI_ADC_PIN if (init->enableRSSI) { - IOInit(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); - adcConfig[ADC_RSSI].adcChannel = adcChannelByTag(IO_TAG(RSSI_ADC_PIN)); //RSSI_ADC_CHANNEL; - adcConfig[ADC_RSSI].dmaIndex = configuredAdcChannels++; - adcConfig[ADC_RSSI].enabled = true; - adcConfig[ADC_RSSI].sampleTime = ADC_SampleTime_480Cycles; + adcConfig[ADC_RSSI].tag = IO_TAG(RSSI_ADC_PIN); //RSSI_ADC_CHANNEL; + } +#endif + +#ifdef EXTERNAL1_ADC_PIN + if (init->enableExternal1) { + adcConfig[ADC_EXTERNAL1].tag = IO_TAG(EXTERNAL1_ADC_PIN); //EXTERNAL1_ADC_CHANNEL; } #endif #ifdef CURRENT_METER_ADC_PIN if (init->enableCurrentMeter) { - IOInit(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); - adcConfig[ADC_CURRENT].adcChannel = adcChannelByTag(IO_TAG(CURRENT_METER_ADC_PIN)); //CURRENT_METER_ADC_CHANNEL; - adcConfig[ADC_CURRENT].dmaIndex = configuredAdcChannels++; - adcConfig[ADC_CURRENT].enabled = true; - adcConfig[ADC_CURRENT].sampleTime = ADC_SampleTime_480Cycles; + adcConfig[ADC_CURRENT].tag = IO_TAG(CURRENT_METER_ADC_PIN); //CURRENT_METER_ADC_CHANNEL; } #endif @@ -150,6 +130,18 @@ void adcInit(drv_adc_config_t *init) adcDevice_t adc = adcHardware[device]; + for (uint8_t i = 0; i < ADC_CHANNEL_COUNT; i++) { + if (!adcConfig[i].tag) + continue; + + IOInit(IOGetByTag(adcConfig[i].tag), OWNER_SYSTEM, RESOURCE_ADC); + IOConfigGPIO(IOGetByTag(adcConfig[i].tag), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); + adcConfig[i].adcChannel = adcChannelByTag(adcConfig[i].tag); + adcConfig[i].dmaIndex = configuredAdcChannels++; + adcConfig[i].sampleTime = ADC_SampleTime_480Cycles; + adcConfig[i].enabled = true; + } + RCC_ClockCmd(adc.rccDMA, ENABLE); RCC_ClockCmd(adc.rccADC, ENABLE); @@ -174,20 +166,20 @@ void adcInit(drv_adc_config_t *init) ADC_CommonInitTypeDef ADC_CommonInitStructure; ADC_CommonStructInit(&ADC_CommonInitStructure); - ADC_CommonInitStructure.ADC_Mode = ADC_Mode_Independent; - ADC_CommonInitStructure.ADC_Prescaler = ADC_Prescaler_Div8; - ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_Disabled; + ADC_CommonInitStructure.ADC_Mode = ADC_Mode_Independent; + ADC_CommonInitStructure.ADC_Prescaler = ADC_Prescaler_Div8; + ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_Disabled; ADC_CommonInitStructure.ADC_TwoSamplingDelay = ADC_TwoSamplingDelay_5Cycles; ADC_CommonInit(&ADC_CommonInitStructure); ADC_StructInit(&ADC_InitStructure); - ADC_InitStructure.ADC_ContinuousConvMode = ENABLE; - ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b; + ADC_InitStructure.ADC_ContinuousConvMode = ENABLE; + ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b; ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_T1_CC1; ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None; - ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; - ADC_InitStructure.ADC_NbrOfConversion = configuredAdcChannels; + ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; + ADC_InitStructure.ADC_NbrOfConversion = configuredAdcChannels; ADC_InitStructure.ADC_ScanConvMode = configuredAdcChannels > 1 ? ENABLE : DISABLE; // 1=scan more that one channel in group ADC_Init(adc.ADCx, &ADC_InitStructure); diff --git a/src/main/drivers/bus_i2c_stm32f30x.c b/src/main/drivers/bus_i2c_stm32f30x.c index adb424390b..cc78da8d60 100644 --- a/src/main/drivers/bus_i2c_stm32f30x.c +++ b/src/main/drivers/bus_i2c_stm32f30x.c @@ -31,6 +31,15 @@ #ifndef SOFT_I2C +#if defined(USE_I2C_PULLUP) +#define IOCFG_I2C IO_CONFIG(GPIO_Mode_AF, 0, GPIO_OType_OD, GPIO_PuPd_UP) +#else +#define IOCFG_I2C IOCFG_AF_OD +#endif + +#define I2C_HIGHSPEED_TIMING 0x00500E30 // 1000 Khz, 72Mhz Clock, Analog Filter Delay ON, Setup 40, Hold 4. +#define I2C_STANDARD_TIMING 0x00E0257A // 400 Khz, 72Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10. + #define I2C_SHORT_TIMEOUT ((uint32_t)0x1000) #define I2C_LONG_TIMEOUT ((uint32_t)(10 * I2C_SHORT_TIMEOUT)) #define I2C_GPIO_AF GPIO_AF_4 @@ -83,8 +92,8 @@ void i2cInit(I2CDevice device) RCC_ClockCmd(i2c->rcc, ENABLE); RCC_I2CCLKConfig(I2Cx == I2C2 ? RCC_I2C2CLK_SYSCLK : RCC_I2C1CLK_SYSCLK); - IOConfigGPIOAF(scl, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_UP), GPIO_AF_4); - IOConfigGPIOAF(sda, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_UP), GPIO_AF_4); + IOConfigGPIOAF(scl, IOCFG_I2C, GPIO_AF_4); + IOConfigGPIOAF(sda, IOCFG_I2C, GPIO_AF_4); I2C_InitTypeDef i2cInit = { .I2C_Mode = I2C_Mode_I2C, @@ -93,10 +102,7 @@ void i2cInit(I2CDevice device) .I2C_OwnAddress1 = 0x00, .I2C_Ack = I2C_Ack_Enable, .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, - .I2C_Timing = i2c->overClock ? - 0x00500E30 : // 1000 Khz, 72Mhz Clock, Analog Filter Delay ON, Setup 40, Hold 4. - 0x00E0257A, // 400 Khz, 72Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10. - //.I2C_Timing = 0x8000050B; + .I2C_Timing = (i2c->overClock ? I2C_HIGHSPEED_TIMING : I2C_STANDARD_TIMING) }; I2C_Init(I2Cx, &i2cInit); diff --git a/src/main/drivers/compass_ak8963.c b/src/main/drivers/compass_ak8963.c index 7b60fe5c46..0668fcdd77 100644 --- a/src/main/drivers/compass_ak8963.c +++ b/src/main/drivers/compass_ak8963.c @@ -30,8 +30,6 @@ #include "common/maths.h" #include "system.h" -#include "gpio.h" -#include "exti.h" #include "bus_i2c.h" #include "bus_spi.h" diff --git a/src/main/drivers/compass_hmc5883l.c b/src/main/drivers/compass_hmc5883l.c index a018ab0f52..c8c398fc3d 100644 --- a/src/main/drivers/compass_hmc5883l.c +++ b/src/main/drivers/compass_hmc5883l.c @@ -29,7 +29,8 @@ #include "system.h" #include "nvic.h" -#include "gpio.h" +#include "io.h" +#include "exti.h" #include "bus_i2c.h" #include "light_led.h" @@ -120,14 +121,14 @@ static float magGain[3] = { 1.0f, 1.0f, 1.0f }; static const hmc5883Config_t *hmc5883Config = NULL; -void MAG_DATA_READY_EXTI_Handler(void) +#ifdef USE_MAG_DATA_READY_SIGNAL + +static IO_t intIO; +static extiCallbackRec_t hmc5883_extiCallbackRec; + +void hmc5883_extiHandler(extiCallbackRec_t* cb) { - if (EXTI_GetITStatus(hmc5883Config->exti_line) == RESET) { - return; - } - - EXTI_ClearITPendingBit(hmc5883Config->exti_line); - + UNUSED(cb); #ifdef DEBUG_MAG_DATA_READY_INTERRUPT // Measure the delta between calls to the interrupt handler // currently should be around 65/66 milli seconds / 15hz output rate @@ -143,57 +144,26 @@ void MAG_DATA_READY_EXTI_Handler(void) lastCalledAt = now; #endif } +#endif static void hmc5883lConfigureDataReadyInterruptHandling(void) { #ifdef USE_MAG_DATA_READY_SIGNAL - if (!(hmc5883Config->exti_port_source && hmc5883Config->exti_pin_source)) { + if (!(hmc5883Config->intTag)) { return; } -#ifdef STM32F10X - // enable AFIO for EXTI support - RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE); -#endif - -#ifdef STM32F303xC - /* Enable SYSCFG clock otherwise the EXTI irq handlers are not called */ - RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE); -#endif - -#ifdef STM32F10X - gpioExtiLineConfig(hmc5883Config->exti_port_source, hmc5883Config->exti_pin_source); -#endif - -#ifdef STM32F303xC - gpioExtiLineConfig(hmc5883Config->exti_port_source, hmc5883Config->exti_pin_source); -#endif - + intIO = IOGetByTag(hmc5883Config->intTag); #ifdef ENSURE_MAG_DATA_READY_IS_HIGH - uint8_t status = GPIO_ReadInputDataBit(hmc5883Config->gpioPort, hmc5883Config->gpioPin); + uint8_t status = IORead(intIO); if (!status) { return; } #endif - registerExtiCallbackHandler(hmc5883Config->exti_irqn, MAG_DATA_READY_EXTI_Handler); - - EXTI_ClearITPendingBit(hmc5883Config->exti_line); - - EXTI_InitTypeDef EXTIInit; - EXTIInit.EXTI_Line = hmc5883Config->exti_line; - EXTIInit.EXTI_Mode = EXTI_Mode_Interrupt; - EXTIInit.EXTI_Trigger = EXTI_Trigger_Falling; - EXTIInit.EXTI_LineCmd = ENABLE; - EXTI_Init(&EXTIInit); - - NVIC_InitTypeDef NVIC_InitStructure; - - NVIC_InitStructure.NVIC_IRQChannel = hmc5883Config->exti_irqn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_MAG_DATA_READY); - NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_MAG_DATA_READY); - NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; - NVIC_Init(&NVIC_InitStructure); + EXTIHandlerInit(&hmc5883_extiCallbackRec, hmc5883_extiHandler); + EXTIConfig(intIO, &hmc5883_extiCallbackRec, NVIC_PRIO_MAG_INT_EXTI, EXTI_Trigger_Rising); + EXTIEnable(intIO, true); #endif } @@ -221,25 +191,6 @@ void hmc5883lInit(void) int32_t xyz_total[3] = { 0, 0, 0 }; // 32 bit totals so they won't overflow. bool bret = true; // Error indicator - gpio_config_t gpio; - - if (hmc5883Config) { -#ifdef STM32F303 - if (hmc5883Config->gpioAHBPeripherals) { - RCC_AHBPeriphClockCmd(hmc5883Config->gpioAHBPeripherals, ENABLE); - } -#endif -#ifdef STM32F10X - if (hmc5883Config->gpioAPB2Peripherals) { - RCC_APB2PeriphClockCmd(hmc5883Config->gpioAPB2Peripherals, ENABLE); - } -#endif - gpio.pin = hmc5883Config->gpioPin; - gpio.speed = Speed_2MHz; - gpio.mode = Mode_IN_FLOATING; - gpioInit(hmc5883Config->gpioPort, &gpio); - } - delay(50); i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to pos bias // Note that the very first measurement after a gain change maintains the same gain as the previous setting. diff --git a/src/main/drivers/compass_hmc5883l.h b/src/main/drivers/compass_hmc5883l.h index 53c4c9f3e5..035a2c936f 100644 --- a/src/main/drivers/compass_hmc5883l.h +++ b/src/main/drivers/compass_hmc5883l.h @@ -17,20 +17,10 @@ #pragma once -typedef struct hmc5883Config_s { -#ifdef STM32F303 - uint32_t gpioAHBPeripherals; -#endif -#ifdef STM32F10X - uint32_t gpioAPB2Peripherals; -#endif - uint16_t gpioPin; - GPIO_TypeDef *gpioPort; +#include "io.h" - uint8_t exti_port_source; - uint32_t exti_line; - uint8_t exti_pin_source; - IRQn_Type exti_irqn; +typedef struct hmc5883Config_s { + ioTag_t intTag; } hmc5883Config_t; bool hmc5883lDetect(mag_t* mag, const hmc5883Config_t *hmc5883ConfigToUse); diff --git a/src/main/drivers/max7456_symbols.h b/src/main/drivers/max7456_symbols.h index 68de428937..2030cb956b 100644 --- a/src/main/drivers/max7456_symbols.h +++ b/src/main/drivers/max7456_symbols.h @@ -125,16 +125,9 @@ #define SYM_MAG11 0xB6 // AH Center screen Graphics -//#define SYM_AH_CENTER 0x01 -#ifdef ALT_CENTER - #define SYM_AH_CENTER_LINE 0xB0 - #define SYM_AH_CENTER 0xB1 - #define SYM_AH_CENTER_LINE_RIGHT 0xB2 -#else - #define SYM_AH_CENTER_LINE 0x26 - #define SYM_AH_CENTER 0x7E - #define SYM_AH_CENTER_LINE_RIGHT 0xBC -#endif +#define SYM_AH_CENTER_LINE 0x26 +#define SYM_AH_CENTER_LINE_RIGHT 0x27 +#define SYM_AH_CENTER 0x7E #define SYM_AH_RIGHT 0x02 #define SYM_AH_LEFT 0x03 #define SYM_AH_DECORATION_UP 0xC9 @@ -183,7 +176,7 @@ #define SYM_FT 0x0F // Voltage and amperage -#define SYM_VOLT 0xA9 +#define SYM_VOLT 0x00 #define SYM_AMP 0x9A #define SYM_MAH 0xA4 #define SYM_WATT 0x57 @@ -214,11 +207,11 @@ #define SYM_FLY_H 0x71 // Throttle Position (%) -#define SYM_THR 0xC8 -#define SYM_THR1 0xC9 +#define SYM_THR 0x04 +#define SYM_THR1 0x05 // RSSI -#define SYM_RSSI 0xBA +#define SYM_RSSI 0x01 // Menu cursor #define SYM_CURSOR SYM_AH_LEFT diff --git a/src/main/drivers/rcc.c b/src/main/drivers/rcc.c index f8cc88b725..b1e759f972 100644 --- a/src/main/drivers/rcc.c +++ b/src/main/drivers/rcc.c @@ -7,7 +7,7 @@ void RCC_ClockCmd(rccPeriphTag_t periphTag, FunctionalState NewState) int tag = periphTag >> 5; uint32_t mask = 1 << (periphTag & 0x1f); switch (tag) { -#if defined(STM32F303xC) +#if defined(STM32F3) || defined(STM32F1) case RCC_AHB: RCC_AHBPeriphClockCmd(mask, NewState); break; @@ -18,7 +18,7 @@ void RCC_ClockCmd(rccPeriphTag_t periphTag, FunctionalState NewState) case RCC_APB1: RCC_APB1PeriphClockCmd(mask, NewState); break; -#if defined(STM32F40_41xxx) || defined(STM32F411xE) +#if defined(STM32F4) case RCC_AHB1: RCC_AHB1PeriphClockCmd(mask, NewState); break; @@ -31,7 +31,7 @@ void RCC_ResetCmd(rccPeriphTag_t periphTag, FunctionalState NewState) int tag = periphTag >> 5; uint32_t mask = 1 << (periphTag & 0x1f); switch (tag) { -#if defined(STM32F303xC) +#if defined(STM32F3) || defined(STM32F10X_CL) case RCC_AHB: RCC_AHBPeriphResetCmd(mask, NewState); break; @@ -42,7 +42,7 @@ void RCC_ResetCmd(rccPeriphTag_t periphTag, FunctionalState NewState) case RCC_APB1: RCC_APB1PeriphResetCmd(mask, NewState); break; -#if defined(STM32F40_41xxx) || defined(STM32F411xE) +#if defined(STM32F4) case RCC_AHB1: RCC_AHB1PeriphResetCmd(mask, NewState); break; diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index 9e56c2abea..dc98ae78ef 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -29,10 +29,9 @@ typedef enum { ACC_MPU6000 = 7, ACC_MPU6500 = 8, ACC_FAKE = 9, + ACC_MAX = ACC_FAKE } accelerationSensor_e; -#define ACC_MAX ACC_FAKE - extern sensor_align_e accAlign; extern acc_t acc; extern uint32_t accTargetLooptime; diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index 7ea2c1e5ba..0ef2638ad0 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -22,11 +22,11 @@ typedef enum { BARO_NONE = 1, BARO_BMP085 = 2, BARO_MS5611 = 3, - BARO_BMP280 = 4 + BARO_BMP280 = 4, + BARO_MAX = BARO_BMP280 } baroSensor_e; #define BARO_SAMPLE_COUNT_MAX 48 -#define BARO_MAX BARO_MS5611 typedef struct barometerConfig_s { uint8_t baro_sample_count; // size of baro filter array @@ -38,7 +38,6 @@ typedef struct barometerConfig_s { extern int32_t BaroAlt; extern int32_t baroTemperature; // Use temperature for telemetry -#ifdef BARO void useBarometerConfig(barometerConfig_t *barometerConfigToUse); bool isBaroCalibrationComplete(void); void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired); @@ -46,4 +45,3 @@ uint32_t baroUpdate(void); bool isBaroReady(void); int32_t baroCalculateAltitude(void); void performBaroCalibrationCycle(void); -#endif diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 26e54eb4ec..21197e07fc 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -20,7 +20,9 @@ #include "rx/rx.h" #include "common/maths.h" +#ifndef VBAT_SCALE_DEFAULT #define VBAT_SCALE_DEFAULT 110 +#endif #define VBAT_RESDIVVAL_DEFAULT 10 #define VBAT_RESDIVMULTIPLIER_DEFAULT 1 #define VBAT_SCALE_MIN 0 diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index de075bfe23..cca2328c04 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -35,7 +35,7 @@ #include "sensors/sensors.h" #include "sensors/compass.h" -#ifdef NAZE +#ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" #endif diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index 0807ba4106..2f5ae7d21a 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -23,15 +23,13 @@ typedef enum { MAG_NONE = 1, MAG_HMC5883 = 2, MAG_AK8975 = 3, - MAG_AK8963 = 4 + MAG_AK8963 = 4, + MAG_MAX = MAG_AK8963 } magSensor_e; -#define MAG_MAX MAG_AK8963 - -#ifdef MAG void compassInit(void); -void updateCompass(flightDynamicsTrims_t *magZero); -#endif +union flightDynamicsTrims_u; +void updateCompass(union flightDynamicsTrims_u *magZero); extern int32_t magADC[XYZ_AXIS_COUNT]; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 48113e934a..c79d29013f 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -41,7 +41,7 @@ sensor_align_e gyroAlign = 0; int32_t gyroADC[XYZ_AXIS_COUNT]; float gyroADCf[XYZ_AXIS_COUNT]; -static int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 }; +static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 }; static const gyroConfig_t *gyroConfig; static biquad_t gyroFilterState[XYZ_AXIS_COUNT]; static uint8_t gyroSoftLpfHz; @@ -160,5 +160,9 @@ void gyroUpdate(void) gyroADCf[axis] = applyBiQuadFilter((float)gyroADC[axis], &gyroFilterState[axis]); gyroADC[axis] = lrintf(gyroADCf[axis]); } + } else { + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + gyroADCf[axis] = gyroADC[axis]; + } } } diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 54069b8a64..2239eb6816 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -27,7 +27,8 @@ typedef enum { GYRO_MPU6000, GYRO_MPU6500, GYRO_MPU9250, - GYRO_FAKE + GYRO_FAKE, + GYRO_MAX = GYRO_FAKE } gyroSensor_e; extern gyro_t gyro; diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 80d6169518..0e75ad15f5 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -81,7 +81,7 @@ extern baro_t baro; extern acc_t acc; extern sensor_align_e gyroAlign; -uint8_t detectedSensors[MAX_SENSORS_TO_DETECT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE }; +uint8_t detectedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE }; const extiConfig_t *selectMPUIntExtiConfig(void) @@ -484,27 +484,12 @@ static void detectMag(magSensor_e magHardwareToUse) #ifdef USE_MAG_HMC5883 const hmc5883Config_t *hmc5883Config = 0; -#ifdef NAZE +#ifdef NAZE // TODO remove this target specific define static const hmc5883Config_t nazeHmc5883Config_v1_v4 = { - .gpioAPB2Peripherals = RCC_APB2Periph_GPIOB, - .gpioPin = Pin_12, - .gpioPort = GPIOB, - - /* Disabled for v4 needs more work. - .exti_port_source = GPIO_PortSourceGPIOB, - .exti_pin_source = GPIO_PinSource12, - .exti_line = EXTI_Line12, - .exti_irqn = EXTI15_10_IRQn - */ + .intTag = IO_TAG(PB12) /* perhaps disabled? */ }; static const hmc5883Config_t nazeHmc5883Config_v5 = { - .gpioAPB2Peripherals = RCC_APB2Periph_GPIOC, - .gpioPin = Pin_14, - .gpioPort = GPIOC, - .exti_port_source = GPIO_PortSourceGPIOC, - .exti_line = EXTI_Line14, - .exti_pin_source = GPIO_PinSource14, - .exti_irqn = EXTI15_10_IRQn + .intTag = IO_TAG(MAG_INT_EXTI) }; if (hardwareRevision < NAZE32_REV5) { hmc5883Config = &nazeHmc5883Config_v1_v4; @@ -513,18 +498,12 @@ static void detectMag(magSensor_e magHardwareToUse) } #endif -#ifdef SPRACINGF3 - static const hmc5883Config_t spRacingF3Hmc5883Config = { - .gpioAHBPeripherals = RCC_AHBPeriph_GPIOC, - .gpioPin = Pin_14, - .gpioPort = GPIOC, - .exti_port_source = EXTI_PortSourceGPIOC, - .exti_pin_source = EXTI_PinSource14, - .exti_line = EXTI_Line14, - .exti_irqn = EXTI15_10_IRQn +#ifdef MAG_INT_EXTI + static const hmc5883Config_t extiHmc5883Config = { + .intTag = IO_TAG(MAG_INT_EXTI) }; - hmc5883Config = &spRacingF3Hmc5883Config; + hmc5883Config = &extiHmc5883Config; #endif #endif diff --git a/src/main/sensors/sensors.h b/src/main/sensors/sensors.h index 92e6907eef..9e6fdda92f 100644 --- a/src/main/sensors/sensors.h +++ b/src/main/sensors/sensors.h @@ -21,12 +21,11 @@ typedef enum { SENSOR_INDEX_GYRO = 0, SENSOR_INDEX_ACC, SENSOR_INDEX_BARO, - SENSOR_INDEX_MAG + SENSOR_INDEX_MAG, + SENSOR_INDEX_COUNT } sensorIndex_e; -#define MAX_SENSORS_TO_DETECT (SENSOR_INDEX_MAG + 1) - -extern uint8_t detectedSensors[MAX_SENSORS_TO_DETECT]; +extern uint8_t detectedSensors[SENSOR_INDEX_COUNT]; typedef struct int16_flightDynamicsTrims_s { int16_t roll; @@ -34,7 +33,7 @@ typedef struct int16_flightDynamicsTrims_s { int16_t yaw; } flightDynamicsTrims_def_t; -typedef union { +typedef union flightDynamicsTrims_u { int16_t raw[3]; flightDynamicsTrims_def_t values; } flightDynamicsTrims_t; diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index 89e9c1a9c6..ccef02d34d 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -18,6 +18,7 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "AFF3" // AlienFlight F3. +#define ALIENFLIGHT #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT @@ -118,9 +119,7 @@ #define ADC_INSTANCE ADC2 //#define BOARD_HAS_VOLTAGE_DIVIDER #define VBAT_ADC_PIN PA4 - -// alternative defaults for AlienFlight F3 target -#define ALIENFLIGHT +#define VBAT_SCALE_DEFAULT 20 #define SPEKTRUM_BIND // USART2, PA3 diff --git a/src/main/target/KISSFC/target.c b/src/main/target/KISSFC/target.c index 30d03b9f69..db129454a4 100644 --- a/src/main/target/KISSFC/target.c +++ b/src/main/target/KISSFC/target.c @@ -7,33 +7,33 @@ const uint16_t multiPPM[] = { PWM7 | (MAP_TO_PPM_INPUT << 8), - PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), 0xFFFF }; const uint16_t multiPWM[] = { - PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), PWM7 | (MAP_TO_PWM_INPUT << 8), PWM8 | (MAP_TO_PWM_INPUT << 8), PWM9 | (MAP_TO_PWM_INPUT << 8), - PWM10 | (MAP_TO_PWM_INPUT << 8), - PWM11 | (MAP_TO_PWM_INPUT << 8), - PWM12 | (MAP_TO_PWM_INPUT << 8), + PWM10 | (MAP_TO_PWM_INPUT << 8), + PWM11 | (MAP_TO_PWM_INPUT << 8), + PWM12 | (MAP_TO_PWM_INPUT << 8), 0xFFFF }; diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h index 6d79046a74..538dc653b8 100644 --- a/src/main/target/KISSFC/target.h +++ b/src/main/target/KISSFC/target.h @@ -72,12 +72,15 @@ #define USE_I2C #define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA -//#define USE_ADC -#define ADC_INSTANCE ADC2 -#define VBAT_ADC_PIN PA4 -#define CURRENT_METER_ADC_PIN PA5 -#define RSSI_ADC_PIN PB2 +#define USE_ADC +#define VBAT_SCALE_DEFAULT 164 +#define ADC_INSTANCE ADC1 +#define VBAT_ADC_PIN PA0 +//#define CURRENT_METER_ADC_PIN PA5 +//#define RSSI_ADC_PIN PB2 +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES FEATURE_VBAT #define SPEKTRUM_BIND #define BIND_PIN PB4 diff --git a/src/main/target/KISSFC/target.mk b/src/main/target/KISSFC/target.mk index 56c635c922..6ed94fd46d 100644 --- a/src/main/target/KISSFC/target.mk +++ b/src/main/target/KISSFC/target.mk @@ -1,5 +1,5 @@ F3_TARGETS += $(TARGET) -FEATURES = VCP +FEATURES = VCP TARGET_SRC = \ drivers/accgyro_mpu.c \ diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 9f6e3d2b22..95f531faf5 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -74,7 +74,8 @@ //#define DEBUG_MAG_DATA_READY_INTERRUPT #define USE_MAG_DATA_READY_SIGNAL - +#define MAG_INT_EXTI PC14 + #define GYRO #define USE_GYRO_MPU3050 #define USE_GYRO_MPU6050 diff --git a/src/main/target/SINGULARITY/target.h b/src/main/target/SINGULARITY/target.h index 192971fd11..f58dcbe969 100644 --- a/src/main/target/SINGULARITY/target.h +++ b/src/main/target/SINGULARITY/target.h @@ -95,6 +95,7 @@ #define ADC_INSTANCE ADC2 #define VBAT_ADC_PIN PB2 +#define VBAT_SCALE_DEFAULT 77 #define LED_STRIP #define LED_STRIP_TIMER TIM1 diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 68413bbe0b..a4f44762dc 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -35,10 +35,6 @@ #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW -#define USE_MAG_DATA_READY_SIGNAL -#define ENSURE_MAG_DATA_READY_IS_HIGH - - #define GYRO #define USE_GYRO_MPU6050 #define GYRO_MPU6050_ALIGN CW270_DEG @@ -57,6 +53,11 @@ #define USE_MAG_HMC5883 #define MAG_HMC5883_ALIGN CW270_DEG +#define USE_MAG_DATA_READY_SIGNAL +#define ENSURE_MAG_DATA_READY_IS_HIGH +#define MAG_INT_EXTI PC14 + + #define USE_FLASHFS #define USE_FLASH_M25P16 diff --git a/src/main/vcpf4/usbd_desc.c b/src/main/vcpf4/usbd_desc.c index f9478327e6..f3f9a3af91 100644 --- a/src/main/vcpf4/usbd_desc.c +++ b/src/main/vcpf4/usbd_desc.c @@ -57,7 +57,7 @@ * @{ */ #define USBD_LANGID_STRING 0x409 -#define USBD_MANUFACTURER_STRING "RaceFlight" +#define USBD_MANUFACTURER_STRING "BetaFlight" #ifdef USBD_PRODUCT_STRING #define USBD_PRODUCT_HS_STRING USBD_PRODUCT_STRING