├── src ├── drv_bma280.h ├── drv_bmp085.h ├── drv_mma845x.h ├── drv_ms5611.h ├── drv_bmp085.c ├── drv_uart.c ├── drv_hmc5883l.c ├── drv_mpu3050.h ├── drv_mpu6050.c ├── drv_l3g4200d.h ├── telemetry_hott.h ├── drv_mpu6500.h ├── drv_ledring.h ├── drv_ak8975.h ├── drv_hmc5883l.h ├── drv_mpu6050.h ├── cli.h ├── drv_adxl345.h ├── drv_hcsr04.h ├── utils.h ├── drv_spi.h ├── telemetry_common.h ├── telemetry_frsky.h ├── drv_i2c.h ├── drv_adc.h ├── drv_system.h ├── rxmsp.c ├── drv_timer.h ├── blackbox.h ├── drv_serial.c ├── drv_uart.h ├── drv_gpio.h ├── drv_ledring.c ├── drv_bma280.c ├── drv_softserial.h ├── drv_serial.h ├── drv_ak8975.c ├── buzzer.h ├── sumd.c ├── drv_pwm.h ├── drv_l3g4200d.c ├── drv_gpio.c ├── drv_spi.c ├── blackbox_fielddefs.h ├── drv_adc.c ├── telemetry_common.c ├── sbus.c ├── drv_mpu3050.c ├── drv_adxl345.c ├── drv_mma845x.c ├── utils.c ├── drv_hcsr04.c ├── printf.h ├── drv_mpu6500.c ├── drv_i2c_soft.c ├── spektrum.c ├── drv_ms5611.c ├── drv_system.c ├── buzzer.c └── printf.c ├── lib ├── STM32F10x_StdPeriph_Driver │ ├── src │ │ ├── stm32f10x_i2c.c │ │ ├── stm32f10x_flash.c │ │ ├── stm32f10x_usart.c │ │ ├── stm32f10x_crc.c │ │ ├── stm32f10x_iwdg.c │ │ ├── stm32f10x_dbgmcu.c │ │ └── stm32f10x_wwdg.c │ └── inc │ │ ├── stm32f10x_crc.h │ │ ├── stm32f10x_wwdg.h │ │ ├── stm32f10x_dbgmcu.h │ │ ├── stm32f10x_iwdg.h │ │ ├── stm32f10x_rtc.h │ │ ├── stm32f10x_pwr.h │ │ ├── stm32f10x_cec.h │ │ └── stm32f10x_exti.h └── CMSIS │ └── CM3 │ └── DeviceSupport │ └── ST │ └── STM32F10x │ ├── stm32f10x.h │ ├── system_stm32f10x.h │ ├── stm32f10x_conf.h │ └── system_stm32f10x.c ├── .gitignore ├── support ├── stmloader │ ├── Makefile │ ├── stmbootloader.h │ ├── serial.h │ ├── loader.c │ └── serial.c └── flash.bat ├── JLinkSettings.ini ├── stm32_flash.ld └── Makefile /src/drv_bma280.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | bool bma280Detect(sensor_t *acc); 4 | -------------------------------------------------------------------------------- /src/drv_bmp085.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | bool bmp085Detect(baro_t *baro); 4 | -------------------------------------------------------------------------------- /src/drv_mma845x.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | bool mma8452Detect(sensor_t *acc); 4 | -------------------------------------------------------------------------------- /src/drv_ms5611.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | bool ms5611Detect(baro_t *baro); 4 | -------------------------------------------------------------------------------- /src/drv_bmp085.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thenickdude/blackbox/HEAD/src/drv_bmp085.c -------------------------------------------------------------------------------- /src/drv_uart.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thenickdude/blackbox/HEAD/src/drv_uart.c -------------------------------------------------------------------------------- /src/drv_hmc5883l.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thenickdude/blackbox/HEAD/src/drv_hmc5883l.c -------------------------------------------------------------------------------- /src/drv_mpu3050.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | bool mpu3050Detect(sensor_t *gyro, uint16_t lpf); 4 | -------------------------------------------------------------------------------- /src/drv_mpu6050.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thenickdude/blackbox/HEAD/src/drv_mpu6050.c -------------------------------------------------------------------------------- /src/drv_l3g4200d.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | bool l3g4200dDetect(sensor_t *gyro, uint16_t lpf); 4 | -------------------------------------------------------------------------------- /src/telemetry_hott.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thenickdude/blackbox/HEAD/src/telemetry_hott.h -------------------------------------------------------------------------------- /src/drv_mpu6500.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | bool mpu6500Detect(sensor_t *acc, sensor_t *gyro, uint16_t lpf); 4 | -------------------------------------------------------------------------------- /src/drv_ledring.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | bool ledringDetect(void); 4 | void ledringState(void); 5 | void ledringBlink(void); 6 | -------------------------------------------------------------------------------- /lib/STM32F10x_StdPeriph_Driver/src/stm32f10x_i2c.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thenickdude/blackbox/HEAD/lib/STM32F10x_StdPeriph_Driver/src/stm32f10x_i2c.c -------------------------------------------------------------------------------- /src/drv_ak8975.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | bool ak8975detect(sensor_t *mag); 4 | void ak8975Init(sensor_align_e align); 5 | void ak8975Read(int16_t *magData); 6 | -------------------------------------------------------------------------------- /lib/CMSIS/CM3/DeviceSupport/ST/STM32F10x/stm32f10x.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thenickdude/blackbox/HEAD/lib/CMSIS/CM3/DeviceSupport/ST/STM32F10x/stm32f10x.h -------------------------------------------------------------------------------- /lib/STM32F10x_StdPeriph_Driver/src/stm32f10x_flash.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thenickdude/blackbox/HEAD/lib/STM32F10x_StdPeriph_Driver/src/stm32f10x_flash.c -------------------------------------------------------------------------------- /lib/STM32F10x_StdPeriph_Driver/src/stm32f10x_usart.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thenickdude/blackbox/HEAD/lib/STM32F10x_StdPeriph_Driver/src/stm32f10x_usart.c -------------------------------------------------------------------------------- /src/drv_hmc5883l.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | bool hmc5883lDetect(sensor_t *mag); 4 | void hmc5883lInit(sensor_align_e align); 5 | void hmc5883lRead(int16_t *magData); 6 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.o 2 | *~ 3 | *.uvopt 4 | *.dep 5 | *.bak 6 | *.uvgui.* 7 | .project 8 | .settings 9 | .cproject 10 | obj/ 11 | patches/ 12 | startup_stm32f10x_md_gcc.s 13 | -------------------------------------------------------------------------------- /src/drv_mpu6050.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | bool mpu6050Detect(sensor_t * acc, sensor_t * gyro, uint16_t lpf, uint8_t *scale); 4 | void mpu6050DmpLoop(void); 5 | void mpu6050DmpResetFifo(void); 6 | -------------------------------------------------------------------------------- /src/cli.h: -------------------------------------------------------------------------------- 1 | /* 2 | * cli.h 3 | * 4 | * Created on: 6 Apr 2014 5 | * Author: Hydra 6 | */ 7 | 8 | #ifndef CLI_H_ 9 | #define CLI_H_ 10 | 11 | extern uint8_t cliMode; 12 | 13 | #endif /* CLI_H_ */ 14 | -------------------------------------------------------------------------------- /src/drv_adxl345.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | typedef struct drv_adxl345_config_t { 4 | bool useFifo; 5 | uint16_t dataRate; 6 | } drv_adxl345_config_t; 7 | 8 | bool adxl345Detect(drv_adxl345_config_t *init, sensor_t *acc); 9 | -------------------------------------------------------------------------------- /src/drv_hcsr04.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | typedef enum { 4 | sonar_pwm56, 5 | sonar_rc78, 6 | } sonar_config_t; 7 | 8 | void hcsr04_init(sonar_config_t config); 9 | void hcsr04_get_distance(volatile int32_t *distance); 10 | -------------------------------------------------------------------------------- /src/utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | int constrain(int amt, int low, int high); 4 | // sensor orientation 5 | void alignSensors(int16_t *src, int16_t *dest, uint8_t rotation); 6 | void initBoardAlignment(void); 7 | void productionDebug(void); 8 | -------------------------------------------------------------------------------- /src/drv_spi.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #define SPI_DEVICE_NONE (0) 4 | #define SPI_DEVICE_FLASH (1) 5 | #define SPI_DEVICE_MPU (2) 6 | 7 | int spiInit(void); 8 | void spiSelect(bool select); 9 | uint8_t spiTransferByte(uint8_t in); 10 | bool spiTransfer(uint8_t *out, uint8_t *in, int len); 11 | -------------------------------------------------------------------------------- /support/stmloader/Makefile: -------------------------------------------------------------------------------- 1 | CC = $(CROSS_COMPILE)gcc 2 | AR = $(CROSS_COMPILE)ar 3 | export CC 4 | export AR 5 | 6 | all: 7 | $(CC) -g -o stmloader -I./ \ 8 | loader.c \ 9 | serial.c \ 10 | stmbootloader.c \ 11 | -Wall 12 | 13 | clean: 14 | rm -f stmloader; rm -rf stmloader.dSYM 15 | -------------------------------------------------------------------------------- /src/telemetry_common.h: -------------------------------------------------------------------------------- 1 | /* 2 | * telemetry_common.h 3 | * 4 | * Created on: 6 Apr 2014 5 | * Author: Hydra 6 | */ 7 | 8 | #ifndef TELEMETRY_COMMON_H_ 9 | #define TELEMETRY_COMMON_H_ 10 | 11 | // telemetry 12 | void initTelemetry(void); 13 | void checkTelemetryState(void); 14 | void handleTelemetry(void); 15 | 16 | #endif /* TELEMETRY_COMMON_H_ */ 17 | -------------------------------------------------------------------------------- /src/telemetry_frsky.h: -------------------------------------------------------------------------------- 1 | /* 2 | * telemetry_frsky.h 3 | * 4 | * Created on: 6 Apr 2014 5 | * Author: Hydra 6 | */ 7 | 8 | #ifndef TELEMETRY_FRSKY_H_ 9 | #define TELEMETRY_FRSKY_H_ 10 | 11 | void handleFrSkyTelemetry(void); 12 | void checkFrSkyTelemetryState(void); 13 | 14 | void configureFrSkyTelemetryPort(void); 15 | void freeFrSkyTelemetryPort(void); 16 | 17 | #endif /* TELEMETRY_FRSKY_H_ */ 18 | -------------------------------------------------------------------------------- /src/drv_i2c.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of baseflight 3 | * Licensed under GPL V3 or modified DCL - see https://github.com/multiwii/baseflight/blob/master/README.md 4 | */ 5 | #pragma once 6 | 7 | typedef enum I2CDevice { 8 | I2CDEV_1, 9 | I2CDEV_2, 10 | I2CDEV_MAX = I2CDEV_2, 11 | } I2CDevice; 12 | 13 | void i2cInit(I2CDevice index); 14 | bool i2cWriteBuffer(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data); 15 | bool i2cWrite(uint8_t addr_, uint8_t reg, uint8_t data); 16 | bool i2cRead(uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* buf); 17 | uint16_t i2cGetErrorCounter(void); 18 | -------------------------------------------------------------------------------- /src/drv_adc.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | typedef enum { 4 | ADC_BATTERY = 0, 5 | ADC_EXTERNAL_PAD = 1, 6 | ADC_EXTERNAL_CURRENT = 2, 7 | ADC_RSSI = 3, 8 | ADC_CHANNEL_MAX = 4 9 | } AdcChannel; 10 | 11 | typedef struct drv_adc_config_t { 12 | uint8_t powerAdcChannel; // which channel used for current monitor, allowed PA1, PB1 (ADC_Channel_1, ADC_Channel_9) 13 | uint8_t rssiAdcChannel; // which channel used for analog-rssi (RC-filter), allowed PA1, PB1 (ADC_Channel_1, ADC_Channel_9) 14 | } drv_adc_config_t; 15 | 16 | void adcInit(drv_adc_config_t *init); 17 | uint16_t adcGetChannel(uint8_t channel); 18 | -------------------------------------------------------------------------------- /src/drv_system.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #define BKP_SOFTRESET (0x50F7B007) 4 | 5 | void systemInit(void); 6 | void delayMicroseconds(uint32_t us); 7 | void delay(uint32_t ms); 8 | 9 | uint32_t micros(void); 10 | uint32_t millis(void); 11 | 12 | // Backup SRAM R/W 13 | uint32_t rccReadBkpDr(void); 14 | void rccWriteBkpDr(uint32_t value); 15 | 16 | // failure 17 | void failureMode(uint8_t mode); 18 | 19 | // bootloader/IAP 20 | void systemReset(bool toBootloader); 21 | 22 | // current auto-detected hardware revision (enum HardwareRevision in board.h) 23 | extern int hw_revision; 24 | // current crystal frequency - 8 or 12MHz 25 | extern uint32_t hse_value; 26 | -------------------------------------------------------------------------------- /src/rxmsp.c: -------------------------------------------------------------------------------- 1 | #include "board.h" 2 | #include "mw.h" 3 | 4 | static bool rxMspFrameDone = false; 5 | static uint16_t mspReadRawRC(uint8_t chan); 6 | 7 | static uint16_t mspReadRawRC(uint8_t chan) 8 | { 9 | return rcData[chan]; 10 | } 11 | 12 | void mspFrameRecieve(void) 13 | { 14 | rxMspFrameDone = true; 15 | } 16 | 17 | bool mspFrameComplete(void) 18 | { 19 | if (rxMspFrameDone) { 20 | failsafeCnt = 0; // clear FailSafe counter 21 | rxMspFrameDone = false; 22 | return true; 23 | } 24 | return false; 25 | } 26 | 27 | void mspInit(rcReadRawDataPtr *callback) 28 | { 29 | if (callback) 30 | *callback = mspReadRawRC; 31 | } 32 | -------------------------------------------------------------------------------- /JLinkSettings.ini: -------------------------------------------------------------------------------- 1 | [BREAKPOINTS] 2 | ShowInfoWin = 1 3 | EnableFlashBP = 2 4 | BPDuringExecution = 0 5 | [CFI] 6 | CFISize = 0x00 7 | CFIAddr = 0x00 8 | [CPU] 9 | OverrideMemMap = 0 10 | AllowSimulation = 1 11 | ScriptFile="" 12 | [FLASH] 13 | MinNumBytesFlashDL = 0 14 | SkipProgOnCRCMatch = 1 15 | VerifyDownload = 1 16 | AllowCaching = 1 17 | EnableFlashDL = 2 18 | Override = 0 19 | Device="AD7160" 20 | [GENERAL] 21 | WorkRAMSize = 0x00 22 | WorkRAMAddr = 0x00 23 | [SWO] 24 | SWOLogFile="" 25 | [MEM] 26 | RdOverrideOrMask = 0x00 27 | RdOverrideAndMask = 0xFFFFFFFF 28 | RdOverrideAddr = 0xFFFFFFFF 29 | WrOverrideOrMask = 0x00 30 | WrOverrideAndMask = 0xFFFFFFFF 31 | WrOverrideAddr = 0xFFFFFFFF 32 | -------------------------------------------------------------------------------- /support/flash.bat: -------------------------------------------------------------------------------- 1 | @@echo OFF 2 | 3 | :: User configuration 4 | 5 | :: set your port number. ie: for COM6 , port is 6 6 | set PORT=2 7 | :: location of stmflashloader.exe, this is default 8 | set FLASH_LOADER="C:\Program Files (x86)\STMicroelectronics\Software\Flash Loader Demonstrator\STMFlashLoader.exe" 9 | :: path to firmware 10 | set FIRMWARE="D:\baseflight.hex" 11 | 12 | :: ---------------------------------------------- 13 | 14 | mode COM%PORT% BAUD=115200 PARITY=N DATA=8 STOP=1 XON=OFF DTR=OFF RTS=OFF 15 | echo/|set /p ="R" > COM%PORT%: 16 | 17 | TIMEOUT /T 3 18 | 19 | cd %LOADER_PATH% 20 | 21 | %FLASH_LOADER% ^ 22 | -c --pn %PORT% --br 115200 --db 8 ^ 23 | -i STM32_Med-density_128K ^ 24 | -e --all ^ 25 | -d --fn %FIRMWARE% ^ 26 | -r --a 0x8000000 27 | -------------------------------------------------------------------------------- /src/drv_timer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | typedef void timerCCCallbackPtr(uint8_t port, uint16_t capture); 4 | 5 | typedef struct { 6 | TIM_TypeDef *tim; 7 | GPIO_TypeDef *gpio; 8 | uint32_t pin; 9 | uint8_t channel; 10 | uint8_t irq; 11 | uint8_t outputEnable; 12 | } timerHardware_t; 13 | 14 | extern const timerHardware_t timerHardware[]; 15 | 16 | void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz); 17 | void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, uint8_t mhz); 18 | void timerNVICConfigure(uint8_t irq); 19 | 20 | void configureTimerInputCaptureCompareChannel(TIM_TypeDef *tim, const uint8_t channel); 21 | void configureTimerCaptureCompareInterrupt(const timerHardware_t *timerHardwarePtr, uint8_t reference, timerCCCallbackPtr *callback); 22 | void configureTimerChannelCallback(TIM_TypeDef *tim, uint8_t channel, uint8_t reference, timerCCCallbackPtr *callback); 23 | -------------------------------------------------------------------------------- /src/blackbox.h: -------------------------------------------------------------------------------- 1 | /* 2 | * blackbox.h 3 | * 4 | * Author: Nicholas Sherlock 5 | */ 6 | 7 | #ifndef BLACKBOX_H_ 8 | #define BLACKBOX_H_ 9 | 10 | #include 11 | 12 | #ifndef XYZ_AXIS_COUNT 13 | #define XYZ_AXIS_COUNT 3 14 | #endif 15 | 16 | typedef struct blackboxValues_t { 17 | uint32_t time; 18 | 19 | int32_t axisPID_P[XYZ_AXIS_COUNT], axisPID_I[XYZ_AXIS_COUNT], axisPID_D[XYZ_AXIS_COUNT]; 20 | 21 | int16_t rcCommand[4]; 22 | int16_t gyroData[XYZ_AXIS_COUNT]; 23 | int16_t accSmooth[XYZ_AXIS_COUNT]; 24 | int16_t motor[MAX_MOTORS]; 25 | int16_t servo[MAX_SERVOS]; 26 | 27 | uint16_t vbatLatest; 28 | 29 | #ifdef BARO 30 | int32_t BaroAlt; 31 | #endif 32 | #ifdef MAG 33 | int16_t magADC[XYZ_AXIS_COUNT]; 34 | #endif 35 | } blackboxValues_t; 36 | 37 | void initBlackbox(void); 38 | void handleBlackbox(void); 39 | void startBlackbox(void); 40 | void finishBlackbox(void); 41 | 42 | #endif /* BLACKBOX_H_ */ 43 | -------------------------------------------------------------------------------- /support/stmloader/stmbootloader.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of AutoQuad. 3 | 4 | AutoQuad is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | AutoQuad is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | You should have received a copy of the GNU General Public License 14 | along with AutoQuad. If not, see . 15 | 16 | Copyright © 2011 Bill Nesbitt 17 | */ 18 | 19 | #ifndef _stmbootloader_h 20 | #define _stmbootloader_h 21 | 22 | #include 23 | #include "serial.h" 24 | 25 | extern void stmLoader(serialStruct_t *s, FILE *fp, unsigned char overrideParity, unsigned char noSendR); 26 | 27 | #endif 28 | -------------------------------------------------------------------------------- /src/drv_serial.c: -------------------------------------------------------------------------------- 1 | #include "board.h" 2 | 3 | void serialPrint(serialPort_t *instance, const char *str) 4 | { 5 | uint8_t ch; 6 | while ((ch = *(str++)) != 0) { 7 | serialWrite(instance, ch); 8 | } 9 | } 10 | 11 | uint32_t serialGetBaudRate(serialPort_t *instance) 12 | { 13 | return instance->baudRate; 14 | } 15 | 16 | void serialWrite(serialPort_t *instance, uint8_t ch) 17 | { 18 | instance->vTable->serialWrite(instance, ch); 19 | } 20 | 21 | uint8_t serialTotalBytesWaiting(serialPort_t *instance) 22 | { 23 | return instance->vTable->serialTotalBytesWaiting(instance); 24 | } 25 | 26 | uint8_t serialRead(serialPort_t *instance) 27 | { 28 | return instance->vTable->serialRead(instance); 29 | } 30 | 31 | void serialSetBaudRate(serialPort_t *instance, uint32_t baudRate) 32 | { 33 | instance->vTable->serialSetBaudRate(instance, baudRate); 34 | } 35 | 36 | bool isSerialTransmitBufferEmpty(serialPort_t *instance) 37 | { 38 | return instance->vTable->isSerialTransmitBufferEmpty(instance); 39 | } 40 | 41 | inline void serialSetMode(serialPort_t *instance, portMode_t mode) 42 | { 43 | instance->vTable->setMode(instance, mode); 44 | } 45 | 46 | -------------------------------------------------------------------------------- /src/drv_uart.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #define UART_BUFFER_SIZE 64 4 | 5 | #define UART1_RX_BUFFER_SIZE 256 6 | #define UART1_TX_BUFFER_SIZE 256 7 | #define UART2_RX_BUFFER_SIZE 128 8 | #define UART2_TX_BUFFER_SIZE 64 9 | #define UART3_RX_BUFFER_SIZE 256 10 | #define UART3_TX_BUFFER_SIZE 256 11 | #define MAX_SERIAL_PORTS 3 12 | 13 | // FIXME this is a uart_t really. Move the generic properties into a separate structure (serialPort_t) and update the code to use it 14 | typedef struct { 15 | serialPort_t port; 16 | 17 | // FIXME these are uart specific and do not belong in here 18 | DMA_Channel_TypeDef *rxDMAChannel; 19 | DMA_Channel_TypeDef *txDMAChannel; 20 | 21 | uint32_t rxDMAIrq; 22 | uint32_t txDMAIrq; 23 | 24 | uint32_t rxDMAPos; 25 | bool txDMAEmpty; 26 | 27 | USART_TypeDef *USARTx; 28 | } uartPort_t; 29 | 30 | extern const struct serialPortVTable uartVTable[]; 31 | 32 | serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode); 33 | 34 | // serialPort API 35 | void uartWrite(serialPort_t *instance, uint8_t ch); 36 | uint8_t uartTotalBytesWaiting(serialPort_t *instance); 37 | uint8_t uartRead(serialPort_t *instance); 38 | void uartSetBaudRate(serialPort_t *s, uint32_t baudRate); 39 | bool isUartTransmitBufferEmpty(serialPort_t *s); 40 | -------------------------------------------------------------------------------- /src/drv_gpio.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | typedef enum { 4 | Mode_AIN = 0x0, 5 | Mode_IN_FLOATING = 0x04, 6 | Mode_IPD = 0x28, 7 | Mode_IPU = 0x48, 8 | Mode_Out_OD = 0x14, 9 | Mode_Out_PP = 0x10, 10 | Mode_AF_OD = 0x1C, 11 | Mode_AF_PP = 0x18 12 | } GPIO_Mode; 13 | 14 | typedef enum { 15 | Speed_10MHz = 1, 16 | Speed_2MHz, 17 | Speed_50MHz 18 | } GPIO_Speed; 19 | 20 | typedef enum { 21 | Pin_0 = 0x0001, 22 | Pin_1 = 0x0002, 23 | Pin_2 = 0x0004, 24 | Pin_3 = 0x0008, 25 | Pin_4 = 0x0010, 26 | Pin_5 = 0x0020, 27 | Pin_6 = 0x0040, 28 | Pin_7 = 0x0080, 29 | Pin_8 = 0x0100, 30 | Pin_9 = 0x0200, 31 | Pin_10 = 0x0400, 32 | Pin_11 = 0x0800, 33 | Pin_12 = 0x1000, 34 | Pin_13 = 0x2000, 35 | Pin_14 = 0x4000, 36 | Pin_15 = 0x8000, 37 | Pin_All = 0xFFFF 38 | } GPIO_Pin; 39 | 40 | typedef struct { 41 | uint16_t pin; 42 | GPIO_Mode mode; 43 | GPIO_Speed speed; 44 | } gpio_config_t; 45 | 46 | #define digitalHi(p, i) { p->BSRR = i; } 47 | #define digitalLo(p, i) { p->BRR = i; } 48 | #define digitalToggle(p, i) { p->ODR ^= i; } 49 | #define digitalIn(p, i) (p->IDR & i) 50 | 51 | void gpioInit(GPIO_TypeDef *gpio, gpio_config_t *config); 52 | void gpioExtiLineConfig(uint8_t portsrc, uint8_t pinsrc); 53 | void gpioPinRemapConfig(uint32_t remap, bool enable); 54 | -------------------------------------------------------------------------------- /src/drv_ledring.c: -------------------------------------------------------------------------------- 1 | #include "board.h" 2 | #include "mw.h" 3 | 4 | // Driver for DFRobot I2C Led Ring 5 | #define LED_RING_ADDRESS 0x6D 6 | 7 | bool ledringDetect(void) 8 | { 9 | bool ack = false; 10 | uint8_t sig = 'e'; 11 | 12 | ack = i2cWrite(LED_RING_ADDRESS, 0xFF, sig); 13 | if (!ack) 14 | return false; 15 | return true; 16 | } 17 | 18 | void ledringState(void) 19 | { 20 | uint8_t b[10]; 21 | static uint8_t state; 22 | 23 | if (state == 0) { 24 | b[0] = 'z'; 25 | b[1] = (180 - heading) / 2; // 1 unit = 2 degrees; 26 | i2cWriteBuffer(LED_RING_ADDRESS, 0xFF, 2, b); 27 | state = 1; 28 | } else if (state == 1) { 29 | b[0] = 'y'; 30 | b[1] = constrain(angle[ROLL] / 10 + 90, 0, 180); 31 | b[2] = constrain(angle[PITCH] / 10 + 90, 0, 180); 32 | i2cWriteBuffer(LED_RING_ADDRESS, 0xFF, 3, b); 33 | state = 2; 34 | } else if (state == 2) { 35 | b[0] = 'd'; // all unicolor GREEN 36 | b[1] = 1; 37 | if (f.ARMED) 38 | b[2] = 1; 39 | else 40 | b[2] = 0; 41 | i2cWriteBuffer(LED_RING_ADDRESS, 0xFF, 3, b); 42 | state = 0; 43 | } 44 | } 45 | 46 | void ledringBlink(void) 47 | { 48 | uint8_t b[3]; 49 | b[0] = 'k'; 50 | b[1] = 10; 51 | b[2] = 10; 52 | i2cWriteBuffer(LED_RING_ADDRESS, 0xFF, 3, b); 53 | } 54 | -------------------------------------------------------------------------------- /src/drv_bma280.c: -------------------------------------------------------------------------------- 1 | #include "board.h" 2 | 3 | // BMA280, default I2C address mode 0x18 4 | #define BMA280_ADDRESS 0x18 5 | #define BMA280_ACC_X_LSB 0x02 6 | #define BMA280_PMU_BW 0x10 7 | #define BMA280_PMU_RANGE 0x0F 8 | 9 | extern uint16_t acc_1G; 10 | 11 | static void bma280Init(sensor_align_e align); 12 | static void bma280Read(int16_t *accelData); 13 | 14 | static sensor_align_e accAlign = CW0_DEG; 15 | 16 | bool bma280Detect(sensor_t *acc) 17 | { 18 | bool ack = false; 19 | uint8_t sig = 0; 20 | 21 | ack = i2cRead(BMA280_ADDRESS, 0x00, 1, &sig); 22 | if (!ack || sig != 0xFB) 23 | return false; 24 | 25 | acc->init = bma280Init; 26 | acc->read = bma280Read; 27 | return true; 28 | } 29 | 30 | static void bma280Init(sensor_align_e align) 31 | { 32 | i2cWrite(BMA280_ADDRESS, BMA280_PMU_RANGE, 0x08); // +-8g range 33 | i2cWrite(BMA280_ADDRESS, BMA280_PMU_BW, 0x0E); // 500Hz BW 34 | 35 | acc_1G = 512 * 8; 36 | 37 | if (align > 0) 38 | accAlign = align; 39 | } 40 | 41 | static void bma280Read(int16_t *accelData) 42 | { 43 | uint8_t buf[6]; 44 | int16_t data[3]; 45 | 46 | i2cRead(BMA280_ADDRESS, BMA280_ACC_X_LSB, 6, buf); 47 | 48 | // Data format is lsb<5:0> | msb<13:6> 49 | data[0] = (int16_t)((buf[0] >> 2) + (buf[1] << 8)); 50 | data[1] = (int16_t)((buf[2] >> 2) + (buf[3] << 8)); 51 | data[2] = (int16_t)((buf[4] >> 2) + (buf[5] << 8)); 52 | 53 | alignSensors(data, accelData, accAlign); 54 | } 55 | -------------------------------------------------------------------------------- /support/stmloader/serial.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of AutoQuad. 3 | 4 | AutoQuad is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | AutoQuad is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | You should have received a copy of the GNU General Public License 14 | along with AutoQuad. If not, see . 15 | 16 | Copyright © 2011 Bill Nesbitt 17 | */ 18 | 19 | #ifndef _serial_h 20 | #define _serial_h 21 | 22 | #define INPUT_BUFFER_SIZE 1024 23 | 24 | typedef struct { 25 | int fd; 26 | } serialStruct_t; 27 | 28 | extern serialStruct_t *initSerial(const char *port, unsigned int baud, char ctsRts); 29 | extern void serialWrite(serialStruct_t *s, const char *str, unsigned int len); 30 | extern void serialWriteChar(serialStruct_t *s, unsigned char c); 31 | extern unsigned char serialAvailable(serialStruct_t *s); 32 | extern void serialFlush(serialStruct_t *s); 33 | extern unsigned char serialRead(serialStruct_t *s); 34 | extern void serialEvenParity(serialStruct_t *s); 35 | extern void serialNoParity(serialStruct_t *s); 36 | extern void serialFree(serialStruct_t *s); 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /src/drv_softserial.h: -------------------------------------------------------------------------------- 1 | /* 2 | * drv_softserial.h 3 | * 4 | * Created on: 23 Aug 2013 5 | * Author: Hydra 6 | */ 7 | 8 | #pragma once 9 | 10 | #define SOFT_SERIAL_BUFFER_SIZE 256 11 | // Max baud rate of current soft serial implementation 12 | #define SOFT_SERIAL_MAX_BAUD_RATE 19200 13 | 14 | typedef struct softSerial_s { 15 | serialPort_t port; 16 | 17 | const timerHardware_t *rxTimerHardware; 18 | volatile uint8_t rxBuffer[SOFT_SERIAL_BUFFER_SIZE]; 19 | 20 | const timerHardware_t *txTimerHardware; 21 | volatile uint8_t txBuffer[SOFT_SERIAL_BUFFER_SIZE]; 22 | 23 | uint8_t isSearchingForStartBit; 24 | uint8_t rxBitIndex; 25 | uint8_t rxLastLeadingEdgeAtBitIndex; 26 | uint8_t rxEdge; 27 | 28 | uint8_t isTransmittingData; 29 | int8_t bitsLeftToTransmit; 30 | 31 | uint16_t internalTxBuffer; // includes start and stop bits 32 | uint16_t internalRxBuffer; // includes start and stop bits 33 | 34 | uint8_t isInverted; 35 | 36 | uint16_t transmissionErrors; 37 | uint16_t receiveErrors; 38 | } softSerial_t; 39 | 40 | extern timerHardware_t* serialTimerHardware; 41 | extern softSerial_t softSerialPorts[]; 42 | 43 | extern const struct serialPortVTable softSerialVTable[]; 44 | 45 | void setupSoftSerialPrimary(uint32_t baud, uint8_t inverted); 46 | void setupSoftSerialSecondary(uint8_t inverted); 47 | 48 | // serialPort API 49 | void softSerialWriteByte(serialPort_t *instance, uint8_t ch); 50 | uint8_t softSerialTotalBytesWaiting(serialPort_t *instance); 51 | uint8_t softSerialReadByte(serialPort_t *instance); 52 | void softSerialSetBaudRate(serialPort_t *s, uint32_t baudRate); 53 | bool isSoftSerialTransmitBufferEmpty(serialPort_t *s); 54 | 55 | -------------------------------------------------------------------------------- /src/drv_serial.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | typedef enum portMode_t { 4 | MODE_RX = 1 << 0, 5 | MODE_TX = 1 << 1, 6 | MODE_RXTX = MODE_RX | MODE_TX, 7 | MODE_SBUS = 1 << 2, 8 | } portMode_t; 9 | 10 | typedef struct serialPort { 11 | 12 | const struct serialPortVTable *vTable; 13 | 14 | portMode_t mode; 15 | uint32_t baudRate; 16 | 17 | uint32_t rxBufferSize; 18 | uint32_t txBufferSize; 19 | volatile uint8_t *rxBuffer; 20 | volatile uint8_t *txBuffer; 21 | uint32_t rxBufferHead; 22 | uint32_t rxBufferTail; 23 | uint32_t txBufferHead; 24 | uint32_t txBufferTail; 25 | 26 | // FIXME rename member to rxCallback 27 | serialReceiveCallbackPtr callback; 28 | } serialPort_t; 29 | 30 | struct serialPortVTable { 31 | void (*serialWrite)(serialPort_t *instance, uint8_t ch); 32 | 33 | uint8_t (*serialTotalBytesWaiting)(serialPort_t *instance); 34 | 35 | uint8_t (*serialRead)(serialPort_t *instance); 36 | 37 | // Specified baud rate may not be allowed by an implementation, use serialGetBaudRate to determine actual baud rate in use. 38 | void (*serialSetBaudRate)(serialPort_t *instance, uint32_t baudRate); 39 | 40 | bool (*isSerialTransmitBufferEmpty)(serialPort_t *instance); 41 | 42 | void (*setMode)(serialPort_t *instance, portMode_t mode); 43 | }; 44 | 45 | void serialWrite(serialPort_t *instance, uint8_t ch); 46 | uint8_t serialTotalBytesWaiting(serialPort_t *instance); 47 | uint8_t serialRead(serialPort_t *instance); 48 | void serialSetBaudRate(serialPort_t *instance, uint32_t baudRate); 49 | void serialSetMode(serialPort_t *instance, portMode_t mode); 50 | bool isSerialTransmitBufferEmpty(serialPort_t *instance); 51 | void serialPrint(serialPort_t *instance, const char *str); 52 | uint32_t serialGetBaudRate(serialPort_t *instance); 53 | -------------------------------------------------------------------------------- /src/drv_ak8975.c: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of baseflight 3 | * Licensed under GPL V3 or modified DCL - see https://github.com/multiwii/baseflight/blob/master/README.md 4 | */ 5 | 6 | #include "board.h" 7 | 8 | // This sensor is available on MPU-9150. The accel/gyro in that chip use the same driver as MPU-6050. 9 | 10 | // AK8975, mag sensor address 11 | #define AK8975_MAG_I2C_ADDRESS 0x0C 12 | #define AK8975_MAG_ID_ADDRESS 0x00 13 | #define AK8975_MAG_DATA_ADDRESS 0x03 14 | #define AK8975_MAG_CONTROL_ADDRESS 0x0A 15 | 16 | static sensor_align_e magAlign = CW180_DEG; 17 | 18 | bool ak8975detect(sensor_t * mag) 19 | { 20 | bool ack = false; 21 | uint8_t sig = 0; 22 | 23 | // device ID is in register 0 and is equal to 'H' 24 | ack = i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_ID_ADDRESS, 1, &sig); 25 | if (!ack || sig != 'H') 26 | return false; 27 | 28 | mag->init = ak8975Init; 29 | mag->read = ak8975Read; 30 | 31 | return true; 32 | } 33 | 34 | void ak8975Init(sensor_align_e align) 35 | { 36 | if (align > 0) 37 | magAlign = align; 38 | 39 | i2cWrite(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_CONTROL_ADDRESS, 0x01); // start reading 40 | } 41 | 42 | void ak8975Read(int16_t *magData) 43 | { 44 | uint8_t buf[6]; 45 | int16_t mag[3]; 46 | 47 | i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_DATA_ADDRESS, 6, buf); 48 | // align sensors to match MPU6050: 49 | // x -> y 50 | // y -> x 51 | // z-> -z 52 | mag[X] = -(int16_t)(buf[3] << 8 | buf[2]) * 4; 53 | mag[Y] = -(int16_t)(buf[1] << 8 | buf[0]) * 4; 54 | mag[Z] = -(int16_t)(buf[5] << 8 | buf[4]) * 4; 55 | 56 | alignSensors(mag, magData, magAlign); 57 | 58 | i2cWrite(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_CONTROL_ADDRESS, 0x01); // start reading again 59 | } 60 | -------------------------------------------------------------------------------- /src/buzzer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of baseflight 3 | * Licensed under GPL V3 or modified DCL - see https://github.com/multiwii/baseflight/blob/master/README.md 4 | */ 5 | #pragma once 6 | 7 | void buzzer(uint8_t mode); 8 | void buzzerUpdate(void); 9 | 10 | /* Buzzer different modes: (lower number is higher priority) 11 | * BUZZER_STOP - Stops beeping 12 | * BUZZER_BAT_CRIT_LOW - Faster warning beeps when battery is critically low (repeats) 13 | * BUZZER_BAT_LOW - Warning beeps when battery is getting low (repeats) 14 | * BUZZER_TX_LOST_ARMED - Beeps SOS when armed and TX is turned off or signal lost (autolanding/autodisarm) 15 | * BUZZER_TX_LOST - Beeps when TX is turned off or signal lost (repeat until TX is okay) 16 | * BUZZER_DISARMING - One beep when disarming the board 17 | * BUZZER_ARMING - One beep when arming the board 18 | * BUZZER_ARMING_GPS_FIX - Beep a tone when arming the board and GPS has fix 19 | * BUZZER_TX_SET - Beeps when aux channel is set for beep or beep sequence how many satellites has found if GPS enabled. 20 | * BUZZER_ARMED - Warning beeps when board is armed. (repeats until board is disarmed or throttle is increased) 21 | * BUZZER_ACC_CALIBRATION - ACC inflight calibration completed confirmation 22 | * BUZZER_ACC_CALIBRATION_FAIL - ACC inflight calibration failed 23 | * BUZZER_READY_BEEP - Ring a tone when board is ready to flight (GPS ready). 24 | */ 25 | enum { 26 | BUZZER_STOP = 0, // Highest priority command which is used only for stopping the buzzer 27 | BUZZER_BAT_CRIT_LOW, 28 | BUZZER_BAT_LOW, 29 | BUZZER_TX_LOST_ARMED, 30 | BUZZER_TX_LOST, 31 | BUZZER_DISARMING, 32 | BUZZER_ARMING, 33 | BUZZER_ARMING_GPS_FIX, 34 | BUZZER_TX_SET, 35 | BUZZER_ARMED, 36 | BUZZER_ACC_CALIBRATION, 37 | BUZZER_ACC_CALIBRATION_FAIL, 38 | BUZZER_READY_BEEP, 39 | BUZZER_STOPPED // State which is used when buzzer is in idle mode 40 | }; 41 | -------------------------------------------------------------------------------- /src/sumd.c: -------------------------------------------------------------------------------- 1 | #include "board.h" 2 | #include "mw.h" 3 | 4 | // driver for SUMD receiver using UART2 5 | 6 | #define SUMD_SYNCBYTE 0xA8 7 | #define SUMD_MAX_CHANNEL 8 8 | #define SUMD_BUFFSIZE (SUMD_MAX_CHANNEL * 2 + 5) // 6 channels + 5 -> 17 bytes for 6 channels 9 | 10 | static bool sumdFrameDone = false; 11 | static void sumdDataReceive(uint16_t c); 12 | static uint16_t sumdReadRawRC(uint8_t chan); 13 | 14 | static uint32_t sumdChannelData[SUMD_MAX_CHANNEL]; 15 | 16 | void sumdInit(rcReadRawDataPtr *callback) 17 | { 18 | core.rcvrport = uartOpen(USART2, sumdDataReceive, 115200, MODE_RX); 19 | if (callback) 20 | *callback = sumdReadRawRC; 21 | } 22 | 23 | static uint8_t sumd[SUMD_BUFFSIZE] = { 0, }; 24 | static uint8_t sumdSize; 25 | 26 | // Receive ISR callback 27 | static void sumdDataReceive(uint16_t c) 28 | { 29 | uint32_t sumdTime; 30 | static uint32_t sumdTimeLast; 31 | static uint8_t sumdIndex; 32 | 33 | sumdTime = micros(); 34 | if ((sumdTime - sumdTimeLast) > 4000) 35 | sumdIndex = 0; 36 | sumdTimeLast = sumdTime; 37 | 38 | if (sumdIndex == 0) { 39 | if (c != SUMD_SYNCBYTE) 40 | return; 41 | else 42 | sumdFrameDone = false; // lazy main loop didnt fetch the stuff 43 | } 44 | if (sumdIndex == 2) 45 | sumdSize = (uint8_t)c; 46 | if (sumdIndex < SUMD_BUFFSIZE) 47 | sumd[sumdIndex] = (uint8_t)c; 48 | sumdIndex++; 49 | if (sumdIndex == sumdSize * 2 + 5) { 50 | sumdIndex = 0; 51 | sumdFrameDone = true; 52 | } 53 | } 54 | 55 | bool sumdFrameComplete(void) 56 | { 57 | uint8_t b; 58 | 59 | if (sumdFrameDone) { 60 | sumdFrameDone = false; 61 | if (sumd[1] == 0x01) { 62 | failsafeCnt = 0; 63 | if (sumdSize > SUMD_MAX_CHANNEL) 64 | sumdSize = SUMD_MAX_CHANNEL; 65 | for (b = 0; b < sumdSize; b++) 66 | sumdChannelData[b] = ((sumd[2 * b + 3] << 8) | sumd[2 * b + 4]); 67 | return true; 68 | } 69 | } 70 | return false; 71 | } 72 | 73 | static uint16_t sumdReadRawRC(uint8_t chan) 74 | { 75 | return sumdChannelData[mcfg.rcmap[chan]] / 8; 76 | } 77 | -------------------------------------------------------------------------------- /src/drv_pwm.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of baseflight 3 | * Licensed under GPL V3 or modified DCL - see https://github.com/multiwii/baseflight/blob/master/README.md 4 | */ 5 | #pragma once 6 | 7 | #define MAX_MOTORS 12 8 | #define MAX_SERVOS 8 9 | #define MAX_INPUTS 8 10 | #define PULSE_1MS (1000) // 1ms pulse width 11 | #define PULSE_MIN (750) // minimum PWM pulse width which is considered valid 12 | #define PULSE_MAX (2250) // maximum PWM pulse width which is considered valid 13 | 14 | typedef struct drv_pwm_config_t { 15 | bool enableInput; 16 | bool usePPM; 17 | bool useUART; 18 | bool useSoftSerial; 19 | bool useServos; 20 | bool extraServos; // configure additional 4 channels in PPM mode as servos, not motors 21 | bool airplane; // fixed wing hardware config, lots of servos etc 22 | uint8_t pwmFilter; // PWM ICFilter value for jittering input 23 | uint8_t adcChannel; // steal one RC input for current sensor 24 | uint16_t motorPwmRate; 25 | uint16_t servoPwmRate; 26 | uint16_t idlePulse; // PWM value to use when initializing the driver. set this to either PULSE_1MS (regular pwm), 27 | // some higher value (used by 3d mode), or 0, for brushed pwm drivers. 28 | uint16_t servoCenterPulse; 29 | uint16_t failsafeThreshold; 30 | 31 | // OUT parameters, filled by driver 32 | uint8_t numServos; 33 | } drv_pwm_config_t; 34 | 35 | // This indexes into the read-only hardware definition structure in drv_pwm.c, as well as into pwmPorts[] structure with dynamic data. 36 | enum { 37 | PWM1 = 0, 38 | PWM2, 39 | PWM3, 40 | PWM4, 41 | PWM5, 42 | PWM6, 43 | PWM7, 44 | PWM8, 45 | PWM9, 46 | PWM10, 47 | PWM11, 48 | PWM12, 49 | PWM13, 50 | PWM14, 51 | MAX_PORTS 52 | }; 53 | 54 | void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity); 55 | 56 | bool pwmInit(drv_pwm_config_t *init); // returns whether driver is asking to calibrate throttle or not 57 | void pwmWriteMotor(uint8_t index, uint16_t value); 58 | void pwmWriteServo(uint8_t index, uint16_t value); 59 | uint16_t pwmRead(uint8_t channel); 60 | 61 | // void pwmWrite(uint8_t channel, uint16_t value); 62 | -------------------------------------------------------------------------------- /lib/CMSIS/CM3/DeviceSupport/ST/STM32F10x/system_stm32f10x.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file system_stm32f10x.h 4 | * @author MCD Application Team 5 | * @version V3.5.0 6 | * @date 11-March-2011 7 | * @brief CMSIS Cortex-M3 Device Peripheral Access Layer System Header File. 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS 12 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE 13 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY 14 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING 15 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE 16 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. 17 | * 18 | *

© COPYRIGHT 2011 STMicroelectronics

19 | ****************************************************************************** 20 | */ 21 | 22 | /** @addtogroup CMSIS 23 | * @{ 24 | */ 25 | 26 | /** @addtogroup stm32f10x_system 27 | * @{ 28 | */ 29 | 30 | /** 31 | * @brief Define to prevent recursive inclusion 32 | */ 33 | #ifndef __SYSTEM_STM32F10X_H 34 | #define __SYSTEM_STM32F10X_H 35 | 36 | #ifdef __cplusplus 37 | extern "C" { 38 | #endif 39 | 40 | /** @addtogroup STM32F10x_System_Includes 41 | * @{ 42 | */ 43 | 44 | /** 45 | * @} 46 | */ 47 | 48 | 49 | /** @addtogroup STM32F10x_System_Exported_types 50 | * @{ 51 | */ 52 | 53 | extern uint32_t SystemCoreClock; /*!< System Clock Frequency (Core Clock) */ 54 | 55 | /** 56 | * @} 57 | */ 58 | 59 | /** @addtogroup STM32F10x_System_Exported_Constants 60 | * @{ 61 | */ 62 | 63 | /** 64 | * @} 65 | */ 66 | 67 | /** @addtogroup STM32F10x_System_Exported_Macros 68 | * @{ 69 | */ 70 | 71 | /** 72 | * @} 73 | */ 74 | 75 | /** @addtogroup STM32F10x_System_Exported_Functions 76 | * @{ 77 | */ 78 | 79 | extern void SystemInit(void); 80 | extern void SystemCoreClockUpdate(void); 81 | /** 82 | * @} 83 | */ 84 | 85 | #ifdef __cplusplus 86 | } 87 | #endif 88 | 89 | #endif /*__SYSTEM_STM32F10X_H */ 90 | 91 | /** 92 | * @} 93 | */ 94 | 95 | /** 96 | * @} 97 | */ 98 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ 99 | -------------------------------------------------------------------------------- /lib/STM32F10x_StdPeriph_Driver/inc/stm32f10x_crc.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f10x_crc.h 4 | * @author MCD Application Team 5 | * @version V3.5.0 6 | * @date 11-March-2011 7 | * @brief This file contains all the functions prototypes for the CRC firmware 8 | * library. 9 | ****************************************************************************** 10 | * @attention 11 | * 12 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS 13 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE 14 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY 15 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING 16 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE 17 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. 18 | * 19 | *

© COPYRIGHT 2011 STMicroelectronics

20 | ****************************************************************************** 21 | */ 22 | 23 | /* Define to prevent recursive inclusion -------------------------------------*/ 24 | #ifndef __STM32F10x_CRC_H 25 | #define __STM32F10x_CRC_H 26 | 27 | #ifdef __cplusplus 28 | extern "C" { 29 | #endif 30 | 31 | /* Includes ------------------------------------------------------------------*/ 32 | #include "stm32f10x.h" 33 | 34 | /** @addtogroup STM32F10x_StdPeriph_Driver 35 | * @{ 36 | */ 37 | 38 | /** @addtogroup CRC 39 | * @{ 40 | */ 41 | 42 | /** @defgroup CRC_Exported_Types 43 | * @{ 44 | */ 45 | 46 | /** 47 | * @} 48 | */ 49 | 50 | /** @defgroup CRC_Exported_Constants 51 | * @{ 52 | */ 53 | 54 | /** 55 | * @} 56 | */ 57 | 58 | /** @defgroup CRC_Exported_Macros 59 | * @{ 60 | */ 61 | 62 | /** 63 | * @} 64 | */ 65 | 66 | /** @defgroup CRC_Exported_Functions 67 | * @{ 68 | */ 69 | 70 | void CRC_ResetDR(void); 71 | uint32_t CRC_CalcCRC(uint32_t Data); 72 | uint32_t CRC_CalcBlockCRC(uint32_t pBuffer[], uint32_t BufferLength); 73 | uint32_t CRC_GetCRC(void); 74 | void CRC_SetIDRegister(uint8_t IDValue); 75 | uint8_t CRC_GetIDRegister(void); 76 | 77 | #ifdef __cplusplus 78 | } 79 | #endif 80 | 81 | #endif /* __STM32F10x_CRC_H */ 82 | /** 83 | * @} 84 | */ 85 | 86 | /** 87 | * @} 88 | */ 89 | 90 | /** 91 | * @} 92 | */ 93 | 94 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ 95 | -------------------------------------------------------------------------------- /src/drv_l3g4200d.c: -------------------------------------------------------------------------------- 1 | #include "board.h" 2 | 3 | // L3G4200D, Standard address 0x68 4 | #define L3G4200D_ADDRESS 0x68 5 | #define L3G4200D_ID 0xD3 6 | #define L3G4200D_AUTOINCR 0x80 7 | 8 | // Registers 9 | #define L3G4200D_WHO_AM_I 0x0F 10 | #define L3G4200D_CTRL_REG1 0x20 11 | #define L3G4200D_CTRL_REG2 0x21 12 | #define L3G4200D_CTRL_REG3 0x22 13 | #define L3G4200D_CTRL_REG4 0x23 14 | #define L3G4200D_CTRL_REG5 0x24 15 | #define L3G4200D_REFERENCE 0x25 16 | #define L3G4200D_STATUS_REG 0x27 17 | #define L3G4200D_GYRO_OUT 0x28 18 | 19 | // Bits 20 | #define L3G4200D_POWER_ON 0x0F 21 | #define L3G4200D_FS_SEL_2000DPS 0xF0 22 | #define L3G4200D_DLPF_32HZ 0x00 23 | #define L3G4200D_DLPF_54HZ 0x40 24 | #define L3G4200D_DLPF_78HZ 0x80 25 | #define L3G4200D_DLPF_93HZ 0xC0 26 | 27 | static uint8_t mpuLowPassFilter = L3G4200D_DLPF_32HZ; 28 | static sensor_align_e gyroAlign = CW0_DEG; 29 | 30 | static void l3g4200dInit(sensor_align_e align); 31 | static void l3g4200dRead(int16_t *gyroData); 32 | 33 | bool l3g4200dDetect(sensor_t *gyro, uint16_t lpf) 34 | { 35 | uint8_t deviceid; 36 | 37 | delay(25); 38 | 39 | i2cRead(L3G4200D_ADDRESS, L3G4200D_WHO_AM_I, 1, &deviceid); 40 | if (deviceid != L3G4200D_ID) 41 | return false; 42 | 43 | gyro->init = l3g4200dInit; 44 | gyro->read = l3g4200dRead; 45 | 46 | // 14.2857dps/lsb scalefactor 47 | gyro->scale = (((32767.0f / 14.2857f) * M_PI) / ((32767.0f / 4.0f) * 180.0f * 1000000.0f)); 48 | 49 | // default LPF is set to 32Hz 50 | switch (lpf) { 51 | default: 52 | case 32: 53 | mpuLowPassFilter = L3G4200D_DLPF_32HZ; 54 | break; 55 | case 54: 56 | mpuLowPassFilter = L3G4200D_DLPF_54HZ; 57 | break; 58 | case 78: 59 | mpuLowPassFilter = L3G4200D_DLPF_78HZ; 60 | break; 61 | case 93: 62 | mpuLowPassFilter = L3G4200D_DLPF_93HZ; 63 | break; 64 | } 65 | 66 | return true; 67 | } 68 | 69 | static void l3g4200dInit(sensor_align_e align) 70 | { 71 | bool ack; 72 | 73 | delay(100); 74 | 75 | ack = i2cWrite(L3G4200D_ADDRESS, L3G4200D_CTRL_REG4, L3G4200D_FS_SEL_2000DPS); 76 | if (!ack) 77 | failureMode(3); 78 | 79 | delay(5); 80 | i2cWrite(L3G4200D_ADDRESS, L3G4200D_CTRL_REG1, L3G4200D_POWER_ON | mpuLowPassFilter); 81 | if (align > 0) 82 | gyroAlign = align; 83 | } 84 | 85 | // Read 3 gyro values into user-provided buffer. No overrun checking is done. 86 | static void l3g4200dRead(int16_t *gyroData) 87 | { 88 | uint8_t buf[6]; 89 | int16_t data[3]; 90 | 91 | i2cRead(L3G4200D_ADDRESS, L3G4200D_AUTOINCR | L3G4200D_GYRO_OUT, 6, buf); 92 | data[X] = (int16_t)((buf[0] << 8) | buf[1]) / 4; 93 | data[Y] = (int16_t)((buf[2] << 8) | buf[3]) / 4; 94 | data[Z] = (int16_t)((buf[4] << 8) | buf[5]) / 4; 95 | 96 | alignSensors(data, gyroData, gyroAlign); 97 | } 98 | -------------------------------------------------------------------------------- /src/drv_gpio.c: -------------------------------------------------------------------------------- 1 | #include "board.h" 2 | 3 | void gpioInit(GPIO_TypeDef *gpio, gpio_config_t *config) 4 | { 5 | uint32_t pinpos; 6 | for (pinpos = 0; pinpos < 16; pinpos++) { 7 | // are we doing this pin? 8 | if (config->pin & (0x1 << pinpos)) { 9 | // reference CRL or CRH, depending whether pin number is 0..7 or 8..15 10 | __IO uint32_t *cr = &gpio->CRL + (pinpos / 8); 11 | // mask out extra bits from pinmode, leaving just CNF+MODE 12 | uint32_t currentmode = config->mode & 0x0F; 13 | // offset to CNF and MODE portions of CRx register 14 | uint32_t shift = (pinpos % 8) * 4; 15 | // Read out current CRx value 16 | uint32_t tmp = *cr; 17 | // if we're in output mode, add speed too. 18 | if (config->mode & 0x10) 19 | currentmode |= config->speed; 20 | // Mask out 4 bits 21 | tmp &= ~(0xF << shift); 22 | // apply current pinmode 23 | tmp |= currentmode << shift; 24 | *cr = tmp; 25 | // Special handling for IPD/IPU 26 | if (config->mode == Mode_IPD) { 27 | gpio->ODR &= ~(1U << pinpos); 28 | } else if (config->mode == Mode_IPU) { 29 | gpio->ODR |= (1U << pinpos); 30 | } 31 | } 32 | } 33 | } 34 | 35 | void gpioExtiLineConfig(uint8_t portsrc, uint8_t pinsrc) 36 | { 37 | uint32_t tmp = 0x00; 38 | 39 | tmp = ((uint32_t)0x0F) << (0x04 * (pinsrc & (uint8_t)0x03)); 40 | AFIO->EXTICR[pinsrc >> 0x02] &= ~tmp; 41 | AFIO->EXTICR[pinsrc >> 0x02] |= (((uint32_t)portsrc) << (0x04 * (pinsrc & (uint8_t)0x03))); 42 | } 43 | 44 | #define LSB_MASK ((uint16_t)0xFFFF) 45 | #define DBGAFR_POSITION_MASK ((uint32_t)0x000F0000) 46 | #define DBGAFR_SWJCFG_MASK ((uint32_t)0xF0FFFFFF) 47 | #define DBGAFR_LOCATION_MASK ((uint32_t)0x00200000) 48 | #define DBGAFR_NUMBITS_MASK ((uint32_t)0x00100000) 49 | 50 | void gpioPinRemapConfig(uint32_t remap, bool enable) 51 | { 52 | uint32_t tmp = 0x00, tmp1 = 0x00, tmpreg = 0x00, tmpmask = 0x00; 53 | if ((remap & 0x80000000) == 0x80000000) 54 | tmpreg = AFIO->MAPR2; 55 | else 56 | tmpreg = AFIO->MAPR; 57 | 58 | tmpmask = (remap & DBGAFR_POSITION_MASK) >> 0x10; 59 | tmp = remap & LSB_MASK; 60 | 61 | if ((remap & (DBGAFR_LOCATION_MASK | DBGAFR_NUMBITS_MASK)) == (DBGAFR_LOCATION_MASK | DBGAFR_NUMBITS_MASK)) { 62 | tmpreg &= DBGAFR_SWJCFG_MASK; 63 | AFIO->MAPR &= DBGAFR_SWJCFG_MASK; 64 | } else if ((remap & DBGAFR_NUMBITS_MASK) == DBGAFR_NUMBITS_MASK) { 65 | tmp1 = ((uint32_t)0x03) << tmpmask; 66 | tmpreg &= ~tmp1; 67 | tmpreg |= ~DBGAFR_SWJCFG_MASK; 68 | } else { 69 | tmpreg &= ~(tmp << ((remap >> 0x15) * 0x10)); 70 | tmpreg |= ~DBGAFR_SWJCFG_MASK; 71 | } 72 | 73 | if (enable) 74 | tmpreg |= (tmp << ((remap >> 0x15) * 0x10)); 75 | 76 | if ((remap & 0x80000000) == 0x80000000) 77 | AFIO->MAPR2 = tmpreg; 78 | else 79 | AFIO->MAPR = tmpreg; 80 | } 81 | -------------------------------------------------------------------------------- /src/drv_spi.c: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of baseflight 3 | * Licensed under GPL V3 or modified DCL - see https://github.com/multiwii/baseflight/blob/master/README.md 4 | */ 5 | 6 | #include "board.h" 7 | 8 | // SPI2 Driver 9 | // PB15 28 SPI2_MOSI 10 | // PB14 27 SPI2_MISO 11 | // PB13 26 SPI2_SCK 12 | // PB12 25 SPI2_NSS 13 | 14 | static int spiDetect(void); 15 | 16 | #define FLASH_M25P16 (0x202015) 17 | 18 | int spiInit(void) 19 | { 20 | gpio_config_t gpio; 21 | SPI_InitTypeDef spi; 22 | 23 | // Enable SPI2 clock 24 | RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2, ENABLE); 25 | 26 | // MOSI + SCK as output 27 | gpio.mode = Mode_AF_PP; 28 | gpio.pin = Pin_13 | Pin_15; 29 | gpio.speed = Speed_50MHz; 30 | gpioInit(GPIOB, &gpio); 31 | // MISO as input 32 | gpio.pin = Pin_14; 33 | gpio.mode = Mode_IN_FLOATING; 34 | gpioInit(GPIOB, &gpio); 35 | // NSS as gpio slave select 36 | gpio.pin = Pin_12; 37 | gpio.mode = Mode_Out_PP; 38 | gpioInit(GPIOB, &gpio); 39 | 40 | // Init SPI2 hardware 41 | SPI_I2S_DeInit(SPI2); 42 | spi.SPI_Mode = SPI_Mode_Master; 43 | spi.SPI_Direction = SPI_Direction_2Lines_FullDuplex; 44 | spi.SPI_DataSize = SPI_DataSize_8b; 45 | spi.SPI_NSS = SPI_NSS_Soft; 46 | spi.SPI_FirstBit = SPI_FirstBit_MSB; 47 | spi.SPI_CRCPolynomial = 7; 48 | spi.SPI_CPOL = SPI_CPOL_High; 49 | spi.SPI_CPHA = SPI_CPHA_2Edge; 50 | spi.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8; 51 | SPI_Init(SPI2, &spi); 52 | SPI_Cmd(SPI2, ENABLE); 53 | 54 | return spiDetect(); 55 | } 56 | 57 | void spiSelect(bool select) 58 | { 59 | if (select) { 60 | digitalLo(GPIOB, Pin_12); 61 | } else { 62 | digitalHi(GPIOB, Pin_12); 63 | } 64 | } 65 | 66 | uint8_t spiTransferByte(uint8_t in) 67 | { 68 | uint8_t rx; 69 | SPI2->DR; 70 | SPI2->DR = in; 71 | while (!(SPI2->SR & SPI_I2S_FLAG_RXNE)); 72 | rx = SPI2->DR; 73 | while (!(SPI2->SR & SPI_I2S_FLAG_TXE)); 74 | while (SPI2->SR & SPI_I2S_FLAG_BSY); 75 | return rx; 76 | } 77 | 78 | bool spiTransfer(uint8_t *out, uint8_t *in, int len) 79 | { 80 | uint8_t b; 81 | SPI2->DR; 82 | while (len--) { 83 | b = in ? *(in++) : 0xFF; 84 | while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET); 85 | SPI_I2S_SendData(SPI2, b); 86 | while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_RXNE) == RESET); 87 | b = SPI_I2S_ReceiveData(SPI2); 88 | if (out) 89 | *(out++) = b; 90 | } 91 | 92 | return true; 93 | } 94 | 95 | static int spiDetect(void) 96 | { 97 | uint8_t out[] = { 0x9F, 0, 0, 0 }; 98 | uint8_t in[4]; 99 | uint32_t flash_id; 100 | 101 | // try autodetect flash chip 102 | spiSelect(true); 103 | spiTransfer(in, out, sizeof(out)); 104 | spiSelect(false); 105 | flash_id = in[1] << 16 | in[2] << 8 | in[3]; 106 | if (flash_id == FLASH_M25P16) 107 | return SPI_DEVICE_FLASH; 108 | 109 | // try autodetect MPU 110 | delay(50); 111 | spiSelect(true); 112 | spiTransferByte(0x75 | 0x80); 113 | in[0] = spiTransferByte(0xff); 114 | spiSelect(false); 115 | if (in[0] == 0x70) 116 | return SPI_DEVICE_MPU; 117 | 118 | return SPI_DEVICE_NONE; 119 | } 120 | -------------------------------------------------------------------------------- /src/blackbox_fielddefs.h: -------------------------------------------------------------------------------- 1 | #ifndef BLACKBOX_FIELDDEFS_H_ 2 | #define BLACKBOX_FIELDDEFS_H_ 3 | 4 | typedef enum FlightLogFieldCondition { 5 | FLIGHT_LOG_FIELD_CONDITION_ALWAYS = 0, 6 | FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1, 7 | FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_2, 8 | FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_3, 9 | FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_4, 10 | FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_5, 11 | FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_6, 12 | FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_7, 13 | FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_8, 14 | FLIGHT_LOG_FIELD_CONDITION_TRICOPTER, 15 | 16 | FLIGHT_LOG_FIELD_CONDITION_MAG, 17 | FLIGHT_LOG_FIELD_CONDITION_BARO, 18 | FLIGHT_LOG_FIELD_CONDITION_VBAT, 19 | 20 | FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0, 21 | FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1, 22 | FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_2, 23 | 24 | FLIGHT_LOG_FIELD_CONDITION_NEVER, 25 | 26 | FLIGHT_LOG_FIELD_CONDITION_FIRST = FLIGHT_LOG_FIELD_CONDITION_ALWAYS, 27 | FLIGHT_LOG_FIELD_CONDITION_LAST = FLIGHT_LOG_FIELD_CONDITION_NEVER 28 | } FlightLogFieldCondition; 29 | 30 | typedef enum FlightLogFieldPredictor { 31 | //No prediction: 32 | FLIGHT_LOG_FIELD_PREDICTOR_0 = 0, 33 | 34 | //Predict that the field is the same as last frame: 35 | FLIGHT_LOG_FIELD_PREDICTOR_PREVIOUS = 1, 36 | 37 | //Predict that the slope between this field and the previous item is the same as that between the past two history items: 38 | FLIGHT_LOG_FIELD_PREDICTOR_STRAIGHT_LINE = 2, 39 | 40 | //Predict that this field is the same as the average of the last two history items: 41 | FLIGHT_LOG_FIELD_PREDICTOR_AVERAGE_2 = 3, 42 | 43 | //Predict that this field is minthrottle 44 | FLIGHT_LOG_FIELD_PREDICTOR_MINTHROTTLE = 4, 45 | 46 | //Predict that this field is the same as motor 0 47 | FLIGHT_LOG_FIELD_PREDICTOR_MOTOR_0 = 5, 48 | 49 | //This field always increments 50 | FLIGHT_LOG_FIELD_PREDICTOR_INC = 6, 51 | 52 | //Predict this GPS co-ordinate is the GPS home co-ordinate (or no prediction if that coordinate is not set) 53 | FLIGHT_LOG_FIELD_PREDICTOR_HOME_COORD = 7, 54 | 55 | //Predict 1500 56 | FLIGHT_LOG_FIELD_PREDICTOR_1500 = 8, 57 | 58 | //Predict vbatref, the reference ADC level stored in the header 59 | FLIGHT_LOG_FIELD_PREDICTOR_VBATREF = 9 60 | 61 | } FlightLogFieldPredictor; 62 | 63 | typedef enum FlightLogFieldEncoding { 64 | FLIGHT_LOG_FIELD_ENCODING_SIGNED_VB = 0, // Signed variable-byte 65 | FLIGHT_LOG_FIELD_ENCODING_UNSIGNED_VB = 1, // Unsigned variable-byte 66 | FLIGHT_LOG_FIELD_ENCODING_NEG_14BIT = 3, // Unsigned variable-byte but we negate the value before storing, value is 14 bits 67 | FLIGHT_LOG_FIELD_ENCODING_TAG8_8SVB = 6, 68 | FLIGHT_LOG_FIELD_ENCODING_TAG2_3S32 = 7, 69 | FLIGHT_LOG_FIELD_ENCODING_TAG8_4S16 = 8, 70 | FLIGHT_LOG_FIELD_ENCODING_NULL = 9 // Nothing is written to the file, take value to be zero 71 | } FlightLogFieldEncoding; 72 | 73 | typedef enum FlightLogFieldSign { 74 | FLIGHT_LOG_FIELD_UNSIGNED = 0, 75 | FLIGHT_LOG_FIELD_SIGNED = 1 76 | } FlightLogFieldSign; 77 | 78 | typedef enum FlightLogEvent { 79 | FLIGHT_LOG_EVENT_SYNC_BEEP = 0, 80 | FLIGHT_LOG_EVENT_LOG_END = 255 81 | } FlightLogEvent; 82 | 83 | #endif 84 | -------------------------------------------------------------------------------- /lib/STM32F10x_StdPeriph_Driver/inc/stm32f10x_wwdg.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f10x_wwdg.h 4 | * @author MCD Application Team 5 | * @version V3.5.0 6 | * @date 11-March-2011 7 | * @brief This file contains all the functions prototypes for the WWDG firmware 8 | * library. 9 | ****************************************************************************** 10 | * @attention 11 | * 12 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS 13 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE 14 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY 15 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING 16 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE 17 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. 18 | * 19 | *

© COPYRIGHT 2011 STMicroelectronics

20 | ****************************************************************************** 21 | */ 22 | 23 | /* Define to prevent recursive inclusion -------------------------------------*/ 24 | #ifndef __STM32F10x_WWDG_H 25 | #define __STM32F10x_WWDG_H 26 | 27 | #ifdef __cplusplus 28 | extern "C" { 29 | #endif 30 | 31 | /* Includes ------------------------------------------------------------------*/ 32 | #include "stm32f10x.h" 33 | 34 | /** @addtogroup STM32F10x_StdPeriph_Driver 35 | * @{ 36 | */ 37 | 38 | /** @addtogroup WWDG 39 | * @{ 40 | */ 41 | 42 | /** @defgroup WWDG_Exported_Types 43 | * @{ 44 | */ 45 | 46 | /** 47 | * @} 48 | */ 49 | 50 | /** @defgroup WWDG_Exported_Constants 51 | * @{ 52 | */ 53 | 54 | /** @defgroup WWDG_Prescaler 55 | * @{ 56 | */ 57 | 58 | #define WWDG_Prescaler_1 ((uint32_t)0x00000000) 59 | #define WWDG_Prescaler_2 ((uint32_t)0x00000080) 60 | #define WWDG_Prescaler_4 ((uint32_t)0x00000100) 61 | #define WWDG_Prescaler_8 ((uint32_t)0x00000180) 62 | #define IS_WWDG_PRESCALER(PRESCALER) (((PRESCALER) == WWDG_Prescaler_1) || \ 63 | ((PRESCALER) == WWDG_Prescaler_2) || \ 64 | ((PRESCALER) == WWDG_Prescaler_4) || \ 65 | ((PRESCALER) == WWDG_Prescaler_8)) 66 | #define IS_WWDG_WINDOW_VALUE(VALUE) ((VALUE) <= 0x7F) 67 | #define IS_WWDG_COUNTER(COUNTER) (((COUNTER) >= 0x40) && ((COUNTER) <= 0x7F)) 68 | 69 | /** 70 | * @} 71 | */ 72 | 73 | /** 74 | * @} 75 | */ 76 | 77 | /** @defgroup WWDG_Exported_Macros 78 | * @{ 79 | */ 80 | /** 81 | * @} 82 | */ 83 | 84 | /** @defgroup WWDG_Exported_Functions 85 | * @{ 86 | */ 87 | 88 | void WWDG_DeInit(void); 89 | void WWDG_SetPrescaler(uint32_t WWDG_Prescaler); 90 | void WWDG_SetWindowValue(uint8_t WindowValue); 91 | void WWDG_EnableIT(void); 92 | void WWDG_SetCounter(uint8_t Counter); 93 | void WWDG_Enable(uint8_t Counter); 94 | FlagStatus WWDG_GetFlagStatus(void); 95 | void WWDG_ClearFlag(void); 96 | 97 | #ifdef __cplusplus 98 | } 99 | #endif 100 | 101 | #endif /* __STM32F10x_WWDG_H */ 102 | 103 | /** 104 | * @} 105 | */ 106 | 107 | /** 108 | * @} 109 | */ 110 | 111 | /** 112 | * @} 113 | */ 114 | 115 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ 116 | -------------------------------------------------------------------------------- /lib/CMSIS/CM3/DeviceSupport/ST/STM32F10x/stm32f10x_conf.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file Project/STM32F10x_StdPeriph_Template/stm32f10x_conf.h 4 | * @author MCD Application Team 5 | * @version V3.5.0 6 | * @date 08-April-2011 7 | * @brief Library configuration file. 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS 12 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE 13 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY 14 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING 15 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE 16 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. 17 | * 18 | *

© COPYRIGHT 2011 STMicroelectronics

19 | ****************************************************************************** 20 | */ 21 | 22 | /* Define to prevent recursive inclusion -------------------------------------*/ 23 | #ifndef __STM32F10x_CONF_H 24 | #define __STM32F10x_CONF_H 25 | 26 | /* Includes ------------------------------------------------------------------*/ 27 | /* Uncomment/Comment the line below to enable/disable peripheral header file inclusion */ 28 | #include "stm32f10x_adc.h" 29 | #include "stm32f10x_bkp.h" 30 | #include "stm32f10x_can.h" 31 | #include "stm32f10x_cec.h" 32 | #include "stm32f10x_crc.h" 33 | #include "stm32f10x_dac.h" 34 | #include "stm32f10x_dbgmcu.h" 35 | #include "stm32f10x_dma.h" 36 | #include "stm32f10x_exti.h" 37 | #include "stm32f10x_flash.h" 38 | #include "stm32f10x_fsmc.h" 39 | #include "stm32f10x_gpio.h" 40 | #include "stm32f10x_i2c.h" 41 | #include "stm32f10x_iwdg.h" 42 | #include "stm32f10x_pwr.h" 43 | #include "stm32f10x_rcc.h" 44 | #include "stm32f10x_rtc.h" 45 | #include "stm32f10x_sdio.h" 46 | #include "stm32f10x_spi.h" 47 | #include "stm32f10x_tim.h" 48 | #include "stm32f10x_usart.h" 49 | #include "stm32f10x_wwdg.h" 50 | #include "misc.h" /* High level functions for NVIC and SysTick (add-on to CMSIS functions) */ 51 | 52 | extern void assert_param(int val); 53 | 54 | /* Exported types ------------------------------------------------------------*/ 55 | /* Exported constants --------------------------------------------------------*/ 56 | /* Uncomment the line below to expanse the "assert_param" macro in the 57 | Standard Peripheral Library drivers code */ 58 | // #define USE_FULL_ASSERT 1 59 | 60 | /* Exported macro ------------------------------------------------------------*/ 61 | #ifdef USE_FULL_ASSERT 62 | 63 | /** 64 | * @brief The assert_param macro is used for function's parameters check. 65 | * @param expr: If expr is false, it calls assert_failed function which reports 66 | * the name of the source file and the source line number of the call 67 | * that failed. If expr is true, it returns no value. 68 | * @retval None 69 | */ 70 | #define assert_param(expr) ((expr) ? (void)0 : assert_failed((uint8_t *)__FILE__, __LINE__)) 71 | /* Exported functions ------------------------------------------------------- */ 72 | void assert_failed(uint8_t* file, uint32_t line); 73 | #else 74 | #define assert_param(expr) ((void)0) 75 | #endif /* USE_FULL_ASSERT */ 76 | 77 | #endif /* __STM32F10x_CONF_H */ 78 | 79 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ 80 | -------------------------------------------------------------------------------- /support/stmloader/loader.c: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of AutoQuad. 3 | 4 | AutoQuad is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | AutoQuad is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | You should have received a copy of the GNU General Public License 14 | along with AutoQuad. If not, see . 15 | 16 | Copyright © 2011 Bill Nesbitt 17 | */ 18 | 19 | #include "serial.h" 20 | #include "stmbootloader.h" 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #define DEFAULT_PORT "/dev/ttyUSB0" 30 | #define DEFAULT_BAUD 115200 31 | #define FIRMWARE_FILENAME "STM32.hex" 32 | 33 | serialStruct_t *s; 34 | 35 | char port[256]; 36 | unsigned int baud; 37 | unsigned char overrideParity; 38 | unsigned char noSendR; 39 | char firmFile[256]; 40 | 41 | void loaderUsage(void) { 42 | fprintf(stderr, "usage: loader <-h> <-p device_file> <-b baud_rate> <-f firmware_file> <-o> <-n>\n"); 43 | } 44 | 45 | unsigned int loaderOptions(int argc, char **argv) { 46 | int ch; 47 | 48 | strncpy(port, DEFAULT_PORT, sizeof(port)); 49 | baud = DEFAULT_BAUD; 50 | overrideParity = 0; 51 | noSendR = 0; 52 | strncpy(firmFile, FIRMWARE_FILENAME, sizeof(firmFile)); 53 | 54 | /* options descriptor */ 55 | static struct option longopts[] = { 56 | { "help", required_argument, NULL, 'h' }, 57 | { "port", required_argument, NULL, 'p' }, 58 | { "baud", required_argument, NULL, 's' }, 59 | { "firm_file", required_argument, NULL, 'f' }, 60 | { "override_parity", no_argument, NULL, 'o' }, 61 | { "no_send_r", no_argument, NULL, 'n' }, 62 | { NULL, 0, NULL, 0 } 63 | }; 64 | 65 | while ((ch = getopt_long(argc, argv, "hp:b:f:o:n", longopts, NULL)) != -1) 66 | switch (ch) { 67 | case 'h': 68 | loaderUsage(); 69 | exit(0); 70 | break; 71 | case 'p': 72 | strncpy(port, optarg, sizeof(port)); 73 | break; 74 | case 'b': 75 | baud = atoi(optarg); 76 | break; 77 | case 'f': 78 | strncpy(firmFile, optarg, sizeof(firmFile)); 79 | break; 80 | case 'o': 81 | overrideParity = 1; 82 | break; 83 | case 'n': 84 | noSendR = 1; 85 | break; 86 | default: 87 | loaderUsage(); 88 | return 0; 89 | } 90 | argc -= optind; 91 | argv += optind; 92 | 93 | return 1; 94 | } 95 | 96 | int main(int argc, char **argv) { 97 | FILE *fw; 98 | 99 | // init 100 | if (!loaderOptions(argc, argv)) { 101 | fprintf(stderr, "Init failed, aborting\n"); 102 | return 1; 103 | } 104 | 105 | s = initSerial(port, baud, 0); 106 | if (!s) { 107 | fprintf(stderr, "Cannot open serial port '%s', aborting.\n", port); 108 | return 1; 109 | } 110 | 111 | fw = fopen(firmFile, "r"); 112 | if (!fw) { 113 | fprintf(stderr, "Cannot open firmware file '%s', aborting.\n", firmFile); 114 | return 1; 115 | } 116 | else { 117 | printf("Upgrading STM on port %s from %s...\n", port, firmFile); 118 | stmLoader(s, fw, overrideParity, noSendR); 119 | } 120 | 121 | return 0; 122 | } 123 | -------------------------------------------------------------------------------- /src/drv_adc.c: -------------------------------------------------------------------------------- 1 | #include "board.h" 2 | 3 | // Driver for STM32F103CB onboard ADC 4 | // VBAT is connected to PA4 (ADC1_IN4) with 10k:1k divider 5 | // rev.5 hardware has PA5 (ADC1_IN5) on breakout pad on bottom of board 6 | // Additional channel can be stolen from RC_CH2 (PA1, ADC1_IN1) or 7 | // RC_CH8 (PB1, ADC1_IN9) by using set power_adc_channel=1|9 8 | 9 | typedef struct adc_config_t { 10 | uint8_t adcChannel; // ADC1_INxx channel number 11 | uint8_t dmaIndex; // index into DMA buffer in case of sparse channels 12 | } adc_config_t; 13 | 14 | static adc_config_t adcConfig[ADC_CHANNEL_MAX]; 15 | static volatile uint16_t adcValues[ADC_CHANNEL_MAX]; 16 | 17 | void adcInit(drv_adc_config_t *init) 18 | { 19 | ADC_InitTypeDef adc; 20 | DMA_InitTypeDef dma; 21 | int numChannels = 1, i, rank = 1; 22 | 23 | // configure always-present battery index (ADC4) 24 | adcConfig[ADC_BATTERY].adcChannel = ADC_Channel_4; 25 | adcConfig[ADC_BATTERY].dmaIndex = numChannels - 1; 26 | 27 | // optional ADC5 input on rev.5 hardware 28 | if (hw_revision >= NAZE32_REV5) { 29 | numChannels++; 30 | adcConfig[ADC_EXTERNAL_PAD].adcChannel = ADC_Channel_5; 31 | adcConfig[ADC_EXTERNAL_PAD].dmaIndex = numChannels - 1; 32 | } 33 | // another channel can be stolen from PWM for current measurement or other things 34 | if (init->powerAdcChannel > 0) { 35 | numChannels++; 36 | adcConfig[ADC_EXTERNAL_CURRENT].adcChannel = init->powerAdcChannel; 37 | adcConfig[ADC_EXTERNAL_CURRENT].dmaIndex = numChannels - 1; 38 | } 39 | 40 | if (init->rssiAdcChannel > 0) { 41 | numChannels++; 42 | adcConfig[ADC_RSSI].adcChannel = init->rssiAdcChannel; 43 | adcConfig[ADC_RSSI].dmaIndex = numChannels - 1; 44 | } 45 | 46 | // ADC driver assumes all the GPIO was already placed in 'AIN' mode 47 | DMA_DeInit(DMA1_Channel1); 48 | dma.DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR; 49 | dma.DMA_MemoryBaseAddr = (uint32_t)adcValues; 50 | dma.DMA_DIR = DMA_DIR_PeripheralSRC; 51 | dma.DMA_BufferSize = numChannels; 52 | dma.DMA_PeripheralInc = DMA_PeripheralInc_Disable; 53 | dma.DMA_MemoryInc = numChannels > 1 ? DMA_MemoryInc_Enable : DMA_MemoryInc_Disable; 54 | dma.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; 55 | dma.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord; 56 | dma.DMA_Mode = DMA_Mode_Circular; 57 | dma.DMA_Priority = DMA_Priority_High; 58 | dma.DMA_M2M = DMA_M2M_Disable; 59 | DMA_Init(DMA1_Channel1, &dma); 60 | DMA_Cmd(DMA1_Channel1, ENABLE); 61 | 62 | adc.ADC_Mode = ADC_Mode_Independent; 63 | adc.ADC_ScanConvMode = numChannels > 1 ? ENABLE : DISABLE; 64 | adc.ADC_ContinuousConvMode = ENABLE; 65 | adc.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None; 66 | adc.ADC_DataAlign = ADC_DataAlign_Right; 67 | adc.ADC_NbrOfChannel = numChannels; 68 | ADC_Init(ADC1, &adc); 69 | 70 | // Configure ADC channels. Skip if channel is zero, means can't use PA0, but OK for this situation. 71 | for (i = 0; i < ADC_CHANNEL_MAX; i++) 72 | if (adcConfig[i].adcChannel > 0) 73 | ADC_RegularChannelConfig(ADC1, adcConfig[i].adcChannel, rank++, ADC_SampleTime_239Cycles5); 74 | ADC_DMACmd(ADC1, ENABLE); 75 | 76 | ADC_Cmd(ADC1, ENABLE); 77 | 78 | // Calibrate ADC 79 | ADC_ResetCalibration(ADC1); 80 | while(ADC_GetResetCalibrationStatus(ADC1)); 81 | ADC_StartCalibration(ADC1); 82 | while(ADC_GetCalibrationStatus(ADC1)); 83 | 84 | // Fire off ADC 85 | ADC_SoftwareStartConvCmd(ADC1, ENABLE); 86 | } 87 | 88 | uint16_t adcGetChannel(uint8_t channel) 89 | { 90 | return adcValues[adcConfig[channel].dmaIndex]; 91 | } 92 | -------------------------------------------------------------------------------- /src/telemetry_common.c: -------------------------------------------------------------------------------- 1 | #include "board.h" 2 | #include "mw.h" 3 | 4 | #include "telemetry_frsky.h" 5 | #include "telemetry_hott.h" 6 | 7 | static bool isTelemetryConfigurationValid = false; // flag used to avoid repeated configuration checks 8 | 9 | bool isTelemetryProviderFrSky(void) 10 | { 11 | return mcfg.telemetry_provider == TELEMETRY_PROVIDER_FRSKY; 12 | } 13 | 14 | bool isTelemetryProviderHoTT(void) 15 | { 16 | return mcfg.telemetry_provider == TELEMETRY_PROVIDER_HOTT; 17 | } 18 | 19 | bool canUseTelemetryWithCurrentConfiguration(void) 20 | { 21 | 22 | if (!feature(FEATURE_TELEMETRY)) { 23 | return false; 24 | } 25 | 26 | if (!feature(FEATURE_SOFTSERIAL)) { 27 | if (mcfg.telemetry_port == TELEMETRY_PORT_SOFTSERIAL_1 || mcfg.telemetry_port == TELEMETRY_PORT_SOFTSERIAL_2) { 28 | // softserial feature must be enabled to use telemetry on softserial ports 29 | return false; 30 | } 31 | } 32 | 33 | if (isTelemetryProviderHoTT()) { 34 | if (mcfg.telemetry_port == TELEMETRY_PORT_UART) { 35 | // HoTT requires a serial port that supports RX/TX mode swapping 36 | return false; 37 | } 38 | } 39 | 40 | return true; 41 | } 42 | 43 | void initTelemetry(void) 44 | { 45 | // Force telemetry to uart when softserial disabled 46 | if (!feature(FEATURE_SOFTSERIAL)) 47 | mcfg.telemetry_port = TELEMETRY_PORT_UART; 48 | 49 | isTelemetryConfigurationValid = canUseTelemetryWithCurrentConfiguration(); 50 | 51 | if (mcfg.telemetry_port == TELEMETRY_PORT_SOFTSERIAL_1) 52 | core.telemport = &(softSerialPorts[0].port); 53 | else if (mcfg.telemetry_port == TELEMETRY_PORT_SOFTSERIAL_2) 54 | core.telemport = &(softSerialPorts[1].port); 55 | else 56 | core.telemport = core.mainport; 57 | 58 | checkTelemetryState(); 59 | } 60 | 61 | static bool telemetryEnabled = false; 62 | 63 | bool determineNewTelemetryEnabledState(void) 64 | { 65 | bool enabled = true; 66 | 67 | if (mcfg.telemetry_port == TELEMETRY_PORT_UART) { 68 | if (!mcfg.telemetry_switch) 69 | enabled = f.ARMED; 70 | else 71 | enabled = rcOptions[BOXTELEMETRY]; 72 | } 73 | 74 | return enabled; 75 | } 76 | 77 | bool shouldChangeTelemetryStateNow(bool newState) 78 | { 79 | return newState != telemetryEnabled; 80 | } 81 | 82 | static void configureTelemetryPort(void) 83 | { 84 | if (isTelemetryProviderFrSky()) { 85 | configureFrSkyTelemetryPort(); 86 | } 87 | 88 | if (isTelemetryProviderHoTT()) { 89 | configureHoTTTelemetryPort(); 90 | } 91 | } 92 | 93 | void freeTelemetryPort(void) 94 | { 95 | if (isTelemetryProviderFrSky()) { 96 | freeFrSkyTelemetryPort(); 97 | } 98 | 99 | if (isTelemetryProviderHoTT()) { 100 | freeHoTTTelemetryPort(); 101 | } 102 | } 103 | 104 | void checkTelemetryState(void) 105 | { 106 | if (!isTelemetryConfigurationValid) { 107 | return; 108 | } 109 | 110 | bool newEnabledState = determineNewTelemetryEnabledState(); 111 | 112 | if (!shouldChangeTelemetryStateNow(newEnabledState)) { 113 | return; 114 | } 115 | 116 | if (newEnabledState) 117 | configureTelemetryPort(); 118 | else 119 | freeTelemetryPort(); 120 | 121 | telemetryEnabled = newEnabledState; 122 | } 123 | 124 | void handleTelemetry(void) 125 | { 126 | if (!isTelemetryConfigurationValid || !determineNewTelemetryEnabledState()) 127 | return; 128 | 129 | if (isTelemetryProviderFrSky()) { 130 | handleFrSkyTelemetry(); 131 | } 132 | 133 | if (isTelemetryProviderHoTT()) { 134 | handleHoTTTelemetry(); 135 | } 136 | } 137 | -------------------------------------------------------------------------------- /src/sbus.c: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of baseflight 3 | * Licensed under GPL V3 or modified DCL - see https://github.com/multiwii/baseflight/blob/master/README.md 4 | */ 5 | 6 | #include "board.h" 7 | #include "mw.h" 8 | 9 | // driver for SBUS receiver using UART2 10 | 11 | #define SBUS_MAX_CHANNEL 8 12 | #define SBUS_FRAME_SIZE 25 13 | #define SBUS_SYNCBYTE 0x0F 14 | 15 | static bool sbusFrameDone = false; 16 | static void sbusDataReceive(uint16_t c); 17 | static uint16_t sbusReadRawRC(uint8_t chan); 18 | 19 | // external vars (ugh) 20 | extern int16_t failsafeCnt; 21 | 22 | static uint32_t sbusChannelData[SBUS_MAX_CHANNEL]; 23 | 24 | void sbusInit(rcReadRawDataPtr *callback) 25 | { 26 | int b; 27 | for (b = 0; b < SBUS_MAX_CHANNEL; b++) 28 | sbusChannelData[b] = (1.6f * mcfg.midrc) - 1408; 29 | 30 | // Configure hardware inverter on PB2. If not available, this has no effect. 31 | INV_ON; 32 | core.rcvrport = uartOpen(USART2, sbusDataReceive, 100000, (portMode_t)(MODE_RX | MODE_SBUS)); 33 | if (callback) 34 | *callback = sbusReadRawRC; 35 | core.numRCChannels = SBUS_MAX_CHANNEL; 36 | } 37 | 38 | struct sbus_dat { 39 | unsigned int chan0 : 11; 40 | unsigned int chan1 : 11; 41 | unsigned int chan2 : 11; 42 | unsigned int chan3 : 11; 43 | unsigned int chan4 : 11; 44 | unsigned int chan5 : 11; 45 | unsigned int chan6 : 11; 46 | unsigned int chan7 : 11; 47 | unsigned int chan8 : 11; 48 | unsigned int chan9 : 11; 49 | unsigned int chan10 : 11; 50 | unsigned int chan11 : 11; 51 | } __attribute__ ((__packed__)); 52 | 53 | typedef union { 54 | uint8_t in[SBUS_FRAME_SIZE]; 55 | struct sbus_dat msg; 56 | } sbus_msg; 57 | 58 | static sbus_msg sbus; 59 | 60 | // Receive ISR callback 61 | static void sbusDataReceive(uint16_t c) 62 | { 63 | uint32_t sbusTime; 64 | static uint32_t sbusTimeLast; 65 | static uint8_t sbusFramePosition; 66 | 67 | sbusTime = micros(); 68 | if ((sbusTime - sbusTimeLast) > 2500) // sbus2 fast timing 69 | sbusFramePosition = 0; 70 | sbusTimeLast = sbusTime; 71 | 72 | if (sbusFramePosition == 0 && c != SBUS_SYNCBYTE) 73 | return; 74 | 75 | sbusFrameDone = false; // lazy main loop didnt fetch the stuff 76 | if (sbusFramePosition != 0) 77 | sbus.in[sbusFramePosition - 1] = (uint8_t)c; 78 | 79 | if (sbusFramePosition == SBUS_FRAME_SIZE - 1) { 80 | sbusFrameDone = true; 81 | sbusFramePosition = 0; 82 | } else { 83 | sbusFramePosition++; 84 | } 85 | } 86 | 87 | bool sbusFrameComplete(void) 88 | { 89 | if (!sbusFrameDone) { 90 | return false; 91 | } 92 | sbusFrameDone = false; 93 | if (feature(FEATURE_FAILSAFE) && ((sbus.in[22] >> 3) & 0x0001)) { 94 | // internal failsafe enabled and rx failsafe flag set 95 | return false; 96 | } 97 | failsafeCnt = 0; // clear FailSafe counter 98 | sbusChannelData[0] = sbus.msg.chan0; 99 | sbusChannelData[1] = sbus.msg.chan1; 100 | sbusChannelData[2] = sbus.msg.chan2; 101 | sbusChannelData[3] = sbus.msg.chan3; 102 | sbusChannelData[4] = sbus.msg.chan4; 103 | sbusChannelData[5] = sbus.msg.chan5; 104 | sbusChannelData[6] = sbus.msg.chan6; 105 | sbusChannelData[7] = sbus.msg.chan7; 106 | // need more channels? No problem. Add them. 107 | return true; 108 | } 109 | 110 | static uint16_t sbusReadRawRC(uint8_t chan) 111 | { 112 | // Linear fitting values read from OpenTX-ppmus and comparing with values received by X4R 113 | // http://www.wolframalpha.com/input/?i=linear+fit+%7B173%2C+988%7D%2C+%7B1812%2C+2012%7D%2C+%7B993%2C+1500%7D 114 | // No actual Futaba hardware to test with. Sorry. 115 | return (0.625f * sbusChannelData[mcfg.rcmap[chan]]) + 880; 116 | } 117 | -------------------------------------------------------------------------------- /src/drv_mpu3050.c: -------------------------------------------------------------------------------- 1 | #include "board.h" 2 | 3 | // MPU3050, Standard address 0x68 4 | #define MPU3050_ADDRESS 0x68 5 | 6 | // Registers 7 | #define MPU3050_SMPLRT_DIV 0x15 8 | #define MPU3050_DLPF_FS_SYNC 0x16 9 | #define MPU3050_INT_CFG 0x17 10 | #define MPU3050_TEMP_OUT 0x1B 11 | #define MPU3050_GYRO_OUT 0x1D 12 | #define MPU3050_USER_CTRL 0x3D 13 | #define MPU3050_PWR_MGM 0x3E 14 | 15 | // Bits 16 | #define MPU3050_FS_SEL_2000DPS 0x18 17 | #define MPU3050_DLPF_10HZ 0x05 18 | #define MPU3050_DLPF_20HZ 0x04 19 | #define MPU3050_DLPF_42HZ 0x03 20 | #define MPU3050_DLPF_98HZ 0x02 21 | #define MPU3050_DLPF_188HZ 0x01 22 | #define MPU3050_DLPF_256HZ 0x00 23 | 24 | #define MPU3050_USER_RESET 0x01 25 | #define MPU3050_CLK_SEL_PLL_GX 0x01 26 | 27 | static uint8_t mpuLowPassFilter = MPU3050_DLPF_42HZ; 28 | static sensor_align_e gyroAlign = CW0_DEG; 29 | 30 | static void mpu3050Init(sensor_align_e align); 31 | static void mpu3050Read(int16_t *gyroData); 32 | static void mpu3050ReadTemp(int16_t *tempData); 33 | 34 | bool mpu3050Detect(sensor_t *gyro, uint16_t lpf) 35 | { 36 | bool ack; 37 | 38 | delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe 39 | 40 | ack = i2cWrite(MPU3050_ADDRESS, MPU3050_SMPLRT_DIV, 0); 41 | if (!ack) 42 | return false; 43 | 44 | gyro->init = mpu3050Init; 45 | gyro->read = mpu3050Read; 46 | gyro->temperature = mpu3050ReadTemp; 47 | // 16.4 dps/lsb scalefactor 48 | gyro->scale = (((32767.0f / 16.4f) * M_PI) / ((32767.0f / 4.0f) * 180.0f * 1000000.0f)); 49 | 50 | // default lpf is 42Hz 51 | switch (lpf) { 52 | case 256: 53 | mpuLowPassFilter = MPU3050_DLPF_256HZ; 54 | break; 55 | case 188: 56 | mpuLowPassFilter = MPU3050_DLPF_188HZ; 57 | break; 58 | case 98: 59 | mpuLowPassFilter = MPU3050_DLPF_98HZ; 60 | break; 61 | default: 62 | case 42: 63 | mpuLowPassFilter = MPU3050_DLPF_42HZ; 64 | break; 65 | case 20: 66 | mpuLowPassFilter = MPU3050_DLPF_20HZ; 67 | break; 68 | case 10: 69 | mpuLowPassFilter = MPU3050_DLPF_10HZ; 70 | break; 71 | } 72 | 73 | return true; 74 | } 75 | 76 | static void mpu3050Init(sensor_align_e align) 77 | { 78 | bool ack; 79 | 80 | delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe 81 | 82 | ack = i2cWrite(MPU3050_ADDRESS, MPU3050_SMPLRT_DIV, 0); 83 | if (!ack) 84 | failureMode(3); 85 | 86 | i2cWrite(MPU3050_ADDRESS, MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | mpuLowPassFilter); 87 | i2cWrite(MPU3050_ADDRESS, MPU3050_INT_CFG, 0); 88 | i2cWrite(MPU3050_ADDRESS, MPU3050_USER_CTRL, MPU3050_USER_RESET); 89 | i2cWrite(MPU3050_ADDRESS, MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX); 90 | 91 | if (align > 0) 92 | gyroAlign = align; 93 | } 94 | 95 | // Read 3 gyro values into user-provided buffer. No overrun checking is done. 96 | static void mpu3050Read(int16_t *gyroData) 97 | { 98 | uint8_t buf[6]; 99 | int16_t data[3]; 100 | 101 | i2cRead(MPU3050_ADDRESS, MPU3050_GYRO_OUT, 6, buf); 102 | data[0] = (int16_t)((buf[0] << 8) | buf[1]) / 4; 103 | data[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4; 104 | data[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4; 105 | 106 | alignSensors(data, gyroData, gyroAlign); 107 | } 108 | 109 | static void mpu3050ReadTemp(int16_t *tempData) 110 | { 111 | uint8_t buf[2]; 112 | i2cRead(MPU3050_ADDRESS, MPU3050_TEMP_OUT, 2, buf); 113 | 114 | *tempData = 35 + ((int32_t)(buf[0] << 8 | buf[1]) + 13200) / 280; 115 | } 116 | -------------------------------------------------------------------------------- /src/drv_adxl345.c: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of baseflight 3 | * Licensed under GPL V3 or modified DCL - see https://github.com/multiwii/baseflight/blob/master/README.md 4 | */ 5 | #include "board.h" 6 | 7 | // ADXL345, Alternative address mode 0x53 8 | #define ADXL345_ADDRESS 0x53 9 | 10 | // Registers 11 | #define ADXL345_BW_RATE 0x2C 12 | #define ADXL345_POWER_CTL 0x2D 13 | #define ADXL345_INT_ENABLE 0x2E 14 | #define ADXL345_DATA_FORMAT 0x31 15 | #define ADXL345_DATA_OUT 0x32 16 | #define ADXL345_FIFO_CTL 0x38 17 | 18 | // BW_RATE values 19 | #define ADXL345_RATE_50 0x09 20 | #define ADXL345_RATE_100 0x0A 21 | #define ADXL345_RATE_200 0x0B 22 | #define ADXL345_RATE_400 0x0C 23 | #define ADXL345_RATE_800 0x0D 24 | #define ADXL345_RATE_1600 0x0E 25 | #define ADXL345_RATE_3200 0x0F 26 | 27 | // various register values 28 | #define ADXL345_POWER_MEAS 0x08 29 | #define ADXL345_FULL_RANGE 0x08 30 | #define ADXL345_RANGE_2G 0x00 31 | #define ADXL345_RANGE_4G 0x01 32 | #define ADXL345_RANGE_8G 0x02 33 | #define ADXL345_RANGE_16G 0x03 34 | #define ADXL345_FIFO_STREAM 0x80 35 | 36 | extern uint16_t acc_1G; 37 | 38 | static void adxl345Init(sensor_align_e align); 39 | static void adxl345Read(int16_t *accelData); 40 | 41 | static bool useFifo = false; 42 | static sensor_align_e accAlign = CW270_DEG; 43 | 44 | bool adxl345Detect(drv_adxl345_config_t *init, sensor_t *acc) 45 | { 46 | bool ack = false; 47 | uint8_t sig = 0; 48 | 49 | // Not supported with this frequency 50 | if (hw_revision >= NAZE32_REV5) 51 | return false; 52 | 53 | ack = i2cRead(ADXL345_ADDRESS, 0x00, 1, &sig); 54 | if (!ack || sig != 0xE5) 55 | return false; 56 | 57 | // use ADXL345's fifo to filter data or not 58 | useFifo = init->useFifo; 59 | 60 | acc->init = adxl345Init; 61 | acc->read = adxl345Read; 62 | return true; 63 | } 64 | 65 | static void adxl345Init(sensor_align_e align) 66 | { 67 | if (useFifo) { 68 | uint8_t fifoDepth = 16; 69 | i2cWrite(ADXL345_ADDRESS, ADXL345_POWER_CTL, ADXL345_POWER_MEAS); 70 | i2cWrite(ADXL345_ADDRESS, ADXL345_DATA_FORMAT, ADXL345_FULL_RANGE | ADXL345_RANGE_8G); 71 | i2cWrite(ADXL345_ADDRESS, ADXL345_BW_RATE, ADXL345_RATE_400); 72 | i2cWrite(ADXL345_ADDRESS, ADXL345_FIFO_CTL, (fifoDepth & 0x1F) | ADXL345_FIFO_STREAM); 73 | } else { 74 | i2cWrite(ADXL345_ADDRESS, ADXL345_POWER_CTL, ADXL345_POWER_MEAS); 75 | i2cWrite(ADXL345_ADDRESS, ADXL345_DATA_FORMAT, ADXL345_FULL_RANGE | ADXL345_RANGE_8G); 76 | i2cWrite(ADXL345_ADDRESS, ADXL345_BW_RATE, ADXL345_RATE_100); 77 | } 78 | acc_1G = 265; // 3.3V operation 79 | 80 | if (align > 0) 81 | accAlign = align; 82 | } 83 | 84 | uint8_t acc_samples = 0; 85 | 86 | static void adxl345Read(int16_t *accelData) 87 | { 88 | uint8_t buf[8]; 89 | int16_t data[3]; 90 | 91 | if (useFifo) { 92 | int32_t x = 0; 93 | int32_t y = 0; 94 | int32_t z = 0; 95 | uint8_t i = 0; 96 | uint8_t samples_remaining; 97 | 98 | do { 99 | i++; 100 | i2cRead(ADXL345_ADDRESS, ADXL345_DATA_OUT, 8, buf); 101 | x += (int16_t)(buf[0] + (buf[1] << 8)); 102 | y += (int16_t)(buf[2] + (buf[3] << 8)); 103 | z += (int16_t)(buf[4] + (buf[5] << 8)); 104 | samples_remaining = buf[7] & 0x7F; 105 | } while ((i < 32) && (samples_remaining > 0)); 106 | data[0] = x / i; 107 | data[1] = y / i; 108 | data[2] = z / i; 109 | acc_samples = i; 110 | } else { 111 | i2cRead(ADXL345_ADDRESS, ADXL345_DATA_OUT, 6, buf); 112 | data[0] = buf[0] + (buf[1] << 8); 113 | data[1] = buf[2] + (buf[3] << 8); 114 | data[2] = buf[4] + (buf[5] << 8); 115 | } 116 | 117 | alignSensors(data, accelData, accAlign); 118 | } 119 | -------------------------------------------------------------------------------- /lib/STM32F10x_StdPeriph_Driver/src/stm32f10x_crc.c: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f10x_crc.c 4 | * @author MCD Application Team 5 | * @version V3.5.0 6 | * @date 11-March-2011 7 | * @brief This file provides all the CRC firmware functions. 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS 12 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE 13 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY 14 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING 15 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE 16 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. 17 | * 18 | *

© COPYRIGHT 2011 STMicroelectronics

19 | ****************************************************************************** 20 | */ 21 | 22 | /* Includes ------------------------------------------------------------------*/ 23 | #include "stm32f10x_crc.h" 24 | 25 | /** @addtogroup STM32F10x_StdPeriph_Driver 26 | * @{ 27 | */ 28 | 29 | /** @defgroup CRC 30 | * @brief CRC driver modules 31 | * @{ 32 | */ 33 | 34 | /** @defgroup CRC_Private_TypesDefinitions 35 | * @{ 36 | */ 37 | 38 | /** 39 | * @} 40 | */ 41 | 42 | /** @defgroup CRC_Private_Defines 43 | * @{ 44 | */ 45 | 46 | /** 47 | * @} 48 | */ 49 | 50 | /** @defgroup CRC_Private_Macros 51 | * @{ 52 | */ 53 | 54 | /** 55 | * @} 56 | */ 57 | 58 | /** @defgroup CRC_Private_Variables 59 | * @{ 60 | */ 61 | 62 | /** 63 | * @} 64 | */ 65 | 66 | /** @defgroup CRC_Private_FunctionPrototypes 67 | * @{ 68 | */ 69 | 70 | /** 71 | * @} 72 | */ 73 | 74 | /** @defgroup CRC_Private_Functions 75 | * @{ 76 | */ 77 | 78 | /** 79 | * @brief Resets the CRC Data register (DR). 80 | * @param None 81 | * @retval None 82 | */ 83 | void CRC_ResetDR(void) 84 | { 85 | /* Reset CRC generator */ 86 | CRC->CR = CRC_CR_RESET; 87 | } 88 | 89 | /** 90 | * @brief Computes the 32-bit CRC of a given data word(32-bit). 91 | * @param Data: data word(32-bit) to compute its CRC 92 | * @retval 32-bit CRC 93 | */ 94 | uint32_t CRC_CalcCRC(uint32_t Data) 95 | { 96 | CRC->DR = Data; 97 | 98 | return (CRC->DR); 99 | } 100 | 101 | /** 102 | * @brief Computes the 32-bit CRC of a given buffer of data word(32-bit). 103 | * @param pBuffer: pointer to the buffer containing the data to be computed 104 | * @param BufferLength: length of the buffer to be computed 105 | * @retval 32-bit CRC 106 | */ 107 | uint32_t CRC_CalcBlockCRC(uint32_t pBuffer[], uint32_t BufferLength) 108 | { 109 | uint32_t index = 0; 110 | 111 | for(index = 0; index < BufferLength; index++) 112 | { 113 | CRC->DR = pBuffer[index]; 114 | } 115 | return (CRC->DR); 116 | } 117 | 118 | /** 119 | * @brief Returns the current CRC value. 120 | * @param None 121 | * @retval 32-bit CRC 122 | */ 123 | uint32_t CRC_GetCRC(void) 124 | { 125 | return (CRC->DR); 126 | } 127 | 128 | /** 129 | * @brief Stores a 8-bit data in the Independent Data(ID) register. 130 | * @param IDValue: 8-bit value to be stored in the ID register 131 | * @retval None 132 | */ 133 | void CRC_SetIDRegister(uint8_t IDValue) 134 | { 135 | CRC->IDR = IDValue; 136 | } 137 | 138 | /** 139 | * @brief Returns the 8-bit data stored in the Independent Data(ID) register 140 | * @param None 141 | * @retval 8-bit value of the ID register 142 | */ 143 | uint8_t CRC_GetIDRegister(void) 144 | { 145 | return (CRC->IDR); 146 | } 147 | 148 | /** 149 | * @} 150 | */ 151 | 152 | /** 153 | * @} 154 | */ 155 | 156 | /** 157 | * @} 158 | */ 159 | 160 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ 161 | -------------------------------------------------------------------------------- /lib/STM32F10x_StdPeriph_Driver/inc/stm32f10x_dbgmcu.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f10x_dbgmcu.h 4 | * @author MCD Application Team 5 | * @version V3.5.0 6 | * @date 11-March-2011 7 | * @brief This file contains all the functions prototypes for the DBGMCU 8 | * firmware library. 9 | ****************************************************************************** 10 | * @attention 11 | * 12 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS 13 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE 14 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY 15 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING 16 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE 17 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. 18 | * 19 | *

© COPYRIGHT 2011 STMicroelectronics

20 | ****************************************************************************** 21 | */ 22 | 23 | /* Define to prevent recursive inclusion -------------------------------------*/ 24 | #ifndef __STM32F10x_DBGMCU_H 25 | #define __STM32F10x_DBGMCU_H 26 | 27 | #ifdef __cplusplus 28 | extern "C" { 29 | #endif 30 | 31 | /* Includes ------------------------------------------------------------------*/ 32 | #include "stm32f10x.h" 33 | 34 | /** @addtogroup STM32F10x_StdPeriph_Driver 35 | * @{ 36 | */ 37 | 38 | /** @addtogroup DBGMCU 39 | * @{ 40 | */ 41 | 42 | /** @defgroup DBGMCU_Exported_Types 43 | * @{ 44 | */ 45 | 46 | /** 47 | * @} 48 | */ 49 | 50 | /** @defgroup DBGMCU_Exported_Constants 51 | * @{ 52 | */ 53 | 54 | #define DBGMCU_SLEEP ((uint32_t)0x00000001) 55 | #define DBGMCU_STOP ((uint32_t)0x00000002) 56 | #define DBGMCU_STANDBY ((uint32_t)0x00000004) 57 | #define DBGMCU_IWDG_STOP ((uint32_t)0x00000100) 58 | #define DBGMCU_WWDG_STOP ((uint32_t)0x00000200) 59 | #define DBGMCU_TIM1_STOP ((uint32_t)0x00000400) 60 | #define DBGMCU_TIM2_STOP ((uint32_t)0x00000800) 61 | #define DBGMCU_TIM3_STOP ((uint32_t)0x00001000) 62 | #define DBGMCU_TIM4_STOP ((uint32_t)0x00002000) 63 | #define DBGMCU_CAN1_STOP ((uint32_t)0x00004000) 64 | #define DBGMCU_I2C1_SMBUS_TIMEOUT ((uint32_t)0x00008000) 65 | #define DBGMCU_I2C2_SMBUS_TIMEOUT ((uint32_t)0x00010000) 66 | #define DBGMCU_TIM8_STOP ((uint32_t)0x00020000) 67 | #define DBGMCU_TIM5_STOP ((uint32_t)0x00040000) 68 | #define DBGMCU_TIM6_STOP ((uint32_t)0x00080000) 69 | #define DBGMCU_TIM7_STOP ((uint32_t)0x00100000) 70 | #define DBGMCU_CAN2_STOP ((uint32_t)0x00200000) 71 | #define DBGMCU_TIM15_STOP ((uint32_t)0x00400000) 72 | #define DBGMCU_TIM16_STOP ((uint32_t)0x00800000) 73 | #define DBGMCU_TIM17_STOP ((uint32_t)0x01000000) 74 | #define DBGMCU_TIM12_STOP ((uint32_t)0x02000000) 75 | #define DBGMCU_TIM13_STOP ((uint32_t)0x04000000) 76 | #define DBGMCU_TIM14_STOP ((uint32_t)0x08000000) 77 | #define DBGMCU_TIM9_STOP ((uint32_t)0x10000000) 78 | #define DBGMCU_TIM10_STOP ((uint32_t)0x20000000) 79 | #define DBGMCU_TIM11_STOP ((uint32_t)0x40000000) 80 | 81 | #define IS_DBGMCU_PERIPH(PERIPH) ((((PERIPH) & 0x800000F8) == 0x00) && ((PERIPH) != 0x00)) 82 | /** 83 | * @} 84 | */ 85 | 86 | /** @defgroup DBGMCU_Exported_Macros 87 | * @{ 88 | */ 89 | 90 | /** 91 | * @} 92 | */ 93 | 94 | /** @defgroup DBGMCU_Exported_Functions 95 | * @{ 96 | */ 97 | 98 | uint32_t DBGMCU_GetREVID(void); 99 | uint32_t DBGMCU_GetDEVID(void); 100 | void DBGMCU_Config(uint32_t DBGMCU_Periph, FunctionalState NewState); 101 | 102 | #ifdef __cplusplus 103 | } 104 | #endif 105 | 106 | #endif /* __STM32F10x_DBGMCU_H */ 107 | /** 108 | * @} 109 | */ 110 | 111 | /** 112 | * @} 113 | */ 114 | 115 | /** 116 | * @} 117 | */ 118 | 119 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ 120 | -------------------------------------------------------------------------------- /lib/STM32F10x_StdPeriph_Driver/inc/stm32f10x_iwdg.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f10x_iwdg.h 4 | * @author MCD Application Team 5 | * @version V3.5.0 6 | * @date 11-March-2011 7 | * @brief This file contains all the functions prototypes for the IWDG 8 | * firmware library. 9 | ****************************************************************************** 10 | * @attention 11 | * 12 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS 13 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE 14 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY 15 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING 16 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE 17 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. 18 | * 19 | *

© COPYRIGHT 2011 STMicroelectronics

20 | ****************************************************************************** 21 | */ 22 | 23 | /* Define to prevent recursive inclusion -------------------------------------*/ 24 | #ifndef __STM32F10x_IWDG_H 25 | #define __STM32F10x_IWDG_H 26 | 27 | #ifdef __cplusplus 28 | extern "C" { 29 | #endif 30 | 31 | /* Includes ------------------------------------------------------------------*/ 32 | #include "stm32f10x.h" 33 | 34 | /** @addtogroup STM32F10x_StdPeriph_Driver 35 | * @{ 36 | */ 37 | 38 | /** @addtogroup IWDG 39 | * @{ 40 | */ 41 | 42 | /** @defgroup IWDG_Exported_Types 43 | * @{ 44 | */ 45 | 46 | /** 47 | * @} 48 | */ 49 | 50 | /** @defgroup IWDG_Exported_Constants 51 | * @{ 52 | */ 53 | 54 | /** @defgroup IWDG_WriteAccess 55 | * @{ 56 | */ 57 | 58 | #define IWDG_WriteAccess_Enable ((uint16_t)0x5555) 59 | #define IWDG_WriteAccess_Disable ((uint16_t)0x0000) 60 | #define IS_IWDG_WRITE_ACCESS(ACCESS) (((ACCESS) == IWDG_WriteAccess_Enable) || \ 61 | ((ACCESS) == IWDG_WriteAccess_Disable)) 62 | /** 63 | * @} 64 | */ 65 | 66 | /** @defgroup IWDG_prescaler 67 | * @{ 68 | */ 69 | 70 | #define IWDG_Prescaler_4 ((uint8_t)0x00) 71 | #define IWDG_Prescaler_8 ((uint8_t)0x01) 72 | #define IWDG_Prescaler_16 ((uint8_t)0x02) 73 | #define IWDG_Prescaler_32 ((uint8_t)0x03) 74 | #define IWDG_Prescaler_64 ((uint8_t)0x04) 75 | #define IWDG_Prescaler_128 ((uint8_t)0x05) 76 | #define IWDG_Prescaler_256 ((uint8_t)0x06) 77 | #define IS_IWDG_PRESCALER(PRESCALER) (((PRESCALER) == IWDG_Prescaler_4) || \ 78 | ((PRESCALER) == IWDG_Prescaler_8) || \ 79 | ((PRESCALER) == IWDG_Prescaler_16) || \ 80 | ((PRESCALER) == IWDG_Prescaler_32) || \ 81 | ((PRESCALER) == IWDG_Prescaler_64) || \ 82 | ((PRESCALER) == IWDG_Prescaler_128)|| \ 83 | ((PRESCALER) == IWDG_Prescaler_256)) 84 | /** 85 | * @} 86 | */ 87 | 88 | /** @defgroup IWDG_Flag 89 | * @{ 90 | */ 91 | 92 | #define IWDG_FLAG_PVU ((uint16_t)0x0001) 93 | #define IWDG_FLAG_RVU ((uint16_t)0x0002) 94 | #define IS_IWDG_FLAG(FLAG) (((FLAG) == IWDG_FLAG_PVU) || ((FLAG) == IWDG_FLAG_RVU)) 95 | #define IS_IWDG_RELOAD(RELOAD) ((RELOAD) <= 0xFFF) 96 | /** 97 | * @} 98 | */ 99 | 100 | /** 101 | * @} 102 | */ 103 | 104 | /** @defgroup IWDG_Exported_Macros 105 | * @{ 106 | */ 107 | 108 | /** 109 | * @} 110 | */ 111 | 112 | /** @defgroup IWDG_Exported_Functions 113 | * @{ 114 | */ 115 | 116 | void IWDG_WriteAccessCmd(uint16_t IWDG_WriteAccess); 117 | void IWDG_SetPrescaler(uint8_t IWDG_Prescaler); 118 | void IWDG_SetReload(uint16_t Reload); 119 | void IWDG_ReloadCounter(void); 120 | void IWDG_Enable(void); 121 | FlagStatus IWDG_GetFlagStatus(uint16_t IWDG_FLAG); 122 | 123 | #ifdef __cplusplus 124 | } 125 | #endif 126 | 127 | #endif /* __STM32F10x_IWDG_H */ 128 | /** 129 | * @} 130 | */ 131 | 132 | /** 133 | * @} 134 | */ 135 | 136 | /** 137 | * @} 138 | */ 139 | 140 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ 141 | -------------------------------------------------------------------------------- /lib/STM32F10x_StdPeriph_Driver/inc/stm32f10x_rtc.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f10x_rtc.h 4 | * @author MCD Application Team 5 | * @version V3.5.0 6 | * @date 11-March-2011 7 | * @brief This file contains all the functions prototypes for the RTC firmware 8 | * library. 9 | ****************************************************************************** 10 | * @attention 11 | * 12 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS 13 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE 14 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY 15 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING 16 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE 17 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. 18 | * 19 | *

© COPYRIGHT 2011 STMicroelectronics

20 | ****************************************************************************** 21 | */ 22 | 23 | /* Define to prevent recursive inclusion -------------------------------------*/ 24 | #ifndef __STM32F10x_RTC_H 25 | #define __STM32F10x_RTC_H 26 | 27 | #ifdef __cplusplus 28 | extern "C" { 29 | #endif 30 | 31 | /* Includes ------------------------------------------------------------------*/ 32 | #include "stm32f10x.h" 33 | 34 | /** @addtogroup STM32F10x_StdPeriph_Driver 35 | * @{ 36 | */ 37 | 38 | /** @addtogroup RTC 39 | * @{ 40 | */ 41 | 42 | /** @defgroup RTC_Exported_Types 43 | * @{ 44 | */ 45 | 46 | /** 47 | * @} 48 | */ 49 | 50 | /** @defgroup RTC_Exported_Constants 51 | * @{ 52 | */ 53 | 54 | /** @defgroup RTC_interrupts_define 55 | * @{ 56 | */ 57 | 58 | #define RTC_IT_OW ((uint16_t)0x0004) /*!< Overflow interrupt */ 59 | #define RTC_IT_ALR ((uint16_t)0x0002) /*!< Alarm interrupt */ 60 | #define RTC_IT_SEC ((uint16_t)0x0001) /*!< Second interrupt */ 61 | #define IS_RTC_IT(IT) ((((IT) & (uint16_t)0xFFF8) == 0x00) && ((IT) != 0x00)) 62 | #define IS_RTC_GET_IT(IT) (((IT) == RTC_IT_OW) || ((IT) == RTC_IT_ALR) || \ 63 | ((IT) == RTC_IT_SEC)) 64 | /** 65 | * @} 66 | */ 67 | 68 | /** @defgroup RTC_interrupts_flags 69 | * @{ 70 | */ 71 | 72 | #define RTC_FLAG_RTOFF ((uint16_t)0x0020) /*!< RTC Operation OFF flag */ 73 | #define RTC_FLAG_RSF ((uint16_t)0x0008) /*!< Registers Synchronized flag */ 74 | #define RTC_FLAG_OW ((uint16_t)0x0004) /*!< Overflow flag */ 75 | #define RTC_FLAG_ALR ((uint16_t)0x0002) /*!< Alarm flag */ 76 | #define RTC_FLAG_SEC ((uint16_t)0x0001) /*!< Second flag */ 77 | #define IS_RTC_CLEAR_FLAG(FLAG) ((((FLAG) & (uint16_t)0xFFF0) == 0x00) && ((FLAG) != 0x00)) 78 | #define IS_RTC_GET_FLAG(FLAG) (((FLAG) == RTC_FLAG_RTOFF) || ((FLAG) == RTC_FLAG_RSF) || \ 79 | ((FLAG) == RTC_FLAG_OW) || ((FLAG) == RTC_FLAG_ALR) || \ 80 | ((FLAG) == RTC_FLAG_SEC)) 81 | #define IS_RTC_PRESCALER(PRESCALER) ((PRESCALER) <= 0xFFFFF) 82 | 83 | /** 84 | * @} 85 | */ 86 | 87 | /** 88 | * @} 89 | */ 90 | 91 | /** @defgroup RTC_Exported_Macros 92 | * @{ 93 | */ 94 | 95 | /** 96 | * @} 97 | */ 98 | 99 | /** @defgroup RTC_Exported_Functions 100 | * @{ 101 | */ 102 | 103 | void RTC_ITConfig(uint16_t RTC_IT, FunctionalState NewState); 104 | void RTC_EnterConfigMode(void); 105 | void RTC_ExitConfigMode(void); 106 | uint32_t RTC_GetCounter(void); 107 | void RTC_SetCounter(uint32_t CounterValue); 108 | void RTC_SetPrescaler(uint32_t PrescalerValue); 109 | void RTC_SetAlarm(uint32_t AlarmValue); 110 | uint32_t RTC_GetDivider(void); 111 | void RTC_WaitForLastTask(void); 112 | void RTC_WaitForSynchro(void); 113 | FlagStatus RTC_GetFlagStatus(uint16_t RTC_FLAG); 114 | void RTC_ClearFlag(uint16_t RTC_FLAG); 115 | ITStatus RTC_GetITStatus(uint16_t RTC_IT); 116 | void RTC_ClearITPendingBit(uint16_t RTC_IT); 117 | 118 | #ifdef __cplusplus 119 | } 120 | #endif 121 | 122 | #endif /* __STM32F10x_RTC_H */ 123 | /** 124 | * @} 125 | */ 126 | 127 | /** 128 | * @} 129 | */ 130 | 131 | /** 132 | * @} 133 | */ 134 | 135 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ 136 | -------------------------------------------------------------------------------- /src/drv_mma845x.c: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of baseflight 3 | * Licensed under GPL V3 or modified DCL - see https://github.com/multiwii/baseflight/blob/master/README.md 4 | */ 5 | #include "board.h" 6 | 7 | // MMA8452QT, Standard address 0x1C 8 | // ACC_INT2 routed to PA5 9 | 10 | #define MMA8452_ADDRESS 0x1C 11 | 12 | #define MMA8452_DEVICE_SIGNATURE 0x2A 13 | #define MMA8451_DEVICE_SIGNATURE 0x1A 14 | 15 | #define MMA8452_STATUS 0x00 16 | #define MMA8452_OUT_X_MSB 0x01 17 | #define MMA8452_WHO_AM_I 0x0D 18 | #define MMA8452_XYZ_DATA_CFG 0x0E 19 | #define MMA8452_HP_FILTER_CUTOFF 0x0F 20 | #define MMA8452_CTRL_REG1 0x2A 21 | #define MMA8452_CTRL_REG2 0x2B 22 | #define MMA8452_CTRL_REG3 0x2C 23 | #define MMA8452_CTRL_REG4 0x2D 24 | #define MMA8452_CTRL_REG5 0x2E 25 | 26 | #define MMA8452_FS_RANGE_8G 0x02 27 | #define MMA8452_FS_RANGE_4G 0x01 28 | #define MMA8452_FS_RANGE_2G 0x00 29 | 30 | #define MMA8452_HPF_CUTOFF_LV1 0x00 31 | #define MMA8452_HPF_CUTOFF_LV2 0x01 32 | #define MMA8452_HPF_CUTOFF_LV3 0x02 33 | #define MMA8452_HPF_CUTOFF_LV4 0x03 34 | 35 | #define MMA8452_CTRL_REG2_B7_ST 0x80 36 | #define MMA8452_CTRL_REG2_B6_RST 0x40 37 | #define MMA8452_CTRL_REG2_B4_SMODS1 0x10 38 | #define MMA8452_CTRL_REG2_B3_SMODS0 0x08 39 | #define MMA8452_CTRL_REG2_B2_SLPE 0x04 40 | #define MMA8452_CTRL_REG2_B1_MODS1 0x02 41 | #define MMA8452_CTRL_REG2_B0_MODS0 0x01 42 | 43 | #define MMA8452_CTRL_REG2_MODS_LP 0x03 44 | #define MMA8452_CTRL_REG2_MODS_HR 0x02 45 | #define MMA8452_CTRL_REG2_MODS_LNLP 0x01 46 | #define MMA8452_CTRL_REG2_MODS_NOR 0x00 47 | 48 | #define MMA8452_CTRL_REG3_IPOL 0x02 49 | #define MMA8452_CTRL_REG4_INT_EN_DRDY 0x01 50 | 51 | #define MMA8452_CTRL_REG1_LNOISE 0x04 52 | #define MMA8452_CTRL_REG1_ACTIVE 0x01 53 | 54 | extern uint16_t acc_1G; 55 | static sensor_align_e accAlign = CW90_DEG; 56 | 57 | static void mma8452Init(sensor_align_e align); 58 | static void mma8452Read(int16_t *accelData); 59 | 60 | bool mma8452Detect(sensor_t *acc) 61 | { 62 | bool ack = false; 63 | uint8_t sig = 0; 64 | 65 | // Not supported with this frequency 66 | if (hw_revision >= NAZE32_REV5) 67 | return false; 68 | 69 | ack = i2cRead(MMA8452_ADDRESS, MMA8452_WHO_AM_I, 1, &sig); 70 | if (!ack || (sig != MMA8452_DEVICE_SIGNATURE && sig != MMA8451_DEVICE_SIGNATURE)) 71 | return false; 72 | 73 | acc->init = mma8452Init; 74 | acc->read = mma8452Read; 75 | return true; 76 | } 77 | 78 | static void mma8452Init(sensor_align_e align) 79 | { 80 | gpio_config_t gpio; 81 | 82 | // PA5 - ACC_INT2 output on rev3/4 hardware 83 | gpio.pin = Pin_5; 84 | gpio.speed = Speed_2MHz; 85 | gpio.mode = Mode_IN_FLOATING; 86 | gpioInit(GPIOA, &gpio); 87 | 88 | i2cWrite(MMA8452_ADDRESS, MMA8452_CTRL_REG1, 0); // Put device in standby to configure stuff 89 | i2cWrite(MMA8452_ADDRESS, MMA8452_XYZ_DATA_CFG, MMA8452_FS_RANGE_8G); 90 | i2cWrite(MMA8452_ADDRESS, MMA8452_HP_FILTER_CUTOFF, MMA8452_HPF_CUTOFF_LV4); 91 | i2cWrite(MMA8452_ADDRESS, MMA8452_CTRL_REG2, MMA8452_CTRL_REG2_MODS_HR | MMA8452_CTRL_REG2_MODS_HR << 3); // High resolution measurement in both sleep and active modes 92 | i2cWrite(MMA8452_ADDRESS, MMA8452_CTRL_REG3, MMA8452_CTRL_REG3_IPOL); // Interrupt polarity (active HIGH) 93 | i2cWrite(MMA8452_ADDRESS, MMA8452_CTRL_REG4, MMA8452_CTRL_REG4_INT_EN_DRDY); // Enable DRDY interrupt (unused by this driver) 94 | i2cWrite(MMA8452_ADDRESS, MMA8452_CTRL_REG5, 0); // DRDY routed to INT2 95 | i2cWrite(MMA8452_ADDRESS, MMA8452_CTRL_REG1, MMA8452_CTRL_REG1_LNOISE | MMA8452_CTRL_REG1_ACTIVE); // Turn on measurements, low noise at max scale mode, Data Rate 800Hz. LNoise mode makes range +-4G. 96 | 97 | acc_1G = 256; 98 | 99 | if (align > 0) 100 | accAlign = align; 101 | } 102 | 103 | static void mma8452Read(int16_t *accelData) 104 | { 105 | uint8_t buf[6]; 106 | int16_t data[3]; 107 | 108 | i2cRead(MMA8452_ADDRESS, MMA8452_OUT_X_MSB, 6, buf); 109 | data[0] = ((int16_t)((buf[0] << 8) | buf[1]) >> 2) / 4; 110 | data[1] = ((int16_t)((buf[2] << 8) | buf[3]) >> 2) / 4; 111 | data[2] = ((int16_t)((buf[4] << 8) | buf[5]) >> 2) / 4; 112 | 113 | alignSensors(data, accelData, accAlign); 114 | } 115 | -------------------------------------------------------------------------------- /src/utils.c: -------------------------------------------------------------------------------- 1 | #include "board.h" 2 | #include "mw.h" 3 | 4 | static bool standardBoardAlignment = true; // board orientation correction 5 | static float boardRotation[3][3]; // matrix 6 | 7 | int constrain(int amt, int low, int high) 8 | { 9 | if (amt < low) 10 | return low; 11 | else if (amt > high) 12 | return high; 13 | else 14 | return amt; 15 | } 16 | 17 | void initBoardAlignment(void) 18 | { 19 | float roll, pitch, yaw; 20 | float cosx, sinx, cosy, siny, cosz, sinz; 21 | float coszcosx, coszcosy, sinzcosx, coszsinx, sinzsinx; 22 | 23 | // standard alignment, nothing to calculate 24 | if (!mcfg.board_align_roll && !mcfg.board_align_pitch && !mcfg.board_align_yaw) 25 | return; 26 | 27 | standardBoardAlignment = false; 28 | 29 | // deg2rad 30 | roll = mcfg.board_align_roll * M_PI / 180.0f; 31 | pitch = mcfg.board_align_pitch * M_PI / 180.0f; 32 | yaw = mcfg.board_align_yaw * M_PI / 180.0f; 33 | 34 | cosx = cosf(roll); 35 | sinx = sinf(roll); 36 | cosy = cosf(pitch); 37 | siny = sinf(pitch); 38 | cosz = cosf(yaw); 39 | sinz = sinf(yaw); 40 | 41 | coszcosx = cosz * cosx; 42 | coszcosy = cosz * cosy; 43 | sinzcosx = sinz * cosx; 44 | coszsinx = sinx * cosz; 45 | sinzsinx = sinx * sinz; 46 | 47 | // define rotation matrix 48 | boardRotation[0][0] = coszcosy; 49 | boardRotation[0][1] = -cosy * sinz; 50 | boardRotation[0][2] = siny; 51 | 52 | boardRotation[1][0] = sinzcosx + (coszsinx * siny); 53 | boardRotation[1][1] = coszcosx - (sinzsinx * siny); 54 | boardRotation[1][2] = -sinx * cosy; 55 | 56 | boardRotation[2][0] = (sinzsinx) - (coszcosx * siny); 57 | boardRotation[2][1] = (coszsinx) + (sinzcosx * siny); 58 | boardRotation[2][2] = cosy * cosx; 59 | } 60 | 61 | void alignBoard(int16_t *vec) 62 | { 63 | int16_t x = vec[X]; 64 | int16_t y = vec[Y]; 65 | int16_t z = vec[Z]; 66 | 67 | vec[X] = lrintf(boardRotation[0][0] * x + boardRotation[1][0] * y + boardRotation[2][0] * z); 68 | vec[Y] = lrintf(boardRotation[0][1] * x + boardRotation[1][1] * y + boardRotation[2][1] * z); 69 | vec[Z] = lrintf(boardRotation[0][2] * x + boardRotation[1][2] * y + boardRotation[2][2] * z); 70 | } 71 | 72 | void alignSensors(int16_t *src, int16_t *dest, uint8_t rotation) 73 | { 74 | switch (rotation) { 75 | case CW0_DEG: 76 | dest[X] = src[X]; 77 | dest[Y] = src[Y]; 78 | dest[Z] = src[Z]; 79 | break; 80 | case CW90_DEG: 81 | dest[X] = src[Y]; 82 | dest[Y] = -src[X]; 83 | dest[Z] = src[Z]; 84 | break; 85 | case CW180_DEG: 86 | dest[X] = -src[X]; 87 | dest[Y] = -src[Y]; 88 | dest[Z] = src[Z]; 89 | break; 90 | case CW270_DEG: 91 | dest[X] = -src[Y]; 92 | dest[Y] = src[X]; 93 | dest[Z] = src[Z]; 94 | break; 95 | case CW0_DEG_FLIP: 96 | dest[X] = -src[X]; 97 | dest[Y] = src[Y]; 98 | dest[Z] = -src[Z]; 99 | break; 100 | case CW90_DEG_FLIP: 101 | dest[X] = src[Y]; 102 | dest[Y] = src[X]; 103 | dest[Z] = -src[Z]; 104 | break; 105 | case CW180_DEG_FLIP: 106 | dest[X] = src[X]; 107 | dest[Y] = -src[Y]; 108 | dest[Z] = -src[Z]; 109 | break; 110 | case CW270_DEG_FLIP: 111 | dest[X] = -src[Y]; 112 | dest[Y] = -src[X]; 113 | dest[Z] = -src[Z]; 114 | break; 115 | default: 116 | break; 117 | } 118 | 119 | if (!standardBoardAlignment) 120 | alignBoard(dest); 121 | } 122 | 123 | #ifdef PROD_DEBUG 124 | void productionDebug(void) 125 | { 126 | gpio_config_t gpio; 127 | 128 | // remap PB6 to USART1_TX 129 | gpio.pin = Pin_6; 130 | gpio.mode = Mode_AF_PP; 131 | gpio.speed = Speed_2MHz; 132 | gpioInit(GPIOB, &gpio); 133 | gpioPinRemapConfig(AFIO_MAPR_USART1_REMAP, true); 134 | serialInit(mcfg.serial_baudrate); 135 | delay(25); 136 | serialPrint(core.mainport, "DBG "); 137 | printf("%08x%08x%08x OK\n", U_ID_0, U_ID_1, U_ID_2); 138 | serialPrint(core.mainport, "EOF"); 139 | delay(25); 140 | gpioPinRemapConfig(AFIO_MAPR_USART1_REMAP, false); 141 | } 142 | #endif 143 | -------------------------------------------------------------------------------- /src/drv_hcsr04.c: -------------------------------------------------------------------------------- 1 | #include "board.h" 2 | #include "mw.h" 3 | 4 | #ifdef SONAR 5 | 6 | /* HC-SR04 consists of ultrasonic transmitter, receiver, and control circuits. 7 | * When trigged it sends out a series of 40KHz ultrasonic pulses and receives 8 | * echo froman object. The distance between the unit and the object is calculated 9 | * by measuring the traveling time of sound and output it as the width of a TTL pulse. 10 | * 11 | * *** Warning: HC-SR04 operates at +5V *** 12 | * 13 | */ 14 | 15 | static uint16_t trigger_pin; 16 | static uint16_t echo_pin; 17 | static uint32_t exti_line; 18 | static uint8_t exti_pin_source; 19 | static IRQn_Type exti_irqn; 20 | 21 | static uint32_t last_measurement; 22 | static volatile int32_t *distance_ptr; 23 | 24 | void ECHO_EXTI_IRQHandler(void) 25 | { 26 | static uint32_t timing_start; 27 | uint32_t timing_stop; 28 | if (digitalIn(GPIOB, echo_pin) != 0) { 29 | timing_start = micros(); 30 | } else { 31 | timing_stop = micros(); 32 | if (timing_stop > timing_start) { 33 | // The speed of sound is 340 m/s or approx. 29 microseconds per centimeter. 34 | // The ping travels out and back, so to find the distance of the 35 | // object we take half of the distance traveled. 36 | // 37 | // 340 m/s = 0.034 cm/microsecond = 29.41176471 *2 = 58.82352941 rounded to 59 38 | int32_t distance = (timing_stop - timing_start) / 59; 39 | // this sonar range is up to 4meter , but 3meter is the safe working range (+tilted and roll) 40 | if (distance > 300) 41 | distance = -1; 42 | *distance_ptr = distance ; 43 | } 44 | } 45 | 46 | EXTI_ClearITPendingBit(exti_line); 47 | } 48 | 49 | void EXTI1_IRQHandler(void) 50 | { 51 | ECHO_EXTI_IRQHandler(); 52 | } 53 | 54 | void EXTI9_5_IRQHandler(void) 55 | { 56 | ECHO_EXTI_IRQHandler(); 57 | } 58 | 59 | void hcsr04_init(sonar_config_t config) 60 | { 61 | gpio_config_t gpio; 62 | EXTI_InitTypeDef EXTIInit; 63 | 64 | // enable AFIO for EXTI support - already done is drv_system.c 65 | // RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO | RCC_APB2Periph, ENABLE); 66 | 67 | switch (config) { 68 | case sonar_pwm56: 69 | trigger_pin = Pin_8; // PWM5 (PB8) - 5v tolerant 70 | echo_pin = Pin_9; // PWM6 (PB9) - 5v tolerant 71 | exti_line = EXTI_Line9; 72 | exti_pin_source = GPIO_PinSource9; 73 | exti_irqn = EXTI9_5_IRQn; 74 | break; 75 | case sonar_rc78: 76 | trigger_pin = Pin_0; // RX7 (PB0) - only 3.3v ( add a 1K Ohms resistor ) 77 | echo_pin = Pin_1; // RX8 (PB1) - only 3.3v ( add a 1K Ohms resistor ) 78 | exti_line = EXTI_Line1; 79 | exti_pin_source = GPIO_PinSource1; 80 | exti_irqn = EXTI1_IRQn; 81 | break; 82 | } 83 | 84 | // tp - trigger pin 85 | gpio.pin = trigger_pin; 86 | gpio.mode = Mode_Out_PP; 87 | gpio.speed = Speed_2MHz; 88 | gpioInit(GPIOB, &gpio); 89 | 90 | // ep - echo pin 91 | gpio.pin = echo_pin; 92 | gpio.mode = Mode_IN_FLOATING; 93 | gpioInit(GPIOB, &gpio); 94 | 95 | // setup external interrupt on echo pin 96 | gpioExtiLineConfig(GPIO_PortSourceGPIOB, exti_pin_source); 97 | 98 | EXTI_ClearITPendingBit(exti_line); 99 | 100 | EXTIInit.EXTI_Line = exti_line; 101 | EXTIInit.EXTI_Mode = EXTI_Mode_Interrupt; 102 | EXTIInit.EXTI_Trigger = EXTI_Trigger_Rising_Falling; 103 | EXTIInit.EXTI_LineCmd = ENABLE; 104 | EXTI_Init(&EXTIInit); 105 | 106 | NVIC_EnableIRQ(exti_irqn); 107 | 108 | last_measurement = millis() - 60; // force 1st measurement in hcsr04_get_distance() 109 | } 110 | 111 | // distance calculation is done asynchronously, using interrupt 112 | void hcsr04_get_distance(volatile int32_t *distance) 113 | { 114 | uint32_t current_time = millis(); 115 | 116 | if (current_time < (last_measurement + 60)) { 117 | // the repeat interval of trig signal should be greater than 60ms 118 | // to avoid interference between connective measurements. 119 | return; 120 | } 121 | 122 | last_measurement = current_time; 123 | distance_ptr = distance; 124 | 125 | digitalHi(GPIOB, trigger_pin); 126 | // The width of trig signal must be greater than 10us 127 | delayMicroseconds(11); 128 | digitalLo(GPIOB, trigger_pin); 129 | } 130 | #endif 131 | -------------------------------------------------------------------------------- /stm32_flash.ld: -------------------------------------------------------------------------------- 1 | /* 2 | ***************************************************************************** 3 | ** 4 | ** File : stm32_flash.ld 5 | ** 6 | ** Abstract : Linker script for STM32F103CB Device with 7 | ** 128KByte FLASH, 20KByte RAM 8 | ** 9 | ***************************************************************************** 10 | */ 11 | 12 | /* Entry Point */ 13 | ENTRY(Reset_Handler) 14 | 15 | /* Highest address of the user mode stack */ 16 | _estack = 0x20005000; /* end of 20K RAM */ 17 | 18 | /* Generate a link error if heap and stack don't fit into RAM */ 19 | _Min_Heap_Size = 0; /* required amount of heap */ 20 | _Min_Stack_Size = 0x400; /* required amount of stack */ 21 | 22 | /* Specify the memory areas. Flash is limited for last 2K for configuration storage */ 23 | MEMORY 24 | { 25 | FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 126K 26 | RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 20K 27 | MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K 28 | } 29 | 30 | /* Define output sections */ 31 | SECTIONS 32 | { 33 | /* The startup code goes first into FLASH */ 34 | .isr_vector : 35 | { 36 | . = ALIGN(4); 37 | KEEP(*(.isr_vector)) /* Startup code */ 38 | . = ALIGN(4); 39 | } >FLASH 40 | 41 | /* The program code and other data goes into FLASH */ 42 | .text : 43 | { 44 | . = ALIGN(4); 45 | *(.text) /* .text sections (code) */ 46 | *(.text*) /* .text* sections (code) */ 47 | *(.rodata) /* .rodata sections (constants, strings, etc.) */ 48 | *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ 49 | *(.glue_7) /* glue arm to thumb code */ 50 | *(.glue_7t) /* glue thumb to arm code */ 51 | *(.eh_frame) 52 | 53 | KEEP (*(.init)) 54 | KEEP (*(.fini)) 55 | 56 | . = ALIGN(4); 57 | _etext = .; /* define a global symbols at end of code */ 58 | } >FLASH 59 | 60 | 61 | .ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH 62 | .ARM : { 63 | __exidx_start = .; 64 | *(.ARM.exidx*) 65 | __exidx_end = .; 66 | } >FLASH 67 | 68 | .preinit_array : 69 | { 70 | PROVIDE_HIDDEN (__preinit_array_start = .); 71 | KEEP (*(.preinit_array*)) 72 | PROVIDE_HIDDEN (__preinit_array_end = .); 73 | } >FLASH 74 | .init_array : 75 | { 76 | PROVIDE_HIDDEN (__init_array_start = .); 77 | KEEP (*(SORT(.init_array.*))) 78 | KEEP (*(.init_array*)) 79 | PROVIDE_HIDDEN (__init_array_end = .); 80 | } >FLASH 81 | .fini_array : 82 | { 83 | PROVIDE_HIDDEN (__fini_array_start = .); 84 | KEEP (*(.fini_array*)) 85 | KEEP (*(SORT(.fini_array.*))) 86 | PROVIDE_HIDDEN (__fini_array_end = .); 87 | } >FLASH 88 | 89 | /* used by the startup to initialize data */ 90 | _sidata = .; 91 | 92 | /* Initialized data sections goes into RAM, load LMA copy after code */ 93 | .data : 94 | { 95 | . = ALIGN(4); 96 | _sdata = .; /* create a global symbol at data start */ 97 | *(.data) /* .data sections */ 98 | *(.data*) /* .data* sections */ 99 | 100 | . = ALIGN(4); 101 | _edata = .; /* define a global symbol at data end */ 102 | } >RAM AT> FLASH 103 | 104 | /* Uninitialized data section */ 105 | . = ALIGN(4); 106 | .bss : 107 | { 108 | /* This is used by the startup in order to initialize the .bss secion */ 109 | _sbss = .; /* define a global symbol at bss start */ 110 | __bss_start__ = _sbss; 111 | *(.bss) 112 | *(.bss*) 113 | *(COMMON) 114 | 115 | . = ALIGN(4); 116 | _ebss = .; /* define a global symbol at bss end */ 117 | __bss_end__ = _ebss; 118 | } >RAM 119 | 120 | /* User_heap_stack section, used to check that there is enough RAM left */ 121 | ._user_heap_stack : 122 | { 123 | . = ALIGN(4); 124 | PROVIDE ( end = . ); 125 | PROVIDE ( _end = . ); 126 | . = . + _Min_Heap_Size; 127 | . = . + _Min_Stack_Size; 128 | . = ALIGN(4); 129 | } >RAM 130 | 131 | /* MEMORY_bank1 section, code must be located here explicitly */ 132 | /* Example: extern int foo(void) __attribute__ ((section (".mb1text"))); */ 133 | .memory_b1_text : 134 | { 135 | *(.mb1text) /* .mb1text sections (code) */ 136 | *(.mb1text*) /* .mb1text* sections (code) */ 137 | *(.mb1rodata) /* read-only data (constants) */ 138 | *(.mb1rodata*) 139 | } >MEMORY_B1 140 | 141 | /* Remove information from the standard libraries */ 142 | /DISCARD/ : 143 | { 144 | libc.a ( * ) 145 | libm.a ( * ) 146 | libgcc.a ( * ) 147 | } 148 | 149 | .ARM.attributes 0 : { *(.ARM.attributes) } 150 | } 151 | -------------------------------------------------------------------------------- /support/stmloader/serial.c: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of AutoQuad. 3 | 4 | AutoQuad is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | AutoQuad is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | You should have received a copy of the GNU General Public License 14 | along with AutoQuad. If not, see . 15 | 16 | Copyright © 2011 Bill Nesbitt 17 | */ 18 | 19 | #ifndef _GNU_SOURCE 20 | #define _GNU_SOURCE 21 | #endif 22 | 23 | #include "serial.h" 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | serialStruct_t *initSerial(const char *port, unsigned int baud, char ctsRts) { 33 | serialStruct_t *s; 34 | struct termios options; 35 | unsigned int brate; 36 | 37 | s = (serialStruct_t *)calloc(1, sizeof(serialStruct_t)); 38 | 39 | s->fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY); 40 | 41 | if (s->fd == -1) { 42 | free(s); 43 | return 0; 44 | } 45 | 46 | fcntl(s->fd, F_SETFL, 0); // clear all flags on descriptor, enable direct I/O 47 | 48 | // bzero(&options, sizeof(options)); 49 | // memset(&options, 0, sizeof(options)); 50 | tcgetattr(s->fd, &options); 51 | 52 | #ifdef B921600 53 | switch (baud) { 54 | case 9600: 55 | brate = B9600; 56 | break; 57 | case 19200: 58 | brate = B19200; 59 | break; 60 | case 38400: 61 | brate = B38400; 62 | break; 63 | case 57600: 64 | brate = B57600; 65 | break; 66 | case 115200: 67 | brate = B115200; 68 | break; 69 | case 230400: 70 | brate = B230400; 71 | break; 72 | case 460800: 73 | brate = B460800; 74 | break; 75 | case 921600: 76 | brate = B921600; 77 | break; 78 | default: 79 | brate = B115200; 80 | break; 81 | } 82 | options.c_cflag = brate; 83 | #else // APPLE 84 | cfsetispeed(&options, baud); 85 | cfsetospeed(&options, baud); 86 | #endif 87 | 88 | options.c_cflag |= CRTSCTS | CS8 | CLOCAL | CREAD; 89 | options.c_iflag = IGNPAR; 90 | options.c_iflag &= (~(IXON|IXOFF|IXANY)); // turn off software flow control 91 | options.c_oflag = 0; 92 | 93 | /* set input mode (non-canonical, no echo,...) */ 94 | options.c_lflag = 0; 95 | 96 | options.c_cc[VTIME] = 0; /* inter-character timer unused */ 97 | options.c_cc[VMIN] = 1; /* blocking read until 1 chars received */ 98 | 99 | #ifdef CCTS_OFLOW 100 | options.c_cflag |= CCTS_OFLOW; 101 | #endif 102 | 103 | if (!ctsRts) 104 | options.c_cflag &= ~(CRTSCTS); // turn off hardware flow control 105 | 106 | // set the new port options 107 | tcsetattr(s->fd, TCSANOW, &options); 108 | 109 | return s; 110 | } 111 | 112 | void serialFree(serialStruct_t *s) { 113 | if (s) { 114 | if (s->fd) 115 | close(s->fd); 116 | free (s); 117 | } 118 | } 119 | 120 | void serialNoParity(serialStruct_t *s) { 121 | struct termios options; 122 | 123 | tcgetattr(s->fd, &options); // read serial port options 124 | 125 | options.c_cflag &= ~(PARENB | CSTOPB); 126 | 127 | tcsetattr(s->fd, TCSANOW, &options); 128 | } 129 | 130 | void serialEvenParity(serialStruct_t *s) { 131 | struct termios options; 132 | 133 | tcgetattr(s->fd, &options); // read serial port options 134 | 135 | options.c_cflag |= (PARENB); 136 | options.c_cflag &= ~(PARODD | CSTOPB); 137 | 138 | tcsetattr(s->fd, TCSANOW, &options); 139 | } 140 | 141 | void serialWriteChar(serialStruct_t *s, unsigned char c) { 142 | char ret; 143 | 144 | ret = write(s->fd, &c, 1); 145 | } 146 | 147 | void serialWrite(serialStruct_t *s, const char *str, unsigned int len) { 148 | char ret; 149 | 150 | ret = write(s->fd, str, len); 151 | } 152 | 153 | unsigned char serialAvailable(serialStruct_t *s) { 154 | fd_set fdSet; 155 | struct timeval timeout; 156 | 157 | FD_ZERO(&fdSet); 158 | FD_SET(s->fd, &fdSet); 159 | memset((char *)&timeout, 0, sizeof(timeout)); 160 | 161 | if (select(s->fd+1, &fdSet, 0, 0, &timeout) == 1) 162 | return 1; 163 | else 164 | return 0; 165 | } 166 | 167 | void serialFlush(serialStruct_t *s) { 168 | while (serialAvailable(s)) 169 | serialRead(s); 170 | } 171 | 172 | unsigned char serialRead(serialStruct_t *s) { 173 | char ret; 174 | unsigned char c; 175 | 176 | ret = read(s->fd, &c, 1); 177 | 178 | return c; 179 | } 180 | -------------------------------------------------------------------------------- /lib/STM32F10x_StdPeriph_Driver/inc/stm32f10x_pwr.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f10x_pwr.h 4 | * @author MCD Application Team 5 | * @version V3.5.0 6 | * @date 11-March-2011 7 | * @brief This file contains all the functions prototypes for the PWR firmware 8 | * library. 9 | ****************************************************************************** 10 | * @attention 11 | * 12 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS 13 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE 14 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY 15 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING 16 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE 17 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. 18 | * 19 | *

© COPYRIGHT 2011 STMicroelectronics

20 | ****************************************************************************** 21 | */ 22 | 23 | /* Define to prevent recursive inclusion -------------------------------------*/ 24 | #ifndef __STM32F10x_PWR_H 25 | #define __STM32F10x_PWR_H 26 | 27 | #ifdef __cplusplus 28 | extern "C" { 29 | #endif 30 | 31 | /* Includes ------------------------------------------------------------------*/ 32 | #include "stm32f10x.h" 33 | 34 | /** @addtogroup STM32F10x_StdPeriph_Driver 35 | * @{ 36 | */ 37 | 38 | /** @addtogroup PWR 39 | * @{ 40 | */ 41 | 42 | /** @defgroup PWR_Exported_Types 43 | * @{ 44 | */ 45 | 46 | /** 47 | * @} 48 | */ 49 | 50 | /** @defgroup PWR_Exported_Constants 51 | * @{ 52 | */ 53 | 54 | /** @defgroup PVD_detection_level 55 | * @{ 56 | */ 57 | 58 | #define PWR_PVDLevel_2V2 ((uint32_t)0x00000000) 59 | #define PWR_PVDLevel_2V3 ((uint32_t)0x00000020) 60 | #define PWR_PVDLevel_2V4 ((uint32_t)0x00000040) 61 | #define PWR_PVDLevel_2V5 ((uint32_t)0x00000060) 62 | #define PWR_PVDLevel_2V6 ((uint32_t)0x00000080) 63 | #define PWR_PVDLevel_2V7 ((uint32_t)0x000000A0) 64 | #define PWR_PVDLevel_2V8 ((uint32_t)0x000000C0) 65 | #define PWR_PVDLevel_2V9 ((uint32_t)0x000000E0) 66 | #define IS_PWR_PVD_LEVEL(LEVEL) (((LEVEL) == PWR_PVDLevel_2V2) || ((LEVEL) == PWR_PVDLevel_2V3)|| \ 67 | ((LEVEL) == PWR_PVDLevel_2V4) || ((LEVEL) == PWR_PVDLevel_2V5)|| \ 68 | ((LEVEL) == PWR_PVDLevel_2V6) || ((LEVEL) == PWR_PVDLevel_2V7)|| \ 69 | ((LEVEL) == PWR_PVDLevel_2V8) || ((LEVEL) == PWR_PVDLevel_2V9)) 70 | /** 71 | * @} 72 | */ 73 | 74 | /** @defgroup Regulator_state_is_STOP_mode 75 | * @{ 76 | */ 77 | 78 | #define PWR_Regulator_ON ((uint32_t)0x00000000) 79 | #define PWR_Regulator_LowPower ((uint32_t)0x00000001) 80 | #define IS_PWR_REGULATOR(REGULATOR) (((REGULATOR) == PWR_Regulator_ON) || \ 81 | ((REGULATOR) == PWR_Regulator_LowPower)) 82 | /** 83 | * @} 84 | */ 85 | 86 | /** @defgroup STOP_mode_entry 87 | * @{ 88 | */ 89 | 90 | #define PWR_STOPEntry_WFI ((uint8_t)0x01) 91 | #define PWR_STOPEntry_WFE ((uint8_t)0x02) 92 | #define IS_PWR_STOP_ENTRY(ENTRY) (((ENTRY) == PWR_STOPEntry_WFI) || ((ENTRY) == PWR_STOPEntry_WFE)) 93 | 94 | /** 95 | * @} 96 | */ 97 | 98 | /** @defgroup PWR_Flag 99 | * @{ 100 | */ 101 | 102 | #define PWR_FLAG_WU ((uint32_t)0x00000001) 103 | #define PWR_FLAG_SB ((uint32_t)0x00000002) 104 | #define PWR_FLAG_PVDO ((uint32_t)0x00000004) 105 | #define IS_PWR_GET_FLAG(FLAG) (((FLAG) == PWR_FLAG_WU) || ((FLAG) == PWR_FLAG_SB) || \ 106 | ((FLAG) == PWR_FLAG_PVDO)) 107 | 108 | #define IS_PWR_CLEAR_FLAG(FLAG) (((FLAG) == PWR_FLAG_WU) || ((FLAG) == PWR_FLAG_SB)) 109 | /** 110 | * @} 111 | */ 112 | 113 | /** 114 | * @} 115 | */ 116 | 117 | /** @defgroup PWR_Exported_Macros 118 | * @{ 119 | */ 120 | 121 | /** 122 | * @} 123 | */ 124 | 125 | /** @defgroup PWR_Exported_Functions 126 | * @{ 127 | */ 128 | 129 | void PWR_DeInit(void); 130 | void PWR_BackupAccessCmd(FunctionalState NewState); 131 | void PWR_PVDCmd(FunctionalState NewState); 132 | void PWR_PVDLevelConfig(uint32_t PWR_PVDLevel); 133 | void PWR_WakeUpPinCmd(FunctionalState NewState); 134 | void PWR_EnterSTOPMode(uint32_t PWR_Regulator, uint8_t PWR_STOPEntry); 135 | void PWR_EnterSTANDBYMode(void); 136 | FlagStatus PWR_GetFlagStatus(uint32_t PWR_FLAG); 137 | void PWR_ClearFlag(uint32_t PWR_FLAG); 138 | 139 | #ifdef __cplusplus 140 | } 141 | #endif 142 | 143 | #endif /* __STM32F10x_PWR_H */ 144 | /** 145 | * @} 146 | */ 147 | 148 | /** 149 | * @} 150 | */ 151 | 152 | /** 153 | * @} 154 | */ 155 | 156 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ 157 | -------------------------------------------------------------------------------- /src/printf.h: -------------------------------------------------------------------------------- 1 | /* 2 | File: printf.h 3 | 4 | Copyright (c) 2004,2012 Kustaa Nyholm / SpareTimeLabs 5 | 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without modification, 9 | are permitted provided that the following conditions are met: 10 | 11 | Redistributions of source code must retain the above copyright notice, this list 12 | of conditions and the following disclaimer. 13 | 14 | Redistributions in binary form must reproduce the above copyright notice, this 15 | list of conditions and the following disclaimer in the documentation and/or other 16 | materials provided with the distribution. 17 | 18 | Neither the name of the Kustaa Nyholm or SpareTimeLabs nor the names of its 19 | contributors may be used to endorse or promote products derived from this software 20 | without specific prior written permission. 21 | 22 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 24 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 25 | IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 27 | NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 28 | OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY 31 | OF SUCH DAMAGE. 32 | 33 | ---------------------------------------------------------------------- 34 | 35 | This library is realy just two files: 'printf.h' and 'printf.c'. 36 | 37 | They provide a simple and small (+200 loc) printf functionality to 38 | be used in embedded systems. 39 | 40 | I've found them so usefull in debugging that I do not bother with a 41 | debugger at all. 42 | 43 | They are distributed in source form, so to use them, just compile them 44 | into your project. 45 | 46 | Two printf variants are provided: printf and sprintf. 47 | 48 | The formats supported by this implementation are: 'd' 'u' 'c' 's' 'x' 'X'. 49 | 50 | Zero padding and field width are also supported. 51 | 52 | If the library is compiled with 'PRINTF_SUPPORT_LONG' defined then the 53 | long specifier is also 54 | supported. Note that this will pull in some long math routines (pun intended!) 55 | and thus make your executable noticably longer. 56 | 57 | The memory foot print of course depends on the target cpu, compiler and 58 | compiler options, but a rough guestimate (based on a H8S target) is about 59 | 1.4 kB for code and some twenty 'int's and 'char's, say 60 bytes of stack space. 60 | Not too bad. Your milage may vary. By hacking the source code you can 61 | get rid of some hunred bytes, I'm sure, but personally I feel the balance of 62 | functionality and flexibility versus code size is close to optimal for 63 | many embedded systems. 64 | 65 | To use the printf you need to supply your own character output function, 66 | something like : 67 | 68 | void putc ( void* p, char c) 69 | { 70 | while (!SERIAL_PORT_EMPTY) ; 71 | SERIAL_PORT_TX_REGISTER = c; 72 | } 73 | 74 | Before you can call printf you need to initialize it to use your 75 | character output function with something like: 76 | 77 | init_printf(NULL,putc); 78 | 79 | Notice the 'NULL' in 'init_printf' and the parameter 'void* p' in 'putc', 80 | the NULL (or any pointer) you pass into the 'init_printf' will eventually be 81 | passed to your 'putc' routine. This allows you to pass some storage space (or 82 | anything realy) to the character output function, if necessary. 83 | This is not often needed but it was implemented like that because it made 84 | implementing the sprintf function so neat (look at the source code). 85 | 86 | The code is re-entrant, except for the 'init_printf' function, so it 87 | is safe to call it from interupts too, although this may result in mixed output. 88 | If you rely on re-entrancy, take care that your 'putc' function is re-entrant! 89 | 90 | The printf and sprintf functions are actually macros that translate to 91 | 'tfp_printf' and 'tfp_sprintf'. This makes it possible 92 | to use them along with 'stdio.h' printf's in a single source file. 93 | You just need to undef the names before you include the 'stdio.h'. 94 | Note that these are not function like macros, so if you have variables 95 | or struct members with these names, things will explode in your face. 96 | Without variadic macros this is the best we can do to wrap these 97 | fucnction. If it is a problem just give up the macros and use the 98 | functions directly or rename them. 99 | 100 | For further details see source code. 101 | 102 | regs Kusti, 23.10.2004 103 | */ 104 | 105 | 106 | #ifndef __TFP_PRINTF__ 107 | #define __TFP_PRINTF__ 108 | 109 | #include 110 | 111 | void init_printf(void *putp, void (*putf) (void *, char)); 112 | 113 | void tfp_printf(char *fmt, ...); 114 | void tfp_sprintf(char *s, char *fmt, ...); 115 | 116 | void tfp_format(void *putp, void (*putf) (void *, char), char *fmt, va_list va); 117 | 118 | #define printf tfp_printf 119 | #define sprintf tfp_sprintf 120 | 121 | #endif 122 | -------------------------------------------------------------------------------- /src/drv_mpu6500.c: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of baseflight 3 | * Licensed under GPL V3 or modified DCL - see https://github.com/multiwii/baseflight/blob/master/README.md 4 | */ 5 | 6 | #include "board.h" 7 | 8 | // MPU6500 (WHO_AM_I 0x70) on SPI bus 9 | 10 | #define MPU6500_RA_WHOAMI (0x75) 11 | #define MPU6500_RA_ACCEL_XOUT_H (0x3B) 12 | #define MPU6500_RA_GYRO_XOUT_H (0x43) 13 | #define MPU6500_RA_BANK_SEL (0x6D) 14 | #define MPU6500_RA_MEM_RW (0x6F) 15 | #define MPU6500_RA_GYRO_CFG (0x1B) 16 | #define MPU6500_RA_PWR_MGMT_1 (0x6B) 17 | #define MPU6500_RA_ACCEL_CFG (0x1C) 18 | #define MPU6500_RA_LPF (0x1A) 19 | #define MPU6500_RA_RATE_DIV (0x19) 20 | 21 | #define MPU6500_WHO_AM_I_CONST (0x70) 22 | #define BIT_RESET (0x80) 23 | 24 | enum lpf_e { 25 | INV_FILTER_256HZ_NOLPF2 = 0, 26 | INV_FILTER_188HZ, 27 | INV_FILTER_98HZ, 28 | INV_FILTER_42HZ, 29 | INV_FILTER_20HZ, 30 | INV_FILTER_10HZ, 31 | INV_FILTER_5HZ, 32 | INV_FILTER_2100HZ_NOLPF, 33 | NUM_FILTER 34 | }; 35 | 36 | enum gyro_fsr_e { 37 | INV_FSR_250DPS = 0, 38 | INV_FSR_500DPS, 39 | INV_FSR_1000DPS, 40 | INV_FSR_2000DPS, 41 | NUM_GYRO_FSR 42 | }; 43 | 44 | enum clock_sel_e { 45 | INV_CLK_INTERNAL = 0, 46 | INV_CLK_PLL, 47 | NUM_CLK 48 | }; 49 | 50 | enum accel_fsr_e { 51 | INV_FSR_2G = 0, 52 | INV_FSR_4G, 53 | INV_FSR_8G, 54 | INV_FSR_16G, 55 | NUM_ACCEL_FSR 56 | }; 57 | 58 | static uint8_t mpuLowPassFilter = INV_FILTER_42HZ; 59 | static sensor_align_e gyroAlign = CW0_DEG; 60 | static sensor_align_e accAlign = CW0_DEG; 61 | 62 | static void mpu6500AccInit(sensor_align_e align); 63 | static void mpu6500AccRead(int16_t *accData); 64 | static void mpu6500GyroInit(sensor_align_e align); 65 | static void mpu6500GyroRead(int16_t *gyroData); 66 | 67 | extern uint16_t acc_1G; 68 | 69 | static void mpu6500WriteRegister(uint8_t reg, uint8_t data) 70 | { 71 | spiSelect(true); 72 | spiTransferByte(reg); 73 | spiTransferByte(data); 74 | spiSelect(false); 75 | } 76 | 77 | static void mpu6500ReadRegister(uint8_t reg, uint8_t *data, int length) 78 | { 79 | spiSelect(true); 80 | spiTransferByte(reg | 0x80); // read transaction 81 | spiTransfer(data, NULL, length); 82 | spiSelect(false); 83 | } 84 | 85 | bool mpu6500Detect(sensor_t *acc, sensor_t *gyro, uint16_t lpf) 86 | { 87 | uint8_t tmp; 88 | 89 | mpu6500ReadRegister(MPU6500_RA_WHOAMI, &tmp, 1); 90 | if (tmp != MPU6500_WHO_AM_I_CONST) 91 | return false; 92 | 93 | acc->init = mpu6500AccInit; 94 | acc->read = mpu6500AccRead; 95 | gyro->init = mpu6500GyroInit; 96 | gyro->read = mpu6500GyroRead; 97 | 98 | // 16.4 dps/lsb scalefactor 99 | gyro->scale = (4.0f / 16.4f) * (M_PI / 180.0f) * 0.000001f; 100 | 101 | // default lpf is 42Hz 102 | if (lpf >= 188) 103 | mpuLowPassFilter = INV_FILTER_188HZ; 104 | else if (lpf >= 98) 105 | mpuLowPassFilter = INV_FILTER_98HZ; 106 | else if (lpf >= 42) 107 | mpuLowPassFilter = INV_FILTER_42HZ; 108 | else if (lpf >= 20) 109 | mpuLowPassFilter = INV_FILTER_20HZ; 110 | else if (lpf >= 10) 111 | mpuLowPassFilter = INV_FILTER_10HZ; 112 | else 113 | mpuLowPassFilter = INV_FILTER_5HZ; 114 | 115 | return true; 116 | } 117 | 118 | static void mpu6500AccInit(sensor_align_e align) 119 | { 120 | acc_1G = 512 * 8; 121 | 122 | if (align > 0) 123 | accAlign = align; 124 | } 125 | 126 | static void mpu6500AccRead(int16_t *accData) 127 | { 128 | uint8_t buf[6]; 129 | int16_t data[3]; 130 | 131 | mpu6500ReadRegister(MPU6500_RA_ACCEL_XOUT_H, buf, 6); 132 | 133 | data[0] = (int16_t)((buf[0] << 8) | buf[1]); 134 | data[1] = (int16_t)((buf[2] << 8) | buf[3]); 135 | data[2] = (int16_t)((buf[4] << 8) | buf[5]); 136 | 137 | alignSensors(data, accData, accAlign); 138 | } 139 | 140 | static void mpu6500GyroInit(sensor_align_e align) 141 | { 142 | gpio_config_t gpio; 143 | 144 | // MPU_INT output on rev5 hardware (PC13). rev4 was on PB13, conflicts with SPI devices 145 | if (hw_revision >= NAZE32_REV5) { 146 | gpio.pin = Pin_13; 147 | gpio.speed = Speed_2MHz; 148 | gpio.mode = Mode_IN_FLOATING; 149 | gpioInit(GPIOC, &gpio); 150 | } 151 | 152 | mpu6500WriteRegister(MPU6500_RA_PWR_MGMT_1, BIT_RESET); 153 | delay(100); 154 | mpu6500WriteRegister(MPU6500_RA_PWR_MGMT_1, 0); 155 | delay(100); 156 | mpu6500WriteRegister(MPU6500_RA_PWR_MGMT_1, INV_CLK_PLL); 157 | mpu6500WriteRegister(MPU6500_RA_GYRO_CFG, INV_FSR_2000DPS << 3); 158 | mpu6500WriteRegister(MPU6500_RA_ACCEL_CFG, INV_FSR_8G << 3); 159 | mpu6500WriteRegister(MPU6500_RA_LPF, mpuLowPassFilter); 160 | mpu6500WriteRegister(MPU6500_RA_RATE_DIV, 0); // 1kHz S/R 161 | 162 | if (align > 0) 163 | gyroAlign = align; 164 | } 165 | 166 | static void mpu6500GyroRead(int16_t *gyroData) 167 | { 168 | uint8_t buf[6]; 169 | int16_t data[3]; 170 | 171 | mpu6500ReadRegister(MPU6500_RA_GYRO_XOUT_H, buf, 6); 172 | 173 | data[0] = (int16_t)((buf[0] << 8) | buf[1]) / 4; 174 | data[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4; 175 | data[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4; 176 | 177 | alignSensors(data, gyroData, gyroAlign); 178 | } 179 | -------------------------------------------------------------------------------- /src/drv_i2c_soft.c: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of baseflight 3 | * Licensed under GPL V3 or modified DCL - see https://github.com/multiwii/baseflight/blob/master/README.md 4 | */ 5 | #include "board.h" 6 | 7 | // Software I2C driver, using same pins as hardware I2C, with hw i2c module disabled. 8 | // Can be configured for I2C2 pinout (SCL: PB10, SDA: PB11) or I2C1 pinout (SCL: PB6, SDA: PB7) 9 | 10 | #ifdef SOFT_I2C 11 | 12 | #ifdef SOFT_I2C_PB1011 13 | #define SCL_H GPIOB->BSRR = Pin_10 14 | #define SCL_L GPIOB->BRR = Pin_10 15 | 16 | #define SDA_H GPIOB->BSRR = Pin_11 17 | #define SDA_L GPIOB->BRR = Pin_11 18 | 19 | #define SCL_read (GPIOB->IDR & Pin_10) 20 | #define SDA_read (GPIOB->IDR & Pin_11) 21 | #define I2C_PINS (Pin_10 | Pin_11) 22 | #define I2C_GPIO GPIOB 23 | #endif 24 | 25 | #ifdef SOFT_I2C_PB67 26 | #define SCL_H GPIOB->BSRR = Pin_6 27 | #define SCL_L GPIOB->BRR = Pin_6 28 | 29 | #define SDA_H GPIOB->BSRR = Pin_7 30 | #define SDA_L GPIOB->BRR = Pin_7 31 | 32 | #define SCL_read (GPIOB->IDR & Pin_6) 33 | #define SDA_read (GPIOB->IDR & Pin_7) 34 | #define I2C_PINS (Pin_6 | Pin_7) 35 | #define I2C_GPIO GPIOB 36 | #endif 37 | 38 | #ifndef SCL_H 39 | #error Need to define SOFT_I2C_PB1011 or SOFT_I2C_PB67 (see board.h) 40 | #endif 41 | 42 | static void I2C_delay(void) 43 | { 44 | volatile int i = 7; 45 | while (i) 46 | i--; 47 | } 48 | 49 | static bool I2C_Start(void) 50 | { 51 | SDA_H; 52 | SCL_H; 53 | I2C_delay(); 54 | if (!SDA_read) 55 | return false; 56 | SDA_L; 57 | I2C_delay(); 58 | if (SDA_read) 59 | return false; 60 | SDA_L; 61 | I2C_delay(); 62 | return true; 63 | } 64 | 65 | static void I2C_Stop(void) 66 | { 67 | SCL_L; 68 | I2C_delay(); 69 | SDA_L; 70 | I2C_delay(); 71 | SCL_H; 72 | I2C_delay(); 73 | SDA_H; 74 | I2C_delay(); 75 | } 76 | 77 | static void I2C_Ack(void) 78 | { 79 | SCL_L; 80 | I2C_delay(); 81 | SDA_L; 82 | I2C_delay(); 83 | SCL_H; 84 | I2C_delay(); 85 | SCL_L; 86 | I2C_delay(); 87 | } 88 | 89 | static void I2C_NoAck(void) 90 | { 91 | SCL_L; 92 | I2C_delay(); 93 | SDA_H; 94 | I2C_delay(); 95 | SCL_H; 96 | I2C_delay(); 97 | SCL_L; 98 | I2C_delay(); 99 | } 100 | 101 | static bool I2C_WaitAck(void) 102 | { 103 | SCL_L; 104 | I2C_delay(); 105 | SDA_H; 106 | I2C_delay(); 107 | SCL_H; 108 | I2C_delay(); 109 | if (SDA_read) { 110 | SCL_L; 111 | return false; 112 | } 113 | SCL_L; 114 | return true; 115 | } 116 | 117 | static void I2C_SendByte(uint8_t byte) 118 | { 119 | uint8_t i = 8; 120 | while (i--) { 121 | SCL_L; 122 | I2C_delay(); 123 | if (byte & 0x80) 124 | SDA_H; 125 | else 126 | SDA_L; 127 | byte <<= 1; 128 | I2C_delay(); 129 | SCL_H; 130 | I2C_delay(); 131 | } 132 | SCL_L; 133 | } 134 | 135 | static uint8_t I2C_ReceiveByte(void) 136 | { 137 | uint8_t i = 8; 138 | uint8_t byte = 0; 139 | 140 | SDA_H; 141 | while (i--) { 142 | byte <<= 1; 143 | SCL_L; 144 | I2C_delay(); 145 | SCL_H; 146 | I2C_delay(); 147 | if (SDA_read) { 148 | byte |= 0x01; 149 | } 150 | } 151 | SCL_L; 152 | return byte; 153 | } 154 | 155 | void i2cInit(I2C_TypeDef * I2C) 156 | { 157 | gpio_config_t gpio; 158 | 159 | gpio.pin = I2C_PINS; 160 | gpio.speed = Speed_2MHz; 161 | gpio.mode = Mode_Out_OD; 162 | gpioInit(I2C_GPIO, &gpio); 163 | } 164 | 165 | bool i2cWriteBuffer(uint8_t addr, uint8_t reg, uint8_t len, uint8_t * data) 166 | { 167 | int i; 168 | if (!I2C_Start()) 169 | return false; 170 | I2C_SendByte(addr << 1 | I2C_Direction_Transmitter); 171 | if (!I2C_WaitAck()) { 172 | I2C_Stop(); 173 | return false; 174 | } 175 | I2C_SendByte(reg); 176 | I2C_WaitAck(); 177 | for (i = 0; i < len; i++) { 178 | I2C_SendByte(data[i]); 179 | if (!I2C_WaitAck()) { 180 | I2C_Stop(); 181 | return false; 182 | } 183 | } 184 | I2C_Stop(); 185 | return true; 186 | } 187 | 188 | bool i2cWrite(uint8_t addr, uint8_t reg, uint8_t data) 189 | { 190 | if (!I2C_Start()) 191 | return false; 192 | I2C_SendByte(addr << 1 | I2C_Direction_Transmitter); 193 | if (!I2C_WaitAck()) { 194 | I2C_Stop(); 195 | return false; 196 | } 197 | I2C_SendByte(reg); 198 | I2C_WaitAck(); 199 | I2C_SendByte(data); 200 | I2C_WaitAck(); 201 | I2C_Stop(); 202 | return true; 203 | } 204 | 205 | bool i2cRead(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf) 206 | { 207 | if (!I2C_Start()) 208 | return false; 209 | I2C_SendByte(addr << 1 | I2C_Direction_Transmitter); 210 | if (!I2C_WaitAck()) { 211 | I2C_Stop(); 212 | return false; 213 | } 214 | I2C_SendByte(reg); 215 | I2C_WaitAck(); 216 | I2C_Start(); 217 | I2C_SendByte(addr << 1 | I2C_Direction_Receiver); 218 | I2C_WaitAck(); 219 | while (len) { 220 | *buf = I2C_ReceiveByte(); 221 | if (len == 1) 222 | I2C_NoAck(); 223 | else 224 | I2C_Ack(); 225 | buf++; 226 | len--; 227 | } 228 | I2C_Stop(); 229 | return true; 230 | } 231 | 232 | uint16_t i2cGetErrorCounter(void) 233 | { 234 | // TODO maybe fix this, but since this is test code, doesn't matter. 235 | return 0; 236 | } 237 | 238 | #endif 239 | -------------------------------------------------------------------------------- /lib/STM32F10x_StdPeriph_Driver/src/stm32f10x_iwdg.c: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f10x_iwdg.c 4 | * @author MCD Application Team 5 | * @version V3.5.0 6 | * @date 11-March-2011 7 | * @brief This file provides all the IWDG firmware functions. 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS 12 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE 13 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY 14 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING 15 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE 16 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. 17 | * 18 | *

© COPYRIGHT 2011 STMicroelectronics

19 | ****************************************************************************** 20 | */ 21 | 22 | /* Includes ------------------------------------------------------------------*/ 23 | #include "stm32f10x_iwdg.h" 24 | 25 | /** @addtogroup STM32F10x_StdPeriph_Driver 26 | * @{ 27 | */ 28 | 29 | /** @defgroup IWDG 30 | * @brief IWDG driver modules 31 | * @{ 32 | */ 33 | 34 | /** @defgroup IWDG_Private_TypesDefinitions 35 | * @{ 36 | */ 37 | 38 | /** 39 | * @} 40 | */ 41 | 42 | /** @defgroup IWDG_Private_Defines 43 | * @{ 44 | */ 45 | 46 | /* ---------------------- IWDG registers bit mask ----------------------------*/ 47 | 48 | /* KR register bit mask */ 49 | #define KR_KEY_Reload ((uint16_t)0xAAAA) 50 | #define KR_KEY_Enable ((uint16_t)0xCCCC) 51 | 52 | /** 53 | * @} 54 | */ 55 | 56 | /** @defgroup IWDG_Private_Macros 57 | * @{ 58 | */ 59 | 60 | /** 61 | * @} 62 | */ 63 | 64 | /** @defgroup IWDG_Private_Variables 65 | * @{ 66 | */ 67 | 68 | /** 69 | * @} 70 | */ 71 | 72 | /** @defgroup IWDG_Private_FunctionPrototypes 73 | * @{ 74 | */ 75 | 76 | /** 77 | * @} 78 | */ 79 | 80 | /** @defgroup IWDG_Private_Functions 81 | * @{ 82 | */ 83 | 84 | /** 85 | * @brief Enables or disables write access to IWDG_PR and IWDG_RLR registers. 86 | * @param IWDG_WriteAccess: new state of write access to IWDG_PR and IWDG_RLR registers. 87 | * This parameter can be one of the following values: 88 | * @arg IWDG_WriteAccess_Enable: Enable write access to IWDG_PR and IWDG_RLR registers 89 | * @arg IWDG_WriteAccess_Disable: Disable write access to IWDG_PR and IWDG_RLR registers 90 | * @retval None 91 | */ 92 | void IWDG_WriteAccessCmd(uint16_t IWDG_WriteAccess) 93 | { 94 | /* Check the parameters */ 95 | assert_param(IS_IWDG_WRITE_ACCESS(IWDG_WriteAccess)); 96 | IWDG->KR = IWDG_WriteAccess; 97 | } 98 | 99 | /** 100 | * @brief Sets IWDG Prescaler value. 101 | * @param IWDG_Prescaler: specifies the IWDG Prescaler value. 102 | * This parameter can be one of the following values: 103 | * @arg IWDG_Prescaler_4: IWDG prescaler set to 4 104 | * @arg IWDG_Prescaler_8: IWDG prescaler set to 8 105 | * @arg IWDG_Prescaler_16: IWDG prescaler set to 16 106 | * @arg IWDG_Prescaler_32: IWDG prescaler set to 32 107 | * @arg IWDG_Prescaler_64: IWDG prescaler set to 64 108 | * @arg IWDG_Prescaler_128: IWDG prescaler set to 128 109 | * @arg IWDG_Prescaler_256: IWDG prescaler set to 256 110 | * @retval None 111 | */ 112 | void IWDG_SetPrescaler(uint8_t IWDG_Prescaler) 113 | { 114 | /* Check the parameters */ 115 | assert_param(IS_IWDG_PRESCALER(IWDG_Prescaler)); 116 | IWDG->PR = IWDG_Prescaler; 117 | } 118 | 119 | /** 120 | * @brief Sets IWDG Reload value. 121 | * @param Reload: specifies the IWDG Reload value. 122 | * This parameter must be a number between 0 and 0x0FFF. 123 | * @retval None 124 | */ 125 | void IWDG_SetReload(uint16_t Reload) 126 | { 127 | /* Check the parameters */ 128 | assert_param(IS_IWDG_RELOAD(Reload)); 129 | IWDG->RLR = Reload; 130 | } 131 | 132 | /** 133 | * @brief Reloads IWDG counter with value defined in the reload register 134 | * (write access to IWDG_PR and IWDG_RLR registers disabled). 135 | * @param None 136 | * @retval None 137 | */ 138 | void IWDG_ReloadCounter(void) 139 | { 140 | IWDG->KR = KR_KEY_Reload; 141 | } 142 | 143 | /** 144 | * @brief Enables IWDG (write access to IWDG_PR and IWDG_RLR registers disabled). 145 | * @param None 146 | * @retval None 147 | */ 148 | void IWDG_Enable(void) 149 | { 150 | IWDG->KR = KR_KEY_Enable; 151 | } 152 | 153 | /** 154 | * @brief Checks whether the specified IWDG flag is set or not. 155 | * @param IWDG_FLAG: specifies the flag to check. 156 | * This parameter can be one of the following values: 157 | * @arg IWDG_FLAG_PVU: Prescaler Value Update on going 158 | * @arg IWDG_FLAG_RVU: Reload Value Update on going 159 | * @retval The new state of IWDG_FLAG (SET or RESET). 160 | */ 161 | FlagStatus IWDG_GetFlagStatus(uint16_t IWDG_FLAG) 162 | { 163 | FlagStatus bitstatus = RESET; 164 | /* Check the parameters */ 165 | assert_param(IS_IWDG_FLAG(IWDG_FLAG)); 166 | if ((IWDG->SR & IWDG_FLAG) != (uint32_t)RESET) 167 | { 168 | bitstatus = SET; 169 | } 170 | else 171 | { 172 | bitstatus = RESET; 173 | } 174 | /* Return the flag status */ 175 | return bitstatus; 176 | } 177 | 178 | /** 179 | * @} 180 | */ 181 | 182 | /** 183 | * @} 184 | */ 185 | 186 | /** 187 | * @} 188 | */ 189 | 190 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ 191 | -------------------------------------------------------------------------------- /lib/STM32F10x_StdPeriph_Driver/src/stm32f10x_dbgmcu.c: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f10x_dbgmcu.c 4 | * @author MCD Application Team 5 | * @version V3.5.0 6 | * @date 11-March-2011 7 | * @brief This file provides all the DBGMCU firmware functions. 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS 12 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE 13 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY 14 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING 15 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE 16 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. 17 | * 18 | *

© COPYRIGHT 2011 STMicroelectronics

19 | ****************************************************************************** 20 | */ 21 | 22 | /* Includes ------------------------------------------------------------------*/ 23 | #include "stm32f10x_dbgmcu.h" 24 | 25 | /** @addtogroup STM32F10x_StdPeriph_Driver 26 | * @{ 27 | */ 28 | 29 | /** @defgroup DBGMCU 30 | * @brief DBGMCU driver modules 31 | * @{ 32 | */ 33 | 34 | /** @defgroup DBGMCU_Private_TypesDefinitions 35 | * @{ 36 | */ 37 | 38 | /** 39 | * @} 40 | */ 41 | 42 | /** @defgroup DBGMCU_Private_Defines 43 | * @{ 44 | */ 45 | 46 | #define IDCODE_DEVID_MASK ((uint32_t)0x00000FFF) 47 | /** 48 | * @} 49 | */ 50 | 51 | /** @defgroup DBGMCU_Private_Macros 52 | * @{ 53 | */ 54 | 55 | /** 56 | * @} 57 | */ 58 | 59 | /** @defgroup DBGMCU_Private_Variables 60 | * @{ 61 | */ 62 | 63 | /** 64 | * @} 65 | */ 66 | 67 | /** @defgroup DBGMCU_Private_FunctionPrototypes 68 | * @{ 69 | */ 70 | 71 | /** 72 | * @} 73 | */ 74 | 75 | /** @defgroup DBGMCU_Private_Functions 76 | * @{ 77 | */ 78 | 79 | /** 80 | * @brief Returns the device revision identifier. 81 | * @param None 82 | * @retval Device revision identifier 83 | */ 84 | uint32_t DBGMCU_GetREVID(void) 85 | { 86 | return(DBGMCU->IDCODE >> 16); 87 | } 88 | 89 | /** 90 | * @brief Returns the device identifier. 91 | * @param None 92 | * @retval Device identifier 93 | */ 94 | uint32_t DBGMCU_GetDEVID(void) 95 | { 96 | return(DBGMCU->IDCODE & IDCODE_DEVID_MASK); 97 | } 98 | 99 | /** 100 | * @brief Configures the specified peripheral and low power mode behavior 101 | * when the MCU under Debug mode. 102 | * @param DBGMCU_Periph: specifies the peripheral and low power mode. 103 | * This parameter can be any combination of the following values: 104 | * @arg DBGMCU_SLEEP: Keep debugger connection during SLEEP mode 105 | * @arg DBGMCU_STOP: Keep debugger connection during STOP mode 106 | * @arg DBGMCU_STANDBY: Keep debugger connection during STANDBY mode 107 | * @arg DBGMCU_IWDG_STOP: Debug IWDG stopped when Core is halted 108 | * @arg DBGMCU_WWDG_STOP: Debug WWDG stopped when Core is halted 109 | * @arg DBGMCU_TIM1_STOP: TIM1 counter stopped when Core is halted 110 | * @arg DBGMCU_TIM2_STOP: TIM2 counter stopped when Core is halted 111 | * @arg DBGMCU_TIM3_STOP: TIM3 counter stopped when Core is halted 112 | * @arg DBGMCU_TIM4_STOP: TIM4 counter stopped when Core is halted 113 | * @arg DBGMCU_CAN1_STOP: Debug CAN2 stopped when Core is halted 114 | * @arg DBGMCU_I2C1_SMBUS_TIMEOUT: I2C1 SMBUS timeout mode stopped when Core is halted 115 | * @arg DBGMCU_I2C2_SMBUS_TIMEOUT: I2C2 SMBUS timeout mode stopped when Core is halted 116 | * @arg DBGMCU_TIM5_STOP: TIM5 counter stopped when Core is halted 117 | * @arg DBGMCU_TIM6_STOP: TIM6 counter stopped when Core is halted 118 | * @arg DBGMCU_TIM7_STOP: TIM7 counter stopped when Core is halted 119 | * @arg DBGMCU_TIM8_STOP: TIM8 counter stopped when Core is halted 120 | * @arg DBGMCU_CAN2_STOP: Debug CAN2 stopped when Core is halted 121 | * @arg DBGMCU_TIM15_STOP: TIM15 counter stopped when Core is halted 122 | * @arg DBGMCU_TIM16_STOP: TIM16 counter stopped when Core is halted 123 | * @arg DBGMCU_TIM17_STOP: TIM17 counter stopped when Core is halted 124 | * @arg DBGMCU_TIM9_STOP: TIM9 counter stopped when Core is halted 125 | * @arg DBGMCU_TIM10_STOP: TIM10 counter stopped when Core is halted 126 | * @arg DBGMCU_TIM11_STOP: TIM11 counter stopped when Core is halted 127 | * @arg DBGMCU_TIM12_STOP: TIM12 counter stopped when Core is halted 128 | * @arg DBGMCU_TIM13_STOP: TIM13 counter stopped when Core is halted 129 | * @arg DBGMCU_TIM14_STOP: TIM14 counter stopped when Core is halted 130 | * @param NewState: new state of the specified peripheral in Debug mode. 131 | * This parameter can be: ENABLE or DISABLE. 132 | * @retval None 133 | */ 134 | void DBGMCU_Config(uint32_t DBGMCU_Periph, FunctionalState NewState) 135 | { 136 | /* Check the parameters */ 137 | assert_param(IS_DBGMCU_PERIPH(DBGMCU_Periph)); 138 | assert_param(IS_FUNCTIONAL_STATE(NewState)); 139 | 140 | if (NewState != DISABLE) 141 | { 142 | DBGMCU->CR |= DBGMCU_Periph; 143 | } 144 | else 145 | { 146 | DBGMCU->CR &= ~DBGMCU_Periph; 147 | } 148 | } 149 | 150 | /** 151 | * @} 152 | */ 153 | 154 | /** 155 | * @} 156 | */ 157 | 158 | /** 159 | * @} 160 | */ 161 | 162 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ 163 | -------------------------------------------------------------------------------- /src/spektrum.c: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of baseflight 3 | * Licensed under GPL V3 or modified DCL - see https://github.com/multiwii/baseflight/blob/master/README.md 4 | */ 5 | 6 | #include "board.h" 7 | #include "mw.h" 8 | 9 | // driver for spektrum satellite receiver / sbus using UART2 (freeing up more motor outputs for stuff) 10 | 11 | #define SPEK_2048_MAX_CHANNEL 8 12 | #define SPEK_1024_MAX_CHANNEL 7 13 | #define SPEK_FRAME_SIZE 16 14 | static uint8_t spek_chan_shift; 15 | static uint8_t spek_chan_mask; 16 | static bool rcFrameComplete = false; 17 | static bool spekHiRes = false; 18 | static bool spekDataIncoming = false; 19 | static GPIO_TypeDef *spekBindPort = NULL; 20 | static USART_TypeDef *spekUart = NULL; 21 | static uint16_t spekBindPin = 0; 22 | 23 | volatile uint8_t spekFrame[SPEK_FRAME_SIZE]; 24 | static void spektrumDataReceive(uint16_t c); 25 | static uint16_t spektrumReadRawRC(uint8_t chan); 26 | 27 | // external vars (ugh) 28 | extern int16_t failsafeCnt; 29 | 30 | void spektrumInit(rcReadRawDataPtr *callback) 31 | { 32 | switch (mcfg.serialrx_type) { 33 | case SERIALRX_SPEKTRUM2048: 34 | // 11 bit frames 35 | spek_chan_shift = 3; 36 | spek_chan_mask = 0x07; 37 | spekHiRes = true; 38 | core.numRCChannels = SPEK_2048_MAX_CHANNEL; 39 | break; 40 | case SERIALRX_SPEKTRUM1024: 41 | // 10 bit frames 42 | spek_chan_shift = 2; 43 | spek_chan_mask = 0x03; 44 | spekHiRes = false; 45 | core.numRCChannels = SPEK_1024_MAX_CHANNEL; 46 | break; 47 | } 48 | 49 | // spekUart is set by spektrumBind() which is called very early at startup 50 | core.rcvrport = uartOpen(spekUart, spektrumDataReceive, 115200, MODE_RX); 51 | 52 | if (callback) 53 | *callback = spektrumReadRawRC; 54 | } 55 | 56 | // Receive ISR callback 57 | static void spektrumDataReceive(uint16_t c) 58 | { 59 | uint32_t spekTime; 60 | static uint32_t spekTimeLast, spekTimeInterval; 61 | static uint8_t spekFramePosition; 62 | 63 | spekDataIncoming = true; 64 | spekTime = micros(); 65 | spekTimeInterval = spekTime - spekTimeLast; 66 | spekTimeLast = spekTime; 67 | if (spekTimeInterval > 5000) 68 | spekFramePosition = 0; 69 | spekFrame[spekFramePosition] = (uint8_t)c; 70 | if (spekFramePosition == SPEK_FRAME_SIZE - 1) { 71 | rcFrameComplete = true; 72 | failsafeCnt = 0; // clear FailSafe counter 73 | } else { 74 | spekFramePosition++; 75 | } 76 | } 77 | 78 | bool spektrumFrameComplete(void) 79 | { 80 | return rcFrameComplete; 81 | } 82 | 83 | static uint16_t spektrumReadRawRC(uint8_t chan) 84 | { 85 | uint16_t data; 86 | static uint32_t spekChannelData[SPEK_2048_MAX_CHANNEL]; 87 | uint8_t b; 88 | 89 | if (rcFrameComplete) { 90 | for (b = 3; b < SPEK_FRAME_SIZE; b += 2) { 91 | uint8_t spekChannel = 0x0F & (spekFrame[b - 1] >> spek_chan_shift); 92 | if (spekChannel < core.numRCChannels) 93 | spekChannelData[spekChannel] = ((uint32_t)(spekFrame[b - 1] & spek_chan_mask) << 8) + spekFrame[b]; 94 | } 95 | rcFrameComplete = false; 96 | } 97 | 98 | if (chan >= core.numRCChannels || !spekDataIncoming) { 99 | data = mcfg.midrc; 100 | } else { 101 | if (spekHiRes) 102 | data = 988 + (spekChannelData[mcfg.rcmap[chan]] >> 1); // 2048 mode 103 | else 104 | data = 988 + spekChannelData[mcfg.rcmap[chan]]; // 1024 mode 105 | } 106 | 107 | return data; 108 | } 109 | 110 | /* spektrumBind function. It's used to bind satellite receiver to TX. 111 | * Function must be called immediately after startup so that we don't miss satellite bind window. 112 | * Known parameters. Tested with DSMX satellite and DX8 radio. Framerate (11ms or 22ms) must be selected from TX. 113 | * 9 = DSMX 11ms / DSMX 22ms 114 | * 5 = DSM2 11ms 2048 / DSM2 22ms 1024 115 | */ 116 | void spektrumBind(void) 117 | { 118 | int i; 119 | gpio_config_t gpio; 120 | 121 | #ifdef HARDWARE_BIND_PLUG 122 | // Check status of bind plug and exit if not active 123 | GPIO_TypeDef *hwBindPort = NULL; 124 | uint16_t hwBindPin = 0; 125 | 126 | hwBindPort = GPIOB; 127 | hwBindPin = Pin_5; 128 | gpio.speed = Speed_2MHz; 129 | gpio.pin = hwBindPin; 130 | gpio.mode = Mode_IPU; 131 | gpioInit(hwBindPort, &gpio); 132 | if (digitalIn(hwBindPort, hwBindPin)) 133 | return; 134 | #endif 135 | 136 | if (mcfg.spektrum_sat_on_flexport) { 137 | // USART3, PB11 138 | spekBindPort = GPIOB; 139 | spekBindPin = Pin_11; 140 | spekUart = USART3; 141 | } else { 142 | // USART2, PA3 143 | spekBindPort = GPIOA; 144 | spekBindPin = Pin_3; 145 | spekUart = USART2; 146 | } 147 | 148 | // don't try to bind if: here after soft reset or bind flag is out of range 149 | if (rccReadBkpDr() == BKP_SOFTRESET || mcfg.spektrum_sat_bind == 0 || mcfg.spektrum_sat_bind > 10) 150 | return; 151 | 152 | gpio.speed = Speed_2MHz; 153 | gpio.pin = spekBindPin; 154 | gpio.mode = Mode_Out_OD; 155 | gpioInit(spekBindPort, &gpio); 156 | // RX line, set high 157 | digitalHi(spekBindPort, spekBindPin); 158 | // Bind window is around 20-140ms after powerup 159 | delay(60); 160 | 161 | for (i = 0; i < mcfg.spektrum_sat_bind; i++) { 162 | // RX line, drive low for 120us 163 | digitalLo(spekBindPort, spekBindPin); 164 | delayMicroseconds(120); 165 | // RX line, drive high for 120us 166 | digitalHi(spekBindPort, spekBindPin); 167 | delayMicroseconds(120); 168 | } 169 | 170 | #ifndef HARDWARE_BIND_PLUG 171 | // If we came here as a result of hard reset (power up, with mcfg.spektrum_sat_bind set), then reset it back to zero and write config 172 | // Don't reset if hardware bind plug is present 173 | if (rccReadBkpDr() != BKP_SOFTRESET) { 174 | mcfg.spektrum_sat_bind = 0; 175 | writeEEPROM(1, true); 176 | } 177 | #endif 178 | 179 | } 180 | -------------------------------------------------------------------------------- /src/drv_ms5611.c: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of baseflight 3 | * Licensed under GPL V3 or modified DCL - see https://github.com/multiwii/baseflight/blob/master/README.md 4 | */ 5 | #include "board.h" 6 | 7 | // MS5611, Standard address 0x77 8 | #define MS5611_ADDR 0x77 9 | // Autodetect: turn off BMP085 while initializing ms5611 and check PROM crc to confirm device 10 | #define BMP085_OFF digitalLo(BARO_GPIO, BARO_PIN); 11 | #define BMP085_ON digitalHi(BARO_GPIO, BARO_PIN); 12 | 13 | #define CMD_RESET 0x1E // ADC reset command 14 | #define CMD_ADC_READ 0x00 // ADC read command 15 | #define CMD_ADC_CONV 0x40 // ADC conversion command 16 | #define CMD_ADC_D1 0x00 // ADC D1 conversion 17 | #define CMD_ADC_D2 0x10 // ADC D2 conversion 18 | #define CMD_ADC_256 0x00 // ADC OSR=256 19 | #define CMD_ADC_512 0x02 // ADC OSR=512 20 | #define CMD_ADC_1024 0x04 // ADC OSR=1024 21 | #define CMD_ADC_2048 0x06 // ADC OSR=2048 22 | #define CMD_ADC_4096 0x08 // ADC OSR=4096 23 | #define CMD_PROM_RD 0xA0 // Prom read command 24 | #define PROM_NB 8 25 | 26 | static void ms5611_reset(void); 27 | static uint16_t ms5611_prom(int8_t coef_num); 28 | static int8_t ms5611_crc(uint16_t *prom); 29 | static uint32_t ms5611_read_adc(void); 30 | static void ms5611_start_ut(void); 31 | static void ms5611_get_ut(void); 32 | static void ms5611_start_up(void); 33 | static void ms5611_get_up(void); 34 | static void ms5611_calculate(int32_t *pressure, int32_t *temperature); 35 | 36 | static uint32_t ms5611_ut; // static result of temperature measurement 37 | static uint32_t ms5611_up; // static result of pressure measurement 38 | static uint16_t ms5611_c[PROM_NB]; // on-chip ROM 39 | static uint8_t ms5611_osr = CMD_ADC_4096; 40 | 41 | bool ms5611Detect(baro_t *baro) 42 | { 43 | bool ack = false; 44 | uint8_t sig; 45 | int i; 46 | 47 | if (hw_revision == NAZE32) { 48 | // PC13 (BMP085's XCLR reset input, which we use to disable it). Only needed when running at 8MHz 49 | gpio_config_t gpio; 50 | gpio.pin = Pin_13; 51 | gpio.speed = Speed_2MHz; 52 | gpio.mode = Mode_Out_PP; 53 | gpioInit(GPIOC, &gpio); 54 | BMP085_OFF; 55 | } 56 | 57 | delay(10); // No idea how long the chip takes to power-up, but let's make it 10ms 58 | 59 | // BMP085 is disabled. If we have a MS5611, it will reply. if no reply, means either 60 | // we have BMP085 or no baro at all. 61 | ack = i2cRead(MS5611_ADDR, CMD_PROM_RD, 1, &sig); 62 | if (!ack) 63 | return false; 64 | 65 | ms5611_reset(); 66 | // read all coefficients 67 | for (i = 0; i < PROM_NB; i++) 68 | ms5611_c[i] = ms5611_prom(i); 69 | // check crc, bail out if wrong - we are probably talking to BMP085 w/o XCLR line! 70 | if (ms5611_crc(ms5611_c) != 0) 71 | return false; 72 | 73 | // TODO prom + CRC 74 | baro->ut_delay = 10000; 75 | baro->up_delay = 10000; 76 | baro->start_ut = ms5611_start_ut; 77 | baro->get_ut = ms5611_get_ut; 78 | baro->start_up = ms5611_start_up; 79 | baro->get_up = ms5611_get_up; 80 | baro->calculate = ms5611_calculate; 81 | 82 | return true; 83 | } 84 | 85 | static void ms5611_reset(void) 86 | { 87 | i2cWrite(MS5611_ADDR, CMD_RESET, 1); 88 | delayMicroseconds(2800); 89 | } 90 | 91 | static uint16_t ms5611_prom(int8_t coef_num) 92 | { 93 | uint8_t rxbuf[2] = { 0, 0 }; 94 | i2cRead(MS5611_ADDR, CMD_PROM_RD + coef_num * 2, 2, rxbuf); // send PROM READ command 95 | return rxbuf[0] << 8 | rxbuf[1]; 96 | } 97 | 98 | static int8_t ms5611_crc(uint16_t *prom) 99 | { 100 | int32_t i, j; 101 | uint32_t res = 0; 102 | uint8_t zero = 1; 103 | uint8_t crc = prom[7] & 0xF; 104 | prom[7] &= 0xFF00; 105 | 106 | // if eeprom is all zeros, we're probably fucked - BUT this will return valid CRC lol 107 | for (i = 0; i < 8; i++) { 108 | if (prom[i] != 0) 109 | zero = 0; 110 | } 111 | if (zero) 112 | return -1; 113 | 114 | for (i = 0; i < 16; i++) { 115 | if (i & 1) 116 | res ^= ((prom[i >> 1]) & 0x00FF); 117 | else 118 | res ^= (prom[i >> 1] >> 8); 119 | for (j = 8; j > 0; j--) { 120 | if (res & 0x8000) 121 | res ^= 0x1800; 122 | res <<= 1; 123 | } 124 | } 125 | prom[7] |= crc; 126 | if (crc == ((res >> 12) & 0xF)) 127 | return 0; 128 | 129 | return -1; 130 | } 131 | 132 | static uint32_t ms5611_read_adc(void) 133 | { 134 | uint8_t rxbuf[3]; 135 | i2cRead(MS5611_ADDR, CMD_ADC_READ, 3, rxbuf); // read ADC 136 | return (rxbuf[0] << 16) | (rxbuf[1] << 8) | rxbuf[2]; 137 | } 138 | 139 | static void ms5611_start_ut(void) 140 | { 141 | i2cWrite(MS5611_ADDR, CMD_ADC_CONV + CMD_ADC_D2 + ms5611_osr, 1); // D2 (temperature) conversion start! 142 | } 143 | 144 | static void ms5611_get_ut(void) 145 | { 146 | ms5611_ut = ms5611_read_adc(); 147 | } 148 | 149 | static void ms5611_start_up(void) 150 | { 151 | i2cWrite(MS5611_ADDR, CMD_ADC_CONV + CMD_ADC_D1 + ms5611_osr, 1); // D1 (pressure) conversion start! 152 | } 153 | 154 | static void ms5611_get_up(void) 155 | { 156 | ms5611_up = ms5611_read_adc(); 157 | } 158 | 159 | static void ms5611_calculate(int32_t *pressure, int32_t *temperature) 160 | { 161 | uint32_t press; 162 | int64_t temp; 163 | int64_t delt; 164 | int32_t dT = (int64_t)ms5611_ut - ((uint64_t)ms5611_c[5] * 256); 165 | int64_t off = ((int64_t)ms5611_c[2] << 16) + (((int64_t)ms5611_c[4] * dT) >> 7); 166 | int64_t sens = ((int64_t)ms5611_c[1] << 15) + (((int64_t)ms5611_c[3] * dT) >> 8); 167 | temp = 2000 + ((dT * (int64_t)ms5611_c[6]) >> 23); 168 | 169 | if (temp < 2000) { // temperature lower than 20degC 170 | delt = temp - 2000; 171 | delt = 5 * delt * delt; 172 | off -= delt >> 1; 173 | sens -= delt >> 2; 174 | if (temp < -1500) { // temperature lower than -15degC 175 | delt = temp + 1500; 176 | delt = delt * delt; 177 | off -= 7 * delt; 178 | sens -= (11 * delt) >> 1; 179 | } 180 | } 181 | press = ((((int64_t)ms5611_up * sens) >> 21) - off) >> 15; 182 | 183 | if (pressure) 184 | *pressure = press; 185 | if (temperature) 186 | *temperature = temp; 187 | } 188 | -------------------------------------------------------------------------------- /lib/STM32F10x_StdPeriph_Driver/src/stm32f10x_wwdg.c: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f10x_wwdg.c 4 | * @author MCD Application Team 5 | * @version V3.5.0 6 | * @date 11-March-2011 7 | * @brief This file provides all the WWDG firmware functions. 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS 12 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE 13 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY 14 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING 15 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE 16 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. 17 | * 18 | *

© COPYRIGHT 2011 STMicroelectronics

19 | ****************************************************************************** 20 | */ 21 | 22 | /* Includes ------------------------------------------------------------------*/ 23 | #include "stm32f10x_wwdg.h" 24 | #include "stm32f10x_rcc.h" 25 | 26 | /** @addtogroup STM32F10x_StdPeriph_Driver 27 | * @{ 28 | */ 29 | 30 | /** @defgroup WWDG 31 | * @brief WWDG driver modules 32 | * @{ 33 | */ 34 | 35 | /** @defgroup WWDG_Private_TypesDefinitions 36 | * @{ 37 | */ 38 | 39 | /** 40 | * @} 41 | */ 42 | 43 | /** @defgroup WWDG_Private_Defines 44 | * @{ 45 | */ 46 | 47 | /* ----------- WWDG registers bit address in the alias region ----------- */ 48 | #define WWDG_OFFSET (WWDG_BASE - PERIPH_BASE) 49 | 50 | /* Alias word address of EWI bit */ 51 | #define CFR_OFFSET (WWDG_OFFSET + 0x04) 52 | #define EWI_BitNumber 0x09 53 | #define CFR_EWI_BB (PERIPH_BB_BASE + (CFR_OFFSET * 32) + (EWI_BitNumber * 4)) 54 | 55 | /* --------------------- WWDG registers bit mask ------------------------ */ 56 | 57 | /* CR register bit mask */ 58 | #define CR_WDGA_Set ((uint32_t)0x00000080) 59 | 60 | /* CFR register bit mask */ 61 | #define CFR_WDGTB_Mask ((uint32_t)0xFFFFFE7F) 62 | #define CFR_W_Mask ((uint32_t)0xFFFFFF80) 63 | #define BIT_Mask ((uint8_t)0x7F) 64 | 65 | /** 66 | * @} 67 | */ 68 | 69 | /** @defgroup WWDG_Private_Macros 70 | * @{ 71 | */ 72 | 73 | /** 74 | * @} 75 | */ 76 | 77 | /** @defgroup WWDG_Private_Variables 78 | * @{ 79 | */ 80 | 81 | /** 82 | * @} 83 | */ 84 | 85 | /** @defgroup WWDG_Private_FunctionPrototypes 86 | * @{ 87 | */ 88 | 89 | /** 90 | * @} 91 | */ 92 | 93 | /** @defgroup WWDG_Private_Functions 94 | * @{ 95 | */ 96 | 97 | /** 98 | * @brief Deinitializes the WWDG peripheral registers to their default reset values. 99 | * @param None 100 | * @retval None 101 | */ 102 | void WWDG_DeInit(void) 103 | { 104 | RCC_APB1PeriphResetCmd(RCC_APB1Periph_WWDG, ENABLE); 105 | RCC_APB1PeriphResetCmd(RCC_APB1Periph_WWDG, DISABLE); 106 | } 107 | 108 | /** 109 | * @brief Sets the WWDG Prescaler. 110 | * @param WWDG_Prescaler: specifies the WWDG Prescaler. 111 | * This parameter can be one of the following values: 112 | * @arg WWDG_Prescaler_1: WWDG counter clock = (PCLK1/4096)/1 113 | * @arg WWDG_Prescaler_2: WWDG counter clock = (PCLK1/4096)/2 114 | * @arg WWDG_Prescaler_4: WWDG counter clock = (PCLK1/4096)/4 115 | * @arg WWDG_Prescaler_8: WWDG counter clock = (PCLK1/4096)/8 116 | * @retval None 117 | */ 118 | void WWDG_SetPrescaler(uint32_t WWDG_Prescaler) 119 | { 120 | uint32_t tmpreg = 0; 121 | /* Check the parameters */ 122 | assert_param(IS_WWDG_PRESCALER(WWDG_Prescaler)); 123 | /* Clear WDGTB[1:0] bits */ 124 | tmpreg = WWDG->CFR & CFR_WDGTB_Mask; 125 | /* Set WDGTB[1:0] bits according to WWDG_Prescaler value */ 126 | tmpreg |= WWDG_Prescaler; 127 | /* Store the new value */ 128 | WWDG->CFR = tmpreg; 129 | } 130 | 131 | /** 132 | * @brief Sets the WWDG window value. 133 | * @param WindowValue: specifies the window value to be compared to the downcounter. 134 | * This parameter value must be lower than 0x80. 135 | * @retval None 136 | */ 137 | void WWDG_SetWindowValue(uint8_t WindowValue) 138 | { 139 | __IO uint32_t tmpreg = 0; 140 | 141 | /* Check the parameters */ 142 | assert_param(IS_WWDG_WINDOW_VALUE(WindowValue)); 143 | /* Clear W[6:0] bits */ 144 | 145 | tmpreg = WWDG->CFR & CFR_W_Mask; 146 | 147 | /* Set W[6:0] bits according to WindowValue value */ 148 | tmpreg |= WindowValue & (uint32_t) BIT_Mask; 149 | 150 | /* Store the new value */ 151 | WWDG->CFR = tmpreg; 152 | } 153 | 154 | /** 155 | * @brief Enables the WWDG Early Wakeup interrupt(EWI). 156 | * @param None 157 | * @retval None 158 | */ 159 | void WWDG_EnableIT(void) 160 | { 161 | *(__IO uint32_t *) CFR_EWI_BB = (uint32_t)ENABLE; 162 | } 163 | 164 | /** 165 | * @brief Sets the WWDG counter value. 166 | * @param Counter: specifies the watchdog counter value. 167 | * This parameter must be a number between 0x40 and 0x7F. 168 | * @retval None 169 | */ 170 | void WWDG_SetCounter(uint8_t Counter) 171 | { 172 | /* Check the parameters */ 173 | assert_param(IS_WWDG_COUNTER(Counter)); 174 | /* Write to T[6:0] bits to configure the counter value, no need to do 175 | a read-modify-write; writing a 0 to WDGA bit does nothing */ 176 | WWDG->CR = Counter & BIT_Mask; 177 | } 178 | 179 | /** 180 | * @brief Enables WWDG and load the counter value. 181 | * @param Counter: specifies the watchdog counter value. 182 | * This parameter must be a number between 0x40 and 0x7F. 183 | * @retval None 184 | */ 185 | void WWDG_Enable(uint8_t Counter) 186 | { 187 | /* Check the parameters */ 188 | assert_param(IS_WWDG_COUNTER(Counter)); 189 | WWDG->CR = CR_WDGA_Set | Counter; 190 | } 191 | 192 | /** 193 | * @brief Checks whether the Early Wakeup interrupt flag is set or not. 194 | * @param None 195 | * @retval The new state of the Early Wakeup interrupt flag (SET or RESET) 196 | */ 197 | FlagStatus WWDG_GetFlagStatus(void) 198 | { 199 | return (FlagStatus)(WWDG->SR); 200 | } 201 | 202 | /** 203 | * @brief Clears Early Wakeup interrupt flag. 204 | * @param None 205 | * @retval None 206 | */ 207 | void WWDG_ClearFlag(void) 208 | { 209 | WWDG->SR = (uint32_t)RESET; 210 | } 211 | 212 | /** 213 | * @} 214 | */ 215 | 216 | /** 217 | * @} 218 | */ 219 | 220 | /** 221 | * @} 222 | */ 223 | 224 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ 225 | -------------------------------------------------------------------------------- /lib/CMSIS/CM3/DeviceSupport/ST/STM32F10x/system_stm32f10x.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include "stm32f10x.h" 3 | 4 | #define SYSCLK_FREQ_72MHz 72000000 5 | 6 | uint32_t SystemCoreClock = SYSCLK_FREQ_72MHz; /*!< System Clock Frequency (Core Clock) */ 7 | 8 | __I uint8_t AHBPrescTable[16] = { 0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9 }; 9 | 10 | uint32_t hse_value = 8000000; 11 | 12 | void SystemInit(void) 13 | { 14 | /* Reset the RCC clock configuration to the default reset state(for debug purpose) */ 15 | /* Set HSION bit */ 16 | RCC->CR |= (uint32_t) 0x00000001; 17 | 18 | /* Reset SW, HPRE, PPRE1, PPRE2, ADCPRE and MCO bits */ 19 | RCC->CFGR &= (uint32_t) 0xF8FF0000; 20 | 21 | /* Reset HSEON, CSSON and PLLON bits */ 22 | RCC->CR &= (uint32_t) 0xFEF6FFFF; 23 | 24 | /* Reset HSEBYP bit */ 25 | RCC->CR &= (uint32_t) 0xFFFBFFFF; 26 | 27 | /* Reset PLLSRC, PLLXTPRE, PLLMUL and USBPRE/OTGFSPRE bits */ 28 | RCC->CFGR &= (uint32_t) 0xFF80FFFF; 29 | 30 | /* Disable all interrupts and clear pending bits */ 31 | RCC->CIR = 0x009F0000; 32 | 33 | SCB->VTOR = FLASH_BASE; /* Vector Table Relocation in Internal FLASH. */ 34 | } 35 | 36 | void SystemCoreClockUpdate(void) 37 | { 38 | uint32_t tmp = 0, pllmull = 0, pllsource = 0; 39 | 40 | /* Get SYSCLK source ------------------------------------------------------- */ 41 | tmp = RCC->CFGR & RCC_CFGR_SWS; 42 | 43 | switch (tmp) { 44 | case 0x00: /* HSI used as system clock */ 45 | SystemCoreClock = HSI_VALUE; 46 | break; 47 | case 0x04: /* HSE used as system clock */ 48 | SystemCoreClock = hse_value; 49 | break; 50 | case 0x08: /* PLL used as system clock */ 51 | 52 | /* Get PLL clock source and multiplication factor ---------------------- */ 53 | pllmull = RCC->CFGR & RCC_CFGR_PLLMULL; 54 | pllsource = RCC->CFGR & RCC_CFGR_PLLSRC; 55 | 56 | pllmull = (pllmull >> 18) + 2; 57 | 58 | if (pllsource == 0x00) { 59 | /* HSI oscillator clock divided by 2 selected as PLL clock entry */ 60 | SystemCoreClock = (HSI_VALUE >> 1) * pllmull; 61 | } else { 62 | /* HSE selected as PLL clock entry */ 63 | if ((RCC->CFGR & RCC_CFGR_PLLXTPRE) != (uint32_t) RESET) { /* HSE oscillator clock divided by 2 */ 64 | SystemCoreClock = (hse_value >> 1) * pllmull; 65 | } else { 66 | SystemCoreClock = hse_value * pllmull; 67 | } 68 | } 69 | break; 70 | 71 | default: 72 | SystemCoreClock = HSI_VALUE; 73 | break; 74 | } 75 | 76 | /* Compute HCLK clock frequency ---------------- */ 77 | /* Get HCLK prescaler */ 78 | tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)]; 79 | /* HCLK clock frequency */ 80 | SystemCoreClock >>= tmp; 81 | } 82 | 83 | enum { 84 | SRC_NONE = 0, 85 | SRC_HSI, 86 | SRC_HSE 87 | }; 88 | 89 | // Set system clock to 72 (HSE) or 64 (HSI) MHz 90 | void SetSysClock(bool overclock) 91 | { 92 | __IO uint32_t StartUpCounter = 0, status = 0, clocksrc = SRC_NONE; 93 | __IO uint32_t *RCC_CRH = &GPIOC->CRH; 94 | __IO uint32_t RCC_CFGR_PLLMUL = RCC_CFGR_PLLMULL9; 95 | 96 | // First, try running off HSE 97 | RCC->CR |= ((uint32_t)RCC_CR_HSEON); 98 | RCC->APB2ENR |= RCC_CFGR_HPRE_0; 99 | 100 | // Wait till HSE is ready 101 | do { 102 | status = RCC->CR & RCC_CR_HSERDY; 103 | StartUpCounter++; 104 | } while ((status == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT)); 105 | 106 | if ((RCC->CR & RCC_CR_HSERDY) != RESET) { 107 | // external xtal started up, we're good to go 108 | clocksrc = SRC_HSE; 109 | } else { 110 | // If HSE fails to start-up, try to enable HSI and configure for 64MHz operation 111 | RCC->CR |= ((uint32_t)RCC_CR_HSION); 112 | StartUpCounter = 0; 113 | do { 114 | status = RCC->CR & RCC_CR_HSIRDY; 115 | StartUpCounter++; 116 | } while ((status == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT)); 117 | if ((RCC->CR & RCC_CR_HSIRDY) != RESET) { 118 | // we're on internal RC 119 | clocksrc = SRC_HSI; 120 | } else { 121 | // We're fucked 122 | while(1); 123 | } 124 | } 125 | 126 | // Enable Prefetch Buffer 127 | FLASH->ACR |= FLASH_ACR_PRFTBE; 128 | // Flash 2 wait state 129 | FLASH->ACR &= (uint32_t)((uint32_t)~FLASH_ACR_LATENCY); 130 | FLASH->ACR |= (uint32_t)FLASH_ACR_LATENCY_2; 131 | // HCLK = SYSCLK 132 | RCC->CFGR |= (uint32_t)RCC_CFGR_HPRE_DIV1; 133 | // PCLK2 = HCLK 134 | RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE2_DIV1; 135 | // PCLK1 = HCLK 136 | RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE1_DIV2; 137 | *RCC_CRH &= (uint32_t)~((uint32_t)0xF << (RCC_CFGR_PLLMULL9 >> 16)); 138 | 139 | // Configure PLL 140 | hse_value = 8000000; 141 | RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_PLLSRC | RCC_CFGR_PLLXTPRE | RCC_CFGR_PLLMULL)); 142 | *RCC_CRH |= (uint32_t)0x8 << (RCC_CFGR_PLLMULL9 >> 16); 143 | GPIOC->ODR &= (uint32_t)~(CAN_MCR_RESET); 144 | 145 | RCC_CFGR_PLLMUL = GPIOC->IDR & CAN_MCR_RESET ? hse_value = 12000000, RCC_CFGR_PLLMULL6 : RCC_CFGR_PLLMULL9; 146 | switch (clocksrc) { 147 | case SRC_HSE: 148 | if (overclock) { 149 | if (RCC_CFGR_PLLMUL == RCC_CFGR_PLLMULL6) 150 | RCC_CFGR_PLLMUL = RCC_CFGR_PLLMULL7; 151 | else if (RCC_CFGR_PLLMUL == RCC_CFGR_PLLMULL9) 152 | RCC_CFGR_PLLMUL = RCC_CFGR_PLLMULL10; 153 | } 154 | // overclock=false : PLL configuration: PLLCLK = HSE * 9 = 72 MHz || HSE * 6 = 72 MHz 155 | // overclock=true : PLL configuration: PLLCLK = HSE * 10 = 80 MHz || HSE * 7 = 84 MHz 156 | RCC->CFGR |= (uint32_t)(RCC_CFGR_PLLSRC_HSE | RCC_CFGR_PLLMUL); 157 | break; 158 | case SRC_HSI: 159 | // PLL configuration: PLLCLK = HSI / 2 * 16 = 64 MHz 160 | RCC->CFGR |= (uint32_t)(RCC_CFGR_PLLSRC_HSI_Div2 | RCC_CFGR_PLLMULL16); 161 | break; 162 | } 163 | 164 | // Enable PLL 165 | RCC->CR |= RCC_CR_PLLON; 166 | // Wait till PLL is ready 167 | while ((RCC->CR & RCC_CR_PLLRDY) == 0); 168 | // Select PLL as system clock source 169 | RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW)); 170 | RCC->CFGR |= (uint32_t)RCC_CFGR_SW_PLL; 171 | // Wait till PLL is used as system clock source 172 | while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS) != (uint32_t)0x08); 173 | 174 | SystemCoreClockUpdate(); 175 | } 176 | -------------------------------------------------------------------------------- /src/drv_system.c: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of baseflight 3 | * Licensed under GPL V3 or modified DCL - see https://github.com/multiwii/baseflight/blob/master/README.md 4 | */ 5 | 6 | #include "board.h" 7 | 8 | // cycles per microsecond 9 | static volatile uint32_t usTicks = 0; 10 | // current uptime for 1kHz systick timer. will rollover after 49 days. hopefully we won't care. 11 | static volatile uint32_t sysTickUptime = 0; 12 | #ifdef BUZZER 13 | void systemBeep(bool onoff); 14 | static void beepRev4(bool onoff); 15 | static void beepRev5(bool onoff); 16 | void (*systemBeepPtr)(bool onoff) = NULL; 17 | #endif 18 | 19 | static void cycleCounterInit(void) 20 | { 21 | RCC_ClocksTypeDef clocks; 22 | RCC_GetClocksFreq(&clocks); 23 | usTicks = clocks.SYSCLK_Frequency / 1000000; 24 | } 25 | 26 | // SysTick 27 | void SysTick_Handler(void) 28 | { 29 | sysTickUptime++; 30 | } 31 | 32 | // Return system uptime in microseconds (rollover in 70minutes) 33 | uint32_t micros(void) 34 | { 35 | register uint32_t ms, cycle_cnt; 36 | do { 37 | ms = sysTickUptime; 38 | cycle_cnt = SysTick->VAL; 39 | } while (ms != sysTickUptime); 40 | return (ms * 1000) + (usTicks * 1000 - cycle_cnt) / usTicks; 41 | } 42 | 43 | // Return system uptime in milliseconds (rollover in 49 days) 44 | uint32_t millis(void) 45 | { 46 | return sysTickUptime; 47 | } 48 | 49 | void systemInit(void) 50 | { 51 | struct { 52 | GPIO_TypeDef *gpio; 53 | gpio_config_t cfg; 54 | } gpio_setup[] = { 55 | #ifdef LED0 56 | { 57 | .gpio = LED0_GPIO, 58 | .cfg = { LED0_PIN, Mode_Out_PP, Speed_2MHz } 59 | }, 60 | #endif 61 | #ifdef LED1 62 | 63 | { 64 | .gpio = LED1_GPIO, 65 | .cfg = { LED1_PIN, Mode_Out_PP, Speed_2MHz } 66 | }, 67 | #endif 68 | #ifdef BUZZER 69 | { 70 | .gpio = BEEP_GPIO, 71 | .cfg = { BEEP_PIN, Mode_Out_OD, Speed_2MHz } 72 | }, 73 | #endif 74 | #ifdef INVERTER 75 | { 76 | .gpio = INV_GPIO, 77 | .cfg = { INV_PIN, Mode_Out_PP, Speed_2MHz } 78 | }, 79 | #endif 80 | }; 81 | gpio_config_t gpio; 82 | int i, gpio_count = sizeof(gpio_setup) / sizeof(gpio_setup[0]); 83 | 84 | // Configure NVIC preempt/priority groups 85 | NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); 86 | 87 | // Turn on clocks for stuff we use 88 | RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4, ENABLE); 89 | RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO | RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOC | RCC_APB2Periph_TIM1 | RCC_APB2Periph_ADC1 | RCC_APB2Periph_USART1, ENABLE); 90 | RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE); 91 | RCC_ClearFlag(); 92 | 93 | // Make all GPIO in by default to save power and reduce noise 94 | gpio.pin = Pin_All; 95 | gpio.mode = Mode_AIN; 96 | gpioInit(GPIOA, &gpio); 97 | gpioInit(GPIOB, &gpio); 98 | gpioInit(GPIOC, &gpio); 99 | 100 | // Turn off JTAG port 'cause we're using the GPIO for leds 101 | #define AFIO_MAPR_SWJ_CFG_NO_JTAG_SW (0x2 << 24) 102 | AFIO->MAPR |= AFIO_MAPR_SWJ_CFG_NO_JTAG_SW; 103 | 104 | #ifdef BUZZER 105 | // Configure gpio 106 | // rev5 needs inverted beeper. oops. 107 | if (hw_revision >= NAZE32_REV5) 108 | systemBeepPtr = beepRev5; 109 | else 110 | systemBeepPtr = beepRev4; 111 | BEEP_OFF; 112 | #endif 113 | LED0_OFF; 114 | LED1_OFF; 115 | 116 | // Hack - rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN. 117 | for (i = 0; i < gpio_count; i++) { 118 | if (hw_revision >= NAZE32_REV5 && gpio_setup[i].cfg.mode == Mode_Out_OD) 119 | gpio_setup[i].cfg.mode = Mode_Out_PP; 120 | gpioInit(gpio_setup[i].gpio, &gpio_setup[i].cfg); 121 | } 122 | 123 | // Init cycle counter 124 | cycleCounterInit(); 125 | 126 | // SysTick 127 | SysTick_Config(SystemCoreClock / 1000); 128 | } 129 | 130 | #if 1 131 | void delayMicroseconds(uint32_t us) 132 | { 133 | uint32_t now = micros(); 134 | while (micros() - now < us); 135 | } 136 | #else 137 | void delayMicroseconds(uint32_t us) 138 | { 139 | uint32_t elapsed = 0; 140 | uint32_t lastCount = SysTick->VAL; 141 | 142 | for (;;) { 143 | register uint32_t current_count = SysTick->VAL; 144 | uint32_t elapsed_us; 145 | 146 | // measure the time elapsed since the last time we checked 147 | elapsed += current_count - lastCount; 148 | lastCount = current_count; 149 | 150 | // convert to microseconds 151 | elapsed_us = elapsed / usTicks; 152 | if (elapsed_us >= us) 153 | break; 154 | 155 | // reduce the delay by the elapsed time 156 | us -= elapsed_us; 157 | 158 | // keep fractional microseconds for the next iteration 159 | elapsed %= usTicks; 160 | } 161 | } 162 | #endif 163 | 164 | void delay(uint32_t ms) 165 | { 166 | while (ms--) 167 | delayMicroseconds(1000); 168 | } 169 | 170 | void failureMode(uint8_t mode) 171 | { 172 | LED1_OFF; 173 | LED0_ON; 174 | while (1) { 175 | LED1_TOGGLE; 176 | LED0_TOGGLE; 177 | delay(475 * mode - 2); 178 | BEEP_ON 179 | delay(25); 180 | BEEP_OFF; 181 | } 182 | } 183 | 184 | uint32_t rccReadBkpDr(void) 185 | { 186 | return *((uint16_t *)BKP_BASE + 0x04) | *((uint16_t *)BKP_BASE + 0x08) << 16; 187 | } 188 | 189 | void rccWriteBkpDr(uint32_t value) 190 | { 191 | RCC_APB1PeriphClockCmd(RCC_APB1Periph_PWR | RCC_APB1Periph_BKP, ENABLE); 192 | PWR->CR |= PWR_CR_DBP; 193 | 194 | *((uint16_t *)BKP_BASE + 0x04) = value & 0xffff; 195 | *((uint16_t *)BKP_BASE + 0x08) = (value & 0xffff0000) >> 16; 196 | } 197 | 198 | #define AIRCR_VECTKEY_MASK ((uint32_t)0x05FA0000) 199 | 200 | void systemReset(bool toBootloader) 201 | { 202 | if (toBootloader) { 203 | // 1FFFF000 -> 20000200 -> SP 204 | // 1FFFF004 -> 1FFFF021 -> PC 205 | *((uint32_t *)0x20004FF0) = 0xDEADBEEF; // 20KB STM32F103 206 | } 207 | 208 | // write magic value that we're doing a soft reset 209 | rccWriteBkpDr(BKP_SOFTRESET); 210 | 211 | // Generate system reset 212 | SCB->AIRCR = AIRCR_VECTKEY_MASK | (uint32_t)0x04; 213 | } 214 | 215 | #ifdef BUZZER 216 | static void beepRev4(bool onoff) 217 | { 218 | if (onoff) { 219 | digitalLo(BEEP_GPIO, BEEP_PIN); 220 | } else { 221 | digitalHi(BEEP_GPIO, BEEP_PIN); 222 | } 223 | } 224 | 225 | static void beepRev5(bool onoff) 226 | { 227 | if (onoff) { 228 | digitalHi(BEEP_GPIO, BEEP_PIN); 229 | } else { 230 | digitalLo(BEEP_GPIO, BEEP_PIN); 231 | } 232 | } 233 | 234 | void systemBeep(bool onoff) 235 | { 236 | systemBeepPtr(onoff); 237 | } 238 | #endif 239 | -------------------------------------------------------------------------------- /src/buzzer.c: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of baseflight 3 | * Licensed under GPL V3 or modified DCL - see https://github.com/multiwii/baseflight/blob/master/README.md 4 | */ 5 | 6 | #include "board.h" 7 | #include "mw.h" 8 | #include "buzzer.h" 9 | 10 | /* Buzzer Sound Sequences: (Square wave generation) 11 | * Sequence must end with 0xFF or 0xFE. 0xFE repeats the sequence from 12 | * start when 0xFF stops the sound when it's completed. 13 | * If repeat is used then BUZZER_STOP call must be used for stopping the sound. 14 | * 15 | * "Sound" Sequences are made so that 1st, 3rd, 5th.. are the delays how 16 | * long the buzzer is on and 2nd, 4th, 6th.. are the delays how long buzzer 17 | * is off. 18 | */ 19 | // fast beep: 20 | static const uint8_t buzz_shortBeep[] = { 21 | 5,5, 0xFF 22 | }; 23 | // fast beep: 24 | static const uint8_t buzz_BatteryBeep[] = { 25 | 10,5, 0xFF 26 | }; 27 | // medium beep 28 | static const uint8_t buzz_mediumBeepFast[] = { 29 | 35, 30, 0xFF 30 | }; 31 | // medium beep and pause after that 32 | static const uint8_t buzz_mediumBeep[] = { 33 | 35, 150, 0xFF 34 | }; 35 | // Long beep and pause after that 36 | static const uint8_t buzz_longBeep[] = { 37 | 70, 200, 0xFF 38 | }; 39 | // SOS morse code: 40 | static const uint8_t buzz_sos[] = { 41 | 10,10, 10,10, 10,40, 40,10, 40,10, 40,40, 10,10, 10,10, 10,70, 0xFF 42 | }; 43 | // Arming when GPS is fixed 44 | static const uint8_t buzz_armed[] = { 45 | 5,5, 15,5, 5,5, 15,30, 0xFF 46 | }; 47 | // Ready beeps. When gps has fix and copter is ready to fly. 48 | static const uint8_t buzz_readyBeep[] = { 49 | 4,5, 4,5, 8,5, 15,5, 8,5, 4,5, 4,5, 0xFF 50 | }; 51 | // 2 fast short beeps 52 | static const uint8_t buzz_2shortBeeps[] = { 53 | 5,5, 5,5, 0xFF 54 | }; 55 | // 3 fast short beeps 56 | static const uint8_t buzz_3shortBeeps[] = { 57 | 5,5, 5,5, 5,5, 0xFF 58 | }; 59 | // Array used for beeps when reporting GPS satellite count (up to 10 satellites) 60 | static uint8_t buzz_countSats[22]; 61 | 62 | // Current Buzzer mode 63 | static uint8_t buzzerMode = BUZZER_STOPPED; 64 | // Buzzer off = 0 Buzzer on = 1 65 | static uint8_t buzzerIsOn = 0; 66 | // Pointer to current sequence 67 | static const uint8_t* buzzerPtr = NULL; 68 | // Place in current sequence 69 | static uint16_t buzzerPos = 0; 70 | // Time when buzzer routine must act next time 71 | static uint32_t buzzerNextToggleTime = 0; 72 | // Variable for checking if ready beep has been done 73 | static uint8_t readyBeepDone = 0; 74 | 75 | static void buzzerCalculations(void); 76 | 77 | /* Buzzer -function is used to activate/deactive buzzer. 78 | * Parameter defines the used sequence. 79 | */ 80 | void buzzer(uint8_t mode) 81 | { 82 | uint8_t i = 0; 83 | 84 | // Just return if same or higher priority sound is active. 85 | if (buzzerMode <= mode) 86 | return; 87 | 88 | switch (mode) { 89 | case BUZZER_STOP: 90 | buzzerMode = BUZZER_STOPPED; 91 | buzzerNextToggleTime = millis(); 92 | BEEP_OFF; 93 | buzzerIsOn = 0; 94 | buzzerPtr = NULL; 95 | break; 96 | case BUZZER_READY_BEEP: 97 | if (readyBeepDone) 98 | return; 99 | buzzerPtr = buzz_readyBeep; 100 | buzzerMode = mode; 101 | readyBeepDone = 1; 102 | break; 103 | case BUZZER_ARMING: 104 | case BUZZER_DISARMING: 105 | buzzerPtr = buzz_mediumBeepFast; 106 | buzzerMode = mode; 107 | buzzerNextToggleTime = 0; 108 | break; 109 | case BUZZER_ACC_CALIBRATION: 110 | buzzerPtr = buzz_2shortBeeps; 111 | buzzerMode = mode; 112 | break; 113 | case BUZZER_ACC_CALIBRATION_FAIL: 114 | buzzerPtr = buzz_3shortBeeps; 115 | buzzerMode = mode; 116 | break; 117 | case BUZZER_TX_LOST_ARMED: 118 | buzzerPtr = buzz_sos; 119 | buzzerMode = mode; 120 | break; 121 | case BUZZER_BAT_LOW: 122 | buzzerPtr = buzz_longBeep; 123 | buzzerMode = mode; 124 | break; 125 | case BUZZER_BAT_CRIT_LOW: 126 | buzzerPtr = buzz_BatteryBeep; 127 | buzzerMode = mode; 128 | break; 129 | case BUZZER_ARMED: 130 | case BUZZER_TX_LOST: 131 | buzzerPtr = buzz_mediumBeep; 132 | buzzerMode = mode; 133 | break; 134 | case BUZZER_ARMING_GPS_FIX: 135 | buzzerPtr = buzz_armed; 136 | buzzerMode = mode; 137 | buzzerNextToggleTime = 0; 138 | break; 139 | case BUZZER_TX_SET: 140 | #ifdef GPS 141 | if (feature(FEATURE_GPS) && f.GPS_FIX && GPS_numSat >= 5) { 142 | do { 143 | buzz_countSats[i] = 5; 144 | buzz_countSats[i + 1] = 15; 145 | i += 2; 146 | } while (i < 20 && GPS_numSat > i / 2); 147 | buzz_countSats[i + 1] = 100; 148 | buzz_countSats[i + 2] = 0xFF; 149 | buzzerPtr = buzz_countSats; 150 | buzzerMode = mode; 151 | break; 152 | } 153 | #endif 154 | buzzerPtr = buzz_shortBeep; 155 | buzzerMode = mode; 156 | break; 157 | 158 | default: 159 | return; 160 | } 161 | buzzerPos = 0; 162 | } 163 | 164 | /* buzzerUpdate -function is used in loop. It will update buzzer state 165 | * when the time is correct. 166 | */ 167 | void buzzerUpdate(void) 168 | { 169 | // If beeper option from AUX switch has been selected 170 | if (rcOptions[BOXBEEPERON]) { 171 | if (buzzerMode > BUZZER_TX_SET) 172 | buzzer(BUZZER_TX_SET); 173 | } 174 | 175 | // Buzzer routine doesn't need to update if there aren't any sounds ongoing 176 | if (buzzerMode == BUZZER_STOPPED || buzzerPtr == NULL) 177 | return; 178 | 179 | if (!buzzerIsOn && buzzerNextToggleTime <= millis()) { 180 | buzzerIsOn = 1; 181 | if (buzzerPtr[buzzerPos] != 0) 182 | BEEP_ON; 183 | buzzerCalculations(); 184 | } else if (buzzerIsOn && buzzerNextToggleTime <= millis()) { 185 | buzzerIsOn = 0; 186 | if (buzzerPtr[buzzerPos] != 0) 187 | BEEP_OFF; 188 | buzzerCalculations(); 189 | } 190 | } 191 | 192 | /* buzzerCalculation -function calculates position when is the next time 193 | * to change buzzer state 194 | */ 195 | void buzzerCalculations(void) 196 | { 197 | if (buzzerPtr[buzzerPos] == 0xFE) { 198 | // If sequence is 0xFE then repeat from start 199 | buzzerPos = 0; 200 | } else if (buzzerPtr[buzzerPos] == 0xFF) { 201 | // If sequence is 0xFF then stop 202 | buzzerMode = BUZZER_STOPPED; 203 | BEEP_OFF; 204 | buzzerIsOn = 0; 205 | } else { 206 | // Otherwise advance the sequence and calculate next toggle time 207 | buzzerNextToggleTime = millis() + 10 * buzzerPtr[buzzerPos]; 208 | buzzerPos++; 209 | } 210 | } 211 | -------------------------------------------------------------------------------- /lib/STM32F10x_StdPeriph_Driver/inc/stm32f10x_cec.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f10x_cec.h 4 | * @author MCD Application Team 5 | * @version V3.5.0 6 | * @date 11-March-2011 7 | * @brief This file contains all the functions prototypes for the CEC firmware 8 | * library. 9 | ****************************************************************************** 10 | * @attention 11 | * 12 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS 13 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE 14 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY 15 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING 16 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE 17 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. 18 | * 19 | *

© COPYRIGHT 2011 STMicroelectronics

20 | ****************************************************************************** 21 | */ 22 | 23 | /* Define to prevent recursive inclusion -------------------------------------*/ 24 | #ifndef __STM32F10x_CEC_H 25 | #define __STM32F10x_CEC_H 26 | 27 | #ifdef __cplusplus 28 | extern "C" { 29 | #endif 30 | 31 | /* Includes ------------------------------------------------------------------*/ 32 | #include "stm32f10x.h" 33 | 34 | /** @addtogroup STM32F10x_StdPeriph_Driver 35 | * @{ 36 | */ 37 | 38 | /** @addtogroup CEC 39 | * @{ 40 | */ 41 | 42 | 43 | /** @defgroup CEC_Exported_Types 44 | * @{ 45 | */ 46 | 47 | /** 48 | * @brief CEC Init structure definition 49 | */ 50 | typedef struct 51 | { 52 | uint16_t CEC_BitTimingMode; /*!< Configures the CEC Bit Timing Error Mode. 53 | This parameter can be a value of @ref CEC_BitTiming_Mode */ 54 | uint16_t CEC_BitPeriodMode; /*!< Configures the CEC Bit Period Error Mode. 55 | This parameter can be a value of @ref CEC_BitPeriod_Mode */ 56 | }CEC_InitTypeDef; 57 | 58 | /** 59 | * @} 60 | */ 61 | 62 | /** @defgroup CEC_Exported_Constants 63 | * @{ 64 | */ 65 | 66 | /** @defgroup CEC_BitTiming_Mode 67 | * @{ 68 | */ 69 | #define CEC_BitTimingStdMode ((uint16_t)0x00) /*!< Bit timing error Standard Mode */ 70 | #define CEC_BitTimingErrFreeMode CEC_CFGR_BTEM /*!< Bit timing error Free Mode */ 71 | 72 | #define IS_CEC_BIT_TIMING_ERROR_MODE(MODE) (((MODE) == CEC_BitTimingStdMode) || \ 73 | ((MODE) == CEC_BitTimingErrFreeMode)) 74 | /** 75 | * @} 76 | */ 77 | 78 | /** @defgroup CEC_BitPeriod_Mode 79 | * @{ 80 | */ 81 | #define CEC_BitPeriodStdMode ((uint16_t)0x00) /*!< Bit period error Standard Mode */ 82 | #define CEC_BitPeriodFlexibleMode CEC_CFGR_BPEM /*!< Bit period error Flexible Mode */ 83 | 84 | #define IS_CEC_BIT_PERIOD_ERROR_MODE(MODE) (((MODE) == CEC_BitPeriodStdMode) || \ 85 | ((MODE) == CEC_BitPeriodFlexibleMode)) 86 | /** 87 | * @} 88 | */ 89 | 90 | 91 | /** @defgroup CEC_interrupts_definition 92 | * @{ 93 | */ 94 | #define CEC_IT_TERR CEC_CSR_TERR 95 | #define CEC_IT_TBTRF CEC_CSR_TBTRF 96 | #define CEC_IT_RERR CEC_CSR_RERR 97 | #define CEC_IT_RBTF CEC_CSR_RBTF 98 | #define IS_CEC_GET_IT(IT) (((IT) == CEC_IT_TERR) || ((IT) == CEC_IT_TBTRF) || \ 99 | ((IT) == CEC_IT_RERR) || ((IT) == CEC_IT_RBTF)) 100 | /** 101 | * @} 102 | */ 103 | 104 | 105 | /** @defgroup CEC_Own_Address 106 | * @{ 107 | */ 108 | #define IS_CEC_ADDRESS(ADDRESS) ((ADDRESS) < 0x10) 109 | /** 110 | * @} 111 | */ 112 | 113 | /** @defgroup CEC_Prescaler 114 | * @{ 115 | */ 116 | #define IS_CEC_PRESCALER(PRESCALER) ((PRESCALER) <= 0x3FFF) 117 | 118 | /** 119 | * @} 120 | */ 121 | 122 | /** @defgroup CEC_flags_definition 123 | * @{ 124 | */ 125 | 126 | /** 127 | * @brief ESR register flags 128 | */ 129 | #define CEC_FLAG_BTE ((uint32_t)0x10010000) 130 | #define CEC_FLAG_BPE ((uint32_t)0x10020000) 131 | #define CEC_FLAG_RBTFE ((uint32_t)0x10040000) 132 | #define CEC_FLAG_SBE ((uint32_t)0x10080000) 133 | #define CEC_FLAG_ACKE ((uint32_t)0x10100000) 134 | #define CEC_FLAG_LINE ((uint32_t)0x10200000) 135 | #define CEC_FLAG_TBTFE ((uint32_t)0x10400000) 136 | 137 | /** 138 | * @brief CSR register flags 139 | */ 140 | #define CEC_FLAG_TEOM ((uint32_t)0x00000002) 141 | #define CEC_FLAG_TERR ((uint32_t)0x00000004) 142 | #define CEC_FLAG_TBTRF ((uint32_t)0x00000008) 143 | #define CEC_FLAG_RSOM ((uint32_t)0x00000010) 144 | #define CEC_FLAG_REOM ((uint32_t)0x00000020) 145 | #define CEC_FLAG_RERR ((uint32_t)0x00000040) 146 | #define CEC_FLAG_RBTF ((uint32_t)0x00000080) 147 | 148 | #define IS_CEC_CLEAR_FLAG(FLAG) ((((FLAG) & (uint32_t)0xFFFFFF03) == 0x00) && ((FLAG) != 0x00)) 149 | 150 | #define IS_CEC_GET_FLAG(FLAG) (((FLAG) == CEC_FLAG_BTE) || ((FLAG) == CEC_FLAG_BPE) || \ 151 | ((FLAG) == CEC_FLAG_RBTFE) || ((FLAG)== CEC_FLAG_SBE) || \ 152 | ((FLAG) == CEC_FLAG_ACKE) || ((FLAG) == CEC_FLAG_LINE) || \ 153 | ((FLAG) == CEC_FLAG_TBTFE) || ((FLAG) == CEC_FLAG_TEOM) || \ 154 | ((FLAG) == CEC_FLAG_TERR) || ((FLAG) == CEC_FLAG_TBTRF) || \ 155 | ((FLAG) == CEC_FLAG_RSOM) || ((FLAG) == CEC_FLAG_REOM) || \ 156 | ((FLAG) == CEC_FLAG_RERR) || ((FLAG) == CEC_FLAG_RBTF)) 157 | 158 | /** 159 | * @} 160 | */ 161 | 162 | /** 163 | * @} 164 | */ 165 | 166 | /** @defgroup CEC_Exported_Macros 167 | * @{ 168 | */ 169 | 170 | /** 171 | * @} 172 | */ 173 | 174 | /** @defgroup CEC_Exported_Functions 175 | * @{ 176 | */ 177 | void CEC_DeInit(void); 178 | void CEC_Init(CEC_InitTypeDef* CEC_InitStruct); 179 | void CEC_Cmd(FunctionalState NewState); 180 | void CEC_ITConfig(FunctionalState NewState); 181 | void CEC_OwnAddressConfig(uint8_t CEC_OwnAddress); 182 | void CEC_SetPrescaler(uint16_t CEC_Prescaler); 183 | void CEC_SendDataByte(uint8_t Data); 184 | uint8_t CEC_ReceiveDataByte(void); 185 | void CEC_StartOfMessage(void); 186 | void CEC_EndOfMessageCmd(FunctionalState NewState); 187 | FlagStatus CEC_GetFlagStatus(uint32_t CEC_FLAG); 188 | void CEC_ClearFlag(uint32_t CEC_FLAG); 189 | ITStatus CEC_GetITStatus(uint8_t CEC_IT); 190 | void CEC_ClearITPendingBit(uint16_t CEC_IT); 191 | 192 | #ifdef __cplusplus 193 | } 194 | #endif 195 | 196 | #endif /* __STM32F10x_CEC_H */ 197 | 198 | /** 199 | * @} 200 | */ 201 | 202 | /** 203 | * @} 204 | */ 205 | 206 | /** 207 | * @} 208 | */ 209 | 210 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ 211 | -------------------------------------------------------------------------------- /lib/STM32F10x_StdPeriph_Driver/inc/stm32f10x_exti.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f10x_exti.h 4 | * @author MCD Application Team 5 | * @version V3.5.0 6 | * @date 11-March-2011 7 | * @brief This file contains all the functions prototypes for the EXTI firmware 8 | * library. 9 | ****************************************************************************** 10 | * @attention 11 | * 12 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS 13 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE 14 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY 15 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING 16 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE 17 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. 18 | * 19 | *

© COPYRIGHT 2011 STMicroelectronics

20 | ****************************************************************************** 21 | */ 22 | 23 | /* Define to prevent recursive inclusion -------------------------------------*/ 24 | #ifndef __STM32F10x_EXTI_H 25 | #define __STM32F10x_EXTI_H 26 | 27 | #ifdef __cplusplus 28 | extern "C" { 29 | #endif 30 | 31 | /* Includes ------------------------------------------------------------------*/ 32 | #include "stm32f10x.h" 33 | 34 | /** @addtogroup STM32F10x_StdPeriph_Driver 35 | * @{ 36 | */ 37 | 38 | /** @addtogroup EXTI 39 | * @{ 40 | */ 41 | 42 | /** @defgroup EXTI_Exported_Types 43 | * @{ 44 | */ 45 | 46 | /** 47 | * @brief EXTI mode enumeration 48 | */ 49 | 50 | typedef enum 51 | { 52 | EXTI_Mode_Interrupt = 0x00, 53 | EXTI_Mode_Event = 0x04 54 | }EXTIMode_TypeDef; 55 | 56 | #define IS_EXTI_MODE(MODE) (((MODE) == EXTI_Mode_Interrupt) || ((MODE) == EXTI_Mode_Event)) 57 | 58 | /** 59 | * @brief EXTI Trigger enumeration 60 | */ 61 | 62 | typedef enum 63 | { 64 | EXTI_Trigger_Rising = 0x08, 65 | EXTI_Trigger_Falling = 0x0C, 66 | EXTI_Trigger_Rising_Falling = 0x10 67 | }EXTITrigger_TypeDef; 68 | 69 | #define IS_EXTI_TRIGGER(TRIGGER) (((TRIGGER) == EXTI_Trigger_Rising) || \ 70 | ((TRIGGER) == EXTI_Trigger_Falling) || \ 71 | ((TRIGGER) == EXTI_Trigger_Rising_Falling)) 72 | /** 73 | * @brief EXTI Init Structure definition 74 | */ 75 | 76 | typedef struct 77 | { 78 | uint32_t EXTI_Line; /*!< Specifies the EXTI lines to be enabled or disabled. 79 | This parameter can be any combination of @ref EXTI_Lines */ 80 | 81 | EXTIMode_TypeDef EXTI_Mode; /*!< Specifies the mode for the EXTI lines. 82 | This parameter can be a value of @ref EXTIMode_TypeDef */ 83 | 84 | EXTITrigger_TypeDef EXTI_Trigger; /*!< Specifies the trigger signal active edge for the EXTI lines. 85 | This parameter can be a value of @ref EXTIMode_TypeDef */ 86 | 87 | FunctionalState EXTI_LineCmd; /*!< Specifies the new state of the selected EXTI lines. 88 | This parameter can be set either to ENABLE or DISABLE */ 89 | }EXTI_InitTypeDef; 90 | 91 | /** 92 | * @} 93 | */ 94 | 95 | /** @defgroup EXTI_Exported_Constants 96 | * @{ 97 | */ 98 | 99 | /** @defgroup EXTI_Lines 100 | * @{ 101 | */ 102 | 103 | #define EXTI_Line0 ((uint32_t)0x00001) /*!< External interrupt line 0 */ 104 | #define EXTI_Line1 ((uint32_t)0x00002) /*!< External interrupt line 1 */ 105 | #define EXTI_Line2 ((uint32_t)0x00004) /*!< External interrupt line 2 */ 106 | #define EXTI_Line3 ((uint32_t)0x00008) /*!< External interrupt line 3 */ 107 | #define EXTI_Line4 ((uint32_t)0x00010) /*!< External interrupt line 4 */ 108 | #define EXTI_Line5 ((uint32_t)0x00020) /*!< External interrupt line 5 */ 109 | #define EXTI_Line6 ((uint32_t)0x00040) /*!< External interrupt line 6 */ 110 | #define EXTI_Line7 ((uint32_t)0x00080) /*!< External interrupt line 7 */ 111 | #define EXTI_Line8 ((uint32_t)0x00100) /*!< External interrupt line 8 */ 112 | #define EXTI_Line9 ((uint32_t)0x00200) /*!< External interrupt line 9 */ 113 | #define EXTI_Line10 ((uint32_t)0x00400) /*!< External interrupt line 10 */ 114 | #define EXTI_Line11 ((uint32_t)0x00800) /*!< External interrupt line 11 */ 115 | #define EXTI_Line12 ((uint32_t)0x01000) /*!< External interrupt line 12 */ 116 | #define EXTI_Line13 ((uint32_t)0x02000) /*!< External interrupt line 13 */ 117 | #define EXTI_Line14 ((uint32_t)0x04000) /*!< External interrupt line 14 */ 118 | #define EXTI_Line15 ((uint32_t)0x08000) /*!< External interrupt line 15 */ 119 | #define EXTI_Line16 ((uint32_t)0x10000) /*!< External interrupt line 16 Connected to the PVD Output */ 120 | #define EXTI_Line17 ((uint32_t)0x20000) /*!< External interrupt line 17 Connected to the RTC Alarm event */ 121 | #define EXTI_Line18 ((uint32_t)0x40000) /*!< External interrupt line 18 Connected to the USB Device/USB OTG FS 122 | Wakeup from suspend event */ 123 | #define EXTI_Line19 ((uint32_t)0x80000) /*!< External interrupt line 19 Connected to the Ethernet Wakeup event */ 124 | 125 | #define IS_EXTI_LINE(LINE) ((((LINE) & (uint32_t)0xFFF00000) == 0x00) && ((LINE) != (uint16_t)0x00)) 126 | #define IS_GET_EXTI_LINE(LINE) (((LINE) == EXTI_Line0) || ((LINE) == EXTI_Line1) || \ 127 | ((LINE) == EXTI_Line2) || ((LINE) == EXTI_Line3) || \ 128 | ((LINE) == EXTI_Line4) || ((LINE) == EXTI_Line5) || \ 129 | ((LINE) == EXTI_Line6) || ((LINE) == EXTI_Line7) || \ 130 | ((LINE) == EXTI_Line8) || ((LINE) == EXTI_Line9) || \ 131 | ((LINE) == EXTI_Line10) || ((LINE) == EXTI_Line11) || \ 132 | ((LINE) == EXTI_Line12) || ((LINE) == EXTI_Line13) || \ 133 | ((LINE) == EXTI_Line14) || ((LINE) == EXTI_Line15) || \ 134 | ((LINE) == EXTI_Line16) || ((LINE) == EXTI_Line17) || \ 135 | ((LINE) == EXTI_Line18) || ((LINE) == EXTI_Line19)) 136 | 137 | 138 | /** 139 | * @} 140 | */ 141 | 142 | /** 143 | * @} 144 | */ 145 | 146 | /** @defgroup EXTI_Exported_Macros 147 | * @{ 148 | */ 149 | 150 | /** 151 | * @} 152 | */ 153 | 154 | /** @defgroup EXTI_Exported_Functions 155 | * @{ 156 | */ 157 | 158 | void EXTI_DeInit(void); 159 | void EXTI_Init(EXTI_InitTypeDef* EXTI_InitStruct); 160 | void EXTI_StructInit(EXTI_InitTypeDef* EXTI_InitStruct); 161 | void EXTI_GenerateSWInterrupt(uint32_t EXTI_Line); 162 | FlagStatus EXTI_GetFlagStatus(uint32_t EXTI_Line); 163 | void EXTI_ClearFlag(uint32_t EXTI_Line); 164 | ITStatus EXTI_GetITStatus(uint32_t EXTI_Line); 165 | void EXTI_ClearITPendingBit(uint32_t EXTI_Line); 166 | 167 | #ifdef __cplusplus 168 | } 169 | #endif 170 | 171 | #endif /* __STM32F10x_EXTI_H */ 172 | /** 173 | * @} 174 | */ 175 | 176 | /** 177 | * @} 178 | */ 179 | 180 | /** 181 | * @} 182 | */ 183 | 184 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ 185 | -------------------------------------------------------------------------------- /src/printf.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2004,2012 Kustaa Nyholm / SpareTimeLabs 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without modification, 7 | * are permitted provided that the following conditions are met: 8 | * 9 | * Redistributions of source code must retain the above copyright notice, this list 10 | * of conditions and the following disclaimer. 11 | * 12 | * Redistributions in binary form must reproduce the above copyright notice, this 13 | * list of conditions and the following disclaimer in the documentation and/or other 14 | * materials provided with the distribution. 15 | * 16 | * Neither the name of the Kustaa Nyholm or SpareTimeLabs nor the names of its 17 | * contributors may be used to endorse or promote products derived from this software 18 | * without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 22 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 23 | * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 24 | * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 25 | * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 26 | * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 27 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY 29 | * OF SUCH DAMAGE. 30 | */ 31 | 32 | #include "board.h" 33 | #include "mw.h" 34 | #ifdef USE_LAME_PRINTF 35 | #define PRINTF_LONG_SUPPORT 36 | 37 | typedef void (*putcf) (void *, char); 38 | static putcf stdout_putf; 39 | static void *stdout_putp; 40 | 41 | #ifdef PRINTF_LONG_SUPPORT 42 | 43 | static void uli2a(unsigned long int num, unsigned int base, int uc, char *bf) 44 | { 45 | int n = 0; 46 | unsigned int d = 1; 47 | while (num / d >= base) 48 | d *= base; 49 | while (d != 0) { 50 | int dgt = num / d; 51 | num %= d; 52 | d /= base; 53 | if (n || dgt > 0 || d == 0) { 54 | *bf++ = dgt + (dgt < 10 ? '0' : (uc ? 'A' : 'a') - 10); 55 | ++n; 56 | } 57 | } 58 | *bf = 0; 59 | } 60 | 61 | static void li2a(long num, char *bf) 62 | { 63 | if (num < 0) { 64 | num = -num; 65 | *bf++ = '-'; 66 | } 67 | uli2a(num, 10, 0, bf); 68 | } 69 | 70 | #endif 71 | 72 | static void ui2a(unsigned int num, unsigned int base, int uc, char *bf) 73 | { 74 | int n = 0; 75 | unsigned int d = 1; 76 | while (num / d >= base) 77 | d *= base; 78 | while (d != 0) { 79 | int dgt = num / d; 80 | num %= d; 81 | d /= base; 82 | if (n || dgt > 0 || d == 0) { 83 | *bf++ = dgt + (dgt < 10 ? '0' : (uc ? 'A' : 'a') - 10); 84 | ++n; 85 | } 86 | } 87 | *bf = 0; 88 | } 89 | 90 | static void i2a(int num, char *bf) 91 | { 92 | if (num < 0) { 93 | num = -num; 94 | *bf++ = '-'; 95 | } 96 | ui2a(num, 10, 0, bf); 97 | } 98 | 99 | static int a2d(char ch) 100 | { 101 | if (ch >= '0' && ch <= '9') 102 | return ch - '0'; 103 | else if (ch >= 'a' && ch <= 'f') 104 | return ch - 'a' + 10; 105 | else if (ch >= 'A' && ch <= 'F') 106 | return ch - 'A' + 10; 107 | else 108 | return -1; 109 | } 110 | 111 | static char a2i(char ch, char **src, int base, int *nump) 112 | { 113 | char *p = *src; 114 | int num = 0; 115 | int digit; 116 | while ((digit = a2d(ch)) >= 0) { 117 | if (digit > base) 118 | break; 119 | num = num * base + digit; 120 | ch = *p++; 121 | } 122 | *src = p; 123 | *nump = num; 124 | return ch; 125 | } 126 | 127 | static void putchw(void *putp, putcf putf, int n, char z, char *bf) 128 | { 129 | char fc = z ? '0' : ' '; 130 | char ch; 131 | char *p = bf; 132 | while (*p++ && n > 0) 133 | n--; 134 | while (n-- > 0) 135 | putf(putp, fc); 136 | while ((ch = *bf++)) 137 | putf(putp, ch); 138 | } 139 | 140 | void tfp_format(void *putp, putcf putf, char *fmt, va_list va) 141 | { 142 | char bf[12]; 143 | 144 | char ch; 145 | 146 | while ((ch = *(fmt++))) { 147 | if (ch != '%') 148 | putf(putp, ch); 149 | else { 150 | char lz = 0; 151 | #ifdef PRINTF_LONG_SUPPORT 152 | char lng = 0; 153 | #endif 154 | int w = 0; 155 | ch = *(fmt++); 156 | if (ch == '0') { 157 | ch = *(fmt++); 158 | lz = 1; 159 | } 160 | if (ch >= '0' && ch <= '9') { 161 | ch = a2i(ch, &fmt, 10, &w); 162 | } 163 | #ifdef PRINTF_LONG_SUPPORT 164 | if (ch == 'l') { 165 | ch = *(fmt++); 166 | lng = 1; 167 | } 168 | #endif 169 | switch (ch) { 170 | case 0: 171 | goto abort; 172 | case 'u':{ 173 | #ifdef PRINTF_LONG_SUPPORT 174 | if (lng) 175 | uli2a(va_arg(va, unsigned long int), 10, 0, bf); 176 | else 177 | #endif 178 | ui2a(va_arg(va, unsigned int), 10, 0, bf); 179 | putchw(putp, putf, w, lz, bf); 180 | break; 181 | } 182 | case 'd':{ 183 | #ifdef PRINTF_LONG_SUPPORT 184 | if (lng) 185 | li2a(va_arg(va, unsigned long int), bf); 186 | else 187 | #endif 188 | i2a(va_arg(va, int), bf); 189 | putchw(putp, putf, w, lz, bf); 190 | break; 191 | } 192 | case 'x': 193 | case 'X': 194 | #ifdef PRINTF_LONG_SUPPORT 195 | if (lng) 196 | uli2a(va_arg(va, unsigned long int), 16, (ch == 'X'), bf); 197 | else 198 | #endif 199 | ui2a(va_arg(va, unsigned int), 16, (ch == 'X'), bf); 200 | putchw(putp, putf, w, lz, bf); 201 | break; 202 | case 'c': 203 | putf(putp, (char) (va_arg(va, int))); 204 | break; 205 | case 's': 206 | putchw(putp, putf, w, 0, va_arg(va, char *)); 207 | break; 208 | case '%': 209 | putf(putp, ch); 210 | default: 211 | break; 212 | } 213 | } 214 | } 215 | abort:; 216 | } 217 | 218 | 219 | void init_printf(void *putp, void (*putf) (void *, char)) 220 | { 221 | stdout_putf = putf; 222 | stdout_putp = putp; 223 | } 224 | 225 | void tfp_printf(char *fmt, ...) 226 | { 227 | va_list va; 228 | va_start(va, fmt); 229 | tfp_format(stdout_putp, stdout_putf, fmt, va); 230 | va_end(va); 231 | while (!isSerialTransmitBufferEmpty(core.mainport)); 232 | } 233 | 234 | static void putcp(void *p, char c) 235 | { 236 | *(*((char **) p))++ = c; 237 | } 238 | 239 | void tfp_sprintf(char *s, char *fmt, ...) 240 | { 241 | va_list va; 242 | va_start(va, fmt); 243 | tfp_format(&s, putcp, fmt, va); 244 | putcp(&s, 0); 245 | va_end(va); 246 | } 247 | #endif /* USE_LAME_PRINTF */ 248 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | # "THE BEER-WARE LICENSE" (Revision 42): 3 | # wrote this file. As long as you retain this notice you 4 | # can do whatever you want with this stuff. If we meet some day, and you think 5 | # this stuff is worth it, you can buy me a beer in return 6 | ############################################################################### 7 | # 8 | # Makefile for building the baseflight firmware. 9 | # 10 | # Invoke this with 'make help' to see the list of supported targets. 11 | # 12 | 13 | ############################################################################### 14 | # Things that the user might override on the commandline 15 | # 16 | 17 | # The target to build, must be one of NAZE or CJMCU 18 | TARGET ?= NAZE 19 | 20 | # Compile-time options 21 | OPTIONS ?= 22 | 23 | # Debugger optons, must be empty or GDB 24 | DEBUG ?= 25 | 26 | # Serial port/Device for flashing 27 | SERIAL_DEVICE ?= /dev/ttyUSB0 28 | 29 | ############################################################################### 30 | # Things that need to be maintained as the source changes 31 | # 32 | 33 | VALID_TARGETS = NAZE CJMCU 34 | 35 | # Working directories 36 | ROOT = $(dir $(lastword $(MAKEFILE_LIST))) 37 | SRC_DIR = $(ROOT)/src 38 | CMSIS_DIR = $(ROOT)/lib/CMSIS 39 | STDPERIPH_DIR = $(ROOT)/lib/STM32F10x_StdPeriph_Driver 40 | OBJECT_DIR = $(ROOT)/obj 41 | BIN_DIR = $(ROOT)/obj 42 | 43 | # Source files common to all targets 44 | COMMON_SRC = buzzer.c \ 45 | cli.c \ 46 | config.c \ 47 | imu.c \ 48 | main.c \ 49 | mixer.c \ 50 | mw.c \ 51 | sensors.c \ 52 | serial.c \ 53 | rxmsp.c \ 54 | drv_gpio.c \ 55 | drv_i2c.c \ 56 | drv_i2c_soft.c \ 57 | drv_system.c \ 58 | drv_serial.c \ 59 | drv_uart.c \ 60 | printf.c \ 61 | utils.c \ 62 | fw_nav.c \ 63 | sbus.c \ 64 | sumd.c \ 65 | spektrum.c \ 66 | startup_stm32f10x_md_gcc.S \ 67 | $(CMSIS_SRC) \ 68 | $(STDPERIPH_SRC) 69 | 70 | # Source files for full-featured systems 71 | HIGHEND_SRC = gps.c \ 72 | drv_softserial.c \ 73 | telemetry_common.c \ 74 | telemetry_frsky.c \ 75 | telemetry_hott.c \ 76 | blackbox.c 77 | 78 | # Source files for the NAZE target 79 | NAZE_SRC = drv_adc.c \ 80 | drv_adxl345.c \ 81 | drv_ak8975.c \ 82 | drv_bma280.c \ 83 | drv_bmp085.c \ 84 | drv_ms5611.c \ 85 | drv_hcsr04.c \ 86 | drv_hmc5883l.c \ 87 | drv_ledring.c \ 88 | drv_mma845x.c \ 89 | drv_mpu3050.c \ 90 | drv_mpu6050.c \ 91 | drv_mpu6500.c \ 92 | drv_l3g4200d.c \ 93 | drv_pwm.c \ 94 | drv_spi.c \ 95 | drv_timer.c \ 96 | $(HIGHEND_SRC) \ 97 | $(COMMON_SRC) 98 | 99 | # Source files for the CJMCU target 100 | CJMCU_SRC = drv_adc.c \ 101 | drv_mpu6050.c \ 102 | drv_hmc5883l.c \ 103 | drv_pwm.c \ 104 | drv_timer.c \ 105 | $(COMMON_SRC) 106 | 107 | # In some cases, %.s regarded as intermediate file, which is actually not. 108 | # This will prevent accidental deletion of startup code. 109 | .PRECIOUS: %.s 110 | 111 | # Search path for baseflight sources 112 | VPATH := $(SRC_DIR):$(SRC_DIR)/baseflight_startups 113 | 114 | # Search path and source files for the CMSIS sources 115 | VPATH := $(VPATH):$(CMSIS_DIR)/CM3/CoreSupport:$(CMSIS_DIR)/CM3/DeviceSupport/ST/STM32F10x 116 | CMSIS_SRC = $(notdir $(wildcard $(CMSIS_DIR)/CM3/CoreSupport/*.c \ 117 | $(CMSIS_DIR)/CM3/DeviceSupport/ST/STM32F10x/*.c)) 118 | 119 | # Search path and source files for the ST stdperiph library 120 | VPATH := $(VPATH):$(STDPERIPH_DIR)/src 121 | STDPERIPH_SRC = $(notdir $(wildcard $(STDPERIPH_DIR)/src/*.c)) 122 | 123 | ############################################################################### 124 | # Things that might need changing to use different tools 125 | # 126 | 127 | # Tool names 128 | CC = arm-none-eabi-gcc 129 | OBJCOPY = arm-none-eabi-objcopy 130 | 131 | # 132 | # Tool options. 133 | # 134 | INCLUDE_DIRS = $(SRC_DIR) \ 135 | $(STDPERIPH_DIR)/inc \ 136 | $(CMSIS_DIR)/CM3/CoreSupport \ 137 | $(CMSIS_DIR)/CM3/DeviceSupport/ST/STM32F10x \ 138 | 139 | ARCH_FLAGS = -mthumb -mcpu=cortex-m3 140 | 141 | ifeq ($(DEBUG),GDB) 142 | OPTIMIZE = -O0 143 | LTO_FLAGS = $(OPTIMIZE) 144 | else 145 | OPTIMIZE = -Os 146 | LTO_FLAGS = -flto -fuse-linker-plugin $(OPTIMIZE) 147 | endif 148 | 149 | DEBUG_FLAGS = -ggdb3 150 | 151 | CFLAGS = $(ARCH_FLAGS) \ 152 | $(LTO_FLAGS) \ 153 | $(addprefix -D,$(OPTIONS)) \ 154 | $(addprefix -I,$(INCLUDE_DIRS)) \ 155 | $(DEBUG_FLAGS) \ 156 | -std=gnu99 \ 157 | -Wall -pedantic -Wextra -Wshadow -Wunsafe-loop-optimizations \ 158 | -ffunction-sections \ 159 | -fdata-sections \ 160 | -DSTM32F10X_MD \ 161 | -DUSE_STDPERIPH_DRIVER \ 162 | -D$(TARGET) 163 | 164 | ASFLAGS = $(ARCH_FLAGS) \ 165 | -x assembler-with-cpp \ 166 | $(addprefix -I,$(INCLUDE_DIRS)) 167 | 168 | # XXX Map/crossref output? 169 | LD_SCRIPT = $(ROOT)/stm32_flash.ld 170 | LDFLAGS = -lm \ 171 | -nostartfiles \ 172 | --specs=nano.specs \ 173 | -lc \ 174 | -lnosys \ 175 | $(ARCH_FLAGS) \ 176 | $(LTO_FLAGS) \ 177 | $(DEBUG_FLAGS) \ 178 | -static \ 179 | -Wl,-gc-sections,-Map,$(TARGET_MAP) \ 180 | -T$(LD_SCRIPT) 181 | 182 | ############################################################################### 183 | # No user-serviceable parts below 184 | ############################################################################### 185 | 186 | # 187 | # Things we will build 188 | # 189 | ifeq ($(filter $(TARGET),$(VALID_TARGETS)),) 190 | $(error Target '$(TARGET)' is not valid, must be one of $(VALID_TARGETS)) 191 | endif 192 | 193 | 194 | TARGET_HEX = $(BIN_DIR)/baseflight_$(TARGET).hex 195 | TARGET_ELF = $(BIN_DIR)/baseflight_$(TARGET).elf 196 | TARGET_OBJS = $(addsuffix .o,$(addprefix $(OBJECT_DIR)/$(TARGET)/,$(basename $($(TARGET)_SRC)))) 197 | TARGET_MAP = $(OBJECT_DIR)/baseflight_$(TARGET).map 198 | 199 | # List of buildable ELF files and their object dependencies. 200 | # It would be nice to compute these lists, but that seems to be just beyond make. 201 | 202 | $(TARGET_HEX): $(TARGET_ELF) 203 | $(OBJCOPY) -O ihex --set-start 0x8000000 $< $@ 204 | 205 | $(TARGET_ELF): $(TARGET_OBJS) 206 | $(CC) -o $@ $^ $(LDFLAGS) 207 | 208 | # Compile 209 | $(OBJECT_DIR)/$(TARGET)/%.o: %.c 210 | @mkdir -p $(dir $@) 211 | @echo %% $(notdir $<) 212 | @$(CC) -c -o $@ $(CFLAGS) $< 213 | 214 | # Assemble 215 | $(OBJECT_DIR)/$(TARGET)/%.o: %.s 216 | @mkdir -p $(dir $@) 217 | @echo %% $(notdir $<) 218 | @$(CC) -c -o $@ $(ASFLAGS) $< 219 | $(OBJECT_DIR)/$(TARGET)/%.o): %.S 220 | @mkdir -p $(dir $@) 221 | @echo %% $(notdir $<) 222 | @$(CC) -c -o $@ $(ASFLAGS) $< 223 | 224 | clean: 225 | rm -f $(TARGET_HEX) $(TARGET_ELF) $(TARGET_OBJS) $(TARGET_MAP) 226 | 227 | flash_$(TARGET): $(TARGET_HEX) 228 | stty -F $(SERIAL_DEVICE) raw speed 115200 -crtscts cs8 -parenb -cstopb -ixon 229 | echo -n 'R' >$(SERIAL_DEVICE) 230 | stm32flash -w $(TARGET_HEX) -v -g 0x0 -b 115200 $(SERIAL_DEVICE) 231 | 232 | flash: flash_$(TARGET) 233 | 234 | 235 | unbrick_$(TARGET): $(TARGET_HEX) 236 | stty -F $(SERIAL_DEVICE) raw speed 115200 -crtscts cs8 -parenb -cstopb -ixon 237 | stm32flash -w $(TARGET_HEX) -v -g 0x0 -b 115200 $(SERIAL_DEVICE) 238 | 239 | unbrick: unbrick_$(TARGET) 240 | 241 | help: 242 | @echo "" 243 | @echo "Makefile for the baseflight firmware" 244 | @echo "" 245 | @echo "Usage:" 246 | @echo " make [TARGET=] [OPTIONS=\"\"]" 247 | @echo "" 248 | @echo "Valid TARGET values are: $(VALID_TARGETS)" 249 | @echo "" 250 | --------------------------------------------------------------------------------