1
0
Fork 0
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:
Dominic Clifton 2014-04-08 22:07:37 +01:00
parent 39adc34278
commit 3bd4cd2ed2
84 changed files with 732 additions and 645 deletions

225
src/drivers/system_common.c Executable file
View 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
}