mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-17 05:15:25 +03:00
Use slightly flatter directory structure since some developers did not
like too many folders. Extracted code from some files into separate files to fit with the new layout.
This commit is contained in:
parent
39adc34278
commit
3bd4cd2ed2
84 changed files with 732 additions and 645 deletions
225
src/drivers/system_common.c
Executable file
225
src/drivers/system_common.c
Executable file
|
@ -0,0 +1,225 @@
|
|||
#include "board.h"
|
||||
|
||||
// cycles per microsecond
|
||||
static volatile uint32_t usTicks = 0;
|
||||
// current uptime for 1kHz systick timer. will rollover after 49 days. hopefully we won't care.
|
||||
static volatile uint32_t sysTickUptime = 0;
|
||||
// from system_stm32f10x.c
|
||||
void SetSysClock(void);
|
||||
#ifdef BUZZER
|
||||
void systemBeep(bool onoff);
|
||||
static void beepRev4(bool onoff);
|
||||
static void beepRev5(bool onoff);
|
||||
void (* systemBeepPtr)(bool onoff) = NULL;
|
||||
#endif
|
||||
|
||||
static void cycleCounterInit(void)
|
||||
{
|
||||
RCC_ClocksTypeDef clocks;
|
||||
RCC_GetClocksFreq(&clocks);
|
||||
usTicks = clocks.SYSCLK_Frequency / 1000000;
|
||||
}
|
||||
|
||||
// SysTick
|
||||
void SysTick_Handler(void)
|
||||
{
|
||||
sysTickUptime++;
|
||||
}
|
||||
|
||||
// Return system uptime in microseconds (rollover in 70minutes)
|
||||
uint32_t micros(void)
|
||||
{
|
||||
register uint32_t ms, cycle_cnt;
|
||||
do {
|
||||
ms = sysTickUptime;
|
||||
cycle_cnt = SysTick->VAL;
|
||||
} while (ms != sysTickUptime);
|
||||
return (ms * 1000) + (usTicks * 1000 - cycle_cnt) / usTicks;
|
||||
}
|
||||
|
||||
// Return system uptime in milliseconds (rollover in 49 days)
|
||||
uint32_t millis(void)
|
||||
{
|
||||
return sysTickUptime;
|
||||
}
|
||||
|
||||
void systemInit(void)
|
||||
{
|
||||
struct {
|
||||
GPIO_TypeDef *gpio;
|
||||
gpio_config_t cfg;
|
||||
} gpio_setup[] = {
|
||||
#ifdef LED0
|
||||
{
|
||||
.gpio = LED0_GPIO,
|
||||
.cfg = { LED0_PIN, Mode_Out_PP, Speed_2MHz }
|
||||
},
|
||||
#endif
|
||||
#ifdef LED1
|
||||
|
||||
{
|
||||
.gpio = LED1_GPIO,
|
||||
.cfg = { LED1_PIN, Mode_Out_PP, Speed_2MHz }
|
||||
},
|
||||
#endif
|
||||
#ifdef BUZZER
|
||||
{
|
||||
.gpio = BEEP_GPIO,
|
||||
.cfg = { BEEP_PIN, Mode_Out_OD, Speed_2MHz }
|
||||
},
|
||||
#endif
|
||||
};
|
||||
gpio_config_t gpio;
|
||||
uint32_t i;
|
||||
uint8_t gpio_count = sizeof(gpio_setup) / sizeof(gpio_setup[0]);
|
||||
|
||||
// Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers
|
||||
// Configure the Flash Latency cycles and enable prefetch buffer
|
||||
SetSysClock();
|
||||
|
||||
// Turn on clocks for stuff we use
|
||||
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4 | RCC_APB1Periph_I2C2, ENABLE);
|
||||
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO | RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOC | RCC_APB2Periph_TIM1 | RCC_APB2Periph_ADC1 | RCC_APB2Periph_USART1, ENABLE);
|
||||
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
|
||||
RCC_ClearFlag();
|
||||
|
||||
// Make all GPIO in by default to save power and reduce noise
|
||||
gpio.pin = Pin_All;
|
||||
gpio.mode = Mode_AIN;
|
||||
gpioInit(GPIOA, &gpio);
|
||||
gpioInit(GPIOB, &gpio);
|
||||
gpioInit(GPIOC, &gpio);
|
||||
|
||||
// Turn off JTAG port 'cause we're using the GPIO for leds
|
||||
#define AFIO_MAPR_SWJ_CFG_NO_JTAG_SW (0x2 << 24)
|
||||
AFIO->MAPR |= AFIO_MAPR_SWJ_CFG_NO_JTAG_SW;
|
||||
|
||||
#ifdef BUZZER
|
||||
// Configure gpio
|
||||
// rev5 needs inverted beeper. oops.
|
||||
if (hse_value == 12000000)
|
||||
systemBeepPtr = beepRev5;
|
||||
else
|
||||
systemBeepPtr = beepRev4;
|
||||
BEEP_OFF;
|
||||
#endif
|
||||
LED0_OFF;
|
||||
LED1_OFF;
|
||||
|
||||
for (i = 0; i < gpio_count; i++) {
|
||||
if (hse_value == 12000000 && gpio_setup[i].cfg.mode == Mode_Out_OD)
|
||||
gpio_setup[i].cfg.mode = Mode_Out_PP;
|
||||
gpioInit(gpio_setup[i].gpio, &gpio_setup[i].cfg);
|
||||
}
|
||||
|
||||
// Init cycle counter
|
||||
cycleCounterInit();
|
||||
|
||||
// SysTick
|
||||
SysTick_Config(SystemCoreClock / 1000);
|
||||
|
||||
// Configure the rest of the stuff
|
||||
#ifndef FY90Q
|
||||
i2cInit(I2C2);
|
||||
#endif
|
||||
spiInit();
|
||||
|
||||
// sleep for 100ms
|
||||
delay(100);
|
||||
}
|
||||
|
||||
#if 1
|
||||
void delayMicroseconds(uint32_t us)
|
||||
{
|
||||
uint32_t now = micros();
|
||||
while (micros() - now < us);
|
||||
}
|
||||
#else
|
||||
void delayMicroseconds(uint32_t us)
|
||||
{
|
||||
uint32_t elapsed = 0;
|
||||
uint32_t lastCount = SysTick->VAL;
|
||||
|
||||
for (;;) {
|
||||
register uint32_t current_count = SysTick->VAL;
|
||||
uint32_t elapsed_us;
|
||||
|
||||
// measure the time elapsed since the last time we checked
|
||||
elapsed += current_count - lastCount;
|
||||
lastCount = current_count;
|
||||
|
||||
// convert to microseconds
|
||||
elapsed_us = elapsed / usTicks;
|
||||
if (elapsed_us >= us)
|
||||
break;
|
||||
|
||||
// reduce the delay by the elapsed time
|
||||
us -= elapsed_us;
|
||||
|
||||
// keep fractional microseconds for the next iteration
|
||||
elapsed %= usTicks;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
void delay(uint32_t ms)
|
||||
{
|
||||
while (ms--)
|
||||
delayMicroseconds(1000);
|
||||
}
|
||||
|
||||
void failureMode(uint8_t mode)
|
||||
{
|
||||
LED1_ON;
|
||||
LED0_OFF;
|
||||
while (1) {
|
||||
LED1_TOGGLE;
|
||||
LED0_TOGGLE;
|
||||
delay(475 * mode - 2);
|
||||
BEEP_ON
|
||||
delay(25);
|
||||
BEEP_OFF;
|
||||
}
|
||||
}
|
||||
|
||||
#define AIRCR_VECTKEY_MASK ((uint32_t)0x05FA0000)
|
||||
|
||||
void systemReset(bool toBootloader)
|
||||
{
|
||||
if (toBootloader) {
|
||||
// 1FFFF000 -> 20000200 -> SP
|
||||
// 1FFFF004 -> 1FFFF021 -> PC
|
||||
*((uint32_t *)0x20004FF0) = 0xDEADBEEF; // 20KB STM32F103
|
||||
}
|
||||
|
||||
// Generate system reset
|
||||
SCB->AIRCR = AIRCR_VECTKEY_MASK | (uint32_t)0x04;
|
||||
}
|
||||
|
||||
#ifdef BUZZER
|
||||
static void beepRev4(bool onoff)
|
||||
{
|
||||
if (onoff) {
|
||||
digitalLo(BEEP_GPIO, BEEP_PIN);
|
||||
} else {
|
||||
digitalHi(BEEP_GPIO, BEEP_PIN);
|
||||
}
|
||||
}
|
||||
|
||||
static void beepRev5(bool onoff)
|
||||
{
|
||||
if (onoff) {
|
||||
digitalHi(BEEP_GPIO, BEEP_PIN);
|
||||
} else {
|
||||
digitalLo(BEEP_GPIO, BEEP_PIN);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
void systemBeep(bool onoff)
|
||||
{
|
||||
#ifdef BUZZER
|
||||
systemBeepPtr(onoff);
|
||||
#endif
|
||||
}
|
||||
|
Loading…
Add table
Add a link
Reference in a new issue