├── cleanflight
├── build_config.h
├── drivers
│ ├── accgyro_mpu6050.h
│ ├── bus_i2c.h
│ ├── system.h
│ ├── nvic.h
│ ├── accgyro.h
│ ├── system.c
│ ├── gpio.h
│ ├── bus_i2c_stm32f0xx.c
│ └── accgyro_mpu6050.c
├── target.h
└── platform.h
├── inc
├── atan.h
├── timers.h
├── rx_bk2423.h
├── spi.h
├── flight.h
├── led.h
├── protospi.h
├── nf_gpio.h
├── misc.h
├── imu.h
├── accgyro_mpu6050.h
├── common.h
├── system_stm32f0xx.h
├── stm32f0xx_conf.h
├── iface_nrf24l01.h
├── pid.h
├── stm32f0xx_gpio.h
├── core_cmFunc.h
└── stm32f0xx_i2c.h
├── README.md
├── src
├── led.c
├── imu.c
├── main.c
├── spi.c
├── pid.c
├── gpio.c
├── atan.c
├── misc.c
├── timers.c
├── rx_bk2423.c
├── nrf24l01.c
├── flight.c
└── system_stm32f0xx.c
├── linker
└── stm32f0_linker.ld
├── Makefile
└── startup
├── startup_stm32f031.s
└── startup_stm32f031.lst
/cleanflight/build_config.h:
--------------------------------------------------------------------------------
1 |
--------------------------------------------------------------------------------
/inc/atan.h:
--------------------------------------------------------------------------------
1 | #ifndef _ATAN_H
2 | #define _ATAN_H
3 | u16 fast_atan(s16 y, s16 x);
4 | #endif
5 |
--------------------------------------------------------------------------------
/inc/timers.h:
--------------------------------------------------------------------------------
1 | #ifndef _TIMERS_H
2 | #define _TIMERS_H
3 | void timers_init(void);
4 | #endif
5 |
--------------------------------------------------------------------------------
/inc/rx_bk2423.h:
--------------------------------------------------------------------------------
1 |
2 | void rx_debug(void);
3 | void rx_init(void);
4 | void rx_bind(void);
5 | void rx_search_channel(void);
6 | void rx_read_payload(void);
7 |
--------------------------------------------------------------------------------
/inc/spi.h:
--------------------------------------------------------------------------------
1 | #ifndef _SPI_H
2 | #define _SPI_H
3 |
4 | #include "common.h"
5 |
6 | void spi_init(void);
7 | void spi_write(u16 data);
8 | u16 spi_read(void);
9 | u16 spi_xfer(u16 data);
10 | #endif
11 |
--------------------------------------------------------------------------------
/inc/flight.h:
--------------------------------------------------------------------------------
1 | #ifndef _FLIGHT_H
2 | #define _FLIGHT_H
3 | typedef struct {
4 | float x;
5 | float y;
6 | float z;
7 | } Axis3f;
8 | void controllerInit(void);
9 | void flightControl(void);
10 | #endif
11 |
--------------------------------------------------------------------------------
/inc/led.h:
--------------------------------------------------------------------------------
1 | #ifndef _LED_H
2 | #define _LED_H
3 |
4 | #define LED0_ON gpio_set(GPIOA, 15)
5 | #define LED0_OFF gpio_clear(GPIOA, 15)
6 | #define LED1_ON gpio_set(GPIOB, 2)
7 | #define LED1_OFF gpio_clear(GPIOB, 2)
8 |
9 | void led_blink_lr (int d);
10 | void led_blink_all (int d);
11 | #endif
12 |
--------------------------------------------------------------------------------
/inc/protospi.h:
--------------------------------------------------------------------------------
1 | #ifndef _SPIPROTO_H_
2 | #define _SPIPROTO_H_
3 |
4 | #include "stm32f0xx.h"
5 | #include "nf_gpio.h"
6 | #include "spi.h"
7 |
8 | #define PROTOSPI_pin_set(io) gpio_set((GPIO_TypeDef*)io.port,io.pin)
9 | #define PROTOSPI_pin_clear(io) gpio_clear((GPIO_TypeDef*)io.port,io.pin)
10 | #define PROTOSPI_xfer(byte) spi_xfer(byte)
11 | #define _NOP() asm volatile ("nop")
12 |
13 | #endif // _SPIPROTO_H_
14 |
--------------------------------------------------------------------------------
/inc/nf_gpio.h:
--------------------------------------------------------------------------------
1 | #ifndef _GPIO_H_
2 | #define _GPIO_H_
3 |
4 | #include "stm32f0xx.h"
5 | #include "stm32f0xx_gpio.h"
6 |
7 | #define MMIO32(addr) (*(volatile uint32_t *)(addr))
8 | #define GPIO_BSRR(port) MMIO32(port + 0x18)
9 | #define GPIO_BRR(port) MMIO32(port + 0x28)
10 |
11 |
12 | void gpio_init(void);
13 | void gpio_set(GPIO_TypeDef* port, u8 pin);
14 | void gpio_clear(GPIO_TypeDef* port, u8 pin);
15 | #endif
16 |
17 |
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | nullflight
2 | ==========
3 |
4 |
5 | This is an open source flight controller project, that runs on the Cheerson CX-10 toy quadcopter.
6 | It can currently fly a tricopter with gyro stabilisation (3DOF). Quadcopter code is there but
7 | untested. Autolevel code might be added later if space and time permit it.
8 |
9 | It contains code from all over the place, so everything is subject to the respective project's license.
10 | Everything I wrote myself is licensed under the latest GNU GPL.
11 |
12 |
--------------------------------------------------------------------------------
/inc/misc.h:
--------------------------------------------------------------------------------
1 | #ifndef MISC_H
2 | #define MISC_H
3 |
4 | // types and variables
5 | typedef struct
6 | {
7 | uint32_t SYSCLK_Frequency;
8 | uint32_t HCLK_Frequency;
9 | uint32_t PCLK_Frequency;
10 | uint32_t ADCCLK_Frequency;
11 | uint32_t CECCLK_Frequency;
12 | uint32_t I2C1CLK_Frequency;
13 | uint32_t USART1CLK_Frequency;
14 | uint32_t USART2CLK_Frequency; /*!< Only applicable for STM32F072 and STM32F091 devices */
15 | uint32_t USART3CLK_Frequency; /*!< Only applicable for STM32F091 devices */
16 | uint32_t USBCLK_Frequency; /*!< Only applicable for STM32F072 devices */
17 | }RCC_ClocksTypeDef;
18 |
19 | // functions and prototypes
20 | //void delay (int a);
21 | void RCC_GetClocksFreq(RCC_ClocksTypeDef* RCC_Clocks);
22 |
23 | #endif
24 |
--------------------------------------------------------------------------------
/inc/imu.h:
--------------------------------------------------------------------------------
1 | #ifndef _IMU_H
2 | #define _IMU_H
3 |
4 | #define GYRO_BIAS_X 40 // Roll (positive value rolls right)
5 | #define GYRO_BIAS_Y 10 // Pitch (positive value pitches forward)
6 | #define GYRO_BIAS_Z 0 // Yaw
7 |
8 | #define IMU_UPDATE_FREQ 500
9 | #define IMU_UPDATE_DT (float)(1.0/IMU_UPDATE_FREQ)
10 |
11 |
12 | #define MPU6050_DEG_PER_LSB_250 (float)((2 * 250.0) / 65536.0)
13 | #define MPU6050_DEG_PER_LSB_500 (float)((2 * 500.0) / 65536.0)
14 | #define MPU6050_DEG_PER_LSB_1000 (float)((2 * 1000.0) / 65536.0)
15 | #define MPU6050_DEG_PER_LSB_2000 (float)((2 * 2000.0) / 65536.0)
16 |
17 | #define IMU_DEG_PER_LSB_CFG MPU6050_DEG_PER_LSB_2000
18 |
19 | extern gyro_t gyro;
20 |
21 | void imu_init(void);
22 | void gyro_read(Axis3f *gyro_data);
23 | #endif
24 |
--------------------------------------------------------------------------------
/src/led.c:
--------------------------------------------------------------------------------
1 | #include "stm32f0xx.h"
2 | #include "common.h"
3 | #include "nf_gpio.h"
4 | #include "misc.h"
5 |
6 | #if 0
7 | void led_blink_all (int d) {
8 | // Set PB2
9 | GPIOB->BSRR = (1<<2);
10 | // Set PA15
11 | GPIOA->BSRR = (1<<15);
12 | delay(d);
13 | // Reset PB2
14 | GPIOB->BRR = (1<<2);
15 | // Reset PA15
16 | GPIOA->BRR = (1<<15);
17 | delay(d);
18 | }
19 |
20 |
21 | void led_blink_lr (int d) {
22 | // Set PB2
23 | GPIOB->BSRR = (1<<2);
24 | // Reset PA15
25 | GPIOA->BRR = (1<<15);
26 | delay(d);
27 | // Reset PB2
28 | GPIOB->BRR = (1<<2);
29 | // Set PA15
30 | GPIOA->BSRR = (1<<15);
31 | delay(d);
32 | }
33 |
34 |
35 | void led_blink_lr (int d) {
36 | // Set PB2
37 | gpio_set(GPIOB, 2);
38 | // Reset PA15
39 | gpio_clear(GPIOA, 15);
40 | delay(d);
41 | // Reset PB2
42 | gpio_clear(GPIOB, 2);
43 | // Set PA15
44 | gpio_set(GPIOA, 15);
45 | delay(d);
46 | }
47 |
48 | #else
49 | #endif
50 |
--------------------------------------------------------------------------------
/src/imu.c:
--------------------------------------------------------------------------------
1 | #include "common.h"
2 | #include "stm32f0xx.h"
3 | #include "misc.h"
4 | #include "led.h"
5 | #include "nf_gpio.h"
6 | #include "timers.h"
7 | #include "spi.h"
8 | #include "iface_nrf24l01.h"
9 | #include "rx_bk2423.h"
10 | #include "flight.h"
11 |
12 | // cleanflight drivers
13 | #include "system.h"
14 | #include "accgyro.h"
15 | #include "accgyro_mpu6050.h"
16 | #include "bus_i2c.h"
17 | #include "flight.h"
18 | #include "imu.h"
19 |
20 | gyro_t gyro;
21 | acc_t acc;
22 | u16 gyroLpf;
23 |
24 | s16 gyroData[3];
25 | s16 accData[3];
26 |
27 | void imu_init(void)
28 | {
29 | if (mpu6050GyroDetect((mpu6050Config_t *) NULL, &gyro, gyroLpf)) {
30 | gyro.init();
31 | gyro.read(gyroData);
32 | }
33 |
34 | if (mpu6050AccDetect((mpu6050Config_t *) NULL, &acc)) {
35 | acc.init();
36 | acc.read(accData);
37 | }
38 | }
39 |
40 | void gyro_read(Axis3f *gyro_out)
41 | {
42 | gyro.read(gyroData);
43 |
44 | gyro_out->x = (gyroData[0] - GYRO_BIAS_X) * IMU_DEG_PER_LSB_CFG;
45 | gyro_out->y = (gyroData[1] - GYRO_BIAS_Y) * IMU_DEG_PER_LSB_CFG;
46 | gyro_out->z = (gyroData[2] - GYRO_BIAS_Z) * IMU_DEG_PER_LSB_CFG;
47 |
48 | }
49 |
--------------------------------------------------------------------------------
/inc/accgyro_mpu6050.h:
--------------------------------------------------------------------------------
1 | /*
2 | * This file is part of Cleanflight.
3 | *
4 | * Cleanflight 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 | * Cleanflight 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 | *
14 | * You should have received a copy of the GNU General Public License
15 | * along with Cleanflight. If not, see .
16 | */
17 |
18 | #pragma once
19 |
20 | typedef struct mpu6050Config_s {
21 | uint32_t gpioAPB2Peripherals;
22 | uint16_t gpioPin;
23 | GPIO_TypeDef *gpioPort;
24 | } mpu6050Config_t;
25 |
26 | bool mpu6050AccDetect(const mpu6050Config_t *config,acc_t *acc);
27 | bool mpu6050GyroDetect(const mpu6050Config_t *config, gyro_t *gyro, uint16_t lpf);
28 | void mpu6050DmpLoop(void);
29 | void mpu6050DmpResetFifo(void);
30 |
--------------------------------------------------------------------------------
/cleanflight/drivers/accgyro_mpu6050.h:
--------------------------------------------------------------------------------
1 | /*
2 | * This file is part of Cleanflight.
3 | *
4 | * Cleanflight 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 | * Cleanflight 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 | *
14 | * You should have received a copy of the GNU General Public License
15 | * along with Cleanflight. If not, see .
16 | */
17 |
18 | #pragma once
19 |
20 | typedef struct mpu6050Config_s {
21 | uint32_t gpioAPB2Peripherals;
22 | uint16_t gpioPin;
23 | GPIO_TypeDef *gpioPort;
24 | } mpu6050Config_t;
25 |
26 | bool mpu6050AccDetect(const mpu6050Config_t *config,acc_t *acc);
27 | bool mpu6050GyroDetect(const mpu6050Config_t *config, gyro_t *gyro, uint16_t lpf);
28 | void mpu6050DmpLoop(void);
29 | void mpu6050DmpResetFifo(void);
30 |
--------------------------------------------------------------------------------
/cleanflight/drivers/bus_i2c.h:
--------------------------------------------------------------------------------
1 | /*
2 | * This file is part of Cleanflight.
3 | *
4 | * Cleanflight 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 | * Cleanflight 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 | *
14 | * You should have received a copy of the GNU General Public License
15 | * along with Cleanflight. If not, see .
16 | */
17 |
18 | #pragma once
19 |
20 | typedef enum I2CDevice {
21 | I2CDEV_1,
22 | I2CDEV_2,
23 | I2CDEV_MAX = I2CDEV_2,
24 | } I2CDevice;
25 |
26 | void i2cInit(void);
27 | bool i2cWriteBuffer(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data);
28 | bool i2cWrite(uint8_t addr_, uint8_t reg, uint8_t data);
29 | bool i2cRead(uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* buf);
30 | uint16_t i2cGetErrorCounter(void);
31 |
--------------------------------------------------------------------------------
/cleanflight/drivers/system.h:
--------------------------------------------------------------------------------
1 | /*
2 | * This file is part of Cleanflight.
3 | *
4 | * Cleanflight 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 | * Cleanflight 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 | *
14 | * You should have received a copy of the GNU General Public License
15 | * along with Cleanflight. If not, see .
16 | */
17 |
18 | #pragma once
19 |
20 | void systemInit(void);
21 | void delayMicroseconds(uint32_t us);
22 | void delay(uint32_t ms);
23 |
24 | uint32_t micros(void);
25 | uint32_t millis(void);
26 |
27 | // failure
28 | void failureMode(uint8_t mode);
29 |
30 | // bootloader/IAP
31 | void systemReset(void);
32 | void systemResetToBootloader(void);
33 |
34 | void enableGPIOPowerUsageAndNoiseReductions(void);
35 | // current crystal frequency - 8 or 12MHz
36 | extern uint32_t hse_value;
37 |
--------------------------------------------------------------------------------
/cleanflight/target.h:
--------------------------------------------------------------------------------
1 | /*
2 | * This file is part of Cleanflight.
3 | *
4 | * Cleanflight 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 | * Cleanflight 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 | *
14 | * You should have received a copy of the GNU General Public License
15 | * along with Cleanflight. If not, see .
16 | */
17 |
18 | #pragma once
19 |
20 | #define STM32F013
21 |
22 | #define TARGET_BOARD_IDENTIFIER "CX10" // STM Discovery F3
23 |
24 | #define FLASH_PAGE_COUNT 16
25 | #define FLASH_PAGE_SIZE ((uint16_t)0x400)
26 |
27 | #define LED0_GPIO GPIOA
28 | #define LED0_PIN Pin_15 // Blue and red LEDs right
29 | #define LED0_PERIPHERAL RCC_AHBPeriph_GPIOA
30 | #define LED1_GPIO GPIOB
31 | #define LED1_PIN Pin_2 // Blue and red LEDs left
32 | #define LED1_PERIPHERAL RCC_AHBPeriph_GPIOB
33 |
34 | #define GYRO
35 | #define USE_GYRO_MPU6050
36 |
37 | #define ACC
38 | #define USE_ACC_MPU6050
39 |
40 | #define LED0
41 | #define LED1
42 |
43 | #define BRUSHED_MOTORS
44 |
45 | #define USE_I2C
46 | #define I2C_DEVICE (I2CDEV_1)
47 |
48 | #define SENSORS_SET (SENSOR_ACC)
49 |
50 | #define SERIAL_PORT_COUNT 0
51 |
--------------------------------------------------------------------------------
/src/main.c:
--------------------------------------------------------------------------------
1 | // includes
2 | #include "common.h"
3 | #include "stm32f0xx.h"
4 | #include "misc.h"
5 | #include "led.h"
6 | #include "nf_gpio.h"
7 | #include "timers.h"
8 | #include "spi.h"
9 | #include "iface_nrf24l01.h"
10 | #include "rx_bk2423.h"
11 | #include "flight.h"
12 |
13 | // cleanflight drivers
14 | #include "system.h"
15 | #include "accgyro.h"
16 | #include "accgyro_mpu6050.h"
17 | #include "bus_i2c.h"
18 |
19 |
20 | #include "imu.h"
21 |
22 | /* Private typedef -----------------------------------------------------------*/
23 | /* Private define ------------------------------------------------------------*/
24 |
25 | u8 payload[];
26 |
27 | /**
28 | * @brief Main program.
29 | * @param None
30 | * @retval None
31 | */
32 | int main(void)
33 | {
34 | /*!< At this stage the microcontroller clock setting is already configured,
35 | this is done through SystemInit() function which is called from startup
36 | file (startup_stm32f0xx.s) before to branch to application main.
37 | To reconfigure the default setting of SystemInit() function, refer to
38 | system_stm32f0xx.c file
39 | */
40 |
41 | systemInit();
42 | gpio_init();
43 | timers_init();
44 |
45 | spi_init();
46 | rx_init();
47 | rx_bind();
48 | rx_search_channel();
49 |
50 | i2cInit();
51 | delay(100);
52 | imu_init();
53 | controllerInit();
54 |
55 | /* what's missing here?
56 | FIXME FIRST: Battery level detection!!!
57 |
58 | FIXME: acc+gyro calibration
59 | FIXME: autolevel code missing
60 |
61 | */
62 |
63 |
64 | while (1) {
65 | rx_read_payload();
66 | flightControl();
67 | }
68 |
69 | return 0;
70 | }
71 |
72 |
--------------------------------------------------------------------------------
/cleanflight/drivers/nvic.h:
--------------------------------------------------------------------------------
1 |
2 | #pragma once
3 |
4 | #define NVIC_PRIORITY_GROUPING NVIC_PriorityGroup_2
5 |
6 | // can't use 0
7 | #define NVIC_PRIO_MAX NVIC_BUILD_PRIORITY(0, 1)
8 | #define NVIC_PRIO_TIMER NVIC_BUILD_PRIORITY(1, 1)
9 | #define NVIC_PRIO_BARO_EXT NVIC_BUILD_PRIORITY(0x0f, 0x0f)
10 | #define NVIC_PRIO_WS2811_DMA NVIC_BUILD_PRIORITY(1, 2) // TODO - is there some reason to use high priority? (or to use DMA IRQ at all?)
11 | #define NVIC_PRIO_SERIALUART1_TXDMA NVIC_BUILD_PRIORITY(1, 1)
12 | #define NVIC_PRIO_SERIALUART1_RXDMA NVIC_BUILD_PRIORITY(1, 1)
13 | #define NVIC_PRIO_SERIALUART1 NVIC_BUILD_PRIORITY(1, 1)
14 | #define NVIC_PRIO_SERIALUART2_TXDMA NVIC_BUILD_PRIORITY(1, 0)
15 | #define NVIC_PRIO_SERIALUART2_RXDMA NVIC_BUILD_PRIORITY(1, 1)
16 | #define NVIC_PRIO_SERIALUART2 NVIC_BUILD_PRIORITY(1, 2)
17 | #define NVIC_PRIO_SERIALUART3 NVIC_BUILD_PRIORITY(1, 2)
18 | #define NVIC_PRIO_I2C_ER NVIC_BUILD_PRIORITY(0, 0)
19 | #define NVIC_PRIO_I2C_EV NVIC_BUILD_PRIORITY(0, 0)
20 | #define NVIC_PRIO_USB NVIC_BUILD_PRIORITY(2, 0)
21 | #define NVIC_PRIO_USB_WUP NVIC_BUILD_PRIORITY(1, 0)
22 | #define NVIC_PRIO_CALLBACK NVIC_BUILD_PRIORITY(0x0f, 0x0f)
23 |
24 | // utility macros to join/split priority
25 | #define NVIC_BUILD_PRIORITY(base,sub) (((((base)<<(4-(7-(NVIC_PRIORITY_GROUPING>>8))))|((sub)&(0x0f>>(7-(NVIC_PRIORITY_GROUPING>>8)))))<<4)&0xf0)
26 | #define NVIC_PRIORITY_BASE(prio) (((prio)>>(4-(7-(NVIC_PRIORITY_GROUPING>>8))))>>4)
27 | #define NVIC_PRIORITY_SUB(prio) (((prio)&(0x0f>>(7-(NVIC_PRIORITY_GROUPING>>8))))>>4)
28 |
--------------------------------------------------------------------------------
/cleanflight/drivers/accgyro.h:
--------------------------------------------------------------------------------
1 | /*
2 | * This file is part of Cleanflight.
3 | *
4 | * Cleanflight 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 | * Cleanflight 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 | *
14 | * You should have received a copy of the GNU General Public License
15 | * along with Cleanflight. If not, see .
16 | */
17 |
18 | #pragma once
19 |
20 | extern uint16_t acc_1G;
21 |
22 | typedef void (*sensorInitFuncPtr)(void); // sensor init prototype
23 | typedef void (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
24 |
25 | typedef struct gyro_s {
26 | sensorInitFuncPtr init; // initialize function
27 | sensorReadFuncPtr read; // read 3 axis data function
28 | sensorReadFuncPtr temperature; // read temperature if available
29 | float scale; // scalefactor
30 | } gyro_t;
31 |
32 | typedef struct acc_s {
33 | sensorInitFuncPtr init; // initialize function
34 | sensorReadFuncPtr read; // read 3 axis data function
35 | char revisionCode; // a revision code for the sensor, if known
36 | } acc_t;
37 |
--------------------------------------------------------------------------------
/cleanflight/platform.h:
--------------------------------------------------------------------------------
1 | /*
2 | * This file is part of Cleanflight.
3 | *
4 | * Cleanflight 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 | * Cleanflight 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 | *
14 | * You should have received a copy of the GNU General Public License
15 | * along with Cleanflight. If not, see .
16 | */
17 |
18 | #pragma once
19 |
20 | #ifdef STM32F303xC
21 | #include "stm32f30x_conf.h"
22 | #include "stm32f30x_rcc.h"
23 | #include "stm32f30x_gpio.h"
24 | #include "core_cm4.h"
25 |
26 | // Chip Unique ID on F303
27 | #define U_ID_0 (*(uint32_t*)0x1FFFF7AC)
28 | #define U_ID_1 (*(uint32_t*)0x1FFFF7B0)
29 | #define U_ID_2 (*(uint32_t*)0x1FFFF7B4)
30 |
31 | #endif
32 |
33 | #ifdef STM32F10X
34 |
35 | #include "stm32f10x_conf.h"
36 | #include "stm32f10x_gpio.h"
37 | #include "core_cm3.h"
38 |
39 | // Chip Unique ID on F103
40 | #define U_ID_0 (*(uint32_t*)0x1FFFF7E8)
41 | #define U_ID_1 (*(uint32_t*)0x1FFFF7EC)
42 | #define U_ID_2 (*(uint32_t*)0x1FFFF7F0)
43 |
44 | #endif // STM32F10X
45 |
46 | #ifdef STM32F031
47 |
48 | #include "common.h"
49 | #include "stm32f0xx_conf.h"
50 | #include "stm32f0xx_rcc.h"
51 | #include "stm32f0xx_gpio.h"
52 | #include "stm32f0xx_i2c.h"
53 | #include "core_cm0.h"
54 |
55 | // Chip Unique ID on F103
56 | //#define U_ID_0 (*(uint32_t*)0x1FFFF7E8)
57 | //#define U_ID_1 (*(uint32_t*)0x1FFFF7EC)
58 | //#define U_ID_2 (*(uint32_t*)0x1FFFF7F0)
59 |
60 | #endif // STM32F031
61 |
62 | #include "target.h"
63 |
64 |
--------------------------------------------------------------------------------
/inc/common.h:
--------------------------------------------------------------------------------
1 | #ifndef _COMMON_H_
2 | #define _COMMON_H_
3 |
4 | #include
5 | #include
6 | #include
7 | #include
8 |
9 | // define the type of aircraft. Allowed types are QUAD_X, QUAD_PLUS, TRI, TRI_TEST and QUAD_TEST
10 | #define TRI
11 |
12 | //Magic macro to check enum size
13 | //#define ctassert(n,e) extern unsigned char n[(e)?0:-1]
14 | #define ctassert(COND,MSG) typedef char static_assertion_##MSG[(COND)?1:-1]
15 | #define usleep _usleep
16 | #define UNUSED(x) (void)(x)
17 |
18 | #define TRUE 1
19 | #define FALSE 0
20 |
21 |
22 | //typedef enum { false, true } bool;
23 |
24 | typedef int8_t s8;
25 | typedef int16_t s16;
26 | typedef int32_t s32;
27 | typedef uint8_t u8;
28 | typedef uint16_t u16;
29 | typedef uint32_t u32;
30 | typedef uint64_t u64;
31 |
32 | extern u8 payload[9];
33 |
34 | // SPI functions
35 | void SPI_ProtoInit(void);
36 |
37 | /* Mixer functions */
38 | //void MIXER_CalcChannels();
39 |
40 | enum TxPower {
41 | TXPOWER_100uW,
42 | TXPOWER_300uW,
43 | TXPOWER_1mW,
44 | TXPOWER_3mW,
45 | TXPOWER_10mW,
46 | TXPOWER_30mW,
47 | TXPOWER_100mW,
48 | TXPOWER_150mW,
49 | TXPOWER_LAST,
50 | };
51 |
52 | struct mcu_pin {
53 | u32 port;
54 | u16 pin;
55 | };
56 |
57 | void _usleep(u32 x);
58 |
59 | /*
60 | // Misc
61 | void Delay(u32 count);
62 | u32 Crc(const void *buffer, u32 size);
63 | const char *utf8_to_u32(const char *str, u32 *ch);
64 | int exact_atoi(const char *str); //Like atoi but will not decode a number followed by non-number
65 | size_t strlcpy(char* dst, const char* src, size_t bufsize);
66 | void tempstring_cpy(const char* src);
67 | int fexists(const char *file);
68 | u32 rand32_r(u32 *seed, u8 update); //LFSR based PRNG
69 | u32 rand32(); //LFSR based PRNG
70 | extern volatile u8 priority_ready;
71 | void medium_priority_cb();
72 | void debug_timing(u32 type, int startend); //This is only defined if TIMING_DEBUG is defined
73 | // Battery
74 | #define BATTERY_CRITICAL 0x01
75 | #define BATTERY_LOW 0x02
76 | u8 BATTERY_Check();
77 | */
78 | #endif
79 |
--------------------------------------------------------------------------------
/cleanflight/drivers/system.c:
--------------------------------------------------------------------------------
1 | /*
2 | * This file is part of Cleanflight.
3 | *
4 | * Cleanflight 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 | * Cleanflight 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 | *
14 | * You should have received a copy of the GNU General Public License
15 | * along with Cleanflight. If not, see .
16 | */
17 |
18 | #include
19 | #include
20 | #include
21 |
22 | #include
23 |
24 | #include "build_config.h"
25 |
26 | #include "gpio.h"
27 | #include "nvic.h"
28 |
29 | #include "system.h"
30 |
31 | #include "stm32f0xx.h"
32 |
33 | // cycles per microsecond
34 | static volatile uint32_t usTicks = 0;
35 | // current uptime for 1kHz systick timer. will rollover after 49 days. hopefully we won't care.
36 | static volatile uint32_t sysTickUptime = 0;
37 |
38 | static void cycleCounterInit(void)
39 | {
40 | RCC_ClocksTypeDef clocks;
41 | RCC_GetClocksFreq(&clocks);
42 | usTicks = clocks.SYSCLK_Frequency / 1000000;
43 | }
44 |
45 | // SysTick
46 | void SysTick_Handler(void)
47 | {
48 | sysTickUptime++;
49 | }
50 |
51 | // Return system uptime in microseconds (rollover in 70minutes)
52 | uint32_t micros(void)
53 | {
54 | register uint32_t ms, cycle_cnt;
55 | do {
56 | ms = sysTickUptime;
57 | cycle_cnt = SysTick->VAL;
58 | } while (ms != sysTickUptime);
59 | return (ms * 1000) + (usTicks * 1000 - cycle_cnt) / usTicks;
60 | }
61 |
62 | // Return system uptime in milliseconds (rollover in 49 days)
63 | uint32_t millis(void)
64 | {
65 | return sysTickUptime;
66 | }
67 |
68 | void systemInit(void)
69 | {
70 | /*
71 | // Configure NVIC preempt/priority groups
72 | NVIC_PriorityGroupConfig(NVIC_PRIORITY_GROUPING);
73 |
74 | RCC_ClearFlag();
75 |
76 |
77 | enableGPIOPowerUsageAndNoiseReductions();
78 | */
79 |
80 | // Init cycle counter
81 | cycleCounterInit();
82 |
83 | // SysTick
84 | SysTick_Config(SystemCoreClock / 1000);
85 | }
86 |
87 | void delayMicroseconds(uint32_t us)
88 | {
89 | uint32_t now = micros();
90 | while (micros() - now < us);
91 | }
92 |
93 | void delay(uint32_t ms)
94 | {
95 | while (ms--)
96 | delayMicroseconds(1000);
97 | }
98 |
99 |
--------------------------------------------------------------------------------
/src/spi.c:
--------------------------------------------------------------------------------
1 |
2 | #include "common.h"
3 | #include "stm32f0xx.h"
4 |
5 | void spi_init(void) {
6 |
7 | /* What it should be:
8 |
9 | MSB first - default
10 | 8 Bits packet length - cr2 ds bits 0-2
11 | CPOL=0 (clock low when inactive) - default
12 | CPHA=0 (data valid on leading edge) - default
13 | Enable is active low - default
14 |
15 | */
16 |
17 | // enable the clock for the spi peripheral
18 | RCC->APB2ENR |= RCC_APB2ENR_SPI1EN;
19 |
20 | // reset both config registers
21 | SPI1->CR1 = 0x0000;
22 | SPI1->CR2 = 0x0000;
23 |
24 | SPI1->CR1 = SPI_CR1_MSTR
25 | | SPI_CR1_BR_2;
26 |
27 | SPI1->CR2 = SPI_CR2_SSOE
28 | | SPI_CR2_FRXTH
29 | | SPI_CR2_DS_2
30 | | SPI_CR2_DS_1
31 | | SPI_CR2_DS_0;
32 |
33 | /*
34 | */
35 |
36 | // SPI enable
37 | SPI1->CR1 |= SPI_CR1_SPE;
38 | }
39 |
40 | /*---------------------------------------------------------------------------*/
41 | /** @brief SPI Data Write.
42 |
43 | Data is written to the SPI interface.
44 |
45 | @param[in] spi Unsigned int32. SPI peripheral identifier @ref spi_reg_base.
46 | @param[in] data Unsigned int16. 8 or 16 bit data to be written.
47 | */
48 |
49 | void spi_write(u8 data)
50 | {
51 | uint32_t spixbase = 0x00;
52 |
53 | spixbase = (uint32_t)SPI1;
54 | spixbase += 0x0C;
55 |
56 | *(__IO uint8_t *) spixbase = data;
57 | }
58 |
59 | /*---------------------------------------------------------------------------*/
60 | /** @brief SPI Data Read.
61 |
62 | Data is read from the SPI interface after the incoming transfer has finished.
63 |
64 | @param[in] spi Unsigned int32. SPI peripheral identifier @ref spi_reg_base.
65 | @returns data Unsigned int16. 8 or 16 bit data.
66 | */
67 |
68 | u8 spi_read(void)
69 | {
70 | uint32_t spixbase = 0x00;
71 |
72 | spixbase = (uint32_t)SPI1;
73 | spixbase += 0x0C;
74 |
75 | //while (!(SPI1->SR & SPI_SR_RXNE));
76 |
77 | return *(__IO uint8_t *) spixbase;
78 | }
79 |
80 | /*---------------------------------------------------------------------------*/
81 | /** @brief SPI Data Write and Read Exchange.
82 |
83 | Data is written to the SPI interface, then a read is done after the incoming
84 | transfer has finished.
85 |
86 | @param[in] spi Unsigned int32. SPI peripheral identifier @ref spi_reg_base.
87 | @param[in] data Unsigned int16. 8 or 16 bit data to be written.
88 | @returns data Unsigned int16. 8 or 16 bit data.
89 | */
90 |
91 | u8 spi_xfer(u8 data)
92 | {
93 | spi_write(data);
94 |
95 | /* Wait for transfer finished. */
96 | while (!(SPI1->SR & SPI_SR_RXNE));
97 |
98 | return spi_read();
99 | }
100 |
101 |
--------------------------------------------------------------------------------
/inc/system_stm32f0xx.h:
--------------------------------------------------------------------------------
1 | /**
2 | ******************************************************************************
3 | * @file system_stm32f0xx.h
4 | * @author MCD Application Team
5 | * @version V1.0.0
6 | * @date 23-March-2012
7 | * @brief CMSIS Cortex-M0 Device Peripheral Access Layer System Header File.
8 | ******************************************************************************
9 | * @attention
10 | *
11 | * © COPYRIGHT 2012 STMicroelectronics
12 | *
13 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License");
14 | * You may not use this file except in compliance with the License.
15 | * You may obtain a copy of the License at:
16 | *
17 | * http://www.st.com/software_license_agreement_liberty_v2
18 | *
19 | * Unless required by applicable law or agreed to in writing, software
20 | * distributed under the License is distributed on an "AS IS" BASIS,
21 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
22 | * See the License for the specific language governing permissions and
23 | * limitations under the License.
24 | *
25 | ******************************************************************************
26 | */
27 |
28 | /** @addtogroup CMSIS
29 | * @{
30 | */
31 |
32 | /** @addtogroup stm32f0xx_system
33 | * @{
34 | */
35 |
36 | /**
37 | * @brief Define to prevent recursive inclusion
38 | */
39 | #ifndef __SYSTEM_STM32F0XX_H
40 | #define __SYSTEM_STM32F0XX_H
41 |
42 | #ifdef __cplusplus
43 | extern "C" {
44 | #endif
45 |
46 | /** @addtogroup STM32F0xx_System_Includes
47 | * @{
48 | */
49 |
50 | /**
51 | * @}
52 | */
53 |
54 |
55 | /** @addtogroup STM32F0xx_System_Exported_types
56 | * @{
57 | */
58 |
59 | extern uint32_t SystemCoreClock; /*!< System Clock Frequency (Core Clock) */
60 |
61 | /**
62 | * @}
63 | */
64 |
65 | /** @addtogroup STM32F0xx_System_Exported_Constants
66 | * @{
67 | */
68 |
69 | /**
70 | * @}
71 | */
72 |
73 | /** @addtogroup STM32F0xx_System_Exported_Macros
74 | * @{
75 | */
76 |
77 | /**
78 | * @}
79 | */
80 |
81 | /** @addtogroup STM32F0xx_System_Exported_Functions
82 | * @{
83 | */
84 |
85 | extern void SystemInit(void);
86 | extern void SystemCoreClockUpdate(void);
87 | /**
88 | * @}
89 | */
90 |
91 | #ifdef __cplusplus
92 | }
93 | #endif
94 |
95 | #endif /*__SYSTEM_STM32F0XX_H */
96 |
97 | /**
98 | * @}
99 | */
100 |
101 | /**
102 | * @}
103 | */
104 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
105 |
--------------------------------------------------------------------------------
/linker/stm32f0_linker.ld:
--------------------------------------------------------------------------------
1 | /******************************************************************************
2 | * This linker file was developed by Hussam Al-Hertani. Please use freely as
3 | * long as you leave this header in place. The author is not responsible for any
4 | * damage or liability that this file might cause.
5 | ******************************************************************************/
6 |
7 | /* Entry Point */
8 | ENTRY(Reset_Handler)
9 |
10 | /* Specify the memory areas */
11 | MEMORY
12 | {
13 | FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 0x04000 /*16K*/
14 | RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 0x01000 /*4K*/
15 | }
16 |
17 | /* define stack size and heap size here */
18 | stack_size = 3072;
19 | heap_size = 0;
20 |
21 | /* define beginning and ending of stack */
22 | _stack_start = ORIGIN(RAM)+LENGTH(RAM);
23 | _stack_end = _stack_start - stack_size;
24 |
25 | /* Define output sections */
26 | SECTIONS
27 | {
28 | /* The startup code goes first into FLASH */
29 | .isr_vector :
30 | {
31 | . = ALIGN(4);
32 | KEEP(*(.isr_vector)) /* Startup code */
33 | . = ALIGN(4);
34 | } >FLASH
35 |
36 | /* The program code and other data goes into FLASH */
37 | .text :
38 | {
39 | . = ALIGN(4);
40 | *(.text) /* .text sections (code) */
41 | *(.text*) /* .text* sections (code) */
42 | *(.rodata) /* .rodata sections (constants, strings, etc.) */
43 | *(.rodata*) /* .rodata* sections (constants, strings, etc.) */
44 | *(.glue_7) /* glue arm to thumb code */
45 | *(.glue_7t) /* glue thumb to arm code */
46 | . = ALIGN(4);
47 | _etext = .; /* define a global symbols at end of code */
48 | } >FLASH
49 |
50 | .ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH
51 | .ARM : {
52 | __exidx_start = .;
53 | *(.ARM.exidx*)
54 | __exidx_end = .;
55 | } >FLASH
56 |
57 | /* used by the startup to initialize data */
58 | _sidata = .;
59 |
60 | /* Initialized data sections goes into RAM, load LMA copy after code */
61 | .data : AT ( _sidata )
62 | {
63 | . = ALIGN(4);
64 | _sdata = .; /* create a global symbol at data start */
65 | *(.data) /* .data sections */
66 | *(.data*) /* .data* sections */
67 |
68 | . = ALIGN(4);
69 | _edata = .; /* define a global symbol at data end */
70 | } >RAM
71 |
72 | /* Uninitialized data section */
73 | . = ALIGN(4);
74 | .bss :
75 | {
76 | /* Used by the startup in order to initialize the .bss secion */
77 | _sbss = .; /* define a global symbol at bss start */
78 | __bss_start__ = _sbss;
79 | *(.bss)
80 | *(.bss*)
81 | *(COMMON)
82 |
83 | . = ALIGN(4);
84 | _ebss = .; /* define a global symbol at bss end */
85 | __bss_end__ = _ebss;
86 | } >RAM
87 |
88 | . = ALIGN(4);
89 | .heap :
90 | {
91 | _heap_start = .;
92 | . = . + heap_size;
93 | _heap_end = .;
94 | } > RAM
95 |
96 |
97 | /* Remove information from the standard libraries */
98 | /DISCARD/ :
99 | {
100 | libc.a ( * )
101 | libm.a ( * )
102 | libgcc.a ( * )
103 | }
104 |
105 | .ARM.attributes 0 : { *(.ARM.attributes) }
106 | }
107 |
--------------------------------------------------------------------------------
/inc/stm32f0xx_conf.h:
--------------------------------------------------------------------------------
1 | /**
2 | ******************************************************************************
3 | * @file IO_Toggle/stm32f0xx_conf.h
4 | * @author MCD Application Team
5 | * @version V1.0.0
6 | * @date 23-March-2012
7 | * @brief Library configuration file.
8 | ******************************************************************************
9 | * @attention
10 | *
11 | * © COPYRIGHT 2012 STMicroelectronics
12 | *
13 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License");
14 | * You may not use this file except in compliance with the License.
15 | * You may obtain a copy of the License at:
16 | *
17 | * http://www.st.com/software_license_agreement_liberty_v2
18 | *
19 | * Unless required by applicable law or agreed to in writing, software
20 | * distributed under the License is distributed on an "AS IS" BASIS,
21 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
22 | * See the License for the specific language governing permissions and
23 | * limitations under the License.
24 | *
25 | ******************************************************************************
26 | */
27 |
28 | /* Define to prevent recursive inclusion -------------------------------------*/
29 | #ifndef __STM32F0XX_CONF_H
30 | #define __STM32F0XX_CONF_H
31 |
32 | /* Includes ------------------------------------------------------------------*/
33 | /* Comment the line below to disable peripheral header file inclusion */
34 | //#include "stm32f0xx_adc.h"
35 | //#include "stm32f0xx_cec.h"
36 | //#include "stm32f0xx_crc.h"
37 | //#include "stm32f0xx_comp.h"
38 | //#include "stm32f0xx_dac.h"
39 | //#include "stm32f0xx_dbgmcu.h"
40 | //#include "stm32f0xx_dma.h"
41 | //#include "stm32f0xx_exti.h"
42 | //#include "stm32f0xx_flash.h"
43 | //#include "stm32f0xx_gpio.h"
44 | //#include "stm32f0xx_syscfg.h"
45 | //#include "stm32f0xx_i2c.h"
46 | //#include "stm32f0xx_iwdg.h"
47 | //#include "stm32f0xx_pwr.h"
48 | //#include "stm32f0xx_rcc.h"
49 | //#include "stm32f0xx_rtc.h"
50 | //#include "stm32f0xx_spi.h"
51 | //#include "stm32f0xx_tim.h"
52 | //#include "stm32f0xx_usart.h"
53 | //#include "stm32f0xx_wwdg.h"
54 | //#include "stm32f0xx_misc.h" /* High level functions for NVIC and SysTick (add-on to CMSIS functions) */
55 |
56 | /* Exported types ------------------------------------------------------------*/
57 | /* Exported constants --------------------------------------------------------*/
58 | /* Uncomment the line below to expanse the "assert_param" macro in the
59 | Standard Peripheral Library drivers code */
60 | /* #define USE_FULL_ASSERT 1 */
61 |
62 | /* Exported macro ------------------------------------------------------------*/
63 | #ifdef USE_FULL_ASSERT
64 |
65 | /**
66 | * @brief The assert_param macro is used for function's parameters check.
67 | * @param expr: If expr is false, it calls assert_failed function which reports
68 | * the name of the source file and the source line number of the call
69 | * that failed. If expr is true, it returns no value.
70 | * @retval None
71 | */
72 | #define assert_param(expr) ((expr) ? (void)0 : assert_failed((uint8_t *)__FILE__, __LINE__))
73 | /* Exported functions ------------------------------------------------------- */
74 | void assert_failed(uint8_t* file, uint32_t line);
75 | #else
76 | #define assert_param(expr) ((void)0)
77 | #endif /* USE_FULL_ASSERT */
78 |
79 | #endif /* __STM32F0XX_CONF_H */
80 |
81 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
82 |
--------------------------------------------------------------------------------
/inc/iface_nrf24l01.h:
--------------------------------------------------------------------------------
1 | #ifndef _IFACE_NRF24L01_H_
2 | #define _IFACE_NRF24L01_H_
3 |
4 | /* Instruction Mnemonics */
5 | #define R_REGISTER 0x00
6 | #define W_REGISTER 0x20
7 | #define REGISTER_MASK 0x1F
8 | #define ACTIVATE 0x50
9 | #define R_RX_PL_WID 0x60
10 | #define R_RX_PAYLOAD 0x61
11 | #define W_TX_PAYLOAD 0xA0
12 | #define W_ACK_PAYLOAD 0xA8
13 | #define FLUSH_TX 0xE1
14 | #define FLUSH_RX 0xE2
15 | #define REUSE_TX_PL 0xE3
16 | #define NOP 0xFF
17 |
18 | // Register map
19 | enum {
20 | NRF24L01_00_CONFIG = 0x00,
21 | NRF24L01_01_EN_AA = 0x01,
22 | NRF24L01_02_EN_RXADDR = 0x02,
23 | NRF24L01_03_SETUP_AW = 0x03,
24 | NRF24L01_04_SETUP_RETR = 0x04,
25 | NRF24L01_05_RF_CH = 0x05,
26 | NRF24L01_06_RF_SETUP = 0x06,
27 | NRF24L01_07_STATUS = 0x07,
28 | NRF24L01_08_OBSERVE_TX = 0x08,
29 | NRF24L01_09_CD = 0x09,
30 | NRF24L01_0A_RX_ADDR_P0 = 0x0A,
31 | NRF24L01_0B_RX_ADDR_P1 = 0x0B,
32 | NRF24L01_0C_RX_ADDR_P2 = 0x0C,
33 | NRF24L01_0D_RX_ADDR_P3 = 0x0D,
34 | NRF24L01_0E_RX_ADDR_P4 = 0x0E,
35 | NRF24L01_0F_RX_ADDR_P5 = 0x0F,
36 | NRF24L01_10_TX_ADDR = 0x10,
37 | NRF24L01_11_RX_PW_P0 = 0x11,
38 | NRF24L01_12_RX_PW_P1 = 0x12,
39 | NRF24L01_13_RX_PW_P2 = 0x13,
40 | NRF24L01_14_RX_PW_P3 = 0x14,
41 | NRF24L01_15_RX_PW_P4 = 0x15,
42 | NRF24L01_16_RX_PW_P5 = 0x16,
43 | NRF24L01_17_FIFO_STATUS = 0x17,
44 | NRF24L01_1C_DYNPD = 0x1C,
45 | NRF24L01_1D_FEATURE = 0x1D,
46 | //Instructions
47 | NRF24L01_61_RX_PAYLOAD = 0x61,
48 | NRF24L01_A0_TX_PAYLOAD = 0xA0,
49 | NRF24L01_E1_FLUSH_TX = 0xE1,
50 | NRF24L01_E2_FLUSH_RX = 0xE2,
51 | NRF24L01_E3_REUSE_TX_PL = 0xE3,
52 | NRF24L01_50_ACTIVATE = 0x50,
53 | NRF24L01_60_R_RX_PL_WID = 0x60,
54 | NRF24L01_B0_TX_PYLD_NOACK = 0xB0,
55 | NRF24L01_FF_NOP = 0xFF,
56 | NRF24L01_A8_W_ACK_PAYLOAD0 = 0xA8,
57 | NRF24L01_A8_W_ACK_PAYLOAD1 = 0xA9,
58 | NRF24L01_A8_W_ACK_PAYLOAD2 = 0xAA,
59 | NRF24L01_A8_W_ACK_PAYLOAD3 = 0xAB,
60 | NRF24L01_A8_W_ACK_PAYLOAD4 = 0xAC,
61 | NRF24L01_A8_W_ACK_PAYLOAD5 = 0xAD,
62 | };
63 |
64 | // Bit mnemonics
65 | enum {
66 | NRF24L01_00_MASK_RX_DR = 6,
67 | NRF24L01_00_MASK_TX_DS = 5,
68 | NRF24L01_00_MASK_MAX_RT = 4,
69 | NRF24L01_00_EN_CRC = 3,
70 | NRF24L01_00_CRCO = 2,
71 | NRF24L01_00_PWR_UP = 1,
72 | NRF24L01_00_PRIM_RX = 0,
73 |
74 | NRF24L01_07_RX_DR = 6,
75 | NRF24L01_07_TX_DS = 5,
76 | NRF24L01_07_MAX_RT = 4,
77 |
78 | NRF2401_1D_EN_DYN_ACK = 0,
79 | NRF2401_1D_EN_ACK_PAY = 1,
80 | NRF2401_1D_EN_DPL = 2,
81 | };
82 |
83 | // Bitrates
84 | enum {
85 | NRF24L01_BR_1M = 0,
86 | NRF24L01_BR_2M,
87 | NRF24L01_BR_250K,
88 | NRF24L01_BR_RSVD
89 | };
90 |
91 |
92 | enum TXRX_State {
93 | TXRX_OFF,
94 | TX_EN,
95 | RX_EN,
96 | };
97 |
98 | void NRF24L01_Initialize(void);
99 | int NRF24L01_Reset(void);
100 | u8 NRF24L01_WriteReg(u8 reg, u8 data);
101 | u8 NRF24L01_WriteRegisterMulti(u8 reg, const u8 data[], u8 length);
102 | u8 NRF24L01_WritePayload(u8 *data, u8 len);
103 | u8 NRF24L01_ReadReg(u8 reg);
104 | u8 NRF24L01_ReadRegisterMulti(u8 reg, u8 data[], u8 length);
105 | u8 NRF24L01_ReadPlWid(void);
106 | u8 NRF24L01_ReadPayload(u8 *data, u8 len);
107 |
108 | u8 NRF24L01_FlushTx(void);
109 | u8 NRF24L01_FlushRx(void);
110 | u8 NRF24L01_Activate(u8 code);
111 |
112 |
113 | // Bitrate 0 - 1Mbps, 1 - 2Mbps, 3 - 250K (for nRF24L01+)
114 | u8 NRF24L01_SetBitrate(u8 bitrate);
115 |
116 | u8 NRF24L01_SetPower(u8 power);
117 | void NRF24L01_SetTxRxMode(enum TXRX_State);
118 | int NRF24L01_Reset(void);
119 |
120 | // To enable radio transmit after WritePayload you need to turn the radio
121 | //void NRF24L01_PulseCE();
122 |
123 |
124 | #endif
125 |
--------------------------------------------------------------------------------
/cleanflight/drivers/gpio.h:
--------------------------------------------------------------------------------
1 | /*
2 | * This file is part of Cleanflight.
3 | *
4 | * Cleanflight 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 | * Cleanflight 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 | *
14 | * You should have received a copy of the GNU General Public License
15 | * along with Cleanflight. If not, see .
16 | */
17 |
18 | #pragma once
19 |
20 | #ifdef STM32F031
21 | typedef enum
22 | {
23 | Mode_AIN = 0x0,
24 | Mode_IN_FLOATING = 0x04,
25 | Mode_IPD = 0x28,
26 | Mode_IPU = 0x48,
27 | Mode_Out_OD = 0x14,
28 | Mode_Out_PP = 0x10,
29 | Mode_AF_OD = 0x1C,
30 | Mode_AF_PP = 0x18
31 | } GPIO_Mode;
32 | #endif
33 |
34 | #ifdef STM32F10X
35 | typedef enum
36 | {
37 | Mode_AIN = 0x0,
38 | Mode_IN_FLOATING = 0x04,
39 | Mode_IPD = 0x28,
40 | Mode_IPU = 0x48,
41 | Mode_Out_OD = 0x14,
42 | Mode_Out_PP = 0x10,
43 | Mode_AF_OD = 0x1C,
44 | Mode_AF_PP = 0x18
45 | } GPIO_Mode;
46 | #endif
47 |
48 | #ifdef STM32F303xC
49 |
50 | /*
51 | typedef enum
52 | {
53 | GPIO_Mode_IN = 0x00, // GPIO Input Mode
54 | GPIO_Mode_OUT = 0x01, // GPIO Output Mode
55 | GPIO_Mode_AF = 0x02, // GPIO Alternate function Mode
56 | GPIO_Mode_AN = 0x03 // GPIO Analog In/Out Mode
57 | }GPIOMode_TypeDef;
58 |
59 | typedef enum
60 | {
61 | GPIO_OType_PP = 0x00,
62 | GPIO_OType_OD = 0x01
63 | }GPIOOType_TypeDef;
64 |
65 | typedef enum
66 | {
67 | GPIO_PuPd_NOPULL = 0x00,
68 | GPIO_PuPd_UP = 0x01,
69 | GPIO_PuPd_DOWN = 0x02
70 | }GPIOPuPd_TypeDef;
71 | */
72 |
73 | typedef enum
74 | {
75 | Mode_AIN = (GPIO_PuPd_NOPULL << 2) | GPIO_Mode_AN,
76 | Mode_IN_FLOATING = (GPIO_PuPd_NOPULL << 2) | GPIO_Mode_IN,
77 | Mode_IPD = (GPIO_PuPd_DOWN << 2) | GPIO_Mode_IN,
78 | Mode_IPU = (GPIO_PuPd_UP << 2) | GPIO_Mode_IN,
79 | Mode_Out_OD = (GPIO_OType_OD << 4) | GPIO_Mode_OUT,
80 | Mode_Out_PP = (GPIO_OType_PP << 4) | GPIO_Mode_OUT,
81 | Mode_AF_OD = (GPIO_OType_OD << 4) | GPIO_Mode_AF,
82 | Mode_AF_PP = (GPIO_OType_PP << 4) | GPIO_Mode_AF,
83 | Mode_AF_PP_PD = (GPIO_OType_PP << 4) | (GPIO_PuPd_DOWN << 2) | GPIO_Mode_AF,
84 | Mode_AF_PP_PU = (GPIO_OType_PP << 4) | (GPIO_PuPd_UP << 2) | GPIO_Mode_AF
85 | } GPIO_Mode;
86 | #endif
87 |
88 | typedef enum
89 | {
90 | Speed_10MHz = 1,
91 | Speed_2MHz,
92 | Speed_50MHz
93 | } GPIO_Speed;
94 |
95 | typedef enum
96 | {
97 | Pin_0 = 0x0001,
98 | Pin_1 = 0x0002,
99 | Pin_2 = 0x0004,
100 | Pin_3 = 0x0008,
101 | Pin_4 = 0x0010,
102 | Pin_5 = 0x0020,
103 | Pin_6 = 0x0040,
104 | Pin_7 = 0x0080,
105 | Pin_8 = 0x0100,
106 | Pin_9 = 0x0200,
107 | Pin_10 = 0x0400,
108 | Pin_11 = 0x0800,
109 | Pin_12 = 0x1000,
110 | Pin_13 = 0x2000,
111 | Pin_14 = 0x4000,
112 | Pin_15 = 0x8000,
113 | Pin_All = 0xFFFF
114 | } GPIO_Pin;
115 |
116 | typedef struct
117 | {
118 | uint16_t pin;
119 | GPIO_Mode mode;
120 | GPIO_Speed speed;
121 | } gpio_config_t;
122 |
123 | static inline void digitalHi(GPIO_TypeDef *p, uint16_t i) { p->BSRR = i; }
124 | static inline void digitalLo(GPIO_TypeDef *p, uint16_t i) { p->BRR = i; }
125 | static inline void digitalToggle(GPIO_TypeDef *p, uint16_t i) { p->ODR ^= i; }
126 | static inline uint16_t digitalIn(GPIO_TypeDef *p, uint16_t i) {return p->IDR & i; }
127 |
128 | void gpioInit(GPIO_TypeDef *gpio, gpio_config_t *config);
129 | void gpioExtiLineConfig(uint8_t portsrc, uint8_t pinsrc);
130 | void gpioPinRemapConfig(uint32_t remap, bool enable);
131 |
--------------------------------------------------------------------------------
/src/pid.c:
--------------------------------------------------------------------------------
1 | /**
2 | * || ____ _ __
3 | * +------+ / __ )(_) /_______________ _____ ___
4 | * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
5 | * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
6 | * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
7 | *
8 | * Crazyflie control firmware
9 | *
10 | * Copyright (C) 2011-2012 Bitcraze AB
11 | *
12 | * This program is free software: you can redistribute it and/or modify
13 | * it under the terms of the GNU General Public License as published by
14 | * the Free Software Foundation, in version 3.
15 | *
16 | * This program is distributed in the hope that it will be useful,
17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19 | * GNU General Public License for more details.
20 | *
21 | * You should have received a copy of the GNU General Public License
22 | * along with this program. If not, see .
23 | *
24 | *
25 | * pid.c - implementation of the PID regulator
26 | */
27 | //#include "stm32f10x_conf.h"
28 | //#include
29 | //
30 | //#include "FreeRTOS.h"
31 | //#include "task.h"
32 | #include "pid.h"
33 | //#include "led.h"
34 | //#include "motors.h"
35 | #define TRUE 1
36 | #define FALSE 0
37 |
38 | void pidInit(PidObject* pid, const float desired, const float kp,
39 | const float ki, const float kd, const float dt)
40 | {
41 | pid->error = 0;
42 | pid->prevError = 0;
43 | pid->integ = 0;
44 | pid->deriv = 0;
45 | pid->desired = desired;
46 | pid->kp = kp;
47 | pid->ki = ki;
48 | pid->kd = kd;
49 | pid->iLimit = DEFAULT_PID_INTEGRATION_LIMIT;
50 | pid->iLimitLow = -DEFAULT_PID_INTEGRATION_LIMIT;
51 | pid->dt = dt;
52 | }
53 |
54 | float pidUpdate(PidObject* pid, const float measured, const bool updateError)
55 | {
56 | float output;
57 |
58 | if (updateError)
59 | {
60 | pid->error = pid->desired - measured;
61 | }
62 |
63 | pid->integ += pid->error * pid->dt;
64 | if (pid->integ > pid->iLimit)
65 | {
66 | pid->integ = pid->iLimit;
67 | }
68 | else if (pid->integ < pid->iLimitLow)
69 | {
70 | pid->integ = pid->iLimitLow;
71 | }
72 |
73 | pid->deriv = (pid->error - pid->prevError) / pid->dt;
74 |
75 | pid->outP = pid->kp * pid->error;
76 | pid->outI = pid->ki * pid->integ;
77 | pid->outD = pid->kd * pid->deriv;
78 |
79 | output = pid->outP + pid->outI + pid->outD;
80 |
81 | pid->prevError = pid->error;
82 |
83 | return output;
84 | }
85 |
86 | void pidSetIntegralLimit(PidObject* pid, const float limit) {
87 | pid->iLimit = limit;
88 | }
89 |
90 |
91 | void pidSetIntegralLimitLow(PidObject* pid, const float limitLow) {
92 | pid->iLimitLow = limitLow;
93 | }
94 |
95 | void pidReset(PidObject* pid)
96 | {
97 | pid->error = 0;
98 | pid->prevError = 0;
99 | pid->integ = 0;
100 | pid->deriv = 0;
101 | }
102 |
103 | void pidSetError(PidObject* pid, const float error)
104 | {
105 | pid->error = error;
106 | }
107 |
108 | void pidSetDesired(PidObject* pid, const float desired)
109 | {
110 | pid->desired = desired;
111 | }
112 |
113 | float pidGetDesired(PidObject* pid)
114 | {
115 | return pid->desired;
116 | }
117 |
118 | bool pidIsActive(PidObject* pid)
119 | {
120 | bool isActive = TRUE;
121 |
122 | if (pid->kp < 0.0001 && pid->ki < 0.0001 && pid->kd < 0.0001)
123 | {
124 | isActive = FALSE;
125 | }
126 |
127 | return isActive;
128 | }
129 |
130 | void pidSetKp(PidObject* pid, const float kp)
131 | {
132 | pid->kp = kp;
133 | }
134 |
135 | void pidSetKi(PidObject* pid, const float ki)
136 | {
137 | pid->ki = ki;
138 | }
139 |
140 | void pidSetKd(PidObject* pid, const float kd)
141 | {
142 | pid->kd = kd;
143 | }
144 | void pidSetDt(PidObject* pid, const float dt) {
145 | pid->dt = dt;
146 | }
147 |
--------------------------------------------------------------------------------
/src/gpio.c:
--------------------------------------------------------------------------------
1 | #include "stm32f0xx.h"
2 | #include "common.h"
3 | #include "nf_gpio.h"
4 | /*
5 | void GPIO_PinAFConfig(GPIO_TypeDef* GPIOx, uint16_t GPIO_PinSource, uint8_t GPIO_AF)
6 | {
7 | uint32_t temp = 0x00;
8 | uint32_t temp_2 = 0x00;
9 |
10 |
11 | temp = ((uint32_t)(GPIO_AF) << ((uint32_t)((uint32_t)GPIO_PinSource & (uint32_t)0x07) * 4));
12 | GPIOx->AFR[GPIO_PinSource >> 0x03] &= ~((uint32_t)0xF << ((uint32_t)((uint32_t)GPIO_PinSource & (uint32_t)0x07) * 4));
13 | temp_2 = GPIOx->AFR[GPIO_PinSource >> 0x03] | temp;
14 | GPIOx->AFR[GPIO_PinSource >> 0x03] = temp_2;
15 | }
16 | */
17 |
18 | void gpio_init(void) {
19 |
20 | /*
21 |
22 | PORT A
23 | ------
24 |
25 | PA0 - unused
26 | PA1 - unused
27 | PA2 - unused
28 | PA3 - TIM2_CH4 - Motor NW (marked "B")
29 | PA4 - SPI1_NSS - BK2423
30 | PA5 - SPI1_SCK - BK2423
31 | PA6 - SPI1_MISO - BK2423
32 | PA7 - SPI1_MOSI - BK2423
33 | PA8 - TIM1_CH1 - Motor SW (marked "L")
34 | PA9 - unused
35 | PA10 - unused
36 | PA11 - TIM1_CH4 - Motor SE (marked "R")
37 | PA12 - unused
38 | PA13 - SWDAT
39 | PA14 - SWCLK
40 | PA15 - LEDs E
41 |
42 | PORT B
43 | ------
44 |
45 | PB0 - unused
46 | PB1 - unused
47 | PB2 - LEDs W
48 | PB3 - unused
49 | PB4 - unused
50 | PB5 - unused
51 | PB6 - I2C1_SCL - MPU6050
52 | PB7 - I2C1_SDA - MPU6050
53 | PB8 - TIM16_CH1 - Motor NE (marked "F")
54 |
55 | */
56 |
57 | // GPIOA and GPIOB Periph clock enable
58 | RCC->AHBENR |= RCC_AHBENR_GPIOAEN;
59 | RCC->AHBENR |= RCC_AHBENR_GPIOBEN;
60 |
61 | // Ensure maximum speed setting (even though it is unnecessary)
62 | GPIOA->OSPEEDR = 0xffffffff;
63 | GPIOB->OSPEEDR = 0xffffffff;
64 |
65 | // Ensure all pull up pull down resistors are disabled
66 | GPIOA->PUPDR = 0;
67 | GPIOB->PUPDR = 0;
68 |
69 | GPIOA->OTYPER = 0; // output type = push pull
70 | GPIOB->OTYPER = 0; // output type = push pull
71 |
72 | // Pin Mode Register settings
73 | GPIOA->MODER |= (GPIO_MODER_MODER3_1) ; // AF - Timer
74 | GPIOA->MODER |= (GPIO_MODER_MODER4_0) ; // GP Output - NSS
75 | GPIOA->MODER |= (GPIO_MODER_MODER5_1) ; // AF - SCK
76 | GPIOA->MODER |= (GPIO_MODER_MODER6_1) ; // AF - MISO
77 | GPIOA->MODER |= (GPIO_MODER_MODER7_1) ; // AF - MOSI
78 | GPIOA->MODER |= (GPIO_MODER_MODER8_1) ; // AF
79 | GPIOA->MODER |= (GPIO_MODER_MODER11_1) ; // AF
80 | GPIOA->MODER |= (GPIO_MODER_MODER15_0) ; // GP Output
81 |
82 | GPIOB->MODER |= (GPIO_MODER_MODER2_0) ; // GP Ooutput
83 | GPIOB->MODER |= (GPIO_MODER_MODER6_1) ; // AF
84 | GPIOB->MODER |= (GPIO_MODER_MODER7_1) ; // AF
85 | GPIOB->MODER |= (GPIO_MODER_MODER8_1) ; // AF
86 |
87 | // enable pulldowns for motor pins
88 | GPIOA->PUPDR |= (2<<3);
89 | GPIOA->PUPDR |= (2<<8);
90 | GPIOA->PUPDR |= (2<<11);
91 | GPIOB->PUPDR |= (2<<8);
92 |
93 | // setup gpio alternate functions
94 |
95 | // SPI
96 | gpio_set(GPIOA, 4);
97 | GPIO_PinAFConfig(GPIOA, 5, 0);
98 | GPIO_PinAFConfig(GPIOA, 6, 0);
99 | GPIO_PinAFConfig(GPIOA, 7, 0);
100 |
101 | // I2C
102 | GPIO_PinAFConfig(GPIOB, 6, 1);
103 | GPIO_PinAFConfig(GPIOB, 7, 1);
104 |
105 | // Motor Timers
106 | GPIO_PinAFConfig(GPIOA, 3, 2);
107 | GPIO_PinAFConfig(GPIOA, 8, 2);
108 | GPIO_PinAFConfig(GPIOA, 11, 2);
109 | GPIO_PinAFConfig(GPIOB, 8, 2);
110 |
111 | /* (1) Timing register value is computed with the AN4235 xls file,
112 | fast Mode @400kHz with I2CCLK = 48MHz, rise time = 140ns,
113 | fall time = 40ns */
114 | /* (2) Periph enable, receive interrupt enable */
115 | /* (3) Slave address = 0x5A, read transfer, 1 byte to receive, autoend */
116 | //I2C2->TIMINGR = ( uint32_t)0x00B01A4B; /* (1) */
117 | //I2C2->CR1 = I2C_CR1_PE | I2C_CR1_RXIE; /* (2) */
118 | //I2C2->CR2 = I2C_CR2_AUTOEND | (1<<16) | I2C_CR2_RD_WRN | (I2C1_OWN_ADDRESS << 1); /* (3) */
119 | }
120 |
121 |
122 | void gpio_set(GPIO_TypeDef* port, u8 pin) {
123 | port->BSRR = (1<BRR = (1<ODR ^= (1<= Y, although any values of X and Y are usable
14 | // provided they are under 1456 so the 16bit multiply does not overflow.
15 | // the result will be 0-45 degrees range
16 |
17 | unsigned char tempdegree;
18 | unsigned char comp;
19 | unsigned int degree; // this will hold the result
20 | //signed int x; // these hold the XY vector at the start
21 | //signed int y; //
22 |
23 | // 1. Calc the scaled "degrees"
24 | degree = (y * 45) / x; // note! X must be >= Y, result will be 0-45
25 |
26 | // 2. Compensate for the 4 degree error curve
27 | comp = 0;
28 | tempdegree = degree; // use an unsigned char for speed!
29 | if(tempdegree > 22) // if top half of range
30 | {
31 | if(tempdegree <= 44) comp++;
32 | if(tempdegree <= 41) comp++;
33 | if(tempdegree <= 37) comp++;
34 | if(tempdegree <= 32) comp++; // max is 4 degrees compensated
35 | }
36 | else // else is lower half of range
37 | {
38 | if(tempdegree >= 2) comp++;
39 | if(tempdegree >= 6) comp++;
40 | if(tempdegree >= 10) comp++;
41 | if(tempdegree >= 15) comp++; // max is 4 degrees compensated
42 | }
43 | degree += comp; // degree is now accurate to +/- 1 degree!
44 | return degree;
45 | }
46 | #elif defined use_atan_360
47 |
48 | u16 fast_atan(s16 x, s16 y) {
49 |
50 | // Fast XY vector to integer degree algorithm - Jan 2011 www.RomanBlack.com
51 | // Converts any XY values including 0 to a degree value that should be
52 | // within +/- 1 degree of the accurate value without needing
53 | // large slow trig functions like ArcTan() or ArcCos().
54 | // NOTE! at least one of the X or Y values must be non-zero!
55 | // This is the full version, for all 4 quadrants and will generate
56 | // the angle in integer degrees from 0-360.
57 | // Any values of X and Y are usable including negative values provided
58 | // they are between -1456 and 1456 so the 16bit multiply does not overflow.
59 |
60 | unsigned char negflag;
61 | unsigned char tempdegree;
62 | unsigned char comp;
63 | unsigned int degree; // this will hold the result
64 | // signed int x; // these hold the XY vector at the start
65 | // signed int y; // (and they will be destroyed)
66 | unsigned int ux;
67 | unsigned int uy;
68 |
69 | // Save the sign flags then remove signs and get XY as unsigned ints
70 | negflag = 0;
71 | if(x < 0)
72 | {
73 | negflag += 0x01; // x flag bit
74 | x = (0 - x); // is now +
75 | }
76 | ux = x; // copy to unsigned var before multiply
77 | if(y < 0)
78 | {
79 | negflag += 0x02; // y flag bit
80 | y = (0 - y); // is now +
81 | }
82 | uy = y; // copy to unsigned var before multiply
83 |
84 | // 1. Calc the scaled "degrees"
85 | if(ux > uy)
86 | {
87 | degree = (uy * 45) / ux; // degree result will be 0-45 range
88 | negflag += 0x10; // octant flag bit
89 | }
90 | else
91 | {
92 | degree = (ux * 45) / uy; // degree result will be 0-45 range
93 | }
94 |
95 | // 2. Compensate for the 4 degree error curve
96 | comp = 0;
97 | tempdegree = degree; // use an unsigned char for speed!
98 | if(tempdegree > 22) // if top half of range
99 | {
100 | if(tempdegree <= 44) comp++;
101 | if(tempdegree <= 41) comp++;
102 | if(tempdegree <= 37) comp++;
103 | if(tempdegree <= 32) comp++; // max is 4 degrees compensated
104 | }
105 | else // else is lower half of range
106 | {
107 | if(tempdegree >= 2) comp++;
108 | if(tempdegree >= 6) comp++;
109 | if(tempdegree >= 10) comp++;
110 | if(tempdegree >= 15) comp++; // max is 4 degrees compensated
111 | }
112 | degree += comp; // degree is now accurate to +/- 1 degree!
113 |
114 | // Invert degree if it was X>Y octant, makes 0-45 into 90-45
115 | if(negflag & 0x10) degree = (90 - degree);
116 |
117 | // 3. Degree is now 0-90 range for this quadrant,
118 | // need to invert it for whichever quadrant it was in
119 | if(negflag & 0x02) // if -Y
120 | {
121 | if(negflag & 0x01) // if -Y -X
122 | degree = (180 + degree);
123 | else // else is -Y +X
124 | degree = (180 - degree);
125 | }
126 | else // else is +Y
127 | {
128 | if(negflag & 0x01) // if +Y -X
129 | degree = (360 - degree);
130 | }
131 |
132 | return degree;
133 | }
134 | #endif
135 |
--------------------------------------------------------------------------------
/Makefile:
--------------------------------------------------------------------------------
1 | #
2 | # !!!! Do NOT edit this makefile with an editor which replace tabs by spaces !!!!
3 | #
4 | ##############################################################################################
5 | #
6 | # On command line:
7 | #
8 | # make all = Create project
9 | #
10 | # make clean = Clean project files.
11 | #
12 | # To rebuild project do "make clean" and "make all".
13 | #
14 | # Included originally in the yagarto projects. Original Author : Michael Fischer
15 | # Modified to suit our purposes by Hussam Al-Hertani
16 | # Use at your own risk!!!!!
17 | ##############################################################################################
18 | # Start of default section
19 | #
20 | CCPREFIX = arm-none-eabi-
21 | CC = $(CCPREFIX)gcc
22 | CP = $(CCPREFIX)objcopy
23 | AS = $(CCPREFIX)gcc -x assembler-with-cpp
24 | GDBTUI = $(CCPREFIX)gdbtui
25 | HEX = $(CP) -O ihex
26 | BIN = $(CP) -O binary -S
27 | MCU = cortex-m0
28 |
29 | # List all C defines here
30 | #DDEFS =
31 | DDEFS = -DSTM32F031
32 | #
33 | # Define project name and Ram/Flash mode here
34 | PROJECT = cx10
35 |
36 | # List C source files here
37 |
38 | SRC = ./src/main.c
39 | SRC += ./src/led.c
40 | SRC += ./src/gpio.c
41 | SRC += ./src/spi.c
42 | SRC += ./src/nrf24l01.c
43 | SRC += ./src/rx_bk2423.c
44 | SRC += ./src/misc.c
45 | SRC += ./src/pid.c
46 | SRC += ./src/imu.c
47 | SRC += ./src/flight.c
48 | SRC += ./src/timers.c
49 | SRC += ./src/atan.c
50 |
51 | SRC += ./src/stm32f0xx_rcc.c
52 | SRC += ./src/stm32f0xx_gpio.c
53 | SRC += ./src/stm32f0xx_i2c.c
54 | SRC += ./src/system_stm32f0xx.c
55 |
56 | SRC += ./cleanflight/drivers/system.c
57 | SRC += ./cleanflight/drivers/bus_i2c_stm32f0xx.c
58 | SRC += ./cleanflight/drivers/accgyro_mpu6050.c
59 |
60 | # List assembly startup source file here
61 | STARTUP = ./startup/startup_stm32f031.s
62 |
63 | # List all include directories here
64 | INCDIRS = ./inc ./cleanflight ./cleanflight/drivers
65 |
66 | # List the user directory to look for the libraries here
67 | LIBDIRS +=
68 |
69 | # List all user libraries here
70 | LIBS =
71 |
72 | # Define optimisation level here
73 | OPT = -Os
74 |
75 |
76 | # Define linker script file here
77 | LINKER_SCRIPT = ./linker/stm32f0_linker.ld
78 |
79 |
80 | INCDIR = $(patsubst %,-I%, $(INCDIRS))
81 | LIBDIR = $(patsubst %,-L%, $(LIBDIRS))
82 | #LIB = $(patsubst %,-l%, $(LIBS))
83 | ##reference only flags for run from ram...not used here
84 | ##DEFS = $(DDEFS) $(UDEFS) -DRUN_FROM_FLASH=0 -DVECT_TAB_SRAM
85 |
86 | ## run from Flash
87 | DEFS = $(DDEFS) -DRUN_FROM_FLASH=1
88 |
89 | OBJS = $(STARTUP:.s=.o) $(SRC:.c=.o)
90 | MCFLAGS = -mcpu=$(MCU)
91 |
92 | ASFLAGS = $(MCFLAGS) -g -gdwarf-2 -mthumb -Wa,-amhls=$(<:.s=.lst)
93 | CPFLAGS = $(MCFLAGS) $(OPT) -g -gdwarf-2 -mthumb -fomit-frame-pointer -Wall -Wstrict-prototypes -fverbose-asm -Wa,-ahlms=$(<:.c=.lst) $(DEFS) -std=c99
94 | LDFLAGS = $(MCFLAGS) -g -gdwarf-2 -mthumb -nostartfiles -T$(LINKER_SCRIPT) -Wl,-Map=$(PROJECT).map,--cref,--no-warn-mismatch $(LIBDIR)
95 |
96 | PWD = `pwd`
97 |
98 | #
99 | # makefile rules
100 | #
101 |
102 | all: $(OBJS) $(PROJECT).elf $(PROJECT).hex $(PROJECT).bin
103 | $(TRGT)size $(PROJECT).elf
104 |
105 | %.o: %.c
106 | $(CC) -c $(CPFLAGS) -I . $(INCDIR) $< -o $@
107 |
108 | %.o: %.s
109 | $(AS) -c $(ASFLAGS) $< -o $@
110 |
111 | %.elf: $(OBJS)
112 | $(CC) $(OBJS) $(LDFLAGS) $(LIBS) -o $@
113 |
114 | %.hex: %.elf
115 | $(HEX) $< $@
116 |
117 | %.bin: %.elf
118 | $(BIN) $< $@
119 |
120 | flash_openocd: $(PROJECT).bin
121 | openocd -f ~/src/openocd/stlink-swd.cfg -c "init" -c "reset halt" -c "sleep 100" -c "wait_halt 2" -c "flash write_image erase $(PROJECT).bin 0x08000000" -c "sleep 100" -c "verify_image $(PROJECT).bin 0x08000000" -c "sleep 100" -c "reset run" -c shutdown
122 |
123 | flash_stlink: $(PROJECT).bin
124 | st-flash write $(PROJECT).bin 0x8000000
125 |
126 | erase_openocd:
127 | openocd -f ~/src/openocd/stlink-swd.cfg -c "init" -c "reset halt" -c "sleep 100" -c "stm32f1x mass_erase 0" -c "sleep 100" -c shutdown
128 |
129 | erase_stlink:
130 | st-flash erase
131 |
132 | kill_gdbtui:
133 | killall $(GDBTUI)
134 |
135 | openocd: $(PROJECT).elf flash_openocd
136 | xterm -e openocd -f ~/src/openocd/stlink-swd.cfg -c "init" -c "halt" -c "reset halt" &
137 |
138 | connect_openocd:
139 | $(GDBTUI) --eval-command="target remote localhost:3333" $(PROJECT).elf
140 |
141 | flash_running: $(PROJECT).bin
142 | echo -ne "init\nreset halt\nsleep 100\nwait_halt 2\nflash write_image erase $(PWD)/$(PROJECT).bin 0x08000000\nsleep 100\nverify_image $(PWD)/$(PROJECT).bin 0x08000000\nsleep 100\nreset run\n" | nc localhost 4444
143 |
144 | halt:
145 | echo "reset halt" | nc localhost 4444
146 |
147 | #debug_openocd: $(PROJECT).elf flash_openocd
148 | # xterm -e openocd -f ~/src/openocd/stlink-swd.cfg -c "init" -c "halt" -c "reset halt" &
149 | # $(GDBTUI) --eval-command="target remote localhost:3333" $(PROJECT).elf
150 |
151 | debug_openocd: openocd connect_openocd kill_gdbtui connect_openocd
152 |
153 | debug_stlink: $(PROJECT).elf
154 | xterm -e st-util &
155 | $(GDBTUI) --eval-command="target remote localhost:4242" $(PROJECT).elf -ex 'load'
156 |
157 | clean:
158 | -rm -rf $(OBJS)
159 | -rm -rf $(PROJECT).elf
160 | -rm -rf $(PROJECT).map
161 | -rm -rf $(PROJECT).hex
162 | -rm -rf $(PROJECT).bin
163 | -rm -rf $(SRC:.c=.lst)
164 | -rm -rf $(ASRC:.s=.lst)
165 | # *** EOF ***
166 |
--------------------------------------------------------------------------------
/inc/pid.h:
--------------------------------------------------------------------------------
1 | /**
2 | * || ____ _ __
3 | * +------+ / __ )(_) /_______________ _____ ___
4 | * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
5 | * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
6 | * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
7 | *
8 | * Crazyflie control firmware
9 | *
10 | * Copyright (C) 2011-2012 Bitcraze AB
11 | *
12 | * This program is free software: you can redistribute it and/or modify
13 | * it under the terms of the GNU General Public License as published by
14 | * the Free Software Foundation, in version 3.
15 | *
16 | * This program is distributed in the hope that it will be useful,
17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19 | * GNU General Public License for more details.
20 | *
21 | * You should have received a copy of the GNU General Public License
22 | * along with this program. If not, see .
23 | *
24 | *
25 | * pid.h - implementation of the PID regulator
26 | */
27 | #ifndef PID_H_
28 | #define PID_H_
29 |
30 | #include
31 |
32 | #define PID_ROLL_RATE_KP 65.0
33 | #define PID_ROLL_RATE_KI 0.0
34 | #define PID_ROLL_RATE_KD 0.0
35 | #define PID_ROLL_RATE_INTEGRATION_LIMIT 100.0
36 |
37 | #define PID_PITCH_RATE_KP 65.0
38 | #define PID_PITCH_RATE_KI 0.0
39 | #define PID_PITCH_RATE_KD 0.0
40 | #define PID_PITCH_RATE_INTEGRATION_LIMIT 100.0
41 |
42 | #define PID_YAW_RATE_KP 69.0
43 | #define PID_YAW_RATE_KI 0.0
44 | #define PID_YAW_RATE_KD 0.0
45 | #define PID_YAW_RATE_INTEGRATION_LIMIT 500.0
46 |
47 | /* this part isn't interesting yet!
48 | #define PID_ROLL_KP 3.5
49 | #define PID_ROLL_KI 2.0
50 | #define PID_ROLL_KD 0.0
51 | #define PID_ROLL_INTEGRATION_LIMIT 20.0
52 |
53 | #define PID_PITCH_KP 3.5
54 | #define PID_PITCH_KI 2.0
55 | #define PID_PITCH_KD 0.0
56 | #define PID_PITCH_INTEGRATION_LIMIT 20.0
57 |
58 | #define PID_YAW_KP 0.0
59 | #define PID_YAW_KI 0.0
60 | #define PID_YAW_KD 0.0
61 | #define PID_YAW_INTEGRATION_LIMIT 360.0
62 | */
63 |
64 |
65 | #define DEFAULT_PID_INTEGRATION_LIMIT 5000.0
66 |
67 | typedef struct
68 | {
69 | float desired; //< set point
70 | float error; //< error
71 | float prevError; //< previous error
72 | float integ; //< integral
73 | float deriv; //< derivative
74 | float kp; //< proportional gain
75 | float ki; //< integral gain
76 | float kd; //< derivative gain
77 | float outP; //< proportional output (debugging)
78 | float outI; //< integral output (debugging)
79 | float outD; //< derivative output (debugging)
80 | float iLimit; //< integral limit
81 | float iLimitLow; //< integral limit
82 | float dt; //< delta-time dt
83 | } PidObject;
84 |
85 | /**
86 | * PID object initialization.
87 | *
88 | * @param[out] pid A pointer to the pid object to initialize.
89 | * @param[in] desired The initial set point.
90 | * @param[in] kp The proportional gain
91 | * @param[in] ki The integral gain
92 | * @param[in] kd The derivative gain
93 | */
94 | void pidInit(PidObject* pid, const float desired, const float kp,
95 | const float ki, const float kd, const float dt);
96 |
97 | /**
98 | * Set the integral limit for this PID in deg.
99 | *
100 | * @param[in] pid A pointer to the pid object.
101 | * @param[in] limit Pid integral swing limit.
102 | */
103 | void pidSetIntegralLimit(PidObject* pid, const float limit);
104 |
105 | /**
106 | * Reset the PID error values
107 | *
108 | * @param[in] pid A pointer to the pid object.
109 | * @param[in] limit Pid integral swing limit.
110 | */
111 | void pidReset(PidObject* pid);
112 |
113 | /**
114 | * Update the PID parameters.
115 | *
116 | * @param[in] pid A pointer to the pid object.
117 | * @param[in] measured The measured value
118 | * @param[in] updateError Set to TRUE if error should be calculated.
119 | * Set to False if pidSetError() has been used.
120 | * @return PID algorithm output
121 | */
122 | float pidUpdate(PidObject* pid, const float measured, const bool updateError);
123 |
124 | /**
125 | * Set a new set point for the PID to track.
126 | *
127 | * @param[in] pid A pointer to the pid object.
128 | * @param[in] angle The new set point
129 | */
130 | void pidSetDesired(PidObject* pid, const float desired);
131 |
132 | /**
133 | * Set a new set point for the PID to track.
134 | * @return The set point
135 | */
136 | float pidGetDesired(PidObject* pid);
137 |
138 | /**
139 | * Find out if PID is active
140 | * @return TRUE if active, FALSE otherwise
141 | */
142 | bool pidIsActive(PidObject* pid);
143 |
144 | /**
145 | * Set a new error. Use if a special error calculation is needed.
146 | *
147 | * @param[in] pid A pointer to the pid object.
148 | * @param[in] error The new error
149 | */
150 | void pidSetError(PidObject* pid, const float error);
151 |
152 | /**
153 | * Set a new proportional gain for the PID.
154 | *
155 | * @param[in] pid A pointer to the pid object.
156 | * @param[in] kp The new proportional gain
157 | */
158 | void pidSetKp(PidObject* pid, const float kp);
159 |
160 | /**
161 | * Set a new integral gain for the PID.
162 | *
163 | * @param[in] pid A pointer to the pid object.
164 | * @param[in] ki The new integral gain
165 | */
166 | void pidSetKi(PidObject* pid, const float ki);
167 |
168 | /**
169 | * Set a new derivative gain for the PID.
170 | *
171 | * @param[in] pid A pointer to the pid object.
172 | * @param[in] kd The derivative gain
173 | */
174 | void pidSetKd(PidObject* pid, const float kd);
175 |
176 | /**
177 | * Set a new dt gain for the PID. Defaults to IMU_UPDATE_DT upon construction
178 | *
179 | * @param[in] pid A pointer to the pid object.
180 | * @param[in] dt Delta time
181 | */
182 | void pidSetDt(PidObject* pid, const float dt);
183 | #endif /* PID_H_ */
184 |
--------------------------------------------------------------------------------
/src/misc.c:
--------------------------------------------------------------------------------
1 | #include "common.h"
2 | #include "misc.h"
3 | #include "stm32f0xx.h"
4 |
5 | /*
6 | void delay (int a)
7 | {
8 | volatile int i,j;
9 |
10 | for (i=0 ; i < a ; i++)
11 | {
12 | j++;
13 | }
14 |
15 | return;
16 | }
17 | */
18 |
19 | void _usleep(u32 x)
20 | {
21 | (void)x;
22 | __asm ("mov r1, #24;"
23 | "mul r0, r0, r1;"
24 | "b _delaycmp;"
25 | "_delayloop:"
26 | "sub r0, r0, #1;"
27 | "_delaycmp:;"
28 | "cmp r0, #0;"
29 | " bne _delayloop;");
30 | }
31 |
32 | #if 0
33 | static __I uint8_t APBAHBPrescTable[16] = {0, 0, 0, 0, 1, 2, 3, 4, 1, 2, 3, 4, 6, 7, 8, 9};
34 |
35 | void RCC_GetClocksFreq(RCC_ClocksTypeDef* RCC_Clocks)
36 | {
37 | uint32_t tmp = 0, pllmull = 0, pllsource = 0, prediv1factor = 0, presc = 0, pllclk = 0;
38 |
39 | /* Get SYSCLK source -------------------------------------------------------*/
40 | tmp = RCC->CFGR & RCC_CFGR_SWS;
41 |
42 | switch (tmp)
43 | {
44 | case 0x00: /* HSI used as system clock */
45 | RCC_Clocks->SYSCLK_Frequency = HSI_VALUE;
46 | break;
47 | case 0x04: /* HSE used as system clock */
48 | RCC_Clocks->SYSCLK_Frequency = HSE_VALUE;
49 | break;
50 | case 0x08: /* PLL used as system clock */
51 | /* Get PLL clock source and multiplication factor ----------------------*/
52 | pllmull = RCC->CFGR & RCC_CFGR_PLLMULL;
53 | pllsource = RCC->CFGR & RCC_CFGR_PLLSRC;
54 | pllmull = ( pllmull >> 18) + 2;
55 |
56 | if (pllsource == 0x00)
57 | {
58 | /* HSI oscillator clock divided by 2 selected as PLL clock entry */
59 | pllclk = (HSI_VALUE >> 1) * pllmull;
60 | }
61 | else
62 | {
63 | prediv1factor = (RCC->CFGR2 & RCC_CFGR2_PREDIV1) + 1;
64 | /* HSE oscillator clock selected as PREDIV1 clock entry */
65 | pllclk = (HSE_VALUE / prediv1factor) * pllmull;
66 | }
67 | RCC_Clocks->SYSCLK_Frequency = pllclk;
68 | break;
69 | case 0x0C: /* HSI48 used as system clock */
70 | RCC_Clocks->SYSCLK_Frequency = HSI48_VALUE;
71 | break;
72 | default: /* HSI used as system clock */
73 | RCC_Clocks->SYSCLK_Frequency = HSI_VALUE;
74 | break;
75 | }
76 | /* Compute HCLK, PCLK clocks frequencies -----------------------------------*/
77 | /* Get HCLK prescaler */
78 | tmp = RCC->CFGR & RCC_CFGR_HPRE;
79 | tmp = tmp >> 4;
80 | presc = APBAHBPrescTable[tmp];
81 | /* HCLK clock frequency */
82 | RCC_Clocks->HCLK_Frequency = RCC_Clocks->SYSCLK_Frequency >> presc;
83 |
84 | /* Get PCLK prescaler */
85 | tmp = RCC->CFGR & RCC_CFGR_PPRE;
86 | tmp = tmp >> 8;
87 | presc = APBAHBPrescTable[tmp];
88 | /* PCLK clock frequency */
89 | RCC_Clocks->PCLK_Frequency = RCC_Clocks->HCLK_Frequency >> presc;
90 |
91 | /* ADCCLK clock frequency */
92 | if((RCC->CFGR3 & RCC_CFGR3_ADCSW) != RCC_CFGR3_ADCSW)
93 | {
94 | /* ADC Clock is HSI14 Osc. */
95 | RCC_Clocks->ADCCLK_Frequency = HSI14_VALUE;
96 | }
97 | else
98 | {
99 | if((RCC->CFGR & RCC_CFGR_ADCPRE) != RCC_CFGR_ADCPRE)
100 | {
101 | /* ADC Clock is derived from PCLK/2 */
102 | RCC_Clocks->ADCCLK_Frequency = RCC_Clocks->PCLK_Frequency >> 1;
103 | }
104 | else
105 | {
106 | /* ADC Clock is derived from PCLK/4 */
107 | RCC_Clocks->ADCCLK_Frequency = RCC_Clocks->PCLK_Frequency >> 2;
108 | }
109 |
110 | }
111 |
112 | /* CECCLK clock frequency */
113 | if((RCC->CFGR3 & RCC_CFGR3_CECSW) != RCC_CFGR3_CECSW)
114 | {
115 | /* CEC Clock is HSI/244 */
116 | RCC_Clocks->CECCLK_Frequency = HSI_VALUE / 244;
117 | }
118 | else
119 | {
120 | /* CECC Clock is LSE Osc. */
121 | RCC_Clocks->CECCLK_Frequency = LSE_VALUE;
122 | }
123 |
124 | /* I2C1CLK clock frequency */
125 | if((RCC->CFGR3 & RCC_CFGR3_I2C1SW) != RCC_CFGR3_I2C1SW)
126 | {
127 | /* I2C1 Clock is HSI Osc. */
128 | RCC_Clocks->I2C1CLK_Frequency = HSI_VALUE;
129 | }
130 | else
131 | {
132 | /* I2C1 Clock is System Clock */
133 | RCC_Clocks->I2C1CLK_Frequency = RCC_Clocks->SYSCLK_Frequency;
134 | }
135 |
136 | /* USART1CLK clock frequency */
137 | if((RCC->CFGR3 & RCC_CFGR3_USART1SW) == 0x0)
138 | {
139 | /* USART1 Clock is PCLK */
140 | RCC_Clocks->USART1CLK_Frequency = RCC_Clocks->PCLK_Frequency;
141 | }
142 | else if((RCC->CFGR3 & RCC_CFGR3_USART1SW) == RCC_CFGR3_USART1SW_0)
143 | {
144 | /* USART1 Clock is System Clock */
145 | RCC_Clocks->USART1CLK_Frequency = RCC_Clocks->SYSCLK_Frequency;
146 | }
147 | else if((RCC->CFGR3 & RCC_CFGR3_USART1SW) == RCC_CFGR3_USART1SW_1)
148 | {
149 | /* USART1 Clock is LSE Osc. */
150 | RCC_Clocks->USART1CLK_Frequency = LSE_VALUE;
151 | }
152 | else if((RCC->CFGR3 & RCC_CFGR3_USART1SW) == RCC_CFGR3_USART1SW)
153 | {
154 | /* USART1 Clock is HSI Osc. */
155 | RCC_Clocks->USART1CLK_Frequency = HSI_VALUE;
156 | }
157 |
158 | /* USART2CLK clock frequency */
159 | if((RCC->CFGR3 & RCC_CFGR3_USART2SW) == 0x0)
160 | {
161 | /* USART Clock is PCLK */
162 | RCC_Clocks->USART2CLK_Frequency = RCC_Clocks->PCLK_Frequency;
163 | }
164 | else if((RCC->CFGR3 & RCC_CFGR3_USART2SW) == RCC_CFGR3_USART2SW_0)
165 | {
166 | /* USART Clock is System Clock */
167 | RCC_Clocks->USART2CLK_Frequency = RCC_Clocks->SYSCLK_Frequency;
168 | }
169 | else if((RCC->CFGR3 & RCC_CFGR3_USART2SW) == RCC_CFGR3_USART2SW_1)
170 | {
171 | /* USART Clock is LSE Osc. */
172 | RCC_Clocks->USART2CLK_Frequency = LSE_VALUE;
173 | }
174 | else if((RCC->CFGR3 & RCC_CFGR3_USART2SW) == RCC_CFGR3_USART2SW)
175 | {
176 | /* USART Clock is HSI Osc. */
177 | RCC_Clocks->USART2CLK_Frequency = HSI_VALUE;
178 | }
179 |
180 | /* USART3CLK clock frequency */
181 | if((RCC->CFGR3 & RCC_CFGR3_USART3SW) == 0x0)
182 | {
183 | /* USART Clock is PCLK */
184 | RCC_Clocks->USART3CLK_Frequency = RCC_Clocks->PCLK_Frequency;
185 | }
186 | else if((RCC->CFGR3 & RCC_CFGR3_USART3SW) == RCC_CFGR3_USART3SW_0)
187 | {
188 | /* USART Clock is System Clock */
189 | RCC_Clocks->USART3CLK_Frequency = RCC_Clocks->SYSCLK_Frequency;
190 | }
191 | else if((RCC->CFGR3 & RCC_CFGR3_USART3SW) == RCC_CFGR3_USART3SW_1)
192 | {
193 | /* USART Clock is LSE Osc. */
194 | RCC_Clocks->USART3CLK_Frequency = LSE_VALUE;
195 | }
196 | else if((RCC->CFGR3 & RCC_CFGR3_USART3SW) == RCC_CFGR3_USART3SW)
197 | {
198 | /* USART Clock is HSI Osc. */
199 | RCC_Clocks->USART3CLK_Frequency = HSI_VALUE;
200 | }
201 |
202 | /* USBCLK clock frequency */
203 | if((RCC->CFGR3 & RCC_CFGR3_USBSW) != RCC_CFGR3_USBSW)
204 | {
205 | /* USB Clock is HSI48 */
206 | RCC_Clocks->USBCLK_Frequency = HSI48_VALUE;
207 | }
208 | else
209 | {
210 | /* USB Clock is PLL clock */
211 | RCC_Clocks->USBCLK_Frequency = pllclk;
212 | }
213 | }
214 | #endif
215 |
--------------------------------------------------------------------------------
/src/timers.c:
--------------------------------------------------------------------------------
1 | #include "common.h"
2 | #include "stm32f0xx.h"
3 | #include "misc.h"
4 | #include "led.h"
5 | #include "nf_gpio.h"
6 | #include "timers.h"
7 | #include "spi.h"
8 | #include "iface_nrf24l01.h"
9 | #include "rx_bk2423.h"
10 | #include "flight.h"
11 |
12 | /*
13 | We need the following timers:
14 | -----------------------------
15 |
16 | TIM2_CH4 - PA03 - Motor NW ("B") - check.
17 | TIM1_CH1 - PA08 - Motor SW ("L") - check.
18 | TIM1_CH4 - PA11 - Motor SE ("R") - check.
19 | TIM16_CH1 - PB08 - Motor NE ("F") - removed broken transistor, so this output can't drive a motor anymore. Let's use this output for the servo!
20 |
21 | */
22 |
23 | // Timer prescaler and period
24 | #define TIM_PSC 6
25 | #define TIM_ARR 327
26 | #define TIM_CCR1 0
27 |
28 | // The last timer (TIM16_CH1) is used for the servo on tricopters
29 | #ifdef TRI
30 | #define TIM_S_PSC 48 // scale the timer clock to 1MHz
31 | #define TIM_S_ARR 20000 // 20000 cycles @ 1MHz = 50Hz
32 | #define TIM_S_CCR1 1500 // 1500 us is the neutral position
33 | #elif defined TRI_TEST
34 | #define TIM_S_PSC 48 // scale the timer clock to 1MHz
35 | #define TIM_S_ARR 20000 // 20000 cycles @ 1MHz = 50Hz
36 | #define TIM_S_CCR1 1500 // 1500 us is the neutral position
37 | #else
38 | #define TIM_S_PSC TIM_PSC
39 | #define TIM_S_ARR TIM_ARR
40 | #define TIM_S_CCR1 TIM_CCR1
41 | #endif
42 |
43 | void timers_init(void) {
44 | #if 1
45 |
46 | RCC->APB1ENR |= RCC_APB1ENR_TIM2EN; // Enable clock for TIM2
47 | RCC->APB2ENR |= RCC_APB2ENR_TIM1EN; // Enable clock for TIM1
48 | RCC->APB2ENR |= RCC_APB2ENR_TIM16EN; // Enable clock for TIM16
49 |
50 | // Timer 1 config
51 | TIM1->PSC = TIM_PSC; // Set prescaler
52 | TIM1->ARR = TIM_ARR; // Set reload value (=TimerPeriod in the stdperiph example)
53 | TIM1->CCMR1 |= TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1M_2; // OC1M = 110 for PWM Mode 1 output on ch1
54 | TIM1->CCMR2 |= TIM_CCMR2_OC4M_1 | TIM_CCMR2_OC4M_2; // OC4M = 110 for PWM Mode 1 output on ch4
55 | TIM1->CCMR1 |= TIM_CCMR1_OC1PE; // Output 1 preload enable
56 | TIM1->CCMR2 |= TIM_CCMR2_OC4PE; // Output 4 preload enable
57 | TIM1->CR1 |= TIM_CR1_ARPE; // Auto-reload preload enable
58 | TIM1->CCER |= TIM_CCER_CC1E; // Enable output for ch1
59 | TIM1->CCER |= TIM_CCER_CC4E; // Enable output for ch4
60 | TIM1->CCR1 = TIM_CCR1;
61 | TIM1->EGR |= TIM_EGR_UG; // Force update
62 | TIM1->SR &= ~TIM_SR_UIF; // Clear the update flag
63 | TIM1->CR1 |= TIM_CR1_CEN; // Enable counter
64 | TIM1->BDTR |= TIM_BDTR_MOE; // Enable main output
65 |
66 | TIM2->PSC = TIM_PSC; // Set prescaler
67 | TIM2->ARR = TIM_ARR; // Set reload value (=TimerPeriod in the stdperiph example)
68 | TIM2->CCMR2 |= TIM_CCMR2_OC4M_1 | TIM_CCMR2_OC4M_2; // OC4M = 110 for PWM Mode 1 output on ch4
69 | TIM2->CCMR2 |= TIM_CCMR2_OC4PE; // Output 4 preload enable
70 | TIM2->CR1 |= TIM_CR1_ARPE; // Auto-reload preload enable
71 | TIM2->CCER |= TIM_CCER_CC4E; // Enable output for ch4
72 | TIM2->CCR1 = TIM_CCR1;
73 | TIM2->EGR |= TIM_EGR_UG; // Force update
74 | TIM2->SR &= ~TIM_SR_UIF; // Clear the update flag
75 | TIM2->CR1 |= TIM_CR1_CEN; // Enable counter
76 | TIM2->BDTR |= TIM_BDTR_MOE; // Enable main output
77 |
78 | TIM16->PSC = TIM_S_PSC; // Set prescaler
79 | TIM16->ARR = TIM_S_ARR; // Set reload value (=TimerPeriod in the stdperiph example)
80 | TIM16->CCMR1 |= TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1M_2; // OC1M = 110 for PWM Mode 1 output on ch1
81 | TIM16->CCMR1 |= TIM_CCMR1_OC1PE; // Output 1 preload enable
82 | TIM16->CR1 |= TIM_CR1_ARPE; // Auto-reload preload enable
83 | TIM16->CCER |= TIM_CCER_CC1E; // Enable output for ch1
84 | TIM16->CCR1 = TIM_S_CCR1;
85 | TIM16->EGR |= TIM_EGR_UG; // Force update
86 | TIM16->SR &= ~TIM_SR_UIF; // Clear the update flag
87 | TIM16->CR1 |= TIM_CR1_CEN; // Enable counter
88 | TIM16->BDTR |= TIM_BDTR_MOE; // Enable main output
89 |
90 | #else
91 |
92 | /* Compute the value to be set in ARR regiter to generate signal frequency at 17.57 Khz */
93 | int TimerPeriod = (SystemCoreClock / 17570 ) - 1;
94 |
95 | /* Compute CCR1 value to generate a duty cycle at 50% for channel 1 and 1N */
96 | Channel1Pulse = (uint16_t) (((uint32_t) 5 * (TimerPeriod - 1)) / 10);
97 | /* Compute CCR2 value to generate a duty cycle at 37.5% for channel 2 and 2N */
98 | Channel2Pulse = (uint16_t) (((uint32_t) 375 * (TimerPeriod - 1)) / 1000);
99 | /* Compute CCR3 value to generate a duty cycle at 25% for channel 3 and 3N */
100 | Channel3Pulse = (uint16_t) (((uint32_t) 25 * (TimerPeriod - 1)) / 100);
101 | /* Compute CCR4 value to generate a duty cycle at 12.5% for channel 4 */
102 | Channel4Pulse = (uint16_t) (((uint32_t) 125 * (TimerPeriod- 1)) / 1000);
103 |
104 | /* Time Base configuration */
105 | TIM_TimeBaseStructure.TIM_Prescaler = 0;
106 | TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
107 | TIM_TimeBaseStructure.TIM_Period = TimerPeriod;
108 | TIM_TimeBaseStructure.TIM_ClockDivision = 0;
109 | TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
110 |
111 | TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure);
112 |
113 | /* Channel 1, 2,3 and 4 Configuration in PWM mode */
114 | TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2;
115 | TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
116 | TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;
117 | TIM_OCInitStructure.TIM_Pulse = Channel1Pulse;
118 | TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low;
119 | TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High;
120 | TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
121 | TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCIdleState_Reset;
122 |
123 | TIM_OC1Init(TIM1, &TIM_OCInitStructure);
124 |
125 | TIM_OCInitStructure.TIM_Pulse = Channel2Pulse;
126 | TIM_OC2Init(TIM1, &TIM_OCInitStructure);
127 |
128 | TIM_OCInitStructure.TIM_Pulse = Channel3Pulse;
129 | TIM_OC3Init(TIM1, &TIM_OCInitStructure);
130 |
131 | TIM_OCInitStructure.TIM_Pulse = Channel4Pulse;
132 | TIM_OC4Init(TIM1, &TIM_OCInitStructure);
133 |
134 | /* TIM1 counter enable */
135 | TIM_Cmd(TIM1, ENABLE);
136 |
137 | /* TIM1 Main Output Enable */
138 | TIM_CtrlPWMOutputs(TIM1, ENABLE);
139 | #endif
140 |
141 | }
142 |
--------------------------------------------------------------------------------
/src/rx_bk2423.c:
--------------------------------------------------------------------------------
1 | // license foo
2 |
3 | #include "common.h"
4 | #include "stm32f0xx.h"
5 | #include "misc.h"
6 | #include "led.h"
7 | #include "nf_gpio.h"
8 | #include "spi.h"
9 | #include "iface_nrf24l01.h"
10 | #include "rx_bk2423.h"
11 | #include "stm32f0xx.h"
12 |
13 | // cleanflight drivers
14 | #include "system.h"
15 |
16 | // Bit vector from bit position
17 | #define BV(bit) (1 << bit)
18 |
19 | static u8 tx_addr[5];
20 | static u8 packet[9];
21 |
22 | void rx_deinit(void) {
23 | NRF24L01_FlushRx(); // Packet ID 1314
24 | NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x40); // Packet ID 1315
25 | NRF24L01_WriteReg(NRF24L01_00_CONFIG, 0x00);
26 |
27 | }
28 |
29 | void rx_init(void)
30 | {
31 |
32 | rx_deinit();
33 | delay(1000);
34 |
35 | NRF24L01_Initialize();
36 |
37 | NRF24L01_ReadReg(NRF24L01_07_STATUS); // Packet ID 0
38 | NRF24L01_Activate(0x53); // Packet ID 1
39 | NRF24L01_ReadRegisterMulti(NRF24L01_08_OBSERVE_TX, tx_addr, 4); // We have to dump the data somewhere and tx_addr is not used yet
40 | NRF24L01_ReadReg(NRF24L01_07_STATUS); // Packet ID 3
41 | NRF24L01_Activate(0x53); // Packet ID 4
42 | //NRF24L01_WriteReg(NRF24L01_00_CONFIG, 0x0b); // Packet ID 5
43 | NRF24L01_WriteReg(NRF24L01_00_CONFIG, BV(NRF24L01_00_PRIM_RX) | BV(NRF24L01_00_EN_CRC) | BV(NRF24L01_00_PWR_UP)); // same thing, only more readable
44 | NRF24L01_WriteReg(NRF24L01_01_EN_AA, 0x3f); // Packet ID 6
45 | NRF24L01_WriteReg(NRF24L01_02_EN_RXADDR, 0x3f); // Packet ID 7
46 | NRF24L01_WriteReg(NRF24L01_03_SETUP_AW, 0x03); // Packet ID 8
47 | NRF24L01_WriteReg(NRF24L01_04_SETUP_RETR, 0x1f); // Packet ID 9
48 | NRF24L01_WriteReg(NRF24L01_05_RF_CH, 0x3c); // Packet ID 10
49 | NRF24L01_WriteReg(NRF24L01_06_RF_SETUP, 0x07); // Packet ID 11
50 | NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x07); // Packet ID 12
51 | NRF24L01_WriteReg(NRF24L01_08_OBSERVE_TX, 0x00); // Packet ID 13
52 | NRF24L01_WriteReg(NRF24L01_09_CD, 0x00); // Packet ID 14
53 | NRF24L01_WriteReg(NRF24L01_0C_RX_ADDR_P2, 0xc3); // Packet ID 15
54 | NRF24L01_WriteReg(NRF24L01_0D_RX_ADDR_P3, 0xc4); // Packet ID 16
55 | NRF24L01_WriteReg(NRF24L01_0E_RX_ADDR_P4, 0xc5); // Packet ID 17
56 | NRF24L01_WriteReg(NRF24L01_0F_RX_ADDR_P5, 0xc6); // Packet ID 18
57 | NRF24L01_WriteReg(NRF24L01_11_RX_PW_P0, 0x09); // Packet ID 19
58 | NRF24L01_WriteReg(NRF24L01_12_RX_PW_P1, 0x09); // Packet ID 20
59 | NRF24L01_WriteReg(NRF24L01_13_RX_PW_P2, 0x09); // Packet ID 21
60 | NRF24L01_WriteReg(NRF24L01_14_RX_PW_P3, 0x09); // Packet ID 22
61 | NRF24L01_WriteReg(NRF24L01_15_RX_PW_P4, 0x09); // Packet ID 23
62 | NRF24L01_WriteReg(NRF24L01_16_RX_PW_P5, 0x09); // Packet ID 24
63 | NRF24L01_WriteReg(NRF24L01_17_FIFO_STATUS, 0x00); // Packet ID 25
64 | NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, (u8 *) "\x65\x65\x65\x65\x65", 5); // Packet ID 26
65 | NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, (u8 *) "\x65\x65\x65\x65\x65", 5); // Packet ID 27
66 | NRF24L01_ReadReg(NRF24L01_1D_FEATURE); // Packet ID 28
67 | NRF24L01_Activate(0x73); // Packet ID 29
68 | NRF24L01_WriteReg(NRF24L01_1C_DYNPD, 0x3f); // Packet ID 30
69 | NRF24L01_WriteReg(NRF24L01_1D_FEATURE, 0x07); // Packet ID 31
70 | NRF24L01_ReadReg(NRF24L01_07_STATUS); // Packet ID 32
71 | NRF24L01_Activate(0x53); // Packet ID 33
72 | NRF24L01_WriteRegisterMulti(NRF24L01_00_CONFIG, (u8 *) "\x40\x4b\x01\xe2", 4); // Packet ID 34
73 | NRF24L01_WriteRegisterMulti(NRF24L01_01_EN_AA, (u8 *) "\xc0\x4b\x00\x00", 4); // Packet ID 35
74 | NRF24L01_WriteRegisterMulti(NRF24L01_02_EN_RXADDR, (u8 *) "\xd0\xfc\x8c\x02", 4); // Packet ID 36
75 | NRF24L01_WriteRegisterMulti(NRF24L01_03_SETUP_AW, (u8 *) "\x99\x00\x39\x21", 4); // Packet ID 37
76 | NRF24L01_WriteRegisterMulti(NRF24L01_04_SETUP_RETR, (u8 *) "\xd9\x96\x82\x1b", 4); // Packet ID 38
77 | NRF24L01_WriteRegisterMulti(NRF24L01_05_RF_CH, (u8 *) "\x24\x06\x7f\xa6", 4); // Packet ID 39
78 | NRF24L01_WriteRegisterMulti(NRF24L01_0C_RX_ADDR_P2, (u8 *) "\x00\x12\x73\x00", 4); // Packet ID 40
79 | NRF24L01_WriteRegisterMulti(NRF24L01_0D_RX_ADDR_P3, (u8 *) "\x46\xb4\x80\x00", 4); // Packet ID 41
80 |
81 | // this is obviously wrong since the W_REGISTER instruction only takes up to 5 data bytes, but the original RX does this, so fuck it!
82 | NRF24L01_WriteRegisterMulti(NRF24L01_0E_RX_ADDR_P4, (u8 *) "\x41\x10\x04\x82\x20\x08\x08\xf2\x7d\xef\xff", 11); // Packet ID 42
83 | NRF24L01_WriteRegisterMulti(NRF24L01_04_SETUP_RETR, (u8 *) "\xdf\x96\x82\x1b", 4); // Packet ID 43
84 | NRF24L01_WriteRegisterMulti(NRF24L01_04_SETUP_RETR, (u8 *) "\xd9\x96\x82\x1b", 4); // Packet ID 44
85 | NRF24L01_ReadReg(NRF24L01_07_STATUS); // Packet ID 45
86 | NRF24L01_Activate(0x53); // Packet ID 46
87 | NRF24L01_FlushRx(); // Packet ID 47
88 | NRF24L01_ReadReg(NRF24L01_07_STATUS); // Packet ID 48
89 | NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x0e); // Packet ID 49
90 | NRF24L01_ReadReg(NRF24L01_00_CONFIG); // Packet ID 50
91 | NRF24L01_WriteReg(NRF24L01_00_CONFIG, 0x0b); // Packet ID 51
92 |
93 | }
94 |
95 |
96 | void rx_bind(void) {
97 | // write addresses for bind packet
98 | NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, (u8 *) "\x65\x65\x65\x65\x65", 5); // Packet ID 52
99 | NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, (u8 *) "\x65\x65\x65\x65\x65", 5); // Packet ID 53
100 |
101 | // wait for bind packet
102 | while (!(NRF24L01_ReadReg(NRF24L01_07_STATUS) & 0x40)) {} // wait for RX_DR (0x40) bit to be set
103 |
104 | while (!(NRF24L01_ReadReg(NRF24L01_17_FIFO_STATUS) & 0x01)) { // wait for RX_EMPTY (0x01) bit to be set
105 | if (NRF24L01_ReadPlWid() == 0x09) {
106 | NRF24L01_ReadPayload(packet, 9); // Packet ID 109
107 | }
108 | }
109 |
110 | NRF24L01_FlushRx(); // Packet ID 117
111 | NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x40); // Clear RX_DR
112 |
113 | // the first four bytes of the packet are the address to use
114 | for (int i = 0; i < 4; i++) {
115 | tx_addr[i] = packet[i];
116 | }
117 |
118 | tx_addr[4] = 0xc1; // no idea what this means
119 |
120 | NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, tx_addr, 5); // Packet ID 119
121 | NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, tx_addr, 5); // Packet ID 120
122 |
123 |
124 | NRF24L01_ReadReg(NRF24L01_07_STATUS); // Packet ID 121
125 | NRF24L01_ReadReg(NRF24L01_07_STATUS); // Packet ID 122
126 |
127 | LED0_ON;
128 | LED1_ON;
129 | delay(1000);
130 | }
131 |
132 | void rx_search_channel(void)
133 | {
134 | u8 channels[] = { 0x3c, 0x02, 0x21, 0x41, 0x0b, 0x4b };
135 | u8 i = 5;
136 | while (!(NRF24L01_ReadReg(NRF24L01_07_STATUS) & 0x40)) {
137 | if (i<5) {
138 | i++;
139 | } else {
140 | i=0;
141 | }
142 | NRF24L01_WriteReg(NRF24L01_05_RF_CH, channels[i]); // Packet ID 123
143 | }
144 | }
145 |
146 |
147 | void rx_read_payload(void)
148 | {
149 | if ((NRF24L01_ReadReg(NRF24L01_07_STATUS) & 0x40)) {
150 | NRF24L01_ReadReg(NRF24L01_60_R_RX_PL_WID); // Packet ID 1311
151 | NRF24L01_ReadPayload(payload, 9); // Packet ID 1312
152 | NRF24L01_ReadReg(NRF24L01_17_FIFO_STATUS); // Packet ID 1313
153 | NRF24L01_FlushRx(); // Packet ID 1314
154 | NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x40); // Packet ID 1315
155 | }
156 | }
157 |
--------------------------------------------------------------------------------
/src/nrf24l01.c:
--------------------------------------------------------------------------------
1 | /*
2 | This project is free software: you can redistribute it and/or modify
3 | it under the terms of the GNU General Public License as published by
4 | the Free Software Foundation, either version 3 of the License, or
5 | (at your option) any later version.
6 |
7 | Deviation is distributed in the hope that it will be useful,
8 | but WITHOUT ANY WARRANTY; without even the implied warranty of
9 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 | GNU General Public License for more details.
11 |
12 | You should have received a copy of the GNU General Public License
13 | along with Deviation. If not, see .
14 | */
15 |
16 | #include "common.h"
17 | #include "iface_nrf24l01.h"
18 | #include "protospi.h"
19 |
20 | static const struct mcu_pin cs_pin = {(u32)GPIOA, 4};
21 | static u8 rf_setup;
22 |
23 | static void CS_HI(void) {
24 | PROTOSPI_pin_set(cs_pin);
25 | }
26 |
27 | static void CS_LO(void) {
28 | PROTOSPI_pin_clear(cs_pin);
29 | }
30 |
31 | void NRF24L01_Initialize(void)
32 | {
33 | rf_setup = 0x0F;
34 | }
35 |
36 | u8 NRF24L01_WriteReg(u8 reg, u8 data)
37 | {
38 | CS_LO();
39 | u8 res = PROTOSPI_xfer(W_REGISTER | (REGISTER_MASK & reg));
40 | PROTOSPI_xfer(data);
41 | CS_HI();
42 | return res;
43 | }
44 |
45 | u8 NRF24L01_WriteRegisterMulti(u8 reg, const u8 data[], u8 length)
46 | {
47 | CS_LO();
48 | u8 res = PROTOSPI_xfer(W_REGISTER | ( REGISTER_MASK & reg));
49 | for (u8 i = 0; i < length; i++)
50 | {
51 | PROTOSPI_xfer(data[i]);
52 | }
53 | CS_HI();
54 | return res;
55 | }
56 |
57 | u8 NRF24L01_WritePayload(u8 *data, u8 length)
58 | {
59 | CS_LO();
60 | u8 res = PROTOSPI_xfer(W_TX_PAYLOAD);
61 | for (u8 i = 0; i < length; i++)
62 | {
63 | PROTOSPI_xfer(data[i]);
64 | }
65 | CS_HI();
66 | return res;
67 | }
68 |
69 | u8 NRF24L01_ReadReg(u8 reg)
70 | {
71 | CS_LO();
72 | PROTOSPI_xfer(R_REGISTER | (REGISTER_MASK & reg));
73 | u8 data = PROTOSPI_xfer(0xa5);
74 | CS_HI();
75 | return data;
76 | }
77 |
78 | u8 NRF24L01_ReadRegisterMulti(u8 reg, u8 data[], u8 length)
79 | {
80 | CS_LO();
81 | u8 res = PROTOSPI_xfer(R_REGISTER | (REGISTER_MASK & reg));
82 | for(u8 i = 0; i < length; i++)
83 | {
84 | data[i] = PROTOSPI_xfer(0xa5);
85 | }
86 | CS_HI();
87 | return res;
88 | }
89 |
90 | u8 NRF24L01_ReadPlWid(void)
91 | {
92 | CS_LO();
93 | PROTOSPI_xfer(R_RX_PL_WID);
94 | u8 data = PROTOSPI_xfer(0xa5);
95 | CS_HI();
96 | return data;
97 | }
98 |
99 | u8 NRF24L01_ReadPayload(u8 *data, u8 length)
100 | {
101 | CS_LO();
102 | u8 res = PROTOSPI_xfer(R_RX_PAYLOAD);
103 | for(u8 i = 0; i < length; i++)
104 | {
105 | data[i] = PROTOSPI_xfer(0xa5);
106 | }
107 | CS_HI();
108 | return res;
109 | }
110 |
111 | static u8 Strobe(u8 state)
112 | {
113 | CS_LO();
114 | u8 res = PROTOSPI_xfer(state);
115 | CS_HI();
116 | return res;
117 | }
118 |
119 | u8 NRF24L01_FlushTx(void)
120 | {
121 | return Strobe(FLUSH_TX);
122 | }
123 |
124 | u8 NRF24L01_FlushRx(void)
125 | {
126 | return Strobe(FLUSH_RX);
127 | }
128 |
129 | u8 NRF24L01_Activate(u8 code)
130 | {
131 | CS_LO();
132 | u8 res = PROTOSPI_xfer(ACTIVATE);
133 | PROTOSPI_xfer(code);
134 | CS_HI();
135 | return res;
136 | }
137 |
138 | u8 NRF24L01_SetBitrate(u8 bitrate)
139 | {
140 | // Note that bitrate 250kbps (and bit RF_DR_LOW) is valid only
141 | // for nRF24L01+. There is no way to programmatically tell it from
142 | // older version, nRF24L01, but the older is practically phased out
143 | // by Nordic, so we assume that we deal with with modern version.
144 |
145 | // Bit 0 goes to RF_DR_HIGH, bit 1 - to RF_DR_LOW
146 | rf_setup = (rf_setup & 0xD7) | ((bitrate & 0x02) << 4) | ((bitrate & 0x01) << 3);
147 | return NRF24L01_WriteReg(NRF24L01_06_RF_SETUP, rf_setup);
148 | }
149 |
150 | // Power setting is 0..3 for nRF24L01
151 | // Claimed power amp for nRF24L01 from eBay is 20dBm.
152 | // Raw w 20dBm PA
153 | // 0 : -18dBm (16uW) 2dBm (1.6mW)
154 | // 1 : -12dBm (60uW) 8dBm (6mW)
155 | // 2 : -6dBm (250uW) 14dBm (25mW)
156 | // 3 : 0dBm (1mW) 20dBm (100mW)
157 | // So it maps to Deviation as follows
158 | /*
159 | TXPOWER_100uW = -10dBm
160 | TXPOWER_300uW = -5dBm
161 | TXPOWER_1mW = 0dBm
162 | TXPOWER_3mW = 5dBm
163 | TXPOWER_10mW = 10dBm
164 | TXPOWER_30mW = 15dBm
165 | TXPOWER_100mW = 20dBm
166 | TXPOWER_150mW = 22dBm
167 | */
168 | u8 NRF24L01_SetPower(u8 power)
169 | {
170 | u8 nrf_power = 0;
171 | switch(power) {
172 | case TXPOWER_100uW: nrf_power = 0; break;
173 | case TXPOWER_300uW: nrf_power = 0; break;
174 | case TXPOWER_1mW: nrf_power = 0; break;
175 | case TXPOWER_3mW: nrf_power = 1; break;
176 | case TXPOWER_10mW: nrf_power = 1; break;
177 | case TXPOWER_30mW: nrf_power = 2; break;
178 | case TXPOWER_100mW: nrf_power = 3; break;
179 | case TXPOWER_150mW: nrf_power = 3; break;
180 | default: nrf_power = 0; break;
181 | };
182 | // Power is in range 0..3 for nRF24L01
183 | rf_setup = (rf_setup & 0xF9) | ((nrf_power & 0x03) << 1);
184 | return NRF24L01_WriteReg(NRF24L01_06_RF_SETUP, rf_setup);
185 | }
186 |
187 | void NRF24L01_SetTxRxMode(enum TXRX_State mode)
188 | {
189 | if(mode == TX_EN) {
190 | NRF24L01_WriteReg(NRF24L01_07_STATUS, (1 << NRF24L01_07_RX_DR) //reset the flag(s)
191 | | (1 << NRF24L01_07_TX_DS)
192 | | (1 << NRF24L01_07_MAX_RT));
193 | NRF24L01_WriteReg(NRF24L01_00_CONFIG, (1 << NRF24L01_00_EN_CRC) // switch to TX mode
194 | | (1 << NRF24L01_00_CRCO)
195 | | (1 << NRF24L01_00_PWR_UP));
196 | usleep(130);
197 | } else if (mode == RX_EN) {
198 | NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x70); // reset the flag(s)
199 | NRF24L01_WriteReg(NRF24L01_00_CONFIG, 0x0F); // switch to RX mode
200 | NRF24L01_WriteReg(NRF24L01_07_STATUS, (1 << NRF24L01_07_RX_DR) //reset the flag(s)
201 | | (1 << NRF24L01_07_TX_DS)
202 | | (1 << NRF24L01_07_MAX_RT));
203 | NRF24L01_WriteReg(NRF24L01_00_CONFIG, (1 << NRF24L01_00_EN_CRC) // switch to RX mode
204 | | (1 << NRF24L01_00_CRCO)
205 | | (1 << NRF24L01_00_PWR_UP)
206 | | (1 << NRF24L01_00_PRIM_RX));
207 | usleep(130);
208 | } else {
209 | NRF24L01_WriteReg(NRF24L01_00_CONFIG, (1 << NRF24L01_00_EN_CRC)); //PowerDown
210 | }
211 | }
212 |
213 | int NRF24L01_Reset()
214 | {
215 | NRF24L01_FlushTx();
216 | NRF24L01_FlushRx();
217 | u8 status1 = Strobe(NOP);
218 | u8 status2 = NRF24L01_ReadReg(0x07);
219 | NRF24L01_SetTxRxMode(TXRX_OFF);
220 | return (status1 == status2 && (status1 & 0x0f) == 0x0e);
221 | }
222 |
--------------------------------------------------------------------------------
/src/flight.c:
--------------------------------------------------------------------------------
1 | #include "common.h"
2 | #include "stm32f0xx.h"
3 | #include "misc.h"
4 | #include "led.h"
5 | #include "nf_gpio.h"
6 | #include "pid.h"
7 |
8 | // cleanflight drivers
9 | #include "system.h"
10 | #include "accgyro.h"
11 | #include "accgyro_mpu6050.h"
12 | #include "bus_i2c.h"
13 | #include "flight.h"
14 | #include "imu.h"
15 |
16 | #define FC_FREQ_HZ 500
17 |
18 | // taken from crazyflie
19 | #define TRUNCATE_SINT16(out, in) (out = (inINT16_MAX)?INT16_MAX:in) )
20 | #define SATURATE_SINT16(in) ( (inINT16_MAX)?INT16_MAX:in) )
21 |
22 |
23 | // Frac 16.16
24 | #define FRAC_MANTISSA 16
25 | #define FRAC_SCALE (1 << FRAC_MANTISSA)
26 |
27 | #define CHAN_MULTIPLIER 100
28 | #define CHAN_MAX_VALUE (100 * CHAN_MULTIPLIER)
29 | #define CHAN_MIN_VALUE (-100 * CHAN_MULTIPLIER)
30 |
31 |
32 | static u32 elapsed_micros;
33 |
34 | static Axis3f gyro_data; // Gyro axis data in deg/s
35 |
36 | static float rollRateDesired;
37 | static float pitchRateDesired;
38 | static float yawRateDesired;
39 |
40 | static PidObject pidRollRate;
41 | static PidObject pidPitchRate;
42 | static PidObject pidYawRate;
43 |
44 | static s16 rollOutput;
45 | static s16 pitchOutput;
46 | static s16 yawOutput;
47 |
48 | static u16 actuatorThrust, m0_val, m1_val, m2_val, m3_val; // yaw, pitch, roll,
49 | //volatile static u8 yaw_trim, pitch_trim, roll_trim;
50 |
51 | // private functions
52 | static uint16_t limitServo(s16 value);
53 | static uint16_t limitThrust(int32_t value);
54 | static void distributePower(const uint16_t thrust, const int16_t roll,
55 | const int16_t pitch, const int16_t yaw);
56 |
57 |
58 | u32 map(u32 x, u32 in_min, u32 in_max, u32 out_min, u32 out_max)
59 | {
60 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
61 | }
62 |
63 |
64 | void controllerInit(void)
65 | {
66 |
67 | pidInit(&pidRollRate, 0, PID_ROLL_RATE_KP, PID_ROLL_RATE_KI, PID_ROLL_RATE_KD, IMU_UPDATE_DT);
68 | pidInit(&pidPitchRate, 0, PID_PITCH_RATE_KP, PID_PITCH_RATE_KI, PID_PITCH_RATE_KD, IMU_UPDATE_DT);
69 | pidInit(&pidYawRate, 0, PID_YAW_RATE_KP, PID_YAW_RATE_KI, PID_YAW_RATE_KD, IMU_UPDATE_DT);
70 | pidSetIntegralLimit(&pidRollRate, PID_ROLL_RATE_INTEGRATION_LIMIT);
71 | pidSetIntegralLimit(&pidPitchRate, PID_PITCH_RATE_INTEGRATION_LIMIT);
72 | pidSetIntegralLimit(&pidYawRate, PID_YAW_RATE_INTEGRATION_LIMIT);
73 |
74 | }
75 |
76 |
77 |
78 | void controllerCorrectRatePID(
79 | float rollRateActual, float pitchRateActual, float yawRateActual,
80 | float rollRateDesired, float pitchRateDesired, float yawRateDesired)
81 | {
82 | pidSetDesired(&pidRollRate, rollRateDesired);
83 | TRUNCATE_SINT16(rollOutput, pidUpdate(&pidRollRate, rollRateActual, TRUE));
84 |
85 | pidSetDesired(&pidPitchRate, pitchRateDesired);
86 | TRUNCATE_SINT16(pitchOutput, pidUpdate(&pidPitchRate, pitchRateActual, TRUE));
87 |
88 | pidSetDesired(&pidYawRate, yawRateDesired);
89 | TRUNCATE_SINT16(yawOutput, pidUpdate(&pidYawRate, yawRateActual, TRUE));
90 | }
91 |
92 |
93 | // Convert fractional 16.16 to float32
94 | static void frac2float(s32 n, float* res)
95 | {
96 | if (n == 0) {
97 | *res = 0.0;
98 | return;
99 | }
100 | u32 m = n < 0 ? -n : n;
101 | int i;
102 | for (i = (31-FRAC_MANTISSA); (m & 0x80000000) == 0; i--, m <<= 1) ;
103 | m <<= 1; // Clear implicit leftmost 1
104 | m >>= 9;
105 | u32 e = 127 + i;
106 | if (n < 0) m |= 0x80000000;
107 | m |= e << 23;
108 | *((u32 *) res) = m;
109 | }
110 |
111 | void flightControl(void) {
112 |
113 |
114 |
115 | // this should run at 500Hz
116 | if ((micros() - elapsed_micros) >= (2000)) {
117 | elapsed_micros = micros();
118 |
119 | // flicker the leds to measure correct speed
120 | GPIOA->ODR ^= 1<<15;
121 | GPIOB->ODR ^= 1<<2;
122 |
123 | // read gyro
124 | gyro_read(&gyro_data);
125 |
126 | // controls
127 | /*
128 | throttle = payload[0] * 0xff;
129 | yaw = payload[1] * 0xff;
130 | pitch = payload[3] * 0xff;
131 | roll = payload[4] * 0xff;
132 |
133 | // trims
134 | yaw_trim = payload[2];
135 | pitch_trim = payload[5];
136 | roll_trim = payload[6];
137 | */
138 |
139 |
140 | // convert channels for crazyflie PID:
141 | #if 0
142 | rollRateDesired = (float) (payload[4] - 128.0) * 256;
143 | pitchRateDesired = (float) (payload[3] - 128.0) * 256;
144 | yawRateDesired = (float) (payload[1] - 128.0) * 256;
145 |
146 | #else
147 | actuatorThrust = payload[0] * 256;
148 | // Roll, aka aileron, float +- 50.0 in degrees
149 | s32 f_roll = (payload[4] - 0x80) * 0x50 * FRAC_SCALE / (10000 / 400);
150 | frac2float(f_roll, &rollRateDesired);
151 |
152 | // Pitch, aka elevator, float +- 50.0 degrees
153 | s32 f_pitch = (payload[3] - 0x80) * 0x50 * FRAC_SCALE / (10000 / 400);
154 | frac2float(f_pitch, &pitchRateDesired);
155 |
156 | // Thrust, aka throttle 0..65535, working range 5535..65535
157 | // No space for overshoot here, hard limit Channel3 by -10000..10000
158 | /*
159 | s32 ch = payload[0] * 0xff;
160 | if (ch < CHAN_MIN_VALUE) {
161 | ch = CHAN_MIN_VALUE;
162 | } else if (ch > CHAN_MAX_VALUE) {
163 | ch = CHAN_MAX_VALUE;
164 | }
165 | actuatorThrust = ch*3L + 35535L;
166 | */
167 |
168 | // Yaw, aka rudder, float +- 400.0 deg/s
169 | s32 f_yaw = (payload[1] - 0x80) * 0x50 * FRAC_SCALE / (10000 / 400);
170 | frac2float(f_yaw, &yawRateDesired);
171 | #endif
172 |
173 | // gyro.* == *rateActual == Data measured by IMU
174 | // *rateDesired == Data from RX
175 | controllerCorrectRatePID(-gyro_data.x, gyro_data.y, -gyro_data.z, rollRateDesired, pitchRateDesired, yawRateDesired);
176 |
177 | //#define TUNE_ROLL
178 |
179 | if (actuatorThrust > 0)
180 | {
181 | #if defined(TUNE_ROLL)
182 | distributePower(actuatorThrust, rollOutput, 0, 0);
183 | #elif defined(TUNE_PITCH)
184 | distributePower(actuatorThrust, 0, pitchOutput, 0);
185 | #elif defined(TUNE_YAW)
186 | distributePower(actuatorThrust, 0, 0, -yawOutput);
187 | #else
188 | distributePower(actuatorThrust, rollOutput, pitchOutput, -yawOutput);
189 | #endif
190 | }
191 | else
192 | {
193 | distributePower(0, 0, 0, 0);
194 | pidReset(&pidRollRate);
195 | pidReset(&pidPitchRate);
196 | pidReset(&pidYawRate);
197 | }
198 |
199 |
200 | }
201 | #if 0
202 | m0_val = actuatorThrust;
203 | m1_val = actuatorThrust;
204 | m2_val = actuatorThrust;
205 | m3_val = ((yawOutput * 1000) / 0xffff) + 1000;
206 | TIM2->CCR4 = m0_val; // Motor "B"
207 | TIM1->CCR1 = m1_val; // Motor "L"
208 | TIM1->CCR4 = m2_val; // Motor "R"
209 | TIM16->CCR1 = m3_val; // Motor "F"
210 | #endif
211 | }
212 |
213 | static void distributePower(const uint16_t thrust, const int16_t roll,
214 | const int16_t pitch, const int16_t yaw)
215 | {
216 | #if defined QUAD_X
217 | roll = roll >> 1;
218 | pitch = pitch >> 1;
219 | m0_val = limitThrust(thrust - roll + pitch + yaw);
220 | m1_val = limitThrust(thrust - roll - pitch - yaw);
221 | m2_val = limitThrust(thrust + roll - pitch + yaw);
222 | m3_val = limitThrust(thrust + roll + pitch - yaw);
223 | #elif defined QUAD_PLUS
224 | m0_val = limitThrust(thrust + pitch + yaw);
225 | m1_val = limitThrust(thrust - roll - yaw);
226 | m2_val = limitThrust(thrust - pitch + yaw);
227 | m3_val = limitThrust(thrust + roll - yaw);
228 | #elif defined broken_QUAD_X
229 | m0_val = thrust + roll + pitch + yaw;
230 | m1_val = thrust - roll + pitch - yaw;
231 | m2_val = thrust + roll - pitch - yaw;
232 | m3_val = thrust - roll - pitch + yaw;
233 | #elif defined broken_QUAD_PLUS
234 | m0_val = thrust + pitch + yaw;
235 | m1_val = thrust - roll - yaw;
236 | m2_val = thrust + roll - yaw;
237 | m3_val = thrust - pitch + yaw;
238 | #elif defined QUAD_TEST
239 | m0_val = thrust;
240 | m1_val = thrust;
241 | m2_val = thrust;
242 | m3_val = thrust;
243 | #elif defined TRI
244 | m0_val = limitThrust(thrust + pitch * 1.333333f) / 200; // Rear
245 | m1_val = limitThrust(thrust - roll - pitch * 0.666667f) / 200; // Front right
246 | m2_val = limitThrust(thrust + roll - pitch * 0.666667f) / 200; // Front left
247 | m3_val = limitServo(map(yaw, -20000, 20000, 1000, 2000)); // Servo
248 | #elif defined TRI_TEST
249 | m0_val = thrust;
250 | m1_val = thrust;
251 | m2_val = thrust;
252 | m3_val = map(yaw, -32768, 32767, 1000, 2000);
253 | #endif
254 |
255 |
256 | // write new values to CC registers
257 | TIM2->CCR4 = m0_val; // Motor "B"
258 | TIM1->CCR1 = m1_val; // Motor "L"
259 | TIM1->CCR4 = m2_val; // Motor "R"
260 | TIM16->CCR1 = m3_val; // Motor "F" or Servo
261 | }
262 |
263 | static uint16_t limitServo(s16 value)
264 | {
265 | if(value > 2000)
266 | {
267 | value = 2000;
268 | }
269 | else if(value < 1000)
270 | {
271 | value = 1000;
272 | }
273 |
274 | return (uint16_t)value;
275 | }
276 |
277 | static uint16_t limitThrust(int32_t value)
278 | {
279 | if(value > UINT16_MAX)
280 | {
281 | value = UINT16_MAX;
282 | }
283 | else if(value < 0)
284 | {
285 | value = 0;
286 | }
287 |
288 | return (uint16_t)value;
289 | }
290 |
291 |
292 |
293 |
--------------------------------------------------------------------------------
/cleanflight/drivers/bus_i2c_stm32f0xx.c:
--------------------------------------------------------------------------------
1 | /*
2 | * This file is part of Cleanflight.
3 | *
4 | * Cleanflight 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 | * Cleanflight 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 | *
14 | * You should have received a copy of the GNU General Public License
15 | * along with Cleanflight. If not, see .
16 | */
17 |
18 | #include
19 | #include
20 |
21 | #include
22 |
23 | #include "build_config.h"
24 |
25 | #include "gpio.h"
26 | //#include "system.h"
27 |
28 | #include "bus_i2c.h"
29 |
30 | #ifndef SOFT_I2C
31 |
32 | #define I2C_SHORT_TIMEOUT ((uint32_t)0x1000)
33 | #define I2C_LONG_TIMEOUT ((uint32_t)(10 * I2C_SHORT_TIMEOUT))
34 |
35 | #define I2C1_SCL_GPIO GPIOB
36 | #define I2C1_SCL_PIN GPIO_Pin_6
37 | #define I2C1_SCL_PIN_SOURCE GPIO_PinSource6
38 | #define I2C1_SCL_CLK_SOURCE RCC_AHBPeriph_GPIOB
39 | #define I2C1_SDA_GPIO GPIOB
40 | #define I2C1_SDA_PIN GPIO_Pin_7
41 | #define I2C1_SDA_PIN_SOURCE GPIO_PinSource7
42 | #define I2C1_SDA_CLK_SOURCE RCC_AHBPeriph_GPIOB
43 |
44 | static uint32_t i2cTimeout;
45 |
46 | static volatile uint16_t i2c1ErrorCount = 0;
47 |
48 | ///////////////////////////////////////////////////////////////////////////////
49 | // I2C TimeoutUserCallback
50 | ///////////////////////////////////////////////////////////////////////////////
51 |
52 | uint32_t i2cTimeoutUserCallback(I2C_TypeDef *I2Cx)
53 | {
54 | i2c1ErrorCount++;
55 | return false;
56 | }
57 |
58 | void i2cInitPort(I2C_TypeDef *I2Cx)
59 | {
60 | GPIO_InitTypeDef GPIO_InitStructure;
61 | I2C_InitTypeDef I2C_InitStructure;
62 |
63 | if (I2Cx == I2C1) {
64 | RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB, ENABLE);
65 | RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE);
66 | RCC_AHBPeriphClockCmd(I2C1_SCL_CLK_SOURCE | I2C1_SDA_CLK_SOURCE, ENABLE);
67 | RCC_I2CCLKConfig(RCC_I2C1CLK_SYSCLK);
68 |
69 | //i2cUnstick(I2Cx); // Clock out stuff to make sure slaves arent stuck
70 |
71 | GPIO_PinAFConfig(I2C1_SCL_GPIO, I2C1_SCL_PIN_SOURCE, GPIO_AF_1);
72 | GPIO_PinAFConfig(I2C1_SDA_GPIO, I2C1_SDA_PIN_SOURCE, GPIO_AF_1);
73 |
74 | GPIO_StructInit(&GPIO_InitStructure);
75 | I2C_StructInit(&I2C_InitStructure);
76 |
77 | // Init pins
78 |
79 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
80 | GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
81 | GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;
82 | GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
83 |
84 | GPIO_InitStructure.GPIO_Pin = I2C1_SCL_PIN;
85 | GPIO_Init(I2C1_SCL_GPIO, &GPIO_InitStructure);
86 |
87 | GPIO_InitStructure.GPIO_Pin = I2C1_SDA_PIN;
88 | GPIO_Init(I2C1_SDA_GPIO, &GPIO_InitStructure);
89 |
90 | I2C_StructInit(&I2C_InitStructure);
91 |
92 | I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
93 | I2C_InitStructure.I2C_AnalogFilter = I2C_AnalogFilter_Enable;
94 | I2C_InitStructure.I2C_DigitalFilter = 0x00;
95 | I2C_InitStructure.I2C_OwnAddress1 = 0x00;
96 | I2C_InitStructure.I2C_Ack = I2C_Ack_Enable;
97 | I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
98 | // In calculator: 10 Khz, 8Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10.
99 | // In reality: ???kHz
100 | //I2C_InitStructure.I2C_Timing = 0x101098F3;
101 |
102 | I2C_InitStructure.I2C_Timing = 0x00901850; // 400 Khz, 48Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10.
103 | //I2C_InitStructure.I2C_Timing = 0x10805E89; // 100 Khz, 48Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10.
104 | //I2C_InitStructure.I2C_Timing = 0x00201D2B; // 100 Khz, 8Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10.
105 | //I2C_InitStructure.I2C_Timing = 0x0010020A; // Fast mode, 400 Khz, 8Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10.
106 |
107 | I2C_Init(I2C1, &I2C_InitStructure);
108 |
109 | I2C_Cmd(I2C1, ENABLE);
110 | }
111 |
112 | }
113 |
114 |
115 | void i2cInit(void)
116 | {
117 | i2cInitPort(I2C1); // FIXME hard coded to use I2C1 for now
118 | }
119 |
120 | uint16_t i2cGetErrorCounter(void)
121 | {
122 | return i2c1ErrorCount;
123 | }
124 |
125 | bool i2cWrite(uint8_t addr_, uint8_t reg, uint8_t data)
126 | {
127 | I2C_TypeDef* I2Cx = I2C1;
128 |
129 | /* Test on BUSY Flag */
130 | i2cTimeout = I2C_LONG_TIMEOUT;
131 | while (I2C_GetFlagStatus(I2Cx, I2C_ISR_BUSY) != RESET) {
132 | if ((i2cTimeout--) == 0) {
133 | return i2cTimeoutUserCallback(I2Cx);
134 | }
135 | }
136 |
137 | /* Configure slave address, nbytes, reload, end mode and start or stop generation */
138 | I2C_TransferHandling(I2Cx, addr_, 1, I2C_Reload_Mode, I2C_Generate_Start_Write);
139 |
140 | /* Wait until TXIS flag is set */
141 | i2cTimeout = I2C_LONG_TIMEOUT;
142 | while (I2C_GetFlagStatus(I2Cx, I2C_ISR_TXIS) == RESET) {
143 | if ((i2cTimeout--) == 0) {
144 | return i2cTimeoutUserCallback(I2Cx);
145 | }
146 | }
147 |
148 | /* Send Register address */
149 | I2C_SendData(I2Cx, (uint8_t) reg);
150 |
151 | /* Wait until TCR flag is set */
152 | i2cTimeout = I2C_LONG_TIMEOUT;
153 | while (I2C_GetFlagStatus(I2Cx, I2C_ISR_TCR) == RESET)
154 | {
155 | if ((i2cTimeout--) == 0) {
156 | return i2cTimeoutUserCallback(I2Cx);
157 | }
158 | }
159 |
160 | /* Configure slave address, nbytes, reload, end mode and start or stop generation */
161 | I2C_TransferHandling(I2Cx, addr_, 1, I2C_AutoEnd_Mode, I2C_No_StartStop);
162 |
163 | /* Wait until TXIS flag is set */
164 | i2cTimeout = I2C_LONG_TIMEOUT;
165 | while (I2C_GetFlagStatus(I2Cx, I2C_ISR_TXIS) == RESET) {
166 | if ((i2cTimeout--) == 0) {
167 | return i2cTimeoutUserCallback(I2Cx);
168 | }
169 | }
170 |
171 | /* Write data to TXDR */
172 | I2C_SendData(I2Cx, data);
173 |
174 | /* Wait until STOPF flag is set */
175 | i2cTimeout = I2C_LONG_TIMEOUT;
176 | while (I2C_GetFlagStatus(I2Cx, I2C_ISR_STOPF) == RESET) {
177 | if ((i2cTimeout--) == 0) {
178 | return i2cTimeoutUserCallback(I2Cx);
179 | }
180 | }
181 |
182 | /* Clear STOPF flag */
183 | I2C_ClearFlag(I2Cx, I2C_ICR_STOPCF);
184 |
185 | return true;
186 | }
187 |
188 | bool i2cRead(uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* buf)
189 | {
190 | I2C_TypeDef* I2Cx = I2C1;
191 | /* Test on BUSY Flag */
192 | i2cTimeout = I2C_LONG_TIMEOUT;
193 | while (I2C_GetFlagStatus(I2Cx, I2C_ISR_BUSY) != RESET) {
194 | if ((i2cTimeout--) == 0) {
195 | return i2cTimeoutUserCallback(I2Cx);
196 | }
197 | }
198 |
199 | /* Configure slave address, nbytes, reload, end mode and start or stop generation */
200 | I2C_TransferHandling(I2Cx, addr_, 1, I2C_SoftEnd_Mode, I2C_Generate_Start_Write);
201 |
202 | /* Wait until TXIS flag is set */
203 | i2cTimeout = I2C_LONG_TIMEOUT;
204 | while (I2C_GetFlagStatus(I2Cx, I2C_ISR_TXIS) == RESET) {
205 | if ((i2cTimeout--) == 0) {
206 | return i2cTimeoutUserCallback(I2Cx);
207 | }
208 | }
209 |
210 | if (len > 1) {
211 | reg |= 0x80;
212 | }
213 |
214 | /* Send Register address */
215 | I2C_SendData(I2Cx, (uint8_t) reg);
216 |
217 | /* Wait until TC flag is set */
218 | i2cTimeout = I2C_LONG_TIMEOUT;
219 | while (I2C_GetFlagStatus(I2Cx, I2C_ISR_TC) == RESET) {
220 | if ((i2cTimeout--) == 0) {
221 | return i2cTimeoutUserCallback(I2Cx);
222 | }
223 | }
224 |
225 | /* Configure slave address, nbytes, reload, end mode and start or stop generation */
226 | I2C_TransferHandling(I2Cx, addr_, len, I2C_AutoEnd_Mode, I2C_Generate_Start_Read);
227 |
228 | /* Wait until all data are received */
229 | while (len) {
230 | /* Wait until RXNE flag is set */
231 | i2cTimeout = I2C_LONG_TIMEOUT;
232 | while (I2C_GetFlagStatus(I2Cx, I2C_ISR_RXNE) == RESET) {
233 | if ((i2cTimeout--) == 0) {
234 | return i2cTimeoutUserCallback(I2Cx);
235 | }
236 | }
237 |
238 | /* Read data from RXDR */
239 | *buf = I2C_ReceiveData(I2Cx);
240 | /* Point to the next location where the byte read will be saved */
241 | buf++;
242 |
243 | /* Decrement the read bytes counter */
244 | len--;
245 | }
246 |
247 | /* Wait until STOPF flag is set */
248 | i2cTimeout = I2C_LONG_TIMEOUT;
249 | while (I2C_GetFlagStatus(I2Cx, I2C_ISR_STOPF) == RESET) {
250 | if ((i2cTimeout--) == 0) {
251 | return i2cTimeoutUserCallback(I2Cx);
252 | }
253 | }
254 |
255 | /* Clear STOPF flag */
256 | I2C_ClearFlag(I2Cx, I2C_ICR_STOPCF);
257 |
258 | /* If all operations OK */
259 | return true;
260 | }
261 |
262 | #endif
263 |
--------------------------------------------------------------------------------
/startup/startup_stm32f031.s:
--------------------------------------------------------------------------------
1 | /**
2 | ******************************************************************************
3 | * @file startup_stm32f0xx.s
4 | * @author MCD Application Team
5 | * @version V1.4.0
6 | * @date 24-July-2014
7 | * @brief STM32F031 Devices vector table for RIDE7 toolchain.
8 | * This module performs:
9 | * - Set the initial SP
10 | * - Set the initial PC == Reset_Handler,
11 | * - Set the vector table entries with the exceptions ISR address
12 | * - Configure the system clock
13 | * - Branches to main in the C library (which eventually
14 | * calls main()).
15 | * After Reset the Cortex-M0 processor is in Thread mode,
16 | * priority is Privileged, and the Stack is set to Main.
17 | ******************************************************************************
18 | * @attention
19 | *
20 | * © COPYRIGHT 2014 STMicroelectronics
21 | *
22 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License");
23 | * You may not use this file except in compliance with the License.
24 | * You may obtain a copy of the License at:
25 | *
26 | * http://www.st.com/software_license_agreement_liberty_v2
27 | *
28 | * Unless required by applicable law or agreed to in writing, software
29 | * distributed under the License is distributed on an "AS IS" BASIS,
30 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
31 | * See the License for the specific language governing permissions and
32 | * limitations under the License.
33 | *
34 | ******************************************************************************
35 | */
36 |
37 | .syntax unified
38 | .cpu cortex-m0
39 | .fpu softvfp
40 | .thumb
41 |
42 | .global g_pfnVectors
43 | .global Default_Handler
44 |
45 | /* start address for the initialization values of the .data section.
46 | defined in linker script */
47 | .word _sidata
48 | /* start address for the .data section. defined in linker script */
49 | .word _sdata
50 | /* end address for the .data section. defined in linker script */
51 | .word _edata
52 | /* start address for the .bss section. defined in linker script */
53 | .word _sbss
54 | /* end address for the .bss section. defined in linker script */
55 | .word _ebss
56 |
57 | .equ BootRAM, 0xF108F85F
58 | /**
59 | * @brief This is the code that gets called when the processor first
60 | * starts execution following a reset event. Only the absolutely
61 | * necessary set is performed, after which the application
62 | * supplied main() routine is called.
63 | * @param None
64 | * @retval : None
65 | */
66 |
67 | .section .text.Reset_Handler
68 | .weak Reset_Handler
69 | .type Reset_Handler, %function
70 | Reset_Handler:
71 | ldr r0, =_stack_start
72 | mov sp, r0 /* set stack pointer */
73 |
74 | /*Check if boot space corresponds to test memory*/
75 |
76 | LDR R0,=0x00000004
77 | LDR R1, [R0]
78 | LSRS R1, R1, #24
79 | LDR R2,=0x1F
80 | CMP R1, R2
81 | BNE ApplicationStart
82 |
83 | /*SYSCFG clock enable*/
84 |
85 | LDR R0,=0x40021018
86 | LDR R1,=0x00000001
87 | STR R1, [R0]
88 |
89 | /*Set CFGR1 register with flash memory remap at address 0*/
90 | LDR R0,=0x40010000
91 | LDR R1,=0x00000000
92 | STR R1, [R0]
93 |
94 | ApplicationStart:
95 | /* Copy the data segment initializers from flash to SRAM */
96 | movs r1, #0
97 | b LoopCopyDataInit
98 |
99 | CopyDataInit:
100 | ldr r3, =_sidata
101 | ldr r3, [r3, r1]
102 | str r3, [r0, r1]
103 | adds r1, r1, #4
104 |
105 | LoopCopyDataInit:
106 | ldr r0, =_sdata
107 | ldr r3, =_edata
108 | adds r2, r0, r1
109 | cmp r2, r3
110 | bcc CopyDataInit
111 | ldr r2, =_sbss
112 | b LoopFillZerobss
113 | /* Zero fill the bss segment. */
114 | FillZerobss:
115 | movs r3, #0
116 | str r3, [r2]
117 | adds r2, r2, #4
118 |
119 |
120 | LoopFillZerobss:
121 | ldr r3, = _ebss
122 | cmp r2, r3
123 | bcc FillZerobss
124 |
125 | /* Call the clock system intitialization function.*/
126 | bl SystemInit
127 |
128 | /* Call the application's entry point.*/
129 | bl main
130 |
131 | LoopForever:
132 | b LoopForever
133 |
134 |
135 | .size Reset_Handler, .-Reset_Handler
136 |
137 | /**
138 | * @brief This is the code that gets called when the processor receives an
139 | * unexpected interrupt. This simply enters an infinite loop, preserving
140 | * the system state for examination by a debugger.
141 | *
142 | * @param None
143 | * @retval : None
144 | */
145 | .section .text.Default_Handler,"ax",%progbits
146 | Default_Handler:
147 | Infinite_Loop:
148 | b Infinite_Loop
149 | .size Default_Handler, .-Default_Handler
150 | /******************************************************************************
151 | *
152 | * The minimal vector table for a Cortex M0. Note that the proper constructs
153 | * must be placed on this to ensure that it ends up at physical address
154 | * 0x0000.0000.
155 | *
156 | ******************************************************************************/
157 | .section .isr_vector,"a",%progbits
158 | .type g_pfnVectors, %object
159 | .size g_pfnVectors, .-g_pfnVectors
160 |
161 |
162 |
163 | g_pfnVectors:
164 | .word _stack_start
165 | .word Reset_Handler
166 |
167 | .word NMI_Handler
168 | .word HardFault_Handler
169 | .word 0
170 | .word 0
171 | .word 0
172 | .word 0
173 | .word 0
174 | .word 0
175 | .word 0
176 | .word SVC_Handler
177 | .word 0
178 | .word 0
179 | .word PendSV_Handler
180 | .word SysTick_Handler
181 |
182 |
183 | .word WWDG_IRQHandler
184 | .word PVD_IRQHandler
185 | .word RTC_IRQHandler
186 | .word FLASH_IRQHandler
187 | .word RCC_IRQHandler
188 | .word EXTI0_1_IRQHandler
189 | .word EXTI2_3_IRQHandler
190 | .word EXTI4_15_IRQHandler
191 | .word 0
192 | .word DMA1_Channel1_IRQHandler
193 | .word DMA1_Channel2_3_IRQHandler
194 | .word DMA1_Channel4_5_IRQHandler
195 | .word ADC1_IRQHandler
196 | .word TIM1_BRK_UP_TRG_COM_IRQHandler
197 | .word TIM1_CC_IRQHandler
198 | .word TIM2_IRQHandler
199 | .word TIM3_IRQHandler
200 | .word 0
201 | .word 0
202 | .word TIM14_IRQHandler
203 | .word 0
204 | .word TIM16_IRQHandler
205 | .word TIM17_IRQHandler
206 | .word I2C1_IRQHandler
207 | .word 0
208 | .word SPI1_IRQHandler
209 | .word 0
210 | .word USART1_IRQHandler
211 | .word 0
212 | .word 0
213 | .word 0
214 | .word 0
215 | .word BootRAM /* @0x108. This is for boot in RAM mode for
216 | STM32F0xx devices. */
217 |
218 | /*******************************************************************************
219 | *
220 | * Provide weak aliases for each Exception handler to the Default_Handler.
221 | * As they are weak aliases, any function with the same name will override
222 | * this definition.
223 | *
224 | *******************************************************************************/
225 |
226 | .weak NMI_Handler
227 | .thumb_set NMI_Handler,Default_Handler
228 |
229 | .weak HardFault_Handler
230 | .thumb_set HardFault_Handler,Default_Handler
231 |
232 | .weak SVC_Handler
233 | .thumb_set SVC_Handler,Default_Handler
234 |
235 | .weak PendSV_Handler
236 | .thumb_set PendSV_Handler,Default_Handler
237 |
238 | .weak SysTick_Handler
239 | .thumb_set SysTick_Handler,Default_Handler
240 |
241 | .weak WWDG_IRQHandler
242 | .thumb_set WWDG_IRQHandler,Default_Handler
243 |
244 | .weak PVD_IRQHandler
245 | .thumb_set PVD_IRQHandler,Default_Handler
246 |
247 | .weak RTC_IRQHandler
248 | .thumb_set RTC_IRQHandler,Default_Handler
249 |
250 | .weak FLASH_IRQHandler
251 | .thumb_set FLASH_IRQHandler,Default_Handler
252 |
253 | .weak RCC_IRQHandler
254 | .thumb_set RCC_IRQHandler,Default_Handler
255 |
256 | .weak EXTI0_1_IRQHandler
257 | .thumb_set EXTI0_1_IRQHandler,Default_Handler
258 |
259 | .weak EXTI2_3_IRQHandler
260 | .thumb_set EXTI2_3_IRQHandler,Default_Handler
261 |
262 | .weak EXTI4_15_IRQHandler
263 | .thumb_set EXTI4_15_IRQHandler,Default_Handler
264 |
265 | .weak DMA1_Channel1_IRQHandler
266 | .thumb_set DMA1_Channel1_IRQHandler,Default_Handler
267 |
268 | .weak DMA1_Channel2_3_IRQHandler
269 | .thumb_set DMA1_Channel2_3_IRQHandler,Default_Handler
270 |
271 | .weak DMA1_Channel4_5_IRQHandler
272 | .thumb_set DMA1_Channel4_5_IRQHandler,Default_Handler
273 |
274 | .weak ADC1_IRQHandler
275 | .thumb_set ADC1_IRQHandler,Default_Handler
276 |
277 | .weak TIM1_BRK_UP_TRG_COM_IRQHandler
278 | .thumb_set TIM1_BRK_UP_TRG_COM_IRQHandler,Default_Handler
279 |
280 | .weak TIM1_CC_IRQHandler
281 | .thumb_set TIM1_CC_IRQHandler,Default_Handler
282 |
283 | .weak TIM2_IRQHandler
284 | .thumb_set TIM2_IRQHandler,Default_Handler
285 |
286 | .weak TIM3_IRQHandler
287 | .thumb_set TIM3_IRQHandler,Default_Handler
288 |
289 | .weak TIM14_IRQHandler
290 | .thumb_set TIM14_IRQHandler,Default_Handler
291 |
292 | .weak TIM16_IRQHandler
293 | .thumb_set TIM16_IRQHandler,Default_Handler
294 |
295 | .weak TIM17_IRQHandler
296 | .thumb_set TIM17_IRQHandler,Default_Handler
297 |
298 | .weak I2C1_IRQHandler
299 | .thumb_set I2C1_IRQHandler,Default_Handler
300 |
301 | .weak SPI1_IRQHandler
302 | .thumb_set SPI1_IRQHandler,Default_Handler
303 |
304 | .weak USART1_IRQHandler
305 | .thumb_set USART1_IRQHandler,Default_Handler
306 |
307 |
308 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
309 |
--------------------------------------------------------------------------------
/cleanflight/drivers/accgyro_mpu6050.c:
--------------------------------------------------------------------------------
1 | /*
2 | * This file is part of Cleanflight.
3 | *
4 | * Cleanflight 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 | * Cleanflight 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 | *
14 | * You should have received a copy of the GNU General Public License
15 | * along with Cleanflight. If not, see .
16 | */
17 |
18 | #include
19 | #include
20 | #include
21 |
22 | #include "platform.h"
23 |
24 | //#include "common/maths.h"
25 |
26 | #include "system.h"
27 | #include "gpio.h"
28 | #include "bus_i2c.h"
29 |
30 | #include "accgyro.h"
31 | #include "accgyro_mpu6050.h"
32 |
33 | // MPU6050, Standard address 0x68, but 0x69 if AD0 is high
34 | #define MPU6050_ADDRESS 0x68<<1
35 |
36 | #define DMP_MEM_START_ADDR 0x6E
37 | #define DMP_MEM_R_W 0x6F
38 |
39 | #define MPU_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
40 | #define MPU_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
41 | #define MPU_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
42 | #define MPU_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN
43 | #define MPU_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN
44 | #define MPU_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN
45 | #define MPU_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS
46 | #define MPU_RA_XA_OFFS_L_TC 0x07
47 | #define MPU_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS
48 | #define MPU_RA_YA_OFFS_L_TC 0x09
49 | #define MPU_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS
50 | #define MPU_RA_ZA_OFFS_L_TC 0x0B
51 | #define MPU_RA_PRODUCT_ID 0x0C // Product ID Register
52 | #define MPU_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR
53 | #define MPU_RA_XG_OFFS_USRL 0x14
54 | #define MPU_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR
55 | #define MPU_RA_YG_OFFS_USRL 0x16
56 | #define MPU_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR
57 | #define MPU_RA_ZG_OFFS_USRL 0x18
58 | #define MPU_RA_SMPLRT_DIV 0x19
59 | #define MPU_RA_CONFIG 0x1A
60 | #define MPU_RA_GYRO_CONFIG 0x1B
61 | #define MPU_RA_ACCEL_CONFIG 0x1C
62 | #define MPU_RA_FF_THR 0x1D
63 | #define MPU_RA_FF_DUR 0x1E
64 | #define MPU_RA_MOT_THR 0x1F
65 | #define MPU_RA_MOT_DUR 0x20
66 | #define MPU_RA_ZRMOT_THR 0x21
67 | #define MPU_RA_ZRMOT_DUR 0x22
68 | #define MPU_RA_FIFO_EN 0x23
69 | #define MPU_RA_I2C_MST_CTRL 0x24
70 | #define MPU_RA_I2C_SLV0_ADDR 0x25
71 | #define MPU_RA_I2C_SLV0_REG 0x26
72 | #define MPU_RA_I2C_SLV0_CTRL 0x27
73 | #define MPU_RA_I2C_SLV1_ADDR 0x28
74 | #define MPU_RA_I2C_SLV1_REG 0x29
75 | #define MPU_RA_I2C_SLV1_CTRL 0x2A
76 | #define MPU_RA_I2C_SLV2_ADDR 0x2B
77 | #define MPU_RA_I2C_SLV2_REG 0x2C
78 | #define MPU_RA_I2C_SLV2_CTRL 0x2D
79 | #define MPU_RA_I2C_SLV3_ADDR 0x2E
80 | #define MPU_RA_I2C_SLV3_REG 0x2F
81 | #define MPU_RA_I2C_SLV3_CTRL 0x30
82 | #define MPU_RA_I2C_SLV4_ADDR 0x31
83 | #define MPU_RA_I2C_SLV4_REG 0x32
84 | #define MPU_RA_I2C_SLV4_DO 0x33
85 | #define MPU_RA_I2C_SLV4_CTRL 0x34
86 | #define MPU_RA_I2C_SLV4_DI 0x35
87 | #define MPU_RA_I2C_MST_STATUS 0x36
88 | #define MPU_RA_INT_PIN_CFG 0x37
89 | #define MPU_RA_INT_ENABLE 0x38
90 | #define MPU_RA_DMP_INT_STATUS 0x39
91 | #define MPU_RA_INT_STATUS 0x3A
92 | #define MPU_RA_ACCEL_XOUT_H 0x3B
93 | #define MPU_RA_ACCEL_XOUT_L 0x3C
94 | #define MPU_RA_ACCEL_YOUT_H 0x3D
95 | #define MPU_RA_ACCEL_YOUT_L 0x3E
96 | #define MPU_RA_ACCEL_ZOUT_H 0x3F
97 | #define MPU_RA_ACCEL_ZOUT_L 0x40
98 | #define MPU_RA_TEMP_OUT_H 0x41
99 | #define MPU_RA_TEMP_OUT_L 0x42
100 | #define MPU_RA_GYRO_XOUT_H 0x43
101 | #define MPU_RA_GYRO_XOUT_L 0x44
102 | #define MPU_RA_GYRO_YOUT_H 0x45
103 | #define MPU_RA_GYRO_YOUT_L 0x46
104 | #define MPU_RA_GYRO_ZOUT_H 0x47
105 | #define MPU_RA_GYRO_ZOUT_L 0x48
106 | #define MPU_RA_EXT_SENS_DATA_00 0x49
107 | #define MPU_RA_MOT_DETECT_STATUS 0x61
108 | #define MPU_RA_I2C_SLV0_DO 0x63
109 | #define MPU_RA_I2C_SLV1_DO 0x64
110 | #define MPU_RA_I2C_SLV2_DO 0x65
111 | #define MPU_RA_I2C_SLV3_DO 0x66
112 | #define MPU_RA_I2C_MST_DELAY_CTRL 0x67
113 | #define MPU_RA_SIGNAL_PATH_RESET 0x68
114 | #define MPU_RA_MOT_DETECT_CTRL 0x69
115 | #define MPU_RA_USER_CTRL 0x6A
116 | #define MPU_RA_PWR_MGMT_1 0x6B
117 | #define MPU_RA_PWR_MGMT_2 0x6C
118 | #define MPU_RA_BANK_SEL 0x6D
119 | #define MPU_RA_MEM_START_ADDR 0x6E
120 | #define MPU_RA_MEM_R_W 0x6F
121 | #define MPU_RA_DMP_CFG_1 0x70
122 | #define MPU_RA_DMP_CFG_2 0x71
123 | #define MPU_RA_FIFO_COUNTH 0x72
124 | #define MPU_RA_FIFO_COUNTL 0x73
125 | #define MPU_RA_FIFO_R_W 0x74
126 | #define MPU_RA_WHO_AM_I 0x75
127 |
128 | #define MPU6050_SMPLRT_DIV 0 // 8000Hz
129 |
130 | enum lpf_e {
131 | INV_FILTER_256HZ_NOLPF2 = 0,
132 | INV_FILTER_188HZ,
133 | INV_FILTER_98HZ,
134 | INV_FILTER_42HZ,
135 | INV_FILTER_20HZ,
136 | INV_FILTER_10HZ,
137 | INV_FILTER_5HZ,
138 | INV_FILTER_2100HZ_NOLPF,
139 | NUM_FILTER
140 | };
141 | enum gyro_fsr_e {
142 | INV_FSR_250DPS = 0,
143 | INV_FSR_500DPS,
144 | INV_FSR_1000DPS,
145 | INV_FSR_2000DPS,
146 | NUM_GYRO_FSR
147 | };
148 | enum clock_sel_e {
149 | INV_CLK_INTERNAL = 0,
150 | INV_CLK_PLL,
151 | NUM_CLK
152 | };
153 | enum accel_fsr_e {
154 | INV_FSR_2G = 0,
155 | INV_FSR_4G,
156 | INV_FSR_8G,
157 | INV_FSR_16G,
158 | NUM_ACCEL_FSR
159 | };
160 |
161 | static uint8_t mpuLowPassFilter = INV_FILTER_42HZ;
162 | static void mpu6050AccInit(void);
163 | static void mpu6050AccRead(int16_t *accData);
164 | static void mpu6050GyroInit(void);
165 | static void mpu6050GyroRead(int16_t *gyroData);
166 |
167 | // only temporary, until acceleration.c gets built
168 | uint16_t acc_1G;
169 |
170 | typedef enum {
171 | MPU_6050_HALF_RESOLUTION,
172 | MPU_6050_FULL_RESOLUTION
173 | } mpu6050Resolution_e;
174 |
175 | static mpu6050Resolution_e mpuAccelTrim;
176 |
177 | /*
178 | void mpu6050GpioInit(void) {
179 | gpio_config_t gpio;
180 |
181 | if (mpu6050Config->gpioAPB2Peripherals) {
182 | RCC_APB2PeriphClockCmd(mpu6050Config->gpioAPB2Peripherals, ENABLE);
183 | }
184 |
185 | gpio.pin = mpu6050Config->gpioPin;
186 | gpio.speed = Speed_2MHz;
187 | gpio.mode = Mode_IN_FLOATING;
188 | gpioInit(mpu6050Config->gpioPort, &gpio);
189 | }
190 | */
191 |
192 | static bool mpu6050Detect(void)
193 | {
194 | bool ack;
195 | uint8_t sig;
196 |
197 | delay(35); // datasheet page 13 says 30ms. other stuff could have been running meanwhile. but we'll be safe
198 |
199 | ack = i2cRead(MPU6050_ADDRESS, MPU_RA_WHO_AM_I, 1, &sig);
200 | if (!ack)
201 | return false;
202 |
203 | // So like, MPU6xxx has a "WHO_AM_I" register, that is used to verify the identity of the device.
204 | // The contents of WHO_AM_I are the upper 6 bits of the MPU-60X0�s 7-bit I2C address.
205 | // The least significant bit of the MPU-60X0�s I2C address is determined by the value of the AD0 pin. (we know that already).
206 | // But here's the best part: The value of the AD0 pin is not reflected in this register.
207 |
208 | if (sig != (MPU6050_ADDRESS>>1 & 0x7e))
209 | return false;
210 |
211 | return true;
212 | }
213 |
214 | bool mpu6050AccDetect(const mpu6050Config_t *configToUse, acc_t *acc)
215 | {
216 | uint8_t readBuffer[6];
217 | uint8_t revision;
218 | uint8_t productId;
219 |
220 | if (!mpu6050Detect()) {
221 | return false;
222 | }
223 |
224 | // There is a map of revision contained in the android source tree which is quite comprehensive and may help to understand this code
225 | // See https://android.googlesource.com/kernel/msm.git/+/eaf36994a3992b8f918c18e4f7411e8b2320a35f/drivers/misc/mpu6050/mldl_cfg.c
226 |
227 | // determine product ID and accel revision
228 | i2cRead(MPU6050_ADDRESS, MPU_RA_XA_OFFS_H, 6, readBuffer);
229 | revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01);
230 | if (revision) {
231 | /* Congrats, these parts are better. */
232 | if (revision == 1) {
233 | mpuAccelTrim = MPU_6050_HALF_RESOLUTION;
234 | } else if (revision == 2) {
235 | mpuAccelTrim = MPU_6050_FULL_RESOLUTION;
236 | } else {
237 | // I'll have to make a new one..
238 | //failureMode(5);
239 | }
240 | } else {
241 | i2cRead(MPU6050_ADDRESS, MPU_RA_PRODUCT_ID, 1, &productId);
242 | revision = productId & 0x0F;
243 | if (!revision) {
244 | //failureMode(5);
245 | } else if (revision == 4) {
246 | mpuAccelTrim = MPU_6050_HALF_RESOLUTION;
247 | } else {
248 | mpuAccelTrim = MPU_6050_FULL_RESOLUTION;
249 | }
250 | }
251 |
252 | acc->init = mpu6050AccInit;
253 | acc->read = mpu6050AccRead;
254 | acc->revisionCode = (mpuAccelTrim == MPU_6050_HALF_RESOLUTION ? 'o' : 'n'); // es/non-es variance between MPU6050 sensors, half of the naze boards are mpu6000ES.
255 |
256 | return true;
257 | }
258 |
259 | bool mpu6050GyroDetect(const mpu6050Config_t *configToUse, gyro_t *gyro, uint16_t lpf)
260 | {
261 | if (!mpu6050Detect()) {
262 | return false;
263 | }
264 |
265 |
266 | gyro->init = mpu6050GyroInit;
267 | gyro->read = mpu6050GyroRead;
268 |
269 | // 16.4 dps/lsb scalefactor
270 | gyro->scale = 1.0f / 16.4f;
271 |
272 | if (lpf >= 188)
273 | mpuLowPassFilter = INV_FILTER_188HZ;
274 | else if (lpf >= 98)
275 | mpuLowPassFilter = INV_FILTER_98HZ;
276 | else if (lpf >= 42)
277 | mpuLowPassFilter = INV_FILTER_42HZ;
278 | else if (lpf >= 20)
279 | mpuLowPassFilter = INV_FILTER_20HZ;
280 | else if (lpf >= 10)
281 | mpuLowPassFilter = INV_FILTER_10HZ;
282 | else
283 | mpuLowPassFilter = INV_FILTER_5HZ;
284 |
285 | return true;
286 | }
287 |
288 | static void mpu6050AccInit(void)
289 | {
290 | /*
291 | if (mpu6050Config) {
292 | mpu6050GpioInit();
293 | mpu6050Config = NULL; // avoid re-initialisation of GPIO;
294 | }
295 | */
296 |
297 | switch (mpuAccelTrim) {
298 | case MPU_6050_HALF_RESOLUTION:
299 | acc_1G = 256 * 8;
300 | break;
301 | case MPU_6050_FULL_RESOLUTION:
302 | acc_1G = 512 * 8;
303 | break;
304 | }
305 | }
306 |
307 | static void mpu6050AccRead(int16_t *accData)
308 | {
309 | uint8_t buf[6];
310 |
311 | i2cRead(MPU6050_ADDRESS, MPU_RA_ACCEL_XOUT_H, 6, buf);
312 | accData[0] = (int16_t)((buf[0] << 8) | buf[1]);
313 | accData[1] = (int16_t)((buf[2] << 8) | buf[3]);
314 | accData[2] = (int16_t)((buf[4] << 8) | buf[5]);
315 | }
316 |
317 | static void mpu6050GyroInit(void)
318 | {
319 | /*
320 | if (mpu6050Config) {
321 | mpu6050GpioInit();
322 | mpu6050Config = NULL; // avoid re-initialisation of GPIO;
323 | }
324 | */
325 |
326 | i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
327 | delay(100);
328 | i2cWrite(MPU6050_ADDRESS, MPU_RA_SMPLRT_DIV, 0x00); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
329 | i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
330 | i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_PIN_CFG,
331 | 0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS
332 | i2cWrite(MPU6050_ADDRESS, MPU_RA_CONFIG, mpuLowPassFilter); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
333 | i2cWrite(MPU6050_ADDRESS, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
334 |
335 | // ACC Init stuff. Moved into gyro init because the reset above would screw up accel config. Oops.
336 | // Accel scale 8g (4096 LSB/g)
337 | i2cWrite(MPU6050_ADDRESS, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
338 | }
339 |
340 | static void mpu6050GyroRead(int16_t *gyroData)
341 | {
342 | uint8_t buf[6];
343 |
344 | i2cRead(MPU6050_ADDRESS, MPU_RA_GYRO_XOUT_H, 6, buf);
345 |
346 | gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]);
347 | gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]);
348 | gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]);
349 | }
350 |
--------------------------------------------------------------------------------
/src/system_stm32f0xx.c:
--------------------------------------------------------------------------------
1 | /**
2 | ******************************************************************************
3 | * @file system_stm32f0xx.c
4 | * @author MCD Application Team
5 | * @version V1.0.0
6 | * @date 23-March-2012
7 | * @brief CMSIS Cortex-M0 Device Peripheral Access Layer System Source File.
8 | * This file contains the system clock configuration for STM32F0xx devices,
9 | * and is customized for use with STM32F0-DISCOVERY Kit.
10 | * The STM32F0xx is configured to run at 48 MHz, following the three
11 | * configuration below:
12 | * - PLL_SOURCE_HSI (default): HSI (~8MHz) used to clock the PLL, and
13 | * the PLL is used as system clock source.
14 | * - PLL_SOURCE_HSE : HSE (8MHz) used to clock the PLL, and
15 | * the PLL is used as system clock source.
16 | * - PLL_SOURCE_HSE_BYPASS : HSE bypassed with an external clock
17 | * (8MHz, coming from ST-Link) used to clock
18 | * the PLL, and the PLL is used as system
19 | * clock source.
20 | *
21 | *
22 | * 1. This file provides two functions and one global variable to be called from
23 | * user application:
24 | * - SystemInit(): Setups the system clock (System clock source, PLL Multiplier
25 | * and Divider factors, AHB/APBx prescalers and Flash settings),
26 | * depending on the configuration selected (see above).
27 | * This function is called at startup just after reset and
28 | * before branch to main program. This call is made inside
29 | * the "startup_stm32f0xx.s" file.
30 | *
31 | * - SystemCoreClock variable: Contains the core clock (HCLK), it can be used
32 | * by the user application to setup the SysTick
33 | * timer or configure other parameters.
34 | *
35 | * - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must
36 | * be called whenever the core clock is changed
37 | * during program execution.
38 | *
39 | * 2. After each device reset the HSI (8 MHz Range) is used as system clock source.
40 | * Then SystemInit() function is called, in "startup_stm32f0xx.s" file, to
41 | * configure the system clock before to branch to main program.
42 | *
43 | * 3. If the system clock source selected by user fails to startup, the SystemInit()
44 | * function will do nothing and HSI still used as system clock source. User can
45 | * add some code to deal with this issue inside the SetSysClock() function.
46 | *
47 | * 4. The default value of HSE crystal is set to 8MHz, refer to "HSE_VALUE" define
48 | * in "stm32f0xx.h" file. When HSE is used as system clock source, directly or
49 | * through PLL, and you are using different crystal you have to adapt the HSE
50 | * value to your own configuration.
51 | *
52 | ******************************************************************************
53 | * @attention
54 | *
55 | * © COPYRIGHT 2012 STMicroelectronics
56 | *
57 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License");
58 | * You may not use this file except in compliance with the License.
59 | * You may obtain a copy of the License at:
60 | *
61 | * http://www.st.com/software_license_agreement_liberty_v2
62 | *
63 | * Unless required by applicable law or agreed to in writing, software
64 | * distributed under the License is distributed on an "AS IS" BASIS,
65 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
66 | * See the License for the specific language governing permissions and
67 | * limitations under the License.
68 | *
69 | ******************************************************************************
70 | */
71 |
72 | /** @addtogroup CMSIS
73 | * @{
74 | */
75 |
76 | /** @addtogroup stm32f0xx_system
77 | * @{
78 | */
79 |
80 | /** @addtogroup STM32F0xx_System_Private_Includes
81 | * @{
82 | */
83 |
84 | #include "stm32f0xx.h"
85 |
86 | /**
87 | * @}
88 | */
89 |
90 | /** @addtogroup STM32F0xx_System_Private_TypesDefinitions
91 | * @{
92 | */
93 |
94 | /**
95 | * @}
96 | */
97 |
98 | /** @addtogroup STM32F0xx_System_Private_Defines
99 | * @{
100 | */
101 | /* Select the PLL clock source */
102 |
103 | #define PLL_SOURCE_HSI // HSI (~8MHz) used to clock the PLL, and the PLL is used as system clock source
104 | //#define PLL_SOURCE_HSE // HSE (8MHz) used to clock the PLL, and the PLL is used as system clock source
105 | //#define PLL_SOURCE_HSE_BYPASS // HSE bypassed with an external clock (8MHz, coming from ST-Link) used to clock
106 | // the PLL, and the PLL is used as system clock source
107 |
108 | /**
109 | * @}
110 | */
111 |
112 | /** @addtogroup STM32F0xx_System_Private_Macros
113 | * @{
114 | */
115 |
116 | /**
117 | * @}
118 | */
119 |
120 | /** @addtogroup STM32F0xx_System_Private_Variables
121 | * @{
122 | */
123 | uint32_t SystemCoreClock = 48000000;
124 | __I uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9};
125 |
126 | /**
127 | * @}
128 | */
129 |
130 | /** @addtogroup STM32F0xx_System_Private_FunctionPrototypes
131 | * @{
132 | */
133 |
134 | static void SetSysClock(void);
135 |
136 | /**
137 | * @}
138 | */
139 |
140 | /** @addtogroup STM32F0xx_System_Private_Functions
141 | * @{
142 | */
143 |
144 | /**
145 | * @brief Setup the microcontroller system.
146 | * Initialize the Embedded Flash Interface, the PLL and update the
147 | * SystemCoreClock variable.
148 | * @param None
149 | * @retval None
150 | */
151 | void SystemInit (void)
152 | {
153 | /* Set HSION bit */
154 | RCC->CR |= (uint32_t)0x00000001;
155 |
156 | /* Reset SW[1:0], HPRE[3:0], PPRE[2:0], ADCPRE and MCOSEL[2:0] bits */
157 | RCC->CFGR &= (uint32_t)0xF8FFB80C;
158 |
159 | /* Reset HSEON, CSSON and PLLON bits */
160 | RCC->CR &= (uint32_t)0xFEF6FFFF;
161 |
162 | /* Reset HSEBYP bit */
163 | RCC->CR &= (uint32_t)0xFFFBFFFF;
164 |
165 | /* Reset PLLSRC, PLLXTPRE and PLLMUL[3:0] bits */
166 | RCC->CFGR &= (uint32_t)0xFFC0FFFF;
167 |
168 | /* Reset PREDIV1[3:0] bits */
169 | RCC->CFGR2 &= (uint32_t)0xFFFFFFF0;
170 |
171 | /* Reset USARTSW[1:0], I2CSW, CECSW and ADCSW bits */
172 | RCC->CFGR3 &= (uint32_t)0xFFFFFEAC;
173 |
174 | /* Reset HSI14 bit */
175 | RCC->CR2 &= (uint32_t)0xFFFFFFFE;
176 |
177 | /* Disable all interrupts */
178 | RCC->CIR = 0x00000000;
179 |
180 | /* Configure the System clock frequency, AHB/APBx prescalers and Flash settings */
181 | SetSysClock();
182 | }
183 |
184 | /**
185 | * @brief Update SystemCoreClock according to Clock Register Values
186 | * The SystemCoreClock variable contains the core clock (HCLK), it can
187 | * be used by the user application to setup the SysTick timer or configure
188 | * other parameters.
189 | *
190 | * @note Each time the core clock (HCLK) changes, this function must be called
191 | * to update SystemCoreClock variable value. Otherwise, any configuration
192 | * based on this variable will be incorrect.
193 | *
194 | * @note - The system frequency computed by this function is not the real
195 | * frequency in the chip. It is calculated based on the predefined
196 | * constant and the selected clock source:
197 | *
198 | * - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*)
199 | *
200 | * - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**)
201 | *
202 | * - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**)
203 | * or HSI_VALUE(*) multiplied/divided by the PLL factors.
204 | *
205 | * (*) HSI_VALUE is a constant defined in stm32f0xx.h file (default value
206 | * 8 MHz) but the real value may vary depending on the variations
207 | * in voltage and temperature.
208 | *
209 | * (**) HSE_VALUE is a constant defined in stm32f0xx.h file (default value
210 | * 8 MHz), user has to ensure that HSE_VALUE is same as the real
211 | * frequency of the crystal used. Otherwise, this function may
212 | * have wrong result.
213 | *
214 | * - The result of this function could be not correct when using fractional
215 | * value for HSE crystal.
216 | * @param None
217 | * @retval None
218 | */
219 | void SystemCoreClockUpdate (void)
220 | {
221 | uint32_t tmp = 0, pllmull = 0, pllsource = 0, prediv1factor = 0;
222 |
223 | /* Get SYSCLK source -------------------------------------------------------*/
224 | tmp = RCC->CFGR & RCC_CFGR_SWS;
225 |
226 | switch (tmp)
227 | {
228 | case 0x00: /* HSI used as system clock */
229 | SystemCoreClock = HSI_VALUE;
230 | break;
231 | case 0x04: /* HSE used as system clock */
232 | SystemCoreClock = HSE_VALUE;
233 | break;
234 | case 0x08: /* PLL used as system clock */
235 | /* Get PLL clock source and multiplication factor ----------------------*/
236 | pllmull = RCC->CFGR & RCC_CFGR_PLLMULL;
237 | pllsource = RCC->CFGR & RCC_CFGR_PLLSRC;
238 | pllmull = ( pllmull >> 18) + 2;
239 |
240 | if (pllsource == 0x00)
241 | {
242 | /* HSI oscillator clock divided by 2 selected as PLL clock entry */
243 | SystemCoreClock = (HSI_VALUE >> 1) * pllmull;
244 | }
245 | else
246 | {
247 | prediv1factor = (RCC->CFGR2 & RCC_CFGR2_PREDIV1) + 1;
248 | /* HSE oscillator clock selected as PREDIV1 clock entry */
249 | SystemCoreClock = (HSE_VALUE / prediv1factor) * pllmull;
250 | }
251 | break;
252 | default: /* HSI used as system clock */
253 | SystemCoreClock = HSI_VALUE;
254 | break;
255 | }
256 | /* Compute HCLK clock frequency ----------------*/
257 | /* Get HCLK prescaler */
258 | tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)];
259 | /* HCLK clock frequency */
260 | SystemCoreClock >>= tmp;
261 | }
262 |
263 | /**
264 | * @brief Configures the System clock frequency, AHB/APBx prescalers and Flash
265 | * settings.
266 | * @note This function should be called only once the RCC clock configuration
267 | * is reset to the default reset state (done in SystemInit() function).
268 | * @param None
269 | * @retval None
270 | */
271 | static void SetSysClock(void)
272 | {
273 | #if defined (PLL_SOURCE_HSE)
274 | __IO uint32_t StartUpCounter = 0, HSEStatus = 0;
275 | #endif
276 |
277 | /* SYSCLK, HCLK, PCLK configuration ----------------------------------------*/
278 | #if defined (PLL_SOURCE_HSI)
279 | /* At this stage the HSI is already enabled */
280 |
281 | /* Enable Prefetch Buffer and set Flash Latency */
282 | FLASH->ACR = FLASH_ACR_PRFTBE | FLASH_ACR_LATENCY;
283 |
284 | /* HCLK = SYSCLK */
285 | RCC->CFGR |= (uint32_t)RCC_CFGR_HPRE_DIV1;
286 |
287 | /* PCLK = HCLK */
288 | RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE_DIV1;
289 |
290 | /* PLL configuration = (HSI/2) * 12 = ~48 MHz */
291 | RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_PLLSRC | RCC_CFGR_PLLXTPRE | RCC_CFGR_PLLMULL));
292 | RCC->CFGR |= (uint32_t)(RCC_CFGR_PLLSRC_HSI_Div2 | RCC_CFGR_PLLXTPRE_PREDIV1 | RCC_CFGR_PLLMULL12);
293 |
294 | /* Enable PLL */
295 | RCC->CR |= RCC_CR_PLLON;
296 |
297 | /* Wait till PLL is ready */
298 | while((RCC->CR & RCC_CR_PLLRDY) == 0)
299 | {
300 | }
301 |
302 | /* Select PLL as system clock source */
303 | RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW));
304 | RCC->CFGR |= (uint32_t)RCC_CFGR_SW_PLL;
305 |
306 | /* Wait till PLL is used as system clock source */
307 | while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS) != (uint32_t)RCC_CFGR_SWS_PLL)
308 | {
309 | }
310 | #else
311 | #if defined (PLL_SOURCE_HSE)
312 | /* Enable HSE */
313 | RCC->CR |= ((uint32_t)RCC_CR_HSEON);
314 | #elif defined (PLL_SOURCE_HSE_BYPASS)
315 | /* HSE oscillator bypassed with external clock */
316 | RCC->CR |= (uint32_t)(RCC_CR_HSEON | RCC_CR_HSEBYP);
317 | #endif /* PLL_SOURCE_HSE */
318 |
319 | /* Wait till HSE is ready and if Time out is reached exit */
320 | do
321 | {
322 | HSEStatus = RCC->CR & RCC_CR_HSERDY;
323 | StartUpCounter++;
324 | } while((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT));
325 |
326 | if ((RCC->CR & RCC_CR_HSERDY) != RESET)
327 | {
328 | HSEStatus = (uint32_t)0x01;
329 | }
330 | else
331 | {
332 | HSEStatus = (uint32_t)0x00;
333 | }
334 |
335 | if (HSEStatus == (uint32_t)0x01)
336 | {
337 | /* Enable Prefetch Buffer and set Flash Latency */
338 | FLASH->ACR = FLASH_ACR_PRFTBE | FLASH_ACR_LATENCY;
339 |
340 | /* HCLK = SYSCLK */
341 | RCC->CFGR |= (uint32_t)RCC_CFGR_HPRE_DIV1;
342 |
343 | /* PCLK = HCLK */
344 | RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE_DIV1;
345 |
346 | /* PLL configuration = HSE * 6 = 48 MHz */
347 | RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_PLLSRC | RCC_CFGR_PLLXTPRE | RCC_CFGR_PLLMULL));
348 | RCC->CFGR |= (uint32_t)(RCC_CFGR_PLLSRC_PREDIV1 | RCC_CFGR_PLLXTPRE_PREDIV1 | RCC_CFGR_PLLMULL6);
349 |
350 | /* Enable PLL */
351 | RCC->CR |= RCC_CR_PLLON;
352 |
353 | /* Wait till PLL is ready */
354 | while((RCC->CR & RCC_CR_PLLRDY) == 0)
355 | {
356 | }
357 |
358 | /* Select PLL as system clock source */
359 | RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW));
360 | RCC->CFGR |= (uint32_t)RCC_CFGR_SW_PLL;
361 |
362 | /* Wait till PLL is used as system clock source */
363 | while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS) != (uint32_t)RCC_CFGR_SWS_PLL)
364 | {
365 | }
366 | }
367 | else
368 | { /* If HSE fails to start-up, the application will have wrong clock
369 | configuration. User can add here some code to deal with this error */
370 | }
371 | #endif /* PLL_SOURCE_HSI */
372 | }
373 |
374 | /**
375 | * @}
376 | */
377 |
378 | /**
379 | * @}
380 | */
381 |
382 | /**
383 | * @}
384 | */
385 |
386 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
387 |
--------------------------------------------------------------------------------
/inc/stm32f0xx_gpio.h:
--------------------------------------------------------------------------------
1 | /**
2 | ******************************************************************************
3 | * @file stm32f0xx_gpio.h
4 | * @author MCD Application Team
5 | * @version V1.4.0
6 | * @date 24-July-2014
7 | * @brief This file contains all the functions prototypes for the GPIO
8 | * firmware library.
9 | ******************************************************************************
10 | * @attention
11 | *
12 | * © COPYRIGHT 2014 STMicroelectronics
13 | *
14 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License");
15 | * You may not use this file except in compliance with the License.
16 | * You may obtain a copy of the License at:
17 | *
18 | * http://www.st.com/software_license_agreement_liberty_v2
19 | *
20 | * Unless required by applicable law or agreed to in writing, software
21 | * distributed under the License is distributed on an "AS IS" BASIS,
22 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
23 | * See the License for the specific language governing permissions and
24 | * limitations under the License.
25 | *
26 | ******************************************************************************
27 | */
28 |
29 | /* Define to prevent recursive inclusion -------------------------------------*/
30 | #ifndef __STM32F0XX_GPIO_H
31 | #define __STM32F0XX_GPIO_H
32 |
33 | #ifdef __cplusplus
34 | extern "C" {
35 | #endif
36 |
37 | /* Includes ------------------------------------------------------------------*/
38 | #include "stm32f0xx.h"
39 |
40 | /** @addtogroup STM32F0xx_StdPeriph_Driver
41 | * @{
42 | */
43 |
44 | /** @addtogroup GPIO
45 | * @{
46 | */
47 | /* Exported types ------------------------------------------------------------*/
48 |
49 | #define IS_GPIO_ALL_PERIPH(PERIPH) (((PERIPH) == GPIOA) || \
50 | ((PERIPH) == GPIOB) || \
51 | ((PERIPH) == GPIOC) || \
52 | ((PERIPH) == GPIOD) || \
53 | ((PERIPH) == GPIOE) || \
54 | ((PERIPH) == GPIOF))
55 |
56 | #define IS_GPIO_LIST_PERIPH(PERIPH) (((PERIPH) == GPIOA) || \
57 | ((PERIPH) == GPIOB))
58 |
59 | /** @defgroup Configuration_Mode_enumeration
60 | * @{
61 | */
62 | typedef enum
63 | {
64 | GPIO_Mode_IN = 0x00, /*!< GPIO Input Mode */
65 | GPIO_Mode_OUT = 0x01, /*!< GPIO Output Mode */
66 | GPIO_Mode_AF = 0x02, /*!< GPIO Alternate function Mode */
67 | GPIO_Mode_AN = 0x03 /*!< GPIO Analog In/Out Mode */
68 | }GPIOMode_TypeDef;
69 |
70 | #define IS_GPIO_MODE(MODE) (((MODE) == GPIO_Mode_IN)|| ((MODE) == GPIO_Mode_OUT) || \
71 | ((MODE) == GPIO_Mode_AF)|| ((MODE) == GPIO_Mode_AN))
72 | /**
73 | * @}
74 | */
75 |
76 | /** @defgroup Output_type_enumeration
77 | * @{
78 | */
79 | typedef enum
80 | {
81 | GPIO_OType_PP = 0x00,
82 | GPIO_OType_OD = 0x01
83 | }GPIOOType_TypeDef;
84 |
85 | #define IS_GPIO_OTYPE(OTYPE) (((OTYPE) == GPIO_OType_PP) || ((OTYPE) == GPIO_OType_OD))
86 |
87 | /**
88 | * @}
89 | */
90 |
91 | /** @defgroup Output_Maximum_frequency_enumeration
92 | * @{
93 | */
94 | typedef enum
95 | {
96 | GPIO_Speed_Level_1 = 0x00, /*!< I/O output speed: Low 2 MHz */
97 | GPIO_Speed_Level_2 = 0x01, /*!< I/O output speed: Medium 10 MHz */
98 | GPIO_Speed_Level_3 = 0x03 /*!< I/O output speed: High 50 MHz */
99 | }GPIOSpeed_TypeDef;
100 |
101 | #define IS_GPIO_SPEED(SPEED) (((SPEED) == GPIO_Speed_Level_1) || ((SPEED) == GPIO_Speed_Level_2) || \
102 | ((SPEED) == GPIO_Speed_Level_3))
103 | /**
104 | * @}
105 | */
106 |
107 | /** @defgroup Configuration_Pull-Up_Pull-Down_enumeration
108 | * @{
109 | */
110 | typedef enum
111 | {
112 | GPIO_PuPd_NOPULL = 0x00,
113 | GPIO_PuPd_UP = 0x01,
114 | GPIO_PuPd_DOWN = 0x02
115 | }GPIOPuPd_TypeDef;
116 |
117 | #define IS_GPIO_PUPD(PUPD) (((PUPD) == GPIO_PuPd_NOPULL) || ((PUPD) == GPIO_PuPd_UP) || \
118 | ((PUPD) == GPIO_PuPd_DOWN))
119 | /**
120 | * @}
121 | */
122 |
123 | /** @defgroup Bit_SET_and_Bit_RESET_enumeration
124 | * @{
125 | */
126 | typedef enum
127 | {
128 | Bit_RESET = 0,
129 | Bit_SET
130 | }BitAction;
131 |
132 | #define IS_GPIO_BIT_ACTION(ACTION) (((ACTION) == Bit_RESET) || ((ACTION) == Bit_SET))
133 | /**
134 | * @}
135 | */
136 |
137 | /**
138 | * @brief GPIO Init structure definition
139 | */
140 | typedef struct
141 | {
142 | uint32_t GPIO_Pin; /*!< Specifies the GPIO pins to be configured.
143 | This parameter can be any value of @ref GPIO_pins_define */
144 |
145 | GPIOMode_TypeDef GPIO_Mode; /*!< Specifies the operating mode for the selected pins.
146 | This parameter can be a value of @ref GPIOMode_TypeDef */
147 |
148 | GPIOSpeed_TypeDef GPIO_Speed; /*!< Specifies the speed for the selected pins.
149 | This parameter can be a value of @ref GPIOSpeed_TypeDef */
150 |
151 | GPIOOType_TypeDef GPIO_OType; /*!< Specifies the operating output type for the selected pins.
152 | This parameter can be a value of @ref GPIOOType_TypeDef */
153 |
154 | GPIOPuPd_TypeDef GPIO_PuPd; /*!< Specifies the operating Pull-up/Pull down for the selected pins.
155 | This parameter can be a value of @ref GPIOPuPd_TypeDef */
156 | }GPIO_InitTypeDef;
157 |
158 | /* Exported constants --------------------------------------------------------*/
159 |
160 | /** @defgroup GPIO_Exported_Constants
161 | * @{
162 | */
163 |
164 | /** @defgroup GPIO_pins_define
165 | * @{
166 | */
167 | #define GPIO_Pin_0 ((uint16_t)0x0001) /*!< Pin 0 selected */
168 | #define GPIO_Pin_1 ((uint16_t)0x0002) /*!< Pin 1 selected */
169 | #define GPIO_Pin_2 ((uint16_t)0x0004) /*!< Pin 2 selected */
170 | #define GPIO_Pin_3 ((uint16_t)0x0008) /*!< Pin 3 selected */
171 | #define GPIO_Pin_4 ((uint16_t)0x0010) /*!< Pin 4 selected */
172 | #define GPIO_Pin_5 ((uint16_t)0x0020) /*!< Pin 5 selected */
173 | #define GPIO_Pin_6 ((uint16_t)0x0040) /*!< Pin 6 selected */
174 | #define GPIO_Pin_7 ((uint16_t)0x0080) /*!< Pin 7 selected */
175 | #define GPIO_Pin_8 ((uint16_t)0x0100) /*!< Pin 8 selected */
176 | #define GPIO_Pin_9 ((uint16_t)0x0200) /*!< Pin 9 selected */
177 | #define GPIO_Pin_10 ((uint16_t)0x0400) /*!< Pin 10 selected */
178 | #define GPIO_Pin_11 ((uint16_t)0x0800) /*!< Pin 11 selected */
179 | #define GPIO_Pin_12 ((uint16_t)0x1000) /*!< Pin 12 selected */
180 | #define GPIO_Pin_13 ((uint16_t)0x2000) /*!< Pin 13 selected */
181 | #define GPIO_Pin_14 ((uint16_t)0x4000) /*!< Pin 14 selected */
182 | #define GPIO_Pin_15 ((uint16_t)0x8000) /*!< Pin 15 selected */
183 | #define GPIO_Pin_All ((uint16_t)0xFFFF) /*!< All pins selected */
184 |
185 | #define IS_GPIO_PIN(PIN) ((PIN) != (uint16_t)0x00)
186 |
187 | #define IS_GET_GPIO_PIN(PIN) (((PIN) == GPIO_Pin_0) || \
188 | ((PIN) == GPIO_Pin_1) || \
189 | ((PIN) == GPIO_Pin_2) || \
190 | ((PIN) == GPIO_Pin_3) || \
191 | ((PIN) == GPIO_Pin_4) || \
192 | ((PIN) == GPIO_Pin_5) || \
193 | ((PIN) == GPIO_Pin_6) || \
194 | ((PIN) == GPIO_Pin_7) || \
195 | ((PIN) == GPIO_Pin_8) || \
196 | ((PIN) == GPIO_Pin_9) || \
197 | ((PIN) == GPIO_Pin_10) || \
198 | ((PIN) == GPIO_Pin_11) || \
199 | ((PIN) == GPIO_Pin_12) || \
200 | ((PIN) == GPIO_Pin_13) || \
201 | ((PIN) == GPIO_Pin_14) || \
202 | ((PIN) == GPIO_Pin_15))
203 |
204 | /**
205 | * @}
206 | */
207 |
208 | /** @defgroup GPIO_Pin_sources
209 | * @{
210 | */
211 | #define GPIO_PinSource0 ((uint8_t)0x00)
212 | #define GPIO_PinSource1 ((uint8_t)0x01)
213 | #define GPIO_PinSource2 ((uint8_t)0x02)
214 | #define GPIO_PinSource3 ((uint8_t)0x03)
215 | #define GPIO_PinSource4 ((uint8_t)0x04)
216 | #define GPIO_PinSource5 ((uint8_t)0x05)
217 | #define GPIO_PinSource6 ((uint8_t)0x06)
218 | #define GPIO_PinSource7 ((uint8_t)0x07)
219 | #define GPIO_PinSource8 ((uint8_t)0x08)
220 | #define GPIO_PinSource9 ((uint8_t)0x09)
221 | #define GPIO_PinSource10 ((uint8_t)0x0A)
222 | #define GPIO_PinSource11 ((uint8_t)0x0B)
223 | #define GPIO_PinSource12 ((uint8_t)0x0C)
224 | #define GPIO_PinSource13 ((uint8_t)0x0D)
225 | #define GPIO_PinSource14 ((uint8_t)0x0E)
226 | #define GPIO_PinSource15 ((uint8_t)0x0F)
227 |
228 | #define IS_GPIO_PIN_SOURCE(PINSOURCE) (((PINSOURCE) == GPIO_PinSource0) || \
229 | ((PINSOURCE) == GPIO_PinSource1) || \
230 | ((PINSOURCE) == GPIO_PinSource2) || \
231 | ((PINSOURCE) == GPIO_PinSource3) || \
232 | ((PINSOURCE) == GPIO_PinSource4) || \
233 | ((PINSOURCE) == GPIO_PinSource5) || \
234 | ((PINSOURCE) == GPIO_PinSource6) || \
235 | ((PINSOURCE) == GPIO_PinSource7) || \
236 | ((PINSOURCE) == GPIO_PinSource8) || \
237 | ((PINSOURCE) == GPIO_PinSource9) || \
238 | ((PINSOURCE) == GPIO_PinSource10) || \
239 | ((PINSOURCE) == GPIO_PinSource11) || \
240 | ((PINSOURCE) == GPIO_PinSource12) || \
241 | ((PINSOURCE) == GPIO_PinSource13) || \
242 | ((PINSOURCE) == GPIO_PinSource14) || \
243 | ((PINSOURCE) == GPIO_PinSource15))
244 | /**
245 | * @}
246 | */
247 |
248 | /** @defgroup GPIO_Alternate_function_selection_define
249 | * @{
250 | */
251 |
252 | /**
253 | * @brief AF 0 selection
254 | */
255 | #define GPIO_AF_0 ((uint8_t)0x00) /* WKUP, EVENTOUT, TIM15, SPI1, TIM17,
256 | MCO, SWDAT, SWCLK, TIM14, BOOT,
257 | USART1, CEC, IR_OUT, SPI2, TS, TIM3,
258 | USART4, CAN, TIM3, USART2, USART3,
259 | CRS, TIM16, TIM1 */
260 | /**
261 | * @brief AF 1 selection
262 | */
263 | #define GPIO_AF_1 ((uint8_t)0x01) /* USART2, CEC, TIM3, USART1, IR,
264 | EVENTOUT, I2C1, I2C2, TIM15, SPI2,
265 | USART3, TS, SPI1 */
266 | /**
267 | * @brief AF 2 selection
268 | */
269 | #define GPIO_AF_2 ((uint8_t)0x02) /* TIM2, TIM1, EVENTOUT, TIM16, TIM17,
270 | USB */
271 | /**
272 | * @brief AF 3 selection
273 | */
274 | #define GPIO_AF_3 ((uint8_t)0x03) /* TS, I2C1, TIM15, EVENTOUT */
275 |
276 | /**
277 | * @brief AF 4 selection
278 | */
279 | #define GPIO_AF_4 ((uint8_t)0x04) /* TIM14, USART4, USART3, CRS, CAN,
280 | I2C1 */
281 |
282 | /**
283 | * @brief AF 5 selection
284 | */
285 | #define GPIO_AF_5 ((uint8_t)0x05) /* TIM16, TIM17, TIM15, SPI2, I2C2,
286 | MCO, I2C1, USB */
287 |
288 | /**
289 | * @brief AF 6 selection
290 | */
291 | #define GPIO_AF_6 ((uint8_t)0x06) /* EVENTOUT */
292 | /**
293 | * @brief AF 7 selection
294 | */
295 | #define GPIO_AF_7 ((uint8_t)0x07) /* COMP1 OUT and COMP2 OUT */
296 |
297 | #define IS_GPIO_AF(AF) (((AF) == GPIO_AF_0) || ((AF) == GPIO_AF_1) || \
298 | ((AF) == GPIO_AF_2) || ((AF) == GPIO_AF_3) || \
299 | ((AF) == GPIO_AF_4) || ((AF) == GPIO_AF_5) || \
300 | ((AF) == GPIO_AF_6) || ((AF) == GPIO_AF_7))
301 |
302 | /**
303 | * @}
304 | */
305 |
306 | /** @defgroup GPIO_Speed_Legacy
307 | * @{
308 | */
309 |
310 | #define GPIO_Speed_2MHz GPIO_Speed_Level_1 /*!< I/O output speed: Low 2 MHz */
311 | #define GPIO_Speed_10MHz GPIO_Speed_Level_2 /*!< I/O output speed: Medium 10 MHz */
312 | #define GPIO_Speed_50MHz GPIO_Speed_Level_3 /*!< I/O output speed: High 50 MHz */
313 |
314 | /**
315 | * @}
316 | */
317 |
318 | /**
319 | * @}
320 | */
321 |
322 | /* Exported macro ------------------------------------------------------------*/
323 | /* Exported functions ------------------------------------------------------- */
324 | /* Function used to set the GPIO configuration to the default reset state *****/
325 | void GPIO_DeInit(GPIO_TypeDef* GPIOx);
326 |
327 | /* Initialization and Configuration functions *********************************/
328 | void GPIO_Init(GPIO_TypeDef* GPIOx, GPIO_InitTypeDef* GPIO_InitStruct);
329 | void GPIO_StructInit(GPIO_InitTypeDef* GPIO_InitStruct);
330 | void GPIO_PinLockConfig(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin);
331 |
332 | /* GPIO Read and Write functions **********************************************/
333 | uint8_t GPIO_ReadInputDataBit(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin);
334 | uint16_t GPIO_ReadInputData(GPIO_TypeDef* GPIOx);
335 | uint8_t GPIO_ReadOutputDataBit(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin);
336 | uint16_t GPIO_ReadOutputData(GPIO_TypeDef* GPIOx);
337 | void GPIO_SetBits(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin);
338 | void GPIO_ResetBits(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin);
339 | void GPIO_WriteBit(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin, BitAction BitVal);
340 | void GPIO_Write(GPIO_TypeDef* GPIOx, uint16_t PortVal);
341 |
342 | /* GPIO Alternate functions configuration functions ***************************/
343 | void GPIO_PinAFConfig(GPIO_TypeDef* GPIOx, uint16_t GPIO_PinSource, uint8_t GPIO_AF);
344 |
345 | #ifdef __cplusplus
346 | }
347 | #endif
348 |
349 | #endif /* __STM32F0XX_GPIO_H */
350 | /**
351 | * @}
352 | */
353 |
354 | /**
355 | * @}
356 | */
357 |
358 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
359 |
--------------------------------------------------------------------------------
/startup/startup_stm32f031.lst:
--------------------------------------------------------------------------------
1 | ARM GAS /tmp/ccSm57Oe.s page 1
2 |
3 |
4 | 1 # 1 "startup/startup_stm32f031.s"
5 | 1 /**
6 | 1 ...
7 | 0
8 | 0
9 | 2 ******************************************************************************
10 | 3 * @file startup_stm32f0xx.s
11 | 4 * @author MCD Application Team
12 | 5 * @version V1.4.0
13 | 6 * @date 24-July-2014
14 | 7 * @brief STM32F031 Devices vector table for RIDE7 toolchain.
15 | 8 * This module performs:
16 | 9 * - Set the initial SP
17 | 10 * - Set the initial PC == Reset_Handler,
18 | 11 * - Set the vector table entries with the exceptions ISR address
19 | 12 * - Configure the system clock
20 | 13 * - Branches to main in the C library (which eventually
21 | 14 * calls main()).
22 | 15 * After Reset the Cortex-M0 processor is in Thread mode,
23 | 16 * priority is Privileged, and the Stack is set to Main.
24 | 17 ******************************************************************************
25 | 18 * @attention
26 | 19 *
27 | 20 * © COPYRIGHT 2014 STMicroelectronics
28 | 21 *
29 | 22 * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License");
30 | 23 * You may not use this file except in compliance with the License.
31 | 24 * You may obtain a copy of the License at:
32 | 25 *
33 | 26 * http://www.st.com/software_license_agreement_liberty_v2
34 | 27 *
35 | 28 * Unless required by applicable law or agreed to in writing, software
36 | 29 * distributed under the License is distributed on an "AS IS" BASIS,
37 | 30 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
38 | 31 * See the License for the specific language governing permissions and
39 | 32 * limitations under the License.
40 | 33 *
41 | 34 ******************************************************************************
42 | 35 */
43 | 36
44 | 37 .syntax unified
45 | 38 .cpu cortex-m0
46 | 39 .fpu softvfp
47 | 40 .thumb
48 | 41
49 | 42 .global g_pfnVectors
50 | 43 .global Default_Handler
51 | 44
52 | 45 /* start address for the initialization values of the .data section.
53 | 46 defined in linker script */
54 | 47 0000 00000000 .word _sidata
55 | 48 /* start address for the .data section. defined in linker script */
56 | 49 0004 00000000 .word _sdata
57 | 50 /* end address for the .data section. defined in linker script */
58 | 51 0008 00000000 .word _edata
59 | 52 /* start address for the .bss section. defined in linker script */
60 | 53 000c 00000000 .word _sbss
61 | ARM GAS /tmp/ccSm57Oe.s page 2
62 |
63 |
64 | 54 /* end address for the .bss section. defined in linker script */
65 | 55 0010 00000000 .word _ebss
66 | 56
67 | 57 .equ BootRAM, 0xF108F85F
68 | 58 /**
69 | 59 * @brief This is the code that gets called when the processor first
70 | 60 * starts execution following a reset event. Only the absolutely
71 | 61 * necessary set is performed, after which the application
72 | 62 * supplied main() routine is called.
73 | 63 * @param None
74 | 64 * @retval : None
75 | 65 */
76 | 66
77 | 67 .section .text.Reset_Handler
78 | 68 .weak Reset_Handler
79 | 69 .type Reset_Handler, %function
80 | 70 Reset_Handler:
81 | 71 0000 1248 ldr r0, =_stack_start
82 | 72 0002 8546 mov sp, r0 /* set stack pointer */
83 | 73
84 | 74 /*Check if boot space corresponds to test memory*/
85 | 75
86 | 76 0004 1248 LDR R0,=0x00000004
87 | 77 0006 0168 LDR R1, [R0]
88 | 78 0008 090E LSRS R1, R1, #24
89 | 79 000a 124A LDR R2,=0x1F
90 | 80 000c 9142 CMP R1, R2
91 | 81 000e 05D1 BNE ApplicationStart
92 | 82
93 | 83 /*SYSCFG clock enable*/
94 | 84
95 | 85 0010 1148 LDR R0,=0x40021018
96 | 86 0012 1249 LDR R1,=0x00000001
97 | 87 0014 0160 STR R1, [R0]
98 | 88
99 | 89 /*Set CFGR1 register with flash memory remap at address 0*/
100 | 90 0016 1248 LDR R0,=0x40010000
101 | 91 0018 1249 LDR R1,=0x00000000
102 | 92 001a 0160 STR R1, [R0]
103 | 93
104 | 94 ApplicationStart:
105 | 95 /* Copy the data segment initializers from flash to SRAM */
106 | 96 001c 0021 movs r1, #0
107 | 97 001e 03E0 b LoopCopyDataInit
108 | 98
109 | 99 CopyDataInit:
110 | 100 0020 114B ldr r3, =_sidata
111 | 101 0022 5B58 ldr r3, [r3, r1]
112 | 102 0024 4350 str r3, [r0, r1]
113 | 103 0026 0431 adds r1, r1, #4
114 | 104
115 | 105 LoopCopyDataInit:
116 | 106 0028 1048 ldr r0, =_sdata
117 | 107 002a 114B ldr r3, =_edata
118 | 108 002c 4218 adds r2, r0, r1
119 | 109 002e 9A42 cmp r2, r3
120 | 110 0030 F6D3 bcc CopyDataInit
121 | ARM GAS /tmp/ccSm57Oe.s page 3
122 |
123 |
124 | 111 0032 104A ldr r2, =_sbss
125 | 112 0034 02E0 b LoopFillZerobss
126 | 113 /* Zero fill the bss segment. */
127 | 114 FillZerobss:
128 | 115 0036 0023 movs r3, #0
129 | 116 0038 1360 str r3, [r2]
130 | 117 003a 0432 adds r2, r2, #4
131 | 118
132 | 119
133 | 120 LoopFillZerobss:
134 | 121 003c 0E4B ldr r3, = _ebss
135 | 122 003e 9A42 cmp r2, r3
136 | 123 0040 F9D3 bcc FillZerobss
137 | 124
138 | 125 /* Call the clock system intitialization function.*/
139 | 126 0042 FFF7FEFF bl SystemInit
140 | 127
141 | 128 /* Call the application's entry point.*/
142 | 129 0046 FFF7FEFF bl main
143 | 130
144 | 131 LoopForever:
145 | 132 004a FEE7 b LoopForever
146 | 133
147 | 134
148 | 135 .size Reset_Handler, .-Reset_Handler
149 | 136
150 | 137 /**
151 | 138 * @brief This is the code that gets called when the processor receives an
152 | 139 * unexpected interrupt. This simply enters an infinite loop, preserving
153 | 140 * the system state for examination by a debugger.
154 | 141 *
155 | 142 * @param None
156 | 143 * @retval : None
157 | 144 */
158 | 145 .section .text.Default_Handler,"ax",%progbits
159 | 146 Default_Handler:
160 | 147 Infinite_Loop:
161 | 148 0000 FEE7 b Infinite_Loop
162 | 149 .size Default_Handler, .-Default_Handler
163 | 150 /******************************************************************************
164 | 151 *
165 | 152 * The minimal vector table for a Cortex M0. Note that the proper constructs
166 | 153 * must be placed on this to ensure that it ends up at physical address
167 | 154 * 0x0000.0000.
168 | 155 *
169 | 156 ******************************************************************************/
170 | 157 .section .isr_vector,"a",%progbits
171 | 158 .type g_pfnVectors, %object
172 | 159 .size g_pfnVectors, .-g_pfnVectors
173 | 160
174 | 161
175 | 162
176 | 163 g_pfnVectors:
177 | 164 0000 00000000 .word _stack_start
178 | 165 0004 00000000 .word Reset_Handler
179 | 166
180 | 167 0008 00000000 .word NMI_Handler
181 | ARM GAS /tmp/ccSm57Oe.s page 4
182 |
183 |
184 | 168 000c 00000000 .word HardFault_Handler
185 | 169 0010 00000000 .word 0
186 | 170 0014 00000000 .word 0
187 | 171 0018 00000000 .word 0
188 | 172 001c 00000000 .word 0
189 | 173 0020 00000000 .word 0
190 | 174 0024 00000000 .word 0
191 | 175 0028 00000000 .word 0
192 | 176 002c 00000000 .word SVC_Handler
193 | 177 0030 00000000 .word 0
194 | 178 0034 00000000 .word 0
195 | 179 0038 00000000 .word PendSV_Handler
196 | 180 003c 00000000 .word SysTick_Handler
197 | 181
198 | 182
199 | 183 0040 00000000 .word WWDG_IRQHandler
200 | 184 0044 00000000 .word PVD_IRQHandler
201 | 185 0048 00000000 .word RTC_IRQHandler
202 | 186 004c 00000000 .word FLASH_IRQHandler
203 | 187 0050 00000000 .word RCC_IRQHandler
204 | 188 0054 00000000 .word EXTI0_1_IRQHandler
205 | 189 0058 00000000 .word EXTI2_3_IRQHandler
206 | 190 005c 00000000 .word EXTI4_15_IRQHandler
207 | 191 0060 00000000 .word 0
208 | 192 0064 00000000 .word DMA1_Channel1_IRQHandler
209 | 193 0068 00000000 .word DMA1_Channel2_3_IRQHandler
210 | 194 006c 00000000 .word DMA1_Channel4_5_IRQHandler
211 | 195 0070 00000000 .word ADC1_IRQHandler
212 | 196 0074 00000000 .word TIM1_BRK_UP_TRG_COM_IRQHandler
213 | 197 0078 00000000 .word TIM1_CC_IRQHandler
214 | 198 007c 00000000 .word TIM2_IRQHandler
215 | 199 0080 00000000 .word TIM3_IRQHandler
216 | 200 0084 00000000 .word 0
217 | 201 0088 00000000 .word 0
218 | 202 008c 00000000 .word TIM14_IRQHandler
219 | 203 0090 00000000 .word 0
220 | 204 0094 00000000 .word TIM16_IRQHandler
221 | 205 0098 00000000 .word TIM17_IRQHandler
222 | 206 009c 00000000 .word I2C1_IRQHandler
223 | 207 00a0 00000000 .word 0
224 | 208 00a4 00000000 .word SPI1_IRQHandler
225 | 209 00a8 00000000 .word 0
226 | 210 00ac 00000000 .word USART1_IRQHandler
227 | 211 00b0 00000000 .word 0
228 | 212 00b4 00000000 .word 0
229 | 213 00b8 00000000 .word 0
230 | 214 00bc 00000000 .word 0
231 | 215 00c0 5FF808F1 .word BootRAM /* @0x108. This is for boot in RAM mode for
232 | 216 STM32F0xx devices. */
233 | 217
234 | 218 /*******************************************************************************
235 | 219 *
236 | 220 * Provide weak aliases for each Exception handler to the Default_Handler.
237 | 221 * As they are weak aliases, any function with the same name will override
238 | 222 * this definition.
239 | 223 *
240 | 224 *******************************************************************************/
241 | ARM GAS /tmp/ccSm57Oe.s page 5
242 |
243 |
244 | 225
245 | 226 .weak NMI_Handler
246 | 227 .thumb_set NMI_Handler,Default_Handler
247 | 228
248 | 229 .weak HardFault_Handler
249 | 230 .thumb_set HardFault_Handler,Default_Handler
250 | 231
251 | 232 .weak SVC_Handler
252 | 233 .thumb_set SVC_Handler,Default_Handler
253 | 234
254 | 235 .weak PendSV_Handler
255 | 236 .thumb_set PendSV_Handler,Default_Handler
256 | 237
257 | 238 .weak SysTick_Handler
258 | 239 .thumb_set SysTick_Handler,Default_Handler
259 | 240
260 | 241 .weak WWDG_IRQHandler
261 | 242 .thumb_set WWDG_IRQHandler,Default_Handler
262 | 243
263 | 244 .weak PVD_IRQHandler
264 | 245 .thumb_set PVD_IRQHandler,Default_Handler
265 | 246
266 | 247 .weak RTC_IRQHandler
267 | 248 .thumb_set RTC_IRQHandler,Default_Handler
268 | 249
269 | 250 .weak FLASH_IRQHandler
270 | 251 .thumb_set FLASH_IRQHandler,Default_Handler
271 | 252
272 | 253 .weak RCC_IRQHandler
273 | 254 .thumb_set RCC_IRQHandler,Default_Handler
274 | 255
275 | 256 .weak EXTI0_1_IRQHandler
276 | 257 .thumb_set EXTI0_1_IRQHandler,Default_Handler
277 | 258
278 | 259 .weak EXTI2_3_IRQHandler
279 | 260 .thumb_set EXTI2_3_IRQHandler,Default_Handler
280 | 261
281 | 262 .weak EXTI4_15_IRQHandler
282 | 263 .thumb_set EXTI4_15_IRQHandler,Default_Handler
283 | 264
284 | 265 .weak DMA1_Channel1_IRQHandler
285 | 266 .thumb_set DMA1_Channel1_IRQHandler,Default_Handler
286 | 267
287 | 268 .weak DMA1_Channel2_3_IRQHandler
288 | 269 .thumb_set DMA1_Channel2_3_IRQHandler,Default_Handler
289 | 270
290 | 271 .weak DMA1_Channel4_5_IRQHandler
291 | 272 .thumb_set DMA1_Channel4_5_IRQHandler,Default_Handler
292 | 273
293 | 274 .weak ADC1_IRQHandler
294 | 275 .thumb_set ADC1_IRQHandler,Default_Handler
295 | 276
296 | 277 .weak TIM1_BRK_UP_TRG_COM_IRQHandler
297 | 278 .thumb_set TIM1_BRK_UP_TRG_COM_IRQHandler,Default_Handler
298 | 279
299 | 280 .weak TIM1_CC_IRQHandler
300 | 281 .thumb_set TIM1_CC_IRQHandler,Default_Handler
301 | ARM GAS /tmp/ccSm57Oe.s page 6
302 |
303 |
304 | 282
305 | 283 .weak TIM2_IRQHandler
306 | 284 .thumb_set TIM2_IRQHandler,Default_Handler
307 | 285
308 | 286 .weak TIM3_IRQHandler
309 | 287 .thumb_set TIM3_IRQHandler,Default_Handler
310 | 288
311 | 289 .weak TIM14_IRQHandler
312 | 290 .thumb_set TIM14_IRQHandler,Default_Handler
313 | 291
314 | 292 .weak TIM16_IRQHandler
315 | 293 .thumb_set TIM16_IRQHandler,Default_Handler
316 | 294
317 | 295 .weak TIM17_IRQHandler
318 | 296 .thumb_set TIM17_IRQHandler,Default_Handler
319 | 297
320 | 298 .weak I2C1_IRQHandler
321 | 299 .thumb_set I2C1_IRQHandler,Default_Handler
322 | 300
323 | 301 .weak SPI1_IRQHandler
324 | 302 .thumb_set SPI1_IRQHandler,Default_Handler
325 | 303
326 | 304 .weak USART1_IRQHandler
327 | 305 .thumb_set USART1_IRQHandler,Default_Handler
328 | ARM GAS /tmp/ccSm57Oe.s page 7
329 |
330 |
331 | DEFINED SYMBOLS
332 | startup/startup_stm32f031.s:163 .isr_vector:0000000000000000 g_pfnVectors
333 | startup/startup_stm32f031.s:146 .text.Default_Handler:0000000000000000 Default_Handler
334 | startup/startup_stm32f031.s:57 *ABS*:00000000f108f85f BootRAM
335 | startup/startup_stm32f031.s:70 .text.Reset_Handler:0000000000000000 Reset_Handler
336 | startup/startup_stm32f031.s:71 .text.Reset_Handler:0000000000000000 $t
337 | startup/startup_stm32f031.s:94 .text.Reset_Handler:000000000000001c ApplicationStart
338 | startup/startup_stm32f031.s:105 .text.Reset_Handler:0000000000000028 LoopCopyDataInit
339 | startup/startup_stm32f031.s:99 .text.Reset_Handler:0000000000000020 CopyDataInit
340 | startup/startup_stm32f031.s:120 .text.Reset_Handler:000000000000003c LoopFillZerobss
341 | startup/startup_stm32f031.s:114 .text.Reset_Handler:0000000000000036 FillZerobss
342 | startup/startup_stm32f031.s:131 .text.Reset_Handler:000000000000004a LoopForever
343 | startup/startup_stm32f031.s:147 .text.Default_Handler:0000000000000000 Infinite_Loop
344 | startup/startup_stm32f031.s:148 .text.Default_Handler:0000000000000000 $t
345 | startup/startup_stm32f031.s:146 .text.Default_Handler:0000000000000000 NMI_Handler
346 | startup/startup_stm32f031.s:146 .text.Default_Handler:0000000000000000 HardFault_Handler
347 | startup/startup_stm32f031.s:146 .text.Default_Handler:0000000000000000 SVC_Handler
348 | startup/startup_stm32f031.s:146 .text.Default_Handler:0000000000000000 PendSV_Handler
349 | startup/startup_stm32f031.s:146 .text.Default_Handler:0000000000000000 SysTick_Handler
350 | startup/startup_stm32f031.s:146 .text.Default_Handler:0000000000000000 WWDG_IRQHandler
351 | startup/startup_stm32f031.s:146 .text.Default_Handler:0000000000000000 PVD_IRQHandler
352 | startup/startup_stm32f031.s:146 .text.Default_Handler:0000000000000000 RTC_IRQHandler
353 | startup/startup_stm32f031.s:146 .text.Default_Handler:0000000000000000 FLASH_IRQHandler
354 | startup/startup_stm32f031.s:146 .text.Default_Handler:0000000000000000 RCC_IRQHandler
355 | startup/startup_stm32f031.s:146 .text.Default_Handler:0000000000000000 EXTI0_1_IRQHandler
356 | startup/startup_stm32f031.s:146 .text.Default_Handler:0000000000000000 EXTI2_3_IRQHandler
357 | startup/startup_stm32f031.s:146 .text.Default_Handler:0000000000000000 EXTI4_15_IRQHandler
358 | startup/startup_stm32f031.s:146 .text.Default_Handler:0000000000000000 DMA1_Channel1_IRQHandler
359 | startup/startup_stm32f031.s:146 .text.Default_Handler:0000000000000000 DMA1_Channel2_3_IRQHandler
360 | startup/startup_stm32f031.s:146 .text.Default_Handler:0000000000000000 DMA1_Channel4_5_IRQHandler
361 | startup/startup_stm32f031.s:146 .text.Default_Handler:0000000000000000 ADC1_IRQHandler
362 | startup/startup_stm32f031.s:146 .text.Default_Handler:0000000000000000 TIM1_BRK_UP_TRG_COM_IRQHandler
363 | startup/startup_stm32f031.s:146 .text.Default_Handler:0000000000000000 TIM1_CC_IRQHandler
364 | startup/startup_stm32f031.s:146 .text.Default_Handler:0000000000000000 TIM2_IRQHandler
365 | startup/startup_stm32f031.s:146 .text.Default_Handler:0000000000000000 TIM3_IRQHandler
366 | startup/startup_stm32f031.s:146 .text.Default_Handler:0000000000000000 TIM14_IRQHandler
367 | startup/startup_stm32f031.s:146 .text.Default_Handler:0000000000000000 TIM16_IRQHandler
368 | startup/startup_stm32f031.s:146 .text.Default_Handler:0000000000000000 TIM17_IRQHandler
369 | startup/startup_stm32f031.s:146 .text.Default_Handler:0000000000000000 I2C1_IRQHandler
370 | startup/startup_stm32f031.s:146 .text.Default_Handler:0000000000000000 SPI1_IRQHandler
371 | startup/startup_stm32f031.s:146 .text.Default_Handler:0000000000000000 USART1_IRQHandler
372 | startup/startup_stm32f031.s:145 .text.Reset_Handler:000000000000004c $d
373 | .debug_aranges:000000000000000c $d
374 |
375 | UNDEFINED SYMBOLS
376 | _sidata
377 | _sdata
378 | _edata
379 | _sbss
380 | _ebss
381 | _stack_start
382 | SystemInit
383 | main
384 |
--------------------------------------------------------------------------------
/inc/core_cmFunc.h:
--------------------------------------------------------------------------------
1 | /**************************************************************************//**
2 | * @file core_cmFunc.h
3 | * @brief CMSIS Cortex-M Core Function Access Header File
4 | * @version V3.20
5 | * @date 25. February 2013
6 | *
7 | * @note
8 | *
9 | ******************************************************************************/
10 | /* Copyright (c) 2009 - 2013 ARM LIMITED
11 |
12 | All rights reserved.
13 | Redistribution and use in source and binary forms, with or without
14 | modification, are permitted provided that the following conditions are met:
15 | - Redistributions of source code must retain the above copyright
16 | notice, this list of conditions and the following disclaimer.
17 | - Redistributions in binary form must reproduce the above copyright
18 | notice, this list of conditions and the following disclaimer in the
19 | documentation and/or other materials provided with the distribution.
20 | - Neither the name of ARM nor the names of its contributors may be used
21 | to endorse or promote products derived from this software without
22 | specific prior written permission.
23 | *
24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
25 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
26 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
27 | ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS AND CONTRIBUTORS BE
28 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
29 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
30 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
31 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
32 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
33 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 | POSSIBILITY OF SUCH DAMAGE.
35 | ---------------------------------------------------------------------------*/
36 |
37 |
38 | #ifndef __CORE_CMFUNC_H
39 | #define __CORE_CMFUNC_H
40 |
41 |
42 | /* ########################### Core Function Access ########################### */
43 | /** \ingroup CMSIS_Core_FunctionInterface
44 | \defgroup CMSIS_Core_RegAccFunctions CMSIS Core Register Access Functions
45 | @{
46 | */
47 |
48 | #if defined ( __CC_ARM ) /*------------------RealView Compiler -----------------*/
49 | /* ARM armcc specific functions */
50 |
51 | #if (__ARMCC_VERSION < 400677)
52 | #error "Please use ARM Compiler Toolchain V4.0.677 or later!"
53 | #endif
54 |
55 | /* intrinsic void __enable_irq(); */
56 | /* intrinsic void __disable_irq(); */
57 |
58 | /** \brief Get Control Register
59 |
60 | This function returns the content of the Control Register.
61 |
62 | \return Control Register value
63 | */
64 | __STATIC_INLINE uint32_t __get_CONTROL(void)
65 | {
66 | register uint32_t __regControl __ASM("control");
67 | return(__regControl);
68 | }
69 |
70 |
71 | /** \brief Set Control Register
72 |
73 | This function writes the given value to the Control Register.
74 |
75 | \param [in] control Control Register value to set
76 | */
77 | __STATIC_INLINE void __set_CONTROL(uint32_t control)
78 | {
79 | register uint32_t __regControl __ASM("control");
80 | __regControl = control;
81 | }
82 |
83 |
84 | /** \brief Get IPSR Register
85 |
86 | This function returns the content of the IPSR Register.
87 |
88 | \return IPSR Register value
89 | */
90 | __STATIC_INLINE uint32_t __get_IPSR(void)
91 | {
92 | register uint32_t __regIPSR __ASM("ipsr");
93 | return(__regIPSR);
94 | }
95 |
96 |
97 | /** \brief Get APSR Register
98 |
99 | This function returns the content of the APSR Register.
100 |
101 | \return APSR Register value
102 | */
103 | __STATIC_INLINE uint32_t __get_APSR(void)
104 | {
105 | register uint32_t __regAPSR __ASM("apsr");
106 | return(__regAPSR);
107 | }
108 |
109 |
110 | /** \brief Get xPSR Register
111 |
112 | This function returns the content of the xPSR Register.
113 |
114 | \return xPSR Register value
115 | */
116 | __STATIC_INLINE uint32_t __get_xPSR(void)
117 | {
118 | register uint32_t __regXPSR __ASM("xpsr");
119 | return(__regXPSR);
120 | }
121 |
122 |
123 | /** \brief Get Process Stack Pointer
124 |
125 | This function returns the current value of the Process Stack Pointer (PSP).
126 |
127 | \return PSP Register value
128 | */
129 | __STATIC_INLINE uint32_t __get_PSP(void)
130 | {
131 | register uint32_t __regProcessStackPointer __ASM("psp");
132 | return(__regProcessStackPointer);
133 | }
134 |
135 |
136 | /** \brief Set Process Stack Pointer
137 |
138 | This function assigns the given value to the Process Stack Pointer (PSP).
139 |
140 | \param [in] topOfProcStack Process Stack Pointer value to set
141 | */
142 | __STATIC_INLINE void __set_PSP(uint32_t topOfProcStack)
143 | {
144 | register uint32_t __regProcessStackPointer __ASM("psp");
145 | __regProcessStackPointer = topOfProcStack;
146 | }
147 |
148 |
149 | /** \brief Get Main Stack Pointer
150 |
151 | This function returns the current value of the Main Stack Pointer (MSP).
152 |
153 | \return MSP Register value
154 | */
155 | __STATIC_INLINE uint32_t __get_MSP(void)
156 | {
157 | register uint32_t __regMainStackPointer __ASM("msp");
158 | return(__regMainStackPointer);
159 | }
160 |
161 |
162 | /** \brief Set Main Stack Pointer
163 |
164 | This function assigns the given value to the Main Stack Pointer (MSP).
165 |
166 | \param [in] topOfMainStack Main Stack Pointer value to set
167 | */
168 | __STATIC_INLINE void __set_MSP(uint32_t topOfMainStack)
169 | {
170 | register uint32_t __regMainStackPointer __ASM("msp");
171 | __regMainStackPointer = topOfMainStack;
172 | }
173 |
174 |
175 | /** \brief Get Priority Mask
176 |
177 | This function returns the current state of the priority mask bit from the Priority Mask Register.
178 |
179 | \return Priority Mask value
180 | */
181 | __STATIC_INLINE uint32_t __get_PRIMASK(void)
182 | {
183 | register uint32_t __regPriMask __ASM("primask");
184 | return(__regPriMask);
185 | }
186 |
187 |
188 | /** \brief Set Priority Mask
189 |
190 | This function assigns the given value to the Priority Mask Register.
191 |
192 | \param [in] priMask Priority Mask
193 | */
194 | __STATIC_INLINE void __set_PRIMASK(uint32_t priMask)
195 | {
196 | register uint32_t __regPriMask __ASM("primask");
197 | __regPriMask = (priMask);
198 | }
199 |
200 |
201 | #if (__CORTEX_M >= 0x03)
202 |
203 | /** \brief Enable FIQ
204 |
205 | This function enables FIQ interrupts by clearing the F-bit in the CPSR.
206 | Can only be executed in Privileged modes.
207 | */
208 | #define __enable_fault_irq __enable_fiq
209 |
210 |
211 | /** \brief Disable FIQ
212 |
213 | This function disables FIQ interrupts by setting the F-bit in the CPSR.
214 | Can only be executed in Privileged modes.
215 | */
216 | #define __disable_fault_irq __disable_fiq
217 |
218 |
219 | /** \brief Get Base Priority
220 |
221 | This function returns the current value of the Base Priority register.
222 |
223 | \return Base Priority register value
224 | */
225 | __STATIC_INLINE uint32_t __get_BASEPRI(void)
226 | {
227 | register uint32_t __regBasePri __ASM("basepri");
228 | return(__regBasePri);
229 | }
230 |
231 |
232 | /** \brief Set Base Priority
233 |
234 | This function assigns the given value to the Base Priority register.
235 |
236 | \param [in] basePri Base Priority value to set
237 | */
238 | __STATIC_INLINE void __set_BASEPRI(uint32_t basePri)
239 | {
240 | register uint32_t __regBasePri __ASM("basepri");
241 | __regBasePri = (basePri & 0xff);
242 | }
243 |
244 |
245 | /** \brief Get Fault Mask
246 |
247 | This function returns the current value of the Fault Mask register.
248 |
249 | \return Fault Mask register value
250 | */
251 | __STATIC_INLINE uint32_t __get_FAULTMASK(void)
252 | {
253 | register uint32_t __regFaultMask __ASM("faultmask");
254 | return(__regFaultMask);
255 | }
256 |
257 |
258 | /** \brief Set Fault Mask
259 |
260 | This function assigns the given value to the Fault Mask register.
261 |
262 | \param [in] faultMask Fault Mask value to set
263 | */
264 | __STATIC_INLINE void __set_FAULTMASK(uint32_t faultMask)
265 | {
266 | register uint32_t __regFaultMask __ASM("faultmask");
267 | __regFaultMask = (faultMask & (uint32_t)1);
268 | }
269 |
270 | #endif /* (__CORTEX_M >= 0x03) */
271 |
272 |
273 | #if (__CORTEX_M == 0x04)
274 |
275 | /** \brief Get FPSCR
276 |
277 | This function returns the current value of the Floating Point Status/Control register.
278 |
279 | \return Floating Point Status/Control register value
280 | */
281 | __STATIC_INLINE uint32_t __get_FPSCR(void)
282 | {
283 | #if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
284 | register uint32_t __regfpscr __ASM("fpscr");
285 | return(__regfpscr);
286 | #else
287 | return(0);
288 | #endif
289 | }
290 |
291 |
292 | /** \brief Set FPSCR
293 |
294 | This function assigns the given value to the Floating Point Status/Control register.
295 |
296 | \param [in] fpscr Floating Point Status/Control value to set
297 | */
298 | __STATIC_INLINE void __set_FPSCR(uint32_t fpscr)
299 | {
300 | #if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
301 | register uint32_t __regfpscr __ASM("fpscr");
302 | __regfpscr = (fpscr);
303 | #endif
304 | }
305 |
306 | #endif /* (__CORTEX_M == 0x04) */
307 |
308 |
309 | #elif defined ( __ICCARM__ ) /*------------------ ICC Compiler -------------------*/
310 | /* IAR iccarm specific functions */
311 |
312 | #include
313 |
314 |
315 | #elif defined ( __TMS470__ ) /*---------------- TI CCS Compiler ------------------*/
316 | /* TI CCS specific functions */
317 |
318 | #include
319 |
320 |
321 | #elif defined ( __GNUC__ ) /*------------------ GNU Compiler ---------------------*/
322 | /* GNU gcc specific functions */
323 |
324 | /** \brief Enable IRQ Interrupts
325 |
326 | This function enables IRQ interrupts by clearing the I-bit in the CPSR.
327 | Can only be executed in Privileged modes.
328 | */
329 | __attribute__( ( always_inline ) ) __STATIC_INLINE void __enable_irq(void)
330 | {
331 | __ASM volatile ("cpsie i" : : : "memory");
332 | }
333 |
334 |
335 | /** \brief Disable IRQ Interrupts
336 |
337 | This function disables IRQ interrupts by setting the I-bit in the CPSR.
338 | Can only be executed in Privileged modes.
339 | */
340 | __attribute__( ( always_inline ) ) __STATIC_INLINE void __disable_irq(void)
341 | {
342 | __ASM volatile ("cpsid i" : : : "memory");
343 | }
344 |
345 |
346 | /** \brief Get Control Register
347 |
348 | This function returns the content of the Control Register.
349 |
350 | \return Control Register value
351 | */
352 | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_CONTROL(void)
353 | {
354 | uint32_t result;
355 |
356 | __ASM volatile ("MRS %0, control" : "=r" (result) );
357 | return(result);
358 | }
359 |
360 |
361 | /** \brief Set Control Register
362 |
363 | This function writes the given value to the Control Register.
364 |
365 | \param [in] control Control Register value to set
366 | */
367 | __attribute__( ( always_inline ) ) __STATIC_INLINE void __set_CONTROL(uint32_t control)
368 | {
369 | __ASM volatile ("MSR control, %0" : : "r" (control) : "memory");
370 | }
371 |
372 |
373 | /** \brief Get IPSR Register
374 |
375 | This function returns the content of the IPSR Register.
376 |
377 | \return IPSR Register value
378 | */
379 | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_IPSR(void)
380 | {
381 | uint32_t result;
382 |
383 | __ASM volatile ("MRS %0, ipsr" : "=r" (result) );
384 | return(result);
385 | }
386 |
387 |
388 | /** \brief Get APSR Register
389 |
390 | This function returns the content of the APSR Register.
391 |
392 | \return APSR Register value
393 | */
394 | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_APSR(void)
395 | {
396 | uint32_t result;
397 |
398 | __ASM volatile ("MRS %0, apsr" : "=r" (result) );
399 | return(result);
400 | }
401 |
402 |
403 | /** \brief Get xPSR Register
404 |
405 | This function returns the content of the xPSR Register.
406 |
407 | \return xPSR Register value
408 | */
409 | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_xPSR(void)
410 | {
411 | uint32_t result;
412 |
413 | __ASM volatile ("MRS %0, xpsr" : "=r" (result) );
414 | return(result);
415 | }
416 |
417 |
418 | /** \brief Get Process Stack Pointer
419 |
420 | This function returns the current value of the Process Stack Pointer (PSP).
421 |
422 | \return PSP Register value
423 | */
424 | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_PSP(void)
425 | {
426 | register uint32_t result;
427 |
428 | __ASM volatile ("MRS %0, psp\n" : "=r" (result) );
429 | return(result);
430 | }
431 |
432 |
433 | /** \brief Set Process Stack Pointer
434 |
435 | This function assigns the given value to the Process Stack Pointer (PSP).
436 |
437 | \param [in] topOfProcStack Process Stack Pointer value to set
438 | */
439 | __attribute__( ( always_inline ) ) __STATIC_INLINE void __set_PSP(uint32_t topOfProcStack)
440 | {
441 | __ASM volatile ("MSR psp, %0\n" : : "r" (topOfProcStack) : "sp");
442 | }
443 |
444 |
445 | /** \brief Get Main Stack Pointer
446 |
447 | This function returns the current value of the Main Stack Pointer (MSP).
448 |
449 | \return MSP Register value
450 | */
451 | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_MSP(void)
452 | {
453 | register uint32_t result;
454 |
455 | __ASM volatile ("MRS %0, msp\n" : "=r" (result) );
456 | return(result);
457 | }
458 |
459 |
460 | /** \brief Set Main Stack Pointer
461 |
462 | This function assigns the given value to the Main Stack Pointer (MSP).
463 |
464 | \param [in] topOfMainStack Main Stack Pointer value to set
465 | */
466 | __attribute__( ( always_inline ) ) __STATIC_INLINE void __set_MSP(uint32_t topOfMainStack)
467 | {
468 | __ASM volatile ("MSR msp, %0\n" : : "r" (topOfMainStack) : "sp");
469 | }
470 |
471 |
472 | /** \brief Get Priority Mask
473 |
474 | This function returns the current state of the priority mask bit from the Priority Mask Register.
475 |
476 | \return Priority Mask value
477 | */
478 | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_PRIMASK(void)
479 | {
480 | uint32_t result;
481 |
482 | __ASM volatile ("MRS %0, primask" : "=r" (result) );
483 | return(result);
484 | }
485 |
486 |
487 | /** \brief Set Priority Mask
488 |
489 | This function assigns the given value to the Priority Mask Register.
490 |
491 | \param [in] priMask Priority Mask
492 | */
493 | __attribute__( ( always_inline ) ) __STATIC_INLINE void __set_PRIMASK(uint32_t priMask)
494 | {
495 | __ASM volatile ("MSR primask, %0" : : "r" (priMask) : "memory");
496 | }
497 |
498 |
499 | #if (__CORTEX_M >= 0x03)
500 |
501 | /** \brief Enable FIQ
502 |
503 | This function enables FIQ interrupts by clearing the F-bit in the CPSR.
504 | Can only be executed in Privileged modes.
505 | */
506 | __attribute__( ( always_inline ) ) __STATIC_INLINE void __enable_fault_irq(void)
507 | {
508 | __ASM volatile ("cpsie f" : : : "memory");
509 | }
510 |
511 |
512 | /** \brief Disable FIQ
513 |
514 | This function disables FIQ interrupts by setting the F-bit in the CPSR.
515 | Can only be executed in Privileged modes.
516 | */
517 | __attribute__( ( always_inline ) ) __STATIC_INLINE void __disable_fault_irq(void)
518 | {
519 | __ASM volatile ("cpsid f" : : : "memory");
520 | }
521 |
522 |
523 | /** \brief Get Base Priority
524 |
525 | This function returns the current value of the Base Priority register.
526 |
527 | \return Base Priority register value
528 | */
529 | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_BASEPRI(void)
530 | {
531 | uint32_t result;
532 |
533 | __ASM volatile ("MRS %0, basepri_max" : "=r" (result) );
534 | return(result);
535 | }
536 |
537 |
538 | /** \brief Set Base Priority
539 |
540 | This function assigns the given value to the Base Priority register.
541 |
542 | \param [in] basePri Base Priority value to set
543 | */
544 | __attribute__( ( always_inline ) ) __STATIC_INLINE void __set_BASEPRI(uint32_t value)
545 | {
546 | __ASM volatile ("MSR basepri, %0" : : "r" (value) : "memory");
547 | }
548 |
549 |
550 | /** \brief Get Fault Mask
551 |
552 | This function returns the current value of the Fault Mask register.
553 |
554 | \return Fault Mask register value
555 | */
556 | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_FAULTMASK(void)
557 | {
558 | uint32_t result;
559 |
560 | __ASM volatile ("MRS %0, faultmask" : "=r" (result) );
561 | return(result);
562 | }
563 |
564 |
565 | /** \brief Set Fault Mask
566 |
567 | This function assigns the given value to the Fault Mask register.
568 |
569 | \param [in] faultMask Fault Mask value to set
570 | */
571 | __attribute__( ( always_inline ) ) __STATIC_INLINE void __set_FAULTMASK(uint32_t faultMask)
572 | {
573 | __ASM volatile ("MSR faultmask, %0" : : "r" (faultMask) : "memory");
574 | }
575 |
576 | #endif /* (__CORTEX_M >= 0x03) */
577 |
578 |
579 | #if (__CORTEX_M == 0x04)
580 |
581 | /** \brief Get FPSCR
582 |
583 | This function returns the current value of the Floating Point Status/Control register.
584 |
585 | \return Floating Point Status/Control register value
586 | */
587 | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_FPSCR(void)
588 | {
589 | #if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
590 | uint32_t result;
591 |
592 | /* Empty asm statement works as a scheduling barrier */
593 | __ASM volatile ("");
594 | __ASM volatile ("VMRS %0, fpscr" : "=r" (result) );
595 | __ASM volatile ("");
596 | return(result);
597 | #else
598 | return(0);
599 | #endif
600 | }
601 |
602 |
603 | /** \brief Set FPSCR
604 |
605 | This function assigns the given value to the Floating Point Status/Control register.
606 |
607 | \param [in] fpscr Floating Point Status/Control value to set
608 | */
609 | __attribute__( ( always_inline ) ) __STATIC_INLINE void __set_FPSCR(uint32_t fpscr)
610 | {
611 | #if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
612 | /* Empty asm statement works as a scheduling barrier */
613 | __ASM volatile ("");
614 | __ASM volatile ("VMSR fpscr, %0" : : "r" (fpscr) : "vfpcc");
615 | __ASM volatile ("");
616 | #endif
617 | }
618 |
619 | #endif /* (__CORTEX_M == 0x04) */
620 |
621 |
622 | #elif defined ( __TASKING__ ) /*------------------ TASKING Compiler --------------*/
623 | /* TASKING carm specific functions */
624 |
625 | /*
626 | * The CMSIS functions have been implemented as intrinsics in the compiler.
627 | * Please use "carm -?i" to get an up to date list of all instrinsics,
628 | * Including the CMSIS ones.
629 | */
630 |
631 | #endif
632 |
633 | /*@} end of CMSIS_Core_RegAccFunctions */
634 |
635 |
636 | #endif /* __CORE_CMFUNC_H */
637 |
--------------------------------------------------------------------------------
/inc/stm32f0xx_i2c.h:
--------------------------------------------------------------------------------
1 | /**
2 | ******************************************************************************
3 | * @file stm32f0xx_i2c.h
4 | * @author MCD Application Team
5 | * @version V1.4.0
6 | * @date 24-July-2014
7 | * @brief This file contains all the functions prototypes for the I2C firmware
8 | * library
9 | ******************************************************************************
10 | * @attention
11 | *
12 | * © COPYRIGHT 2014 STMicroelectronics
13 | *
14 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License");
15 | * You may not use this file except in compliance with the License.
16 | * You may obtain a copy of the License at:
17 | *
18 | * http://www.st.com/software_license_agreement_liberty_v2
19 | *
20 | * Unless required by applicable law or agreed to in writing, software
21 | * distributed under the License is distributed on an "AS IS" BASIS,
22 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
23 | * See the License for the specific language governing permissions and
24 | * limitations under the License.
25 | *
26 | ******************************************************************************
27 | */
28 |
29 | /* Define to prevent recursive inclusion -------------------------------------*/
30 | #ifndef __STM32F0XX_I2C_H
31 | #define __STM32F0XX_I2C_H
32 |
33 | #ifdef __cplusplus
34 | extern "C" {
35 | #endif
36 |
37 | /* Includes ------------------------------------------------------------------*/
38 | #include "stm32f0xx.h"
39 |
40 | /** @addtogroup STM32F0xx_StdPeriph_Driver
41 | * @{
42 | */
43 |
44 | /** @addtogroup I2C
45 | * @{
46 | */
47 |
48 | /* Exported types ------------------------------------------------------------*/
49 |
50 | /**
51 | * @brief I2C Init structure definition
52 | */
53 |
54 | typedef struct
55 | {
56 | uint32_t I2C_Timing; /*!< Specifies the I2C_TIMINGR_register value.
57 | This parameter must be set by referring to I2C_Timing_Config_Tool*/
58 |
59 | uint32_t I2C_AnalogFilter; /*!< Enables or disables analog noise filter.
60 | This parameter can be a value of @ref I2C_Analog_Filter*/
61 |
62 | uint32_t I2C_DigitalFilter; /*!< Configures the digital noise filter.
63 | This parameter can be a number between 0x00 and 0x0F*/
64 |
65 | uint32_t I2C_Mode; /*!< Specifies the I2C mode.
66 | This parameter can be a value of @ref I2C_mode*/
67 |
68 | uint32_t I2C_OwnAddress1; /*!< Specifies the device own address 1.
69 | This parameter can be a 7-bit or 10-bit address*/
70 |
71 | uint32_t I2C_Ack; /*!< Enables or disables the acknowledgement.
72 | This parameter can be a value of @ref I2C_acknowledgement*/
73 |
74 | uint32_t I2C_AcknowledgedAddress; /*!< Specifies if 7-bit or 10-bit address is acknowledged.
75 | This parameter can be a value of @ref I2C_acknowledged_address*/
76 | }I2C_InitTypeDef;
77 |
78 | /* Exported constants --------------------------------------------------------*/
79 |
80 |
81 | /** @defgroup I2C_Exported_Constants
82 | * @{
83 | */
84 |
85 | #define IS_I2C_ALL_PERIPH(PERIPH) (((PERIPH) == I2C1) || \
86 | ((PERIPH) == I2C2))
87 |
88 | #define IS_I2C_1_PERIPH(PERIPH) ((PERIPH) == I2C1)
89 |
90 | /** @defgroup I2C_Analog_Filter
91 | * @{
92 | */
93 |
94 | #define I2C_AnalogFilter_Enable ((uint32_t)0x00000000)
95 | #define I2C_AnalogFilter_Disable I2C_CR1_ANFOFF
96 |
97 | #define IS_I2C_ANALOG_FILTER(FILTER) (((FILTER) == I2C_AnalogFilter_Enable) || \
98 | ((FILTER) == I2C_AnalogFilter_Disable))
99 | /**
100 | * @}
101 | */
102 |
103 | /** @defgroup I2C_Digital_Filter
104 | * @{
105 | */
106 |
107 | #define IS_I2C_DIGITAL_FILTER(FILTER) ((FILTER) <= 0x0000000F)
108 | /**
109 | * @}
110 | */
111 |
112 | /** @defgroup I2C_mode
113 | * @{
114 | */
115 |
116 | #define I2C_Mode_I2C ((uint32_t)0x00000000)
117 | #define I2C_Mode_SMBusDevice I2C_CR1_SMBDEN
118 | #define I2C_Mode_SMBusHost I2C_CR1_SMBHEN
119 |
120 | #define IS_I2C_MODE(MODE) (((MODE) == I2C_Mode_I2C) || \
121 | ((MODE) == I2C_Mode_SMBusDevice) || \
122 | ((MODE) == I2C_Mode_SMBusHost))
123 | /**
124 | * @}
125 | */
126 |
127 | /** @defgroup I2C_acknowledgement
128 | * @{
129 | */
130 |
131 | #define I2C_Ack_Enable ((uint32_t)0x00000000)
132 | #define I2C_Ack_Disable I2C_CR2_NACK
133 |
134 | #define IS_I2C_ACK(ACK) (((ACK) == I2C_Ack_Enable) || \
135 | ((ACK) == I2C_Ack_Disable))
136 | /**
137 | * @}
138 | */
139 |
140 | /** @defgroup I2C_acknowledged_address
141 | * @{
142 | */
143 |
144 | #define I2C_AcknowledgedAddress_7bit ((uint32_t)0x00000000)
145 | #define I2C_AcknowledgedAddress_10bit I2C_OAR1_OA1MODE
146 |
147 | #define IS_I2C_ACKNOWLEDGE_ADDRESS(ADDRESS) (((ADDRESS) == I2C_AcknowledgedAddress_7bit) || \
148 | ((ADDRESS) == I2C_AcknowledgedAddress_10bit))
149 | /**
150 | * @}
151 | */
152 |
153 | /** @defgroup I2C_own_address1
154 | * @{
155 | */
156 |
157 | #define IS_I2C_OWN_ADDRESS1(ADDRESS1) ((ADDRESS1) <= (uint32_t)0x000003FF)
158 | /**
159 | * @}
160 | */
161 |
162 | /** @defgroup I2C_transfer_direction
163 | * @{
164 | */
165 |
166 | #define I2C_Direction_Transmitter ((uint16_t)0x0000)
167 | #define I2C_Direction_Receiver ((uint16_t)0x0400)
168 |
169 | #define IS_I2C_DIRECTION(DIRECTION) (((DIRECTION) == I2C_Direction_Transmitter) || \
170 | ((DIRECTION) == I2C_Direction_Receiver))
171 | /**
172 | * @}
173 | */
174 |
175 | /** @defgroup I2C_DMA_transfer_requests
176 | * @{
177 | */
178 |
179 | #define I2C_DMAReq_Tx I2C_CR1_TXDMAEN
180 | #define I2C_DMAReq_Rx I2C_CR1_RXDMAEN
181 |
182 | #define IS_I2C_DMA_REQ(REQ) ((((REQ) & (uint32_t)0xFFFF3FFF) == 0x00) && ((REQ) != 0x00))
183 | /**
184 | * @}
185 | */
186 |
187 | /** @defgroup I2C_slave_address
188 | * @{
189 | */
190 |
191 | #define IS_I2C_SLAVE_ADDRESS(ADDRESS) ((ADDRESS) <= (uint16_t)0x03FF)
192 | /**
193 | * @}
194 | */
195 |
196 |
197 | /** @defgroup I2C_own_address2
198 | * @{
199 | */
200 |
201 | #define IS_I2C_OWN_ADDRESS2(ADDRESS2) ((ADDRESS2) <= (uint16_t)0x00FF)
202 |
203 | /**
204 | * @}
205 | */
206 |
207 | /** @defgroup I2C_own_address2_mask
208 | * @{
209 | */
210 |
211 | #define I2C_OA2_NoMask ((uint8_t)0x00)
212 | #define I2C_OA2_Mask01 ((uint8_t)0x01)
213 | #define I2C_OA2_Mask02 ((uint8_t)0x02)
214 | #define I2C_OA2_Mask03 ((uint8_t)0x03)
215 | #define I2C_OA2_Mask04 ((uint8_t)0x04)
216 | #define I2C_OA2_Mask05 ((uint8_t)0x05)
217 | #define I2C_OA2_Mask06 ((uint8_t)0x06)
218 | #define I2C_OA2_Mask07 ((uint8_t)0x07)
219 |
220 | #define IS_I2C_OWN_ADDRESS2_MASK(MASK) (((MASK) == I2C_OA2_NoMask) || \
221 | ((MASK) == I2C_OA2_Mask01) || \
222 | ((MASK) == I2C_OA2_Mask02) || \
223 | ((MASK) == I2C_OA2_Mask03) || \
224 | ((MASK) == I2C_OA2_Mask04) || \
225 | ((MASK) == I2C_OA2_Mask05) || \
226 | ((MASK) == I2C_OA2_Mask06) || \
227 | ((MASK) == I2C_OA2_Mask07))
228 |
229 | /**
230 | * @}
231 | */
232 |
233 | /** @defgroup I2C_timeout
234 | * @{
235 | */
236 |
237 | #define IS_I2C_TIMEOUT(TIMEOUT) ((TIMEOUT) <= (uint16_t)0x0FFF)
238 |
239 | /**
240 | * @}
241 | */
242 |
243 | /** @defgroup I2C_registers
244 | * @{
245 | */
246 |
247 | #define I2C_Register_CR1 ((uint8_t)0x00)
248 | #define I2C_Register_CR2 ((uint8_t)0x04)
249 | #define I2C_Register_OAR1 ((uint8_t)0x08)
250 | #define I2C_Register_OAR2 ((uint8_t)0x0C)
251 | #define I2C_Register_TIMINGR ((uint8_t)0x10)
252 | #define I2C_Register_TIMEOUTR ((uint8_t)0x14)
253 | #define I2C_Register_ISR ((uint8_t)0x18)
254 | #define I2C_Register_ICR ((uint8_t)0x1C)
255 | #define I2C_Register_PECR ((uint8_t)0x20)
256 | #define I2C_Register_RXDR ((uint8_t)0x24)
257 | #define I2C_Register_TXDR ((uint8_t)0x28)
258 |
259 | #define IS_I2C_REGISTER(REGISTER) (((REGISTER) == I2C_Register_CR1) || \
260 | ((REGISTER) == I2C_Register_CR2) || \
261 | ((REGISTER) == I2C_Register_OAR1) || \
262 | ((REGISTER) == I2C_Register_OAR2) || \
263 | ((REGISTER) == I2C_Register_TIMINGR) || \
264 | ((REGISTER) == I2C_Register_TIMEOUTR) || \
265 | ((REGISTER) == I2C_Register_ISR) || \
266 | ((REGISTER) == I2C_Register_ICR) || \
267 | ((REGISTER) == I2C_Register_PECR) || \
268 | ((REGISTER) == I2C_Register_RXDR) || \
269 | ((REGISTER) == I2C_Register_TXDR))
270 | /**
271 | * @}
272 | */
273 |
274 | /** @defgroup I2C_interrupts_definition
275 | * @{
276 | */
277 |
278 | #define I2C_IT_ERRI I2C_CR1_ERRIE
279 | #define I2C_IT_TCI I2C_CR1_TCIE
280 | #define I2C_IT_STOPI I2C_CR1_STOPIE
281 | #define I2C_IT_NACKI I2C_CR1_NACKIE
282 | #define I2C_IT_ADDRI I2C_CR1_ADDRIE
283 | #define I2C_IT_RXI I2C_CR1_RXIE
284 | #define I2C_IT_TXI I2C_CR1_TXIE
285 |
286 | #define IS_I2C_CONFIG_IT(IT) ((((IT) & (uint32_t)0xFFFFFF01) == 0x00) && ((IT) != 0x00))
287 |
288 | /**
289 | * @}
290 | */
291 |
292 | /** @defgroup I2C_flags_definition
293 | * @{
294 | */
295 |
296 | #define I2C_FLAG_TXE I2C_ISR_TXE
297 | #define I2C_FLAG_TXIS I2C_ISR_TXIS
298 | #define I2C_FLAG_RXNE I2C_ISR_RXNE
299 | #define I2C_FLAG_ADDR I2C_ISR_ADDR
300 | #define I2C_FLAG_NACKF I2C_ISR_NACKF
301 | #define I2C_FLAG_STOPF I2C_ISR_STOPF
302 | #define I2C_FLAG_TC I2C_ISR_TC
303 | #define I2C_FLAG_TCR I2C_ISR_TCR
304 | #define I2C_FLAG_BERR I2C_ISR_BERR
305 | #define I2C_FLAG_ARLO I2C_ISR_ARLO
306 | #define I2C_FLAG_OVR I2C_ISR_OVR
307 | #define I2C_FLAG_PECERR I2C_ISR_PECERR
308 | #define I2C_FLAG_TIMEOUT I2C_ISR_TIMEOUT
309 | #define I2C_FLAG_ALERT I2C_ISR_ALERT
310 | #define I2C_FLAG_BUSY I2C_ISR_BUSY
311 |
312 | #define IS_I2C_CLEAR_FLAG(FLAG) ((((FLAG) & (uint32_t)0xFFFF4000) == 0x00) && ((FLAG) != 0x00))
313 |
314 | #define IS_I2C_GET_FLAG(FLAG) (((FLAG) == I2C_FLAG_TXE) || ((FLAG) == I2C_FLAG_TXIS) || \
315 | ((FLAG) == I2C_FLAG_RXNE) || ((FLAG) == I2C_FLAG_ADDR) || \
316 | ((FLAG) == I2C_FLAG_NACKF) || ((FLAG) == I2C_FLAG_STOPF) || \
317 | ((FLAG) == I2C_FLAG_TC) || ((FLAG) == I2C_FLAG_TCR) || \
318 | ((FLAG) == I2C_FLAG_BERR) || ((FLAG) == I2C_FLAG_ARLO) || \
319 | ((FLAG) == I2C_FLAG_OVR) || ((FLAG) == I2C_FLAG_PECERR) || \
320 | ((FLAG) == I2C_FLAG_TIMEOUT) || ((FLAG) == I2C_FLAG_ALERT) || \
321 | ((FLAG) == I2C_FLAG_BUSY))
322 |
323 | /**
324 | * @}
325 | */
326 |
327 |
328 | /** @defgroup I2C_interrupts_definition
329 | * @{
330 | */
331 |
332 | #define I2C_IT_TXIS I2C_ISR_TXIS
333 | #define I2C_IT_RXNE I2C_ISR_RXNE
334 | #define I2C_IT_ADDR I2C_ISR_ADDR
335 | #define I2C_IT_NACKF I2C_ISR_NACKF
336 | #define I2C_IT_STOPF I2C_ISR_STOPF
337 | #define I2C_IT_TC I2C_ISR_TC
338 | #define I2C_IT_TCR I2C_ISR_TCR
339 | #define I2C_IT_BERR I2C_ISR_BERR
340 | #define I2C_IT_ARLO I2C_ISR_ARLO
341 | #define I2C_IT_OVR I2C_ISR_OVR
342 | #define I2C_IT_PECERR I2C_ISR_PECERR
343 | #define I2C_IT_TIMEOUT I2C_ISR_TIMEOUT
344 | #define I2C_IT_ALERT I2C_ISR_ALERT
345 |
346 | #define IS_I2C_CLEAR_IT(IT) ((((IT) & (uint32_t)0xFFFFC001) == 0x00) && ((IT) != 0x00))
347 |
348 | #define IS_I2C_GET_IT(IT) (((IT) == I2C_IT_TXIS) || ((IT) == I2C_IT_RXNE) || \
349 | ((IT) == I2C_IT_ADDR) || ((IT) == I2C_IT_NACKF) || \
350 | ((IT) == I2C_IT_STOPF) || ((IT) == I2C_IT_TC) || \
351 | ((IT) == I2C_IT_TCR) || ((IT) == I2C_IT_BERR) || \
352 | ((IT) == I2C_IT_ARLO) || ((IT) == I2C_IT_OVR) || \
353 | ((IT) == I2C_IT_PECERR) || ((IT) == I2C_IT_TIMEOUT) || \
354 | ((IT) == I2C_IT_ALERT))
355 |
356 |
357 | /**
358 | * @}
359 | */
360 |
361 | /** @defgroup I2C_ReloadEndMode_definition
362 | * @{
363 | */
364 |
365 | #define I2C_Reload_Mode I2C_CR2_RELOAD
366 | #define I2C_AutoEnd_Mode I2C_CR2_AUTOEND
367 | #define I2C_SoftEnd_Mode ((uint32_t)0x00000000)
368 |
369 |
370 | #define IS_RELOAD_END_MODE(MODE) (((MODE) == I2C_Reload_Mode) || \
371 | ((MODE) == I2C_AutoEnd_Mode) || \
372 | ((MODE) == I2C_SoftEnd_Mode))
373 |
374 |
375 | /**
376 | * @}
377 | */
378 |
379 | /** @defgroup I2C_StartStopMode_definition
380 | * @{
381 | */
382 |
383 | #define I2C_No_StartStop ((uint32_t)0x00000000)
384 | #define I2C_Generate_Stop I2C_CR2_STOP
385 | #define I2C_Generate_Start_Read (uint32_t)(I2C_CR2_START | I2C_CR2_RD_WRN)
386 | #define I2C_Generate_Start_Write I2C_CR2_START
387 |
388 |
389 | #define IS_START_STOP_MODE(MODE) (((MODE) == I2C_Generate_Stop) || \
390 | ((MODE) == I2C_Generate_Start_Read) || \
391 | ((MODE) == I2C_Generate_Start_Write) || \
392 | ((MODE) == I2C_No_StartStop))
393 |
394 |
395 | /**
396 | * @}
397 | */
398 |
399 | /**
400 | * @}
401 | */
402 |
403 | /* Exported macro ------------------------------------------------------------*/
404 | /* Exported functions ------------------------------------------------------- */
405 |
406 |
407 | /* Initialization and Configuration functions *********************************/
408 | void I2C_DeInit(I2C_TypeDef* I2Cx);
409 | void I2C_Init(I2C_TypeDef* I2Cx, I2C_InitTypeDef* I2C_InitStruct);
410 | void I2C_StructInit(I2C_InitTypeDef* I2C_InitStruct);
411 | void I2C_Cmd(I2C_TypeDef* I2Cx, FunctionalState NewState);
412 | void I2C_SoftwareResetCmd(I2C_TypeDef* I2Cx);
413 | void I2C_ITConfig(I2C_TypeDef* I2Cx, uint32_t I2C_IT, FunctionalState NewState);
414 | void I2C_StretchClockCmd(I2C_TypeDef* I2Cx, FunctionalState NewState);
415 | void I2C_StopModeCmd(I2C_TypeDef* I2Cx, FunctionalState NewState); /*!< not applicable for STM32F030 devices */
416 | void I2C_DualAddressCmd(I2C_TypeDef* I2Cx, FunctionalState NewState);
417 | void I2C_OwnAddress2Config(I2C_TypeDef* I2Cx, uint16_t Address, uint8_t Mask);
418 | void I2C_GeneralCallCmd(I2C_TypeDef* I2Cx, FunctionalState NewState);
419 | void I2C_SlaveByteControlCmd(I2C_TypeDef* I2Cx, FunctionalState NewState);
420 | void I2C_SlaveAddressConfig(I2C_TypeDef* I2Cx, uint16_t Address);
421 | void I2C_10BitAddressingModeCmd(I2C_TypeDef* I2Cx, FunctionalState NewState);
422 |
423 | /* Communications handling functions ******************************************/
424 | void I2C_AutoEndCmd(I2C_TypeDef* I2Cx, FunctionalState NewState);
425 | void I2C_ReloadCmd(I2C_TypeDef* I2Cx, FunctionalState NewState);
426 | void I2C_NumberOfBytesConfig(I2C_TypeDef* I2Cx, uint8_t Number_Bytes);
427 | void I2C_MasterRequestConfig(I2C_TypeDef* I2Cx, uint16_t I2C_Direction);
428 | void I2C_GenerateSTART(I2C_TypeDef* I2Cx, FunctionalState NewState);
429 | void I2C_GenerateSTOP(I2C_TypeDef* I2Cx, FunctionalState NewState);
430 | void I2C_10BitAddressHeaderCmd(I2C_TypeDef* I2Cx, FunctionalState NewState);
431 | void I2C_AcknowledgeConfig(I2C_TypeDef* I2Cx, FunctionalState NewState);
432 | uint8_t I2C_GetAddressMatched(I2C_TypeDef* I2Cx);
433 | uint16_t I2C_GetTransferDirection(I2C_TypeDef* I2Cx);
434 | void I2C_TransferHandling(I2C_TypeDef* I2Cx, uint16_t Address, uint8_t Number_Bytes, uint32_t ReloadEndMode, uint32_t StartStopMode);
435 |
436 | /* SMBUS management functions ************************************************/
437 | void I2C_SMBusAlertCmd(I2C_TypeDef* I2Cx, FunctionalState NewState);
438 | void I2C_ClockTimeoutCmd(I2C_TypeDef* I2Cx, FunctionalState NewState);
439 | void I2C_ExtendedClockTimeoutCmd(I2C_TypeDef* I2Cx, FunctionalState NewState);
440 | void I2C_IdleClockTimeoutCmd(I2C_TypeDef* I2Cx, FunctionalState NewState);
441 | void I2C_TimeoutAConfig(I2C_TypeDef* I2Cx, uint16_t Timeout);
442 | void I2C_TimeoutBConfig(I2C_TypeDef* I2Cx, uint16_t Timeout);
443 | void I2C_CalculatePEC(I2C_TypeDef* I2Cx, FunctionalState NewState);
444 | void I2C_PECRequestCmd(I2C_TypeDef* I2Cx, FunctionalState NewState);
445 | uint8_t I2C_GetPEC(I2C_TypeDef* I2Cx);
446 |
447 | /* I2C registers management functions *****************************************/
448 | uint32_t I2C_ReadRegister(I2C_TypeDef* I2Cx, uint8_t I2C_Register);
449 |
450 | /* Data transfers management functions ****************************************/
451 | void I2C_SendData(I2C_TypeDef* I2Cx, uint8_t Data);
452 | uint8_t I2C_ReceiveData(I2C_TypeDef* I2Cx);
453 |
454 | /* DMA transfers management functions *****************************************/
455 | void I2C_DMACmd(I2C_TypeDef* I2Cx, uint32_t I2C_DMAReq, FunctionalState NewState);
456 |
457 | /* Interrupts and flags management functions **********************************/
458 | FlagStatus I2C_GetFlagStatus(I2C_TypeDef* I2Cx, uint32_t I2C_FLAG);
459 | void I2C_ClearFlag(I2C_TypeDef* I2Cx, uint32_t I2C_FLAG);
460 | ITStatus I2C_GetITStatus(I2C_TypeDef* I2Cx, uint32_t I2C_IT);
461 | void I2C_ClearITPendingBit(I2C_TypeDef* I2Cx, uint32_t I2C_IT);
462 |
463 |
464 | #ifdef __cplusplus
465 | }
466 | #endif
467 |
468 | #endif /*__STM32F0XX_I2C_H */
469 |
470 | /**
471 | * @}
472 | */
473 |
474 | /**
475 | * @}
476 | */
477 |
478 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
479 |
--------------------------------------------------------------------------------