├── .gitignore
├── GUI
└── SerialTool.py
├── JLinkSettings.ini
├── LICENSE
├── Listings
├── mrobot.map
├── mrobot_full.map
└── startup_stm32f10x_hd.lst
├── Objects
├── mrobot.axf
├── mrobot.hex
└── mrobot.lnp
├── README.md
├── RTE
├── Device
│ ├── STM32F103RC
│ │ ├── RTE_Device.h
│ │ ├── startup_stm32f10x_hd.s
│ │ ├── stm32f10x_conf.h
│ │ └── system_stm32f10x.c
│ └── STM32F103VE
│ │ ├── RTE_Device.h
│ │ ├── startup_stm32f10x_hd.s
│ │ ├── stm32f10x_conf.h
│ │ └── system_stm32f10x.c
└── RTE_Components.h
├── keilkilll.bat
├── mrobot.uvoptx
├── mrobot.uvprojx
├── ros_node
└── Serial_node.cpp
└── src
├── driver
├── battery.c
├── beep.c
├── boarddrv.c
├── crc.c
├── exti.c
├── led.c
├── motor.c
├── mpu6050
│ ├── eMPL
│ │ ├── inv_mpu.c
│ │ └── inv_mpu_dmp_motion_driver.c
│ ├── mpu6050.c
│ └── mpuiic.c
├── protocol.c
├── softdelay.c
├── sonar.c
├── usart.c
└── watchdog.c
├── include
├── battery.h
├── beep.h
├── boarddef.h
├── boardfunc.h
├── common.h
├── crc.h
├── dmpKey.h
├── dmpmap.h
├── exti.h
├── filters.h
├── inv_mpu.h
├── inv_mpu_dmp_motion_driver.h
├── led.h
├── motor.h
├── mpu6050.h
├── mpuiic.h
├── protocol.h
├── rpstm32.h
├── softdelay.h
├── sonar.h
├── stm32f10x_it.h
├── sys.h
├── types.h
├── usart.h
└── watchdog.h
├── main.c
└── utils
├── filters.c
├── hwconf.c
└── stm32f10x_it.c
/.gitignore:
--------------------------------------------------------------------------------
1 | .DS_Store
2 | *.o
3 | *.d
4 | *.crf
5 | *.dep
6 | *.inp
7 | *.sct
8 | *.htm
9 | *.dbgconf
--------------------------------------------------------------------------------
/JLinkSettings.ini:
--------------------------------------------------------------------------------
1 | [BREAKPOINTS]
2 | ForceImpTypeAny = 0
3 | ShowInfoWin = 1
4 | EnableFlashBP = 2
5 | BPDuringExecution = 0
6 | [CFI]
7 | CFISize = 0x00
8 | CFIAddr = 0x00
9 | [CPU]
10 | MonModeVTableAddr = 0xFFFFFFFF
11 | MonModeDebug = 0
12 | MaxNumAPs = 0
13 | LowPowerHandlingMode = 0
14 | OverrideMemMap = 0
15 | AllowSimulation = 1
16 | ScriptFile=""
17 | [FLASH]
18 | CacheExcludeSize = 0x00
19 | CacheExcludeAddr = 0x00
20 | MinNumBytesFlashDL = 0
21 | SkipProgOnCRCMatch = 1
22 | VerifyDownload = 1
23 | AllowCaching = 1
24 | EnableFlashDL = 2
25 | Override = 0
26 | Device="ARM7"
27 | [GENERAL]
28 | WorkRAMSize = 0x00
29 | WorkRAMAddr = 0x00
30 | RAMUsageLimit = 0x00
31 | [SWO]
32 | SWOLogFile=""
33 | [MEM]
34 | RdOverrideOrMask = 0x00
35 | RdOverrideAndMask = 0xFFFFFFFF
36 | RdOverrideAddr = 0xFFFFFFFF
37 | WrOverrideOrMask = 0x00
38 | WrOverrideAndMask = 0xFFFFFFFF
39 | WrOverrideAddr = 0xFFFFFFFF
40 |
--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
1 | MIT License
2 |
3 | Copyright (c) 2017 Ray
4 |
5 | Permission is hereby granted, free of charge, to any person obtaining a copy
6 | of this software and associated documentation files (the "Software"), to deal
7 | in the Software without restriction, including without limitation the rights
8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 |
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 |
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 |
--------------------------------------------------------------------------------
/Objects/mrobot.axf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/zsirui/mrobot/5f7474090a7d357fdad973d72531111f9844cb04/Objects/mrobot.axf
--------------------------------------------------------------------------------
/Objects/mrobot.lnp:
--------------------------------------------------------------------------------
1 | --cpu Cortex-M3
2 | ".\objects\main.o"
3 | ".\objects\battery.o"
4 | ".\objects\beep.o"
5 | ".\objects\boarddrv.o"
6 | ".\objects\crc.o"
7 | ".\objects\led.o"
8 | ".\objects\motor.o"
9 | ".\objects\softdelay.o"
10 | ".\objects\usart.o"
11 | ".\objects\watchdog.o"
12 | ".\objects\exti.o"
13 | ".\objects\sonar.o"
14 | ".\objects\protocol.o"
15 | ".\objects\mpu6050.o"
16 | ".\objects\mpuiic.o"
17 | ".\objects\inv_mpu.o"
18 | ".\objects\inv_mpu_dmp_motion_driver.o"
19 | ".\objects\filters.o"
20 | ".\objects\hwconf.o"
21 | ".\objects\stm32f10x_it.o"
22 | ".\objects\startup_stm32f10x_hd.o"
23 | ".\objects\system_stm32f10x.o"
24 | ".\objects\misc.o"
25 | ".\objects\stm32f10x_adc.o"
26 | ".\objects\stm32f10x_dma.o"
27 | ".\objects\stm32f10x_exti.o"
28 | ".\objects\stm32f10x_gpio.o"
29 | ".\objects\stm32f10x_i2c.o"
30 | ".\objects\stm32f10x_rcc.o"
31 | ".\objects\stm32f10x_tim.o"
32 | ".\objects\stm32f10x_usart.o"
33 | ".\objects\stm32f10x_wwdg.o"
34 | --library_type=microlib --strict --scatter ".\Objects\mrobot.sct"
35 | --summary_stderr --info summarysizes --map --xref --callgraph --symbols
36 | --info sizes --info totals --info unused --info veneers
37 | --list ".\Listings\mrobot.map" -o .\Objects\mrobot.axf
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # mrobot
2 |
3 | ### 主要功能:
4 |
5 | 1.电机控制(PID调速)
6 |
7 | 2.串口通讯(自定义串口通讯协议)
8 |
9 | 3.超声波测距
10 |
11 | 4.电池电量检测(ADC采样)
12 |
13 | 5.蜂鸣器
14 |
15 | 6.三色LED闪烁
16 |
17 | 7.六轴传感器MPU6050
18 |
19 | ### 宏定义:
20 |
21 | _RGB:切换大小RGB-LED灯,全局定义为使用小灯,全局未定义为使用大灯,默认全局未定义
22 |
23 | _TEST:测试按键功能,全局定义为开启测试按键功能,全局未定义为关闭测试按键功能,默认全局定义
24 |
25 | _DOUBLE:编码器开启双精度,全局定义为开启双精度编码器,全局未定义为关闭双精度编码器,默认全局未定义
26 |
27 | _ENABLE_MPU6050:开启MPU6050功能,全局定义为开启6050,全局未定义为关闭6050,默认全局未定义
28 |
29 | _PRINT:串口打印信息功能,全局定义为开启通过串口打印相关信息,全局未定义为关闭打印信息,默认全局未定义(注:开启后会影响串口和车载计算机通讯功能,默认使用串口3)
30 |
31 | _DMA_USART:DMA方式串口通讯功能,全局定义为使用DMA方式进行串口通讯,全局未定义为不使用DMA方式,默认全局定义
32 |
33 | _REMAP:串口3重映射功能,全局定义为将串口3重映射为PC10,PC11(串口4),全局未定义为不开启串口3重映射,默认全局未定义
34 |
35 | ### 串口通信协议:
36 |
37 | **一帧数据为:[消息头(2字节)] [命令(2字节)] [长度(1字节)] [数据(n字节,n=长度)] [校验(1字节)] [消息尾(2字节)]**
38 |
39 | **消息头固定为[0x55 0xaa],消息尾固定为[0x0d 0x0a]**
40 |
41 | ### 主控发送用命令参数:
42 |
43 | (1)0x5a 0x5a:发送速度信息和电池信息
44 |
45 | (2)0x5a 0x55:发送速度信息,电池信息和超声波信息
46 |
47 | (3)0x5a 0xaa:发送速度信息,电池信息和六轴传感器信息
48 |
49 | (4)0x5a 0xa5:发送速度信息,电池信息,超声波信息和六轴传感器信息
50 |
51 | (5)0xa5 0x5a:发送速度信息
52 |
53 | (6)0xa5 0x55:发送电池信息
54 |
55 | (7)0xa5 0xaa:发送超声波信息
56 |
57 | (8)0xa5 0xa5:发送六轴传感器信息
58 |
59 | ### 主控接收用命令参数:
60 |
61 | (1)0x55 0xaa:请求发送速度信息和电池信息
62 |
63 | (2)0x55 0x55:请求发送速度信息,电池信息和超声波信息
64 |
65 | (3)0x55 0xa5:请求发送速度信息,电池信息和六轴传感器信息
66 |
67 | (4)0x55 0x5a:请求发送速度信息,电池信息,超声波信息和六轴传感器信息
68 |
69 | (5)0xaa 0xaa:请求发送速度信息
70 |
71 | (6)0xaa 0x55:请求发送电池信息
72 |
73 | (7)0xaa 0xa5:请求发送超声波信息
74 |
75 | (8)0xaa 0x5a:请求发送六轴传感器信息
76 |
77 | ### 主控端发送数据段结构:
78 |
79 | (1)0x5a 0x5a:[左轮速度(4字节浮点数),右轮速度(4字节浮点数),电池电量(4字节浮点数),电量百分比(4字节浮点数)]
80 |
81 | (2)0x5a 0x55:[左轮速度(4字节浮点数),右轮速度(4字节浮点数),电池电量(4字节浮点数),电量百分比(4字节浮点数),前超声波(4字节浮点数),后超声波(4字节浮点数),左超声波(4字节浮点数),右超声波(4字节浮点数)]
82 |
83 | (3)0x5a 0xaa:[左轮速度(4字节浮点数),右轮速度(4字节浮点数),电池电量(4字节浮点数),电量百分比(4字节浮点数),pitch角(4字节浮点数),roll角(4字节浮点数),yaw角(4字节浮点数),温度(4字节浮点数)]
84 |
85 | (4)0x5a 0xa5:[左轮速度(4字节浮点数),右轮速度(4字节浮点数),电池电量(4字节浮点数),电量百分比(4字节浮点数),pitch角(4字节浮点数),roll角(4字节浮点数),yaw角(4字节浮点数),温度(4字节浮点数),前超声波(4字节浮点数),后超声波(4字节浮点数),左超声波(4字节浮点数),右超声波(4字节浮点数)]
86 |
87 | (5)0xa5 0x5a:[左轮速度(4字节浮点数),右轮速度(4字节浮点数)]
88 |
89 | (6)0xa5 0x55:[电池电量(4字节浮点数),电量百分比(4字节浮点数)]
90 |
91 | (7)0xa5 0xaa:[前超声波(4字节浮点数),后超声波(4字节浮点数),左超声波(4字节浮点数),右超声波(4字节浮点数)]
92 |
93 | (8)0xa5 0xa5:[pitch角(4字节浮点数),roll角(4字节浮点数),yaw角(4字节浮点数),温度(4字节浮点数)]
94 |
95 | ### 主控端接收数据段结构:
96 |
97 | [左轮速度(4字节整数),右轮速度(4字节整数)]
98 |
99 | ### GUI
100 |
101 | 使用PyQt编写的GUI小程序,用来检测小车串口通讯是否正常
102 |
103 | ### ROS Node
104 |
105 | ROS中node节点cpp文件,是小车和ROS通讯的重要桥梁
106 |
107 |
--------------------------------------------------------------------------------
/RTE/Device/STM32F103RC/startup_stm32f10x_hd.s:
--------------------------------------------------------------------------------
1 | ;******************** (C) COPYRIGHT 2011 STMicroelectronics ********************
2 | ;* File Name : startup_stm32f10x_hd.s
3 | ;* Author : MCD Application Team
4 | ;* Version : V3.5.0
5 | ;* Date : 11-March-2011
6 | ;* Description : STM32F10x High Density Devices vector table for MDK-ARM
7 | ;* 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 clock system and also configure the external
13 | ;* SRAM mounted on STM3210E-EVAL board to be used as data
14 | ;* memory (optional, to be enabled by user)
15 | ;* - Branches to __main in the C library (which eventually
16 | ;* calls main()).
17 | ;* After Reset the CortexM3 processor is in Thread mode,
18 | ;* priority is Privileged, and the Stack is set to Main.
19 | ;* <<< Use Configuration Wizard in Context Menu >>>
20 | ;*******************************************************************************
21 | ; THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
22 | ; WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
23 | ; AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
24 | ; INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
25 | ; CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
26 | ; INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
27 | ;*******************************************************************************
28 |
29 | ; Amount of memory (in bytes) allocated for Stack
30 | ; Tailor this value to your application needs
31 | ; Stack Configuration
32 | ; Stack Size (in Bytes) <0x0-0xFFFFFFFF:8>
33 | ;
34 |
35 | Stack_Size EQU 0x00000400
36 |
37 | AREA STACK, NOINIT, READWRITE, ALIGN=3
38 | Stack_Mem SPACE Stack_Size
39 | __initial_sp
40 |
41 | ; Heap Configuration
42 | ; Heap Size (in Bytes) <0x0-0xFFFFFFFF:8>
43 | ;
44 |
45 | Heap_Size EQU 0x00000200
46 |
47 | AREA HEAP, NOINIT, READWRITE, ALIGN=3
48 | __heap_base
49 | Heap_Mem SPACE Heap_Size
50 | __heap_limit
51 |
52 | PRESERVE8
53 | THUMB
54 |
55 |
56 | ; Vector Table Mapped to Address 0 at Reset
57 | AREA RESET, DATA, READONLY
58 | EXPORT __Vectors
59 | EXPORT __Vectors_End
60 | EXPORT __Vectors_Size
61 |
62 | __Vectors DCD __initial_sp ; Top of Stack
63 | DCD Reset_Handler ; Reset Handler
64 | DCD NMI_Handler ; NMI Handler
65 | DCD HardFault_Handler ; Hard Fault Handler
66 | DCD MemManage_Handler ; MPU Fault Handler
67 | DCD BusFault_Handler ; Bus Fault Handler
68 | DCD UsageFault_Handler ; Usage Fault Handler
69 | DCD 0 ; Reserved
70 | DCD 0 ; Reserved
71 | DCD 0 ; Reserved
72 | DCD 0 ; Reserved
73 | DCD SVC_Handler ; SVCall Handler
74 | DCD DebugMon_Handler ; Debug Monitor Handler
75 | DCD 0 ; Reserved
76 | DCD PendSV_Handler ; PendSV Handler
77 | DCD SysTick_Handler ; SysTick Handler
78 |
79 | ; External Interrupts
80 | DCD WWDG_IRQHandler ; Window Watchdog
81 | DCD PVD_IRQHandler ; PVD through EXTI Line detect
82 | DCD TAMPER_IRQHandler ; Tamper
83 | DCD RTC_IRQHandler ; RTC
84 | DCD FLASH_IRQHandler ; Flash
85 | DCD RCC_IRQHandler ; RCC
86 | DCD EXTI0_IRQHandler ; EXTI Line 0
87 | DCD EXTI1_IRQHandler ; EXTI Line 1
88 | DCD EXTI2_IRQHandler ; EXTI Line 2
89 | DCD EXTI3_IRQHandler ; EXTI Line 3
90 | DCD EXTI4_IRQHandler ; EXTI Line 4
91 | DCD DMA1_Channel1_IRQHandler ; DMA1 Channel 1
92 | DCD DMA1_Channel2_IRQHandler ; DMA1 Channel 2
93 | DCD DMA1_Channel3_IRQHandler ; DMA1 Channel 3
94 | DCD DMA1_Channel4_IRQHandler ; DMA1 Channel 4
95 | DCD DMA1_Channel5_IRQHandler ; DMA1 Channel 5
96 | DCD DMA1_Channel6_IRQHandler ; DMA1 Channel 6
97 | DCD DMA1_Channel7_IRQHandler ; DMA1 Channel 7
98 | DCD ADC1_2_IRQHandler ; ADC1 & ADC2
99 | DCD USB_HP_CAN1_TX_IRQHandler ; USB High Priority or CAN1 TX
100 | DCD USB_LP_CAN1_RX0_IRQHandler ; USB Low Priority or CAN1 RX0
101 | DCD CAN1_RX1_IRQHandler ; CAN1 RX1
102 | DCD CAN1_SCE_IRQHandler ; CAN1 SCE
103 | DCD EXTI9_5_IRQHandler ; EXTI Line 9..5
104 | DCD TIM1_BRK_IRQHandler ; TIM1 Break
105 | DCD TIM1_UP_IRQHandler ; TIM1 Update
106 | DCD TIM1_TRG_COM_IRQHandler ; TIM1 Trigger and Commutation
107 | DCD TIM1_CC_IRQHandler ; TIM1 Capture Compare
108 | DCD TIM2_IRQHandler ; TIM2
109 | DCD TIM3_IRQHandler ; TIM3
110 | DCD TIM4_IRQHandler ; TIM4
111 | DCD I2C1_EV_IRQHandler ; I2C1 Event
112 | DCD I2C1_ER_IRQHandler ; I2C1 Error
113 | DCD I2C2_EV_IRQHandler ; I2C2 Event
114 | DCD I2C2_ER_IRQHandler ; I2C2 Error
115 | DCD SPI1_IRQHandler ; SPI1
116 | DCD SPI2_IRQHandler ; SPI2
117 | DCD USART1_IRQHandler ; USART1
118 | DCD USART2_IRQHandler ; USART2
119 | DCD USART3_IRQHandler ; USART3
120 | DCD EXTI15_10_IRQHandler ; EXTI Line 15..10
121 | DCD RTCAlarm_IRQHandler ; RTC Alarm through EXTI Line
122 | DCD USBWakeUp_IRQHandler ; USB Wakeup from suspend
123 | DCD TIM8_BRK_IRQHandler ; TIM8 Break
124 | DCD TIM8_UP_IRQHandler ; TIM8 Update
125 | DCD TIM8_TRG_COM_IRQHandler ; TIM8 Trigger and Commutation
126 | DCD TIM8_CC_IRQHandler ; TIM8 Capture Compare
127 | DCD ADC3_IRQHandler ; ADC3
128 | DCD FSMC_IRQHandler ; FSMC
129 | DCD SDIO_IRQHandler ; SDIO
130 | DCD TIM5_IRQHandler ; TIM5
131 | DCD SPI3_IRQHandler ; SPI3
132 | DCD UART4_IRQHandler ; UART4
133 | DCD UART5_IRQHandler ; UART5
134 | DCD TIM6_IRQHandler ; TIM6
135 | DCD TIM7_IRQHandler ; TIM7
136 | DCD DMA2_Channel1_IRQHandler ; DMA2 Channel1
137 | DCD DMA2_Channel2_IRQHandler ; DMA2 Channel2
138 | DCD DMA2_Channel3_IRQHandler ; DMA2 Channel3
139 | DCD DMA2_Channel4_5_IRQHandler ; DMA2 Channel4 & Channel5
140 | __Vectors_End
141 |
142 | __Vectors_Size EQU __Vectors_End - __Vectors
143 |
144 | AREA |.text|, CODE, READONLY
145 |
146 | ; Reset handler
147 | Reset_Handler PROC
148 | EXPORT Reset_Handler [WEAK]
149 | IMPORT __main
150 | IMPORT SystemInit
151 | LDR R0, =SystemInit
152 | BLX R0
153 | LDR R0, =__main
154 | BX R0
155 | ENDP
156 |
157 | ; Dummy Exception Handlers (infinite loops which can be modified)
158 |
159 | NMI_Handler PROC
160 | EXPORT NMI_Handler [WEAK]
161 | B .
162 | ENDP
163 | HardFault_Handler\
164 | PROC
165 | EXPORT HardFault_Handler [WEAK]
166 | B .
167 | ENDP
168 | MemManage_Handler\
169 | PROC
170 | EXPORT MemManage_Handler [WEAK]
171 | B .
172 | ENDP
173 | BusFault_Handler\
174 | PROC
175 | EXPORT BusFault_Handler [WEAK]
176 | B .
177 | ENDP
178 | UsageFault_Handler\
179 | PROC
180 | EXPORT UsageFault_Handler [WEAK]
181 | B .
182 | ENDP
183 | SVC_Handler PROC
184 | EXPORT SVC_Handler [WEAK]
185 | B .
186 | ENDP
187 | DebugMon_Handler\
188 | PROC
189 | EXPORT DebugMon_Handler [WEAK]
190 | B .
191 | ENDP
192 | PendSV_Handler PROC
193 | EXPORT PendSV_Handler [WEAK]
194 | B .
195 | ENDP
196 | SysTick_Handler PROC
197 | EXPORT SysTick_Handler [WEAK]
198 | B .
199 | ENDP
200 |
201 | Default_Handler PROC
202 |
203 | EXPORT WWDG_IRQHandler [WEAK]
204 | EXPORT PVD_IRQHandler [WEAK]
205 | EXPORT TAMPER_IRQHandler [WEAK]
206 | EXPORT RTC_IRQHandler [WEAK]
207 | EXPORT FLASH_IRQHandler [WEAK]
208 | EXPORT RCC_IRQHandler [WEAK]
209 | EXPORT EXTI0_IRQHandler [WEAK]
210 | EXPORT EXTI1_IRQHandler [WEAK]
211 | EXPORT EXTI2_IRQHandler [WEAK]
212 | EXPORT EXTI3_IRQHandler [WEAK]
213 | EXPORT EXTI4_IRQHandler [WEAK]
214 | EXPORT DMA1_Channel1_IRQHandler [WEAK]
215 | EXPORT DMA1_Channel2_IRQHandler [WEAK]
216 | EXPORT DMA1_Channel3_IRQHandler [WEAK]
217 | EXPORT DMA1_Channel4_IRQHandler [WEAK]
218 | EXPORT DMA1_Channel5_IRQHandler [WEAK]
219 | EXPORT DMA1_Channel6_IRQHandler [WEAK]
220 | EXPORT DMA1_Channel7_IRQHandler [WEAK]
221 | EXPORT ADC1_2_IRQHandler [WEAK]
222 | EXPORT USB_HP_CAN1_TX_IRQHandler [WEAK]
223 | EXPORT USB_LP_CAN1_RX0_IRQHandler [WEAK]
224 | EXPORT CAN1_RX1_IRQHandler [WEAK]
225 | EXPORT CAN1_SCE_IRQHandler [WEAK]
226 | EXPORT EXTI9_5_IRQHandler [WEAK]
227 | EXPORT TIM1_BRK_IRQHandler [WEAK]
228 | EXPORT TIM1_UP_IRQHandler [WEAK]
229 | EXPORT TIM1_TRG_COM_IRQHandler [WEAK]
230 | EXPORT TIM1_CC_IRQHandler [WEAK]
231 | EXPORT TIM2_IRQHandler [WEAK]
232 | EXPORT TIM3_IRQHandler [WEAK]
233 | EXPORT TIM4_IRQHandler [WEAK]
234 | EXPORT I2C1_EV_IRQHandler [WEAK]
235 | EXPORT I2C1_ER_IRQHandler [WEAK]
236 | EXPORT I2C2_EV_IRQHandler [WEAK]
237 | EXPORT I2C2_ER_IRQHandler [WEAK]
238 | EXPORT SPI1_IRQHandler [WEAK]
239 | EXPORT SPI2_IRQHandler [WEAK]
240 | EXPORT USART1_IRQHandler [WEAK]
241 | EXPORT USART2_IRQHandler [WEAK]
242 | EXPORT USART3_IRQHandler [WEAK]
243 | EXPORT EXTI15_10_IRQHandler [WEAK]
244 | EXPORT RTCAlarm_IRQHandler [WEAK]
245 | EXPORT USBWakeUp_IRQHandler [WEAK]
246 | EXPORT TIM8_BRK_IRQHandler [WEAK]
247 | EXPORT TIM8_UP_IRQHandler [WEAK]
248 | EXPORT TIM8_TRG_COM_IRQHandler [WEAK]
249 | EXPORT TIM8_CC_IRQHandler [WEAK]
250 | EXPORT ADC3_IRQHandler [WEAK]
251 | EXPORT FSMC_IRQHandler [WEAK]
252 | EXPORT SDIO_IRQHandler [WEAK]
253 | EXPORT TIM5_IRQHandler [WEAK]
254 | EXPORT SPI3_IRQHandler [WEAK]
255 | EXPORT UART4_IRQHandler [WEAK]
256 | EXPORT UART5_IRQHandler [WEAK]
257 | EXPORT TIM6_IRQHandler [WEAK]
258 | EXPORT TIM7_IRQHandler [WEAK]
259 | EXPORT DMA2_Channel1_IRQHandler [WEAK]
260 | EXPORT DMA2_Channel2_IRQHandler [WEAK]
261 | EXPORT DMA2_Channel3_IRQHandler [WEAK]
262 | EXPORT DMA2_Channel4_5_IRQHandler [WEAK]
263 |
264 | WWDG_IRQHandler
265 | PVD_IRQHandler
266 | TAMPER_IRQHandler
267 | RTC_IRQHandler
268 | FLASH_IRQHandler
269 | RCC_IRQHandler
270 | EXTI0_IRQHandler
271 | EXTI1_IRQHandler
272 | EXTI2_IRQHandler
273 | EXTI3_IRQHandler
274 | EXTI4_IRQHandler
275 | DMA1_Channel1_IRQHandler
276 | DMA1_Channel2_IRQHandler
277 | DMA1_Channel3_IRQHandler
278 | DMA1_Channel4_IRQHandler
279 | DMA1_Channel5_IRQHandler
280 | DMA1_Channel6_IRQHandler
281 | DMA1_Channel7_IRQHandler
282 | ADC1_2_IRQHandler
283 | USB_HP_CAN1_TX_IRQHandler
284 | USB_LP_CAN1_RX0_IRQHandler
285 | CAN1_RX1_IRQHandler
286 | CAN1_SCE_IRQHandler
287 | EXTI9_5_IRQHandler
288 | TIM1_BRK_IRQHandler
289 | TIM1_UP_IRQHandler
290 | TIM1_TRG_COM_IRQHandler
291 | TIM1_CC_IRQHandler
292 | TIM2_IRQHandler
293 | TIM3_IRQHandler
294 | TIM4_IRQHandler
295 | I2C1_EV_IRQHandler
296 | I2C1_ER_IRQHandler
297 | I2C2_EV_IRQHandler
298 | I2C2_ER_IRQHandler
299 | SPI1_IRQHandler
300 | SPI2_IRQHandler
301 | USART1_IRQHandler
302 | USART2_IRQHandler
303 | USART3_IRQHandler
304 | EXTI15_10_IRQHandler
305 | RTCAlarm_IRQHandler
306 | USBWakeUp_IRQHandler
307 | TIM8_BRK_IRQHandler
308 | TIM8_UP_IRQHandler
309 | TIM8_TRG_COM_IRQHandler
310 | TIM8_CC_IRQHandler
311 | ADC3_IRQHandler
312 | FSMC_IRQHandler
313 | SDIO_IRQHandler
314 | TIM5_IRQHandler
315 | SPI3_IRQHandler
316 | UART4_IRQHandler
317 | UART5_IRQHandler
318 | TIM6_IRQHandler
319 | TIM7_IRQHandler
320 | DMA2_Channel1_IRQHandler
321 | DMA2_Channel2_IRQHandler
322 | DMA2_Channel3_IRQHandler
323 | DMA2_Channel4_5_IRQHandler
324 | B .
325 |
326 | ENDP
327 |
328 | ALIGN
329 |
330 | ;*******************************************************************************
331 | ; User Stack and Heap initialization
332 | ;*******************************************************************************
333 | IF :DEF:__MICROLIB
334 |
335 | EXPORT __initial_sp
336 | EXPORT __heap_base
337 | EXPORT __heap_limit
338 |
339 | ELSE
340 |
341 | IMPORT __use_two_region_memory
342 | EXPORT __user_initial_stackheap
343 |
344 | __user_initial_stackheap
345 |
346 | LDR R0, = Heap_Mem
347 | LDR R1, =(Stack_Mem + Stack_Size)
348 | LDR R2, = (Heap_Mem + Heap_Size)
349 | LDR R3, = Stack_Mem
350 | BX LR
351 |
352 | ALIGN
353 |
354 | ENDIF
355 |
356 | END
357 |
358 | ;******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE*****
359 |
--------------------------------------------------------------------------------
/RTE/Device/STM32F103RC/stm32f10x_conf.h:
--------------------------------------------------------------------------------
1 | /**
2 | ******************************************************************************
3 | * @file Project/STM32F10x_StdPeriph_Template/stm32f10x_conf.h
4 | * @author MCD Application Team
5 | * @version V3.5.0
6 | * @date 08-April-2011
7 | * @brief Library configuration file.
8 | ******************************************************************************
9 | * @attention
10 | *
11 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
12 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
13 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
14 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
15 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
16 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
17 | *
18 | *
© COPYRIGHT 2011 STMicroelectronics
19 | ******************************************************************************
20 | */
21 |
22 | /* Define to prevent recursive inclusion -------------------------------------*/
23 | #ifndef __STM32F10x_CONF_H
24 | #define __STM32F10x_CONF_H
25 |
26 | /* Run Time Environment will set specific #define for each selected module below */
27 | #include "RTE_Components.h"
28 |
29 | #ifdef RTE_DEVICE_STDPERIPH_ADC
30 | #include "stm32f10x_adc.h"
31 | #endif
32 | #ifdef RTE_DEVICE_STDPERIPH_BKP
33 | #include "stm32f10x_bkp.h"
34 | #endif
35 | #ifdef RTE_DEVICE_STDPERIPH_CAN
36 | #include "stm32f10x_can.h"
37 | #endif
38 | #ifdef RTE_DEVICE_STDPERIPH_CEC
39 | #include "stm32f10x_cec.h"
40 | #endif
41 | #ifdef RTE_DEVICE_STDPERIPH_CRC
42 | #include "stm32f10x_crc.h"
43 | #endif
44 | #ifdef RTE_DEVICE_STDPERIPH_DAC
45 | #include "stm32f10x_dac.h"
46 | #endif
47 | #ifdef RTE_DEVICE_STDPERIPH_DBGMCU
48 | #include "stm32f10x_dbgmcu.h"
49 | #endif
50 | #ifdef RTE_DEVICE_STDPERIPH_DMA
51 | #include "stm32f10x_dma.h"
52 | #endif
53 | #ifdef RTE_DEVICE_STDPERIPH_EXTI
54 | #include "stm32f10x_exti.h"
55 | #endif
56 | #ifdef RTE_DEVICE_STDPERIPH_FLASH
57 | #include "stm32f10x_flash.h"
58 | #endif
59 | #ifdef RTE_DEVICE_STDPERIPH_FSMC
60 | #include "stm32f10x_fsmc.h"
61 | #endif
62 | #ifdef RTE_DEVICE_STDPERIPH_GPIO
63 | #include "stm32f10x_gpio.h"
64 | #endif
65 | #ifdef RTE_DEVICE_STDPERIPH_I2C
66 | #include "stm32f10x_i2c.h"
67 | #endif
68 | #ifdef RTE_DEVICE_STDPERIPH_IWDG
69 | #include "stm32f10x_iwdg.h"
70 | #endif
71 | #ifdef RTE_DEVICE_STDPERIPH_PWR
72 | #include "stm32f10x_pwr.h"
73 | #endif
74 | #ifdef RTE_DEVICE_STDPERIPH_RCC
75 | #include "stm32f10x_rcc.h"
76 | #endif
77 | #ifdef RTE_DEVICE_STDPERIPH_RTC
78 | #include "stm32f10x_rtc.h"
79 | #endif
80 | #ifdef RTE_DEVICE_STDPERIPH_SDIO
81 | #include "stm32f10x_sdio.h"
82 | #endif
83 | #ifdef RTE_DEVICE_STDPERIPH_SPI
84 | #include "stm32f10x_spi.h"
85 | #endif
86 | #ifdef RTE_DEVICE_STDPERIPH_TIM
87 | #include "stm32f10x_tim.h"
88 | #endif
89 | #ifdef RTE_DEVICE_STDPERIPH_USART
90 | #include "stm32f10x_usart.h"
91 | #endif
92 | #ifdef RTE_DEVICE_STDPERIPH_WWDG
93 | #include "stm32f10x_wwdg.h"
94 | #endif
95 | #ifdef RTE_DEVICE_STDPERIPH_FRAMEWORK
96 | #include "misc.h" /* High level functions for NVIC and SysTick (add-on to CMSIS functions) */
97 | #endif
98 |
99 | /* Exported types ------------------------------------------------------------*/
100 | /* Exported constants --------------------------------------------------------*/
101 | /* Uncomment the line below to expanse the "assert_param" macro in the
102 | Standard Peripheral Library drivers code */
103 | /* #define USE_FULL_ASSERT 1 */
104 |
105 | /* Exported macro ------------------------------------------------------------*/
106 | #ifdef USE_FULL_ASSERT
107 |
108 | /**
109 | * @brief The assert_param macro is used for function's parameters check.
110 | * @param expr: If expr is false, it calls assert_failed function which reports
111 | * the name of the source file and the source line number of the call
112 | * that failed. If expr is true, it returns no value.
113 | * @retval None
114 | */
115 | #define assert_param(expr) ((expr) ? (void)0 : assert_failed((uint8_t *)__FILE__, __LINE__))
116 | /* Exported functions ------------------------------------------------------- */
117 | void assert_failed(uint8_t* file, uint32_t line);
118 | #else
119 | #define assert_param(expr) ((void)0)
120 | #endif /* USE_FULL_ASSERT */
121 |
122 | #endif /* __STM32F10x_CONF_H */
123 |
124 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/
125 |
--------------------------------------------------------------------------------
/RTE/Device/STM32F103VE/startup_stm32f10x_hd.s:
--------------------------------------------------------------------------------
1 | ;******************** (C) COPYRIGHT 2011 STMicroelectronics ********************
2 | ;* File Name : startup_stm32f10x_hd.s
3 | ;* Author : MCD Application Team
4 | ;* Version : V3.5.0
5 | ;* Date : 11-March-2011
6 | ;* Description : STM32F10x High Density Devices vector table for MDK-ARM
7 | ;* 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 clock system and also configure the external
13 | ;* SRAM mounted on STM3210E-EVAL board to be used as data
14 | ;* memory (optional, to be enabled by user)
15 | ;* - Branches to __main in the C library (which eventually
16 | ;* calls main()).
17 | ;* After Reset the CortexM3 processor is in Thread mode,
18 | ;* priority is Privileged, and the Stack is set to Main.
19 | ;* <<< Use Configuration Wizard in Context Menu >>>
20 | ;*******************************************************************************
21 | ; THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
22 | ; WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
23 | ; AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
24 | ; INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
25 | ; CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
26 | ; INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
27 | ;*******************************************************************************
28 |
29 | ; Amount of memory (in bytes) allocated for Stack
30 | ; Tailor this value to your application needs
31 | ; Stack Configuration
32 | ; Stack Size (in Bytes) <0x0-0xFFFFFFFF:8>
33 | ;
34 |
35 | Stack_Size EQU 0x00000400
36 |
37 | AREA STACK, NOINIT, READWRITE, ALIGN=3
38 | Stack_Mem SPACE Stack_Size
39 | __initial_sp
40 |
41 | ; Heap Configuration
42 | ; Heap Size (in Bytes) <0x0-0xFFFFFFFF:8>
43 | ;
44 |
45 | Heap_Size EQU 0x00000200
46 |
47 | AREA HEAP, NOINIT, READWRITE, ALIGN=3
48 | __heap_base
49 | Heap_Mem SPACE Heap_Size
50 | __heap_limit
51 |
52 | PRESERVE8
53 | THUMB
54 |
55 |
56 | ; Vector Table Mapped to Address 0 at Reset
57 | AREA RESET, DATA, READONLY
58 | EXPORT __Vectors
59 | EXPORT __Vectors_End
60 | EXPORT __Vectors_Size
61 |
62 | __Vectors DCD __initial_sp ; Top of Stack
63 | DCD Reset_Handler ; Reset Handler
64 | DCD NMI_Handler ; NMI Handler
65 | DCD HardFault_Handler ; Hard Fault Handler
66 | DCD MemManage_Handler ; MPU Fault Handler
67 | DCD BusFault_Handler ; Bus Fault Handler
68 | DCD UsageFault_Handler ; Usage Fault Handler
69 | DCD 0 ; Reserved
70 | DCD 0 ; Reserved
71 | DCD 0 ; Reserved
72 | DCD 0 ; Reserved
73 | DCD SVC_Handler ; SVCall Handler
74 | DCD DebugMon_Handler ; Debug Monitor Handler
75 | DCD 0 ; Reserved
76 | DCD PendSV_Handler ; PendSV Handler
77 | DCD SysTick_Handler ; SysTick Handler
78 |
79 | ; External Interrupts
80 | DCD WWDG_IRQHandler ; Window Watchdog
81 | DCD PVD_IRQHandler ; PVD through EXTI Line detect
82 | DCD TAMPER_IRQHandler ; Tamper
83 | DCD RTC_IRQHandler ; RTC
84 | DCD FLASH_IRQHandler ; Flash
85 | DCD RCC_IRQHandler ; RCC
86 | DCD EXTI0_IRQHandler ; EXTI Line 0
87 | DCD EXTI1_IRQHandler ; EXTI Line 1
88 | DCD EXTI2_IRQHandler ; EXTI Line 2
89 | DCD EXTI3_IRQHandler ; EXTI Line 3
90 | DCD EXTI4_IRQHandler ; EXTI Line 4
91 | DCD DMA1_Channel1_IRQHandler ; DMA1 Channel 1
92 | DCD DMA1_Channel2_IRQHandler ; DMA1 Channel 2
93 | DCD DMA1_Channel3_IRQHandler ; DMA1 Channel 3
94 | DCD DMA1_Channel4_IRQHandler ; DMA1 Channel 4
95 | DCD DMA1_Channel5_IRQHandler ; DMA1 Channel 5
96 | DCD DMA1_Channel6_IRQHandler ; DMA1 Channel 6
97 | DCD DMA1_Channel7_IRQHandler ; DMA1 Channel 7
98 | DCD ADC1_2_IRQHandler ; ADC1 & ADC2
99 | DCD USB_HP_CAN1_TX_IRQHandler ; USB High Priority or CAN1 TX
100 | DCD USB_LP_CAN1_RX0_IRQHandler ; USB Low Priority or CAN1 RX0
101 | DCD CAN1_RX1_IRQHandler ; CAN1 RX1
102 | DCD CAN1_SCE_IRQHandler ; CAN1 SCE
103 | DCD EXTI9_5_IRQHandler ; EXTI Line 9..5
104 | DCD TIM1_BRK_IRQHandler ; TIM1 Break
105 | DCD TIM1_UP_IRQHandler ; TIM1 Update
106 | DCD TIM1_TRG_COM_IRQHandler ; TIM1 Trigger and Commutation
107 | DCD TIM1_CC_IRQHandler ; TIM1 Capture Compare
108 | DCD TIM2_IRQHandler ; TIM2
109 | DCD TIM3_IRQHandler ; TIM3
110 | DCD TIM4_IRQHandler ; TIM4
111 | DCD I2C1_EV_IRQHandler ; I2C1 Event
112 | DCD I2C1_ER_IRQHandler ; I2C1 Error
113 | DCD I2C2_EV_IRQHandler ; I2C2 Event
114 | DCD I2C2_ER_IRQHandler ; I2C2 Error
115 | DCD SPI1_IRQHandler ; SPI1
116 | DCD SPI2_IRQHandler ; SPI2
117 | DCD USART1_IRQHandler ; USART1
118 | DCD USART2_IRQHandler ; USART2
119 | DCD USART3_IRQHandler ; USART3
120 | DCD EXTI15_10_IRQHandler ; EXTI Line 15..10
121 | DCD RTCAlarm_IRQHandler ; RTC Alarm through EXTI Line
122 | DCD USBWakeUp_IRQHandler ; USB Wakeup from suspend
123 | DCD TIM8_BRK_IRQHandler ; TIM8 Break
124 | DCD TIM8_UP_IRQHandler ; TIM8 Update
125 | DCD TIM8_TRG_COM_IRQHandler ; TIM8 Trigger and Commutation
126 | DCD TIM8_CC_IRQHandler ; TIM8 Capture Compare
127 | DCD ADC3_IRQHandler ; ADC3
128 | DCD FSMC_IRQHandler ; FSMC
129 | DCD SDIO_IRQHandler ; SDIO
130 | DCD TIM5_IRQHandler ; TIM5
131 | DCD SPI3_IRQHandler ; SPI3
132 | DCD UART4_IRQHandler ; UART4
133 | DCD UART5_IRQHandler ; UART5
134 | DCD TIM6_IRQHandler ; TIM6
135 | DCD TIM7_IRQHandler ; TIM7
136 | DCD DMA2_Channel1_IRQHandler ; DMA2 Channel1
137 | DCD DMA2_Channel2_IRQHandler ; DMA2 Channel2
138 | DCD DMA2_Channel3_IRQHandler ; DMA2 Channel3
139 | DCD DMA2_Channel4_5_IRQHandler ; DMA2 Channel4 & Channel5
140 | __Vectors_End
141 |
142 | __Vectors_Size EQU __Vectors_End - __Vectors
143 |
144 | AREA |.text|, CODE, READONLY
145 |
146 | ; Reset handler
147 | Reset_Handler PROC
148 | EXPORT Reset_Handler [WEAK]
149 | IMPORT __main
150 | IMPORT SystemInit
151 | LDR R0, =SystemInit
152 | BLX R0
153 | LDR R0, =__main
154 | BX R0
155 | ENDP
156 |
157 | ; Dummy Exception Handlers (infinite loops which can be modified)
158 |
159 | NMI_Handler PROC
160 | EXPORT NMI_Handler [WEAK]
161 | B .
162 | ENDP
163 | HardFault_Handler\
164 | PROC
165 | EXPORT HardFault_Handler [WEAK]
166 | B .
167 | ENDP
168 | MemManage_Handler\
169 | PROC
170 | EXPORT MemManage_Handler [WEAK]
171 | B .
172 | ENDP
173 | BusFault_Handler\
174 | PROC
175 | EXPORT BusFault_Handler [WEAK]
176 | B .
177 | ENDP
178 | UsageFault_Handler\
179 | PROC
180 | EXPORT UsageFault_Handler [WEAK]
181 | B .
182 | ENDP
183 | SVC_Handler PROC
184 | EXPORT SVC_Handler [WEAK]
185 | B .
186 | ENDP
187 | DebugMon_Handler\
188 | PROC
189 | EXPORT DebugMon_Handler [WEAK]
190 | B .
191 | ENDP
192 | PendSV_Handler PROC
193 | EXPORT PendSV_Handler [WEAK]
194 | B .
195 | ENDP
196 | SysTick_Handler PROC
197 | EXPORT SysTick_Handler [WEAK]
198 | B .
199 | ENDP
200 |
201 | Default_Handler PROC
202 |
203 | EXPORT WWDG_IRQHandler [WEAK]
204 | EXPORT PVD_IRQHandler [WEAK]
205 | EXPORT TAMPER_IRQHandler [WEAK]
206 | EXPORT RTC_IRQHandler [WEAK]
207 | EXPORT FLASH_IRQHandler [WEAK]
208 | EXPORT RCC_IRQHandler [WEAK]
209 | EXPORT EXTI0_IRQHandler [WEAK]
210 | EXPORT EXTI1_IRQHandler [WEAK]
211 | EXPORT EXTI2_IRQHandler [WEAK]
212 | EXPORT EXTI3_IRQHandler [WEAK]
213 | EXPORT EXTI4_IRQHandler [WEAK]
214 | EXPORT DMA1_Channel1_IRQHandler [WEAK]
215 | EXPORT DMA1_Channel2_IRQHandler [WEAK]
216 | EXPORT DMA1_Channel3_IRQHandler [WEAK]
217 | EXPORT DMA1_Channel4_IRQHandler [WEAK]
218 | EXPORT DMA1_Channel5_IRQHandler [WEAK]
219 | EXPORT DMA1_Channel6_IRQHandler [WEAK]
220 | EXPORT DMA1_Channel7_IRQHandler [WEAK]
221 | EXPORT ADC1_2_IRQHandler [WEAK]
222 | EXPORT USB_HP_CAN1_TX_IRQHandler [WEAK]
223 | EXPORT USB_LP_CAN1_RX0_IRQHandler [WEAK]
224 | EXPORT CAN1_RX1_IRQHandler [WEAK]
225 | EXPORT CAN1_SCE_IRQHandler [WEAK]
226 | EXPORT EXTI9_5_IRQHandler [WEAK]
227 | EXPORT TIM1_BRK_IRQHandler [WEAK]
228 | EXPORT TIM1_UP_IRQHandler [WEAK]
229 | EXPORT TIM1_TRG_COM_IRQHandler [WEAK]
230 | EXPORT TIM1_CC_IRQHandler [WEAK]
231 | EXPORT TIM2_IRQHandler [WEAK]
232 | EXPORT TIM3_IRQHandler [WEAK]
233 | EXPORT TIM4_IRQHandler [WEAK]
234 | EXPORT I2C1_EV_IRQHandler [WEAK]
235 | EXPORT I2C1_ER_IRQHandler [WEAK]
236 | EXPORT I2C2_EV_IRQHandler [WEAK]
237 | EXPORT I2C2_ER_IRQHandler [WEAK]
238 | EXPORT SPI1_IRQHandler [WEAK]
239 | EXPORT SPI2_IRQHandler [WEAK]
240 | EXPORT USART1_IRQHandler [WEAK]
241 | EXPORT USART2_IRQHandler [WEAK]
242 | EXPORT USART3_IRQHandler [WEAK]
243 | EXPORT EXTI15_10_IRQHandler [WEAK]
244 | EXPORT RTCAlarm_IRQHandler [WEAK]
245 | EXPORT USBWakeUp_IRQHandler [WEAK]
246 | EXPORT TIM8_BRK_IRQHandler [WEAK]
247 | EXPORT TIM8_UP_IRQHandler [WEAK]
248 | EXPORT TIM8_TRG_COM_IRQHandler [WEAK]
249 | EXPORT TIM8_CC_IRQHandler [WEAK]
250 | EXPORT ADC3_IRQHandler [WEAK]
251 | EXPORT FSMC_IRQHandler [WEAK]
252 | EXPORT SDIO_IRQHandler [WEAK]
253 | EXPORT TIM5_IRQHandler [WEAK]
254 | EXPORT SPI3_IRQHandler [WEAK]
255 | EXPORT UART4_IRQHandler [WEAK]
256 | EXPORT UART5_IRQHandler [WEAK]
257 | EXPORT TIM6_IRQHandler [WEAK]
258 | EXPORT TIM7_IRQHandler [WEAK]
259 | EXPORT DMA2_Channel1_IRQHandler [WEAK]
260 | EXPORT DMA2_Channel2_IRQHandler [WEAK]
261 | EXPORT DMA2_Channel3_IRQHandler [WEAK]
262 | EXPORT DMA2_Channel4_5_IRQHandler [WEAK]
263 |
264 | WWDG_IRQHandler
265 | PVD_IRQHandler
266 | TAMPER_IRQHandler
267 | RTC_IRQHandler
268 | FLASH_IRQHandler
269 | RCC_IRQHandler
270 | EXTI0_IRQHandler
271 | EXTI1_IRQHandler
272 | EXTI2_IRQHandler
273 | EXTI3_IRQHandler
274 | EXTI4_IRQHandler
275 | DMA1_Channel1_IRQHandler
276 | DMA1_Channel2_IRQHandler
277 | DMA1_Channel3_IRQHandler
278 | DMA1_Channel4_IRQHandler
279 | DMA1_Channel5_IRQHandler
280 | DMA1_Channel6_IRQHandler
281 | DMA1_Channel7_IRQHandler
282 | ADC1_2_IRQHandler
283 | USB_HP_CAN1_TX_IRQHandler
284 | USB_LP_CAN1_RX0_IRQHandler
285 | CAN1_RX1_IRQHandler
286 | CAN1_SCE_IRQHandler
287 | EXTI9_5_IRQHandler
288 | TIM1_BRK_IRQHandler
289 | TIM1_UP_IRQHandler
290 | TIM1_TRG_COM_IRQHandler
291 | TIM1_CC_IRQHandler
292 | TIM2_IRQHandler
293 | TIM3_IRQHandler
294 | TIM4_IRQHandler
295 | I2C1_EV_IRQHandler
296 | I2C1_ER_IRQHandler
297 | I2C2_EV_IRQHandler
298 | I2C2_ER_IRQHandler
299 | SPI1_IRQHandler
300 | SPI2_IRQHandler
301 | USART1_IRQHandler
302 | USART2_IRQHandler
303 | USART3_IRQHandler
304 | EXTI15_10_IRQHandler
305 | RTCAlarm_IRQHandler
306 | USBWakeUp_IRQHandler
307 | TIM8_BRK_IRQHandler
308 | TIM8_UP_IRQHandler
309 | TIM8_TRG_COM_IRQHandler
310 | TIM8_CC_IRQHandler
311 | ADC3_IRQHandler
312 | FSMC_IRQHandler
313 | SDIO_IRQHandler
314 | TIM5_IRQHandler
315 | SPI3_IRQHandler
316 | UART4_IRQHandler
317 | UART5_IRQHandler
318 | TIM6_IRQHandler
319 | TIM7_IRQHandler
320 | DMA2_Channel1_IRQHandler
321 | DMA2_Channel2_IRQHandler
322 | DMA2_Channel3_IRQHandler
323 | DMA2_Channel4_5_IRQHandler
324 | B .
325 |
326 | ENDP
327 |
328 | ALIGN
329 |
330 | ;*******************************************************************************
331 | ; User Stack and Heap initialization
332 | ;*******************************************************************************
333 | IF :DEF:__MICROLIB
334 |
335 | EXPORT __initial_sp
336 | EXPORT __heap_base
337 | EXPORT __heap_limit
338 |
339 | ELSE
340 |
341 | IMPORT __use_two_region_memory
342 | EXPORT __user_initial_stackheap
343 |
344 | __user_initial_stackheap
345 |
346 | LDR R0, = Heap_Mem
347 | LDR R1, =(Stack_Mem + Stack_Size)
348 | LDR R2, = (Heap_Mem + Heap_Size)
349 | LDR R3, = Stack_Mem
350 | BX LR
351 |
352 | ALIGN
353 |
354 | ENDIF
355 |
356 | END
357 |
358 | ;******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE*****
359 |
--------------------------------------------------------------------------------
/RTE/Device/STM32F103VE/stm32f10x_conf.h:
--------------------------------------------------------------------------------
1 | /**
2 | ******************************************************************************
3 | * @file Project/STM32F10x_StdPeriph_Template/stm32f10x_conf.h
4 | * @author MCD Application Team
5 | * @version V3.5.0
6 | * @date 08-April-2011
7 | * @brief Library configuration file.
8 | ******************************************************************************
9 | * @attention
10 | *
11 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
12 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
13 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
14 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
15 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
16 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
17 | *
18 | * © COPYRIGHT 2011 STMicroelectronics
19 | ******************************************************************************
20 | */
21 |
22 | /* Define to prevent recursive inclusion -------------------------------------*/
23 | #ifndef __STM32F10x_CONF_H
24 | #define __STM32F10x_CONF_H
25 |
26 | /* Run Time Environment will set specific #define for each selected module below */
27 | #include "RTE_Components.h"
28 |
29 | #ifdef RTE_DEVICE_STDPERIPH_ADC
30 | #include "stm32f10x_adc.h"
31 | #endif
32 | #ifdef RTE_DEVICE_STDPERIPH_BKP
33 | #include "stm32f10x_bkp.h"
34 | #endif
35 | #ifdef RTE_DEVICE_STDPERIPH_CAN
36 | #include "stm32f10x_can.h"
37 | #endif
38 | #ifdef RTE_DEVICE_STDPERIPH_CEC
39 | #include "stm32f10x_cec.h"
40 | #endif
41 | #ifdef RTE_DEVICE_STDPERIPH_CRC
42 | #include "stm32f10x_crc.h"
43 | #endif
44 | #ifdef RTE_DEVICE_STDPERIPH_DAC
45 | #include "stm32f10x_dac.h"
46 | #endif
47 | #ifdef RTE_DEVICE_STDPERIPH_DBGMCU
48 | #include "stm32f10x_dbgmcu.h"
49 | #endif
50 | #ifdef RTE_DEVICE_STDPERIPH_DMA
51 | #include "stm32f10x_dma.h"
52 | #endif
53 | #ifdef RTE_DEVICE_STDPERIPH_EXTI
54 | #include "stm32f10x_exti.h"
55 | #endif
56 | #ifdef RTE_DEVICE_STDPERIPH_FLASH
57 | #include "stm32f10x_flash.h"
58 | #endif
59 | #ifdef RTE_DEVICE_STDPERIPH_FSMC
60 | #include "stm32f10x_fsmc.h"
61 | #endif
62 | #ifdef RTE_DEVICE_STDPERIPH_GPIO
63 | #include "stm32f10x_gpio.h"
64 | #endif
65 | #ifdef RTE_DEVICE_STDPERIPH_I2C
66 | #include "stm32f10x_i2c.h"
67 | #endif
68 | #ifdef RTE_DEVICE_STDPERIPH_IWDG
69 | #include "stm32f10x_iwdg.h"
70 | #endif
71 | #ifdef RTE_DEVICE_STDPERIPH_PWR
72 | #include "stm32f10x_pwr.h"
73 | #endif
74 | #ifdef RTE_DEVICE_STDPERIPH_RCC
75 | #include "stm32f10x_rcc.h"
76 | #endif
77 | #ifdef RTE_DEVICE_STDPERIPH_RTC
78 | #include "stm32f10x_rtc.h"
79 | #endif
80 | #ifdef RTE_DEVICE_STDPERIPH_SDIO
81 | #include "stm32f10x_sdio.h"
82 | #endif
83 | #ifdef RTE_DEVICE_STDPERIPH_SPI
84 | #include "stm32f10x_spi.h"
85 | #endif
86 | #ifdef RTE_DEVICE_STDPERIPH_TIM
87 | #include "stm32f10x_tim.h"
88 | #endif
89 | #ifdef RTE_DEVICE_STDPERIPH_USART
90 | #include "stm32f10x_usart.h"
91 | #endif
92 | #ifdef RTE_DEVICE_STDPERIPH_WWDG
93 | #include "stm32f10x_wwdg.h"
94 | #endif
95 | #ifdef RTE_DEVICE_STDPERIPH_FRAMEWORK
96 | #include "misc.h" /* High level functions for NVIC and SysTick (add-on to CMSIS functions) */
97 | #endif
98 |
99 | /* Exported types ------------------------------------------------------------*/
100 | /* Exported constants --------------------------------------------------------*/
101 | /* Uncomment the line below to expanse the "assert_param" macro in the
102 | Standard Peripheral Library drivers code */
103 | /* #define USE_FULL_ASSERT 1 */
104 |
105 | /* Exported macro ------------------------------------------------------------*/
106 | #ifdef USE_FULL_ASSERT
107 |
108 | /**
109 | * @brief The assert_param macro is used for function's parameters check.
110 | * @param expr: If expr is false, it calls assert_failed function which reports
111 | * the name of the source file and the source line number of the call
112 | * that failed. If expr is true, it returns no value.
113 | * @retval None
114 | */
115 | #define assert_param(expr) ((expr) ? (void)0 : assert_failed((uint8_t *)__FILE__, __LINE__))
116 | /* Exported functions ------------------------------------------------------- */
117 | void assert_failed(uint8_t* file, uint32_t line);
118 | #else
119 | #define assert_param(expr) ((void)0)
120 | #endif /* USE_FULL_ASSERT */
121 |
122 | #endif /* __STM32F10x_CONF_H */
123 |
124 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/
125 |
--------------------------------------------------------------------------------
/RTE/RTE_Components.h:
--------------------------------------------------------------------------------
1 |
2 | /*
3 | * Auto generated Run-Time-Environment Component Configuration File
4 | * *** Do not modify ! ***
5 | *
6 | * Project: 'mrobot'
7 | * Target: 'mrobot'
8 | */
9 |
10 | #ifndef RTE_COMPONENTS_H
11 | #define RTE_COMPONENTS_H
12 |
13 |
14 | /*
15 | * Define the Device Header File:
16 | */
17 | #define CMSIS_device_header "stm32f10x.h"
18 |
19 | #define RTE_DEVICE_STDPERIPH_ADC
20 | #define RTE_DEVICE_STDPERIPH_DMA
21 | #define RTE_DEVICE_STDPERIPH_EXTI
22 | #define RTE_DEVICE_STDPERIPH_FRAMEWORK
23 | #define RTE_DEVICE_STDPERIPH_GPIO
24 | #define RTE_DEVICE_STDPERIPH_I2C
25 | #define RTE_DEVICE_STDPERIPH_RCC
26 | #define RTE_DEVICE_STDPERIPH_TIM
27 | #define RTE_DEVICE_STDPERIPH_USART
28 | #define RTE_DEVICE_STDPERIPH_WWDG
29 |
30 | #endif /* RTE_COMPONENTS_H */
31 |
--------------------------------------------------------------------------------
/keilkilll.bat:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/zsirui/mrobot/5f7474090a7d357fdad973d72531111f9844cb04/keilkilll.bat
--------------------------------------------------------------------------------
/mrobot.uvoptx:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 | 1.0
5 |
6 | ### uVision Project, (C) Keil Software
7 |
8 |
9 | *.c
10 | *.s*; *.src; *.a*
11 | *.obj
12 | *.lib
13 | *.txt; *.h; *.inc
14 | *.plm
15 | *.cpp
16 | 0
17 |
18 |
19 |
20 | 0
21 | 0
22 |
23 |
24 |
25 | mrobot
26 | 0x4
27 | ARM-ADS
28 |
29 | 12000000
30 |
31 | 0
32 | 1
33 | 0
34 | 1
35 | 0
36 |
37 |
38 | 1
39 | 65535
40 | 0
41 | 0
42 | 0
43 |
44 |
45 | 79
46 | 66
47 | 8
48 | .\Listings\
49 |
50 |
51 | 1
52 | 1
53 | 1
54 | 0
55 | 1
56 | 1
57 | 0
58 | 1
59 | 0
60 | 0
61 | 0
62 | 0
63 |
64 |
65 | 1
66 | 1
67 | 1
68 | 1
69 | 1
70 | 1
71 | 1
72 | 0
73 | 0
74 |
75 |
76 | 1
77 | 0
78 | 1
79 |
80 | 0
81 |
82 |
83 | 0
84 | Quick Start Guide (MCBSTM32E)
85 | d:\Keil_v5\ARM\PACK\Keil\STM32F1xx_DFP\2.2.0\Boards\Keil\MCBSTM32E\Documentation\STM32E_QSG.pdf
86 |
87 |
88 | 1
89 | Base Board Schematics (MCBSTM32E)
90 | d:\Keil_v5\ARM\PACK\Keil\STM32F1xx_DFP\2.2.0\Boards\Keil\MCBSTM32E\Documentation\mcbstm32e-base-board-schematics.pdf
91 |
92 |
93 | 2
94 | Display Board Schematics (MCBSTM32E)
95 | d:\Keil_v5\ARM\PACK\Keil\STM32F1xx_DFP\2.2.0\Boards\Keil\MCBSTM32E\Documentation\mcbstm32e-display-board-schematics.pdf
96 |
97 |
98 | 3
99 | User Manual (MCBSTM32E)
100 | d:\Keil_v5\ARM\PACK\Keil\STM32F1xx_DFP\2.2.0\Boards\Keil\MCBSTM32E\Documentation\mcbstm32e.chm
101 |
102 |
103 | 4
104 | User Manual (NUCLEO-F103RB)
105 | d:\Keil_v5\ARM\PACK\Keil\STM32NUCLEO_BSP\1.6.0\Documents\DM00105823.pdf
106 |
107 |
108 | 5
109 | Overview (NUCLEO-F103RB)
110 | d:\Keil_v5\ARM\PACK\Keil\STM32NUCLEO_BSP\1.6.0\Documents\DM00105918.pdf
111 |
112 |
113 | 6
114 | Getting started (NUCLEO-F103RB)
115 | d:\Keil_v5\ARM\PACK\Keil\STM32NUCLEO_BSP\1.6.0\Documents\DM00105928.pdf
116 |
117 |
118 | 7
119 | Schematics (NUCLEO-F103RB)
120 | d:\Keil_v5\ARM\PACK\Keil\STM32NUCLEO_BSP\1.6.0\Documents\MB1136.pdf
121 |
122 |
123 | 8
124 | MCBSTM32E Evaluation Board Web Page (MCBSTM32E)
125 | http://www.keil.com/mcbstm32e/
126 |
127 |
128 | 9
129 | STM32 Nucleo board (NUCLEO-F103RB)
130 | http://www.st.com/web/catalog/tools/FM116/SC959/SS1532/LN1847/PF259875
131 |
132 |
133 |
134 | 0
135 | 1
136 | 1
137 | 1
138 | 1
139 | 1
140 | 1
141 | 1
142 | 1
143 | 1
144 | 1
145 | 1
146 | 1
147 | 1
148 | 0
149 | 1
150 | 1
151 | 1
152 | 1
153 | 0
154 | 0
155 | 0
156 |
157 |
158 |
159 |
160 |
161 |
162 |
163 |
164 |
165 |
166 | BIN\UL2CM3.DLL
167 |
168 |
169 |
170 | 0
171 | UL2CM3
172 | UL2CM3(-S0 -C0 -P0 -FD20000000 -FC1000 -FN1 -FF0STM32F10x_512 -FS08000000 -FL080000 -FP0($$Device:STM32F103RC$Flash\STM32F10x_512.FLM))
173 |
174 |
175 |
176 |
177 | 0
178 |
179 |
180 | 0
181 | 0
182 | 0
183 | 0
184 | 0
185 | 0
186 | 0
187 | 0
188 | 0
189 | 0
190 | 0
191 | 0
192 | 0
193 | 0
194 | 0
195 | 0
196 | 0
197 | 0
198 | 0
199 | 0
200 | 0
201 | 0
202 | 0
203 | 0
204 |
205 |
206 |
207 | 0
208 |
209 |
210 | 0
211 |
212 | 1
213 | 0
214 | 2
215 | 10000000
216 |
217 |
218 |
219 |
220 |
221 | Source
222 | 0
223 | 0
224 | 0
225 | 0
226 |
227 | 1
228 | 1
229 | 1
230 | 0
231 | 0
232 | 0
233 | .\src\main.c
234 | main.c
235 | 0
236 | 0
237 |
238 |
239 |
240 |
241 | Driver
242 | 0
243 | 0
244 | 0
245 | 0
246 |
247 | 2
248 | 2
249 | 1
250 | 0
251 | 0
252 | 0
253 | .\src\driver\battery.c
254 | battery.c
255 | 0
256 | 0
257 |
258 |
259 | 2
260 | 3
261 | 1
262 | 0
263 | 0
264 | 0
265 | .\src\driver\beep.c
266 | beep.c
267 | 0
268 | 0
269 |
270 |
271 | 2
272 | 4
273 | 1
274 | 0
275 | 0
276 | 0
277 | .\src\driver\boarddrv.c
278 | boarddrv.c
279 | 0
280 | 0
281 |
282 |
283 | 2
284 | 5
285 | 1
286 | 0
287 | 0
288 | 0
289 | .\src\driver\crc.c
290 | crc.c
291 | 0
292 | 0
293 |
294 |
295 | 2
296 | 6
297 | 1
298 | 0
299 | 0
300 | 0
301 | .\src\driver\led.c
302 | led.c
303 | 0
304 | 0
305 |
306 |
307 | 2
308 | 7
309 | 1
310 | 0
311 | 0
312 | 0
313 | .\src\driver\motor.c
314 | motor.c
315 | 0
316 | 0
317 |
318 |
319 | 2
320 | 8
321 | 1
322 | 0
323 | 0
324 | 0
325 | .\src\driver\softdelay.c
326 | softdelay.c
327 | 0
328 | 0
329 |
330 |
331 | 2
332 | 9
333 | 1
334 | 0
335 | 0
336 | 0
337 | .\src\driver\usart.c
338 | usart.c
339 | 0
340 | 0
341 |
342 |
343 | 2
344 | 10
345 | 1
346 | 0
347 | 0
348 | 0
349 | .\src\driver\watchdog.c
350 | watchdog.c
351 | 0
352 | 0
353 |
354 |
355 | 2
356 | 11
357 | 1
358 | 0
359 | 0
360 | 0
361 | .\src\driver\exti.c
362 | exti.c
363 | 0
364 | 0
365 |
366 |
367 | 2
368 | 12
369 | 1
370 | 0
371 | 0
372 | 0
373 | .\src\driver\sonar.c
374 | sonar.c
375 | 0
376 | 0
377 |
378 |
379 | 2
380 | 13
381 | 1
382 | 0
383 | 0
384 | 0
385 | .\src\driver\protocol.c
386 | protocol.c
387 | 0
388 | 0
389 |
390 |
391 |
392 |
393 | MPU6050
394 | 0
395 | 0
396 | 0
397 | 0
398 |
399 | 3
400 | 14
401 | 1
402 | 0
403 | 0
404 | 0
405 | .\src\driver\mpu6050\mpu6050.c
406 | mpu6050.c
407 | 0
408 | 0
409 |
410 |
411 | 3
412 | 15
413 | 1
414 | 0
415 | 0
416 | 0
417 | .\src\driver\mpu6050\mpuiic.c
418 | mpuiic.c
419 | 0
420 | 0
421 |
422 |
423 | 3
424 | 16
425 | 1
426 | 0
427 | 0
428 | 0
429 | .\src\driver\mpu6050\eMPL\inv_mpu.c
430 | inv_mpu.c
431 | 0
432 | 0
433 |
434 |
435 | 3
436 | 17
437 | 1
438 | 0
439 | 0
440 | 0
441 | .\src\driver\mpu6050\eMPL\inv_mpu_dmp_motion_driver.c
442 | inv_mpu_dmp_motion_driver.c
443 | 0
444 | 0
445 |
446 |
447 |
448 |
449 | utils
450 | 0
451 | 0
452 | 0
453 | 0
454 |
455 | 4
456 | 18
457 | 1
458 | 0
459 | 0
460 | 0
461 | .\src\utils\filters.c
462 | filters.c
463 | 0
464 | 0
465 |
466 |
467 | 4
468 | 19
469 | 1
470 | 0
471 | 0
472 | 0
473 | .\src\utils\hwconf.c
474 | hwconf.c
475 | 0
476 | 0
477 |
478 |
479 | 4
480 | 20
481 | 1
482 | 0
483 | 0
484 | 0
485 | .\src\utils\stm32f10x_it.c
486 | stm32f10x_it.c
487 | 0
488 | 0
489 |
490 |
491 |
492 |
493 | ::CMSIS
494 | 0
495 | 0
496 | 0
497 | 1
498 |
499 |
500 |
501 | ::Device
502 | 0
503 | 0
504 | 0
505 | 1
506 |
507 |
508 |
509 |
--------------------------------------------------------------------------------
/ros_node/Serial_node.cpp:
--------------------------------------------------------------------------------
1 | #include
2 | #include // 包含ROS的头文件
3 | #include
4 | #include
5 | #include
6 | #include
7 | #include //包含boost库函数
8 | #include
9 | #include
10 | #include
11 | #include
12 | #include
13 | #include
14 |
15 | using namespace std;
16 | using namespace boost::asio; //定义一个命名空间,用于后面的读写操作
17 | ros::Time current_time, last_time;
18 | void writeSerial(const geometry_msgs::Twist& msg);
19 |
20 | io_service iosev; //节点文件
21 | serial_port sp(iosev, "/dev/mrobot");
22 | char* FILENAME;
23 | void readConfigFile(char* filename);
24 | void writeConfigFile(char* filename);
25 |
26 |
27 | #define R 105.00
28 | #define L 210.50
29 | #define fastAbs(n) n > 0 ? n : -n
30 | #define PI 3.1415926535897932f
31 |
32 | int _ID_FLAG = 0, _READ = 1;
33 | unsigned char Get_Crc8(unsigned char *ptr, unsigned short len)
34 | {
35 | unsigned char crc;
36 | unsigned char i;
37 | crc = 0;
38 | while(len--)
39 | {
40 | crc ^= *ptr++;
41 | for(i = 0; i < 8; i++)
42 | {
43 | if(crc&0x01)crc=(crc>>1)^0x8C;
44 | else crc >>= 1;
45 | }
46 | }
47 | return crc;
48 | }
49 |
50 | const string toHexString(const unsigned char* input, const int datasize)
51 | {
52 | string output;
53 | char ch[3];
54 |
55 | for(int i = 0; i < datasize; ++i)
56 | {
57 | snprintf(ch, 3, "%02x", input[i]);
58 | output += ch;
59 | }
60 | return output;
61 | }
62 |
63 | bool compareID(const unsigned char* ID, string ID_FROM_FILE)
64 | {
65 | string temp;
66 | temp = toHexString(ID, 12);
67 | if (temp == ID_FROM_FILE)
68 | {
69 | return true;
70 | } else
71 | {
72 | return false;
73 | }
74 | }
75 |
76 | double x = 0.0;
77 | double y = 0.0;
78 | double th = 0.0;
79 |
80 | double vx = 0.0;
81 | double vy = 0.0;
82 | double vth = 0.0;
83 |
84 | enum SendCommandType {
85 | SIMPLE_INFO = 23130,
86 | WITH_SONAR_INFO = 23125,
87 | WITH_SIX_AXIS_SENSOR_INFO = 23210,
88 | ALL_INFO = 23205,
89 | SPEED_INFO = 42330,
90 | BATTERY_INFO = 42325,
91 | SONAR_INFO = 42410,
92 | SIX_AXIS_SENSOR_INFO = 42405,
93 | };
94 |
95 | enum ReceiveCommandType {
96 | REQUIRE_WITH_SIMPLE_INFO = 0,
97 | REQUIRE_WITH_SONAR_INFO,
98 | REQUIRE_WITH_SIX_AXIS_SENSOR_INFO,
99 | REQUIRE_WITH_ALL_INFO,
100 | REQUIRE_WITH_SPEED_INFO,
101 | REQUIRE_WITH_BATTERY_INFO,
102 | REQUIRE_WITH_ONLY_SONAR_INFO,
103 | REQUIRE_WITH_ONLY_SIX_AXIS_SENSOR_INFO,
104 | };
105 |
106 | union sendData
107 | {
108 | int d;
109 | unsigned char data[4];
110 | }leftdata, rightdata;
111 |
112 | union checkSum
113 | {
114 | short d;
115 | unsigned char data[1];
116 | }SendCheckSum, ReceiveCheckSum;
117 |
118 | union receiveHeader
119 | {
120 | int d;
121 | unsigned char data[2];
122 | }receive_command, receive_header;
123 |
124 | union receiveEnder
125 | {
126 | int d;
127 | unsigned char data[2];
128 | }receive_ender;
129 |
130 | union sendCommand
131 | {
132 | int d;
133 | unsigned char data[2];
134 | }send_command;
135 |
136 | union odometry //里程计数据共用体
137 | {
138 | float odoemtry_float;
139 | unsigned char odometry_char[4];
140 | }vel_left, vel_right, temprature, odom_yaw, odom_pitch, odom_roll, sonar_front, sonar_back, sonar_left, sonar_right;
141 |
142 | union battery //电池数据共用体
143 | {
144 | float battery_float;
145 | unsigned char battery_char[4];
146 | }electricity, battery_percentage;
147 |
148 | unsigned char buf[200]; //定义字符串长度
149 | unsigned char _ID[12]; //存放芯片ID的临时变量
150 | string read_str;
151 | short command = -1;
152 | const unsigned char ender[2] = {0x0d, 0x0a};
153 | const unsigned char header[2] = {0x55, 0xaa};
154 |
155 | void readSerial()
156 | {
157 | int i, _length = 0;
158 | unsigned char _checkSum;
159 | read(sp, buffer(buf));
160 | ros::Time curr_time;
161 | for (i = 0; i < 2; i++)
162 | {
163 | receive_header.data[i] = buf[i];
164 | }
165 | if (receive_header.data[0] == header[0] && receive_header.data[1] == header[1])
166 | {
167 | for (i = 0; i < 2; i++)
168 | {
169 | receive_command.data[i] = buf[i + 2];
170 | }
171 | _length = buf[4];
172 | _checkSum = Get_Crc8(buf, 5 + _length);
173 | switch(receive_command.d)
174 | {
175 | case SIMPLE_INFO:
176 | command = REQUIRE_WITH_SIMPLE_INFO;
177 | if (buf[6 + _length] == ender[0] && buf[6 + _length + 1] == ender[1])
178 | {
179 | ReceiveCheckSum.data[0] = buf[5 + _length];
180 | if (_checkSum == ReceiveCheckSum.d)
181 | {
182 | for(i = 0; i < 4; i++)
183 | {
184 | vel_left.odometry_char[i] = buf[i + 5];
185 | vel_right.odometry_char[i] = buf[i + 9];
186 | electricity.battery_char[i] = buf[i + 13];
187 | battery_percentage.battery_char[i] = buf[i + 17];
188 | }
189 | vx = (vel_right.odoemtry_float + vel_left.odoemtry_float) / 2 / 1000;
190 | vth = (vel_right.odoemtry_float - vel_left.odoemtry_float) / L;
191 |
192 | curr_time = ros::Time::now();
193 |
194 | double dt = (curr_time - last_time).toSec();
195 | double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
196 | double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
197 | double delta_th = vth * dt;
198 |
199 | x += delta_x;
200 | y += delta_y;
201 | th += delta_th;
202 | last_time = curr_time;
203 | }
204 | else
205 | {
206 | ROS_INFO("Received data check sum error!");
207 | }
208 | }
209 | break;
210 | case WITH_SONAR_INFO:
211 | command = REQUIRE_WITH_SONAR_INFO;
212 | if (buf[6 + _length] == ender[0] && buf[6 + _length + 1] == ender[1])
213 | {
214 | ReceiveCheckSum.data[0] = buf[5 + _length];
215 | if (_checkSum == ReceiveCheckSum.d)
216 | {
217 | for(i = 0; i < 4; i++)
218 | {
219 | vel_left.odometry_char[i] = buf[i + 5];
220 | vel_right.odometry_char[i] = buf[i + 9];
221 | electricity.battery_char[i] = buf[i + 13];
222 | battery_percentage.battery_char[i] = buf[i + 17];
223 | sonar_front.odometry_char[i] = buf[i + 21];
224 | sonar_back.odometry_char[i] = buf[i + 25];
225 | sonar_left.odometry_char[i] = buf[i + 29];
226 | sonar_right.odometry_char[i] = buf[i + 33];
227 | }
228 | vx = (vel_right.odoemtry_float + vel_left.odoemtry_float) / 2 / 1000;
229 | vth = (vel_right.odoemtry_float - vel_left.odoemtry_float) / L;
230 |
231 | curr_time = ros::Time::now();
232 |
233 | double dt = (curr_time - last_time).toSec();
234 | double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
235 | double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
236 | double delta_th = vth * dt;
237 |
238 | x += delta_x;
239 | y += delta_y;
240 | th += delta_th;
241 | last_time = curr_time;
242 | }
243 | else
244 | {
245 | ROS_INFO("Received data check sum error!");
246 | }
247 | }
248 | break;
249 | case WITH_SIX_AXIS_SENSOR_INFO:
250 | command = REQUIRE_WITH_SIX_AXIS_SENSOR_INFO;
251 | if (buf[6 + _length] == ender[0] && buf[6 + _length + 1] == ender[1])
252 | {
253 | ReceiveCheckSum.data[0] = buf[5 + _length];
254 | if (_checkSum == ReceiveCheckSum.d)
255 | {
256 | for(i = 0; i < 4; i++)
257 | {
258 | vel_left.odometry_char[i] = buf[i + 5];
259 | vel_right.odometry_char[i] = buf[i + 9];
260 | electricity.battery_char[i] = buf[i + 13];
261 | battery_percentage.battery_char[i] = buf[i + 17];
262 | odom_pitch.odometry_char[i] = buf[i + 21];
263 | odom_roll.odometry_char[i] = buf[i + 25];
264 | odom_yaw.odometry_char[i] = buf[i + 29];
265 | temprature.odometry_char[i] = buf[i + 33];
266 | }
267 | vx = (vel_right.odoemtry_float + vel_left.odoemtry_float) / 2 / 1000;
268 | vth = (vel_right.odoemtry_float - vel_left.odoemtry_float) / L;
269 |
270 | curr_time = ros::Time::now();
271 |
272 | double dt = (curr_time - last_time).toSec();
273 | double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
274 | double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
275 | double delta_th = vth * dt;
276 |
277 | x += delta_x;
278 | y += delta_y;
279 | th += delta_th;
280 | last_time = curr_time;
281 | }
282 | else
283 | {
284 | ROS_INFO("Received data check sum error!");
285 | }
286 | }
287 | break;
288 | case ALL_INFO:
289 | command = REQUIRE_WITH_ALL_INFO;
290 | if (buf[6 + _length] == ender[0] && buf[6 + _length + 1] == ender[1])
291 | {
292 | ReceiveCheckSum.data[0] = buf[5 + _length];
293 | if (_checkSum == ReceiveCheckSum.d)
294 | {
295 | for(i = 0; i < 4; i++)
296 | {
297 | vel_left.odometry_char[i] = buf[i + 5];
298 | vel_right.odometry_char[i] = buf[i + 9];
299 | electricity.battery_char[i] = buf[i + 13];
300 | battery_percentage.battery_char[i] = buf[i + 17];
301 | odom_pitch.odometry_char[i] = buf[i + 21];
302 | odom_roll.odometry_char[i] = buf[i + 25];
303 | odom_yaw.odometry_char[i] = buf[i + 29];
304 | temprature.odometry_char[i] = buf[i + 33];
305 | sonar_front.odometry_char[i] = buf[i + 37];
306 | sonar_back.odometry_char[i] = buf[i + 41];
307 | sonar_left.odometry_char[i] = buf[i + 45];
308 | sonar_right.odometry_char[i] = buf[i + 49];
309 | }
310 | vx = (vel_right.odoemtry_float + vel_left.odoemtry_float) / 2 / 1000;
311 | vth = (vel_right.odoemtry_float - vel_left.odoemtry_float) / L;
312 |
313 | curr_time = ros::Time::now();
314 |
315 | double dt = (curr_time - last_time).toSec();
316 | double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
317 | double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
318 | double delta_th = vth * dt;
319 |
320 | x += delta_x;
321 | y += delta_y;
322 | th += delta_th;
323 | last_time = curr_time;
324 | }
325 | else
326 | {
327 | ROS_INFO("Received data check sum error!");
328 | }
329 | }
330 | break;
331 | case SPEED_INFO:
332 | command = REQUIRE_WITH_SPEED_INFO;
333 | if (buf[6 + _length] == ender[0] && buf[6 + _length + 1] == ender[1])
334 | {
335 | ReceiveCheckSum.data[0] = buf[5 + _length];
336 | if (_checkSum == ReceiveCheckSum.d)
337 | {
338 | for(i = 0; i < 4; i++)
339 | {
340 | vel_left.odometry_char[i] = buf[i + 5];
341 | vel_right.odometry_char[i] = buf[i + 9];
342 | }
343 | vx = (vel_right.odoemtry_float + vel_left.odoemtry_float) / 2 / 1000;
344 | vth = (vel_right.odoemtry_float - vel_left.odoemtry_float) / L;
345 |
346 | curr_time = ros::Time::now();
347 |
348 | double dt = (curr_time - last_time).toSec();
349 | double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
350 | double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
351 | double delta_th = vth * dt;
352 |
353 | x += delta_x;
354 | y += delta_y;
355 | th += delta_th;
356 | last_time = curr_time;
357 | }
358 | else
359 | {
360 | ROS_INFO("Received data check sum error!");
361 | }
362 | }
363 | break;
364 | case BATTERY_INFO:
365 | command = REQUIRE_WITH_BATTERY_INFO;
366 | if (buf[6 + _length] == ender[0] && buf[6 + _length + 1] == ender[1])
367 | {
368 | ReceiveCheckSum.data[0] = buf[5 + _length];
369 | if (_checkSum == ReceiveCheckSum.d)
370 | {
371 | for(i = 0; i < 4; i++)
372 | {
373 | electricity.battery_char[i] = buf[i + 5];
374 | battery_percentage.battery_char[i] = buf[i + 9];
375 | }
376 | vx = (vel_right.odoemtry_float + vel_left.odoemtry_float) / 2 / 1000;
377 | vth = (vel_right.odoemtry_float - vel_left.odoemtry_float) / L;
378 |
379 | curr_time = ros::Time::now();
380 |
381 | double dt = (curr_time - last_time).toSec();
382 | double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
383 | double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
384 | double delta_th = vth * dt;
385 |
386 | x += delta_x;
387 | y += delta_y;
388 | th += delta_th;
389 | last_time = curr_time;
390 | }
391 | else
392 | {
393 | ROS_INFO("Received data check sum error!");
394 | }
395 | }
396 | break;
397 | case SONAR_INFO:
398 | command = REQUIRE_WITH_ONLY_SONAR_INFO;
399 | if (buf[6 + _length] == ender[0] && buf[6 + _length + 1] == ender[1])
400 | {
401 | ReceiveCheckSum.data[0] = buf[5 + _length];
402 | if (_checkSum == ReceiveCheckSum.d)
403 | {
404 | for(i = 0; i < 4; i++)
405 | {
406 | sonar_front.odometry_char[i] = buf[i + 5];
407 | sonar_back.odometry_char[i] = buf[i + 9];
408 | sonar_left.odometry_char[i] = buf[i + 13];
409 | sonar_right.odometry_char[i] = buf[i + 17];
410 | }
411 | vx = (vel_right.odoemtry_float + vel_left.odoemtry_float) / 2 / 1000;
412 | vth = (vel_right.odoemtry_float - vel_left.odoemtry_float) / L;
413 |
414 | curr_time = ros::Time::now();
415 |
416 | double dt = (curr_time - last_time).toSec();
417 | double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
418 | double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
419 | double delta_th = vth * dt;
420 |
421 | x += delta_x;
422 | y += delta_y;
423 | th += delta_th;
424 | last_time = curr_time;
425 | }
426 | else
427 | {
428 | ROS_INFO("Received data check sum error!");
429 | }
430 | }
431 | break;
432 | case SIX_AXIS_SENSOR_INFO:
433 | command = REQUIRE_WITH_ONLY_SIX_AXIS_SENSOR_INFO;
434 | if (buf[6 + _length] == ender[0] && buf[6 + _length + 1] == ender[1])
435 | {
436 | ReceiveCheckSum.data[0] = buf[5 + _length];
437 | if (_checkSum == ReceiveCheckSum.d)
438 | {
439 | for(i = 0; i < 4; i++)
440 | {
441 | odom_pitch.odometry_char[i] = buf[i + 5];
442 | odom_roll.odometry_char[i] = buf[i + 9];
443 | odom_yaw.odometry_char[i] = buf[i + 13];
444 | temprature.odometry_char[i] = buf[i + 17];
445 | }
446 | vx = (vel_right.odoemtry_float + vel_left.odoemtry_float) / 2 / 1000;
447 | vth = (vel_right.odoemtry_float - vel_left.odoemtry_float) / L;
448 |
449 | curr_time = ros::Time::now();
450 |
451 | double dt = (curr_time - last_time).toSec();
452 | double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
453 | double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
454 | double delta_th = vth * dt;
455 |
456 | x += delta_x;
457 | y += delta_y;
458 | th += delta_th;
459 | last_time = curr_time;
460 | }
461 | else
462 | {
463 | ROS_INFO("Received data check sum error!");
464 | }
465 | }
466 | break;
467 | }
468 | for(i = 0; i < 12; i++)
469 | {
470 | _ID[i] = buf[8 + _length + i];
471 | }
472 | readConfigFile(FILENAME);
473 | }
474 | }
475 |
476 | void writeSerial(const geometry_msgs::Twist& msg)
477 | {
478 | double RobotV = msg.linear.x * 1000;
479 | double YawRate = msg.angular.z;
480 | unsigned char buf_[16] = {0};
481 | int i, length = 0;
482 | double r = RobotV / YawRate;
483 | if(RobotV == 0)
484 | {
485 | leftdata.d = -YawRate * R;
486 | rightdata.d = YawRate * R;
487 | } else if(YawRate == 0)
488 | {
489 | leftdata.d = RobotV;
490 | rightdata.d = RobotV;
491 | }
492 | else
493 | {
494 | leftdata.d = YawRate * (r - R);
495 | rightdata.d = YawRate * (r + R);
496 | }
497 | for(i = 0; i < 2; i++)
498 | {
499 | buf_[i] = header[i];
500 | }
501 | send_command.d = 21850;
502 | for(i = 0; i < 2; i++)
503 | {
504 | buf_[i + 2] = send_command.data[i];
505 | }
506 | length = 8;
507 | buf_[4] = length;
508 | for(i = 0; i < 4; i++)
509 | {
510 | buf_[i + 5] = rightdata.data[i];
511 | buf_[i + 9] = leftdata.data[i];
512 | }
513 | buf_[5 + length] = Get_Crc8(buf_, 5 + length);
514 | buf_[6 + length] = ender[0];
515 | buf_[6 + length + 1] = ender[1];
516 | write(sp, buffer(buf_));
517 | readSerial();
518 | }
519 |
520 | void readConfigFile(char* filename)
521 | {
522 | if (_READ)
523 | {
524 | ifstream file;
525 | file.open(filename, ios::in | ios::binary);
526 | if (file)
527 | {
528 | file >> read_str;
529 | if (compareID(_ID, read_str))
530 | {
531 | ROS_INFO("ID Compare Successful");
532 | }else
533 | {
534 | ROS_ERROR("ID Compare Failed");
535 | exit(-1);
536 | }
537 | } else
538 | {
539 | writeConfigFile(filename);
540 | }
541 | file.close();
542 | _READ = 0;
543 | }
544 | }
545 |
546 | void writeConfigFile(char* filename)
547 | {
548 | ofstream file;
549 | file.open(filename, ios::out | ios::app | ios::binary);
550 | if (file)
551 | {
552 | file << toHexString(_ID, 12);
553 | } else
554 | {
555 | ROS_INFO("Error opening Config file %s!", filename);
556 | }
557 | file.close();
558 | _READ = 1;
559 | }
560 |
561 | int main(int argc, char** argv)
562 | {
563 | FILENAME = strcat(getenv("HOME"), "/mrobot.config");
564 | try
565 | {
566 | // 设置参数
567 | sp.set_option(serial_port::baud_rate(115200));
568 | sp.set_option(serial_port::flow_control(serial_port::flow_control::none));
569 | sp.set_option(serial_port::parity(serial_port::parity::none));
570 | sp.set_option(serial_port::stop_bits(serial_port::stop_bits::one));
571 | sp.set_option(serial_port::character_size(8));
572 | ROS_INFO("Serial Port initialized successful.");
573 | ros::init(argc, argv, "serial_node"); //初始化节点
574 | ros::Time::init();
575 | current_time = ros::Time::now();
576 | last_time = ros::Time::now();
577 | ros::Rate loop_rate(50);
578 | ros::NodeHandle nh;
579 | ros::Publisher pub = nh.advertise("odom", 50); //定义发布消息的名称
580 | ros::Subscriber sub = nh.subscribe("cmd_vel", 50, &writeSerial);
581 | tf::TransformBroadcaster odom_broadcaster;
582 | ROS_INFO("ROS Node initialized successful.");
583 |
584 | while (ros::ok()) {
585 | ros::spinOnce();
586 |
587 | current_time = ros::Time::now();
588 | geometry_msgs::TransformStamped odom_trans;
589 |
590 | odom_trans.header.stamp = current_time;
591 | odom_trans.header.frame_id = "odom";
592 | odom_trans.child_frame_id = "base_footprint";
593 |
594 | geometry_msgs::Quaternion odom_quat;
595 | if (command == REQUIRE_WITH_ALL_INFO)
596 | {
597 | odom_quat = tf::createQuaternionMsgFromYaw(th);
598 | odom_trans.transform.translation.x = x;
599 | odom_trans.transform.translation.y = y;
600 | odom_trans.transform.translation.z = 0.0;
601 | odom_trans.transform.rotation = odom_quat;
602 | ROS_INFO("Left Speed: %.4f m/s.", vel_left.odoemtry_float / 1000);
603 | ROS_INFO("Right Speed: %.4f m/s.", vel_right.odoemtry_float / 1000);
604 | ROS_INFO("Battery Voltage: %.3f mV.", electricity.battery_float);
605 | ROS_INFO("Battery Electricity Percentage: %.3f%%.", battery_percentage.battery_float);
606 | ROS_INFO("Temprature: %.2f C.", temprature.odoemtry_float);
607 | ROS_INFO("Pitch: %.3f.", odom_pitch.odoemtry_float);
608 | ROS_INFO("Roll: %.3f.", odom_roll.odoemtry_float);
609 | ROS_INFO("Yaw: %.3f.", odom_yaw.odoemtry_float);
610 | ROS_INFO("Sonar front: %.3f m.", sonar_front.odoemtry_float);
611 | ROS_INFO("Sonar back: %.3f m.", sonar_back.odoemtry_float);
612 | ROS_INFO("Sonar left: %.3f m.", sonar_left.odoemtry_float);
613 | ROS_INFO("Sonar right: %.3f m.", sonar_right.odoemtry_float);
614 | } else if (command == REQUIRE_WITH_SIMPLE_INFO)
615 | {
616 | odom_quat = tf::createQuaternionMsgFromYaw(th);
617 | odom_trans.transform.translation.x = x;
618 | odom_trans.transform.translation.y = y;
619 | odom_trans.transform.translation.z = 0.0;
620 | odom_trans.transform.rotation = odom_quat;
621 | ROS_INFO("Left Speed: %.4f m/s.", vel_left.odoemtry_float / 1000);
622 | ROS_INFO("Right Speed: %.4f m/s.", vel_right.odoemtry_float / 1000);
623 | ROS_INFO("Battery Voltage: %.3f mV.", electricity.battery_float);
624 | ROS_INFO("Battery Electricity Percentage: %.3f%%.", battery_percentage.battery_float);
625 | } else if(command == REQUIRE_WITH_SIX_AXIS_SENSOR_INFO)
626 | {
627 | odom_quat = tf::createQuaternionMsgFromYaw(th);
628 | odom_trans.transform.translation.x = x;
629 | odom_trans.transform.translation.y = y;
630 | odom_trans.transform.translation.z = 0.0;
631 | odom_trans.transform.rotation = odom_quat;
632 | ROS_INFO("Left Speed: %.4f m/s.", vel_left.odoemtry_float / 1000);
633 | ROS_INFO("Right Speed: %.4f m/s.", vel_right.odoemtry_float / 1000);
634 | ROS_INFO("Battery Voltage: %.3f mV.", electricity.battery_float);
635 | ROS_INFO("Battery Electricity Percentage: %.3f%%.", battery_percentage.battery_float);
636 | ROS_INFO("Temprature: %.2f C.", temprature.odoemtry_float);
637 | ROS_INFO("Pitch: %.3f.", odom_pitch.odoemtry_float);
638 | ROS_INFO("Roll: %.3f.", odom_roll.odoemtry_float);
639 | ROS_INFO("Yaw: %.3f.", odom_yaw.odoemtry_float);
640 | } else
641 | {
642 | odom_quat = tf::createQuaternionMsgFromYaw(th);
643 | odom_trans.transform.translation.x = x;
644 | odom_trans.transform.translation.y = y;
645 | odom_trans.transform.translation.z = 0.0;
646 | odom_trans.transform.rotation = odom_quat;
647 | }
648 |
649 | //send the transform
650 | odom_broadcaster.sendTransform(odom_trans);
651 |
652 | nav_msgs::Odometry msgl;
653 | msgl.header.stamp = current_time;
654 | msgl.header.frame_id = "odom";
655 |
656 | msgl.pose.pose.position.x = x;
657 | msgl.pose.pose.position.y = y;
658 | msgl.pose.pose.position.z = 0.0;
659 | msgl.pose.pose.orientation = odom_quat;
660 |
661 | msgl.child_frame_id = "base_footprint";
662 | msgl.twist.twist.linear.x = vx;
663 | msgl.twist.twist.linear.y = vy;
664 | msgl.twist.twist.angular.z = vth;
665 |
666 | pub.publish(msgl); //发布消息
667 | command = -1;
668 | loop_rate.sleep();
669 | }
670 | iosev.run();
671 | }
672 | catch (exception& err)
673 | {
674 | ROS_ERROR("Exception Error: %s", err.what());
675 | return -1;
676 | }
677 | return 0;
678 | }
--------------------------------------------------------------------------------
/src/driver/battery.c:
--------------------------------------------------------------------------------
1 | #include "common.h"
2 | #include "battery.h"
3 |
4 | static _u32 batteryFrequency = 0;
5 | static float batteryElectricityPercentage = 0;
6 | extern _u32 voltage;
7 |
8 | int init_battery(void)
9 | {
10 | GPIO_InitTypeDef GPIO_InitStructure;
11 |
12 | RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
13 | RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);
14 |
15 | GPIO_InitStructure.GPIO_Pin = BATT_DETECT_PIN;
16 | GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;
17 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
18 | GPIO_Init(BATT_DETECT_PORT, &GPIO_InitStructure);
19 | return 1;
20 | }
21 |
22 | /*
23 | * 获取电池电压函数
24 | * 返回电压值,单位:mV
25 | */
26 | float get_electricity(void)
27 | {
28 | return (float)voltage * (3.3 / 4096) * 9090.91 - 2359.09;
29 | }
30 | /*
31 | * 获取电池容量百分比函数
32 | * 返回百分比0-100%
33 | */
34 | float get_electricitypercentage(void)
35 | {
36 | return batteryElectricityPercentage;
37 | }
38 |
39 | /*
40 | * 电池相关模块函数
41 | * 充电状态的判定等
42 | */
43 | void heartbeat_battery(void)
44 | {
45 | _u32 currentVolt;
46 | if ((getms() - batteryFrequency) >= 3000) {
47 | //3秒检测一次电池容量及计算百分比
48 | batteryFrequency = getms();
49 | currentVolt = get_electricity();
50 | if (currentVolt < BATTERY_VOLTAGE_EMPTY) {
51 | batteryElectricityPercentage = 0.0;
52 | } else if (currentVolt > BATTERY_VOLTAGE_FULL) {
53 | batteryElectricityPercentage = 100.0;
54 | } else {
55 | batteryElectricityPercentage = (float)(currentVolt - BATTERY_VOLTAGE_EMPTY) * 100 / (float)(BATTERY_VOLTAGE_FULL - BATTERY_VOLTAGE_EMPTY);
56 | }
57 | }
58 | }
59 |
--------------------------------------------------------------------------------
/src/driver/beep.c:
--------------------------------------------------------------------------------
1 | #include "beep.h"
2 |
3 | static _u32 _delay = 0;
4 | static _u32 _startMs = 0;
5 | static _u32 _beeper_ticks = 0;
6 | static _u8 _beeper_status = 0;
7 | /*
8 | * 蜂鸣器初始化函数
9 | * PWM输出,频率可调
10 | */
11 | int init_beep(void)
12 | {
13 | GPIO_InitTypeDef GPIO_InitStructure;
14 | TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
15 | TIM_OCInitTypeDef TIM_OCInitStructure;
16 |
17 | RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
18 | RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
19 | RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
20 | GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);
21 | GPIO_PinRemapConfig(GPIO_FullRemap_TIM2, ENABLE);
22 |
23 | GPIO_InitStructure.GPIO_Pin = BEEP_PIN;
24 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
25 | GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
26 | GPIO_Init(BEEP_GPIO, &GPIO_InitStructure);
27 |
28 | PIN_SET(BEEP_GPIO, BEEP_PIN, LOW);
29 |
30 | TIM_TimeBaseStructure.TIM_Period = TIME_PERIOD;
31 | TIM_TimeBaseStructure.TIM_Prescaler = CPU_FREQ / TIME_PRESCALER / BEEP_INIT_HZ;
32 | TIM_TimeBaseStructure.TIM_ClockDivision = 0;
33 | TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
34 | TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
35 |
36 | TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2;
37 | TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
38 | TIM_OCInitStructure.TIM_Pulse = 1;
39 | TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
40 | TIM_OC1Init(TIM2, &TIM_OCInitStructure);
41 |
42 | TIM_CtrlPWMOutputs(TIM2, ENABLE);
43 | TIM_OC1PreloadConfig(TIM2, TIM_OCPreload_Enable);
44 | TIM_ARRPreloadConfig(TIM2, ENABLE);
45 | TIM_Cmd(TIM2, ENABLE);
46 |
47 | return 1;
48 | }
49 | /*
50 | * 蜂鸣器使能函数
51 | */
52 | static void enable_beep(void)
53 | {
54 | GPIO_InitTypeDef GPIO_InitStructure;
55 | GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
56 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
57 |
58 | GPIO_InitStructure.GPIO_Pin = BEEP_PIN;
59 | GPIO_Init(BEEP_GPIO, &GPIO_InitStructure);
60 | }
61 | /*
62 | * 蜂鸣器禁止函数
63 | */
64 | static void disable_beep(void)
65 | {
66 | GPIO_InitTypeDef GPIO_InitStructure;
67 | GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
68 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
69 |
70 | GPIO_InitStructure.GPIO_Pin = BEEP_PIN;
71 | GPIO_Init(BEEP_GPIO, &GPIO_InitStructure);
72 | }
73 | /*
74 | * 蜂鸣器打开函数
75 | */
76 | void start_beep(void)
77 | {
78 | enable_beep();
79 | }
80 | /*
81 | * 蜂鸣器关闭函数
82 | */
83 | void stop_beep(void)
84 | {
85 | disable_beep();
86 | }
87 |
88 | static _u32 HighFrequency[] = { 0, 1046, 1175, 1318, 1397, 1568, 1760, 1976 }; //高频发声
89 | /*
90 | * 蜂鸣器PWM频率设置函数
91 | */
92 | static void set_beep_frequency(_u32 frequency)
93 | {
94 | TIM2->PSC = CPU_FREQ / TIME_PRESCALER / frequency;
95 | }
96 | /*
97 | * 蜂鸣器PWM脉宽设置函数
98 | */
99 | static void set_beep_sound(_u32 sound)
100 | {
101 | TIM2->CCR1 = sound;
102 | }
103 | /*
104 | * 蜂鸣器上电特定发声函数
105 | */
106 | void play_poweron(void)
107 | {
108 | stop_beep();
109 | start_beep();
110 | PIN_SET(BEEP_GPIO, BEEP_PIN, HIGH);
111 | _delay_ms(500);
112 | PIN_SET(BEEP_GPIO, BEEP_PIN, LOW);
113 | stop_beep();
114 | _delay_ms(1000);
115 | start_beep();
116 | PIN_SET(BEEP_GPIO, BEEP_PIN, HIGH);
117 | _delay_ms(500);
118 | PIN_SET(BEEP_GPIO, BEEP_PIN, LOW);
119 | stop_beep();
120 | }
121 |
122 | void play_poweron_frequency(void)
123 | {
124 | stop_beep();
125 | set_beep_frequency(HighFrequency[6]);
126 | start_beep();
127 | _delay_ms(500);
128 | stop_beep();
129 | _delay_ms(800);
130 | set_beep_frequency(HighFrequency[6]);
131 | start_beep();
132 | _delay_ms(500);
133 | stop_beep();
134 | }
135 |
136 | /*
137 | * 蜂鸣器任意发声函数
138 | */
139 | void beep_beeper(_u32 frequency, _u32 delay, _u8 sound)
140 | {
141 | _delay = delay;
142 | _startMs = getms();
143 | stop_beep();
144 | set_beep_sound(sound);
145 | set_beep_frequency(frequency);
146 | start_beep();
147 | }
148 | /*
149 | * 蜂鸣器延时关闭函数
150 | */
151 | void heartbeat_beep(float n)
152 | {
153 | if ((getms() - _startMs) > _delay) {
154 | stop_beep();
155 | }
156 | if (n < 15) {
157 | if (getms() - _beeper_ticks < 100) {
158 | return;
159 | }
160 | if(_beeper_status){
161 | start_beep();
162 | PIN_SET(BEEP_GPIO, BEEP_PIN, HIGH);
163 | _beeper_status = _beeper_status ? 0 : 1;
164 | _beeper_ticks = getms();
165 | }else{
166 | PIN_SET(BEEP_GPIO, BEEP_PIN, LOW);
167 | stop_beep();
168 | _beeper_status = _beeper_status ? 0 : 1;
169 | _beeper_ticks = getms();
170 | }
171 | }else if (n < 30) {
172 | if(_beeper_status){
173 | if (getms() - _beeper_ticks < 15000) {
174 | return;
175 | }
176 | start_beep();
177 | PIN_SET(BEEP_GPIO, BEEP_PIN, HIGH);
178 | _beeper_status = _beeper_status ? 0 : 1;
179 | _beeper_ticks = getms();
180 | }else{
181 | if (getms() - _beeper_ticks < 500) {
182 | return;
183 | }
184 | PIN_SET(BEEP_GPIO, BEEP_PIN, LOW);
185 | stop_beep();
186 | _beeper_status = _beeper_status ? 0 : 1;
187 | _beeper_ticks = getms();
188 | }
189 | }
190 | }
191 |
192 | void on_abort_mode(void)
193 | {
194 | while(1)
195 | {
196 | stop_beep();
197 | // set_beep_frequency(HighFrequency[6]);
198 | start_beep();
199 | PIN_SET(BEEP_GPIO, BEEP_PIN, HIGH);
200 | _delay_ms(1500);
201 | PIN_SET(BEEP_GPIO, BEEP_PIN, LOW);
202 | stop_beep();
203 | // set_beep_frequency(HighFrequency[3]);
204 | _delay_ms(100);
205 | start_beep();
206 | PIN_SET(BEEP_GPIO, BEEP_PIN, HIGH);
207 | _delay_ms(1500);
208 | PIN_SET(BEEP_GPIO, BEEP_PIN, LOW);
209 | stop_beep();
210 | }
211 | }
212 |
--------------------------------------------------------------------------------
/src/driver/boarddrv.c:
--------------------------------------------------------------------------------
1 | #include "common.h"
2 | #include "watchdog.h"
3 |
4 | static void _default_abort_proc(void);
5 | static volatile uint8_t alert_flg = 0;
6 | static abort_proc_t _abort_proc = _default_abort_proc;
7 | static volatile uint32_t _systick_val = 0;
8 | static void _default_abort_proc()
9 | {
10 | while(1);
11 | }
12 |
13 | /*
14 | * 系统定时器节拍中断
15 | * 节拍:1ms
16 | */
17 | void SysTick_Handler(void)
18 | {
19 | _systick_val ++;
20 | }
21 |
22 | void alert(void)
23 | {
24 | alert_flg = 1;
25 | }
26 |
27 | int is_alert(void)
28 | {
29 | return alert_flg;
30 | }
31 |
32 | void clear_alert(void)
33 | {
34 | alert_flg = 0;
35 | }
36 |
37 | void board_set_abort_proc( abort_proc_t proc)
38 | {
39 | _abort_proc = proc;
40 | }
41 |
42 | void board_abort_mode()
43 | {
44 | cli();
45 | _abort_proc();
46 | }
47 |
48 | /*
49 | * 获取毫秒总累计数
50 | * 单位:ms
51 | */
52 | uint32_t getms()
53 | {
54 | return _systick_val;
55 | }
56 |
57 | #define SYSTICK_uS_PER_TICK 1000L/SYSTICK_1MS_TICKS
58 | /*
59 | * 获取微秒总累计数
60 | * 单位:us
61 | */
62 | uint64_t getus()
63 | {
64 | register _u32 cached_ms, cached_tick;
65 |
66 | _u32 context = enter_critical_section();
67 |
68 | cached_ms = _systick_val;
69 | cached_tick = SysTick->VAL;
70 |
71 | if (SCB->ICSR & SCB_ICSR_PENDSTSET_Msk) {
72 | ++cached_ms;
73 | cached_tick = SysTick->VAL;
74 | }
75 |
76 | leave_critical_section(context);
77 |
78 | return (_u64)cached_ms*1000 + ((SYSTICK_1MS_TICKS-1 - cached_tick)*SYSTICK_uS_PER_TICK);
79 |
80 | }
81 |
82 | /*
83 | * 不可打断的延时函数
84 | * 延时单位:ms
85 | */
86 | void delay(uint32_t ms)
87 | {
88 | uint32_t targettime = getms() + ms;
89 |
90 | while( getms() < targettime);
91 | }
92 | /*
93 | * 可打断的延时函数
94 | * 延时单位:ms
95 | */
96 | void delay_alert(uint32_t ms)
97 | {
98 | uint32_t targettime = getms() + ms;
99 | while( getms() < targettime && (!alert_flg));
100 | clear_alert();
101 | }
102 |
103 | uint16_t get_adc(uint8_t ch)
104 | {
105 | ADC_RegularChannelConfig(ADC1, ch, 1, ADC_SampleTime_239Cycles5);
106 | ADC_SoftwareStartConvCmd(ADC1, ENABLE);
107 | while(!ADC_GetFlagStatus(ADC1, ADC_FLAG_EOC));
108 | return ADC_GetConversionValue(ADC1);
109 | }
110 |
111 | uint16_t get_adc_average(uint8_t ch, uint8_t times)
112 | {
113 | uint32_t temp_val = 0;
114 | uint8_t t;
115 | for(t = 0; t < times; t++)
116 | {
117 | temp_val += get_adc(ch);
118 | mark_watchdog();
119 | _delay_ms(5);
120 | }
121 | return temp_val / times;
122 | }
123 |
--------------------------------------------------------------------------------
/src/driver/crc.c:
--------------------------------------------------------------------------------
1 | #include "crc.h"
2 |
3 | const u8 auchCRCHi[] = {
4 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
5 | 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
6 | 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
7 | 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
8 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
9 | 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
10 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
11 | 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
12 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
13 | 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
14 | 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
15 | 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
16 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
17 | 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
18 | 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
19 | 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
20 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
21 | 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
22 | 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
23 | 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
24 | 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
25 | 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
26 | 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
27 | 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
28 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
29 | 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40
30 | };
31 |
32 | const u8 auchCRCLo[]={
33 | 0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06,
34 | 0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD,
35 | 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09,
36 | 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A,
37 | 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, 0x14, 0xD4,
38 | 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3,
39 | 0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3,
40 | 0xF2, 0x32, 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4,
41 | 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A,
42 | 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29,
43 | 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED,
44 | 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,
45 | 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60,
46 | 0x61, 0xA1, 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67,
47 | 0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F,
48 | 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68,
49 | 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, 0xBE, 0x7E,
50 | 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5,
51 | 0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71,
52 | 0x70, 0xB0, 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92,
53 | 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C,
54 | 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B,
55 | 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B,
56 | 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,
57 | 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42,
58 | 0x43, 0x83, 0x41, 0x81, 0x80, 0x40
59 | };
60 |
61 | u16 Get_Crc16(u8 *puchMsg,u16 usDataLen)
62 | {
63 | u8 uchCRCHi = 0xFF;
64 | u8 uchCRCLo = 0xFF;
65 | u32 uIndex;
66 | while(usDataLen--)
67 | {
68 | uIndex = uchCRCHi^*puchMsg++;
69 | uchCRCHi = uchCRCLo^auchCRCHi[uIndex];
70 | uchCRCLo = auchCRCLo[uIndex];
71 | }
72 | return (uchCRCHi<<8 | uchCRCLo);
73 | }
74 |
75 | u8 Get_Crc8(u8 *ptr,u16 len)
76 | {
77 | u8 crc;
78 | u8 i;
79 | crc = 0;
80 | while(len--)
81 | {
82 | crc ^= *ptr++;
83 | for(i = 0; i < 8; i++)
84 | {
85 | if(crc&0x01)crc = (crc>>1)^0x8C;
86 | else crc >>= 1;
87 | }
88 | }
89 | return crc;
90 | }
91 |
--------------------------------------------------------------------------------
/src/driver/exti.c:
--------------------------------------------------------------------------------
1 | #include "common.h"
2 | #include "exti.h"
3 |
4 | /**
5 | @brief External line interrupt call back function.
6 | */
7 | static exti_cb_t g_exti_cb[CONFIG_EXTI_MAX_NUM];
8 | static _u8 g_irq_flag[CONFIG_EXIT_IRQ_NUM];
9 |
10 | /**
11 | @brief External line 0 interrupt.
12 | @param none.
13 | @return none.
14 | */
15 | void EXTI0_IRQHandler(void)
16 | {
17 | if ((EXTI->PR & EXTI_Line0) != (uint32_t)RESET) {
18 | EXTI_ClearITPendingBit(EXTI_Line0);
19 | if (g_exti_cb[0] != NULL) {
20 | g_exti_cb[0]();
21 | }
22 | }
23 | }
24 |
25 | /**
26 | @brief External line 1 interrupt.
27 | @param none.
28 | @return none.
29 | */
30 | void EXTI1_IRQHandler(void)
31 | {
32 | if ((EXTI->PR & EXTI_Line1) != (uint32_t)RESET) {
33 | EXTI_ClearITPendingBit(EXTI_Line1);
34 | if (g_exti_cb[1] != NULL) {
35 | g_exti_cb[1]();
36 | }
37 | }
38 | }
39 |
40 | /**
41 | @brief External line 2 interrupt.
42 | @param none.
43 | @return none.
44 | */
45 | void EXTI2_IRQHandler(void)
46 | {
47 | if ((EXTI->PR & EXTI_Line2) != (uint32_t)RESET) {
48 | EXTI_ClearITPendingBit(EXTI_Line2);
49 | if (g_exti_cb[2] != NULL) {
50 | g_exti_cb[2]();
51 | }
52 | }
53 | }
54 |
55 | /**
56 | @brief External line 3 interrupt.
57 | @param none.
58 | @return none.
59 | */
60 | void EXTI3_IRQHandler(void)
61 | {
62 | if ((EXTI->PR & EXTI_Line3) != (uint32_t)RESET) {
63 | EXTI_ClearITPendingBit(EXTI_Line3);
64 | if (g_exti_cb[3] != NULL) {
65 | g_exti_cb[3]();
66 | }
67 | }
68 | }
69 |
70 | /**
71 | @brief External line 4 interrupt.
72 | @param none.
73 | @return none.
74 | */
75 | void EXTI4_IRQHandler(void)
76 | {
77 | if ((EXTI->PR & EXTI_Line4) != (uint32_t)RESET) {
78 | EXTI_ClearITPendingBit(EXTI_Line4);
79 | if (g_exti_cb[4] != NULL) {
80 | g_exti_cb[4]();
81 | }
82 | }
83 | }
84 |
85 | /**
86 | @brief External line 5~9 interrupt.
87 | @param none.
88 | @return none.
89 | */
90 | void EXTI9_5_IRQHandler(void)
91 | {
92 | _u8 i;
93 |
94 | for (i = 5; i <= 9; i++) {
95 | if ((EXTI->PR & EXTI_LINE(i)) != (uint32_t)RESET) {
96 | EXTI_ClearITPendingBit(EXTI_LINE(i));
97 | if (g_exti_cb[i] != NULL) {
98 | g_exti_cb[i]();
99 | }
100 | }
101 | }
102 | }
103 |
104 | /**
105 | @brief External line 10~15 interrupt.
106 | @param none.
107 | @return none.
108 | */
109 | void EXTI15_10_IRQHandler(void)
110 | {
111 | _u8 i;
112 |
113 | for (i = 10; i <= 15; i++) {
114 | if (i == 14)
115 | _delay_ms(10);
116 | if ((EXTI->PR & EXTI_LINE(i)) != (uint32_t)RESET) {
117 | EXTI_ClearITPendingBit(EXTI_LINE(i));
118 | if (g_exti_cb[i] != NULL) {
119 | g_exti_cb[i]();
120 | }
121 | }
122 | }
123 | }
124 |
125 | /**
126 | @brief Register external line interrupt callback.
127 | @param exti - external line interrupt number.
128 | @param type - interrupt trigger type.
129 | @param cb - call back function.
130 | @return return true if success, or false if not.
131 |
132 | This fuction will return false if the call back has already been registered.
133 | */
134 | bool exti_reg_callback(_u8 exti, EXTITrigger_TypeDef type, exti_cb_t cb)
135 | {
136 | _u8 irq;
137 | _u8 pri;
138 | EXTI_InitTypeDef exti_cfg;
139 | NVIC_InitTypeDef nvic_cfg;
140 |
141 | if (exti >= CONFIG_EXTI_MAX_NUM || cb == NULL) {
142 | return false;
143 | }
144 | /* Check if the call back has been registered already. */
145 | if (g_exti_cb[exti] != NULL) {
146 | return false;
147 | }
148 |
149 | EXTI_ClearITPendingBit(EXTI_LINE(exti));
150 |
151 | exti_cfg.EXTI_Line = EXTI_LINE(exti);
152 | exti_cfg.EXTI_Mode = EXTI_Mode_Interrupt;
153 | exti_cfg.EXTI_Trigger = type;
154 | exti_cfg.EXTI_LineCmd = ENABLE;
155 | EXTI_Init(&exti_cfg);
156 |
157 | if (exti < 5) {
158 | irq = EXTI0_IRQn + exti;
159 | pri = exti;
160 | g_irq_flag[pri] = 1;
161 | } else if (exti < 10) {
162 | irq = EXTI9_5_IRQn;
163 | pri = 5;
164 | g_irq_flag[pri] |= (1 << (exti-5));
165 | } else {
166 | irq = EXTI15_10_IRQn;
167 | pri = 6;
168 | g_irq_flag[pri] |= (1 << (exti-10));
169 | }
170 |
171 | nvic_cfg.NVIC_IRQChannel = irq;
172 | nvic_cfg.NVIC_IRQChannelPreemptionPriority = 2;
173 | nvic_cfg.NVIC_IRQChannelSubPriority = pri;
174 | nvic_cfg.NVIC_IRQChannelCmd = ENABLE;
175 | NVIC_Init(&nvic_cfg);
176 |
177 | g_exti_cb[exti] = cb;
178 | return true;
179 | }
180 |
181 | /**
182 | @brief Un-register external line interrupt callback.
183 | @param exti - external line interrupt number.
184 | @return none.
185 | */
186 | void exti_unreg_callback(_u8 exti)
187 | {
188 | _u8 irq;
189 | _u8 pri;
190 | EXTI_InitTypeDef exti_cfg;
191 | NVIC_InitTypeDef nvic_cfg;
192 |
193 | if (exti >= CONFIG_EXTI_MAX_NUM) {
194 | return ;
195 | }
196 | /* Check if the call back has been registered already. */
197 | if (g_exti_cb[exti] == NULL) {
198 | return ;
199 | }
200 |
201 | EXTI_ClearITPendingBit(EXTI_LINE(exti));
202 |
203 | exti_cfg.EXTI_Line = EXTI_LINE(exti);
204 | exti_cfg.EXTI_Mode = EXTI_Mode_Interrupt;
205 | exti_cfg.EXTI_Trigger = EXTI_Trigger_Rising_Falling;
206 | exti_cfg.EXTI_LineCmd = DISABLE;
207 | EXTI_Init(&exti_cfg);
208 |
209 | if (exti < 5) {
210 | irq = EXTI0_IRQn + exti;
211 | pri = exti;
212 | g_irq_flag[pri] = 0;
213 | } else if (exti < 10) {
214 | irq = EXTI9_5_IRQn;
215 | pri = 5;
216 | g_irq_flag[pri] &= ~(1 << (exti-5));
217 | } else {
218 | irq = EXTI15_10_IRQn;
219 | pri = 6;
220 | g_irq_flag[pri] &= ~(1 << (exti-10));
221 | }
222 |
223 | if (!g_irq_flag[pri]) {
224 | nvic_cfg.NVIC_IRQChannel = irq;
225 | nvic_cfg.NVIC_IRQChannelPreemptionPriority = 1;
226 | nvic_cfg.NVIC_IRQChannelSubPriority = pri;
227 | nvic_cfg.NVIC_IRQChannelCmd = DISABLE;
228 | NVIC_Init(&nvic_cfg);
229 | }
230 |
231 | g_exti_cb[exti] = NULL;
232 | return ;
233 | }
234 |
--------------------------------------------------------------------------------
/src/driver/led.c:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/zsirui/mrobot/5f7474090a7d357fdad973d72531111f9844cb04/src/driver/led.c
--------------------------------------------------------------------------------
/src/driver/motor.c:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/zsirui/mrobot/5f7474090a7d357fdad973d72531111f9844cb04/src/driver/motor.c
--------------------------------------------------------------------------------
/src/driver/mpu6050/eMPL/inv_mpu.c:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/zsirui/mrobot/5f7474090a7d357fdad973d72531111f9844cb04/src/driver/mpu6050/eMPL/inv_mpu.c
--------------------------------------------------------------------------------
/src/driver/mpu6050/eMPL/inv_mpu_dmp_motion_driver.c:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/zsirui/mrobot/5f7474090a7d357fdad973d72531111f9844cb04/src/driver/mpu6050/eMPL/inv_mpu_dmp_motion_driver.c
--------------------------------------------------------------------------------
/src/driver/mpu6050/mpu6050.c:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/zsirui/mrobot/5f7474090a7d357fdad973d72531111f9844cb04/src/driver/mpu6050/mpu6050.c
--------------------------------------------------------------------------------
/src/driver/mpu6050/mpuiic.c:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/zsirui/mrobot/5f7474090a7d357fdad973d72531111f9844cb04/src/driver/mpu6050/mpuiic.c
--------------------------------------------------------------------------------
/src/driver/protocol.c:
--------------------------------------------------------------------------------
1 | #include "protocol.h"
2 | #include "crc.h"
3 | #include "usart.h"
4 | #include "motor.h"
5 | #include "sonar.h"
6 | #include "battery.h"
7 | #include "mpu6050.h"
8 | #include "inv_mpu.h"
9 |
10 | enum diretion {
11 | FRONT = 0,
12 | BACK,
13 | LEFT,
14 | RIGHT,
15 | };
16 |
17 | enum SendCommandType {
18 | SIMPLE_INFO = 0,
19 | WITH_SONAR_INFO,
20 | WITH_SIX_AXIS_SENSOR_INFO,
21 | ALL_INFO,
22 | SPEED_INFO,
23 | BATTERY_INFO,
24 | SONAR_INFO,
25 | SIX_AXIS_SENSOR_INFO,
26 | };
27 |
28 | enum ReceiveCommandType {
29 | SETTING_SPEED_WITH_SIMPLE_INFO = 21930, //0x55AA
30 | SETTING_SPEED_WITH_SONAR_INFO = 21845, //0x5555
31 | SETTING_SPEED_WITH_SIX_AXIS_SENSOR_INFO = 21925, //0x55A5
32 | SETTING_SPEED_WITH_ALL_INFO = 21850, //0x555A
33 | SETTING_SPEED_WITH_SPEED_INFO = 43690, //0xAAAA
34 | SETTING_SPEED_WITH_BATTERY_INFO = 43605, //0xAA55
35 | SETTING_SPEED_WITH_ONLY_SONAR_INFO = 43685, //0xAAA5
36 | SETTING_SPEED_WITH_ONLY_SIX_AXIS_SENSOR_INFO = 43610, //0xAA5A
37 | };
38 |
39 | union checkSum
40 | {
41 | u8 d;
42 | unsigned char data[1];
43 | }SendCheckSum, ReceiveCheckSum;
44 |
45 | union receiveData //接收到的数据
46 | {
47 | _s32 d; //左右轮速度
48 | unsigned char data[4];
49 | }leftdata, rightdata; //接收的左右轮数据
50 |
51 | union receiveHeader
52 | {
53 | _u16 d;
54 | unsigned char data[2];
55 | }receive_command, receive_header;
56 |
57 | union receiveEnder
58 | {
59 | _u16 d;
60 | unsigned char data[2];
61 | }receive_ender;
62 |
63 | union sendCommand
64 | {
65 | _u16 d;
66 | unsigned char data[2];
67 | }send_command;
68 |
69 | union odometry //里程计数据共用体
70 | {
71 | float odoemtry_float;
72 | unsigned char odometry_char[4];
73 | }vel_left, vel_right, temprature, odom_yaw, odom_pitch, odom_roll, sonar_front, sonar_back, sonar_left, sonar_right;
74 |
75 | union battery //电池数据共用体
76 | {
77 | float battery_float;
78 | unsigned char battery_char[4];
79 | }electricity, battery_percentage;
80 |
81 | short command = -1;
82 | const unsigned char ender[2] = {0x0d, 0x0a};
83 | const unsigned char header[2] = {0x55, 0xaa};
84 | /////////////////////////////////////////////
85 | // 通讯协议概述
86 | // [消息头(2字节)][命令(2字节)][长度(1字节)][数据(n字节,n=长度)][校验(1字节)][消息尾(2字节)]
87 | // 消息头固定为[0x55 0xaa],消息尾固定为[0x0d 0x0a]
88 | // 发送用命令参数:
89 | // (1)0x5a 0x5a:发送速度信息和电池信息
90 | // (2)0x5a 0x55:发送速度信息,电池信息和超声波信息
91 | // (3)0x5a 0xaa:发送速度信息,电池信息和六轴传感器信息
92 | // (4)0x5a 0xa5:发送速度信息,电池信息,超声波信息和六轴传感器信息
93 | // (5)0xa5 0x5a:发送速度信息
94 | // (6)0xa5 0x55:发送电池信息
95 | // (7)0xa5 0xaa:发送超声波信息
96 | // (8)0xa5 0xa5:发送六轴传感器信息
97 | // 接收用命令参数:
98 | // (1)0x55 0xaa:请求发送速度信息和电池信息
99 | // (2)0x55 0x55:请求发送速度信息,电池信息和超声波信息
100 | // (3)0x55 0xa5:请求发送速度信息,电池信息和六轴传感器信息
101 | // (4)0x55 0x5a:请求发送速度信息,电池信息,超声波信息和六轴传感器信息
102 | // (5)0xaa 0xaa:请求发送速度信息
103 | // (6)0xaa 0x55:请求发送电池信息
104 | // (7)0xaa 0xa5:请求发送超声波信息
105 | // (8)0xaa 0x5a:请求发送六轴传感器信息
106 | /////////////////////////////////////////////
107 |
108 | unsigned char _ID[12]; //存放芯片ID的临时变量
109 |
110 | void GetChipID(void)
111 | {
112 | u32 _ID0, _ID1, _ID2;
113 | _ID0 = *(__IO u32*)(0x1FFFF7E8); //产品唯一身份标识寄存器(96位)
114 | _ID1 = *(__IO u32*)(0x1FFFF7EC);
115 | _ID2 = *(__IO u32*)(0x1FFFF7F0);
116 |
117 | _ID[0] = (u8)(_ID0 & 0x000000FF);
118 | _ID[1] = (u8)((_ID0 & 0x0000FF00)>>8);
119 | _ID[2] = (u8)((_ID0 & 0x00FF0000)>>16);
120 | _ID[3] = (u8)((_ID0 & 0xFF000000)>>24);
121 | _ID[4] = (u8)(_ID1 & 0x000000FF);
122 | _ID[5] = (u8)((_ID1 & 0x0000FF00)>>8);
123 | _ID[6] = (u8)((_ID1 & 0x00FF0000)>>16);
124 | _ID[7] = (u8)((_ID1 & 0xFF000000)>>24);
125 | _ID[8] = (u8)(_ID2 & 0x000000FF);
126 | _ID[9] = (u8)((_ID2 & 0x0000FF00)>>8);
127 | _ID[10] = (u8)((_ID2 & 0x00FF0000)>>16);
128 | _ID[11] = (u8)((_ID2 & 0xFF000000)>>24);
129 | }
130 |
131 | void SendData(short Temperature, float pitch, float roll, float yaw)
132 | {
133 | int i, length = 0;
134 | vel_left.odoemtry_float = get_walkingmotor_lspeed_mm();
135 | vel_right.odoemtry_float = get_walkingmotor_rspeed_mm();
136 | sonar_front.odoemtry_float = sonar_get(FRONT);
137 | sonar_back.odoemtry_float = sonar_get(BACK);
138 | sonar_left.odoemtry_float = sonar_get(LEFT);
139 | sonar_right.odoemtry_float = sonar_get(RIGHT);
140 | electricity.battery_float = get_electricity();
141 | battery_percentage.battery_float = get_electricitypercentage();
142 | temprature.odoemtry_float = (float)Temperature / 100;
143 | odom_pitch.odoemtry_float = pitch;
144 | odom_roll.odoemtry_float = roll;
145 | odom_yaw.odoemtry_float = yaw;
146 | for(i = 0; i < 2; i++)
147 | {
148 | send_data[i] = header[i];
149 | }
150 | switch(command)
151 | {
152 | case SIMPLE_INFO:
153 | send_command.d = 23130;
154 | for(i = 0; i < 2; i++)
155 | {
156 | send_data[i + 2] = send_command.data[i];
157 | }
158 | length = 16;
159 | send_data[4] = length;
160 | for(i = 0; i < 4; i++)
161 | {
162 | send_data[i + 5] = vel_left.odometry_char[i];
163 | send_data[i + 9] = vel_right.odometry_char[i];
164 | send_data[i + 13] = electricity.battery_char[i];
165 | send_data[i + 17] = battery_percentage.battery_char[i];
166 | }
167 | break;
168 | case WITH_SONAR_INFO:
169 | send_command.d = 23125;
170 | for(i = 0; i < 2; i++)
171 | {
172 | send_data[i + 2] = send_command.data[i];
173 | }
174 | length = 32;
175 | send_data[4] = length;
176 | for(i = 0; i < 4; i++)
177 | {
178 | send_data[i + 5] = vel_left.odometry_char[i];
179 | send_data[i + 9] = vel_right.odometry_char[i];
180 | send_data[i + 13] = electricity.battery_char[i];
181 | send_data[i + 17] = battery_percentage.battery_char[i];
182 | send_data[i + 21] = sonar_front.odometry_char[i];
183 | send_data[i + 25] = sonar_back.odometry_char[i];
184 | send_data[i + 29] = sonar_left.odometry_char[i];
185 | send_data[i + 33] = sonar_right.odometry_char[i];
186 | }
187 | break;
188 | case WITH_SIX_AXIS_SENSOR_INFO:
189 | send_command.d = 23210;
190 | for(i = 0; i < 2; i++)
191 | {
192 | send_data[i + 2] = send_command.data[i];
193 | }
194 | length = 32;
195 | send_data[4] = length;
196 | for(i = 0; i < 4; i++)
197 | {
198 | send_data[i + 5] = vel_left.odometry_char[i];
199 | send_data[i + 9] = vel_right.odometry_char[i];
200 | send_data[i + 13] = electricity.battery_char[i];
201 | send_data[i + 17] = battery_percentage.battery_char[i];
202 | send_data[i + 21] = odom_pitch.odometry_char[i];
203 | send_data[i + 25] = odom_roll.odometry_char[i];
204 | send_data[i + 29] = odom_yaw.odometry_char[i];
205 | send_data[i + 33] = temprature.odometry_char[i];
206 | }
207 | break;
208 | case ALL_INFO:
209 | send_command.d = 23205;
210 | for(i = 0; i < 2; i++)
211 | {
212 | send_data[i + 2] = send_command.data[i];
213 | }
214 | length = 48;
215 | send_data[4] = length;
216 | for(i = 0; i < 4; i++)
217 | {
218 | send_data[i + 5] = vel_left.odometry_char[i];
219 | send_data[i + 9] = vel_right.odometry_char[i];
220 | send_data[i + 13] = electricity.battery_char[i];
221 | send_data[i + 17] = battery_percentage.battery_char[i];
222 | send_data[i + 21] = odom_pitch.odometry_char[i];
223 | send_data[i + 25] = odom_roll.odometry_char[i];
224 | send_data[i + 29] = odom_yaw.odometry_char[i];
225 | send_data[i + 33] = temprature.odometry_char[i];
226 | send_data[i + 37] = sonar_front.odometry_char[i];
227 | send_data[i + 41] = sonar_back.odometry_char[i];
228 | send_data[i + 45] = sonar_left.odometry_char[i];
229 | send_data[i + 49] = sonar_right.odometry_char[i];
230 | }
231 | break;
232 | case SPEED_INFO:
233 | send_command.d = 42330;
234 | for(i = 0; i < 2; i++)
235 | {
236 | send_data[i + 2] = send_command.data[i];
237 | }
238 | length = 8;
239 | send_data[4] = length;
240 | for(i = 0; i < 4; i++)
241 | {
242 | send_data[i + 5] = vel_left.odometry_char[i];
243 | send_data[i + 9] = vel_right.odometry_char[i];
244 | }
245 | break;
246 | case BATTERY_INFO:
247 | send_command.d = 42325;
248 | for(i = 0; i < 2; i++)
249 | {
250 | send_data[i + 2] = send_command.data[i];
251 | }
252 | length = 8;
253 | send_data[4] = length;
254 | for(i = 0; i < 4; i++)
255 | {
256 | send_data[i + 5] = electricity.battery_char[i];
257 | send_data[i + 9] = battery_percentage.battery_char[i];
258 | }
259 | break;
260 | case SONAR_INFO:
261 | send_command.d = 42410;
262 | for(i = 0; i < 2; i++)
263 | {
264 | send_data[i + 2] = send_command.data[i];
265 | }
266 | length = 16;
267 | send_data[4] = length;
268 | for(i = 0; i < 4; i++)
269 | {
270 | send_data[i + 5] = sonar_front.odometry_char[i];
271 | send_data[i + 9] = sonar_back.odometry_char[i];
272 | send_data[i + 13] = sonar_left.odometry_char[i];
273 | send_data[i + 17] = sonar_right.odometry_char[i];
274 | }
275 | break;
276 | case SIX_AXIS_SENSOR_INFO:
277 | send_command.d = 42405;
278 | for(i = 0; i < 2; i++)
279 | {
280 | send_data[i + 2] = send_command.data[i];
281 | }
282 | length = 16;
283 | send_data[4] = length;
284 | for(i = 0; i < 4; i++)
285 | {
286 | send_data[i + 5] = odom_pitch.odometry_char[i];
287 | send_data[i + 9] = odom_roll.odometry_char[i];
288 | send_data[i + 13] = odom_yaw.odometry_char[i];
289 | send_data[i + 17] = temprature.odometry_char[i];
290 | }
291 | break;
292 | }
293 | send_data[5 + length] = Get_Crc8(send_data, 5 + length);
294 | send_data[6 + length] = ender[0];
295 | send_data[6 + length + 1] = ender[1];
296 | GetChipID();
297 | for(i = 0; i < 12; i++)
298 | {
299 | send_data[8 + length + i] = _ID[i];
300 | }
301 | #ifdef _DMA_USART
302 | USART_DMACmd(USART3, USART_DMAReq_Tx, ENABLE);
303 | DMA_Enable(DMA1_Channel2);
304 | while(1)
305 | {
306 | if(DMA_GetFlagStatus(DMA1_FLAG_TC2) != RESET)
307 | {
308 | DMA_ClearFlag(DMA1_FLAG_TC2);
309 | break;
310 | }
311 | }
312 | #else
313 | for(i = 0; i < 200; i++)
314 | {
315 | USART_SendData(USART3, send_data[i]); //发送一个字节到串口
316 | while(USART_GetFlagStatus(USART3, USART_FLAG_TC) != SET); //等待发送结束
317 | }
318 | #endif
319 | }
320 |
321 | void ReceiveData(short Temperature, float pitch, float roll, float yaw)
322 | {
323 | int i, length, _checkSum;
324 | _s32 lspeed = 0, rspeed = 0;
325 | if(USART3_RX_STA)
326 | {
327 | for (i = 0; i < 2; i++)
328 | {
329 | receive_header.data[i] = USART3_RX_BUF[i];
330 | }
331 | if (receive_header.data[0] == header[0] && receive_header.data[1] == header[1])
332 | {
333 | for (i = 0; i < 2; i++)
334 | {
335 | receive_command.data[i] = USART3_RX_BUF[i + 2];
336 | }
337 | length = USART3_RX_BUF[4];
338 | _checkSum = Get_Crc8(USART3_RX_BUF, 5 + length);
339 | switch(receive_command.d)
340 | {
341 | case SETTING_SPEED_WITH_SIMPLE_INFO:
342 | if (USART3_RX_BUF[6 + length] == ender[0] && USART3_RX_BUF[6 + length + 1] == ender[1])
343 | {
344 | ReceiveCheckSum.data[0] = USART3_RX_BUF[5 + length];
345 | if (_checkSum == ReceiveCheckSum.d)
346 | {
347 | for(i = 0; i < 4; i++)
348 | {
349 | rightdata.data[i] = USART3_RX_BUF[i + 5];
350 | leftdata.data[i] = USART3_RX_BUF[i + 9];
351 | }
352 | rspeed = rightdata.d;
353 | lspeed = leftdata.d;
354 | command = SIMPLE_INFO;
355 | }
356 | }
357 | USART3_RX_STA = 0; //清楚接收标志位
358 | Res3 = 0;
359 | set_walkingmotor_speed(lspeed, rspeed);
360 | SendData(Temperature, pitch, roll, yaw);
361 | break;
362 | case SETTING_SPEED_WITH_SONAR_INFO:
363 | if (USART3_RX_BUF[6 + length] == ender[0] && USART3_RX_BUF[6 + length + 1] == ender[1])
364 | {
365 | ReceiveCheckSum.data[0] = USART3_RX_BUF[5 + length];
366 | if (_checkSum == ReceiveCheckSum.d)
367 | {
368 | for(i = 0; i < 4; i++)
369 | {
370 | rightdata.data[i] = USART3_RX_BUF[i + 5];
371 | leftdata.data[i] = USART3_RX_BUF[i + 9];
372 | }
373 | rspeed = rightdata.d;
374 | lspeed = leftdata.d;
375 | command = WITH_SONAR_INFO;
376 | }
377 | }
378 | USART3_RX_STA = 0; //清楚接收标志位
379 | Res3 = 0;
380 | set_walkingmotor_speed(lspeed, rspeed);
381 | SendData(Temperature, pitch, roll, yaw);
382 | break;
383 | case SETTING_SPEED_WITH_SIX_AXIS_SENSOR_INFO:
384 | if (USART3_RX_BUF[6 + length] == ender[0] && USART3_RX_BUF[6 + length + 1] == ender[1])
385 | {
386 | ReceiveCheckSum.data[0] = USART3_RX_BUF[5 + length];
387 | if (_checkSum == ReceiveCheckSum.d)
388 | {
389 | for(i = 0; i < 4; i++)
390 | {
391 | rightdata.data[i] = USART3_RX_BUF[i + 5];
392 | leftdata.data[i] = USART3_RX_BUF[i + 9];
393 | }
394 | rspeed = rightdata.d;
395 | lspeed = leftdata.d;
396 | command = WITH_SIX_AXIS_SENSOR_INFO;
397 | }
398 | }
399 | USART3_RX_STA = 0; //清楚接收标志位
400 | Res3 = 0;
401 | set_walkingmotor_speed(lspeed, rspeed);
402 | SendData(Temperature, pitch, roll, yaw);
403 | break;
404 | case SETTING_SPEED_WITH_ALL_INFO:
405 | if (USART3_RX_BUF[6 + length] == ender[0] && USART3_RX_BUF[6 + length + 1] == ender[1])
406 | {
407 | ReceiveCheckSum.data[0] = USART3_RX_BUF[5 + length];
408 | if (_checkSum == ReceiveCheckSum.d)
409 | {
410 | for(i = 0; i < 4; i++)
411 | {
412 | rightdata.data[i] = USART3_RX_BUF[i + 5];
413 | leftdata.data[i] = USART3_RX_BUF[i + 9];
414 | }
415 | rspeed = rightdata.d;
416 | lspeed = leftdata.d;
417 | command = ALL_INFO;
418 | }
419 | }
420 | USART3_RX_STA = 0; //清楚接收标志位
421 | Res3 = 0;
422 | set_walkingmotor_speed(lspeed, rspeed);
423 | SendData(Temperature, pitch, roll, yaw);
424 | break;
425 | case SETTING_SPEED_WITH_SPEED_INFO:
426 | if (USART3_RX_BUF[6 + length] == ender[0] && USART3_RX_BUF[6 + length + 1] == ender[1])
427 | {
428 | ReceiveCheckSum.data[0] = USART3_RX_BUF[5 + length];
429 | if (_checkSum == ReceiveCheckSum.d)
430 | {
431 | for(i = 0; i < 4; i++)
432 | {
433 | rightdata.data[i] = USART3_RX_BUF[i + 5];
434 | leftdata.data[i] = USART3_RX_BUF[i + 9];
435 | }
436 | rspeed = rightdata.d;
437 | lspeed = leftdata.d;
438 | command = SPEED_INFO;
439 | }
440 | }
441 | USART3_RX_STA = 0; //清楚接收标志位
442 | Res3 = 0;
443 | set_walkingmotor_speed(lspeed, rspeed);
444 | SendData(Temperature, pitch, roll, yaw);
445 | break;
446 | case SETTING_SPEED_WITH_BATTERY_INFO:
447 | if (USART3_RX_BUF[6 + length] == ender[0] && USART3_RX_BUF[6 + length + 1] == ender[1])
448 | {
449 | ReceiveCheckSum.data[0] = USART3_RX_BUF[5 + length];
450 | if (_checkSum == ReceiveCheckSum.d)
451 | {
452 | for(i = 0; i < 4; i++)
453 | {
454 | rightdata.data[i] = USART3_RX_BUF[i + 5];
455 | leftdata.data[i] = USART3_RX_BUF[i + 9];
456 | }
457 | rspeed = rightdata.d;
458 | lspeed = leftdata.d;
459 | command = BATTERY_INFO;
460 | }
461 | }
462 | USART3_RX_STA = 0; //清楚接收标志位
463 | Res3 = 0;
464 | set_walkingmotor_speed(lspeed, rspeed);
465 | SendData(Temperature, pitch, roll, yaw);
466 | break;
467 | case SETTING_SPEED_WITH_ONLY_SONAR_INFO:
468 | if (USART3_RX_BUF[6 + length] == ender[0] && USART3_RX_BUF[6 + length + 1] == ender[1])
469 | {
470 | ReceiveCheckSum.data[0] = USART3_RX_BUF[5 + length];
471 | if (_checkSum == ReceiveCheckSum.d)
472 | {
473 | for(i = 0; i < 4; i++)
474 | {
475 | rightdata.data[i] = USART3_RX_BUF[i + 5];
476 | leftdata.data[i] = USART3_RX_BUF[i + 9];
477 | }
478 | rspeed = rightdata.d;
479 | lspeed = leftdata.d;
480 | command = SONAR_INFO;
481 | }
482 | }
483 | USART3_RX_STA = 0; //清楚接收标志位
484 | Res3 = 0;
485 | set_walkingmotor_speed(lspeed, rspeed);
486 | SendData(Temperature, pitch, roll, yaw);
487 | break;
488 | case SETTING_SPEED_WITH_ONLY_SIX_AXIS_SENSOR_INFO:
489 | if (USART3_RX_BUF[6 + length] == ender[0] && USART3_RX_BUF[6 + length + 1] == ender[1])
490 | {
491 | ReceiveCheckSum.data[0] = USART3_RX_BUF[5 + length];
492 | if (_checkSum == ReceiveCheckSum.d)
493 | {
494 | for(i = 0; i < 4; i++)
495 | {
496 | rightdata.data[i] = USART3_RX_BUF[i + 5];
497 | leftdata.data[i] = USART3_RX_BUF[i + 9];
498 | }
499 | rspeed = rightdata.d;
500 | lspeed = leftdata.d;
501 | command = SIX_AXIS_SENSOR_INFO;
502 | }
503 | }
504 | USART3_RX_STA = 0; //清楚接收标志位
505 | Res3 = 0;
506 | set_walkingmotor_speed(lspeed, rspeed);
507 | SendData(Temperature, pitch, roll, yaw);
508 | break;
509 | default:
510 | USART3_RX_STA = 0; //清楚接收标志位
511 | Res3 = 0;
512 | break;
513 | }
514 | }
515 | }
516 | }
517 |
--------------------------------------------------------------------------------
/src/driver/softdelay.c:
--------------------------------------------------------------------------------
1 | #include "common.h"
2 |
3 | _u32 _ticks_per_us = 8; //true for 72Mhz with 2cycles' flash delay
4 | #define CALIBRATION_TICKS 500000UL
5 |
6 | static void _delay_loop(volatile _u32 count)
7 | {
8 | while(count--);
9 | }
10 | /*
11 | * 微秒级延时矫正函数
12 | */
13 | void softdelay_calibrate()
14 | {
15 | _u64 usedTime;
16 | _u64 startUs = getus();
17 | _delay_loop(CALIBRATION_TICKS);
18 | usedTime = getus() - startUs;
19 |
20 | if (!usedTime) usedTime = 1;
21 |
22 | _ticks_per_us = CALIBRATION_TICKS/usedTime;
23 | if (!_ticks_per_us) _ticks_per_us = 1;
24 | }
25 | /*
26 | * 微秒级延时函数
27 | */
28 | void _delay_us(volatile uint32_t us)
29 | {
30 | _delay_loop(us * _ticks_per_us);
31 | }
32 |
--------------------------------------------------------------------------------
/src/driver/sonar.c:
--------------------------------------------------------------------------------
1 | #include "common.h"
2 | #include "filters.h"
3 | #include "exti.h"
4 | #include "led.h"
5 | #include "sonar.h"
6 | //#include
7 |
8 | /**
9 | @defgroup sonar ultrasonic sonar module.
10 | @addtogroup drivers
11 | @{
12 |
13 | Resource consumption: GPIOE 5, 7 ~ 12, 15.
14 | EXTI line 5,7,8,9 with interrupt.
15 | TIM6 as counter timer.
16 | */
17 | #define SONAR_TIMER TIM6
18 |
19 | static uint8_t g_sonar_ch = 0; /**< Active sonar channel index. */
20 |
21 | /**
22 | @brief Global sonar channel descriptors.
23 | */
24 | static sonar_channel_t g_sonar[CONFIG_SONAR_CHANNEL_NUM];
25 |
26 | /**
27 | @brief Global sonar channel configurations.
28 | */
29 | static const sonar_cfg_t g_sonar_cfg[] = {
30 | {SONAR_TRIG1_PORT, SONAR_TRIG1_PIN, SONAR_ECHO1_PORT, SONAR_ECHO1_PIN, 5},
31 | {SONAR_TRIG2_PORT, SONAR_TRIG2_PIN, SONAR_ECHO2_PORT, SONAR_ECHO2_PIN, 7},
32 | {SONAR_TRIG3_PORT, SONAR_TRIG3_PIN, SONAR_ECHO3_PORT, SONAR_ECHO3_PIN, 8},
33 | {SONAR_TRIG4_PORT, SONAR_TRIG4_PIN, SONAR_ECHO4_PORT, SONAR_ECHO4_PIN, 9},
34 | };
35 |
36 | /**
37 | @brief Sonar echo read.
38 | @param ch - sonar channel index, 0 ~ CONFIG_SONAR_CHANNEL_NUM.
39 | @return none.
40 | */
41 | static inline uint8_t SONAR_ECHO(uint8_t ch)
42 | {
43 | return GPIO_ReadInputDataBit(g_sonar_cfg[ch].echo_port, g_sonar_cfg[ch].echo_pin);
44 | }
45 |
46 | /**
47 | @brief Sonar trigger function.
48 | @param ch - sonar channel index, 1 ~ CONFIG_SONAR_CHANNEL_NUM-1.
49 | @param level - sonar trigger level.
50 | @return none.
51 | */
52 | static inline void SONAR_TRIG(uint8_t ch, uint8_t level)
53 | {
54 | if (level == HIGH) {
55 | GPIO_SetBits(g_sonar_cfg[ch].trig_port, g_sonar_cfg[ch].trig_pin);
56 | } else {
57 | GPIO_ResetBits(g_sonar_cfg[ch].trig_port, g_sonar_cfg[ch].trig_pin);
58 | }
59 | }
60 |
61 | static void sonar_exti_cb(void)
62 | {
63 | uint8_t id;
64 | uint8_t status;
65 | TIM_TimeBaseInitTypeDef tim_base;
66 |
67 | status = SONAR_ECHO(g_sonar_ch);
68 | if (status == HIGH) {
69 | RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE);
70 | /* Start counter timer. */
71 | tim_base.TIM_Period = 60000;
72 | tim_base.TIM_Prescaler = (SYSTICK_1MS_TICKS/1000-1);
73 | tim_base.TIM_ClockDivision = TIM_CKD_DIV1;
74 | tim_base.TIM_CounterMode = TIM_CounterMode_Up;
75 |
76 | TIM_TimeBaseInit(SONAR_TIMER, &tim_base);
77 | TIM_SetCounter(SONAR_TIMER, 0);
78 | TIM_Cmd(SONAR_TIMER, ENABLE);
79 | } else {
80 | /* Stop counter timer. */
81 | TIM_Cmd(SONAR_TIMER, DISABLE);
82 |
83 | id = g_sonar[g_sonar_ch].id;
84 | if (id >= CONFIG_SONAR_SAMPLE_SIZE) {
85 | id = 0;
86 | }
87 | g_sonar[g_sonar_ch].sample[id++] = TIM_GetCounter(SONAR_TIMER);
88 | g_sonar[g_sonar_ch].state++; /* Move to next state. */
89 | g_sonar[g_sonar_ch].id = id;
90 | if (g_sonar[g_sonar_ch].cnt < CONFIG_SONAR_SAMPLE_SIZE) {
91 | g_sonar[g_sonar_ch].cnt++;
92 | }
93 | }
94 | }
95 |
96 | /**
97 | @brief Trigger a sonar channel.
98 | @param ch - channel number to be triggered, 1 ~ CONFIG_SONAR_CHANNEL_NUM.
99 | @return none.
100 |
101 | */
102 | static void sonar_trigger(uint8_t ch)
103 | {
104 | if (ch >= _countof(g_sonar_cfg)) {
105 | return ;
106 | }
107 |
108 | /* Send trigger wave. */
109 | SONAR_TRIG(ch, HIGH);
110 | _delay_us(20);
111 | SONAR_TRIG(ch, LOW);
112 |
113 | /* Prepare for echo rising edge interrupt. */
114 | GPIO_EXTILineConfig(GPIO_PortSourceGPIOE, g_sonar_cfg[ch].exti_line);
115 | exti_reg_callback(g_sonar_cfg[ch].exti_line, EXTI_Trigger_Rising_Falling, sonar_exti_cb);
116 | return ;
117 | }
118 |
119 | /**
120 | @brief Shutdown a sonar channel.
121 | @param ch - channel number to be shutdown, 0 ~ CONFIG_SONAR_CHANNEL_NUM.
122 | @return none.
123 |
124 | */
125 | static void sonar_shutdown(uint8_t ch)
126 | {
127 | if (ch >= _countof(g_sonar_cfg)) {
128 | return ;
129 | }
130 |
131 | SONAR_TRIG(ch, LOW);
132 | exti_unreg_callback(g_sonar_cfg[ch].exti_line);
133 | return ;
134 | }
135 |
136 | /**
137 | @brief Calculate sonar distance by sample value.
138 | @param ch - sonar channel, 0 ~ CONFIG_SONAR_CHANNEL_NUM
139 | @return none.
140 |
141 | This distance is in mm.
142 | */
143 | float sonar_distance(uint8_t ch)
144 | {
145 | uint8_t i;
146 | uint32_t avg;
147 | float d;
148 |
149 | if (ch >= _countof(g_sonar_cfg)) {
150 | return -1;
151 | }
152 |
153 | avg = 0;
154 | for (i = 0; i < g_sonar[ch].cnt; i++) {
155 | avg += g_sonar[ch].sample[i];
156 | }
157 | if (i > 0) {
158 | avg /= i;
159 | } else {
160 | avg = 0;
161 | }
162 |
163 | d = CONFIG_SONAR_COE_A + CONFIG_SONAR_COE_B * (float)(Temperature / 100);
164 | d *= avg / 2 / 1000.0;
165 | g_sonar[ch].distance = (float)d;
166 | return d;
167 | }
168 |
169 | /**
170 | @brief ultrasonic sonar module heartbeat.
171 | @param None.
172 | @return None.
173 | ___ ___
174 | TRIG ___| |_______________________________________________________| |____
175 | >10us _ _ _ _ _ _ _ _ at least 10ms wait
176 | SENSOR _________| |_| |_| |_| |_| |_| |_| |_| |________________________________
177 | Send 8 40KHz wave
178 | ________
179 | ECHO __________________________________________| |____________________
180 | 10us ~ 18ms
181 | */
182 | void sonar_heartbeat(void)
183 | {
184 | if (g_sonar_ch >= _countof(g_sonar_cfg)) {
185 | g_sonar_ch = 0;
186 | g_sonar[g_sonar_ch].state = SONAR_INIT;
187 | }
188 |
189 | switch (g_sonar[g_sonar_ch].state) {
190 | case SONAR_INIT:
191 | sonar_trigger(g_sonar_ch);
192 | g_sonar[g_sonar_ch].state++; /* Move to next state. */
193 | g_sonar[g_sonar_ch].ticks = getms();
194 | break;
195 | case SONAR_MEASURE: /* Wait until measurement done. */
196 | if (getms() - g_sonar[g_sonar_ch].ticks > CONFIG_SONAR_TIMEOUT_MS) {
197 | /* Timeout. Abort measurement and move to next channel. */
198 | // _SONAR = -1;
199 | TIM_Cmd(SONAR_TIMER, DISABLE);
200 | sonar_shutdown(g_sonar_ch);
201 | g_sonar[g_sonar_ch].ticks = getms();
202 | g_sonar[g_sonar_ch].state = SONAR_IDLE;
203 | memset(g_sonar[g_sonar_ch].sample, 0, sizeof(g_sonar[g_sonar_ch].sample));
204 | g_sonar_ch++;
205 | if (g_sonar_ch >= _countof(g_sonar_cfg)) {
206 | g_sonar_ch = 0;
207 | }
208 | // drv_led_set(0, 0, 0);
209 | }
210 | break;
211 | case SONAR_DONE: /* Measurement is done. */
212 | if (g_sonar[g_sonar_ch].cnt == 0) {
213 | break;
214 | }
215 | if (g_sonar[g_sonar_ch].cnt >= CONFIG_SONAR_SAMPLE_SIZE) {
216 | g_sonar[g_sonar_ch].cnt = CONFIG_SONAR_SAMPLE_SIZE;
217 | }
218 | // printf("sonar distance: %.4f", sonar_distance(g_sonar_ch));
219 | g_sonar[g_sonar_ch].state++;
220 | /* Blink the led by channel. */
221 | // drv_led_set((g_sonar_ch%3)==0?1:0, (g_sonar_ch%3)==1?1:0, (g_sonar_ch%3)==2?1:0);
222 | // sonar_dbg("ch %d, distance %d\r\n", g_sonar_ch, g_sonar[g_sonar_ch].distance);
223 | break;
224 | case SONAR_EXIT: /* Channel measurement is done. Move to next. */
225 | sonar_shutdown(g_sonar_ch);
226 | g_sonar[g_sonar_ch].ticks = getms();
227 | g_sonar[g_sonar_ch].state = SONAR_IDLE;
228 | g_sonar_ch++;
229 | if (g_sonar_ch >= _countof(g_sonar_cfg)) {
230 | g_sonar_ch = 0;
231 | }
232 | break;
233 | default:
234 | if (getms() - g_sonar[g_sonar_ch].ticks < CONFIG_SONAR_TICKS) {
235 | break;
236 | }
237 | g_sonar[g_sonar_ch].state = SONAR_INIT;
238 | break;
239 | }
240 |
241 | return ;
242 | }
243 |
244 | /**
245 | @brief Get Sonar channel distance measurement value.
246 | @param ch - Sonar channel number, 1 ~ CONFIG_SONAR_CHANNEL_NUM.
247 | @return return disatance measured in mm.
248 | */
249 | uint32_t sonar_get(uint8_t ch)
250 | {
251 | if (ch >= _countof(g_sonar_cfg)) {
252 | return 0;
253 | }
254 |
255 | sonar_distance(ch);
256 | return g_sonar[ch].distance;
257 | }
258 |
259 | /**
260 | @brief Initialize ultrasonic sonar module.
261 | @param None.
262 | @return None.
263 | */
264 | int sonar_init(void)
265 | {
266 | uint8_t ch;
267 |
268 | /* These pins are pull up by default. So pull down them. */
269 | for (ch = 0; ch < _countof(g_sonar_cfg); ch++) {
270 | pinMode(g_sonar_cfg[ch].echo_port, g_sonar_cfg[ch].echo_pin,
271 | GPIO_Mode_IPD, GPIO_Speed_10MHz);
272 | pinMode(g_sonar_cfg[ch].trig_port, g_sonar_cfg[ch].trig_pin,
273 | GPIO_Mode_Out_PP, GPIO_Speed_50MHz);
274 | SONAR_TRIG(ch, 0);
275 | }
276 |
277 | /* Enable line5 ~ line9 external interrupt. */
278 |
279 | memset(g_sonar, 0, sizeof(g_sonar));
280 | return 1;
281 | }
282 |
283 | /**
284 | @brief Clean ultrasonic sonar module.
285 | @param None.
286 | @return None.
287 | */
288 | void sonar_exit(void)
289 | {
290 | uint8_t ch;
291 |
292 | for (ch = 0; ch < _countof(g_sonar_cfg); ch++) {
293 | SONAR_TRIG(ch, 0);
294 | }
295 |
296 | /* Disable line5 ~ line9 external interrupt. */
297 |
298 | memset(g_sonar, 0, sizeof(g_sonar));
299 | }
300 |
--------------------------------------------------------------------------------
/src/driver/usart.c:
--------------------------------------------------------------------------------
1 | #include "usart.h"
2 | #include "stdio.h"
3 |
4 | #ifdef _PRINT
5 | #pragma import(__use_no_semihosting)
6 |
7 | struct __FILE
8 | {
9 | int handle;
10 | };
11 |
12 | FILE __stdout;
13 |
14 | int fputc(int ch, FILE *f)
15 | {
16 | while((USART3->SR&0X40)==0){};
17 | USART3->DR = (u8) ch;
18 | return ch;
19 | }
20 | #endif
21 | u8 send_data[USART_MAX_LEN];
22 | u8 USART2_RX_BUF[USART_MAX_LEN]; //接收缓冲,最大USART_MAX_LEN个字节.
23 | u16 USART2_RX_STA = 0; //接收状态标记
24 | u8 Res2 = 0;
25 | u8 USART3_RX_BUF[USART_MAX_LEN]; //接收缓冲,最大USART_MAX_LEN个字节.
26 | u16 USART3_RX_STA = 0; //接收状态标记
27 | u8 Res3 = 0;
28 |
29 | int usart3_init(u32 bound){
30 | //GPIO端口设置
31 | GPIO_InitTypeDef GPIO_InitStructure;
32 | USART_InitTypeDef USART_InitStructure;
33 | NVIC_InitTypeDef NVIC_InitStructure;
34 | #ifdef _REMAP
35 | RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO | RCC_APB2Periph_GPIOC, ENABLE);
36 | RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE);
37 |
38 | GPIO_PinRemapConfig(GPIO_PartialRemap_USART3, ENABLE); //将USART3局部重映射到PC10,PC11
39 | #else
40 | RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO | RCC_APB2Periph_GPIOB, ENABLE);
41 | RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE);
42 | #endif
43 | //USART3_TX GPIOB.10
44 | GPIO_InitStructure.GPIO_Pin = USART3_TX; //PB.10
45 | GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
46 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; //复用推挽输出
47 | GPIO_Init(USART3_PORT, &GPIO_InitStructure); //初始化GPIOB.10
48 |
49 | //USART3_RX GPIOB.11初始化
50 | GPIO_InitStructure.GPIO_Pin = USART3_RX; //PC.11
51 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //浮空输入
52 | GPIO_Init(USART3_PORT, &GPIO_InitStructure); //初始化GPIOB.11
53 |
54 | //USART3 NVIC 配置
55 | NVIC_InitStructure.NVIC_IRQChannel = USART3_IRQn;
56 | NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3; //抢占优先级3
57 | NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3; //子优先级3
58 | NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //IRQ通道使能
59 | NVIC_Init(&NVIC_InitStructure); //根据指定的参数初始化VIC寄存器
60 |
61 | //USART 初始化设置
62 |
63 | USART_InitStructure.USART_BaudRate = bound; //串口波特率
64 | USART_InitStructure.USART_WordLength = USART_WordLength_8b; //字长为8位数据格式
65 | USART_InitStructure.USART_StopBits = USART_StopBits_1; //一个停止位
66 | USART_InitStructure.USART_Parity = USART_Parity_No; //无奇偶校验位
67 | USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; //无硬件数据流控制
68 | USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx; //收发模式
69 |
70 | USART_Init(USART3, &USART_InitStructure); //初始化串口3
71 | #ifdef _DMA_USART
72 | USART_ITConfig(USART3, USART_IT_IDLE, ENABLE);
73 | #endif
74 | USART_ITConfig(USART3, USART_IT_RXNE, ENABLE); //开启串口接受中断
75 | USART_Cmd(USART3, ENABLE); //使能串口3
76 | #ifdef _DMA_USART
77 | DMA_InitTypeDef DMA_InitStructure;
78 |
79 | DMA_DeInit(DMA1_Channel2); //将DMA的通道12寄存器重设为缺省值 串口1对应的是DMA通道5
80 | DMA_InitStructure.DMA_PeripheralBaseAddr = (u32)&USART3->DR; //DMA外设usart基地址
81 | DMA_InitStructure.DMA_MemoryBaseAddr = (u32)send_data; //DMA内存基地址
82 | DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST; //数据传输方向,从外设读取发送到内存
83 | DMA_InitStructure.DMA_BufferSize = USART_MAX_LEN; //DMA通道的DMA缓存的大小
84 | DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; //外设地址寄存器不变
85 | DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; //内存地址寄存器递增
86 | DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; //数据宽度为8位
87 | DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; //数据宽度为8位
88 | DMA_InitStructure.DMA_Mode = DMA_Mode_Normal; //工作在正常缓存模式
89 | DMA_InitStructure.DMA_Priority = DMA_Priority_Medium; //DMA通道12拥有中优先级
90 | DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; //DMA通道12没有设置为内存到内存传输
91 | DMA_Init(DMA1_Channel2, &DMA_InitStructure); //根据DMA_InitStruct中指定的参数初始化DMA的通道
92 | DMA_Cmd(DMA1_Channel2, ENABLE); //正式驱动DMA传输
93 |
94 | //DMA串口接收配置
95 | DMA_DeInit(DMA1_Channel3); //将DMA的通道13寄存器重设为缺省值 串口1对应的是DMA通道5
96 | DMA_InitStructure.DMA_PeripheralBaseAddr = (u32)&USART3->DR; //DMA外设usart基地址
97 | DMA_InitStructure.DMA_MemoryBaseAddr = (u32)USART3_RX_BUF; //DMA内存基地址
98 | DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC; //数据传输方向,从外设读取发送到内存
99 | DMA_InitStructure.DMA_BufferSize = 16; //DMA通道的DMA缓存的大小
100 | DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; //外设地址寄存器不变
101 | DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; //内存地址寄存器递增
102 | DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; //数据宽度为8位
103 | DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; //数据宽度为8位
104 | DMA_InitStructure.DMA_Mode = DMA_Mode_Normal; //工作在正常缓存模式
105 | DMA_InitStructure.DMA_Priority = DMA_Priority_Medium; //DMA通道13拥有中优先级
106 | DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; //DMA通道13没有设置为内存到内存传输
107 | DMA_Init(DMA1_Channel3, &DMA_InitStructure); //根据DMA_InitStruct中指定的参数初始化DMA的通道
108 | DMA_Cmd(DMA1_Channel3, ENABLE); //正式驱动DMA传输
109 |
110 | USART_DMACmd(USART3, USART_DMAReq_Tx, ENABLE); //开启串口DMA发送
111 | USART_DMACmd(USART3, USART_DMAReq_Rx, ENABLE); //开启串口DMA接收
112 | #endif
113 | return 1;
114 | }
115 |
116 | #ifdef _DMA_USART
117 | //int usart3_dma_init(u32 bound){
118 | // //GPIO端口设置
119 | // GPIO_InitTypeDef GPIO_InitStructure;
120 | // USART_InitTypeDef USART_InitStructure;
121 | // NVIC_InitTypeDef NVIC_InitStructure;
122 | // DMA_InitTypeDef DMA_InitStructure;
123 |
124 | //#ifdef _REMAP
125 | // RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO | RCC_APB2Periph_GPIOC, ENABLE);
126 | // RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE);
127 |
128 | // GPIO_PinRemapConfig(GPIO_PartialRemap_USART3, ENABLE); //将USART3局部重映射到PC10,PC11
129 | //#else
130 | // RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO | RCC_APB2Periph_GPIOB, ENABLE);
131 | // RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE);
132 | //#endif
133 |
134 | // //USART3_TX GPIOB.10
135 | // GPIO_InitStructure.GPIO_Pin = USART3_TX; //PB.10
136 | // GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
137 | // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; //复用推挽输出
138 | // GPIO_Init(USART3_PORT, &GPIO_InitStructure); //初始化GPIOB.10
139 |
140 | // //USART3_RX GPIOB.11初始化
141 | // GPIO_InitStructure.GPIO_Pin = USART3_RX; //PC.11
142 | // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //浮空输入
143 | // GPIO_Init(USART3_PORT, &GPIO_InitStructure); //初始化GPIOB.11
144 |
145 | // //USART3 NVIC 配置
146 | // NVIC_InitStructure.NVIC_IRQChannel = USART3_IRQn;
147 | // NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3; //抢占优先级3
148 | // NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3; //子优先级3
149 | // NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //IRQ通道使能
150 | // NVIC_Init(&NVIC_InitStructure); //根据指定的参数初始化VIC寄存器
151 |
152 | // //USART 初始化设置
153 |
154 | // USART_InitStructure.USART_BaudRate = bound; //串口波特率
155 | // USART_InitStructure.USART_WordLength = USART_WordLength_8b; //字长为8位数据格式
156 | // USART_InitStructure.USART_StopBits = USART_StopBits_1; //一个停止位
157 | // USART_InitStructure.USART_Parity = USART_Parity_No; //无奇偶校验位
158 | // USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; //无硬件数据流控制
159 | // USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx; //收发模式
160 |
161 | // USART_Init(USART3, &USART_InitStructure); //初始化串口3
162 | // USART_ITConfig(USART3, USART_IT_IDLE, ENABLE);
163 | // USART_ITConfig(USART3, USART_IT_RXNE, ENABLE); //开启串口接受中断
164 | // USART_Cmd(USART3, ENABLE); //使能串口3
165 |
166 | // //DMA串口发送配置
167 | // DMA_DeInit(DMA1_Channel2); //将DMA的通道12寄存器重设为缺省值 串口1对应的是DMA通道5
168 | // DMA_InitStructure.DMA_PeripheralBaseAddr = (u32)&USART3->DR; //DMA外设usart基地址
169 | // DMA_InitStructure.DMA_MemoryBaseAddr = (u32)send_data; //DMA内存基地址
170 | // DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST; //数据传输方向,从外设读取发送到内存
171 | // DMA_InitStructure.DMA_BufferSize = USART_MAX_LEN; //DMA通道的DMA缓存的大小
172 | // DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; //外设地址寄存器不变
173 | // DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; //内存地址寄存器递增
174 | // DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; //数据宽度为8位
175 | // DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; //数据宽度为8位
176 | // DMA_InitStructure.DMA_Mode = DMA_Mode_Normal; //工作在正常缓存模式
177 | // DMA_InitStructure.DMA_Priority = DMA_Priority_Medium; //DMA通道12拥有中优先级
178 | // DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; //DMA通道12没有设置为内存到内存传输
179 | // DMA_Init(DMA1_Channel2, &DMA_InitStructure); //根据DMA_InitStruct中指定的参数初始化DMA的通道
180 | // DMA_Cmd(DMA1_Channel2, ENABLE); //正式驱动DMA传输
181 |
182 | // //DMA串口接收配置
183 | // DMA_DeInit(DMA1_Channel3); //将DMA的通道13寄存器重设为缺省值 串口1对应的是DMA通道5
184 | // DMA_InitStructure.DMA_PeripheralBaseAddr = (u32)&USART3->DR; //DMA外设usart基地址
185 | // DMA_InitStructure.DMA_MemoryBaseAddr = (u32)USART3_RX_BUF; //DMA内存基地址
186 | // DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC; //数据传输方向,从外设读取发送到内存
187 | // DMA_InitStructure.DMA_BufferSize = USART_MAX_LEN; //DMA通道的DMA缓存的大小
188 | // DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; //外设地址寄存器不变
189 | // DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; //内存地址寄存器递增
190 | // DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; //数据宽度为8位
191 | // DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; //数据宽度为8位
192 | // DMA_InitStructure.DMA_Mode = DMA_Mode_Normal; //工作在正常缓存模式
193 | // DMA_InitStructure.DMA_Priority = DMA_Priority_Medium; //DMA通道13拥有中优先级
194 | // DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; //DMA通道13没有设置为内存到内存传输
195 | // DMA_Init(DMA1_Channel3, &DMA_InitStructure); //根据DMA_InitStruct中指定的参数初始化DMA的通道
196 | // DMA_Cmd(DMA1_Channel3, ENABLE); //正式驱动DMA传输
197 |
198 | // USART_DMACmd(USART3, USART_DMAReq_Tx, ENABLE); //开启串口DMA发送
199 | // USART_DMACmd(USART3, USART_DMAReq_Rx, ENABLE); //开启串口DMA接收
200 |
201 | // return 1;
202 | //}
203 |
204 | void DMA_Enable(DMA_Channel_TypeDef * DMA_CHx)
205 | {
206 | DMA_Cmd(DMA_CHx, DISABLE);
207 | DMA_SetCurrDataCounter(DMA_CHx, USART_MAX_LEN);
208 | DMA_Cmd(DMA_CHx, ENABLE);
209 | }
210 | #endif
211 |
212 | int usart2_init(u32 bound){
213 | //GPIO端口设置
214 | GPIO_InitTypeDef GPIO_InitStructure;
215 | USART_InitTypeDef USART_InitStructure;
216 | NVIC_InitTypeDef NVIC_InitStructure;
217 |
218 | RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO | RCC_APB2Periph_GPIOA, ENABLE);
219 | RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE);
220 |
221 | //USART2_TX GPIOA.2
222 | GPIO_InitStructure.GPIO_Pin = USART2_TX; //PA.2
223 | GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
224 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; //复用推挽输出
225 | GPIO_Init(USART2_PORT, &GPIO_InitStructure);
226 |
227 | //USART2_RX GPIOA.3
228 | GPIO_InitStructure.GPIO_Pin = USART2_RX;//PA.3
229 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //浮空输入
230 | GPIO_Init(USART2_PORT, &GPIO_InitStructure);
231 |
232 | //USART2 NVIC 配置
233 | NVIC_InitStructure.NVIC_IRQChannel = USART2_IRQn;
234 | NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3; //抢占优先级3
235 | NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2; //子优先级2
236 | NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //IRQ通道使能
237 | NVIC_Init(&NVIC_InitStructure); //根据指定的参数初始化VIC寄存器
238 |
239 | //USART 初始化设置
240 |
241 | USART_InitStructure.USART_BaudRate = bound; //串口波特率
242 | USART_InitStructure.USART_WordLength = USART_WordLength_8b; //字长为8位数据格式
243 | USART_InitStructure.USART_StopBits = USART_StopBits_1; //一个停止位
244 | USART_InitStructure.USART_Parity = USART_Parity_No; //无奇偶校验位
245 | USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; //无硬件数据流控制
246 | USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx; //收发模式
247 |
248 | USART_Init(USART2, &USART_InitStructure); //初始化串口2
249 | USART_ITConfig(USART2, USART_IT_RXNE, ENABLE); //开启串口接受中断
250 | USART_Cmd(USART2, ENABLE); //使能串口2
251 | return 1;
252 | }
253 | #ifndef _REMAP
254 |
255 | u8 UART4_RX_BUF[USART_MAX_LEN]; //接收缓冲,最大USART_MAX_LEN个字节.
256 | u16 UART4_RX_STA = 0; //接收状态标记
257 | u8 Res4 = 0;
258 |
259 | int uart4_init(u32 bound){
260 | //GPIO端口设置
261 | GPIO_InitTypeDef GPIO_InitStructure;
262 | USART_InitTypeDef USART_InitStructure;
263 | NVIC_InitTypeDef NVIC_InitStructure;
264 |
265 | RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO | RCC_APB2Periph_GPIOC, ENABLE);
266 | RCC_APB1PeriphClockCmd(RCC_APB1Periph_UART4, ENABLE);
267 |
268 | //UART4_TX GPIOC.10
269 | GPIO_InitStructure.GPIO_Pin = UART4_TX; //PC.10
270 | GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
271 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; //复用推挽输出
272 | GPIO_Init(UART4_PORT, &GPIO_InitStructure); //初始化GPIOC.10
273 |
274 | //UART4_RX GPIOC.11初始化
275 | GPIO_InitStructure.GPIO_Pin = UART4_RX; //PC.11
276 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //浮空输入
277 | GPIO_Init(UART4_PORT, &GPIO_InitStructure); //初始化GPIOC.11
278 |
279 | //UART4 NVIC 配置
280 | NVIC_InitStructure.NVIC_IRQChannel = UART4_IRQn;
281 | NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3; //抢占优先级3
282 | NVIC_InitStructure.NVIC_IRQChannelSubPriority = 4; //子优先级4
283 | NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //IRQ通道使能
284 | NVIC_Init(&NVIC_InitStructure); //根据指定的参数初始化VIC寄存器
285 |
286 | //USART 初始化设置
287 |
288 | USART_InitStructure.USART_BaudRate = bound; //串口波特率
289 | USART_InitStructure.USART_WordLength = USART_WordLength_8b; //字长为8位数据格式
290 | USART_InitStructure.USART_StopBits = USART_StopBits_1; //一个停止位
291 | USART_InitStructure.USART_Parity = USART_Parity_No; //无奇偶校验位
292 | USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; //无硬件数据流控制
293 | USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx; //收发模式
294 |
295 | USART_Init(UART4, &USART_InitStructure); //初始化串口4
296 | USART_ITConfig(UART4, USART_IT_RXNE, ENABLE); //开启串口接受中断
297 | USART_Cmd(UART4, ENABLE); //使能串口4
298 | return 1;
299 | }
300 |
301 | void UART4_IRQHandler(void) //串口4中断服务程序
302 | {
303 | if(USART_GetITStatus(UART4, USART_IT_RXNE) != RESET)
304 | {
305 | USART_ClearITPendingBit(UART4, USART_IT_RXNE);
306 | UART4_RX_BUF[Res4] = USART_ReceiveData(UART4);
307 | Res4++;
308 | }
309 |
310 | if((UART4_RX_BUF[Res4 - 2] == 0x0D)&&(USART2_RX_BUF[Res4 - 1] == 0x0A)) UART4_RX_STA = 1;
311 | if(USART_GetFlagStatus(UART4, USART_FLAG_ORE) == SET)
312 | {
313 | USART_ClearFlag(UART4, USART_FLAG_ORE);
314 | USART_ReceiveData(UART4);
315 | }
316 | }
317 | #endif
318 |
319 | #ifdef _DMA_USART
320 | void USART3_IRQHandler(void)
321 | {
322 | if(USART_GetITStatus(USART3, USART_IT_IDLE) != RESET)
323 | {
324 | USART_ReceiveData(USART3);
325 | USART3_RX_STA = 1;
326 | USART_ClearITPendingBit(USART3, USART_IT_IDLE);
327 | DMA_Enable(DMA1_Channel3);
328 | }
329 | }
330 | #else
331 | void USART3_IRQHandler(void) //串口3中断服务程序
332 | {
333 | if(USART_GetITStatus(USART3, USART_IT_RXNE) != RESET)
334 | {
335 | USART_ClearITPendingBit(USART3, USART_IT_RXNE);
336 | USART3_RX_BUF[Res3] = USART_ReceiveData(USART3);
337 | Res3++;
338 | }
339 |
340 | if((USART3_RX_BUF[Res3 - 2] == 0x0D)&&(USART3_RX_BUF[Res3 - 1] == 0x0A)) USART3_RX_STA = 1;
341 | if(USART_GetFlagStatus(USART3, USART_FLAG_ORE) == SET)
342 | {
343 | USART_ClearFlag(USART3, USART_FLAG_ORE);
344 | USART_ReceiveData(USART3);
345 | }
346 | }
347 | #endif
348 |
349 | void USART2_IRQHandler(void) //串口2中断服务程序
350 | {
351 | if(USART_GetITStatus(USART2, USART_IT_RXNE) != RESET)
352 | {
353 | USART_ClearITPendingBit(USART2, USART_IT_RXNE);
354 | USART2_RX_BUF[Res2] = USART_ReceiveData(USART2);
355 | Res2++;
356 | }
357 |
358 | if((USART2_RX_BUF[Res2 - 2] == 0x0D)&&(USART2_RX_BUF[Res2 - 1] == 0x0A)) USART2_RX_STA = 1;
359 | if(USART_GetFlagStatus(USART2, USART_FLAG_ORE) == SET)
360 | {
361 | USART_ClearFlag(USART2, USART_FLAG_ORE);
362 | USART_ReceiveData(USART2);
363 | }
364 | }
365 |
--------------------------------------------------------------------------------
/src/driver/watchdog.c:
--------------------------------------------------------------------------------
1 | #include "watchdog.h"
2 | /*
3 | * 看门狗使能函数
4 | * 打开看门狗
5 | */
6 | void enable_watchdog()
7 | {
8 | RCC_ClearFlag();
9 | RCC_APB1PeriphClockCmd(RCC_APB1Periph_WWDG, ENABLE);
10 | WWDG_SetPrescaler(WWDG_Prescaler_8);
11 | WWDG_SetWindowValue(127);
12 | WWDG_Enable(127);
13 | }
14 | /*
15 | * 看门狗禁止函数
16 | * 关闭看门狗
17 | */
18 | void disable_watchdog()
19 | {
20 | mark_watchdog();
21 | RCC_APB1PeriphClockCmd(RCC_APB1Periph_WWDG, DISABLE);
22 | }
23 | /*
24 | * 喂狗函数
25 | */
26 | void mark_watchdog()
27 | {
28 | WWDG_SetCounter(127);
29 | }
30 |
--------------------------------------------------------------------------------
/src/include/battery.h:
--------------------------------------------------------------------------------
1 | #ifndef __BATTERY_H
2 | #define __BATTERY_H
3 |
4 | #define BATT_DETECT_PORT GPIOC
5 | #define BATT_DETECT_PIN GPIO_Pin_0
6 |
7 |
8 | #define BATTERY_VOLTAGE_FULL ((int)(12.4 * 1000)) //mV
9 | #define BATTERY_VOLTAGE_EMPTY ((int)(9.25 * 1000)) //mV
10 |
11 | int init_battery(void);
12 | float get_electricity(void);
13 | float get_electricitypercentage(void);
14 | void heartbeat_battery(void);
15 |
16 | #endif
17 |
--------------------------------------------------------------------------------
/src/include/beep.h:
--------------------------------------------------------------------------------
1 | #ifndef __BEEP_H
2 | #define __BEEP_H
3 |
4 | #include "common.h"
5 |
6 | #define BEEP_GPIO GPIOA
7 | #define BEEP_PIN GPIO_Pin_15
8 | #define BEEP_PWM TIM2
9 |
10 | #define TIME_PERIOD 255
11 | #define TIME_PRESCALER (TIME_PERIOD + 1)
12 | #define BEEP_INIT_HZ 1568
13 |
14 | //#define BEEP_OPEN() (GPIO_SetBits(GPIOC, GPIO_Pin_12))
15 | //#define BEEP_CLOSE() (GPIO_ResetBits(GPIOC, GPIO_Pin_12))
16 | enum {
17 | BEEP_START = 1,
18 | BEEP_STOP = 2,
19 | BEEP_WORKING = 3,
20 | };
21 | enum {
22 | BEEP_POWERON = 1,
23 | BEEP_POWEROFF = 2,
24 | BEEP_POWERLOW = 3,
25 | BEEP_POWERCHARGE = 4,
26 | BEEP_PLAYWORKING = 5,
27 | };
28 | int init_beep(void);
29 | void start_beep(void);
30 | void stop_beep(void);
31 | void play_music(void);
32 | void play_poweron(void);
33 | void play_poweron_frequency(void);
34 | void beep_beeper(_u32 frequency, _u32 delay, _u8 sound);
35 | void heartbeat_beep(float n);
36 | void on_abort_mode(void);
37 | #endif
38 |
--------------------------------------------------------------------------------
/src/include/boarddef.h:
--------------------------------------------------------------------------------
1 | //MCU主频
2 | #define CPU_FREQ 72000000L
3 |
4 | //IO端口重映射
5 | #define PERFORM_IO_REMAP() GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);\
6 | GPIO_PinRemapConfig(GPIO_FullRemap_TIM3, ENABLE)
7 |
8 | //APB2总线上的外设
9 | #define APB2PERIPH_INIT_LIST \
10 | ( RCC_APB2Periph_GPIOA \
11 | | RCC_APB2Periph_GPIOB \
12 | | RCC_APB2Periph_GPIOC \
13 | | RCC_APB2Periph_AFIO \
14 | | RCC_APB2Periph_ADC1 \
15 | )
16 | //APB1总线上的外设
17 | #define APB1PERIPH_INIT_LIST \
18 | ( RCC_APB1Periph_USART2 \
19 | | RCC_APB1Periph_TIM3 \
20 | | RCC_APB1Periph_USART3 \
21 | )
22 |
--------------------------------------------------------------------------------
/src/include/boardfunc.h:
--------------------------------------------------------------------------------
1 | #pragma once
2 | #include "types.h"
3 |
4 | #define SYSTICK_1MS_TICKS (CPU_FREQ/1000)
5 |
6 | #define cli __disable_irq
7 | #define sei __enable_irq
8 |
9 | static _u32 enter_critical_section(void)
10 | {
11 | _u32 context=__get_PRIMASK();
12 | cli();
13 | return context;
14 | }
15 |
16 | static void leave_critical_section(_u32 context)
17 | {
18 | __set_PRIMASK(context);
19 | }
20 |
21 |
22 | typedef void (* abort_proc_t ) (void);
23 | void board_abort_mode(void);
24 | void board_set_abort_proc( abort_proc_t proc);
25 | uint32_t getms(void);
26 | uint64_t getus(void);
27 | void delay(uint32_t ms);
28 | void delay_alert(uint32_t ms);
29 | extern void _delay_us(volatile uint32_t us);
30 | static void _delay_ms(uint32_t ms)
31 | {
32 | while(ms--)
33 | {
34 | _delay_us(1000);
35 | }
36 | }
37 | void alert(void);
38 | void clear_alert(void);
39 | int is_alert(void);
40 |
41 | /*
42 | * GPIO相关
43 | */
44 | #define HIGH 1
45 | #define LOW 0
46 |
47 | #define _PIN_SET_1(port, pin) port->BSRR = pin
48 | #define _PIN_SET_0(port, pin) port->BRR = pin
49 | #define _PIN_SET(port, pin, val) _PIN_SET_##val(port, pin)
50 |
51 | #define PIN_SET(port, pin, val) EXPAND_WRAPPER(_PIN_SET, port, pin, val)
52 | #define PIN_READ(port, pin) ((port)->IDR & pin)
53 |
54 | #define pinSet GPIO_WriteBit
55 | #define pinRead GPIO_ReadInputDataBit
56 |
57 | #define _RAW_PWM_SET(channel, timer, val) \
58 | TIM##timer->CCR##channel = val
59 |
60 | #define RAW_PWM_SET(channel, timer, val) \
61 | EXPAND_WRAPPER(_RAW_PWM_SET, channel, timer, val)
62 |
63 | static void pinMode(GPIO_TypeDef* GPIOx, uint16_t pin,
64 | GPIOMode_TypeDef mode, GPIOSpeed_TypeDef GPIO_Speed)
65 | {
66 | //GPIO_InitTypeDef init = { pin, GPIO_Speed, mode};
67 | GPIO_InitTypeDef init;
68 | init.GPIO_Pin = pin;
69 | init.GPIO_Speed = GPIO_Speed;
70 | init.GPIO_Mode = mode;
71 | GPIO_Init(GPIOx, &init);
72 | }
73 |
74 | /*
75 | * ADC相关
76 | */
77 | void adc_read_start(ADC_TypeDef * adc_dev, uint8_t ADC_Channel);
78 | uint16_t adc_read_final(ADC_TypeDef * adc_dev);
79 | uint16_t adc_read_wait(ADC_TypeDef * adc_dev);
80 | static int adc_read_is_ready(ADC_TypeDef * adc_dev)
81 | {
82 | return (adc_dev->SR & ADC_FLAG_EOC);
83 | }
84 | float board_get_temperature(void);
85 | uint16_t get_adc(uint8_t ch);
86 | uint16_t get_adc_average(uint8_t ch, uint8_t times);
87 | #include "softdelay.h"
88 | #include "rpstm32.h"
89 | #include "usart.h"
90 |
--------------------------------------------------------------------------------
/src/include/common.h:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include "rpstm32.h"
4 |
5 | /**
6 | @brief input port definitions.
7 | */
8 | typedef struct _in_port {
9 | GPIO_TypeDef *port; /**< input port. */
10 | _u16 pin; /**< input pin. */
11 | _u8 level; /**< input trigger level. */
12 | } in_port_t;
13 |
14 | /**
15 | @brief output definitions.
16 | */
17 | typedef struct _out_port {
18 | GPIO_TypeDef *port; /**< output port. */
19 | _u16 pin; /**< output pin. */
20 | _u8 level; /**< output default level. */
21 | } out_port_t;
22 |
23 | /**
24 | @brief pwm control port definitions.
25 | */
26 | typedef struct _pwm_port {
27 | GPIO_TypeDef *port; /**< pwm control port. */
28 | _u16 pin; /**< pwm control pin. */
29 | TIM_TypeDef *tim; /**< pwm timer. */
30 | _u8 tim_ch; /**< pwm channel. */
31 | } pwm_port_t;
32 |
33 | /**
34 | @brief external interrupt port definitions.
35 | */
36 | typedef struct _exti_cfg {
37 | GPIO_TypeDef *port; /**< EXTI port. */
38 | _u16 pin; /**< EXTI pin. */
39 | _u8 exti; /**< external interrupt. */
40 | } exti_port_t;
41 |
42 | typedef struct _dire_cfg {
43 | GPIO_TypeDef *port; /**< direction port. */
44 | _u16 f_pin; /**< direction pin. */
45 | _u16 b_pin; /**< direction pin. */
46 | } dire_port_t;
47 |
48 | _s32 init_board(void);
49 |
--------------------------------------------------------------------------------
/src/include/crc.h:
--------------------------------------------------------------------------------
1 | #ifndef __CRC_H
2 | #define __CRC_H
3 | #include "stm32f10x.h"
4 |
5 | u8 Get_Crc8(u8 *ptr,u16 len);
6 | u16 Get_Crc16(u8 *puchMsg,u16 usDataLen);
7 |
8 | #endif
9 |
--------------------------------------------------------------------------------
/src/include/dmpKey.h:
--------------------------------------------------------------------------------
1 | /*
2 | $License:
3 | Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
4 | $
5 | */
6 | #ifndef DMPKEY_H__
7 | #define DMPKEY_H__
8 |
9 | #define KEY_CFG_25 (0)
10 | #define KEY_CFG_24 (KEY_CFG_25 + 1)
11 | #define KEY_CFG_26 (KEY_CFG_24 + 1)
12 | #define KEY_CFG_27 (KEY_CFG_26 + 1)
13 | #define KEY_CFG_21 (KEY_CFG_27 + 1)
14 | #define KEY_CFG_20 (KEY_CFG_21 + 1)
15 | #define KEY_CFG_TAP4 (KEY_CFG_20 + 1)
16 | #define KEY_CFG_TAP5 (KEY_CFG_TAP4 + 1)
17 | #define KEY_CFG_TAP6 (KEY_CFG_TAP5 + 1)
18 | #define KEY_CFG_TAP7 (KEY_CFG_TAP6 + 1)
19 | #define KEY_CFG_TAP0 (KEY_CFG_TAP7 + 1)
20 | #define KEY_CFG_TAP1 (KEY_CFG_TAP0 + 1)
21 | #define KEY_CFG_TAP2 (KEY_CFG_TAP1 + 1)
22 | #define KEY_CFG_TAP3 (KEY_CFG_TAP2 + 1)
23 | #define KEY_CFG_TAP_QUANTIZE (KEY_CFG_TAP3 + 1)
24 | #define KEY_CFG_TAP_JERK (KEY_CFG_TAP_QUANTIZE + 1)
25 | #define KEY_CFG_DR_INT (KEY_CFG_TAP_JERK + 1)
26 | #define KEY_CFG_AUTH (KEY_CFG_DR_INT + 1)
27 | #define KEY_CFG_TAP_SAVE_ACCB (KEY_CFG_AUTH + 1)
28 | #define KEY_CFG_TAP_CLEAR_STICKY (KEY_CFG_TAP_SAVE_ACCB + 1)
29 | #define KEY_CFG_FIFO_ON_EVENT (KEY_CFG_TAP_CLEAR_STICKY + 1)
30 | #define KEY_FCFG_ACCEL_INPUT (KEY_CFG_FIFO_ON_EVENT + 1)
31 | #define KEY_FCFG_ACCEL_INIT (KEY_FCFG_ACCEL_INPUT + 1)
32 | #define KEY_CFG_23 (KEY_FCFG_ACCEL_INIT + 1)
33 | #define KEY_FCFG_1 (KEY_CFG_23 + 1)
34 | #define KEY_FCFG_3 (KEY_FCFG_1 + 1)
35 | #define KEY_FCFG_2 (KEY_FCFG_3 + 1)
36 | #define KEY_CFG_3D (KEY_FCFG_2 + 1)
37 | #define KEY_CFG_3B (KEY_CFG_3D + 1)
38 | #define KEY_CFG_3C (KEY_CFG_3B + 1)
39 | #define KEY_FCFG_5 (KEY_CFG_3C + 1)
40 | #define KEY_FCFG_4 (KEY_FCFG_5 + 1)
41 | #define KEY_FCFG_7 (KEY_FCFG_4 + 1)
42 | #define KEY_FCFG_FSCALE (KEY_FCFG_7 + 1)
43 | #define KEY_FCFG_AZ (KEY_FCFG_FSCALE + 1)
44 | #define KEY_FCFG_6 (KEY_FCFG_AZ + 1)
45 | #define KEY_FCFG_LSB4 (KEY_FCFG_6 + 1)
46 | #define KEY_CFG_12 (KEY_FCFG_LSB4 + 1)
47 | #define KEY_CFG_14 (KEY_CFG_12 + 1)
48 | #define KEY_CFG_15 (KEY_CFG_14 + 1)
49 | #define KEY_CFG_16 (KEY_CFG_15 + 1)
50 | #define KEY_CFG_18 (KEY_CFG_16 + 1)
51 | #define KEY_CFG_6 (KEY_CFG_18 + 1)
52 | #define KEY_CFG_7 (KEY_CFG_6 + 1)
53 | #define KEY_CFG_4 (KEY_CFG_7 + 1)
54 | #define KEY_CFG_5 (KEY_CFG_4 + 1)
55 | #define KEY_CFG_2 (KEY_CFG_5 + 1)
56 | #define KEY_CFG_3 (KEY_CFG_2 + 1)
57 | #define KEY_CFG_1 (KEY_CFG_3 + 1)
58 | #define KEY_CFG_EXTERNAL (KEY_CFG_1 + 1)
59 | #define KEY_CFG_8 (KEY_CFG_EXTERNAL + 1)
60 | #define KEY_CFG_9 (KEY_CFG_8 + 1)
61 | #define KEY_CFG_ORIENT_3 (KEY_CFG_9 + 1)
62 | #define KEY_CFG_ORIENT_2 (KEY_CFG_ORIENT_3 + 1)
63 | #define KEY_CFG_ORIENT_1 (KEY_CFG_ORIENT_2 + 1)
64 | #define KEY_CFG_GYRO_SOURCE (KEY_CFG_ORIENT_1 + 1)
65 | #define KEY_CFG_ORIENT_IRQ_1 (KEY_CFG_GYRO_SOURCE + 1)
66 | #define KEY_CFG_ORIENT_IRQ_2 (KEY_CFG_ORIENT_IRQ_1 + 1)
67 | #define KEY_CFG_ORIENT_IRQ_3 (KEY_CFG_ORIENT_IRQ_2 + 1)
68 | #define KEY_FCFG_MAG_VAL (KEY_CFG_ORIENT_IRQ_3 + 1)
69 | #define KEY_FCFG_MAG_MOV (KEY_FCFG_MAG_VAL + 1)
70 | #define KEY_CFG_LP_QUAT (KEY_FCFG_MAG_MOV + 1)
71 |
72 | /* MPU6050 keys */
73 | #define KEY_CFG_ACCEL_FILTER (KEY_CFG_LP_QUAT + 1)
74 | #define KEY_CFG_MOTION_BIAS (KEY_CFG_ACCEL_FILTER + 1)
75 | #define KEY_TEMPLABEL (KEY_CFG_MOTION_BIAS + 1)
76 |
77 | #define KEY_D_0_22 (KEY_TEMPLABEL + 1)
78 | #define KEY_D_0_24 (KEY_D_0_22 + 1)
79 | #define KEY_D_0_36 (KEY_D_0_24 + 1)
80 | #define KEY_D_0_52 (KEY_D_0_36 + 1)
81 | #define KEY_D_0_96 (KEY_D_0_52 + 1)
82 | #define KEY_D_0_104 (KEY_D_0_96 + 1)
83 | #define KEY_D_0_108 (KEY_D_0_104 + 1)
84 | #define KEY_D_0_163 (KEY_D_0_108 + 1)
85 | #define KEY_D_0_188 (KEY_D_0_163 + 1)
86 | #define KEY_D_0_192 (KEY_D_0_188 + 1)
87 | #define KEY_D_0_224 (KEY_D_0_192 + 1)
88 | #define KEY_D_0_228 (KEY_D_0_224 + 1)
89 | #define KEY_D_0_232 (KEY_D_0_228 + 1)
90 | #define KEY_D_0_236 (KEY_D_0_232 + 1)
91 |
92 | #define KEY_DMP_PREVPTAT (KEY_D_0_236 + 1)
93 | #define KEY_D_1_2 (KEY_DMP_PREVPTAT + 1)
94 | #define KEY_D_1_4 (KEY_D_1_2 + 1)
95 | #define KEY_D_1_8 (KEY_D_1_4 + 1)
96 | #define KEY_D_1_10 (KEY_D_1_8 + 1)
97 | #define KEY_D_1_24 (KEY_D_1_10 + 1)
98 | #define KEY_D_1_28 (KEY_D_1_24 + 1)
99 | #define KEY_D_1_36 (KEY_D_1_28 + 1)
100 | #define KEY_D_1_40 (KEY_D_1_36 + 1)
101 | #define KEY_D_1_44 (KEY_D_1_40 + 1)
102 | #define KEY_D_1_72 (KEY_D_1_44 + 1)
103 | #define KEY_D_1_74 (KEY_D_1_72 + 1)
104 | #define KEY_D_1_79 (KEY_D_1_74 + 1)
105 | #define KEY_D_1_88 (KEY_D_1_79 + 1)
106 | #define KEY_D_1_90 (KEY_D_1_88 + 1)
107 | #define KEY_D_1_92 (KEY_D_1_90 + 1)
108 | #define KEY_D_1_96 (KEY_D_1_92 + 1)
109 | #define KEY_D_1_98 (KEY_D_1_96 + 1)
110 | #define KEY_D_1_100 (KEY_D_1_98 + 1)
111 | #define KEY_D_1_106 (KEY_D_1_100 + 1)
112 | #define KEY_D_1_108 (KEY_D_1_106 + 1)
113 | #define KEY_D_1_112 (KEY_D_1_108 + 1)
114 | #define KEY_D_1_128 (KEY_D_1_112 + 1)
115 | #define KEY_D_1_152 (KEY_D_1_128 + 1)
116 | #define KEY_D_1_160 (KEY_D_1_152 + 1)
117 | #define KEY_D_1_168 (KEY_D_1_160 + 1)
118 | #define KEY_D_1_175 (KEY_D_1_168 + 1)
119 | #define KEY_D_1_176 (KEY_D_1_175 + 1)
120 | #define KEY_D_1_178 (KEY_D_1_176 + 1)
121 | #define KEY_D_1_179 (KEY_D_1_178 + 1)
122 | #define KEY_D_1_218 (KEY_D_1_179 + 1)
123 | #define KEY_D_1_232 (KEY_D_1_218 + 1)
124 | #define KEY_D_1_236 (KEY_D_1_232 + 1)
125 | #define KEY_D_1_240 (KEY_D_1_236 + 1)
126 | #define KEY_D_1_244 (KEY_D_1_240 + 1)
127 | #define KEY_D_1_250 (KEY_D_1_244 + 1)
128 | #define KEY_D_1_252 (KEY_D_1_250 + 1)
129 | #define KEY_D_2_12 (KEY_D_1_252 + 1)
130 | #define KEY_D_2_96 (KEY_D_2_12 + 1)
131 | #define KEY_D_2_108 (KEY_D_2_96 + 1)
132 | #define KEY_D_2_208 (KEY_D_2_108 + 1)
133 | #define KEY_FLICK_MSG (KEY_D_2_208 + 1)
134 | #define KEY_FLICK_COUNTER (KEY_FLICK_MSG + 1)
135 | #define KEY_FLICK_LOWER (KEY_FLICK_COUNTER + 1)
136 | #define KEY_CFG_FLICK_IN (KEY_FLICK_LOWER + 1)
137 | #define KEY_FLICK_UPPER (KEY_CFG_FLICK_IN + 1)
138 | #define KEY_CGNOTICE_INTR (KEY_FLICK_UPPER + 1)
139 | #define KEY_D_2_224 (KEY_CGNOTICE_INTR + 1)
140 | #define KEY_D_2_244 (KEY_D_2_224 + 1)
141 | #define KEY_D_2_248 (KEY_D_2_244 + 1)
142 | #define KEY_D_2_252 (KEY_D_2_248 + 1)
143 |
144 | #define KEY_D_GYRO_BIAS_X (KEY_D_2_252 + 1)
145 | #define KEY_D_GYRO_BIAS_Y (KEY_D_GYRO_BIAS_X + 1)
146 | #define KEY_D_GYRO_BIAS_Z (KEY_D_GYRO_BIAS_Y + 1)
147 | #define KEY_D_ACC_BIAS_X (KEY_D_GYRO_BIAS_Z + 1)
148 | #define KEY_D_ACC_BIAS_Y (KEY_D_ACC_BIAS_X + 1)
149 | #define KEY_D_ACC_BIAS_Z (KEY_D_ACC_BIAS_Y + 1)
150 | #define KEY_D_GYRO_ENABLE (KEY_D_ACC_BIAS_Z + 1)
151 | #define KEY_D_ACCEL_ENABLE (KEY_D_GYRO_ENABLE + 1)
152 | #define KEY_D_QUAT_ENABLE (KEY_D_ACCEL_ENABLE +1)
153 | #define KEY_D_OUTPUT_ENABLE (KEY_D_QUAT_ENABLE + 1)
154 | #define KEY_D_CR_TIME_G (KEY_D_OUTPUT_ENABLE + 1)
155 | #define KEY_D_CR_TIME_A (KEY_D_CR_TIME_G + 1)
156 | #define KEY_D_CR_TIME_Q (KEY_D_CR_TIME_A + 1)
157 | #define KEY_D_CS_TAX (KEY_D_CR_TIME_Q + 1)
158 | #define KEY_D_CS_TAY (KEY_D_CS_TAX + 1)
159 | #define KEY_D_CS_TAZ (KEY_D_CS_TAY + 1)
160 | #define KEY_D_CS_TGX (KEY_D_CS_TAZ + 1)
161 | #define KEY_D_CS_TGY (KEY_D_CS_TGX + 1)
162 | #define KEY_D_CS_TGZ (KEY_D_CS_TGY + 1)
163 | #define KEY_D_CS_TQ0 (KEY_D_CS_TGZ + 1)
164 | #define KEY_D_CS_TQ1 (KEY_D_CS_TQ0 + 1)
165 | #define KEY_D_CS_TQ2 (KEY_D_CS_TQ1 + 1)
166 | #define KEY_D_CS_TQ3 (KEY_D_CS_TQ2 + 1)
167 |
168 | /* Compass keys */
169 | #define KEY_CPASS_BIAS_X (KEY_D_CS_TQ3 + 1)
170 | #define KEY_CPASS_BIAS_Y (KEY_CPASS_BIAS_X + 1)
171 | #define KEY_CPASS_BIAS_Z (KEY_CPASS_BIAS_Y + 1)
172 | #define KEY_CPASS_MTX_00 (KEY_CPASS_BIAS_Z + 1)
173 | #define KEY_CPASS_MTX_01 (KEY_CPASS_MTX_00 + 1)
174 | #define KEY_CPASS_MTX_02 (KEY_CPASS_MTX_01 + 1)
175 | #define KEY_CPASS_MTX_10 (KEY_CPASS_MTX_02 + 1)
176 | #define KEY_CPASS_MTX_11 (KEY_CPASS_MTX_10 + 1)
177 | #define KEY_CPASS_MTX_12 (KEY_CPASS_MTX_11 + 1)
178 | #define KEY_CPASS_MTX_20 (KEY_CPASS_MTX_12 + 1)
179 | #define KEY_CPASS_MTX_21 (KEY_CPASS_MTX_20 + 1)
180 | #define KEY_CPASS_MTX_22 (KEY_CPASS_MTX_21 + 1)
181 |
182 | /* Gesture Keys */
183 | #define KEY_DMP_TAPW_MIN (KEY_CPASS_MTX_22 + 1)
184 | #define KEY_DMP_TAP_THR_X (KEY_DMP_TAPW_MIN + 1)
185 | #define KEY_DMP_TAP_THR_Y (KEY_DMP_TAP_THR_X + 1)
186 | #define KEY_DMP_TAP_THR_Z (KEY_DMP_TAP_THR_Y + 1)
187 | #define KEY_DMP_SH_TH_Y (KEY_DMP_TAP_THR_Z + 1)
188 | #define KEY_DMP_SH_TH_X (KEY_DMP_SH_TH_Y + 1)
189 | #define KEY_DMP_SH_TH_Z (KEY_DMP_SH_TH_X + 1)
190 | #define KEY_DMP_ORIENT (KEY_DMP_SH_TH_Z + 1)
191 | #define KEY_D_ACT0 (KEY_DMP_ORIENT + 1)
192 | #define KEY_D_ACSX (KEY_D_ACT0 + 1)
193 | #define KEY_D_ACSY (KEY_D_ACSX + 1)
194 | #define KEY_D_ACSZ (KEY_D_ACSY + 1)
195 |
196 | #define KEY_X_GRT_Y_TMP (KEY_D_ACSZ + 1)
197 | #define KEY_SKIP_X_GRT_Y_TMP (KEY_X_GRT_Y_TMP + 1)
198 | #define KEY_SKIP_END_COMPARE (KEY_SKIP_X_GRT_Y_TMP + 1)
199 | #define KEY_END_COMPARE_Y_X_TMP2 (KEY_SKIP_END_COMPARE + 1)
200 | #define KEY_CFG_ANDROID_ORIENT_INT (KEY_END_COMPARE_Y_X_TMP2 + 1)
201 | #define KEY_NO_ORIENT_INTERRUPT (KEY_CFG_ANDROID_ORIENT_INT + 1)
202 | #define KEY_END_COMPARE_Y_X_TMP (KEY_NO_ORIENT_INTERRUPT + 1)
203 | #define KEY_END_ORIENT_1 (KEY_END_COMPARE_Y_X_TMP + 1)
204 | #define KEY_END_COMPARE_Y_X (KEY_END_ORIENT_1 + 1)
205 | #define KEY_END_ORIENT (KEY_END_COMPARE_Y_X + 1)
206 | #define KEY_X_GRT_Y (KEY_END_ORIENT + 1)
207 | #define KEY_NOT_TIME_MINUS_1 (KEY_X_GRT_Y + 1)
208 | #define KEY_END_COMPARE_Y_X_TMP3 (KEY_NOT_TIME_MINUS_1 + 1)
209 | #define KEY_X_GRT_Y_TMP2 (KEY_END_COMPARE_Y_X_TMP3 + 1)
210 |
211 | /* Authenticate Keys */
212 | #define KEY_D_AUTH_OUT (KEY_X_GRT_Y_TMP2 + 1)
213 | #define KEY_D_AUTH_IN (KEY_D_AUTH_OUT + 1)
214 | #define KEY_D_AUTH_A (KEY_D_AUTH_IN + 1)
215 | #define KEY_D_AUTH_B (KEY_D_AUTH_A + 1)
216 |
217 | /* Pedometer standalone only keys */
218 | #define KEY_D_PEDSTD_BP_B (KEY_D_AUTH_B + 1)
219 | #define KEY_D_PEDSTD_HP_A (KEY_D_PEDSTD_BP_B + 1)
220 | #define KEY_D_PEDSTD_HP_B (KEY_D_PEDSTD_HP_A + 1)
221 | #define KEY_D_PEDSTD_BP_A4 (KEY_D_PEDSTD_HP_B + 1)
222 | #define KEY_D_PEDSTD_BP_A3 (KEY_D_PEDSTD_BP_A4 + 1)
223 | #define KEY_D_PEDSTD_BP_A2 (KEY_D_PEDSTD_BP_A3 + 1)
224 | #define KEY_D_PEDSTD_BP_A1 (KEY_D_PEDSTD_BP_A2 + 1)
225 | #define KEY_D_PEDSTD_INT_THRSH (KEY_D_PEDSTD_BP_A1 + 1)
226 | #define KEY_D_PEDSTD_CLIP (KEY_D_PEDSTD_INT_THRSH + 1)
227 | #define KEY_D_PEDSTD_SB (KEY_D_PEDSTD_CLIP + 1)
228 | #define KEY_D_PEDSTD_SB_TIME (KEY_D_PEDSTD_SB + 1)
229 | #define KEY_D_PEDSTD_PEAKTHRSH (KEY_D_PEDSTD_SB_TIME + 1)
230 | #define KEY_D_PEDSTD_TIML (KEY_D_PEDSTD_PEAKTHRSH + 1)
231 | #define KEY_D_PEDSTD_TIMH (KEY_D_PEDSTD_TIML + 1)
232 | #define KEY_D_PEDSTD_PEAK (KEY_D_PEDSTD_TIMH + 1)
233 | #define KEY_D_PEDSTD_TIMECTR (KEY_D_PEDSTD_PEAK + 1)
234 | #define KEY_D_PEDSTD_STEPCTR (KEY_D_PEDSTD_TIMECTR + 1)
235 | #define KEY_D_PEDSTD_WALKTIME (KEY_D_PEDSTD_STEPCTR + 1)
236 | #define KEY_D_PEDSTD_DECI (KEY_D_PEDSTD_WALKTIME + 1)
237 |
238 | /*Host Based No Motion*/
239 | #define KEY_D_HOST_NO_MOT (KEY_D_PEDSTD_DECI + 1)
240 |
241 | /* EIS keys */
242 | #define KEY_P_EIS_FIFO_FOOTER (KEY_D_HOST_NO_MOT + 1)
243 | #define KEY_P_EIS_FIFO_YSHIFT (KEY_P_EIS_FIFO_FOOTER + 1)
244 | #define KEY_P_EIS_DATA_RATE (KEY_P_EIS_FIFO_YSHIFT + 1)
245 | #define KEY_P_EIS_FIFO_XSHIFT (KEY_P_EIS_DATA_RATE + 1)
246 | #define KEY_P_EIS_FIFO_SYNC (KEY_P_EIS_FIFO_XSHIFT + 1)
247 | #define KEY_P_EIS_FIFO_ZSHIFT (KEY_P_EIS_FIFO_SYNC + 1)
248 | #define KEY_P_EIS_FIFO_READY (KEY_P_EIS_FIFO_ZSHIFT + 1)
249 | #define KEY_DMP_FOOTER (KEY_P_EIS_FIFO_READY + 1)
250 | #define KEY_DMP_INTX_HC (KEY_DMP_FOOTER + 1)
251 | #define KEY_DMP_INTX_PH (KEY_DMP_INTX_HC + 1)
252 | #define KEY_DMP_INTX_SH (KEY_DMP_INTX_PH + 1)
253 | #define KEY_DMP_AINV_SH (KEY_DMP_INTX_SH + 1)
254 | #define KEY_DMP_A_INV_XH (KEY_DMP_AINV_SH + 1)
255 | #define KEY_DMP_AINV_PH (KEY_DMP_A_INV_XH + 1)
256 | #define KEY_DMP_CTHX_H (KEY_DMP_AINV_PH + 1)
257 | #define KEY_DMP_CTHY_H (KEY_DMP_CTHX_H + 1)
258 | #define KEY_DMP_CTHZ_H (KEY_DMP_CTHY_H + 1)
259 | #define KEY_DMP_NCTHX_H (KEY_DMP_CTHZ_H + 1)
260 | #define KEY_DMP_NCTHY_H (KEY_DMP_NCTHX_H + 1)
261 | #define KEY_DMP_NCTHZ_H (KEY_DMP_NCTHY_H + 1)
262 | #define KEY_DMP_CTSQ_XH (KEY_DMP_NCTHZ_H + 1)
263 | #define KEY_DMP_CTSQ_YH (KEY_DMP_CTSQ_XH + 1)
264 | #define KEY_DMP_CTSQ_ZH (KEY_DMP_CTSQ_YH + 1)
265 | #define KEY_DMP_INTX_H (KEY_DMP_CTSQ_ZH + 1)
266 | #define KEY_DMP_INTY_H (KEY_DMP_INTX_H + 1)
267 | #define KEY_DMP_INTZ_H (KEY_DMP_INTY_H + 1)
268 | //#define KEY_DMP_HPX_H (KEY_DMP_INTZ_H + 1)
269 | //#define KEY_DMP_HPY_H (KEY_DMP_HPX_H + 1)
270 | //#define KEY_DMP_HPZ_H (KEY_DMP_HPY_H + 1)
271 |
272 | /* Stream keys */
273 | #define KEY_STREAM_P_GYRO_Z (KEY_DMP_INTZ_H + 1)
274 | #define KEY_STREAM_P_GYRO_Y (KEY_STREAM_P_GYRO_Z + 1)
275 | #define KEY_STREAM_P_GYRO_X (KEY_STREAM_P_GYRO_Y + 1)
276 | #define KEY_STREAM_P_TEMP (KEY_STREAM_P_GYRO_X + 1)
277 | #define KEY_STREAM_P_AUX_Y (KEY_STREAM_P_TEMP + 1)
278 | #define KEY_STREAM_P_AUX_X (KEY_STREAM_P_AUX_Y + 1)
279 | #define KEY_STREAM_P_AUX_Z (KEY_STREAM_P_AUX_X + 1)
280 | #define KEY_STREAM_P_ACCEL_Y (KEY_STREAM_P_AUX_Z + 1)
281 | #define KEY_STREAM_P_ACCEL_X (KEY_STREAM_P_ACCEL_Y + 1)
282 | #define KEY_STREAM_P_FOOTER (KEY_STREAM_P_ACCEL_X + 1)
283 | #define KEY_STREAM_P_ACCEL_Z (KEY_STREAM_P_FOOTER + 1)
284 |
285 | #define NUM_KEYS (KEY_STREAM_P_ACCEL_Z + 1)
286 |
287 | typedef struct {
288 | unsigned short key;
289 | unsigned short addr;
290 | } tKeyLabel;
291 |
292 | #define DINA0A 0x0a
293 | #define DINA22 0x22
294 | #define DINA42 0x42
295 | #define DINA5A 0x5a
296 |
297 | #define DINA06 0x06
298 | #define DINA0E 0x0e
299 | #define DINA16 0x16
300 | #define DINA1E 0x1e
301 | #define DINA26 0x26
302 | #define DINA2E 0x2e
303 | #define DINA36 0x36
304 | #define DINA3E 0x3e
305 | #define DINA46 0x46
306 | #define DINA4E 0x4e
307 | #define DINA56 0x56
308 | #define DINA5E 0x5e
309 | #define DINA66 0x66
310 | #define DINA6E 0x6e
311 | #define DINA76 0x76
312 | #define DINA7E 0x7e
313 |
314 | #define DINA00 0x00
315 | #define DINA08 0x08
316 | #define DINA10 0x10
317 | #define DINA18 0x18
318 | #define DINA20 0x20
319 | #define DINA28 0x28
320 | #define DINA30 0x30
321 | #define DINA38 0x38
322 | #define DINA40 0x40
323 | #define DINA48 0x48
324 | #define DINA50 0x50
325 | #define DINA58 0x58
326 | #define DINA60 0x60
327 | #define DINA68 0x68
328 | #define DINA70 0x70
329 | #define DINA78 0x78
330 |
331 | #define DINA04 0x04
332 | #define DINA0C 0x0c
333 | #define DINA14 0x14
334 | #define DINA1C 0x1C
335 | #define DINA24 0x24
336 | #define DINA2C 0x2c
337 | #define DINA34 0x34
338 | #define DINA3C 0x3c
339 | #define DINA44 0x44
340 | #define DINA4C 0x4c
341 | #define DINA54 0x54
342 | #define DINA5C 0x5c
343 | #define DINA64 0x64
344 | #define DINA6C 0x6c
345 | #define DINA74 0x74
346 | #define DINA7C 0x7c
347 |
348 | #define DINA01 0x01
349 | #define DINA09 0x09
350 | #define DINA11 0x11
351 | #define DINA19 0x19
352 | #define DINA21 0x21
353 | #define DINA29 0x29
354 | #define DINA31 0x31
355 | #define DINA39 0x39
356 | #define DINA41 0x41
357 | #define DINA49 0x49
358 | #define DINA51 0x51
359 | #define DINA59 0x59
360 | #define DINA61 0x61
361 | #define DINA69 0x69
362 | #define DINA71 0x71
363 | #define DINA79 0x79
364 |
365 | #define DINA25 0x25
366 | #define DINA2D 0x2d
367 | #define DINA35 0x35
368 | #define DINA3D 0x3d
369 | #define DINA4D 0x4d
370 | #define DINA55 0x55
371 | #define DINA5D 0x5D
372 | #define DINA6D 0x6d
373 | #define DINA75 0x75
374 | #define DINA7D 0x7d
375 |
376 | #define DINADC 0xdc
377 | #define DINAF2 0xf2
378 | #define DINAAB 0xab
379 | #define DINAAA 0xaa
380 | #define DINAF1 0xf1
381 | #define DINADF 0xdf
382 | #define DINADA 0xda
383 | #define DINAB1 0xb1
384 | #define DINAB9 0xb9
385 | #define DINAF3 0xf3
386 | #define DINA8B 0x8b
387 | #define DINAA3 0xa3
388 | #define DINA91 0x91
389 | #define DINAB6 0xb6
390 | #define DINAB4 0xb4
391 |
392 |
393 | #define DINC00 0x00
394 | #define DINC01 0x01
395 | #define DINC02 0x02
396 | #define DINC03 0x03
397 | #define DINC08 0x08
398 | #define DINC09 0x09
399 | #define DINC0A 0x0a
400 | #define DINC0B 0x0b
401 | #define DINC10 0x10
402 | #define DINC11 0x11
403 | #define DINC12 0x12
404 | #define DINC13 0x13
405 | #define DINC18 0x18
406 | #define DINC19 0x19
407 | #define DINC1A 0x1a
408 | #define DINC1B 0x1b
409 |
410 | #define DINC20 0x20
411 | #define DINC21 0x21
412 | #define DINC22 0x22
413 | #define DINC23 0x23
414 | #define DINC28 0x28
415 | #define DINC29 0x29
416 | #define DINC2A 0x2a
417 | #define DINC2B 0x2b
418 | #define DINC30 0x30
419 | #define DINC31 0x31
420 | #define DINC32 0x32
421 | #define DINC33 0x33
422 | #define DINC38 0x38
423 | #define DINC39 0x39
424 | #define DINC3A 0x3a
425 | #define DINC3B 0x3b
426 |
427 | #define DINC40 0x40
428 | #define DINC41 0x41
429 | #define DINC42 0x42
430 | #define DINC43 0x43
431 | #define DINC48 0x48
432 | #define DINC49 0x49
433 | #define DINC4A 0x4a
434 | #define DINC4B 0x4b
435 | #define DINC50 0x50
436 | #define DINC51 0x51
437 | #define DINC52 0x52
438 | #define DINC53 0x53
439 | #define DINC58 0x58
440 | #define DINC59 0x59
441 | #define DINC5A 0x5a
442 | #define DINC5B 0x5b
443 |
444 | #define DINC60 0x60
445 | #define DINC61 0x61
446 | #define DINC62 0x62
447 | #define DINC63 0x63
448 | #define DINC68 0x68
449 | #define DINC69 0x69
450 | #define DINC6A 0x6a
451 | #define DINC6B 0x6b
452 | #define DINC70 0x70
453 | #define DINC71 0x71
454 | #define DINC72 0x72
455 | #define DINC73 0x73
456 | #define DINC78 0x78
457 | #define DINC79 0x79
458 | #define DINC7A 0x7a
459 | #define DINC7B 0x7b
460 |
461 | #define DIND40 0x40
462 |
463 |
464 | #define DINA80 0x80
465 | #define DINA90 0x90
466 | #define DINAA0 0xa0
467 | #define DINAC9 0xc9
468 | #define DINACB 0xcb
469 | #define DINACD 0xcd
470 | #define DINACF 0xcf
471 | #define DINAC8 0xc8
472 | #define DINACA 0xca
473 | #define DINACC 0xcc
474 | #define DINACE 0xce
475 | #define DINAD8 0xd8
476 | #define DINADD 0xdd
477 | #define DINAF8 0xf0
478 | #define DINAFE 0xfe
479 |
480 | #define DINBF8 0xf8
481 | #define DINAC0 0xb0
482 | #define DINAC1 0xb1
483 | #define DINAC2 0xb4
484 | #define DINAC3 0xb5
485 | #define DINAC4 0xb8
486 | #define DINAC5 0xb9
487 | #define DINBC0 0xc0
488 | #define DINBC2 0xc2
489 | #define DINBC4 0xc4
490 | #define DINBC6 0xc6
491 |
492 |
493 |
494 | #endif // DMPKEY_H__
495 |
--------------------------------------------------------------------------------
/src/include/dmpmap.h:
--------------------------------------------------------------------------------
1 | /*
2 | $License:
3 | Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
4 | $
5 | */
6 | #ifndef DMPMAP_H
7 | #define DMPMAP_H
8 |
9 | #ifdef __cplusplus
10 | extern "C"
11 | {
12 | #endif
13 |
14 | #define DMP_PTAT 0
15 | #define DMP_XGYR 2
16 | #define DMP_YGYR 4
17 | #define DMP_ZGYR 6
18 | #define DMP_XACC 8
19 | #define DMP_YACC 10
20 | #define DMP_ZACC 12
21 | #define DMP_ADC1 14
22 | #define DMP_ADC2 16
23 | #define DMP_ADC3 18
24 | #define DMP_BIASUNC 20
25 | #define DMP_FIFORT 22
26 | #define DMP_INVGSFH 24
27 | #define DMP_INVGSFL 26
28 | #define DMP_1H 28
29 | #define DMP_1L 30
30 | #define DMP_BLPFSTCH 32
31 | #define DMP_BLPFSTCL 34
32 | #define DMP_BLPFSXH 36
33 | #define DMP_BLPFSXL 38
34 | #define DMP_BLPFSYH 40
35 | #define DMP_BLPFSYL 42
36 | #define DMP_BLPFSZH 44
37 | #define DMP_BLPFSZL 46
38 | #define DMP_BLPFMTC 48
39 | #define DMP_SMC 50
40 | #define DMP_BLPFMXH 52
41 | #define DMP_BLPFMXL 54
42 | #define DMP_BLPFMYH 56
43 | #define DMP_BLPFMYL 58
44 | #define DMP_BLPFMZH 60
45 | #define DMP_BLPFMZL 62
46 | #define DMP_BLPFC 64
47 | #define DMP_SMCTH 66
48 | #define DMP_0H2 68
49 | #define DMP_0L2 70
50 | #define DMP_BERR2H 72
51 | #define DMP_BERR2L 74
52 | #define DMP_BERR2NH 76
53 | #define DMP_SMCINC 78
54 | #define DMP_ANGVBXH 80
55 | #define DMP_ANGVBXL 82
56 | #define DMP_ANGVBYH 84
57 | #define DMP_ANGVBYL 86
58 | #define DMP_ANGVBZH 88
59 | #define DMP_ANGVBZL 90
60 | #define DMP_BERR1H 92
61 | #define DMP_BERR1L 94
62 | #define DMP_ATCH 96
63 | #define DMP_BIASUNCSF 98
64 | #define DMP_ACT2H 100
65 | #define DMP_ACT2L 102
66 | #define DMP_GSFH 104
67 | #define DMP_GSFL 106
68 | #define DMP_GH 108
69 | #define DMP_GL 110
70 | #define DMP_0_5H 112
71 | #define DMP_0_5L 114
72 | #define DMP_0_0H 116
73 | #define DMP_0_0L 118
74 | #define DMP_1_0H 120
75 | #define DMP_1_0L 122
76 | #define DMP_1_5H 124
77 | #define DMP_1_5L 126
78 | #define DMP_TMP1AH 128
79 | #define DMP_TMP1AL 130
80 | #define DMP_TMP2AH 132
81 | #define DMP_TMP2AL 134
82 | #define DMP_TMP3AH 136
83 | #define DMP_TMP3AL 138
84 | #define DMP_TMP4AH 140
85 | #define DMP_TMP4AL 142
86 | #define DMP_XACCW 144
87 | #define DMP_TMP5 146
88 | #define DMP_XACCB 148
89 | #define DMP_TMP8 150
90 | #define DMP_YACCB 152
91 | #define DMP_TMP9 154
92 | #define DMP_ZACCB 156
93 | #define DMP_TMP10 158
94 | #define DMP_DZH 160
95 | #define DMP_DZL 162
96 | #define DMP_XGCH 164
97 | #define DMP_XGCL 166
98 | #define DMP_YGCH 168
99 | #define DMP_YGCL 170
100 | #define DMP_ZGCH 172
101 | #define DMP_ZGCL 174
102 | #define DMP_YACCW 176
103 | #define DMP_TMP7 178
104 | #define DMP_AFB1H 180
105 | #define DMP_AFB1L 182
106 | #define DMP_AFB2H 184
107 | #define DMP_AFB2L 186
108 | #define DMP_MAGFBH 188
109 | #define DMP_MAGFBL 190
110 | #define DMP_QT1H 192
111 | #define DMP_QT1L 194
112 | #define DMP_QT2H 196
113 | #define DMP_QT2L 198
114 | #define DMP_QT3H 200
115 | #define DMP_QT3L 202
116 | #define DMP_QT4H 204
117 | #define DMP_QT4L 206
118 | #define DMP_CTRL1H 208
119 | #define DMP_CTRL1L 210
120 | #define DMP_CTRL2H 212
121 | #define DMP_CTRL2L 214
122 | #define DMP_CTRL3H 216
123 | #define DMP_CTRL3L 218
124 | #define DMP_CTRL4H 220
125 | #define DMP_CTRL4L 222
126 | #define DMP_CTRLS1 224
127 | #define DMP_CTRLSF1 226
128 | #define DMP_CTRLS2 228
129 | #define DMP_CTRLSF2 230
130 | #define DMP_CTRLS3 232
131 | #define DMP_CTRLSFNLL 234
132 | #define DMP_CTRLS4 236
133 | #define DMP_CTRLSFNL2 238
134 | #define DMP_CTRLSFNL 240
135 | #define DMP_TMP30 242
136 | #define DMP_CTRLSFJT 244
137 | #define DMP_TMP31 246
138 | #define DMP_TMP11 248
139 | #define DMP_CTRLSF2_2 250
140 | #define DMP_TMP12 252
141 | #define DMP_CTRLSF1_2 254
142 | #define DMP_PREVPTAT 256
143 | #define DMP_ACCZB 258
144 | #define DMP_ACCXB 264
145 | #define DMP_ACCYB 266
146 | #define DMP_1HB 272
147 | #define DMP_1LB 274
148 | #define DMP_0H 276
149 | #define DMP_0L 278
150 | #define DMP_ASR22H 280
151 | #define DMP_ASR22L 282
152 | #define DMP_ASR6H 284
153 | #define DMP_ASR6L 286
154 | #define DMP_TMP13 288
155 | #define DMP_TMP14 290
156 | #define DMP_FINTXH 292
157 | #define DMP_FINTXL 294
158 | #define DMP_FINTYH 296
159 | #define DMP_FINTYL 298
160 | #define DMP_FINTZH 300
161 | #define DMP_FINTZL 302
162 | #define DMP_TMP1BH 304
163 | #define DMP_TMP1BL 306
164 | #define DMP_TMP2BH 308
165 | #define DMP_TMP2BL 310
166 | #define DMP_TMP3BH 312
167 | #define DMP_TMP3BL 314
168 | #define DMP_TMP4BH 316
169 | #define DMP_TMP4BL 318
170 | #define DMP_STXG 320
171 | #define DMP_ZCTXG 322
172 | #define DMP_STYG 324
173 | #define DMP_ZCTYG 326
174 | #define DMP_STZG 328
175 | #define DMP_ZCTZG 330
176 | #define DMP_CTRLSFJT2 332
177 | #define DMP_CTRLSFJTCNT 334
178 | #define DMP_PVXG 336
179 | #define DMP_TMP15 338
180 | #define DMP_PVYG 340
181 | #define DMP_TMP16 342
182 | #define DMP_PVZG 344
183 | #define DMP_TMP17 346
184 | #define DMP_MNMFLAGH 352
185 | #define DMP_MNMFLAGL 354
186 | #define DMP_MNMTMH 356
187 | #define DMP_MNMTML 358
188 | #define DMP_MNMTMTHRH 360
189 | #define DMP_MNMTMTHRL 362
190 | #define DMP_MNMTHRH 364
191 | #define DMP_MNMTHRL 366
192 | #define DMP_ACCQD4H 368
193 | #define DMP_ACCQD4L 370
194 | #define DMP_ACCQD5H 372
195 | #define DMP_ACCQD5L 374
196 | #define DMP_ACCQD6H 376
197 | #define DMP_ACCQD6L 378
198 | #define DMP_ACCQD7H 380
199 | #define DMP_ACCQD7L 382
200 | #define DMP_ACCQD0H 384
201 | #define DMP_ACCQD0L 386
202 | #define DMP_ACCQD1H 388
203 | #define DMP_ACCQD1L 390
204 | #define DMP_ACCQD2H 392
205 | #define DMP_ACCQD2L 394
206 | #define DMP_ACCQD3H 396
207 | #define DMP_ACCQD3L 398
208 | #define DMP_XN2H 400
209 | #define DMP_XN2L 402
210 | #define DMP_XN1H 404
211 | #define DMP_XN1L 406
212 | #define DMP_YN2H 408
213 | #define DMP_YN2L 410
214 | #define DMP_YN1H 412
215 | #define DMP_YN1L 414
216 | #define DMP_YH 416
217 | #define DMP_YL 418
218 | #define DMP_B0H 420
219 | #define DMP_B0L 422
220 | #define DMP_A1H 424
221 | #define DMP_A1L 426
222 | #define DMP_A2H 428
223 | #define DMP_A2L 430
224 | #define DMP_SEM1 432
225 | #define DMP_FIFOCNT 434
226 | #define DMP_SH_TH_X 436
227 | #define DMP_PACKET 438
228 | #define DMP_SH_TH_Y 440
229 | #define DMP_FOOTER 442
230 | #define DMP_SH_TH_Z 444
231 | #define DMP_TEMP29 448
232 | #define DMP_TEMP30 450
233 | #define DMP_XACCB_PRE 452
234 | #define DMP_XACCB_PREL 454
235 | #define DMP_YACCB_PRE 456
236 | #define DMP_YACCB_PREL 458
237 | #define DMP_ZACCB_PRE 460
238 | #define DMP_ZACCB_PREL 462
239 | #define DMP_TMP22 464
240 | #define DMP_TAP_TIMER 466
241 | #define DMP_TAP_THX 468
242 | #define DMP_TAP_THY 472
243 | #define DMP_TAP_THZ 476
244 | #define DMP_TAPW_MIN 478
245 | #define DMP_TMP25 480
246 | #define DMP_TMP26 482
247 | #define DMP_TMP27 484
248 | #define DMP_TMP28 486
249 | #define DMP_ORIENT 488
250 | #define DMP_THRSH 490
251 | #define DMP_ENDIANH 492
252 | #define DMP_ENDIANL 494
253 | #define DMP_BLPFNMTCH 496
254 | #define DMP_BLPFNMTCL 498
255 | #define DMP_BLPFNMXH 500
256 | #define DMP_BLPFNMXL 502
257 | #define DMP_BLPFNMYH 504
258 | #define DMP_BLPFNMYL 506
259 | #define DMP_BLPFNMZH 508
260 | #define DMP_BLPFNMZL 510
261 | #ifdef __cplusplus
262 | }
263 | #endif
264 | #endif // DMPMAP_H
265 |
--------------------------------------------------------------------------------
/src/include/exti.h:
--------------------------------------------------------------------------------
1 | #ifndef __EXTI_H
2 | #define __EXTI_H
3 |
4 | #include "common.h"
5 | #include "types.h"
6 | #include
7 |
8 | #define CONFIG_EXTI_MAX_NUM 16 /**< Maximum external line channels. */
9 | #define CONFIG_EXIT_IRQ_NUM 7 /**< External irq number, irq EXTI0~EXTI4, EXTI9-5, EXTI15-10 */
10 |
11 | /**
12 | @brief get gpio port source by port id.
13 | @param port - gpio port id.
14 | @return return gpio source number.
15 | */
16 | static _u8 GPIO_PortSource(GPIO_TypeDef *port)
17 | {
18 | if (port == NULL) {
19 | return 0;
20 | }
21 | if (port == GPIOA) {
22 | return GPIO_PortSourceGPIOA;
23 | } else if (port == GPIOB) {
24 | return GPIO_PortSourceGPIOB;
25 | } else if (port == GPIOC) {
26 | return GPIO_PortSourceGPIOC;
27 | } else if (port == GPIOD) {
28 | return GPIO_PortSourceGPIOD;
29 | } else if (port == GPIOE) {
30 | return GPIO_PortSourceGPIOE;
31 | } else if (port == GPIOF) {
32 | return GPIO_PortSourceGPIOF;
33 | } else if (port == GPIOG) {
34 | return GPIO_PortSourceGPIOG;
35 | } else {
36 | return 0;
37 | }
38 | }
39 |
40 | #define EXTI_LINE(l) (EXTI_Line0 << (l))
41 |
42 | /**
43 | @brief External line interrupt call back function.
44 | */
45 | typedef void (*exti_cb_t)(void);
46 |
47 | bool exti_reg_callback(_u8 exti, EXTITrigger_TypeDef type, exti_cb_t cb);
48 | void exti_unreg_callback(_u8 exti);
49 |
50 | #endif
51 |
--------------------------------------------------------------------------------
/src/include/filters.h:
--------------------------------------------------------------------------------
1 | #pragma once
2 | #include "common.h"
3 | extern float angle, angle_dot;
4 | void Kalman_Filter(float Accel, float Gyro);
5 | _u16 add_to_avg_filter_u16(_u16 new_data, _u16 * filter, size_t pos, size_t size);
6 | _s16 add_to_avg_filter_s16(_s16 new_data, _s16 * filter, size_t pos, size_t size);
7 | _u32 add_to_avg_filter_u32(_u32 new_data, _u32 * filter, size_t pos, size_t size);
8 |
--------------------------------------------------------------------------------
/src/include/inv_mpu.h:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/zsirui/mrobot/5f7474090a7d357fdad973d72531111f9844cb04/src/include/inv_mpu.h
--------------------------------------------------------------------------------
/src/include/inv_mpu_dmp_motion_driver.h:
--------------------------------------------------------------------------------
1 | /*
2 | $License:
3 | Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
4 | See included License.txt for License information.
5 | $
6 | */
7 | /**
8 | * @addtogroup DRIVERS Sensor Driver Layer
9 | * @brief Hardware drivers to communicate with sensors via I2C.
10 | *
11 | * @{
12 | * @file inv_mpu_dmp_motion_driver.h
13 | * @brief DMP image and interface functions.
14 | * @details All functions are preceded by the dmp_ prefix to
15 | * differentiate among MPL and general driver function calls.
16 | */
17 | #ifndef _INV_MPU_DMP_MOTION_DRIVER_H_
18 | #define _INV_MPU_DMP_MOTION_DRIVER_H_
19 |
20 | #define TAP_X (0x01)
21 | #define TAP_Y (0x02)
22 | #define TAP_Z (0x04)
23 | #define TAP_XYZ (0x07)
24 |
25 | #define TAP_X_UP (0x01)
26 | #define TAP_X_DOWN (0x02)
27 | #define TAP_Y_UP (0x03)
28 | #define TAP_Y_DOWN (0x04)
29 | #define TAP_Z_UP (0x05)
30 | #define TAP_Z_DOWN (0x06)
31 |
32 | #define ANDROID_ORIENT_PORTRAIT (0x00)
33 | #define ANDROID_ORIENT_LANDSCAPE (0x01)
34 | #define ANDROID_ORIENT_REVERSE_PORTRAIT (0x02)
35 | #define ANDROID_ORIENT_REVERSE_LANDSCAPE (0x03)
36 |
37 | #define DMP_INT_GESTURE (0x01)
38 | #define DMP_INT_CONTINUOUS (0x02)
39 |
40 | #define DMP_FEATURE_TAP (0x001)
41 | #define DMP_FEATURE_ANDROID_ORIENT (0x002)
42 | #define DMP_FEATURE_LP_QUAT (0x004)
43 | #define DMP_FEATURE_PEDOMETER (0x008)
44 | #define DMP_FEATURE_6X_LP_QUAT (0x010)
45 | #define DMP_FEATURE_GYRO_CAL (0x020)
46 | #define DMP_FEATURE_SEND_RAW_ACCEL (0x040)
47 | #define DMP_FEATURE_SEND_RAW_GYRO (0x080)
48 | #define DMP_FEATURE_SEND_CAL_GYRO (0x100)
49 |
50 | #define INV_WXYZ_QUAT (0x100)
51 |
52 | /* Set up functions. */
53 | int dmp_load_motion_driver_firmware(void);
54 | int dmp_set_fifo_rate(unsigned short rate);
55 | int dmp_get_fifo_rate(unsigned short *rate);
56 | int dmp_enable_feature(unsigned short mask);
57 | int dmp_get_enabled_features(unsigned short *mask);
58 | int dmp_set_interrupt_mode(unsigned char mode);
59 | int dmp_set_orientation(unsigned short orient);
60 | int dmp_set_gyro_bias(long *bias);
61 | int dmp_set_accel_bias(long *bias);
62 |
63 | /* Tap functions. */
64 | int dmp_register_tap_cb(void (*func)(unsigned char, unsigned char));
65 | int dmp_set_tap_thresh(unsigned char axis, unsigned short thresh);
66 | int dmp_set_tap_axes(unsigned char axis);
67 | int dmp_set_tap_count(unsigned char min_taps);
68 | int dmp_set_tap_time(unsigned short time);
69 | int dmp_set_tap_time_multi(unsigned short time);
70 | int dmp_set_shake_reject_thresh(long sf, unsigned short thresh);
71 | int dmp_set_shake_reject_time(unsigned short time);
72 | int dmp_set_shake_reject_timeout(unsigned short time);
73 |
74 | /* Android orientation functions. */
75 | int dmp_register_android_orient_cb(void (*func)(unsigned char));
76 |
77 | /* LP quaternion functions. */
78 | int dmp_enable_lp_quat(unsigned char enable);
79 | int dmp_enable_6x_lp_quat(unsigned char enable);
80 |
81 | /* Pedometer functions. */
82 | int dmp_get_pedometer_step_count(unsigned long *count);
83 | int dmp_set_pedometer_step_count(unsigned long count);
84 | int dmp_get_pedometer_walk_time(unsigned long *time);
85 | int dmp_set_pedometer_walk_time(unsigned long time);
86 |
87 | /* DMP gyro calibration functions. */
88 | int dmp_enable_gyro_cal(unsigned char enable);
89 |
90 | /* Read function. This function should be called whenever the MPU interrupt is
91 | * detected.
92 | */
93 | int dmp_read_fifo(short *gyro, short *accel, long *quat,
94 | unsigned long *timestamp, short *sensors, unsigned char *more);
95 |
96 | #endif /* #ifndef _INV_MPU_DMP_MOTION_DRIVER_H_ */
97 |
98 |
--------------------------------------------------------------------------------
/src/include/led.h:
--------------------------------------------------------------------------------
1 | #ifndef _LED_H_
2 | #define _LED_H_
3 |
4 | #ifdef _RGB
5 | #define LED_COLOR_PORT GPIOA
6 | #define LED_COLOR_RED GPIO_Pin_8
7 | #define LED_COLOR_GREEN GPIO_Pin_9
8 | #define LED_COLOR_BLUE GPIO_Pin_12
9 | #define LED_COLOR_PINs LED_COLOR_RED | LED_COLOR_GREEN | LED_COLOR_BLUE
10 | #else
11 | #define LED_COLOR_PORT GPIOB
12 | #define LED_COLOR_PIN GPIO_Pin_12
13 | #endif
14 |
15 | int drv_led_init(void);
16 | void drv_led_shutdown(void);
17 | void drv_led_set(int r, int g, int b);
18 | void led_heartbeat(float n);
19 | #endif
20 |
--------------------------------------------------------------------------------
/src/include/motor.h:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/zsirui/mrobot/5f7474090a7d357fdad973d72531111f9844cb04/src/include/motor.h
--------------------------------------------------------------------------------
/src/include/mpu6050.h:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/zsirui/mrobot/5f7474090a7d357fdad973d72531111f9844cb04/src/include/mpu6050.h
--------------------------------------------------------------------------------
/src/include/mpuiic.h:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/zsirui/mrobot/5f7474090a7d357fdad973d72531111f9844cb04/src/include/mpuiic.h
--------------------------------------------------------------------------------
/src/include/protocol.h:
--------------------------------------------------------------------------------
1 | #ifndef _PROTOCOL_H_
2 | #define _PROTOCOL_H_
3 | #include "types.h"
4 | #include "stm32f10x.h"
5 |
6 | void SendData(short Temperature, float pitch, float roll, float yaw);
7 | void ReceiveData(short Temperature, float pitch, float roll, float yaw);
8 |
9 | #endif
10 |
--------------------------------------------------------------------------------
/src/include/rpstm32.h:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #ifndef RPSTM32_HEADER_H
4 | #define RPSTM32_HEADER_H
5 |
6 | #define EXPAND_WRAPPER( __NEXTLEVEL__, ...) __NEXTLEVEL__( __VA_ARGS__ )
7 |
8 | // Helper Macros
9 |
10 | #define _GET_ADC_PERIPH(x) RCC_APB2Periph_ADC##x
11 | #define _GET_TIM(x) TIM##x
12 | #define GET_ADC_PERIPH(x) EXPAND_WRAPPER(_GET_ADC_PERIPH,x)
13 | #define GET_TIM(x) EXPAND_WRAPPER(_GET_TIM,x)
14 |
15 | // ST-FWlib
16 | #include "stm32f10x.h"
17 |
18 | // Lib-c
19 | #include
20 | #include
21 | #include
22 |
23 | #include "types.h"
24 |
25 | // Board Def
26 | #include "boarddef.h"
27 |
28 | // Board HAL
29 | #include "boardfunc.h"
30 |
31 | #endif // #ifndef RPSTM32_HEADER_H
32 |
--------------------------------------------------------------------------------
/src/include/softdelay.h:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | void softdelay_calibrate(void);
4 | void _delay_us(volatile uint32_t us);
5 |
--------------------------------------------------------------------------------
/src/include/sonar.h:
--------------------------------------------------------------------------------
1 | #include
2 | #include
3 | #include "common.h"
4 |
5 | /**
6 | @addtogroup sonar
7 | @{
8 | */
9 |
10 | #ifndef _SONAR_H
11 | #define _SONAR_H
12 | extern short Temperature;
13 | //extern int _SONAR;
14 | /**
15 | @brief ultrasonic speed coefficient is different by temperature.
16 |
17 | Tempearture -30 -20 -10 0 10 20 30 100
18 | Speed(m/s) 313 319 325 332 338 344 349 386
19 |
20 | V = 331.45 + 0.607 * T
21 | S = V * t / 2
22 | FIXME: use tempearture sensor to select speed coefficient.
23 | */
24 | #define CONFIG_SONAR_COE_A 331.45 /*< Sonar distance coefficient A. */
25 | #define CONFIG_SONAR_COE_B 0.607 /*< Sonar distance coefficient B. */
26 | //#define CONFIG_SONAR_COE_T 20 /*< Sonar distance coefficient T at 20. */
27 |
28 | #define CONFIG_SONAR_CHANNEL_NUM 4 /**< Sonar channel number. */
29 | #define CONFIG_SONAR_SAMPLE_SIZE 4 /**< Sonar sample buffer size. */
30 | #define CONFIG_SONAR_TIMEOUT_MS 50 /**< Sonar detection timeout in ms. */
31 | #define CONFIG_SONAR_TICKS 100 /**< Sonar detection timeout in ms. */
32 |
33 | #define CONFIG_SONAR_BLOCK_DIST 100 /**< Sonar block distance in mm. */
34 |
35 | #define SONAR_TRIG1_PORT GPIOB
36 | #define SONAR_TRIG1_PIN GPIO_Pin_0
37 |
38 | #define SONAR_ECHO1_PORT GPIOB
39 | #define SONAR_ECHO1_PIN GPIO_Pin_1
40 |
41 | #define SONAR_TRIG2_PORT GPIOB
42 | #define SONAR_TRIG2_PIN GPIO_Pin_3
43 |
44 | #define SONAR_ECHO2_PORT GPIOB
45 | #define SONAR_ECHO2_PIN GPIO_Pin_4
46 |
47 | #define SONAR_TRIG3_PORT GPIOB
48 | #define SONAR_TRIG3_PIN GPIO_Pin_8
49 |
50 | #define SONAR_ECHO3_PORT GPIOB
51 | #define SONAR_ECHO3_PIN GPIO_Pin_9
52 |
53 | #define SONAR_TRIG4_PORT GPIOB
54 | #define SONAR_TRIG4_PIN GPIO_Pin_14
55 |
56 | #define SONAR_ECHO4_PORT GPIOB
57 | #define SONAR_ECHO4_PIN GPIO_Pin_15
58 |
59 | #if !defined(_countof)
60 | #define _countof(_Array) (sizeof(_Array) / sizeof(_Array[0]))
61 | #endif
62 |
63 | /**
64 | @brief sonar channel info structure.
65 | */
66 | typedef struct _sonar_channel {
67 | uint8_t state; /**< Sonar sensor state. */
68 | uint8_t id; /**< Sonar smaple buffer id. */
69 | uint8_t cnt; /**< Sonar smaple count. */
70 | uint16_t sample[CONFIG_SONAR_SAMPLE_SIZE]; /**< Sonar sample buffer. */
71 | uint32_t ticks; /**< Sonar sample ticks. */
72 | float distance; /**< Sonar sampel result. */
73 | } sonar_channel_t;
74 |
75 | /**
76 | @brief sonar channel configuration.
77 | */
78 | typedef struct _sonar_cfg {
79 | GPIO_TypeDef *trig_port; /**< Sonar channel trigger IO port. */
80 | uint16_t trig_pin; /**< Sonar channel tirgger IO pin. */
81 | GPIO_TypeDef *echo_port; /**< Sonar channel echo input IO port. */
82 | uint16_t echo_pin; /**< Sonar channel echo input IO pin. */
83 | uint32_t exti_line; /**< Sonar channel echo input interrupt line. */
84 | } sonar_cfg_t;
85 |
86 | /**
87 | @brief Sonar sensor states.
88 | */
89 | enum _sonar_state {
90 | SONAR_IDLE = 0,
91 | SONAR_INIT,
92 | SONAR_MEASURE,
93 | SONAR_DONE,
94 | SONAR_EXIT,
95 | };
96 |
97 | int sonar_init(void);
98 | void sonar_exit(void);
99 | void sonar_heartbeat(void);
100 | uint32_t sonar_get(uint8_t ch);
101 | float sonar_distance(uint8_t ch);
102 |
103 | #endif
104 |
105 | /** @} */
106 |
--------------------------------------------------------------------------------
/src/include/stm32f10x_it.h:
--------------------------------------------------------------------------------
1 | /* Define to prevent recursive inclusion -------------------------------------*/
2 | #ifndef __STM32F10x_IT_H
3 | #define __STM32F10x_IT_H
4 |
5 | /* Includes ------------------------------------------------------------------*/
6 | #include "stm32f10x.h"
7 |
8 | /* Exported types ------------------------------------------------------------*/
9 | /* Exported constants --------------------------------------------------------*/
10 | /* Exported macro ------------------------------------------------------------*/
11 | /* Exported functions ------------------------------------------------------- */
12 |
13 | void NMI_Handler(void);
14 | void HardFault_Handler(void);
15 | void MemManage_Handler(void);
16 | void BusFault_Handler(void);
17 | void UsageFault_Handler(void);
18 | void SVC_Handler(void);
19 | void DebugMon_Handler(void);
20 | void PendSV_Handler(void);
21 | void SysTick_Handler(void);
22 |
23 | #endif /* __STM32F10x_IT_H */
24 |
--------------------------------------------------------------------------------
/src/include/sys.h:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/zsirui/mrobot/5f7474090a7d357fdad973d72531111f9844cb04/src/include/sys.h
--------------------------------------------------------------------------------
/src/include/types.h:
--------------------------------------------------------------------------------
1 | #ifndef _TYPES_H_
2 | #define _TYPES_H_
3 |
4 | #include
5 |
6 | //based on stdint.h
7 | typedef int8_t _s8;
8 | typedef uint8_t _u8;
9 |
10 | typedef int16_t _s16;
11 | typedef uint16_t _u16;
12 |
13 | typedef int32_t _s32;
14 | typedef uint32_t _u32;
15 |
16 | typedef int64_t _s64;
17 | typedef uint64_t _u64;
18 |
19 | #endif
20 |
--------------------------------------------------------------------------------
/src/include/usart.h:
--------------------------------------------------------------------------------
1 | #pragma once
2 | #include "common.h"
3 |
4 | #define USART_MAX_LEN 200 //定义最大接收字节数 200
5 |
6 | extern u8 send_data[USART_MAX_LEN];
7 | extern u8 USART3_RX_BUF[USART_MAX_LEN]; //接收缓冲,最大USART_MAX_LEN个字节.末字节为换行符
8 | extern u16 USART3_RX_STA; //接收状态标记
9 | extern u8 Res3;
10 | extern u8 USART2_RX_BUF[USART_MAX_LEN]; //接收缓冲,最大USART_MAX_LEN个字节.末字节为换行符
11 | extern u16 USART2_RX_STA; //接收状态标记
12 | extern u8 Res2;
13 |
14 | #define USART2_PORT GPIOA
15 | #define USART2_TX GPIO_Pin_2
16 | #define USART2_RX GPIO_Pin_3
17 | #ifdef _REMAP
18 | #define USART3_PORT GPIOC
19 | #else
20 | #define USART3_PORT GPIOB
21 | #endif
22 | #define USART3_TX GPIO_Pin_10
23 | #define USART3_RX GPIO_Pin_11
24 | #define UART4_PORT GPIOC
25 | #define UART4_TX GPIO_Pin_10
26 | #define UART4_RX GPIO_Pin_11
27 |
28 | int usart2_init(u32 bound);
29 | int usart3_init(u32 bound);
30 | #ifdef _DMA_USART
31 | //int usart3_dma_init(u32 bound);
32 | void DMA_Enable(DMA_Channel_TypeDef * DMA_CHx);
33 | #endif
34 | #ifndef _REMAP
35 | extern u8 UART4_RX_BUF[USART_MAX_LEN]; //接收缓冲,最大USART_MAX_LEN个字节.末字节为换行符
36 | extern u16 UART4_RX_STA; //接收状态标记
37 | extern u8 Res4;
38 | int uart4_init(u32 bound);
39 | #endif
40 |
--------------------------------------------------------------------------------
/src/include/watchdog.h:
--------------------------------------------------------------------------------
1 | #pragma once
2 | #include "common.h"
3 |
4 | void enable_watchdog(void);
5 | void disable_watchdog(void);
6 | void mark_watchdog(void);
7 |
--------------------------------------------------------------------------------
/src/main.c:
--------------------------------------------------------------------------------
1 | #include "common.h"
2 | #include
3 | #include
4 | #include "beep.h"
5 | #include "battery.h"
6 | #include "motor.h"
7 | #include "watchdog.h"
8 | #include "led.h"
9 | #include "sonar.h"
10 | #include "mpu6050.h"
11 | #include "inv_mpu.h"
12 | #include "filters.h"
13 | #include "protocol.h"
14 |
15 | #define PI 3.14159265358979f
16 | #define fastAbs(n) n > 0 ? n : -n
17 |
18 | int _MPU6050 = 1, _MPU6050_DMP = 1, _MOTOR, _USART3, _LED, _BATTERY, _BEEP, _SONAR;
19 | short Temperature; //温度
20 | float pitch, roll, yaw; //欧拉角
21 |
22 | /*
23 | * 初始化板级外设函数
24 | */
25 | static _s32 init_dev(void)
26 | {
27 | //#ifdef _PRINT
28 | // int _USART2;
29 | // _USART2 = usart2_init(115200);
30 | //#endif
31 | _USART3 = usart3_init(115200);
32 | _LED = drv_led_init();
33 | _BATTERY = init_battery(); //初始化电池电量、充电相关
34 | _BEEP = init_beep(); //初始化蜂鸣器,用于各种声音提示
35 | _MOTOR = InitMotor(); //初始化两路行走电机,输出的速度分辨率为-300 ~ 300
36 | stalldetector_init();
37 | _SONAR = sonar_init();
38 | #ifdef _ENABLE_MPU6050
39 | _MPU6050 = MPU_Init();
40 | _MPU6050_DMP = mpu_dmp_init();
41 | #endif
42 | set_walkingmotor_speed(0, 0); //速度设定 左:0mm/s 右:0mm/s
43 | return 1;
44 | }
45 |
46 | extern _u8 sdp_status;
47 | /*
48 | * 模块循环处理函数
49 | */
50 | static void dev_heartbeat(void)
51 | {
52 | #ifdef _ENABLE_MPU6050
53 | int get_data;
54 | short aacx, aacy, aacz; //加速度传感器原始数据
55 | short gyrox, gyroy, gyroz; //陀螺仪原始数据
56 | #ifdef _PRINT
57 | printf("_MPU6050: %d\r\n", _MPU6050);
58 | printf("_MPU6050_DMP: %d\r\n", _MPU6050_DMP);
59 | #endif
60 | if(_MPU6050 == 0 && _MPU6050_DMP == 0)
61 | {
62 | get_data = mpu_dmp_get_data(&pitch, &roll, &yaw);
63 | #ifdef _PRINT
64 | printf("get_data: %d\r\n", get_data);
65 | #endif
66 | if(get_data == 0)
67 | {
68 | Temperature = MPU_Get_Temperature(); //得到温度值
69 | MPU_Get_Accelerometer(&aacx, &aacy, &aacz); //得到加速度传感器数据
70 | MPU_Get_Gyroscope(&gyrox, &gyroy, &gyroz); //得到陀螺仪数据
71 | Kalman_Filter(atan2(aacx, aacy) * 180 / PI, -(float)gyroz /16.4);
72 | #ifdef _PRINT
73 | printf("\r\nAccelerometer: x: %.6f, y: %.6f, z: %.6f\r\n\r\n", (float)aacx / 16384, (float)aacy / 16384, (float)aacz / 16384);
74 | printf("Gyroscope: x: %.6f, y: %.6f, z: %.6f\r\n\r\n", (float)gyrox / 16.4, (float)gyroy / 16.4, (float)gyroz / 16.4);
75 | printf("Pitch: %.2f, Roll: %.2f, Yaw: %.2f\r\n\r\n", pitch, roll, yaw);
76 | printf("Kalman Filter Yaw: %.2f\r\n\r\n", angle);
77 | printf("Temperature: %.2f\r\n\r\n", (float)Temperature / 100);
78 | #endif
79 | }
80 | }
81 | #endif
82 | heartbeat_battery();
83 | heartbeat_beep(get_electricitypercentage());
84 | ReceiveData(Temperature, pitch, roll, yaw);
85 | speedctl_heartbeat();
86 | stalldetector_heartbeat();
87 | sonar_heartbeat();
88 | #ifdef _PRINT
89 | printf("Voltage of battery: %.2f mV\r\n", get_electricity());
90 | printf("Percentage of battery: %.2f%%\r\n", get_electricitypercentage());
91 | #endif
92 | }
93 |
94 | /*
95 | * 主循环函数
96 | */
97 | static _s32 loop(void)
98 | {
99 | led_heartbeat(get_electricitypercentage());
100 | dev_heartbeat(); //处理各个功能模块
101 | return 1;
102 | }
103 | /*
104 | * 主程序函数
105 | */
106 | int main(void)
107 | {
108 | _delay_ms(100); //等待上电电源稳定
109 |
110 | init_board(); //MCU低级初始化
111 | if (!init_dev())
112 | { //所有外设初始化
113 | goto _on_fail;
114 | }
115 |
116 | play_poweron(); //开机发声
117 | enable_watchdog();
118 |
119 | while (loop())
120 | {
121 | mark_watchdog();
122 | }
123 |
124 | _on_fail:
125 | disable_watchdog();
126 | drv_led_shutdown();
127 | set_walkingmotor_speed(0, 0);
128 | cli();
129 | on_abort_mode();
130 | return 0;
131 | }
132 |
--------------------------------------------------------------------------------
/src/utils/filters.c:
--------------------------------------------------------------------------------
1 | #include "common.h"
2 | #include "filters.h"
3 | float angle, angle_dot;
4 | float Q_angle = 0.001;
5 | float Q_gyro = 0.003;
6 | float R_angle = 0.5;
7 | float dt = 0.005;
8 | char C_0 = 1;
9 | float Q_bias, Angle_err;
10 | float PCt_0, PCt_1, E;
11 | float K_0, K_1, t_0, t_1;
12 | float Pdot[4] = {0,0,0,0};
13 | float PP[2][2] = { { 1, 0 },{ 0, 1 } };
14 |
15 | void Kalman_Filter(float Accel, float Gyro)
16 | {
17 | angle+=(Gyro - Q_bias) * dt;
18 | Pdot[0]=Q_angle - PP[0][1] - PP[1][0];
19 |
20 | Pdot[1]=-PP[1][1];
21 | Pdot[2]=-PP[1][1];
22 | Pdot[3]=Q_gyro;
23 | PP[0][0] += Pdot[0] * dt;
24 | PP[0][1] += Pdot[1] * dt;
25 | PP[1][0] += Pdot[2] * dt;
26 | PP[1][1] += Pdot[3] * dt;
27 |
28 | Angle_err = Accel - angle;
29 |
30 | PCt_0 = C_0 * PP[0][0];
31 | PCt_1 = C_0 * PP[1][0];
32 |
33 | E = R_angle + C_0 * PCt_0;
34 |
35 | K_0 = PCt_0 / E;
36 | K_1 = PCt_1 / E;
37 |
38 | t_0 = PCt_0;
39 | t_1 = C_0 * PP[0][1];
40 |
41 | PP[0][0] -= K_0 * t_0;
42 | PP[0][1] -= K_0 * t_1;
43 | PP[1][0] -= K_1 * t_0;
44 | PP[1][1] -= K_1 * t_1;
45 |
46 | angle += K_0 * Angle_err;
47 | Q_bias += K_1 * Angle_err;
48 | angle_dot = Gyro - Q_bias;
49 | }
50 | /*
51 | * 平均滤波函数
52 | * 将新数据放入滤波器再求平均
53 | * 数据类型:unsigned short
54 | */
55 | _u16 add_to_avg_filter_u16(_u16 new_data, _u16 * filter, size_t pos, size_t size)
56 | {
57 | _s32 * sum = (_s32 *)(filter + size);
58 |
59 | *sum -= (_s32)filter[pos];
60 | *sum += (_s32)new_data;
61 | filter[pos] = new_data;
62 |
63 | return (_u16)((*sum)/size);
64 | }
65 |
66 | /*
67 | * 平均滤波函数
68 | * 将新数据放入滤波器再求平均
69 | * 数据类型:short
70 | */
71 | _s16 add_to_avg_filter_s16(_s16 new_data, _s16 * filter, size_t pos, size_t size)
72 | {
73 | _s32 * sum = (_s32 *)(filter + size);
74 |
75 | *sum -= (_s32)filter[pos];
76 | *sum += (_s32)new_data;
77 | filter[pos] = new_data;
78 |
79 | return (_s16)((*sum)/((_s32)size));
80 | }
81 |
82 | /*
83 | * 平均滤波函数
84 | * 将新数据放入滤波器再求平均
85 | * 数据类型:unsigned int
86 | */
87 | _u32 add_to_avg_filter_u32(_u32 new_data, _u32 * filter, size_t pos, size_t size)
88 | {
89 | _s64 * sum = (_s64 *)(filter + size);
90 |
91 | *sum -= (_s64)filter[pos];
92 | *sum += (_s64)new_data;
93 | filter[pos] = new_data;
94 |
95 | return (_u32)((*sum)/size);
96 | }
97 |
--------------------------------------------------------------------------------
/src/utils/hwconf.c:
--------------------------------------------------------------------------------
1 | #include "common.h"
2 |
3 | static void RCC_Configuration(void);
4 | static void NVIC_Configuration(void);
5 | static void ADC_Configuration(void);
6 |
7 | /*
8 | * 系统时钟设定函数,时间:1ms
9 | */
10 | static void set_board_systick()
11 | {
12 | SysTick->LOAD = SYSTICK_1MS_TICKS - 1;
13 | NVIC_SetPriority(SysTick_IRQn, 0);
14 | SysTick->VAL = 0;
15 | SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk | SysTick_CTRL_TICKINT_Msk | SysTick_CTRL_ENABLE_Msk;
16 | softdelay_calibrate();
17 | }
18 | /*
19 | * RCC clock配置函数
20 | */
21 | static void RCC_Configuration(void)
22 | {
23 | RCC_ADCCLKConfig(RCC_PCLK2_Div6);
24 | RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
25 | RCC_APB2PeriphClockCmd(APB2PERIPH_INIT_LIST, ENABLE);
26 | RCC_APB1PeriphClockCmd(APB1PERIPH_INIT_LIST, ENABLE);
27 | }
28 | /*
29 | * 中断控制器配置函数
30 | */
31 | static void NVIC_Configuration(void)
32 | {
33 | NVIC_InitTypeDef NVIC_InitStructure;
34 | NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
35 | NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
36 | set_board_systick();
37 |
38 | /* Enable line5 ~ line9 external interrupt. */
39 | // NVIC_InitStructure.NVIC_IRQChannel = EXTI9_5_IRQn;
40 | // NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
41 | // NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
42 | // NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
43 | NVIC_Init(&NVIC_InitStructure);
44 | }
45 | /*
46 | * ADC配置函数
47 | */
48 | #define ADC1_DR_Address ADC1_BASE + 0x4c
49 | _u32 voltage;
50 |
51 | static void ADC_Configuration(void)
52 | {
53 | DMA_InitTypeDef DMA_InitStructure;
54 | ADC_InitTypeDef ADC_InitStructure;
55 | DMA_DeInit(DMA1_Channel1);
56 | DMA_InitStructure.DMA_PeripheralBaseAddr = ADC1_DR_Address;
57 | DMA_InitStructure.DMA_MemoryBaseAddr = (u32)&voltage;
58 | DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
59 | DMA_InitStructure.DMA_BufferSize = 1;
60 | DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
61 | DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Disable;
62 | DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord;
63 | DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord;
64 | DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
65 | DMA_InitStructure.DMA_Priority = DMA_Priority_High;
66 | DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
67 | DMA_Init(DMA1_Channel1, &DMA_InitStructure);
68 |
69 | DMA_Cmd(DMA1_Channel1, ENABLE);
70 |
71 | ADC_InitStructure.ADC_Mode = ADC_Mode_Independent;
72 | ADC_InitStructure.ADC_ScanConvMode = ENABLE;
73 | ADC_InitStructure.ADC_ContinuousConvMode = ENABLE;
74 | ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None;
75 | ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
76 | ADC_InitStructure.ADC_NbrOfChannel = 1;
77 | ADC_Init(ADC1, &ADC_InitStructure);
78 |
79 | ADC_RegularChannelConfig(ADC1, 10, 1, ADC_SampleTime_239Cycles5);
80 |
81 | ADC_DMACmd(ADC1, ENABLE);
82 | ADC_Cmd(ADC1, ENABLE);
83 | // ADC_TempSensorVrefintCmd(ENABLE);
84 | ADC_ResetCalibration(ADC1);
85 |
86 | while (ADC_GetResetCalibrationStatus(ADC1)){};
87 |
88 | ADC_StartCalibration(ADC1);
89 | while (ADC_GetCalibrationStatus(ADC1)){};
90 |
91 | ADC_SoftwareStartConvCmd(ADC1, ENABLE);
92 | }
93 | /*
94 | * MCU低级初始化
95 | */
96 | _s32 init_board()
97 | {
98 | RCC_Configuration();
99 | PERFORM_IO_REMAP();
100 | NVIC_Configuration();
101 | ADC_Configuration();
102 | return 1;
103 | }
104 |
--------------------------------------------------------------------------------
/src/utils/stm32f10x_it.c:
--------------------------------------------------------------------------------
1 | #include "stm32f10x_it.h"
2 |
3 | /**
4 | * @brief This function handles NMI exception.
5 | * @param None
6 | * @retval : None
7 | */
8 | void NMI_Handler(void)
9 | {
10 | }
11 |
12 | /**
13 | * @brief This function handles Hard Fault exception.
14 | * @param None
15 | * @retval : None
16 | */
17 | void HardFault_Handler(void)
18 | {
19 | /* Go to infinite loop when Hard Fault exception occurs */
20 | while (1) {
21 | }
22 | }
23 |
24 | /**
25 | * @brief This function handles Memory Manage exception.
26 | * @param None
27 | * @retval : None
28 | */
29 | void MemManage_Handler(void)
30 | {
31 | /* Go to infinite loop when Memory Manage exception occurs */
32 | while (1) {
33 | }
34 | }
35 |
36 | /**
37 | * @brief This function handles Bus Fault exception.
38 | * @param None
39 | * @retval : None
40 | */
41 | void BusFault_Handler(void)
42 | {
43 | /* Go to infinite loop when Bus Fault exception occurs */
44 | while (1) {
45 | }
46 | }
47 |
48 | /**
49 | * @brief This function handles Usage Fault exception.
50 | * @param None
51 | * @retval : None
52 | */
53 | void UsageFault_Handler(void)
54 | {
55 | /* Go to infinite loop when Usage Fault exception occurs */
56 | while (1) {
57 | }
58 | }
59 |
60 | /**
61 | * @brief This function handles Debug Monitor exception.
62 | * @param None
63 | * @retval : None
64 | */
65 | void DebugMon_Handler(void)
66 | {
67 | }
68 |
69 | /**
70 | * @brief This function handles SVCall exception.
71 | * @param None
72 | * @retval : None
73 | */
74 | void SVC_Handler(void)
75 | {
76 | }
77 |
78 | /**
79 | * @brief This function handles PendSV_Handler exception.
80 | * @param None
81 | * @retval : None
82 | */
83 | void PendSV_Handler(void)
84 | {
85 | }
86 |
--------------------------------------------------------------------------------