├── CMakeLists.txt
├── LICENSE
├── Log
├── dbg.txt
├── fast_livo_time_log.csv
├── guide.md
├── imu.txt
├── mat_out.txt
├── mat_pre.txt
├── plot.py
└── pos_log.txt
├── README.md
├── config
├── NTU_VIRAL.yaml
├── avia_resize.yaml
├── camera_NTU_VIRAL.yaml
├── camera_pinhole.yaml
├── camera_pinhole_hk.yaml
├── camera_pinhole_resize.yaml
├── mid360.yaml
└── update.txt
├── hardware
└── stm32_timersync-open
│ ├── CORE
│ ├── core_cm3.c
│ ├── core_cm3.h
│ └── startup_stm32f10x_hd.s
│ ├── HARDWARE
│ ├── KEY
│ │ ├── key.c
│ │ └── key.h
│ ├── LED
│ │ ├── led.c
│ │ └── led.h
│ └── TIMER
│ │ ├── timer.c
│ │ └── timer.h
│ ├── STM32F10x_FWLib
│ ├── inc
│ │ ├── misc.h
│ │ ├── stm32f10x_adc.h
│ │ ├── stm32f10x_bkp.h
│ │ ├── stm32f10x_can.h
│ │ ├── stm32f10x_cec.h
│ │ ├── stm32f10x_crc.h
│ │ ├── stm32f10x_dac.h
│ │ ├── stm32f10x_dbgmcu.h
│ │ ├── stm32f10x_dma.h
│ │ ├── stm32f10x_exti.h
│ │ ├── stm32f10x_flash.h
│ │ ├── stm32f10x_fsmc.h
│ │ ├── stm32f10x_gpio.h
│ │ ├── stm32f10x_i2c.h
│ │ ├── stm32f10x_iwdg.h
│ │ ├── stm32f10x_pwr.h
│ │ ├── stm32f10x_rcc.h
│ │ ├── stm32f10x_rtc.h
│ │ ├── stm32f10x_sdio.h
│ │ ├── stm32f10x_spi.h
│ │ ├── stm32f10x_tim.h
│ │ ├── stm32f10x_usart.h
│ │ └── stm32f10x_wwdg.h
│ └── src
│ │ ├── misc.c
│ │ ├── stm32f10x_adc.c
│ │ ├── stm32f10x_bkp.c
│ │ ├── stm32f10x_can.c
│ │ ├── stm32f10x_cec.c
│ │ ├── stm32f10x_crc.c
│ │ ├── stm32f10x_dac.c
│ │ ├── stm32f10x_dbgmcu.c
│ │ ├── stm32f10x_dma.c
│ │ ├── stm32f10x_exti.c
│ │ ├── stm32f10x_flash.c
│ │ ├── stm32f10x_fsmc.c
│ │ ├── stm32f10x_gpio.c
│ │ ├── stm32f10x_i2c.c
│ │ ├── stm32f10x_iwdg.c
│ │ ├── stm32f10x_pwr.c
│ │ ├── stm32f10x_rcc.c
│ │ ├── stm32f10x_rtc.c
│ │ ├── stm32f10x_sdio.c
│ │ ├── stm32f10x_spi.c
│ │ ├── stm32f10x_tim.c
│ │ ├── stm32f10x_usart.c
│ │ └── stm32f10x_wwdg.c
│ ├── SYSTEM
│ ├── delay
│ │ ├── delay.c
│ │ └── delay.h
│ ├── sys
│ │ ├── sys.c
│ │ └── sys.h
│ └── usart
│ │ ├── usart.c
│ │ └── usart.h
│ ├── USER
│ ├── DebugConfig
│ │ ├── PWM_STM32F103C6_1.0.0.dbgconf
│ │ ├── PWM_STM32F103C8_1.0.0.dbgconf
│ │ └── PWM_STM32F103ZE_1.0.0.dbgconf
│ ├── JLinkLog.txt
│ ├── JLinkSettings.ini
│ ├── PWM.uvguix.Administrator
│ ├── PWM.uvguix.Sheng
│ ├── PWM.uvguix.admin
│ ├── PWM.uvguix.lzy
│ ├── PWM.uvguix.theodore
│ ├── PWM.uvoptx
│ ├── PWM.uvprojx
│ ├── PWM50.map
│ ├── main.c
│ ├── startup_stm32f10x_hd.lst
│ ├── stm32f10x.h
│ ├── stm32f10x_conf.h
│ ├── stm32f10x_it.c
│ ├── stm32f10x_it.h
│ ├── system_stm32f10x.c
│ └── system_stm32f10x.h
│ └── keilkilll.bat
├── img
├── Demo.png
├── Demo1.png
├── Framework.svg
├── cover.bmp
├── cover.jpg
└── time_sysnc.png
├── include
├── FOV_Checker
│ ├── FOV_Checker.cpp
│ └── FOV_Checker.h
├── IKFoM_toolkit
│ ├── esekfom
│ │ ├── esekfom.hpp
│ │ └── util.hpp
│ └── mtk
│ │ ├── build_manifold.hpp
│ │ ├── src
│ │ ├── SubManifold.hpp
│ │ ├── mtkmath.hpp
│ │ └── vectview.hpp
│ │ ├── startIdx.hpp
│ │ └── types
│ │ ├── S2.hpp
│ │ ├── SOn.hpp
│ │ ├── vect.hpp
│ │ └── wrapped_cv_mat.hpp
├── IMU_Processing.h
├── common_lib.h
├── feature.h
├── frame.h
├── ikd-Tree
│ ├── .vscode
│ │ ├── c_cpp_properties.json
│ │ └── settings.json
│ ├── README.md
│ ├── ikd_Tree.cpp
│ └── ikd_Tree.h
├── lidar_selection.h
├── map.h
├── point.h
├── preprocess.h
├── so3_math.h
└── use-ikfom.hpp
├── launch
├── mapping_avia.launch
├── mapping_avia_ntu.launch
├── mapping_mid360.launch
└── mapping_rviz.launch
├── msg
├── Pose6D.msg
└── States.msg
├── package.xml
├── rviz_cfg
├── loam_livox.rviz
└── loam_livox_ouster.rviz
├── src
├── IMU_Processing.cpp
├── frame.cpp
├── laserMapping.cpp
├── lidar_selection.cpp
├── map.cpp
├── point.cpp
└── preprocess.cpp
└── structure
├── T_cam_livox.STL
├── handle.SLDASM
├── handle_mid360.SLDPRT
├── handle_mid360.STL
├── handle_zhijia.SLDPRT
└── handle_zhijia.STL
/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 2.8.3)
2 | project(fast_livo)
3 |
4 | SET(CMAKE_BUILD_TYPE "Debug")
5 |
6 | ADD_COMPILE_OPTIONS(-std=c++14 )
7 | ADD_COMPILE_OPTIONS(-std=c++14 )
8 | set( CMAKE_CXX_FLAGS "-std=c++14 -O3" )
9 |
10 | add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\")
11 |
12 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" )
13 | set(CMAKE_CXX_STANDARD 14)
14 | set(CMAKE_CXX_STANDARD_REQUIRED ON)
15 | set(CMAKE_CXX_EXTENSIONS OFF)
16 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -pthread -std=c++0x -std=c++14 -fexceptions")
17 |
18 | message("Current CPU archtecture: ${CMAKE_SYSTEM_PROCESSOR}")
19 | if(CMAKE_SYSTEM_PROCESSOR MATCHES "(x86)|(X86)|(amd64)|(AMD64)" )
20 | include(ProcessorCount)
21 | ProcessorCount(N)
22 | message("Processer number: ${N}")
23 | if(N GREATER 5)
24 | add_definitions(-DMP_EN)
25 | add_definitions(-DMP_PROC_NUM=4)
26 | message("core for MP: 4")
27 | elseif(N GREATER 3)
28 | math(EXPR PROC_NUM "${N} - 2")
29 | add_definitions(-DMP_EN)
30 | add_definitions(-DMP_PROC_NUM="${PROC_NUM}")
31 | message("core for MP: ${PROC_NUM}")
32 | else()
33 | add_definitions(-DMP_PROC_NUM=1)
34 | endif()
35 | else()
36 | add_definitions(-DMP_PROC_NUM=1)
37 | endif()
38 |
39 | find_package(OpenMP QUIET)
40 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
41 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
42 |
43 | find_package(PythonLibs REQUIRED)
44 | find_path(MATPLOTLIB_CPP_INCLUDE_DIRS "matplotlibcpp.h")
45 | find_package(OpenMP REQUIRED)
46 | if(OPENMP_FOUND)
47 | message("Found OpenMP.")
48 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
49 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
50 | set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
51 | else()
52 | message("OpenMP not found, parallelization disabled.")
53 | endif()
54 | find_package(catkin REQUIRED COMPONENTS
55 | geometry_msgs
56 | nav_msgs
57 | sensor_msgs
58 | roscpp
59 | rospy
60 | std_msgs
61 | pcl_ros
62 | tf
63 | livox_ros_driver2
64 | message_generation
65 | eigen_conversions
66 | vikit_common
67 | vikit_ros
68 | cv_bridge
69 | image_transport
70 | )
71 |
72 | find_package(Eigen3 REQUIRED)
73 | find_package(PCL REQUIRED)
74 | FIND_PACKAGE(OpenCV REQUIRED)
75 | FIND_PACKAGE(Sophus REQUIRED)
76 | FIND_PACKAGE(Boost REQUIRED COMPONENTS thread)
77 | set(Sophus_LIBRARIES libSophus.so)
78 |
79 | message(Eigen: ${EIGEN3_INCLUDE_DIR})
80 |
81 | include_directories(
82 | ${catkin_INCLUDE_DIRS}
83 | ${EIGEN3_INCLUDE_DIR}
84 | ${PCL_INCLUDE_DIRS}
85 | ${PYTHON_INCLUDE_DIRS}
86 | ${OpenCV_INCLUDE_DIRS}
87 | ${Sophus_INCLUDE_DIRS}
88 | include)
89 |
90 | add_message_files(
91 | FILES
92 | Pose6D.msg
93 | States.msg
94 | )
95 |
96 | generate_messages(
97 | DEPENDENCIES
98 | geometry_msgs
99 | )
100 |
101 | catkin_package(
102 | CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime cv_bridge image_transport vikit_common vikit_ros
103 | DEPENDS EIGEN3 PCL OpenCV Sophus
104 | INCLUDE_DIRS include
105 | )
106 |
107 | # add_executable(loam_feat_extract src/feature_extract.cpp)
108 | # target_link_libraries(loam_feat_extract ${catkin_LIBRARIES} ${PCL_LIBRARIES})
109 | add_library(ikdtree include/ikd-Tree/ikd_Tree.cpp
110 | # include/ikd-Forest/ikd_Forest.cpp
111 | include/FOV_Checker/FOV_Checker.cpp
112 | )
113 | add_library(vio src/lidar_selection.cpp
114 | src/frame.cpp
115 | src/point.cpp
116 | src/map.cpp
117 | )
118 | add_executable(fastlivo_mapping src/laserMapping.cpp
119 | src/IMU_Processing.cpp
120 | src/preprocess.cpp
121 | )
122 | target_link_libraries(fastlivo_mapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES} vio ikdtree)
123 | target_include_directories(fastlivo_mapping PRIVATE ${PYTHON_INCLUDE_DIRS})
124 |
125 | # add_executable(kd_tree_test include/ikd-Tree/ikd_Tree.cpp src/kd_tree_test.cpp)
126 | # target_link_libraries(kd_tree_test ${PCL_LIBRARIES})
127 |
128 | # add_executable(ikd_Forest_Test include/ikd-Forest/ikd_Forest.cpp test/ikd_Forest_Test.cpp)
129 | # target_link_libraries(ikd_Forest_Test ${PCL_LIBRARIES})
130 |
131 | # add_executable(fov_test src/fov_test.cpp include/FOV_Checker/FOV_Checker.cpp)
132 |
133 |
134 |
--------------------------------------------------------------------------------
/Log/dbg.txt:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/WilsonGuo/FastLivo_Replication/eea82215c31adde5d914e54f9a38817e3aa9c942/Log/dbg.txt
--------------------------------------------------------------------------------
/Log/guide.md:
--------------------------------------------------------------------------------
1 | Here saved the debug records which can be drew by the ../log/plot.py. The record function can be found frm the MACRO: DEBUG_FILE_DIR(name) in common_lib.h.
2 |
--------------------------------------------------------------------------------
/Log/plot.py:
--------------------------------------------------------------------------------
1 | # import matplotlib
2 | # matplotlib.use('Agg')
3 | import numpy as np
4 | import matplotlib.pyplot as plt
5 |
6 | fig, axs = plt.subplots(3,2)
7 | lab_pre = ['', 'pre-x', 'pre-y', 'pre-z']
8 | lab_out = ['', 'out-x', 'out-y', 'out-z']
9 | plot_ind = range(7,10)
10 | a_pre=np.loadtxt('mat_pre.txt')
11 | a_out=np.loadtxt('mat_out.txt')
12 | time=a_pre[:,0]
13 | axs[0,0].set_title('Attitude')
14 | axs[1,0].set_title('Translation')
15 | axs[2,0].set_title('Velocity')
16 | axs[0,1].set_title('bg')
17 | axs[1,1].set_title('ba')
18 | axs[2,1].set_title('Gravity')
19 | for i in range(1,4):
20 | for j in range(6):
21 | axs[j%3, j/3].plot(time, a_pre[:,i+j*3],'.-', label=lab_pre[i])
22 | axs[j%3, j/3].plot(time, a_out[:,i+j*3],'.-', label=lab_out[i])
23 | for j in range(6):
24 | # axs[j].set_xlim(386,389)
25 | axs[j%3, j/3].grid()
26 | axs[j%3, j/3].legend()
27 | plt.grid()
28 |
29 |
30 | #### Draw IMU data
31 | fig, axs = plt.subplots(2)
32 | imu=np.loadtxt('imu.txt')
33 | time=imu[:,0]
34 | axs[0].set_title('Gyroscope')
35 | axs[1].set_title('Accelerameter')
36 | lab_1 = ['gyr-x', 'gyr-y', 'gyr-z']
37 | lab_2 = ['acc-x', 'acc-y', 'acc-z']
38 | for i in range(3):
39 | # if i==1:
40 | axs[0].plot(time, imu[:,i+1],'.-', label=lab_1[i])
41 | axs[1].plot(time, imu[:,i+4],'.-', label=lab_2[i])
42 | for i in range(2):
43 | # axs[i].set_xlim(386,389)
44 | axs[i].grid()
45 | axs[i].legend()
46 | plt.grid()
47 |
48 | # #### Draw time calculation
49 | # plt.figure(3)
50 | # fig = plt.figure()
51 | # font1 = {'family' : 'Times New Roman',
52 | # 'weight' : 'normal',
53 | # 'size' : 12,
54 | # }
55 | # c="red"
56 | # a_out1=np.loadtxt('Log/mat_out_time_indoor1.txt')
57 | # a_out2=np.loadtxt('Log/mat_out_time_indoor2.txt')
58 | # a_out3=np.loadtxt('Log/mat_out_time_outdoor.txt')
59 | # # n = a_out[:,1].size
60 | # # time_mean = a_out[:,1].mean()
61 | # # time_se = a_out[:,1].std() / np.sqrt(n)
62 | # # time_err = a_out[:,1] - time_mean
63 | # # feat_mean = a_out[:,2].mean()
64 | # # feat_err = a_out[:,2] - feat_mean
65 | # # feat_se = a_out[:,2].std() / np.sqrt(n)
66 | # ax1 = fig.add_subplot(111)
67 | # ax1.set_ylabel('Effective Feature Numbers',font1)
68 | # ax1.boxplot(a_out1[:,2], showfliers=False, positions=[0.9])
69 | # ax1.boxplot(a_out2[:,2], showfliers=False, positions=[1.9])
70 | # ax1.boxplot(a_out3[:,2], showfliers=False, positions=[2.9])
71 | # ax1.set_ylim([0, 3000])
72 |
73 | # ax2 = ax1.twinx()
74 | # ax2.spines['right'].set_color('red')
75 | # ax2.set_ylabel('Compute Time (ms)',font1)
76 | # ax2.yaxis.label.set_color('red')
77 | # ax2.tick_params(axis='y', colors='red')
78 | # ax2.boxplot(a_out1[:,1]*1000, showfliers=False, positions=[1.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c))
79 | # ax2.boxplot(a_out2[:,1]*1000, showfliers=False, positions=[2.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c))
80 | # ax2.boxplot(a_out3[:,1]*1000, showfliers=False, positions=[3.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c))
81 | # ax2.set_xlim([0.5, 3.5])
82 | # ax2.set_ylim([0, 100])
83 |
84 | # plt.xticks([1,2,3], ('Outdoor Scene', 'Indoor Scene 1', 'Indoor Scene 2'))
85 | # # # print(time_se)
86 | # # # print(a_out3[:,2])
87 | # plt.grid()
88 | # plt.savefig("time.pdf", dpi=1200)
89 | plt.show()
90 |
--------------------------------------------------------------------------------
/Log/pos_log.txt:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/WilsonGuo/FastLivo_Replication/eea82215c31adde5d914e54f9a38817e3aa9c942/Log/pos_log.txt
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # This paper reproduces the SLAM research results from Mars lab of the University of Hong Kong -Fast Livo
2 |
3 | [Video Tutorial](https://www.bilibili.com/video/BV1T142197ci/?share_source=copy_web&vd_source=dd0669827093367276172cd819a1ae99)
4 |
5 | ## What's new:
6 | 1. Adapted to [Mid360 LiDAR](https://github.com/WilsonGuo/livox_ros_driver2)
7 | 2. Corrected the incorrect verification of virtual GPRMC instruction in STM32，
8 | See：[HardWare](https://github.com/WilsonGuo/FastLivo_Replication/tree/main/hardware)
9 |
10 |
11 |
12 | See the wiki for more updates：https://github.com/WilsonGuo/FastLivo_Replication/wiki
13 |
14 |
15 |
16 |
21 |
22 |
34 |
35 |
© COPYRIGHT 2011 STMicroelectronics
20 | ******************************************************************************
21 | */
22 |
23 | /* Define to prevent recursive inclusion -------------------------------------*/
24 | #ifndef __STM32F10x_BKP_H
25 | #define __STM32F10x_BKP_H
26 |
27 | #ifdef __cplusplus
28 | extern "C" {
29 | #endif
30 |
31 | /* Includes ------------------------------------------------------------------*/
32 | #include "stm32f10x.h"
33 |
34 | /** @addtogroup STM32F10x_StdPeriph_Driver
35 | * @{
36 | */
37 |
38 | /** @addtogroup BKP
39 | * @{
40 | */
41 |
42 | /** @defgroup BKP_Exported_Types
43 | * @{
44 | */
45 |
46 | /**
47 | * @}
48 | */
49 |
50 | /** @defgroup BKP_Exported_Constants
51 | * @{
52 | */
53 |
54 | /** @defgroup Tamper_Pin_active_level
55 | * @{
56 | */
57 |
58 | #define BKP_TamperPinLevel_High ((uint16_t)0x0000)
59 | #define BKP_TamperPinLevel_Low ((uint16_t)0x0001)
60 | #define IS_BKP_TAMPER_PIN_LEVEL(LEVEL) (((LEVEL) == BKP_TamperPinLevel_High) || \
61 | ((LEVEL) == BKP_TamperPinLevel_Low))
62 | /**
63 | * @}
64 | */
65 |
66 | /** @defgroup RTC_output_source_to_output_on_the_Tamper_pin
67 | * @{
68 | */
69 |
70 | #define BKP_RTCOutputSource_None ((uint16_t)0x0000)
71 | #define BKP_RTCOutputSource_CalibClock ((uint16_t)0x0080)
72 | #define BKP_RTCOutputSource_Alarm ((uint16_t)0x0100)
73 | #define BKP_RTCOutputSource_Second ((uint16_t)0x0300)
74 | #define IS_BKP_RTC_OUTPUT_SOURCE(SOURCE) (((SOURCE) == BKP_RTCOutputSource_None) || \
75 | ((SOURCE) == BKP_RTCOutputSource_CalibClock) || \
76 | ((SOURCE) == BKP_RTCOutputSource_Alarm) || \
77 | ((SOURCE) == BKP_RTCOutputSource_Second))
78 | /**
79 | * @}
80 | */
81 |
82 | /** @defgroup Data_Backup_Register
83 | * @{
84 | */
85 |
86 | #define BKP_DR1 ((uint16_t)0x0004)
87 | #define BKP_DR2 ((uint16_t)0x0008)
88 | #define BKP_DR3 ((uint16_t)0x000C)
89 | #define BKP_DR4 ((uint16_t)0x0010)
90 | #define BKP_DR5 ((uint16_t)0x0014)
91 | #define BKP_DR6 ((uint16_t)0x0018)
92 | #define BKP_DR7 ((uint16_t)0x001C)
93 | #define BKP_DR8 ((uint16_t)0x0020)
94 | #define BKP_DR9 ((uint16_t)0x0024)
95 | #define BKP_DR10 ((uint16_t)0x0028)
96 | #define BKP_DR11 ((uint16_t)0x0040)
97 | #define BKP_DR12 ((uint16_t)0x0044)
98 | #define BKP_DR13 ((uint16_t)0x0048)
99 | #define BKP_DR14 ((uint16_t)0x004C)
100 | #define BKP_DR15 ((uint16_t)0x0050)
101 | #define BKP_DR16 ((uint16_t)0x0054)
102 | #define BKP_DR17 ((uint16_t)0x0058)
103 | #define BKP_DR18 ((uint16_t)0x005C)
104 | #define BKP_DR19 ((uint16_t)0x0060)
105 | #define BKP_DR20 ((uint16_t)0x0064)
106 | #define BKP_DR21 ((uint16_t)0x0068)
107 | #define BKP_DR22 ((uint16_t)0x006C)
108 | #define BKP_DR23 ((uint16_t)0x0070)
109 | #define BKP_DR24 ((uint16_t)0x0074)
110 | #define BKP_DR25 ((uint16_t)0x0078)
111 | #define BKP_DR26 ((uint16_t)0x007C)
112 | #define BKP_DR27 ((uint16_t)0x0080)
113 | #define BKP_DR28 ((uint16_t)0x0084)
114 | #define BKP_DR29 ((uint16_t)0x0088)
115 | #define BKP_DR30 ((uint16_t)0x008C)
116 | #define BKP_DR31 ((uint16_t)0x0090)
117 | #define BKP_DR32 ((uint16_t)0x0094)
118 | #define BKP_DR33 ((uint16_t)0x0098)
119 | #define BKP_DR34 ((uint16_t)0x009C)
120 | #define BKP_DR35 ((uint16_t)0x00A0)
121 | #define BKP_DR36 ((uint16_t)0x00A4)
122 | #define BKP_DR37 ((uint16_t)0x00A8)
123 | #define BKP_DR38 ((uint16_t)0x00AC)
124 | #define BKP_DR39 ((uint16_t)0x00B0)
125 | #define BKP_DR40 ((uint16_t)0x00B4)
126 | #define BKP_DR41 ((uint16_t)0x00B8)
127 | #define BKP_DR42 ((uint16_t)0x00BC)
128 |
129 | #define IS_BKP_DR(DR) (((DR) == BKP_DR1) || ((DR) == BKP_DR2) || ((DR) == BKP_DR3) || \
130 | ((DR) == BKP_DR4) || ((DR) == BKP_DR5) || ((DR) == BKP_DR6) || \
131 | ((DR) == BKP_DR7) || ((DR) == BKP_DR8) || ((DR) == BKP_DR9) || \
132 | ((DR) == BKP_DR10) || ((DR) == BKP_DR11) || ((DR) == BKP_DR12) || \
133 | ((DR) == BKP_DR13) || ((DR) == BKP_DR14) || ((DR) == BKP_DR15) || \
134 | ((DR) == BKP_DR16) || ((DR) == BKP_DR17) || ((DR) == BKP_DR18) || \
135 | ((DR) == BKP_DR19) || ((DR) == BKP_DR20) || ((DR) == BKP_DR21) || \
136 | ((DR) == BKP_DR22) || ((DR) == BKP_DR23) || ((DR) == BKP_DR24) || \
137 | ((DR) == BKP_DR25) || ((DR) == BKP_DR26) || ((DR) == BKP_DR27) || \
138 | ((DR) == BKP_DR28) || ((DR) == BKP_DR29) || ((DR) == BKP_DR30) || \
139 | ((DR) == BKP_DR31) || ((DR) == BKP_DR32) || ((DR) == BKP_DR33) || \
140 | ((DR) == BKP_DR34) || ((DR) == BKP_DR35) || ((DR) == BKP_DR36) || \
141 | ((DR) == BKP_DR37) || ((DR) == BKP_DR38) || ((DR) == BKP_DR39) || \
142 | ((DR) == BKP_DR40) || ((DR) == BKP_DR41) || ((DR) == BKP_DR42))
143 |
144 | #define IS_BKP_CALIBRATION_VALUE(VALUE) ((VALUE) <= 0x7F)
145 | /**
146 | * @}
147 | */
148 |
149 | /**
150 | * @}
151 | */
152 |
153 | /** @defgroup BKP_Exported_Macros
154 | * @{
155 | */
156 |
157 | /**
158 | * @}
159 | */
160 |
161 | /** @defgroup BKP_Exported_Functions
162 | * @{
163 | */
164 |
165 | void BKP_DeInit(void);
166 | void BKP_TamperPinLevelConfig(uint16_t BKP_TamperPinLevel);
167 | void BKP_TamperPinCmd(FunctionalState NewState);
168 | void BKP_ITConfig(FunctionalState NewState);
169 | void BKP_RTCOutputConfig(uint16_t BKP_RTCOutputSource);
170 | void BKP_SetRTCCalibrationValue(uint8_t CalibrationValue);
171 | void BKP_WriteBackupRegister(uint16_t BKP_DR, uint16_t Data);
172 | uint16_t BKP_ReadBackupRegister(uint16_t BKP_DR);
173 | FlagStatus BKP_GetFlagStatus(void);
174 | void BKP_ClearFlag(void);
175 | ITStatus BKP_GetITStatus(void);
176 | void BKP_ClearITPendingBit(void);
177 |
178 | #ifdef __cplusplus
179 | }
180 | #endif
181 |
182 | #endif /* __STM32F10x_BKP_H */
183 | /**
184 | * @}
185 | */
186 |
187 | /**
188 | * @}
189 | */
190 |
191 | /**
192 | * @}
193 | */
194 |
195 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/
196 |
--------------------------------------------------------------------------------
/hardware/stm32_timersync-open/STM32F10x_FWLib/inc/stm32f10x_cec.h:
--------------------------------------------------------------------------------
1 | /**
2 | ******************************************************************************
3 | * @file stm32f10x_cec.h
4 | * @author MCD Application Team
5 | * @version V3.5.0
6 | * @date 11-March-2011
7 | * @brief This file contains all the functions prototypes for the CEC firmware
8 | * library.
9 | ******************************************************************************
10 | * @attention
11 | *
12 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
13 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
14 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
15 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
16 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
17 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
18 | *
19 | * © COPYRIGHT 2011 STMicroelectronics
20 | ******************************************************************************
21 | */
22 |
23 | /* Define to prevent recursive inclusion -------------------------------------*/
24 | #ifndef __STM32F10x_CEC_H
25 | #define __STM32F10x_CEC_H
26 |
27 | #ifdef __cplusplus
28 | extern "C" {
29 | #endif
30 |
31 | /* Includes ------------------------------------------------------------------*/
32 | #include "stm32f10x.h"
33 |
34 | /** @addtogroup STM32F10x_StdPeriph_Driver
35 | * @{
36 | */
37 |
38 | /** @addtogroup CEC
39 | * @{
40 | */
41 |
42 |
43 | /** @defgroup CEC_Exported_Types
44 | * @{
45 | */
46 |
47 | /**
48 | * @brief CEC Init structure definition
49 | */
50 | typedef struct
51 | {
52 | uint16_t CEC_BitTimingMode; /*!< Configures the CEC Bit Timing Error Mode.
53 | This parameter can be a value of @ref CEC_BitTiming_Mode */
54 | uint16_t CEC_BitPeriodMode; /*!< Configures the CEC Bit Period Error Mode.
55 | This parameter can be a value of @ref CEC_BitPeriod_Mode */
56 | }CEC_InitTypeDef;
57 |
58 | /**
59 | * @}
60 | */
61 |
62 | /** @defgroup CEC_Exported_Constants
63 | * @{
64 | */
65 |
66 | /** @defgroup CEC_BitTiming_Mode
67 | * @{
68 | */
69 | #define CEC_BitTimingStdMode ((uint16_t)0x00) /*!< Bit timing error Standard Mode */
70 | #define CEC_BitTimingErrFreeMode CEC_CFGR_BTEM /*!< Bit timing error Free Mode */
71 |
72 | #define IS_CEC_BIT_TIMING_ERROR_MODE(MODE) (((MODE) == CEC_BitTimingStdMode) || \
73 | ((MODE) == CEC_BitTimingErrFreeMode))
74 | /**
75 | * @}
76 | */
77 |
78 | /** @defgroup CEC_BitPeriod_Mode
79 | * @{
80 | */
81 | #define CEC_BitPeriodStdMode ((uint16_t)0x00) /*!< Bit period error Standard Mode */
82 | #define CEC_BitPeriodFlexibleMode CEC_CFGR_BPEM /*!< Bit period error Flexible Mode */
83 |
84 | #define IS_CEC_BIT_PERIOD_ERROR_MODE(MODE) (((MODE) == CEC_BitPeriodStdMode) || \
85 | ((MODE) == CEC_BitPeriodFlexibleMode))
86 | /**
87 | * @}
88 | */
89 |
90 |
91 | /** @defgroup CEC_interrupts_definition
92 | * @{
93 | */
94 | #define CEC_IT_TERR CEC_CSR_TERR
95 | #define CEC_IT_TBTRF CEC_CSR_TBTRF
96 | #define CEC_IT_RERR CEC_CSR_RERR
97 | #define CEC_IT_RBTF CEC_CSR_RBTF
98 | #define IS_CEC_GET_IT(IT) (((IT) == CEC_IT_TERR) || ((IT) == CEC_IT_TBTRF) || \
99 | ((IT) == CEC_IT_RERR) || ((IT) == CEC_IT_RBTF))
100 | /**
101 | * @}
102 | */
103 |
104 |
105 | /** @defgroup CEC_Own_Address
106 | * @{
107 | */
108 | #define IS_CEC_ADDRESS(ADDRESS) ((ADDRESS) < 0x10)
109 | /**
110 | * @}
111 | */
112 |
113 | /** @defgroup CEC_Prescaler
114 | * @{
115 | */
116 | #define IS_CEC_PRESCALER(PRESCALER) ((PRESCALER) <= 0x3FFF)
117 |
118 | /**
119 | * @}
120 | */
121 |
122 | /** @defgroup CEC_flags_definition
123 | * @{
124 | */
125 |
126 | /**
127 | * @brief ESR register flags
128 | */
129 | #define CEC_FLAG_BTE ((uint32_t)0x10010000)
130 | #define CEC_FLAG_BPE ((uint32_t)0x10020000)
131 | #define CEC_FLAG_RBTFE ((uint32_t)0x10040000)
132 | #define CEC_FLAG_SBE ((uint32_t)0x10080000)
133 | #define CEC_FLAG_ACKE ((uint32_t)0x10100000)
134 | #define CEC_FLAG_LINE ((uint32_t)0x10200000)
135 | #define CEC_FLAG_TBTFE ((uint32_t)0x10400000)
136 |
137 | /**
138 | * @brief CSR register flags
139 | */
140 | #define CEC_FLAG_TEOM ((uint32_t)0x00000002)
141 | #define CEC_FLAG_TERR ((uint32_t)0x00000004)
142 | #define CEC_FLAG_TBTRF ((uint32_t)0x00000008)
143 | #define CEC_FLAG_RSOM ((uint32_t)0x00000010)
144 | #define CEC_FLAG_REOM ((uint32_t)0x00000020)
145 | #define CEC_FLAG_RERR ((uint32_t)0x00000040)
146 | #define CEC_FLAG_RBTF ((uint32_t)0x00000080)
147 |
148 | #define IS_CEC_CLEAR_FLAG(FLAG) ((((FLAG) & (uint32_t)0xFFFFFF03) == 0x00) && ((FLAG) != 0x00))
149 |
150 | #define IS_CEC_GET_FLAG(FLAG) (((FLAG) == CEC_FLAG_BTE) || ((FLAG) == CEC_FLAG_BPE) || \
151 | ((FLAG) == CEC_FLAG_RBTFE) || ((FLAG)== CEC_FLAG_SBE) || \
152 | ((FLAG) == CEC_FLAG_ACKE) || ((FLAG) == CEC_FLAG_LINE) || \
153 | ((FLAG) == CEC_FLAG_TBTFE) || ((FLAG) == CEC_FLAG_TEOM) || \
154 | ((FLAG) == CEC_FLAG_TERR) || ((FLAG) == CEC_FLAG_TBTRF) || \
155 | ((FLAG) == CEC_FLAG_RSOM) || ((FLAG) == CEC_FLAG_REOM) || \
156 | ((FLAG) == CEC_FLAG_RERR) || ((FLAG) == CEC_FLAG_RBTF))
157 |
158 | /**
159 | * @}
160 | */
161 |
162 | /**
163 | * @}
164 | */
165 |
166 | /** @defgroup CEC_Exported_Macros
167 | * @{
168 | */
169 |
170 | /**
171 | * @}
172 | */
173 |
174 | /** @defgroup CEC_Exported_Functions
175 | * @{
176 | */
177 | void CEC_DeInit(void);
178 | void CEC_Init(CEC_InitTypeDef* CEC_InitStruct);
179 | void CEC_Cmd(FunctionalState NewState);
180 | void CEC_ITConfig(FunctionalState NewState);
181 | void CEC_OwnAddressConfig(uint8_t CEC_OwnAddress);
182 | void CEC_SetPrescaler(uint16_t CEC_Prescaler);
183 | void CEC_SendDataByte(uint8_t Data);
184 | uint8_t CEC_ReceiveDataByte(void);
185 | void CEC_StartOfMessage(void);
186 | void CEC_EndOfMessageCmd(FunctionalState NewState);
187 | FlagStatus CEC_GetFlagStatus(uint32_t CEC_FLAG);
188 | void CEC_ClearFlag(uint32_t CEC_FLAG);
189 | ITStatus CEC_GetITStatus(uint8_t CEC_IT);
190 | void CEC_ClearITPendingBit(uint16_t CEC_IT);
191 |
192 | #ifdef __cplusplus
193 | }
194 | #endif
195 |
196 | #endif /* __STM32F10x_CEC_H */
197 |
198 | /**
199 | * @}
200 | */
201 |
202 | /**
203 | * @}
204 | */
205 |
206 | /**
207 | * @}
208 | */
209 |
210 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/
211 |
--------------------------------------------------------------------------------
/hardware/stm32_timersync-open/STM32F10x_FWLib/inc/stm32f10x_crc.h:
--------------------------------------------------------------------------------
1 | /**
2 | ******************************************************************************
3 | * @file stm32f10x_crc.h
4 | * @author MCD Application Team
5 | * @version V3.5.0
6 | * @date 11-March-2011
7 | * @brief This file contains all the functions prototypes for the CRC firmware
8 | * library.
9 | ******************************************************************************
10 | * @attention
11 | *
12 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
13 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
14 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
15 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
16 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
17 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
18 | *
19 | * © COPYRIGHT 2011 STMicroelectronics
20 | ******************************************************************************
21 | */
22 |
23 | /* Define to prevent recursive inclusion -------------------------------------*/
24 | #ifndef __STM32F10x_CRC_H
25 | #define __STM32F10x_CRC_H
26 |
27 | #ifdef __cplusplus
28 | extern "C" {
29 | #endif
30 |
31 | /* Includes ------------------------------------------------------------------*/
32 | #include "stm32f10x.h"
33 |
34 | /** @addtogroup STM32F10x_StdPeriph_Driver
35 | * @{
36 | */
37 |
38 | /** @addtogroup CRC
39 | * @{
40 | */
41 |
42 | /** @defgroup CRC_Exported_Types
43 | * @{
44 | */
45 |
46 | /**
47 | * @}
48 | */
49 |
50 | /** @defgroup CRC_Exported_Constants
51 | * @{
52 | */
53 |
54 | /**
55 | * @}
56 | */
57 |
58 | /** @defgroup CRC_Exported_Macros
59 | * @{
60 | */
61 |
62 | /**
63 | * @}
64 | */
65 |
66 | /** @defgroup CRC_Exported_Functions
67 | * @{
68 | */
69 |
70 | void CRC_ResetDR(void);
71 | uint32_t CRC_CalcCRC(uint32_t Data);
72 | uint32_t CRC_CalcBlockCRC(uint32_t pBuffer[], uint32_t BufferLength);
73 | uint32_t CRC_GetCRC(void);
74 | void CRC_SetIDRegister(uint8_t IDValue);
75 | uint8_t CRC_GetIDRegister(void);
76 |
77 | #ifdef __cplusplus
78 | }
79 | #endif
80 |
81 | #endif /* __STM32F10x_CRC_H */
82 | /**
83 | * @}
84 | */
85 |
86 | /**
87 | * @}
88 | */
89 |
90 | /**
91 | * @}
92 | */
93 |
94 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/
95 |
--------------------------------------------------------------------------------
/hardware/stm32_timersync-open/STM32F10x_FWLib/inc/stm32f10x_dbgmcu.h:
--------------------------------------------------------------------------------
1 | /**
2 | ******************************************************************************
3 | * @file stm32f10x_dbgmcu.h
4 | * @author MCD Application Team
5 | * @version V3.5.0
6 | * @date 11-March-2011
7 | * @brief This file contains all the functions prototypes for the DBGMCU
8 | * firmware library.
9 | ******************************************************************************
10 | * @attention
11 | *
12 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
13 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
14 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
15 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
16 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
17 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
18 | *
19 | * © COPYRIGHT 2011 STMicroelectronics
20 | ******************************************************************************
21 | */
22 |
23 | /* Define to prevent recursive inclusion -------------------------------------*/
24 | #ifndef __STM32F10x_DBGMCU_H
25 | #define __STM32F10x_DBGMCU_H
26 |
27 | #ifdef __cplusplus
28 | extern "C" {
29 | #endif
30 |
31 | /* Includes ------------------------------------------------------------------*/
32 | #include "stm32f10x.h"
33 |
34 | /** @addtogroup STM32F10x_StdPeriph_Driver
35 | * @{
36 | */
37 |
38 | /** @addtogroup DBGMCU
39 | * @{
40 | */
41 |
42 | /** @defgroup DBGMCU_Exported_Types
43 | * @{
44 | */
45 |
46 | /**
47 | * @}
48 | */
49 |
50 | /** @defgroup DBGMCU_Exported_Constants
51 | * @{
52 | */
53 |
54 | #define DBGMCU_SLEEP ((uint32_t)0x00000001)
55 | #define DBGMCU_STOP ((uint32_t)0x00000002)
56 | #define DBGMCU_STANDBY ((uint32_t)0x00000004)
57 | #define DBGMCU_IWDG_STOP ((uint32_t)0x00000100)
58 | #define DBGMCU_WWDG_STOP ((uint32_t)0x00000200)
59 | #define DBGMCU_TIM1_STOP ((uint32_t)0x00000400)
60 | #define DBGMCU_TIM2_STOP ((uint32_t)0x00000800)
61 | #define DBGMCU_TIM3_STOP ((uint32_t)0x00001000)
62 | #define DBGMCU_TIM4_STOP ((uint32_t)0x00002000)
63 | #define DBGMCU_CAN1_STOP ((uint32_t)0x00004000)
64 | #define DBGMCU_I2C1_SMBUS_TIMEOUT ((uint32_t)0x00008000)
65 | #define DBGMCU_I2C2_SMBUS_TIMEOUT ((uint32_t)0x00010000)
66 | #define DBGMCU_TIM8_STOP ((uint32_t)0x00020000)
67 | #define DBGMCU_TIM5_STOP ((uint32_t)0x00040000)
68 | #define DBGMCU_TIM6_STOP ((uint32_t)0x00080000)
69 | #define DBGMCU_TIM7_STOP ((uint32_t)0x00100000)
70 | #define DBGMCU_CAN2_STOP ((uint32_t)0x00200000)
71 | #define DBGMCU_TIM15_STOP ((uint32_t)0x00400000)
72 | #define DBGMCU_TIM16_STOP ((uint32_t)0x00800000)
73 | #define DBGMCU_TIM17_STOP ((uint32_t)0x01000000)
74 | #define DBGMCU_TIM12_STOP ((uint32_t)0x02000000)
75 | #define DBGMCU_TIM13_STOP ((uint32_t)0x04000000)
76 | #define DBGMCU_TIM14_STOP ((uint32_t)0x08000000)
77 | #define DBGMCU_TIM9_STOP ((uint32_t)0x10000000)
78 | #define DBGMCU_TIM10_STOP ((uint32_t)0x20000000)
79 | #define DBGMCU_TIM11_STOP ((uint32_t)0x40000000)
80 |
81 | #define IS_DBGMCU_PERIPH(PERIPH) ((((PERIPH) & 0x800000F8) == 0x00) && ((PERIPH) != 0x00))
82 | /**
83 | * @}
84 | */
85 |
86 | /** @defgroup DBGMCU_Exported_Macros
87 | * @{
88 | */
89 |
90 | /**
91 | * @}
92 | */
93 |
94 | /** @defgroup DBGMCU_Exported_Functions
95 | * @{
96 | */
97 |
98 | uint32_t DBGMCU_GetREVID(void);
99 | uint32_t DBGMCU_GetDEVID(void);
100 | void DBGMCU_Config(uint32_t DBGMCU_Periph, FunctionalState NewState);
101 |
102 | #ifdef __cplusplus
103 | }
104 | #endif
105 |
106 | #endif /* __STM32F10x_DBGMCU_H */
107 | /**
108 | * @}
109 | */
110 |
111 | /**
112 | * @}
113 | */
114 |
115 | /**
116 | * @}
117 | */
118 |
119 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/
120 |
--------------------------------------------------------------------------------
/hardware/stm32_timersync-open/STM32F10x_FWLib/inc/stm32f10x_exti.h:
--------------------------------------------------------------------------------
1 | /**
2 | ******************************************************************************
3 | * @file stm32f10x_exti.h
4 | * @author MCD Application Team
5 | * @version V3.5.0
6 | * @date 11-March-2011
7 | * @brief This file contains all the functions prototypes for the EXTI firmware
8 | * library.
9 | ******************************************************************************
10 | * @attention
11 | *
12 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
13 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
14 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
15 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
16 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
17 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
18 | *
19 | * © COPYRIGHT 2011 STMicroelectronics
20 | ******************************************************************************
21 | */
22 |
23 | /* Define to prevent recursive inclusion -------------------------------------*/
24 | #ifndef __STM32F10x_EXTI_H
25 | #define __STM32F10x_EXTI_H
26 |
27 | #ifdef __cplusplus
28 | extern "C" {
29 | #endif
30 |
31 | /* Includes ------------------------------------------------------------------*/
32 | #include "stm32f10x.h"
33 |
34 | /** @addtogroup STM32F10x_StdPeriph_Driver
35 | * @{
36 | */
37 |
38 | /** @addtogroup EXTI
39 | * @{
40 | */
41 |
42 | /** @defgroup EXTI_Exported_Types
43 | * @{
44 | */
45 |
46 | /**
47 | * @brief EXTI mode enumeration
48 | */
49 |
50 | typedef enum
51 | {
52 | EXTI_Mode_Interrupt = 0x00,
53 | EXTI_Mode_Event = 0x04
54 | }EXTIMode_TypeDef;
55 |
56 | #define IS_EXTI_MODE(MODE) (((MODE) == EXTI_Mode_Interrupt) || ((MODE) == EXTI_Mode_Event))
57 |
58 | /**
59 | * @brief EXTI Trigger enumeration
60 | */
61 |
62 | typedef enum
63 | {
64 | EXTI_Trigger_Rising = 0x08,
65 | EXTI_Trigger_Falling = 0x0C,
66 | EXTI_Trigger_Rising_Falling = 0x10
67 | }EXTITrigger_TypeDef;
68 |
69 | #define IS_EXTI_TRIGGER(TRIGGER) (((TRIGGER) == EXTI_Trigger_Rising) || \
70 | ((TRIGGER) == EXTI_Trigger_Falling) || \
71 | ((TRIGGER) == EXTI_Trigger_Rising_Falling))
72 | /**
73 | * @brief EXTI Init Structure definition
74 | */
75 |
76 | typedef struct
77 | {
78 | uint32_t EXTI_Line; /*!< Specifies the EXTI lines to be enabled or disabled.
79 | This parameter can be any combination of @ref EXTI_Lines */
80 |
81 | EXTIMode_TypeDef EXTI_Mode; /*!< Specifies the mode for the EXTI lines.
82 | This parameter can be a value of @ref EXTIMode_TypeDef */
83 |
84 | EXTITrigger_TypeDef EXTI_Trigger; /*!< Specifies the trigger signal active edge for the EXTI lines.
85 | This parameter can be a value of @ref EXTIMode_TypeDef */
86 |
87 | FunctionalState EXTI_LineCmd; /*!< Specifies the new state of the selected EXTI lines.
88 | This parameter can be set either to ENABLE or DISABLE */
89 | }EXTI_InitTypeDef;
90 |
91 | /**
92 | * @}
93 | */
94 |
95 | /** @defgroup EXTI_Exported_Constants
96 | * @{
97 | */
98 |
99 | /** @defgroup EXTI_Lines
100 | * @{
101 | */
102 |
103 | #define EXTI_Line0 ((uint32_t)0x00001) /*!< External interrupt line 0 */
104 | #define EXTI_Line1 ((uint32_t)0x00002) /*!< External interrupt line 1 */
105 | #define EXTI_Line2 ((uint32_t)0x00004) /*!< External interrupt line 2 */
106 | #define EXTI_Line3 ((uint32_t)0x00008) /*!< External interrupt line 3 */
107 | #define EXTI_Line4 ((uint32_t)0x00010) /*!< External interrupt line 4 */
108 | #define EXTI_Line5 ((uint32_t)0x00020) /*!< External interrupt line 5 */
109 | #define EXTI_Line6 ((uint32_t)0x00040) /*!< External interrupt line 6 */
110 | #define EXTI_Line7 ((uint32_t)0x00080) /*!< External interrupt line 7 */
111 | #define EXTI_Line8 ((uint32_t)0x00100) /*!< External interrupt line 8 */
112 | #define EXTI_Line9 ((uint32_t)0x00200) /*!< External interrupt line 9 */
113 | #define EXTI_Line10 ((uint32_t)0x00400) /*!< External interrupt line 10 */
114 | #define EXTI_Line11 ((uint32_t)0x00800) /*!< External interrupt line 11 */
115 | #define EXTI_Line12 ((uint32_t)0x01000) /*!< External interrupt line 12 */
116 | #define EXTI_Line13 ((uint32_t)0x02000) /*!< External interrupt line 13 */
117 | #define EXTI_Line14 ((uint32_t)0x04000) /*!< External interrupt line 14 */
118 | #define EXTI_Line15 ((uint32_t)0x08000) /*!< External interrupt line 15 */
119 | #define EXTI_Line16 ((uint32_t)0x10000) /*!< External interrupt line 16 Connected to the PVD Output */
120 | #define EXTI_Line17 ((uint32_t)0x20000) /*!< External interrupt line 17 Connected to the RTC Alarm event */
121 | #define EXTI_Line18 ((uint32_t)0x40000) /*!< External interrupt line 18 Connected to the USB Device/USB OTG FS
122 | Wakeup from suspend event */
123 | #define EXTI_Line19 ((uint32_t)0x80000) /*!< External interrupt line 19 Connected to the Ethernet Wakeup event */
124 |
125 | #define IS_EXTI_LINE(LINE) ((((LINE) & (uint32_t)0xFFF00000) == 0x00) && ((LINE) != (uint16_t)0x00))
126 | #define IS_GET_EXTI_LINE(LINE) (((LINE) == EXTI_Line0) || ((LINE) == EXTI_Line1) || \
127 | ((LINE) == EXTI_Line2) || ((LINE) == EXTI_Line3) || \
128 | ((LINE) == EXTI_Line4) || ((LINE) == EXTI_Line5) || \
129 | ((LINE) == EXTI_Line6) || ((LINE) == EXTI_Line7) || \
130 | ((LINE) == EXTI_Line8) || ((LINE) == EXTI_Line9) || \
131 | ((LINE) == EXTI_Line10) || ((LINE) == EXTI_Line11) || \
132 | ((LINE) == EXTI_Line12) || ((LINE) == EXTI_Line13) || \
133 | ((LINE) == EXTI_Line14) || ((LINE) == EXTI_Line15) || \
134 | ((LINE) == EXTI_Line16) || ((LINE) == EXTI_Line17) || \
135 | ((LINE) == EXTI_Line18) || ((LINE) == EXTI_Line19))
136 |
137 |
138 | /**
139 | * @}
140 | */
141 |
142 | /**
143 | * @}
144 | */
145 |
146 | /** @defgroup EXTI_Exported_Macros
147 | * @{
148 | */
149 |
150 | /**
151 | * @}
152 | */
153 |
154 | /** @defgroup EXTI_Exported_Functions
155 | * @{
156 | */
157 |
158 | void EXTI_DeInit(void);
159 | void EXTI_Init(EXTI_InitTypeDef* EXTI_InitStruct);
160 | void EXTI_StructInit(EXTI_InitTypeDef* EXTI_InitStruct);
161 | void EXTI_GenerateSWInterrupt(uint32_t EXTI_Line);
162 | FlagStatus EXTI_GetFlagStatus(uint32_t EXTI_Line);
163 | void EXTI_ClearFlag(uint32_t EXTI_Line);
164 | ITStatus EXTI_GetITStatus(uint32_t EXTI_Line);
165 | void EXTI_ClearITPendingBit(uint32_t EXTI_Line);
166 |
167 | #ifdef __cplusplus
168 | }
169 | #endif
170 |
171 | #endif /* __STM32F10x_EXTI_H */
172 | /**
173 | * @}
174 | */
175 |
176 | /**
177 | * @}
178 | */
179 |
180 | /**
181 | * @}
182 | */
183 |
184 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/
185 |
--------------------------------------------------------------------------------
/hardware/stm32_timersync-open/STM32F10x_FWLib/inc/stm32f10x_iwdg.h:
--------------------------------------------------------------------------------
1 | /**
2 | ******************************************************************************
3 | * @file stm32f10x_iwdg.h
4 | * @author MCD Application Team
5 | * @version V3.5.0
6 | * @date 11-March-2011
7 | * @brief This file contains all the functions prototypes for the IWDG
8 | * firmware library.
9 | ******************************************************************************
10 | * @attention
11 | *
12 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
13 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
14 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
15 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
16 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
17 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
18 | *
19 | * © COPYRIGHT 2011 STMicroelectronics
20 | ******************************************************************************
21 | */
22 |
23 | /* Define to prevent recursive inclusion -------------------------------------*/
24 | #ifndef __STM32F10x_IWDG_H
25 | #define __STM32F10x_IWDG_H
26 |
27 | #ifdef __cplusplus
28 | extern "C" {
29 | #endif
30 |
31 | /* Includes ------------------------------------------------------------------*/
32 | #include "stm32f10x.h"
33 |
34 | /** @addtogroup STM32F10x_StdPeriph_Driver
35 | * @{
36 | */
37 |
38 | /** @addtogroup IWDG
39 | * @{
40 | */
41 |
42 | /** @defgroup IWDG_Exported_Types
43 | * @{
44 | */
45 |
46 | /**
47 | * @}
48 | */
49 |
50 | /** @defgroup IWDG_Exported_Constants
51 | * @{
52 | */
53 |
54 | /** @defgroup IWDG_WriteAccess
55 | * @{
56 | */
57 |
58 | #define IWDG_WriteAccess_Enable ((uint16_t)0x5555)
59 | #define IWDG_WriteAccess_Disable ((uint16_t)0x0000)
60 | #define IS_IWDG_WRITE_ACCESS(ACCESS) (((ACCESS) == IWDG_WriteAccess_Enable) || \
61 | ((ACCESS) == IWDG_WriteAccess_Disable))
62 | /**
63 | * @}
64 | */
65 |
66 | /** @defgroup IWDG_prescaler
67 | * @{
68 | */
69 |
70 | #define IWDG_Prescaler_4 ((uint8_t)0x00)
71 | #define IWDG_Prescaler_8 ((uint8_t)0x01)
72 | #define IWDG_Prescaler_16 ((uint8_t)0x02)
73 | #define IWDG_Prescaler_32 ((uint8_t)0x03)
74 | #define IWDG_Prescaler_64 ((uint8_t)0x04)
75 | #define IWDG_Prescaler_128 ((uint8_t)0x05)
76 | #define IWDG_Prescaler_256 ((uint8_t)0x06)
77 | #define IS_IWDG_PRESCALER(PRESCALER) (((PRESCALER) == IWDG_Prescaler_4) || \
78 | ((PRESCALER) == IWDG_Prescaler_8) || \
79 | ((PRESCALER) == IWDG_Prescaler_16) || \
80 | ((PRESCALER) == IWDG_Prescaler_32) || \
81 | ((PRESCALER) == IWDG_Prescaler_64) || \
82 | ((PRESCALER) == IWDG_Prescaler_128)|| \
83 | ((PRESCALER) == IWDG_Prescaler_256))
84 | /**
85 | * @}
86 | */
87 |
88 | /** @defgroup IWDG_Flag
89 | * @{
90 | */
91 |
92 | #define IWDG_FLAG_PVU ((uint16_t)0x0001)
93 | #define IWDG_FLAG_RVU ((uint16_t)0x0002)
94 | #define IS_IWDG_FLAG(FLAG) (((FLAG) == IWDG_FLAG_PVU) || ((FLAG) == IWDG_FLAG_RVU))
95 | #define IS_IWDG_RELOAD(RELOAD) ((RELOAD) <= 0xFFF)
96 | /**
97 | * @}
98 | */
99 |
100 | /**
101 | * @}
102 | */
103 |
104 | /** @defgroup IWDG_Exported_Macros
105 | * @{
106 | */
107 |
108 | /**
109 | * @}
110 | */
111 |
112 | /** @defgroup IWDG_Exported_Functions
113 | * @{
114 | */
115 |
116 | void IWDG_WriteAccessCmd(uint16_t IWDG_WriteAccess);
117 | void IWDG_SetPrescaler(uint8_t IWDG_Prescaler);
118 | void IWDG_SetReload(uint16_t Reload);
119 | void IWDG_ReloadCounter(void);
120 | void IWDG_Enable(void);
121 | FlagStatus IWDG_GetFlagStatus(uint16_t IWDG_FLAG);
122 |
123 | #ifdef __cplusplus
124 | }
125 | #endif
126 |
127 | #endif /* __STM32F10x_IWDG_H */
128 | /**
129 | * @}
130 | */
131 |
132 | /**
133 | * @}
134 | */
135 |
136 | /**
137 | * @}
138 | */
139 |
140 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/
141 |
--------------------------------------------------------------------------------
/hardware/stm32_timersync-open/STM32F10x_FWLib/inc/stm32f10x_pwr.h:
--------------------------------------------------------------------------------
1 | /**
2 | ******************************************************************************
3 | * @file stm32f10x_pwr.h
4 | * @author MCD Application Team
5 | * @version V3.5.0
6 | * @date 11-March-2011
7 | * @brief This file contains all the functions prototypes for the PWR firmware
8 | * library.
9 | ******************************************************************************
10 | * @attention
11 | *
12 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
13 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
14 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
15 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
16 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
17 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
18 | *
19 | * © COPYRIGHT 2011 STMicroelectronics
20 | ******************************************************************************
21 | */
22 |
23 | /* Define to prevent recursive inclusion -------------------------------------*/
24 | #ifndef __STM32F10x_PWR_H
25 | #define __STM32F10x_PWR_H
26 |
27 | #ifdef __cplusplus
28 | extern "C" {
29 | #endif
30 |
31 | /* Includes ------------------------------------------------------------------*/
32 | #include "stm32f10x.h"
33 |
34 | /** @addtogroup STM32F10x_StdPeriph_Driver
35 | * @{
36 | */
37 |
38 | /** @addtogroup PWR
39 | * @{
40 | */
41 |
42 | /** @defgroup PWR_Exported_Types
43 | * @{
44 | */
45 |
46 | /**
47 | * @}
48 | */
49 |
50 | /** @defgroup PWR_Exported_Constants
51 | * @{
52 | */
53 |
54 | /** @defgroup PVD_detection_level
55 | * @{
56 | */
57 |
58 | #define PWR_PVDLevel_2V2 ((uint32_t)0x00000000)
59 | #define PWR_PVDLevel_2V3 ((uint32_t)0x00000020)
60 | #define PWR_PVDLevel_2V4 ((uint32_t)0x00000040)
61 | #define PWR_PVDLevel_2V5 ((uint32_t)0x00000060)
62 | #define PWR_PVDLevel_2V6 ((uint32_t)0x00000080)
63 | #define PWR_PVDLevel_2V7 ((uint32_t)0x000000A0)
64 | #define PWR_PVDLevel_2V8 ((uint32_t)0x000000C0)
65 | #define PWR_PVDLevel_2V9 ((uint32_t)0x000000E0)
66 | #define IS_PWR_PVD_LEVEL(LEVEL) (((LEVEL) == PWR_PVDLevel_2V2) || ((LEVEL) == PWR_PVDLevel_2V3)|| \
67 | ((LEVEL) == PWR_PVDLevel_2V4) || ((LEVEL) == PWR_PVDLevel_2V5)|| \
68 | ((LEVEL) == PWR_PVDLevel_2V6) || ((LEVEL) == PWR_PVDLevel_2V7)|| \
69 | ((LEVEL) == PWR_PVDLevel_2V8) || ((LEVEL) == PWR_PVDLevel_2V9))
70 | /**
71 | * @}
72 | */
73 |
74 | /** @defgroup Regulator_state_is_STOP_mode
75 | * @{
76 | */
77 |
78 | #define PWR_Regulator_ON ((uint32_t)0x00000000)
79 | #define PWR_Regulator_LowPower ((uint32_t)0x00000001)
80 | #define IS_PWR_REGULATOR(REGULATOR) (((REGULATOR) == PWR_Regulator_ON) || \
81 | ((REGULATOR) == PWR_Regulator_LowPower))
82 | /**
83 | * @}
84 | */
85 |
86 | /** @defgroup STOP_mode_entry
87 | * @{
88 | */
89 |
90 | #define PWR_STOPEntry_WFI ((uint8_t)0x01)
91 | #define PWR_STOPEntry_WFE ((uint8_t)0x02)
92 | #define IS_PWR_STOP_ENTRY(ENTRY) (((ENTRY) == PWR_STOPEntry_WFI) || ((ENTRY) == PWR_STOPEntry_WFE))
93 |
94 | /**
95 | * @}
96 | */
97 |
98 | /** @defgroup PWR_Flag
99 | * @{
100 | */
101 |
102 | #define PWR_FLAG_WU ((uint32_t)0x00000001)
103 | #define PWR_FLAG_SB ((uint32_t)0x00000002)
104 | #define PWR_FLAG_PVDO ((uint32_t)0x00000004)
105 | #define IS_PWR_GET_FLAG(FLAG) (((FLAG) == PWR_FLAG_WU) || ((FLAG) == PWR_FLAG_SB) || \
106 | ((FLAG) == PWR_FLAG_PVDO))
107 |
108 | #define IS_PWR_CLEAR_FLAG(FLAG) (((FLAG) == PWR_FLAG_WU) || ((FLAG) == PWR_FLAG_SB))
109 | /**
110 | * @}
111 | */
112 |
113 | /**
114 | * @}
115 | */
116 |
117 | /** @defgroup PWR_Exported_Macros
118 | * @{
119 | */
120 |
121 | /**
122 | * @}
123 | */
124 |
125 | /** @defgroup PWR_Exported_Functions
126 | * @{
127 | */
128 |
129 | void PWR_DeInit(void);
130 | void PWR_BackupAccessCmd(FunctionalState NewState);
131 | void PWR_PVDCmd(FunctionalState NewState);
132 | void PWR_PVDLevelConfig(uint32_t PWR_PVDLevel);
133 | void PWR_WakeUpPinCmd(FunctionalState NewState);
134 | void PWR_EnterSTOPMode(uint32_t PWR_Regulator, uint8_t PWR_STOPEntry);
135 | void PWR_EnterSTANDBYMode(void);
136 | FlagStatus PWR_GetFlagStatus(uint32_t PWR_FLAG);
137 | void PWR_ClearFlag(uint32_t PWR_FLAG);
138 |
139 | #ifdef __cplusplus
140 | }
141 | #endif
142 |
143 | #endif /* __STM32F10x_PWR_H */
144 | /**
145 | * @}
146 | */
147 |
148 | /**
149 | * @}
150 | */
151 |
152 | /**
153 | * @}
154 | */
155 |
156 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/
157 |
--------------------------------------------------------------------------------
/hardware/stm32_timersync-open/STM32F10x_FWLib/inc/stm32f10x_rtc.h:
--------------------------------------------------------------------------------
1 | /**
2 | ******************************************************************************
3 | * @file stm32f10x_rtc.h
4 | * @author MCD Application Team
5 | * @version V3.5.0
6 | * @date 11-March-2011
7 | * @brief This file contains all the functions prototypes for the RTC firmware
8 | * library.
9 | ******************************************************************************
10 | * @attention
11 | *
12 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
13 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
14 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
15 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
16 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
17 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
18 | *
19 | * © COPYRIGHT 2011 STMicroelectronics
20 | ******************************************************************************
21 | */
22 |
23 | /* Define to prevent recursive inclusion -------------------------------------*/
24 | #ifndef __STM32F10x_RTC_H
25 | #define __STM32F10x_RTC_H
26 |
27 | #ifdef __cplusplus
28 | extern "C" {
29 | #endif
30 |
31 | /* Includes ------------------------------------------------------------------*/
32 | #include "stm32f10x.h"
33 |
34 | /** @addtogroup STM32F10x_StdPeriph_Driver
35 | * @{
36 | */
37 |
38 | /** @addtogroup RTC
39 | * @{
40 | */
41 |
42 | /** @defgroup RTC_Exported_Types
43 | * @{
44 | */
45 |
46 | /**
47 | * @}
48 | */
49 |
50 | /** @defgroup RTC_Exported_Constants
51 | * @{
52 | */
53 |
54 | /** @defgroup RTC_interrupts_define
55 | * @{
56 | */
57 |
58 | #define RTC_IT_OW ((uint16_t)0x0004) /*!< Overflow interrupt */
59 | #define RTC_IT_ALR ((uint16_t)0x0002) /*!< Alarm interrupt */
60 | #define RTC_IT_SEC ((uint16_t)0x0001) /*!< Second interrupt */
61 | #define IS_RTC_IT(IT) ((((IT) & (uint16_t)0xFFF8) == 0x00) && ((IT) != 0x00))
62 | #define IS_RTC_GET_IT(IT) (((IT) == RTC_IT_OW) || ((IT) == RTC_IT_ALR) || \
63 | ((IT) == RTC_IT_SEC))
64 | /**
65 | * @}
66 | */
67 |
68 | /** @defgroup RTC_interrupts_flags
69 | * @{
70 | */
71 |
72 | #define RTC_FLAG_RTOFF ((uint16_t)0x0020) /*!< RTC Operation OFF flag */
73 | #define RTC_FLAG_RSF ((uint16_t)0x0008) /*!< Registers Synchronized flag */
74 | #define RTC_FLAG_OW ((uint16_t)0x0004) /*!< Overflow flag */
75 | #define RTC_FLAG_ALR ((uint16_t)0x0002) /*!< Alarm flag */
76 | #define RTC_FLAG_SEC ((uint16_t)0x0001) /*!< Second flag */
77 | #define IS_RTC_CLEAR_FLAG(FLAG) ((((FLAG) & (uint16_t)0xFFF0) == 0x00) && ((FLAG) != 0x00))
78 | #define IS_RTC_GET_FLAG(FLAG) (((FLAG) == RTC_FLAG_RTOFF) || ((FLAG) == RTC_FLAG_RSF) || \
79 | ((FLAG) == RTC_FLAG_OW) || ((FLAG) == RTC_FLAG_ALR) || \
80 | ((FLAG) == RTC_FLAG_SEC))
81 | #define IS_RTC_PRESCALER(PRESCALER) ((PRESCALER) <= 0xFFFFF)
82 |
83 | /**
84 | * @}
85 | */
86 |
87 | /**
88 | * @}
89 | */
90 |
91 | /** @defgroup RTC_Exported_Macros
92 | * @{
93 | */
94 |
95 | /**
96 | * @}
97 | */
98 |
99 | /** @defgroup RTC_Exported_Functions
100 | * @{
101 | */
102 |
103 | void RTC_ITConfig(uint16_t RTC_IT, FunctionalState NewState);
104 | void RTC_EnterConfigMode(void);
105 | void RTC_ExitConfigMode(void);
106 | uint32_t RTC_GetCounter(void);
107 | void RTC_SetCounter(uint32_t CounterValue);
108 | void RTC_SetPrescaler(uint32_t PrescalerValue);
109 | void RTC_SetAlarm(uint32_t AlarmValue);
110 | uint32_t RTC_GetDivider(void);
111 | void RTC_WaitForLastTask(void);
112 | void RTC_WaitForSynchro(void);
113 | FlagStatus RTC_GetFlagStatus(uint16_t RTC_FLAG);
114 | void RTC_ClearFlag(uint16_t RTC_FLAG);
115 | ITStatus RTC_GetITStatus(uint16_t RTC_IT);
116 | void RTC_ClearITPendingBit(uint16_t RTC_IT);
117 |
118 | #ifdef __cplusplus
119 | }
120 | #endif
121 |
122 | #endif /* __STM32F10x_RTC_H */
123 | /**
124 | * @}
125 | */
126 |
127 | /**
128 | * @}
129 | */
130 |
131 | /**
132 | * @}
133 | */
134 |
135 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/
136 |
--------------------------------------------------------------------------------
/hardware/stm32_timersync-open/STM32F10x_FWLib/inc/stm32f10x_wwdg.h:
--------------------------------------------------------------------------------
1 | /**
2 | ******************************************************************************
3 | * @file stm32f10x_wwdg.h
4 | * @author MCD Application Team
5 | * @version V3.5.0
6 | * @date 11-March-2011
7 | * @brief This file contains all the functions prototypes for the WWDG firmware
8 | * library.
9 | ******************************************************************************
10 | * @attention
11 | *
12 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
13 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
14 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
15 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
16 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
17 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
18 | *
19 | * © COPYRIGHT 2011 STMicroelectronics
20 | ******************************************************************************
21 | */
22 |
23 | /* Define to prevent recursive inclusion -------------------------------------*/
24 | #ifndef __STM32F10x_WWDG_H
25 | #define __STM32F10x_WWDG_H
26 |
27 | #ifdef __cplusplus
28 | extern "C" {
29 | #endif
30 |
31 | /* Includes ------------------------------------------------------------------*/
32 | #include "stm32f10x.h"
33 |
34 | /** @addtogroup STM32F10x_StdPeriph_Driver
35 | * @{
36 | */
37 |
38 | /** @addtogroup WWDG
39 | * @{
40 | */
41 |
42 | /** @defgroup WWDG_Exported_Types
43 | * @{
44 | */
45 |
46 | /**
47 | * @}
48 | */
49 |
50 | /** @defgroup WWDG_Exported_Constants
51 | * @{
52 | */
53 |
54 | /** @defgroup WWDG_Prescaler
55 | * @{
56 | */
57 |
58 | #define WWDG_Prescaler_1 ((uint32_t)0x00000000)
59 | #define WWDG_Prescaler_2 ((uint32_t)0x00000080)
60 | #define WWDG_Prescaler_4 ((uint32_t)0x00000100)
61 | #define WWDG_Prescaler_8 ((uint32_t)0x00000180)
62 | #define IS_WWDG_PRESCALER(PRESCALER) (((PRESCALER) == WWDG_Prescaler_1) || \
63 | ((PRESCALER) == WWDG_Prescaler_2) || \
64 | ((PRESCALER) == WWDG_Prescaler_4) || \
65 | ((PRESCALER) == WWDG_Prescaler_8))
66 | #define IS_WWDG_WINDOW_VALUE(VALUE) ((VALUE) <= 0x7F)
67 | #define IS_WWDG_COUNTER(COUNTER) (((COUNTER) >= 0x40) && ((COUNTER) <= 0x7F))
68 |
69 | /**
70 | * @}
71 | */
72 |
73 | /**
74 | * @}
75 | */
76 |
77 | /** @defgroup WWDG_Exported_Macros
78 | * @{
79 | */
80 | /**
81 | * @}
82 | */
83 |
84 | /** @defgroup WWDG_Exported_Functions
85 | * @{
86 | */
87 |
88 | void WWDG_DeInit(void);
89 | void WWDG_SetPrescaler(uint32_t WWDG_Prescaler);
90 | void WWDG_SetWindowValue(uint8_t WindowValue);
91 | void WWDG_EnableIT(void);
92 | void WWDG_SetCounter(uint8_t Counter);
93 | void WWDG_Enable(uint8_t Counter);
94 | FlagStatus WWDG_GetFlagStatus(void);
95 | void WWDG_ClearFlag(void);
96 |
97 | #ifdef __cplusplus
98 | }
99 | #endif
100 |
101 | #endif /* __STM32F10x_WWDG_H */
102 |
103 | /**
104 | * @}
105 | */
106 |
107 | /**
108 | * @}
109 | */
110 |
111 | /**
112 | * @}
113 | */
114 |
115 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/
116 |
--------------------------------------------------------------------------------
/hardware/stm32_timersync-open/STM32F10x_FWLib/src/misc.c:
--------------------------------------------------------------------------------
1 | /**
2 | ******************************************************************************
3 | * @file misc.c
4 | * @author MCD Application Team
5 | * @version V3.5.0
6 | * @date 11-March-2011
7 | * @brief This file provides all the miscellaneous firmware functions (add-on
8 | * to CMSIS functions).
9 | ******************************************************************************
10 | * @attention
11 | *
12 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
13 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
14 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
15 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
16 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
17 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
18 | *
19 | * © COPYRIGHT 2011 STMicroelectronics
20 | ******************************************************************************
21 | */
22 |
23 | /* Includes ------------------------------------------------------------------*/
24 | #include "misc.h"
25 |
26 | /** @addtogroup STM32F10x_StdPeriph_Driver
27 | * @{
28 | */
29 |
30 | /** @defgroup MISC
31 | * @brief MISC driver modules
32 | * @{
33 | */
34 |
35 | /** @defgroup MISC_Private_TypesDefinitions
36 | * @{
37 | */
38 |
39 | /**
40 | * @}
41 | */
42 |
43 | /** @defgroup MISC_Private_Defines
44 | * @{
45 | */
46 |
47 | #define AIRCR_VECTKEY_MASK ((uint32_t)0x05FA0000)
48 | /**
49 | * @}
50 | */
51 |
52 | /** @defgroup MISC_Private_Macros
53 | * @{
54 | */
55 |
56 | /**
57 | * @}
58 | */
59 |
60 | /** @defgroup MISC_Private_Variables
61 | * @{
62 | */
63 |
64 | /**
65 | * @}
66 | */
67 |
68 | /** @defgroup MISC_Private_FunctionPrototypes
69 | * @{
70 | */
71 |
72 | /**
73 | * @}
74 | */
75 |
76 | /** @defgroup MISC_Private_Functions
77 | * @{
78 | */
79 |
80 | /**
81 | * @brief Configures the priority grouping: pre-emption priority and subpriority.
82 | * @param NVIC_PriorityGroup: specifies the priority grouping bits length.
83 | * This parameter can be one of the following values:
84 | * @arg NVIC_PriorityGroup_0: 0 bits for pre-emption priority
85 | * 4 bits for subpriority
86 | * @arg NVIC_PriorityGroup_1: 1 bits for pre-emption priority
87 | * 3 bits for subpriority
88 | * @arg NVIC_PriorityGroup_2: 2 bits for pre-emption priority
89 | * 2 bits for subpriority
90 | * @arg NVIC_PriorityGroup_3: 3 bits for pre-emption priority
91 | * 1 bits for subpriority
92 | * @arg NVIC_PriorityGroup_4: 4 bits for pre-emption priority
93 | * 0 bits for subpriority
94 | * @retval None
95 | */
96 | void NVIC_PriorityGroupConfig(uint32_t NVIC_PriorityGroup)
97 | {
98 | /* Check the parameters */
99 | assert_param(IS_NVIC_PRIORITY_GROUP(NVIC_PriorityGroup));
100 |
101 | /* Set the PRIGROUP[10:8] bits according to NVIC_PriorityGroup value */
102 | SCB->AIRCR = AIRCR_VECTKEY_MASK | NVIC_PriorityGroup;
103 | }
104 |
105 | /**
106 | * @brief Initializes the NVIC peripheral according to the specified
107 | * parameters in the NVIC_InitStruct.
108 | * @param NVIC_InitStruct: pointer to a NVIC_InitTypeDef structure that contains
109 | * the configuration information for the specified NVIC peripheral.
110 | * @retval None
111 | */
112 | void NVIC_Init(NVIC_InitTypeDef* NVIC_InitStruct)
113 | {
114 | uint32_t tmppriority = 0x00, tmppre = 0x00, tmpsub = 0x0F;
115 |
116 | /* Check the parameters */
117 | assert_param(IS_FUNCTIONAL_STATE(NVIC_InitStruct->NVIC_IRQChannelCmd));
118 | assert_param(IS_NVIC_PREEMPTION_PRIORITY(NVIC_InitStruct->NVIC_IRQChannelPreemptionPriority));
119 | assert_param(IS_NVIC_SUB_PRIORITY(NVIC_InitStruct->NVIC_IRQChannelSubPriority));
120 |
121 | if (NVIC_InitStruct->NVIC_IRQChannelCmd != DISABLE)
122 | {
123 | /* Compute the Corresponding IRQ Priority --------------------------------*/
124 | tmppriority = (0x700 - ((SCB->AIRCR) & (uint32_t)0x700))>> 0x08;
125 | tmppre = (0x4 - tmppriority);
126 | tmpsub = tmpsub >> tmppriority;
127 |
128 | tmppriority = (uint32_t)NVIC_InitStruct->NVIC_IRQChannelPreemptionPriority << tmppre;
129 | tmppriority |= NVIC_InitStruct->NVIC_IRQChannelSubPriority & tmpsub;
130 | tmppriority = tmppriority << 0x04;
131 |
132 | NVIC->IP[NVIC_InitStruct->NVIC_IRQChannel] = tmppriority;
133 |
134 | /* Enable the Selected IRQ Channels --------------------------------------*/
135 | NVIC->ISER[NVIC_InitStruct->NVIC_IRQChannel >> 0x05] =
136 | (uint32_t)0x01 << (NVIC_InitStruct->NVIC_IRQChannel & (uint8_t)0x1F);
137 | }
138 | else
139 | {
140 | /* Disable the Selected IRQ Channels -------------------------------------*/
141 | NVIC->ICER[NVIC_InitStruct->NVIC_IRQChannel >> 0x05] =
142 | (uint32_t)0x01 << (NVIC_InitStruct->NVIC_IRQChannel & (uint8_t)0x1F);
143 | }
144 | }
145 |
146 | /**
147 | * @brief Sets the vector table location and Offset.
148 | * @param NVIC_VectTab: specifies if the vector table is in RAM or FLASH memory.
149 | * This parameter can be one of the following values:
150 | * @arg NVIC_VectTab_RAM
151 | * @arg NVIC_VectTab_FLASH
152 | * @param Offset: Vector Table base offset field. This value must be a multiple
153 | * of 0x200.
154 | * @retval None
155 | */
156 | void NVIC_SetVectorTable(uint32_t NVIC_VectTab, uint32_t Offset)
157 | {
158 | /* Check the parameters */
159 | assert_param(IS_NVIC_VECTTAB(NVIC_VectTab));
160 | assert_param(IS_NVIC_OFFSET(Offset));
161 |
162 | SCB->VTOR = NVIC_VectTab | (Offset & (uint32_t)0x1FFFFF80);
163 | }
164 |
165 | /**
166 | * @brief Selects the condition for the system to enter low power mode.
167 | * @param LowPowerMode: Specifies the new mode for the system to enter low power mode.
168 | * This parameter can be one of the following values:
169 | * @arg NVIC_LP_SEVONPEND
170 | * @arg NVIC_LP_SLEEPDEEP
171 | * @arg NVIC_LP_SLEEPONEXIT
172 | * @param NewState: new state of LP condition. This parameter can be: ENABLE or DISABLE.
173 | * @retval None
174 | */
175 | void NVIC_SystemLPConfig(uint8_t LowPowerMode, FunctionalState NewState)
176 | {
177 | /* Check the parameters */
178 | assert_param(IS_NVIC_LP(LowPowerMode));
179 | assert_param(IS_FUNCTIONAL_STATE(NewState));
180 |
181 | if (NewState != DISABLE)
182 | {
183 | SCB->SCR |= LowPowerMode;
184 | }
185 | else
186 | {
187 | SCB->SCR &= (uint32_t)(~(uint32_t)LowPowerMode);
188 | }
189 | }
190 |
191 | /**
192 | * @brief Configures the SysTick clock source.
193 | * @param SysTick_CLKSource: specifies the SysTick clock source.
194 | * This parameter can be one of the following values:
195 | * @arg SysTick_CLKSource_HCLK_Div8: AHB clock divided by 8 selected as SysTick clock source.
196 | * @arg SysTick_CLKSource_HCLK: AHB clock selected as SysTick clock source.
197 | * @retval None
198 | */
199 | void SysTick_CLKSourceConfig(uint32_t SysTick_CLKSource)
200 | {
201 | /* Check the parameters */
202 | assert_param(IS_SYSTICK_CLK_SOURCE(SysTick_CLKSource));
203 | if (SysTick_CLKSource == SysTick_CLKSource_HCLK)
204 | {
205 | SysTick->CTRL |= SysTick_CLKSource_HCLK;
206 | }
207 | else
208 | {
209 | SysTick->CTRL &= SysTick_CLKSource_HCLK_Div8;
210 | }
211 | }
212 |
213 | /**
214 | * @}
215 | */
216 |
217 | /**
218 | * @}
219 | */
220 |
221 | /**
222 | * @}
223 | */
224 |
225 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/
226 |
--------------------------------------------------------------------------------
/hardware/stm32_timersync-open/STM32F10x_FWLib/src/stm32f10x_crc.c:
--------------------------------------------------------------------------------
1 | /**
2 | ******************************************************************************
3 | * @file stm32f10x_crc.c
4 | * @author MCD Application Team
5 | * @version V3.5.0
6 | * @date 11-March-2011
7 | * @brief This file provides all the CRC firmware functions.
8 | ******************************************************************************
9 | * @attention
10 | *
11 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
12 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
13 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
14 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
15 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
16 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
17 | *
18 | * © COPYRIGHT 2011 STMicroelectronics
19 | ******************************************************************************
20 | */
21 |
22 | /* Includes ------------------------------------------------------------------*/
23 | #include "stm32f10x_crc.h"
24 |
25 | /** @addtogroup STM32F10x_StdPeriph_Driver
26 | * @{
27 | */
28 |
29 | /** @defgroup CRC
30 | * @brief CRC driver modules
31 | * @{
32 | */
33 |
34 | /** @defgroup CRC_Private_TypesDefinitions
35 | * @{
36 | */
37 |
38 | /**
39 | * @}
40 | */
41 |
42 | /** @defgroup CRC_Private_Defines
43 | * @{
44 | */
45 |
46 | /**
47 | * @}
48 | */
49 |
50 | /** @defgroup CRC_Private_Macros
51 | * @{
52 | */
53 |
54 | /**
55 | * @}
56 | */
57 |
58 | /** @defgroup CRC_Private_Variables
59 | * @{
60 | */
61 |
62 | /**
63 | * @}
64 | */
65 |
66 | /** @defgroup CRC_Private_FunctionPrototypes
67 | * @{
68 | */
69 |
70 | /**
71 | * @}
72 | */
73 |
74 | /** @defgroup CRC_Private_Functions
75 | * @{
76 | */
77 |
78 | /**
79 | * @brief Resets the CRC Data register (DR).
80 | * @param None
81 | * @retval None
82 | */
83 | void CRC_ResetDR(void)
84 | {
85 | /* Reset CRC generator */
86 | CRC->CR = CRC_CR_RESET;
87 | }
88 |
89 | /**
90 | * @brief Computes the 32-bit CRC of a given data word(32-bit).
91 | * @param Data: data word(32-bit) to compute its CRC
92 | * @retval 32-bit CRC
93 | */
94 | uint32_t CRC_CalcCRC(uint32_t Data)
95 | {
96 | CRC->DR = Data;
97 |
98 | return (CRC->DR);
99 | }
100 |
101 | /**
102 | * @brief Computes the 32-bit CRC of a given buffer of data word(32-bit).
103 | * @param pBuffer: pointer to the buffer containing the data to be computed
104 | * @param BufferLength: length of the buffer to be computed
105 | * @retval 32-bit CRC
106 | */
107 | uint32_t CRC_CalcBlockCRC(uint32_t pBuffer[], uint32_t BufferLength)
108 | {
109 | uint32_t index = 0;
110 |
111 | for(index = 0; index < BufferLength; index++)
112 | {
113 | CRC->DR = pBuffer[index];
114 | }
115 | return (CRC->DR);
116 | }
117 |
118 | /**
119 | * @brief Returns the current CRC value.
120 | * @param None
121 | * @retval 32-bit CRC
122 | */
123 | uint32_t CRC_GetCRC(void)
124 | {
125 | return (CRC->DR);
126 | }
127 |
128 | /**
129 | * @brief Stores a 8-bit data in the Independent Data(ID) register.
130 | * @param IDValue: 8-bit value to be stored in the ID register
131 | * @retval None
132 | */
133 | void CRC_SetIDRegister(uint8_t IDValue)
134 | {
135 | CRC->IDR = IDValue;
136 | }
137 |
138 | /**
139 | * @brief Returns the 8-bit data stored in the Independent Data(ID) register
140 | * @param None
141 | * @retval 8-bit value of the ID register
142 | */
143 | uint8_t CRC_GetIDRegister(void)
144 | {
145 | return (CRC->IDR);
146 | }
147 |
148 | /**
149 | * @}
150 | */
151 |
152 | /**
153 | * @}
154 | */
155 |
156 | /**
157 | * @}
158 | */
159 |
160 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/
161 |
--------------------------------------------------------------------------------
/hardware/stm32_timersync-open/STM32F10x_FWLib/src/stm32f10x_dbgmcu.c:
--------------------------------------------------------------------------------
1 | /**
2 | ******************************************************************************
3 | * @file stm32f10x_dbgmcu.c
4 | * @author MCD Application Team
5 | * @version V3.5.0
6 | * @date 11-March-2011
7 | * @brief This file provides all the DBGMCU firmware functions.
8 | ******************************************************************************
9 | * @attention
10 | *
11 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
12 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
13 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
14 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
15 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
16 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
17 | *
18 | * © COPYRIGHT 2011 STMicroelectronics
19 | ******************************************************************************
20 | */
21 |
22 | /* Includes ------------------------------------------------------------------*/
23 | #include "stm32f10x_dbgmcu.h"
24 |
25 | /** @addtogroup STM32F10x_StdPeriph_Driver
26 | * @{
27 | */
28 |
29 | /** @defgroup DBGMCU
30 | * @brief DBGMCU driver modules
31 | * @{
32 | */
33 |
34 | /** @defgroup DBGMCU_Private_TypesDefinitions
35 | * @{
36 | */
37 |
38 | /**
39 | * @}
40 | */
41 |
42 | /** @defgroup DBGMCU_Private_Defines
43 | * @{
44 | */
45 |
46 | #define IDCODE_DEVID_MASK ((uint32_t)0x00000FFF)
47 | /**
48 | * @}
49 | */
50 |
51 | /** @defgroup DBGMCU_Private_Macros
52 | * @{
53 | */
54 |
55 | /**
56 | * @}
57 | */
58 |
59 | /** @defgroup DBGMCU_Private_Variables
60 | * @{
61 | */
62 |
63 | /**
64 | * @}
65 | */
66 |
67 | /** @defgroup DBGMCU_Private_FunctionPrototypes
68 | * @{
69 | */
70 |
71 | /**
72 | * @}
73 | */
74 |
75 | /** @defgroup DBGMCU_Private_Functions
76 | * @{
77 | */
78 |
79 | /**
80 | * @brief Returns the device revision identifier.
81 | * @param None
82 | * @retval Device revision identifier
83 | */
84 | uint32_t DBGMCU_GetREVID(void)
85 | {
86 | return(DBGMCU->IDCODE >> 16);
87 | }
88 |
89 | /**
90 | * @brief Returns the device identifier.
91 | * @param None
92 | * @retval Device identifier
93 | */
94 | uint32_t DBGMCU_GetDEVID(void)
95 | {
96 | return(DBGMCU->IDCODE & IDCODE_DEVID_MASK);
97 | }
98 |
99 | /**
100 | * @brief Configures the specified peripheral and low power mode behavior
101 | * when the MCU under Debug mode.
102 | * @param DBGMCU_Periph: specifies the peripheral and low power mode.
103 | * This parameter can be any combination of the following values:
104 | * @arg DBGMCU_SLEEP: Keep debugger connection during SLEEP mode
105 | * @arg DBGMCU_STOP: Keep debugger connection during STOP mode
106 | * @arg DBGMCU_STANDBY: Keep debugger connection during STANDBY mode
107 | * @arg DBGMCU_IWDG_STOP: Debug IWDG stopped when Core is halted
108 | * @arg DBGMCU_WWDG_STOP: Debug WWDG stopped when Core is halted
109 | * @arg DBGMCU_TIM1_STOP: TIM1 counter stopped when Core is halted
110 | * @arg DBGMCU_TIM2_STOP: TIM2 counter stopped when Core is halted
111 | * @arg DBGMCU_TIM3_STOP: TIM3 counter stopped when Core is halted
112 | * @arg DBGMCU_TIM4_STOP: TIM4 counter stopped when Core is halted
113 | * @arg DBGMCU_CAN1_STOP: Debug CAN2 stopped when Core is halted
114 | * @arg DBGMCU_I2C1_SMBUS_TIMEOUT: I2C1 SMBUS timeout mode stopped when Core is halted
115 | * @arg DBGMCU_I2C2_SMBUS_TIMEOUT: I2C2 SMBUS timeout mode stopped when Core is halted
116 | * @arg DBGMCU_TIM5_STOP: TIM5 counter stopped when Core is halted
117 | * @arg DBGMCU_TIM6_STOP: TIM6 counter stopped when Core is halted
118 | * @arg DBGMCU_TIM7_STOP: TIM7 counter stopped when Core is halted
119 | * @arg DBGMCU_TIM8_STOP: TIM8 counter stopped when Core is halted
120 | * @arg DBGMCU_CAN2_STOP: Debug CAN2 stopped when Core is halted
121 | * @arg DBGMCU_TIM15_STOP: TIM15 counter stopped when Core is halted
122 | * @arg DBGMCU_TIM16_STOP: TIM16 counter stopped when Core is halted
123 | * @arg DBGMCU_TIM17_STOP: TIM17 counter stopped when Core is halted
124 | * @arg DBGMCU_TIM9_STOP: TIM9 counter stopped when Core is halted
125 | * @arg DBGMCU_TIM10_STOP: TIM10 counter stopped when Core is halted
126 | * @arg DBGMCU_TIM11_STOP: TIM11 counter stopped when Core is halted
127 | * @arg DBGMCU_TIM12_STOP: TIM12 counter stopped when Core is halted
128 | * @arg DBGMCU_TIM13_STOP: TIM13 counter stopped when Core is halted
129 | * @arg DBGMCU_TIM14_STOP: TIM14 counter stopped when Core is halted
130 | * @param NewState: new state of the specified peripheral in Debug mode.
131 | * This parameter can be: ENABLE or DISABLE.
132 | * @retval None
133 | */
134 | void DBGMCU_Config(uint32_t DBGMCU_Periph, FunctionalState NewState)
135 | {
136 | /* Check the parameters */
137 | assert_param(IS_DBGMCU_PERIPH(DBGMCU_Periph));
138 | assert_param(IS_FUNCTIONAL_STATE(NewState));
139 |
140 | if (NewState != DISABLE)
141 | {
142 | DBGMCU->CR |= DBGMCU_Periph;
143 | }
144 | else
145 | {
146 | DBGMCU->CR &= ~DBGMCU_Periph;
147 | }
148 | }
149 |
150 | /**
151 | * @}
152 | */
153 |
154 | /**
155 | * @}
156 | */
157 |
158 | /**
159 | * @}
160 | */
161 |
162 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/
163 |
--------------------------------------------------------------------------------
/hardware/stm32_timersync-open/STM32F10x_FWLib/src/stm32f10x_exti.c:
--------------------------------------------------------------------------------
1 | /**
2 | ******************************************************************************
3 | * @file stm32f10x_exti.c
4 | * @author MCD Application Team
5 | * @version V3.5.0
6 | * @date 11-March-2011
7 | * @brief This file provides all the EXTI firmware functions.
8 | ******************************************************************************
9 | * @attention
10 | *
11 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
12 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
13 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
14 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
15 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
16 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
17 | *
18 | * © COPYRIGHT 2011 STMicroelectronics
19 | ******************************************************************************
20 | */
21 |
22 | /* Includes ------------------------------------------------------------------*/
23 | #include "stm32f10x_exti.h"
24 |
25 | /** @addtogroup STM32F10x_StdPeriph_Driver
26 | * @{
27 | */
28 |
29 | /** @defgroup EXTI
30 | * @brief EXTI driver modules
31 | * @{
32 | */
33 |
34 | /** @defgroup EXTI_Private_TypesDefinitions
35 | * @{
36 | */
37 |
38 | /**
39 | * @}
40 | */
41 |
42 | /** @defgroup EXTI_Private_Defines
43 | * @{
44 | */
45 |
46 | #define EXTI_LINENONE ((uint32_t)0x00000) /* No interrupt selected */
47 |
48 | /**
49 | * @}
50 | */
51 |
52 | /** @defgroup EXTI_Private_Macros
53 | * @{
54 | */
55 |
56 | /**
57 | * @}
58 | */
59 |
60 | /** @defgroup EXTI_Private_Variables
61 | * @{
62 | */
63 |
64 | /**
65 | * @}
66 | */
67 |
68 | /** @defgroup EXTI_Private_FunctionPrototypes
69 | * @{
70 | */
71 |
72 | /**
73 | * @}
74 | */
75 |
76 | /** @defgroup EXTI_Private_Functions
77 | * @{
78 | */
79 |
80 | /**
81 | * @brief Deinitializes the EXTI peripheral registers to their default reset values.
82 | * @param None
83 | * @retval None
84 | */
85 | void EXTI_DeInit(void)
86 | {
87 | EXTI->IMR = 0x00000000;
88 | EXTI->EMR = 0x00000000;
89 | EXTI->RTSR = 0x00000000;
90 | EXTI->FTSR = 0x00000000;
91 | EXTI->PR = 0x000FFFFF;
92 | }
93 |
94 | /**
95 | * @brief Initializes the EXTI peripheral according to the specified
96 | * parameters in the EXTI_InitStruct.
97 | * @param EXTI_InitStruct: pointer to a EXTI_InitTypeDef structure
98 | * that contains the configuration information for the EXTI peripheral.
99 | * @retval None
100 | */
101 | void EXTI_Init(EXTI_InitTypeDef* EXTI_InitStruct)
102 | {
103 | uint32_t tmp = 0;
104 |
105 | /* Check the parameters */
106 | assert_param(IS_EXTI_MODE(EXTI_InitStruct->EXTI_Mode));
107 | assert_param(IS_EXTI_TRIGGER(EXTI_InitStruct->EXTI_Trigger));
108 | assert_param(IS_EXTI_LINE(EXTI_InitStruct->EXTI_Line));
109 | assert_param(IS_FUNCTIONAL_STATE(EXTI_InitStruct->EXTI_LineCmd));
110 |
111 | tmp = (uint32_t)EXTI_BASE;
112 |
113 | if (EXTI_InitStruct->EXTI_LineCmd != DISABLE)
114 | {
115 | /* Clear EXTI line configuration */
116 | EXTI->IMR &= ~EXTI_InitStruct->EXTI_Line;
117 | EXTI->EMR &= ~EXTI_InitStruct->EXTI_Line;
118 |
119 | tmp += EXTI_InitStruct->EXTI_Mode;
120 |
121 | *(__IO uint32_t *) tmp |= EXTI_InitStruct->EXTI_Line;
122 |
123 | /* Clear Rising Falling edge configuration */
124 | EXTI->RTSR &= ~EXTI_InitStruct->EXTI_Line;
125 | EXTI->FTSR &= ~EXTI_InitStruct->EXTI_Line;
126 |
127 | /* Select the trigger for the selected external interrupts */
128 | if (EXTI_InitStruct->EXTI_Trigger == EXTI_Trigger_Rising_Falling)
129 | {
130 | /* Rising Falling edge */
131 | EXTI->RTSR |= EXTI_InitStruct->EXTI_Line;
132 | EXTI->FTSR |= EXTI_InitStruct->EXTI_Line;
133 | }
134 | else
135 | {
136 | tmp = (uint32_t)EXTI_BASE;
137 | tmp += EXTI_InitStruct->EXTI_Trigger;
138 |
139 | *(__IO uint32_t *) tmp |= EXTI_InitStruct->EXTI_Line;
140 | }
141 | }
142 | else
143 | {
144 | tmp += EXTI_InitStruct->EXTI_Mode;
145 |
146 | /* Disable the selected external lines */
147 | *(__IO uint32_t *) tmp &= ~EXTI_InitStruct->EXTI_Line;
148 | }
149 | }
150 |
151 | /**
152 | * @brief Fills each EXTI_InitStruct member with its reset value.
153 | * @param EXTI_InitStruct: pointer to a EXTI_InitTypeDef structure which will
154 | * be initialized.
155 | * @retval None
156 | */
157 | void EXTI_StructInit(EXTI_InitTypeDef* EXTI_InitStruct)
158 | {
159 | EXTI_InitStruct->EXTI_Line = EXTI_LINENONE;
160 | EXTI_InitStruct->EXTI_Mode = EXTI_Mode_Interrupt;
161 | EXTI_InitStruct->EXTI_Trigger = EXTI_Trigger_Falling;
162 | EXTI_InitStruct->EXTI_LineCmd = DISABLE;
163 | }
164 |
165 | /**
166 | * @brief Generates a Software interrupt.
167 | * @param EXTI_Line: specifies the EXTI lines to be enabled or disabled.
168 | * This parameter can be any combination of EXTI_Linex where x can be (0..19).
169 | * @retval None
170 | */
171 | void EXTI_GenerateSWInterrupt(uint32_t EXTI_Line)
172 | {
173 | /* Check the parameters */
174 | assert_param(IS_EXTI_LINE(EXTI_Line));
175 |
176 | EXTI->SWIER |= EXTI_Line;
177 | }
178 |
179 | /**
180 | * @brief Checks whether the specified EXTI line flag is set or not.
181 | * @param EXTI_Line: specifies the EXTI line flag to check.
182 | * This parameter can be:
183 | * @arg EXTI_Linex: External interrupt line x where x(0..19)
184 | * @retval The new state of EXTI_Line (SET or RESET).
185 | */
186 | FlagStatus EXTI_GetFlagStatus(uint32_t EXTI_Line)
187 | {
188 | FlagStatus bitstatus = RESET;
189 | /* Check the parameters */
190 | assert_param(IS_GET_EXTI_LINE(EXTI_Line));
191 |
192 | if ((EXTI->PR & EXTI_Line) != (uint32_t)RESET)
193 | {
194 | bitstatus = SET;
195 | }
196 | else
197 | {
198 | bitstatus = RESET;
199 | }
200 | return bitstatus;
201 | }
202 |
203 | /**
204 | * @brief Clears the EXTI's line pending flags.
205 | * @param EXTI_Line: specifies the EXTI lines flags to clear.
206 | * This parameter can be any combination of EXTI_Linex where x can be (0..19).
207 | * @retval None
208 | */
209 | void EXTI_ClearFlag(uint32_t EXTI_Line)
210 | {
211 | /* Check the parameters */
212 | assert_param(IS_EXTI_LINE(EXTI_Line));
213 |
214 | EXTI->PR = EXTI_Line;
215 | }
216 |
217 | /**
218 | * @brief Checks whether the specified EXTI line is asserted or not.
219 | * @param EXTI_Line: specifies the EXTI line to check.
220 | * This parameter can be:
221 | * @arg EXTI_Linex: External interrupt line x where x(0..19)
222 | * @retval The new state of EXTI_Line (SET or RESET).
223 | */
224 | ITStatus EXTI_GetITStatus(uint32_t EXTI_Line)
225 | {
226 | ITStatus bitstatus = RESET;
227 | uint32_t enablestatus = 0;
228 | /* Check the parameters */
229 | assert_param(IS_GET_EXTI_LINE(EXTI_Line));
230 |
231 | enablestatus = EXTI->IMR & EXTI_Line;
232 | if (((EXTI->PR & EXTI_Line) != (uint32_t)RESET) && (enablestatus != (uint32_t)RESET))
233 | {
234 | bitstatus = SET;
235 | }
236 | else
237 | {
238 | bitstatus = RESET;
239 | }
240 | return bitstatus;
241 | }
242 |
243 | /**
244 | * @brief Clears the EXTI's line pending bits.
245 | * @param EXTI_Line: specifies the EXTI lines to clear.
246 | * This parameter can be any combination of EXTI_Linex where x can be (0..19).
247 | * @retval None
248 | */
249 | void EXTI_ClearITPendingBit(uint32_t EXTI_Line)
250 | {
251 | /* Check the parameters */
252 | assert_param(IS_EXTI_LINE(EXTI_Line));
253 |
254 | EXTI->PR = EXTI_Line;
255 | }
256 |
257 | /**
258 | * @}
259 | */
260 |
261 | /**
262 | * @}
263 | */
264 |
265 | /**
266 | * @}
267 | */
268 |
269 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/
270 |
--------------------------------------------------------------------------------
/hardware/stm32_timersync-open/STM32F10x_FWLib/src/stm32f10x_flash.c:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/WilsonGuo/FastLivo_Replication/eea82215c31adde5d914e54f9a38817e3aa9c942/hardware/stm32_timersync-open/STM32F10x_FWLib/src/stm32f10x_flash.c
--------------------------------------------------------------------------------
/hardware/stm32_timersync-open/STM32F10x_FWLib/src/stm32f10x_i2c.c:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/WilsonGuo/FastLivo_Replication/eea82215c31adde5d914e54f9a38817e3aa9c942/hardware/stm32_timersync-open/STM32F10x_FWLib/src/stm32f10x_i2c.c
--------------------------------------------------------------------------------
/hardware/stm32_timersync-open/STM32F10x_FWLib/src/stm32f10x_iwdg.c:
--------------------------------------------------------------------------------
1 | /**
2 | ******************************************************************************
3 | * @file stm32f10x_iwdg.c
4 | * @author MCD Application Team
5 | * @version V3.5.0
6 | * @date 11-March-2011
7 | * @brief This file provides all the IWDG firmware functions.
8 | ******************************************************************************
9 | * @attention
10 | *
11 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
12 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
13 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
14 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
15 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
16 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
17 | *
18 | * © COPYRIGHT 2011 STMicroelectronics
19 | ******************************************************************************
20 | */
21 |
22 | /* Includes ------------------------------------------------------------------*/
23 | #include "stm32f10x_iwdg.h"
24 |
25 | /** @addtogroup STM32F10x_StdPeriph_Driver
26 | * @{
27 | */
28 |
29 | /** @defgroup IWDG
30 | * @brief IWDG driver modules
31 | * @{
32 | */
33 |
34 | /** @defgroup IWDG_Private_TypesDefinitions
35 | * @{
36 | */
37 |
38 | /**
39 | * @}
40 | */
41 |
42 | /** @defgroup IWDG_Private_Defines
43 | * @{
44 | */
45 |
46 | /* ---------------------- IWDG registers bit mask ----------------------------*/
47 |
48 | /* KR register bit mask */
49 | #define KR_KEY_Reload ((uint16_t)0xAAAA)
50 | #define KR_KEY_Enable ((uint16_t)0xCCCC)
51 |
52 | /**
53 | * @}
54 | */
55 |
56 | /** @defgroup IWDG_Private_Macros
57 | * @{
58 | */
59 |
60 | /**
61 | * @}
62 | */
63 |
64 | /** @defgroup IWDG_Private_Variables
65 | * @{
66 | */
67 |
68 | /**
69 | * @}
70 | */
71 |
72 | /** @defgroup IWDG_Private_FunctionPrototypes
73 | * @{
74 | */
75 |
76 | /**
77 | * @}
78 | */
79 |
80 | /** @defgroup IWDG_Private_Functions
81 | * @{
82 | */
83 |
84 | /**
85 | * @brief Enables or disables write access to IWDG_PR and IWDG_RLR registers.
86 | * @param IWDG_WriteAccess: new state of write access to IWDG_PR and IWDG_RLR registers.
87 | * This parameter can be one of the following values:
88 | * @arg IWDG_WriteAccess_Enable: Enable write access to IWDG_PR and IWDG_RLR registers
89 | * @arg IWDG_WriteAccess_Disable: Disable write access to IWDG_PR and IWDG_RLR registers
90 | * @retval None
91 | */
92 | void IWDG_WriteAccessCmd(uint16_t IWDG_WriteAccess)
93 | {
94 | /* Check the parameters */
95 | assert_param(IS_IWDG_WRITE_ACCESS(IWDG_WriteAccess));
96 | IWDG->KR = IWDG_WriteAccess;
97 | }
98 |
99 | /**
100 | * @brief Sets IWDG Prescaler value.
101 | * @param IWDG_Prescaler: specifies the IWDG Prescaler value.
102 | * This parameter can be one of the following values:
103 | * @arg IWDG_Prescaler_4: IWDG prescaler set to 4
104 | * @arg IWDG_Prescaler_8: IWDG prescaler set to 8
105 | * @arg IWDG_Prescaler_16: IWDG prescaler set to 16
106 | * @arg IWDG_Prescaler_32: IWDG prescaler set to 32
107 | * @arg IWDG_Prescaler_64: IWDG prescaler set to 64
108 | * @arg IWDG_Prescaler_128: IWDG prescaler set to 128
109 | * @arg IWDG_Prescaler_256: IWDG prescaler set to 256
110 | * @retval None
111 | */
112 | void IWDG_SetPrescaler(uint8_t IWDG_Prescaler)
113 | {
114 | /* Check the parameters */
115 | assert_param(IS_IWDG_PRESCALER(IWDG_Prescaler));
116 | IWDG->PR = IWDG_Prescaler;
117 | }
118 |
119 | /**
120 | * @brief Sets IWDG Reload value.
121 | * @param Reload: specifies the IWDG Reload value.
122 | * This parameter must be a number between 0 and 0x0FFF.
123 | * @retval None
124 | */
125 | void IWDG_SetReload(uint16_t Reload)
126 | {
127 | /* Check the parameters */
128 | assert_param(IS_IWDG_RELOAD(Reload));
129 | IWDG->RLR = Reload;
130 | }
131 |
132 | /**
133 | * @brief Reloads IWDG counter with value defined in the reload register
134 | * (write access to IWDG_PR and IWDG_RLR registers disabled).
135 | * @param None
136 | * @retval None
137 | */
138 | void IWDG_ReloadCounter(void)
139 | {
140 | IWDG->KR = KR_KEY_Reload;
141 | }
142 |
143 | /**
144 | * @brief Enables IWDG (write access to IWDG_PR and IWDG_RLR registers disabled).
145 | * @param None
146 | * @retval None
147 | */
148 | void IWDG_Enable(void)
149 | {
150 | IWDG->KR = KR_KEY_Enable;
151 | }
152 |
153 | /**
154 | * @brief Checks whether the specified IWDG flag is set or not.
155 | * @param IWDG_FLAG: specifies the flag to check.
156 | * This parameter can be one of the following values:
157 | * @arg IWDG_FLAG_PVU: Prescaler Value Update on going
158 | * @arg IWDG_FLAG_RVU: Reload Value Update on going
159 | * @retval The new state of IWDG_FLAG (SET or RESET).
160 | */
161 | FlagStatus IWDG_GetFlagStatus(uint16_t IWDG_FLAG)
162 | {
163 | FlagStatus bitstatus = RESET;
164 | /* Check the parameters */
165 | assert_param(IS_IWDG_FLAG(IWDG_FLAG));
166 | if ((IWDG->SR & IWDG_FLAG) != (uint32_t)RESET)
167 | {
168 | bitstatus = SET;
169 | }
170 | else
171 | {
172 | bitstatus = RESET;
173 | }
174 | /* Return the flag status */
175 | return bitstatus;
176 | }
177 |
178 | /**
179 | * @}
180 | */
181 |
182 | /**
183 | * @}
184 | */
185 |
186 | /**
187 | * @}
188 | */
189 |
190 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/
191 |
--------------------------------------------------------------------------------
/hardware/stm32_timersync-open/STM32F10x_FWLib/src/stm32f10x_usart.c:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/WilsonGuo/FastLivo_Replication/eea82215c31adde5d914e54f9a38817e3aa9c942/hardware/stm32_timersync-open/STM32F10x_FWLib/src/stm32f10x_usart.c
--------------------------------------------------------------------------------
/hardware/stm32_timersync-open/STM32F10x_FWLib/src/stm32f10x_wwdg.c:
--------------------------------------------------------------------------------
1 | /**
2 | ******************************************************************************
3 | * @file stm32f10x_wwdg.c
4 | * @author MCD Application Team
5 | * @version V3.5.0
6 | * @date 11-March-2011
7 | * @brief This file provides all the WWDG firmware functions.
8 | ******************************************************************************
9 | * @attention
10 | *
11 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
12 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
13 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
14 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
15 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
16 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
17 | *
18 | * © COPYRIGHT 2011 STMicroelectronics
19 | ******************************************************************************
20 | */
21 |
22 | /* Includes ------------------------------------------------------------------*/
23 | #include "stm32f10x_wwdg.h"
24 | #include "stm32f10x_rcc.h"
25 |
26 | /** @addtogroup STM32F10x_StdPeriph_Driver
27 | * @{
28 | */
29 |
30 | /** @defgroup WWDG
31 | * @brief WWDG driver modules
32 | * @{
33 | */
34 |
35 | /** @defgroup WWDG_Private_TypesDefinitions
36 | * @{
37 | */
38 |
39 | /**
40 | * @}
41 | */
42 |
43 | /** @defgroup WWDG_Private_Defines
44 | * @{
45 | */
46 |
47 | /* ----------- WWDG registers bit address in the alias region ----------- */
48 | #define WWDG_OFFSET (WWDG_BASE - PERIPH_BASE)
49 |
50 | /* Alias word address of EWI bit */
51 | #define CFR_OFFSET (WWDG_OFFSET + 0x04)
52 | #define EWI_BitNumber 0x09
53 | #define CFR_EWI_BB (PERIPH_BB_BASE + (CFR_OFFSET * 32) + (EWI_BitNumber * 4))
54 |
55 | /* --------------------- WWDG registers bit mask ------------------------ */
56 |
57 | /* CR register bit mask */
58 | #define CR_WDGA_Set ((uint32_t)0x00000080)
59 |
60 | /* CFR register bit mask */
61 | #define CFR_WDGTB_Mask ((uint32_t)0xFFFFFE7F)
62 | #define CFR_W_Mask ((uint32_t)0xFFFFFF80)
63 | #define BIT_Mask ((uint8_t)0x7F)
64 |
65 | /**
66 | * @}
67 | */
68 |
69 | /** @defgroup WWDG_Private_Macros
70 | * @{
71 | */
72 |
73 | /**
74 | * @}
75 | */
76 |
77 | /** @defgroup WWDG_Private_Variables
78 | * @{
79 | */
80 |
81 | /**
82 | * @}
83 | */
84 |
85 | /** @defgroup WWDG_Private_FunctionPrototypes
86 | * @{
87 | */
88 |
89 | /**
90 | * @}
91 | */
92 |
93 | /** @defgroup WWDG_Private_Functions
94 | * @{
95 | */
96 |
97 | /**
98 | * @brief Deinitializes the WWDG peripheral registers to their default reset values.
99 | * @param None
100 | * @retval None
101 | */
102 | void WWDG_DeInit(void)
103 | {
104 | RCC_APB1PeriphResetCmd(RCC_APB1Periph_WWDG, ENABLE);
105 | RCC_APB1PeriphResetCmd(RCC_APB1Periph_WWDG, DISABLE);
106 | }
107 |
108 | /**
109 | * @brief Sets the WWDG Prescaler.
110 | * @param WWDG_Prescaler: specifies the WWDG Prescaler.
111 | * This parameter can be one of the following values:
112 | * @arg WWDG_Prescaler_1: WWDG counter clock = (PCLK1/4096)/1
113 | * @arg WWDG_Prescaler_2: WWDG counter clock = (PCLK1/4096)/2
114 | * @arg WWDG_Prescaler_4: WWDG counter clock = (PCLK1/4096)/4
115 | * @arg WWDG_Prescaler_8: WWDG counter clock = (PCLK1/4096)/8
116 | * @retval None
117 | */
118 | void WWDG_SetPrescaler(uint32_t WWDG_Prescaler)
119 | {
120 | uint32_t tmpreg = 0;
121 | /* Check the parameters */
122 | assert_param(IS_WWDG_PRESCALER(WWDG_Prescaler));
123 | /* Clear WDGTB[1:0] bits */
124 | tmpreg = WWDG->CFR & CFR_WDGTB_Mask;
125 | /* Set WDGTB[1:0] bits according to WWDG_Prescaler value */
126 | tmpreg |= WWDG_Prescaler;
127 | /* Store the new value */
128 | WWDG->CFR = tmpreg;
129 | }
130 |
131 | /**
132 | * @brief Sets the WWDG window value.
133 | * @param WindowValue: specifies the window value to be compared to the downcounter.
134 | * This parameter value must be lower than 0x80.
135 | * @retval None
136 | */
137 | void WWDG_SetWindowValue(uint8_t WindowValue)
138 | {
139 | __IO uint32_t tmpreg = 0;
140 |
141 | /* Check the parameters */
142 | assert_param(IS_WWDG_WINDOW_VALUE(WindowValue));
143 | /* Clear W[6:0] bits */
144 |
145 | tmpreg = WWDG->CFR & CFR_W_Mask;
146 |
147 | /* Set W[6:0] bits according to WindowValue value */
148 | tmpreg |= WindowValue & (uint32_t) BIT_Mask;
149 |
150 | /* Store the new value */
151 | WWDG->CFR = tmpreg;
152 | }
153 |
154 | /**
155 | * @brief Enables the WWDG Early Wakeup interrupt(EWI).
156 | * @param None
157 | * @retval None
158 | */
159 | void WWDG_EnableIT(void)
160 | {
161 | *(__IO uint32_t *) CFR_EWI_BB = (uint32_t)ENABLE;
162 | }
163 |
164 | /**
165 | * @brief Sets the WWDG counter value.
166 | * @param Counter: specifies the watchdog counter value.
167 | * This parameter must be a number between 0x40 and 0x7F.
168 | * @retval None
169 | */
170 | void WWDG_SetCounter(uint8_t Counter)
171 | {
172 | /* Check the parameters */
173 | assert_param(IS_WWDG_COUNTER(Counter));
174 | /* Write to T[6:0] bits to configure the counter value, no need to do
175 | a read-modify-write; writing a 0 to WDGA bit does nothing */
176 | WWDG->CR = Counter & BIT_Mask;
177 | }
178 |
179 | /**
180 | * @brief Enables WWDG and load the counter value.
181 | * @param Counter: specifies the watchdog counter value.
182 | * This parameter must be a number between 0x40 and 0x7F.
183 | * @retval None
184 | */
185 | void WWDG_Enable(uint8_t Counter)
186 | {
187 | /* Check the parameters */
188 | assert_param(IS_WWDG_COUNTER(Counter));
189 | WWDG->CR = CR_WDGA_Set | Counter;
190 | }
191 |
192 | /**
193 | * @brief Checks whether the Early Wakeup interrupt flag is set or not.
194 | * @param None
195 | * @retval The new state of the Early Wakeup interrupt flag (SET or RESET)
196 | */
197 | FlagStatus WWDG_GetFlagStatus(void)
198 | {
199 | return (FlagStatus)(WWDG->SR);
200 | }
201 |
202 | /**
203 | * @brief Clears Early Wakeup interrupt flag.
204 | * @param None
205 | * @retval None
206 | */
207 | void WWDG_ClearFlag(void)
208 | {
209 | WWDG->SR = (uint32_t)RESET;
210 | }
211 |
212 | /**
213 | * @}
214 | */
215 |
216 | /**
217 | * @}
218 | */
219 |
220 | /**
221 | * @}
222 | */
223 |
224 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/
225 |
--------------------------------------------------------------------------------
/hardware/stm32_timersync-open/SYSTEM/delay/delay.c:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/WilsonGuo/FastLivo_Replication/eea82215c31adde5d914e54f9a38817e3aa9c942/hardware/stm32_timersync-open/SYSTEM/delay/delay.c
--------------------------------------------------------------------------------
/hardware/stm32_timersync-open/SYSTEM/delay/delay.h:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/WilsonGuo/FastLivo_Replication/eea82215c31adde5d914e54f9a38817e3aa9c942/hardware/stm32_timersync-open/SYSTEM/delay/delay.h
--------------------------------------------------------------------------------
/hardware/stm32_timersync-open/SYSTEM/sys/sys.c:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/WilsonGuo/FastLivo_Replication/eea82215c31adde5d914e54f9a38817e3aa9c942/hardware/stm32_timersync-open/SYSTEM/sys/sys.c
--------------------------------------------------------------------------------
/hardware/stm32_timersync-open/SYSTEM/sys/sys.h:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/WilsonGuo/FastLivo_Replication/eea82215c31adde5d914e54f9a38817e3aa9c942/hardware/stm32_timersync-open/SYSTEM/sys/sys.h
--------------------------------------------------------------------------------
/hardware/stm32_timersync-open/SYSTEM/usart/usart.c:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/WilsonGuo/FastLivo_Replication/eea82215c31adde5d914e54f9a38817e3aa9c942/hardware/stm32_timersync-open/SYSTEM/usart/usart.c
--------------------------------------------------------------------------------
/hardware/stm32_timersync-open/SYSTEM/usart/usart.h:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/WilsonGuo/FastLivo_Replication/eea82215c31adde5d914e54f9a38817e3aa9c942/hardware/stm32_timersync-open/SYSTEM/usart/usart.h
--------------------------------------------------------------------------------
/hardware/stm32_timersync-open/USER/DebugConfig/PWM_STM32F103C6_1.0.0.dbgconf:
--------------------------------------------------------------------------------
1 | // File: STM32F101_102_103_105_107.dbgconf
2 | // Version: 1.0.0
3 | // Note: refer to STM32F101xx STM32F102xx STM32F103xx STM32F105xx STM32F107xx Reference manual (RM0008)
4 | // STM32F101xx STM32F102xx STM32F103xx STM32F105xx STM32F107xx datasheets
5 |
6 | // <<< Use Configuration Wizard in Context Menu >>>
7 |
8 | // Debug MCU configuration register (DBGMCU_CR)
9 | // Reserved bits must be kept at reset value
10 | // DBG_TIM11_STOP TIM11 counter stopped when core is halted
11 | // DBG_TIM10_STOP TIM10 counter stopped when core is halted
12 | // DBG_TIM9_STOP TIM9 counter stopped when core is halted
13 | // DBG_TIM14_STOP TIM14 counter stopped when core is halted
14 | // DBG_TIM13_STOP TIM13 counter stopped when core is halted
15 | // DBG_TIM12_STOP TIM12 counter stopped when core is halted
16 | // DBG_CAN2_STOP Debug CAN2 stopped when core is halted
17 | // DBG_TIM7_STOP TIM7 counter stopped when core is halted
18 | // DBG_TIM6_STOP TIM6 counter stopped when core is halted
19 | // DBG_TIM5_STOP TIM5 counter stopped when core is halted
20 | // DBG_TIM8_STOP TIM8 counter stopped when core is halted
21 | // DBG_I2C2_SMBUS_TIMEOUT SMBUS timeout mode stopped when core is halted
22 | // DBG_I2C1_SMBUS_TIMEOUT SMBUS timeout mode stopped when core is halted
23 | // DBG_CAN1_STOP Debug CAN1 stopped when Core is halted
24 | // DBG_TIM4_STOP TIM4 counter stopped when core is halted
25 | // DBG_TIM3_STOP TIM3 counter stopped when core is halted
26 | // DBG_TIM2_STOP TIM2 counter stopped when core is halted
27 | // DBG_TIM1_STOP TIM1 counter stopped when core is halted
28 | // DBG_WWDG_STOP Debug window watchdog stopped when core is halted
29 | // DBG_IWDG_STOP Debug independent watchdog stopped when core is halted
30 | // DBG_STANDBY Debug standby mode
31 | // DBG_STOP Debug stop mode
32 | // DBG_SLEEP Debug sleep mode
33 | //
34 | DbgMCU_CR = 0x00000007;
35 |
36 | // <<< end of configuration section >>>
37 |
--------------------------------------------------------------------------------
/hardware/stm32_timersync-open/USER/DebugConfig/PWM_STM32F103C8_1.0.0.dbgconf:
--------------------------------------------------------------------------------
1 | // File: STM32F101_102_103_105_107.dbgconf
2 | // Version: 1.0.0
3 | // Note: refer to STM32F101xx STM32F102xx STM32F103xx STM32F105xx STM32F107xx Reference manual (RM0008)
4 | // STM32F101xx STM32F102xx STM32F103xx STM32F105xx STM32F107xx datasheets
5 |
6 | // <<< Use Configuration Wizard in Context Menu >>>
7 |
8 | // Debug MCU configuration register (DBGMCU_CR)
9 | // Reserved bits must be kept at reset value
10 | // DBG_TIM11_STOP TIM11 counter stopped when core is halted
11 | // DBG_TIM10_STOP TIM10 counter stopped when core is halted
12 | // DBG_TIM9_STOP TIM9 counter stopped when core is halted
13 | // DBG_TIM14_STOP TIM14 counter stopped when core is halted
14 | // DBG_TIM13_STOP TIM13 counter stopped when core is halted
15 | // DBG_TIM12_STOP TIM12 counter stopped when core is halted
16 | // DBG_CAN2_STOP Debug CAN2 stopped when core is halted
17 | // DBG_TIM7_STOP TIM7 counter stopped when core is halted
18 | // DBG_TIM6_STOP TIM6 counter stopped when core is halted
19 | // DBG_TIM5_STOP TIM5 counter stopped when core is halted
20 | // DBG_TIM8_STOP TIM8 counter stopped when core is halted
21 | // DBG_I2C2_SMBUS_TIMEOUT SMBUS timeout mode stopped when core is halted
22 | // DBG_I2C1_SMBUS_TIMEOUT SMBUS timeout mode stopped when core is halted
23 | // DBG_CAN1_STOP Debug CAN1 stopped when Core is halted
24 | // DBG_TIM4_STOP TIM4 counter stopped when core is halted
25 | // DBG_TIM3_STOP TIM3 counter stopped when core is halted
26 | // DBG_TIM2_STOP TIM2 counter stopped when core is halted
27 | // DBG_TIM1_STOP TIM1 counter stopped when core is halted
28 | // DBG_WWDG_STOP Debug window watchdog stopped when core is halted
29 | // DBG_IWDG_STOP Debug independent watchdog stopped when core is halted
30 | // DBG_STANDBY Debug standby mode
31 | // DBG_STOP Debug stop mode
32 | // DBG_SLEEP Debug sleep mode
33 | //
34 | DbgMCU_CR = 0x00000007;
35 |
36 | // <<< end of configuration section >>>
37 |
--------------------------------------------------------------------------------
/hardware/stm32_timersync-open/USER/DebugConfig/PWM_STM32F103ZE_1.0.0.dbgconf:
--------------------------------------------------------------------------------
1 | // <<< Use Configuration Wizard in Context Menu >>>
2 | // Debug MCU Configuration
3 | // DBG_SLEEP
4 | // Debug Sleep Mode
5 | // 0: (FCLK=On, HCLK=Off) FCLK is clocked by the system clock as previously configured by the software while HCLK is disabled
6 | // 1: (FCLK=On, HCLK=On) HCLK is fed by the same clock that is provided to FCLK
7 | // DBG_STOP
8 | // Debug Stop Mode
9 | // 0: (FCLK=Off, HCLK=Off) Clock controller disables all clocks
10 | // 1: (FCLK=On, HCLK=On) FCLK and HCLK are provided by the internal RC oscillator which remains active
11 | // DBG_STANDBY
12 | // Debug Standby Mode
13 | // 0: (FCLK=Off, HCLK=Off) The whole digital part is unpowered.
14 | // 1: (FCLK=On, HCLK=On) Digital part is powered and FCLK and HCLK are provided by the internal RC oscillator which remains active
15 | // DBG_IWDG_STOP
16 | // Debug independent watchdog stopped when core is halted
17 | // 0: The watchdog counter clock continues even if the core is halted
18 | // 1: The watchdog counter clock is stopped when the core is halted
19 | // DBG_WWDG_STOP
20 | // Debug window watchdog stopped when core is halted
21 | // 0: The window watchdog counter clock continues even if the core is halted
22 | // 1: The window watchdog counter clock is stopped when the core is halted
23 | // DBG_TIM1_STOP
24 | // Timer 1 counter stopped when core is halted
25 | // 0: The clock of the involved Timer Counter is fed even if the core is halted
26 | // 1: The clock of the involved Timer counter is stopped when the core is halted
27 | // DBG_TIM2_STOP
28 | // Timer 2 counter stopped when core is halted
29 | // 0: The clock of the involved Timer Counter is fed even if the core is halted
30 | // 1: The clock of the involved Timer counter is stopped when the core is halted
31 | // DBG_TIM3_STOP
32 | // Timer 3 counter stopped when core is halted
33 | // 0: The clock of the involved Timer Counter is fed even if the core is halted
34 | // 1: The clock of the involved Timer counter is stopped when the core is halted
35 | // DBG_TIM4_STOP
36 | // Timer 4 counter stopped when core is halted
37 | // 0: The clock of the involved Timer Counter is fed even if the core is halted
38 | // 1: The clock of the involved Timer counter is stopped when the core is halted
39 | // DBG_CAN1_STOP
40 | // Debug CAN1 stopped when Core is halted
41 | // 0: Same behavior as in normal mode
42 | // 1: CAN1 receive registers are frozen
43 | // DBG_I2C1_SMBUS_TIMEOUT
44 | // I2C1 SMBUS timeout mode stopped when Core is halted
45 | // 0: Same behavior as in normal mode
46 | // 1: The SMBUS timeout is frozen
47 | // DBG_I2C2_SMBUS_TIMEOUT
48 | // I2C2 SMBUS timeout mode stopped when Core is halted
49 | // 0: Same behavior as in normal mode
50 | // 1: The SMBUS timeout is frozen
51 | // DBG_TIM8_STOP
52 | // Timer 8 counter stopped when core is halted
53 | // 0: The clock of the involved timer counter is fed even if the core is halted, and the outputs behave normally.
54 | // 1: The clock of the involved timer counter is stopped when the core is halted, and the outputs are disabled (as if there were an emergency stop in response to a break event).
55 | // DBG_TIM5_STOP
56 | // Timer 5 counter stopped when core is halted
57 | // 0: The clock of the involved timer counter is fed even if the core is halted, and the outputs behave normally.
58 | // 1: The clock of the involved timer counter is stopped when the core is halted, and the outputs are disabled (as if there were an emergency stop in response to a break event).
59 | // DBG_TIM6_STOP
60 | // Timer 6 counter stopped when core is halted
61 | // 0: The clock of the involved timer counter is fed even if the core is halted, and the outputs behave normally.
62 | // 1: The clock of the involved timer counter is stopped when the core is halted, and the outputs are disabled (as if there were an emergency stop in response to a break event).
63 | // DBG_TIM7_STOP
64 | // Timer 7 counter stopped when core is halted
65 | // 0: The clock of the involved timer counter is fed even if the core is halted, and the outputs behave normally.
66 | // 1: The clock of the involved timer counter is stopped when the core is halted, and the outputs are disabled (as if there were an emergency stop in response to a break event).
67 | // DBG_CAN2_STOP
68 | // Debug CAN2 stopped when Core is halted
69 | // 0: Same behavior as in normal mode
70 | // 1: CAN2 receive registers are frozen
71 | // DBG_TIM12_STOP
72 | // Timer 12 counter stopped when core is halted
73 | // 0: The clock of the involved timer counter is fed even if the core is halted, and the outputs behave normally.
74 | // 1: The clock of the involved timer counter is stopped when the core is halted, and the outputs are disabled (as if there were an emergency stop in response to a break event).
75 | // DBG_TIM13_STOP
76 | // Timer 13 counter stopped when core is halted
77 | // 0: The clock of the involved timer counter is fed even if the core is halted, and the outputs behave normally.
78 | // 1: The clock of the involved timer counter is stopped when the core is halted, and the outputs are disabled (as if there were an emergency stop in response to a break event).
79 | // DBG_TIM14_STOP
80 | // Timer 14 counter stopped when core is halted
81 | // 0: The clock of the involved timer counter is fed even if the core is halted, and the outputs behave normally.
82 | // 1: The clock of the involved timer counter is stopped when the core is halted, and the outputs are disabled (as if there were an emergency stop in response to a break event).
83 | // DBG_TIM9_STOP
84 | // Timer 9 counter stopped when core is halted
85 | // 0: The clock of the involved timer counter is fed even if the core is halted, and the outputs behave normally.
86 | // 1: The clock of the involved timer counter is stopped when the core is halted, and the outputs are disabled (as if there were an emergency stop in response to a break event).
87 | // DBG_TIM10_STOP
88 | // Timer 10 counter stopped when core is halted
89 | // 0: The clock of the involved timer counter is fed even if the core is halted, and the outputs behave normally.
90 | // 1: The clock of the involved timer counter is stopped when the core is halted, and the outputs are disabled (as if there were an emergency stop in response to a break event).
91 | // DBG_TIM11_STOP
92 | // Timer 11 counter stopped when core is halted
93 | // 0: The clock of the involved timer counter is fed even if the core is halted, and the outputs behave normally.
94 | // 1: The clock of the involved timer counter is stopped when the core is halted, and the outputs are disabled (as if there were an emergency stop in response to a break event).
95 | //
96 | DbgMCU_CR = 0x00000007;
97 | // <<< end of configuration section >>>
--------------------------------------------------------------------------------
/hardware/stm32_timersync-open/USER/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 | OverrideMemMap = 0
11 | AllowSimulation = 1
12 | ScriptFile=""
13 | [FLASH]
14 | CacheExcludeSize = 0x00
15 | CacheExcludeAddr = 0x00
16 | MinNumBytesFlashDL = 0
17 | SkipProgOnCRCMatch = 1
18 | VerifyDownload = 1
19 | AllowCaching = 1
20 | EnableFlashDL = 2
21 | Override = 0
22 | Device="UNSPECIFIED"
23 | [GENERAL]
24 | WorkRAMSize = 0x00
25 | WorkRAMAddr = 0x00
26 | RAMUsageLimit = 0x00
27 | [SWO]
28 | SWOLogFile=""
29 | [MEM]
30 | RdOverrideOrMask = 0x00
31 | RdOverrideAndMask = 0xFFFFFFFF
32 | RdOverrideAddr = 0xFFFFFFFF
33 | WrOverrideOrMask = 0x00
34 | WrOverrideAndMask = 0xFFFFFFFF
35 | WrOverrideAddr = 0xFFFFFFFF
36 |
--------------------------------------------------------------------------------
/hardware/stm32_timersync-open/USER/main.c:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/WilsonGuo/FastLivo_Replication/eea82215c31adde5d914e54f9a38817e3aa9c942/hardware/stm32_timersync-open/USER/main.c
--------------------------------------------------------------------------------
/hardware/stm32_timersync-open/USER/stm32f10x.h:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/WilsonGuo/FastLivo_Replication/eea82215c31adde5d914e54f9a38817e3aa9c942/hardware/stm32_timersync-open/USER/stm32f10x.h
--------------------------------------------------------------------------------
/hardware/stm32_timersync-open/USER/stm32f10x_conf.h:
--------------------------------------------------------------------------------
1 | /**
2 | ******************************************************************************
3 | * @file GPIO/IOToggle/stm32f10x_conf.h
4 | * @author MCD Application Team
5 | * @version V3.5.0
6 | * @date 08-April-2011
7 | * @brief Library configuration file.
8 | ******************************************************************************
9 | * @attention
10 | *
11 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
12 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
13 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
14 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
15 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
16 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
17 | *
18 | * © COPYRIGHT 2011 STMicroelectronics
19 | ******************************************************************************
20 | */
21 |
22 | /* Define to prevent recursive inclusion -------------------------------------*/
23 | #ifndef __STM32F10x_CONF_H
24 | #define __STM32F10x_CONF_H
25 |
26 | /* Includes ------------------------------------------------------------------*/
27 | /* Uncomment/Comment the line below to enable/disable peripheral header file inclusion */
28 | #include "stm32f10x_adc.h"
29 | #include "stm32f10x_bkp.h"
30 | #include "stm32f10x_can.h"
31 | #include "stm32f10x_cec.h"
32 | #include "stm32f10x_crc.h"
33 | #include "stm32f10x_dac.h"
34 | #include "stm32f10x_dbgmcu.h"
35 | #include "stm32f10x_dma.h"
36 | #include "stm32f10x_exti.h"
37 | #include "stm32f10x_flash.h"
38 | #include "stm32f10x_fsmc.h"
39 | #include "stm32f10x_gpio.h"
40 | #include "stm32f10x_i2c.h"
41 | #include "stm32f10x_iwdg.h"
42 | #include "stm32f10x_pwr.h"
43 | #include "stm32f10x_rcc.h"
44 | #include "stm32f10x_rtc.h"
45 | #include "stm32f10x_sdio.h"
46 | #include "stm32f10x_spi.h"
47 | #include "stm32f10x_tim.h"
48 | #include "stm32f10x_usart.h"
49 | #include "stm32f10x_wwdg.h"
50 | #include "misc.h" /* High level functions for NVIC and SysTick (add-on to CMSIS functions) */
51 |
52 | /* Exported types ------------------------------------------------------------*/
53 | /* Exported constants --------------------------------------------------------*/
54 | /* Uncomment the line below to expanse the "assert_param" macro in the
55 | Standard Peripheral Library drivers code */
56 | /* #define USE_FULL_ASSERT 1 */
57 |
58 | /* Exported macro ------------------------------------------------------------*/
59 | #ifdef USE_FULL_ASSERT
60 |
61 | /**
62 | * @brief The assert_param macro is used for function's parameters check.
63 | * @param expr: If expr is false, it calls assert_failed function which reports
64 | * the name of the source file and the source line number of the call
65 | * that failed. If expr is true, it returns no value.
66 | * @retval None
67 | */
68 | #define assert_param(expr) ((expr) ? (void)0 : assert_failed((uint8_t *)__FILE__, __LINE__))
69 | /* Exported functions ------------------------------------------------------- */
70 | void assert_failed(uint8_t* file, uint32_t line);
71 | #else
72 | #define assert_param(expr) ((void)0)
73 | #endif /* USE_FULL_ASSERT */
74 |
75 | #endif /* __STM32F10x_CONF_H */
76 |
77 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/
78 |
--------------------------------------------------------------------------------
/hardware/stm32_timersync-open/USER/stm32f10x_it.c:
--------------------------------------------------------------------------------
1 | /**
2 | ******************************************************************************
3 | * @file GPIO/IOToggle/stm32f10x_it.c
4 | * @author MCD Application Team
5 | * @version V3.5.0
6 | * @date 08-April-2011
7 | * @brief Main Interrupt Service Routines.
8 | * This file provides template for all exceptions handler and peripherals
9 | * interrupt service routine.
10 | ******************************************************************************
11 | * @attention
12 | *
13 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
14 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
15 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
16 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
17 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
18 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
19 | *
20 | * © COPYRIGHT 2011 STMicroelectronics
21 | ******************************************************************************
22 | */
23 |
24 | /* Includes ------------------------------------------------------------------*/
25 | #include "stm32f10x_it.h"
26 |
27 |
28 |
29 | void NMI_Handler(void)
30 | {
31 | }
32 |
33 | void HardFault_Handler(void)
34 | {
35 | /* Go to infinite loop when Hard Fault exception occurs */
36 | while (1)
37 | {
38 | }
39 | }
40 |
41 | void MemManage_Handler(void)
42 | {
43 | /* Go to infinite loop when Memory Manage exception occurs */
44 | while (1)
45 | {
46 | }
47 | }
48 |
49 |
50 | void BusFault_Handler(void)
51 | {
52 | /* Go to infinite loop when Bus Fault exception occurs */
53 | while (1)
54 | {
55 | }
56 | }
57 |
58 | void UsageFault_Handler(void)
59 | {
60 | /* Go to infinite loop when Usage Fault exception occurs */
61 | while (1)
62 | {
63 | }
64 | }
65 |
66 | void SVC_Handler(void)
67 | {
68 | }
69 |
70 | void DebugMon_Handler(void)
71 | {
72 | }
73 |
74 | void PendSV_Handler(void)
75 | {
76 | }
77 |
78 | void SysTick_Handler(void)
79 | {
80 | }
81 |
82 | /******************************************************************************/
83 | /* STM32F10x Peripherals Interrupt Handlers */
84 | /* Add here the Interrupt Handler for the used peripheral(s) (PPP), for the */
85 | /* available peripheral interrupt handler's name please refer to the startup */
86 | /* file (startup_stm32f10x_xx.s). */
87 | /******************************************************************************/
88 |
--------------------------------------------------------------------------------
/hardware/stm32_timersync-open/USER/stm32f10x_it.h:
--------------------------------------------------------------------------------
1 | /**
2 | ******************************************************************************
3 | * @file GPIO/IOToggle/stm32f10x_it.h
4 | * @author MCD Application Team
5 | * @version V3.5.0
6 | * @date 08-April-2011
7 | * @brief This file contains the headers of the interrupt handlers.
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_IT_H
24 | #define __STM32F10x_IT_H
25 |
26 | /* Includes ------------------------------------------------------------------*/
27 | #include "stm32f10x.h"
28 |
29 | /* Exported types ------------------------------------------------------------*/
30 | /* Exported constants --------------------------------------------------------*/
31 | /* Exported macro ------------------------------------------------------------*/
32 | /* Exported functions ------------------------------------------------------- */
33 |
34 | void NMI_Handler(void);
35 | void HardFault_Handler(void);
36 | void MemManage_Handler(void);
37 | void BusFault_Handler(void);
38 | void UsageFault_Handler(void);
39 | void SVC_Handler(void);
40 | void DebugMon_Handler(void);
41 | void PendSV_Handler(void);
42 | void SysTick_Handler(void);
43 |
44 | #endif /* __STM32F10x_IT_H */
45 |
46 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/
47 |
--------------------------------------------------------------------------------
/hardware/stm32_timersync-open/USER/system_stm32f10x.h:
--------------------------------------------------------------------------------
1 | /**
2 | ******************************************************************************
3 | * @file system_stm32f10x.h
4 | * @author MCD Application Team
5 | * @version V3.5.0
6 | * @date 11-March-2011
7 | * @brief CMSIS Cortex-M3 Device Peripheral Access Layer System Header File.
8 | ******************************************************************************
9 | * @attention
10 | *
11 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
12 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
13 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
14 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
15 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
16 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
17 | *
18 | * © COPYRIGHT 2011 STMicroelectronics
19 | ******************************************************************************
20 | */
21 |
22 | /** @addtogroup CMSIS
23 | * @{
24 | */
25 |
26 | /** @addtogroup stm32f10x_system
27 | * @{
28 | */
29 |
30 | /**
31 | * @brief Define to prevent recursive inclusion
32 | */
33 | #ifndef __SYSTEM_STM32F10X_H
34 | #define __SYSTEM_STM32F10X_H
35 |
36 | #ifdef __cplusplus
37 | extern "C" {
38 | #endif
39 |
40 | /** @addtogroup STM32F10x_System_Includes
41 | * @{
42 | */
43 |
44 | /**
45 | * @}
46 | */
47 |
48 |
49 | /** @addtogroup STM32F10x_System_Exported_types
50 | * @{
51 | */
52 |
53 | extern uint32_t SystemCoreClock; /*!< System Clock Frequency (Core Clock) */
54 |
55 | /**
56 | * @}
57 | */
58 |
59 | /** @addtogroup STM32F10x_System_Exported_Constants
60 | * @{
61 | */
62 |
63 | /**
64 | * @}
65 | */
66 |
67 | /** @addtogroup STM32F10x_System_Exported_Macros
68 | * @{
69 | */
70 |
71 | /**
72 | * @}
73 | */
74 |
75 | /** @addtogroup STM32F10x_System_Exported_Functions
76 | * @{
77 | */
78 |
79 | extern void SystemInit(void);
80 | extern void SystemCoreClockUpdate(void);
81 | /**
82 | * @}
83 | */
84 |
85 | #ifdef __cplusplus
86 | }
87 | #endif
88 |
89 | #endif /*__SYSTEM_STM32F10X_H */
90 |
91 | /**
92 | * @}
93 | */
94 |
95 | /**
96 | * @}
97 | */
98 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/
99 |
--------------------------------------------------------------------------------
/hardware/stm32_timersync-open/keilkilll.bat:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/WilsonGuo/FastLivo_Replication/eea82215c31adde5d914e54f9a38817e3aa9c942/hardware/stm32_timersync-open/keilkilll.bat
--------------------------------------------------------------------------------
/img/Demo.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/WilsonGuo/FastLivo_Replication/eea82215c31adde5d914e54f9a38817e3aa9c942/img/Demo.png
--------------------------------------------------------------------------------
/img/Demo1.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/WilsonGuo/FastLivo_Replication/eea82215c31adde5d914e54f9a38817e3aa9c942/img/Demo1.png
--------------------------------------------------------------------------------
/img/cover.bmp:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/WilsonGuo/FastLivo_Replication/eea82215c31adde5d914e54f9a38817e3aa9c942/img/cover.bmp
--------------------------------------------------------------------------------
/img/cover.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/WilsonGuo/FastLivo_Replication/eea82215c31adde5d914e54f9a38817e3aa9c942/img/cover.jpg
--------------------------------------------------------------------------------
/img/time_sysnc.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/WilsonGuo/FastLivo_Replication/eea82215c31adde5d914e54f9a38817e3aa9c942/img/time_sysnc.png
--------------------------------------------------------------------------------
/include/FOV_Checker/FOV_Checker.h:
--------------------------------------------------------------------------------
1 | // Include Files
2 | #pragma once
3 | #include
4 | #include
5 | #include "ikd-Tree/ikd_Tree.h"
6 | #include
7 | #include
8 |
9 | #define eps_value 1e-6
10 |
11 | struct PlaneType{
12 | Eigen::Vector3d p[4];
13 | };
14 |
15 | class FOV_Checker{
16 | public:
17 | FOV_Checker();
18 | ~FOV_Checker();
19 | void Set_Env(BoxPointType env_param);
20 | void Set_BoxLength(double box_len_param);
21 | void check_fov(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, vector &boxes);
22 | bool check_box(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, const BoxPointType box);
23 | bool check_line(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, Eigen::Vector3d line_p, Eigen::Vector3d line_vec);
24 | bool check_surface(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, PlaneType plane);
25 | bool check_point(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, Eigen::Vector3d point);
26 | bool check_box_in_env(BoxPointType box);
27 | private:
28 | BoxPointType env;
29 | double box_length;
30 | FILE *fp;
31 |
32 | };
33 |
34 |
--------------------------------------------------------------------------------
/include/IKFoM_toolkit/esekfom/util.hpp:
--------------------------------------------------------------------------------
1 | /*
2 | * Copyright (c) 2019--2023, The University of Hong Kong
3 | * All rights reserved.
4 | *
5 | * Author: Dongjiao HE
6 | *
7 | * Redistribution and use in source and binary forms, with or without
8 | * modification, are permitted provided that the following conditions
9 | * are met:
10 | *
11 | * * Redistributions of source code must retain the above copyright
12 | * notice, this list of conditions and the following disclaimer.
13 | * * Redistributions in binary form must reproduce the above
14 | * copyright notice, this list of conditions and the following
15 | * disclaimer in the documentation and/or other materials provided
16 | * with the distribution.
17 | * * Neither the name of the Universitaet Bremen nor the names of its
18 | * contributors may be used to endorse or promote products derived
19 | * from this software without specific prior written permission.
20 | *
21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 | * POSSIBILITY OF SUCH DAMAGE.
33 | */
34 |
35 | #ifndef __MEKFOM_UTIL_HPP__
36 | #define __MEKFOM_UTIL_HPP__
37 |
38 | #include
39 | #include "../mtk/src/mtkmath.hpp"
40 | namespace esekfom {
41 |
42 | template
43 | class is_same {
44 | public:
45 | operator bool() {
46 | return false;
47 | }
48 | };
49 | template
50 | class is_same {
51 | public:
52 | operator bool() {
53 | return true;
54 | }
55 | };
56 |
57 | template
58 | class is_double {
59 | public:
60 | operator bool() {
61 | return false;
62 | }
63 | };
64 |
65 | template<>
66 | class is_double {
67 | public:
68 | operator bool() {
69 | return true;
70 | }
71 | };
72 |
73 | template
74 | static T
75 | id(const T &x)
76 | {
77 | return x;
78 | }
79 |
80 | } // namespace esekfom
81 |
82 | #endif // __MEKFOM_UTIL_HPP__
83 |
--------------------------------------------------------------------------------
/include/IKFoM_toolkit/mtk/src/SubManifold.hpp:
--------------------------------------------------------------------------------
1 | // This is an advanced implementation of the algorithm described in the
2 | // following paper:
3 | // C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.
4 | // CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119
5 |
6 | /*
7 | * Copyright (c) 2019--2023, The University of Hong Kong
8 | * All rights reserved.
9 | *
10 | * Modifier: Dongjiao HE
11 | *
12 | * Redistribution and use in source and binary forms, with or without
13 | * modification, are permitted provided that the following conditions
14 | * are met:
15 | *
16 | * * Redistributions of source code must retain the above copyright
17 | * notice, this list of conditions and the following disclaimer.
18 | * * Redistributions in binary form must reproduce the above
19 | * copyright notice, this list of conditions and the following
20 | * disclaimer in the documentation and/or other materials provided
21 | * with the distribution.
22 | * * Neither the name of the Universitaet Bremen nor the names of its
23 | * contributors may be used to endorse or promote products derived
24 | * from this software without specific prior written permission.
25 | *
26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
28 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
29 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
30 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
31 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
32 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
33 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
34 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
35 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
36 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
37 | * POSSIBILITY OF SUCH DAMAGE.
38 | */
39 |
40 | /*
41 | * Copyright (c) 2008--2011, Universitaet Bremen
42 | * All rights reserved.
43 | *
44 | * Author: Christoph Hertzberg
45 | *
46 | * Redistribution and use in source and binary forms, with or without
47 | * modification, are permitted provided that the following conditions
48 | * are met:
49 | *
50 | * * Redistributions of source code must retain the above copyright
51 | * notice, this list of conditions and the following disclaimer.
52 | * * Redistributions in binary form must reproduce the above
53 | * copyright notice, this list of conditions and the following
54 | * disclaimer in the documentation and/or other materials provided
55 | * with the distribution.
56 | * * Neither the name of the Universitaet Bremen nor the names of its
57 | * contributors may be used to endorse or promote products derived
58 | * from this software without specific prior written permission.
59 | *
60 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
61 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
62 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
63 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
64 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
65 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
66 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
67 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
68 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
69 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
70 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
71 | * POSSIBILITY OF SUCH DAMAGE.
72 | */
73 | /**
74 | * @file mtk/src/SubManifold.hpp
75 | * @brief Defines the SubManifold class
76 | */
77 |
78 |
79 | #ifndef SUBMANIFOLD_HPP_
80 | #define SUBMANIFOLD_HPP_
81 |
82 |
83 | #include "vectview.hpp"
84 |
85 |
86 | namespace MTK {
87 |
88 | /**
89 | * @ingroup SubManifolds
90 | * Helper class for compound manifolds.
91 | * This class wraps a manifold T and provides an enum IDX refering to the
92 | * index of the SubManifold within the compound manifold.
93 | *
94 | * Memberpointers to a submanifold can be used for @ref SubManifolds "functions accessing submanifolds".
95 | *
96 | * @tparam T The manifold type of the sub-type
97 | * @tparam idx The index of the sub-type within the compound manifold
98 | */
99 | template
100 | struct SubManifold : public T
101 | {
102 | enum {IDX = idx, DIM = dim /*!< index of the sub-type within the compound manifold */ };
103 | //! manifold type
104 | typedef T type;
105 |
106 | //! Construct from derived type
107 | template
108 | explicit
109 | SubManifold(const X& t) : T(t) {};
110 |
111 | //! Construct from internal type
112 | //explicit
113 | SubManifold(const T& t) : T(t) {};
114 |
115 | //! inherit assignment operator
116 | using T::operator=;
117 |
118 | };
119 |
120 | } // namespace MTK
121 |
122 |
123 | #endif /* SUBMANIFOLD_HPP_ */
124 |
--------------------------------------------------------------------------------
/include/IKFoM_toolkit/mtk/src/vectview.hpp:
--------------------------------------------------------------------------------
1 |
2 | /*
3 | * Copyright (c) 2008--2011, Universitaet Bremen
4 | * All rights reserved.
5 | *
6 | * Author: Christoph Hertzberg
7 | *
8 | * Redistribution and use in source and binary forms, with or without
9 | * modification, are permitted provided that the following conditions
10 | * are met:
11 | *
12 | * * Redistributions of source code must retain the above copyright
13 | * notice, this list of conditions and the following disclaimer.
14 | * * Redistributions in binary form must reproduce the above
15 | * copyright notice, this list of conditions and the following
16 | * disclaimer in the documentation and/or other materials provided
17 | * with the distribution.
18 | * * Neither the name of the Universitaet Bremen nor the names of its
19 | * contributors may be used to endorse or promote products derived
20 | * from this software without specific prior written permission.
21 | *
22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 | * POSSIBILITY OF SUCH DAMAGE.
34 | */
35 | /**
36 | * @file mtk/src/vectview.hpp
37 | * @brief Wrapper class around a pointer used as interface for plain vectors.
38 | */
39 |
40 | #ifndef VECTVIEW_HPP_
41 | #define VECTVIEW_HPP_
42 |
43 | #include
44 |
45 | namespace MTK {
46 |
47 | /**
48 | * A view to a vector.
49 | * Essentially, @c vectview is only a pointer to @c scalar but can be used directly in @c Eigen expressions.
50 | * The dimension of the vector is given as template parameter and type-checked when used in expressions.
51 | * Data has to be modifiable.
52 | *
53 | * @tparam scalar Scalar type of the vector.
54 | * @tparam dim Dimension of the vector.
55 | *
56 | * @todo @c vectview can be replaced by simple inheritance of @c Eigen::Map, as soon as they get const-correct
57 | */
58 | namespace internal {
59 | template
60 | struct CovBlock {
61 | typedef typename Eigen::Block, T1::DOF, T2::DOF> Type;
62 | typedef typename Eigen::Block, T1::DOF, T2::DOF> ConstType;
63 | };
64 |
65 | template
66 | struct CovBlock_ {
67 | typedef typename Eigen::Block, T1::DIM, T2::DIM> Type;
68 | typedef typename Eigen::Block, T1::DIM, T2::DIM> ConstType;
69 | };
70 |
71 | template
72 | struct CrossCovBlock {
73 | typedef typename Eigen::Block, T1::DOF, T2::DOF> Type;
74 | typedef typename Eigen::Block, T1::DOF, T2::DOF> ConstType;
75 | };
76 |
77 | template
78 | struct CrossCovBlock_ {
79 | typedef typename Eigen::Block, T1::DIM, T2::DIM> Type;
80 | typedef typename Eigen::Block, T1::DIM, T2::DIM> ConstType;
81 | };
82 |
83 | template
84 | struct VectviewBase {
85 | typedef Eigen::Matrix matrix_type;
86 | typedef typename matrix_type::MapType Type;
87 | typedef typename matrix_type::ConstMapType ConstType;
88 | };
89 |
90 | template
91 | struct UnalignedType {
92 | typedef T type;
93 | };
94 | }
95 |
96 | template
97 | class vectview : public internal::VectviewBase::Type {
98 | typedef internal::VectviewBase VectviewBase;
99 | public:
100 | //! plain matrix type
101 | typedef typename VectviewBase::matrix_type matrix_type;
102 | //! base type
103 | typedef typename VectviewBase::Type base;
104 | //! construct from pointer
105 | explicit
106 | vectview(scalar* data, int dim_=dim) : base(data, dim_) {}
107 | //! construct from plain matrix
108 | vectview(matrix_type& m) : base(m.data(), m.size()) {}
109 | //! construct from another @c vectview
110 | vectview(const vectview &v) : base(v) {}
111 | //! construct from Eigen::Block:
112 | template
113 | vectview(Eigen::VectorBlock block) : base(&block.coeffRef(0), block.size()) {}
114 | template
115 | vectview(Eigen::Block block) : base(&block.coeffRef(0), block.size()) {}
116 |
117 | //! inherit assignment operator
118 | using base::operator=;
119 | //! data pointer
120 | scalar* data() {return const_cast(base::data());}
121 | };
122 |
123 | /**
124 | * @c const version of @c vectview.
125 | * Compared to @c Eigen::Map this implementation is const correct, i.e.,
126 | * data will not be modifiable using this view.
127 | *
128 | * @tparam scalar Scalar type of the vector.
129 | * @tparam dim Dimension of the vector.
130 | *
131 | * @sa vectview
132 | */
133 | template
134 | class vectview : public internal::VectviewBase::ConstType {
135 | typedef internal::VectviewBase VectviewBase;
136 | public:
137 | //! plain matrix type
138 | typedef typename VectviewBase::matrix_type matrix_type;
139 | //! base type
140 | typedef typename VectviewBase::ConstType base;
141 | //! construct from const pointer
142 | explicit
143 | vectview(const scalar* data, int dim_ = dim) : base(data, dim_) {}
144 | //! construct from column vector
145 | template
146 | vectview(const Eigen::Matrix& m) : base(m.data()) {}
147 | //! construct from row vector
148 | template
149 | vectview(const Eigen::Matrix& m) : base(m.data()) {}
150 | //! construct from another @c vectview
151 | vectview(vectview x) : base(x.data()) {}
152 | //! construct from base
153 | vectview(const base &x) : base(x) {}
154 | /**
155 | * Construct from Block
156 | * @todo adapt this, when Block gets const-correct
157 | */
158 | template
159 | vectview(Eigen::VectorBlock block) : base(&block.coeffRef(0)) {}
160 | template
161 | vectview(Eigen::Block block) : base(&block.coeffRef(0)) {}
162 |
163 | };
164 |
165 |
166 | } // namespace MTK
167 |
168 | #endif /* VECTVIEW_HPP_ */
169 |
--------------------------------------------------------------------------------
/include/IKFoM_toolkit/mtk/types/wrapped_cv_mat.hpp:
--------------------------------------------------------------------------------
1 | /*
2 | * Copyright (c) 2010--2011, Universitaet Bremen and DFKI GmbH
3 | * All rights reserved.
4 | *
5 | * Author: Rene Wagner
6 | * Christoph Hertzberg
7 | *
8 | * Redistribution and use in source and binary forms, with or without
9 | * modification, are permitted provided that the following conditions
10 | * are met:
11 | *
12 | * * Redistributions of source code must retain the above copyright
13 | * notice, this list of conditions and the following disclaimer.
14 | * * Redistributions in binary form must reproduce the above
15 | * copyright notice, this list of conditions and the following
16 | * disclaimer in the documentation and/or other materials provided
17 | * with the distribution.
18 | * * Neither the name of the Universitaet Bremen nor the DFKI GmbH
19 | * nor the names of its contributors may be used to endorse or
20 | * promote products derived from this software without specific
21 | * prior written permission.
22 | *
23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 | * POSSIBILITY OF SUCH DAMAGE.
35 | */
36 |
37 | #ifndef WRAPPED_CV_MAT_HPP_
38 | #define WRAPPED_CV_MAT_HPP_
39 |
40 | #include
41 | #include
42 |
43 | namespace MTK {
44 |
45 | template
46 | struct cv_f_type;
47 |
48 | template<>
49 | struct cv_f_type
50 | {
51 | enum {value = CV_64F};
52 | };
53 |
54 | template<>
55 | struct cv_f_type
56 | {
57 | enum {value = CV_32F};
58 | };
59 |
60 | /**
61 | * cv_mat wraps a CvMat around an Eigen Matrix
62 | */
63 | template
64 | class cv_mat : public matrix
65 | {
66 | typedef matrix base_type;
67 | enum {type_ = cv_f_type::value};
68 | CvMat cv_mat_;
69 |
70 | public:
71 | cv_mat()
72 | {
73 | cv_mat_ = cvMat(rows, cols, type_, base_type::data());
74 | }
75 |
76 | cv_mat(const cv_mat& oth) : base_type(oth)
77 | {
78 | cv_mat_ = cvMat(rows, cols, type_, base_type::data());
79 | }
80 |
81 | template
82 | cv_mat(const Eigen::MatrixBase &value) : base_type(value)
83 | {
84 | cv_mat_ = cvMat(rows, cols, type_, base_type::data());
85 | }
86 |
87 | template
88 | cv_mat& operator=(const Eigen::MatrixBase &value)
89 | {
90 | base_type::operator=(value);
91 | return *this;
92 | }
93 |
94 | cv_mat& operator=(const cv_mat& value)
95 | {
96 | base_type::operator=(value);
97 | return *this;
98 | }
99 |
100 | // FIXME: Maybe overloading operator& is not a good idea ...
101 | CvMat* operator&()
102 | {
103 | return &cv_mat_;
104 | }
105 | const CvMat* operator&() const
106 | {
107 | return &cv_mat_;
108 | }
109 | };
110 |
111 | } // namespace MTK
112 |
113 | #endif /* WRAPPED_CV_MAT_HPP_ */
114 |
--------------------------------------------------------------------------------
/include/IMU_Processing.h:
--------------------------------------------------------------------------------
1 |
2 | #ifndef IMU_PROCESSING_H
3 | #define IMU_PROCESSING_H
4 | #include
5 | #include
6 | #include
7 | #include
8 | #include
9 | #include
10 | #include
11 | #include
12 | #include
13 | #include
14 | #include
15 | #include
16 | #include
17 | #include
18 | #include
19 | #include
20 | #include
21 | #include
22 | #include
23 | #include
24 | #include
25 | #include
26 | #include
27 | #include
28 | #include
29 |
30 | #ifdef USE_IKFOM
31 | #include "use-ikfom.hpp"
32 | #endif
33 |
34 | /// *************Preconfiguration
35 |
36 | #define MAX_INI_COUNT (200)
37 |
38 | const bool time_list(PointType &x, PointType &y); //{return (x.curvature < y.curvature);};
39 |
40 | /// *************IMU Process and undistortion
41 | class ImuProcess
42 | {
43 | public:
44 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW
45 |
46 | ImuProcess();
47 | ~ImuProcess();
48 |
49 | void Reset();
50 | void Reset(double start_timestamp, const sensor_msgs::ImuConstPtr &lastimu);
51 | void push_update_state(double offs_t, StatesGroup state);
52 | void set_extrinsic(const V3D &transl, const M3D &rot);
53 | void set_extrinsic(const V3D &transl);
54 | void set_extrinsic(const MD(4,4) &T);
55 | void set_gyr_cov_scale(const V3D &scaler);
56 | void set_acc_cov_scale(const V3D &scaler);
57 | void set_gyr_bias_cov(const V3D &b_g);
58 | void set_acc_bias_cov(const V3D &b_a);
59 | #ifdef USE_IKFOM
60 | Eigen::Matrix Q;
61 | void Process(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI::Ptr pcl_un_);
62 | #else
63 | void Process(const LidarMeasureGroup &lidar_meas, StatesGroup &stat, PointCloudXYZI::Ptr cur_pcl_un_);
64 | void Process2(LidarMeasureGroup &lidar_meas, StatesGroup &stat, PointCloudXYZI::Ptr cur_pcl_un_);
65 | void UndistortPcl(LidarMeasureGroup &lidar_meas, StatesGroup &state_inout, PointCloudXYZI &pcl_out);
66 | #endif
67 |
68 | ros::NodeHandle nh;
69 | ofstream fout_imu;
70 | V3D cov_acc;
71 | V3D cov_gyr;
72 | V3D cov_acc_scale;
73 | V3D cov_gyr_scale;
74 | V3D cov_bias_gyr;
75 | V3D cov_bias_acc;
76 | double first_lidar_time;
77 |
78 | private:
79 | #ifdef USE_IKFOM
80 | void IMU_init(const MeasureGroup &meas, esekfom::esekf &kf_state, int &N);
81 | void UndistortPcl(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI &pcl_in_out);
82 | #else
83 | void IMU_init(const MeasureGroup &meas, StatesGroup &state, int &N);
84 | void Forward(const MeasureGroup &meas, StatesGroup &state_inout, double pcl_beg_time, double end_time);
85 | void Backward(const LidarMeasureGroup &lidar_meas, StatesGroup &state_inout, PointCloudXYZI &pcl_out);
86 | #endif
87 |
88 | PointCloudXYZI::Ptr cur_pcl_un_;
89 | sensor_msgs::ImuConstPtr last_imu_;
90 | deque v_imu_;
91 | vector IMUpose;
92 | vector v_rot_pcl_;
93 | M3D Lid_rot_to_IMU;
94 | V3D Lid_offset_to_IMU;
95 | V3D mean_acc;
96 | V3D mean_gyr;
97 | V3D angvel_last;
98 | V3D acc_s_last;
99 | V3D last_acc;
100 | V3D last_ang;
101 | double start_timestamp_;
102 | double last_lidar_end_time_;
103 | int init_iter_num = 1;
104 | bool b_first_frame_ = true;
105 | bool imu_need_init_ = true;
106 | };
107 | #endif
108 |
--------------------------------------------------------------------------------
/include/feature.h:
--------------------------------------------------------------------------------
1 | // This file is part of SVO - Semi-direct Visual Odometry.
2 | //
3 | // Copyright (C) 2014 Christian Forster
4 | // (Robotics and Perception Group, University of Zurich, Switzerland).
5 | //
6 | // SVO is free software: you can redistribute it and/or modify it under the
7 | // terms of the GNU General Public License as published by the Free Software
8 | // Foundation, either version 3 of the License, or any later version.
9 | //
10 | // SVO is distributed in the hope that it will be useful, but WITHOUT ANY
11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
13 | //
14 | // You should have received a copy of the GNU General Public License
15 | // along with this program. If not, see .
16 |
17 | #ifndef SVO_FEATURE_H_
18 | #define SVO_FEATURE_H_
19 |
20 | #include
21 | #include
22 | #include
23 |
24 | namespace lidar_selection {
25 |
26 | // A salient image region that is tracked across frames.
27 | struct Feature
28 | {
29 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW
30 |
31 | enum FeatureType {
32 | CORNER,
33 | EDGELET
34 | };
35 | int id_;
36 | FeatureType type; //!< Type can be corner or edgelet.
37 | Frame* frame; //!< Pointer to frame in which the feature was detected.
38 | cv::Mat img;
39 | vector ImgPyr;
40 | Vector2d px; //!< Coordinates in pixels on pyramid level 0.
41 | Vector3d f; //!< Unit-bearing vector of the feature.
42 | int level; //!< Image pyramid level where feature was extracted.
43 | PointPtr point; //!< Pointer to 3D point which corresponds to the feature.
44 | Vector2d grad; //!< Dominant gradient direction for edglets, normalized.
45 | float score;
46 | float error;
47 | // Vector2d grad_cur_; //!< edgelete grad direction in cur frame
48 | SE3 T_f_w_;
49 | float* patch;
50 | Feature(float* _patch, const Vector2d& _px, const Vector3d& _f, const SE3& _T_f_w, const float &_score, int _level) :
51 | type(CORNER),
52 | px(_px),
53 | f(_f),
54 | T_f_w_(_T_f_w),
55 | level(_level),
56 | patch(_patch),
57 | score(_score)
58 | {}
59 | inline Vector3d pos() const { return T_f_w_.inverse().translation(); }
60 | ~Feature()
61 | {
62 | // printf("The feature %d has been destructed.", id_);
63 | delete[] patch;
64 | }
65 | };
66 |
67 | } // namespace lidar_selection
68 |
69 | #endif // SVO_FEATURE_H_
70 |
--------------------------------------------------------------------------------
/include/frame.h:
--------------------------------------------------------------------------------
1 | // This file is part of SVO - Semi-direct Visual Odometry.
2 | //
3 | // Copyright (C) 2014 Christian Forster
4 | // (Robotics and Perception Group, University of Zurich, Switzerland).
5 | //
6 | // SVO is free software: you can redistribute it and/or modify it under the
7 | // terms of the GNU General Public License as published by the Free Software
8 | // Foundation, either version 3 of the License, or any later version.
9 | //
10 | // SVO is distributed in the hope that it will be useful, but WITHOUT ANY
11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
13 | //
14 | // You should have received a copy of the GNU General Public License
15 | // along with this program. If not, see .
16 |
17 | #ifndef SVO_FRAME_H_
18 | #define SVO_FRAME_H_
19 |
20 | #include
21 | #include
22 | #include
23 | #include
24 |
25 | namespace lidar_selection {
26 |
27 | struct Feature;
28 | typedef std::shared_ptr FeaturePtr;
29 |
30 | class Point;
31 | typedef std::shared_ptr PointPtr;
32 |
33 | typedef list Features;
34 | typedef vector ImgPyr;
35 |
36 | /// A frame saves the image, the associated features and the estimated pose.
37 | class Frame : boost::noncopyable
38 | {
39 | public:
40 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW
41 |
42 | static int frame_counter_; //!< Counts the number of created frames. Used to set the unique id.
43 | int id_; //!< Unique id of the frame.
44 | // double timestamp_; //!< Timestamp of when the image was recorded.
45 | vk::AbstractCamera* cam_; //!< Camera model.
46 | SE3 T_f_w_; //!< Transform (f)rame from (w)orld.
47 | Matrix Cov_; //!< Covariance.
48 | ImgPyr img_pyr_; //!< Image Pyramid.
49 | Features fts_; //!< List of features in the image.
50 | vector key_pts_; //!< Five features and associated 3D points which are used to detect if two frames have overlapping field of view.
51 | bool is_keyframe_; //!< Was this frames selected as keyframe
52 |
53 | Frame(vk::AbstractCamera* cam, const cv::Mat& img);
54 | ~Frame();
55 |
56 | /// Initialize new frame and create image pyramid.
57 | void initFrame(const cv::Mat& img);
58 |
59 | /// Select this frame as keyframe.
60 | void setKeyframe();
61 |
62 | /// Add a feature to the image
63 | void addFeature(FeaturePtr ftr);
64 |
65 | /// The KeyPoints are those five features which are closest to the 4 image corners
66 | /// and to the center and which have a 3D point assigned. These points are used
67 | /// to quickly check whether two frames have overlapping field of view.
68 | void setKeyPoints();
69 |
70 | /// Check if we can select five better key-points.
71 | void checkKeyPoints(FeaturePtr ftr);
72 |
73 | /// If a point is deleted, we must remove the corresponding key-point.
74 | void removeKeyPoint(FeaturePtr ftr);
75 |
76 | /// Return number of point observations.
77 | inline size_t nObs() const { return fts_.size(); }
78 |
79 | /// Check if a point in (w)orld coordinate frame is visible in the image.
80 | bool isVisible(const Vector3d& xyz_w) const;
81 |
82 | /// Full resolution image stored in the frame.
83 | inline const cv::Mat& img() const { return img_pyr_[0]; }
84 |
85 | /// Was this frame selected as keyframe?
86 | inline bool isKeyframe() const { return is_keyframe_; }
87 |
88 | /// Transforms point coordinates in world-frame (w) to camera pixel coordinates (c).
89 | inline Vector2d w2c(const Vector3d& xyz_w) const { return cam_->world2cam( T_f_w_ * xyz_w ); }
90 |
91 | /// Transforms pixel coordinates (c) to frame unit sphere coordinates (f).
92 | inline Vector3d c2f(const Vector2d& px) const { return cam_->cam2world(px[0], px[1]); }
93 |
94 | /// Transforms pixel coordinates (c) to frame unit sphere coordinates (f).
95 | inline Vector3d c2f(const double x, const double y) const { return cam_->cam2world(x, y); }
96 |
97 | /// Transforms point coordinates in world-frame (w) to camera-frams (f).
98 | inline Vector3d w2f(const Vector3d& xyz_w) const { return T_f_w_ * xyz_w; }
99 |
100 | /// Transforms point from frame unit sphere (f) frame to world coordinate frame (w).
101 | inline Vector3d f2w(const Vector3d& f) const { return T_f_w_.inverse() * f; }
102 |
103 | /// Projects Point from unit sphere (f) in camera pixels (c).
104 | inline Vector2d f2c(const Vector3d& f) const { return cam_->world2cam( f ); }
105 |
106 | /// Return the pose of the frame in the (w)orld coordinate frame.
107 | inline Vector3d pos() const { return T_f_w_.inverse().translation(); }
108 |
109 | /// Frame jacobian for projection of 3D point in (f)rame coordinate to
110 | /// unit plane coordinates uv (focal length = 1).
111 | inline static void jacobian_xyz2uv_change(
112 | const Vector3d& xyz_in_world,
113 | const Vector3d& xyz_in_f,
114 | Matrix& J,
115 | SE3& Tbc,
116 | SE3& T_ref_w,
117 | double fx)
118 | {
119 | //Vector3d xyz_in_imu = Tbc * xyz_in_world;
120 | //Vector3d xyz_in_imu = xyz_in_world;
121 | const double x = xyz_in_f[0];
122 | const double y = xyz_in_f[1];
123 | //const double z = xyz_in_f[2];
124 | //! minus symbol is added here.
125 | const double z_inv = 1./xyz_in_f[2];
126 | const double z_inv_2 = z_inv*z_inv;
127 |
128 | const double x_in_world = xyz_in_world[0];
129 | const double y_in_world = xyz_in_world[1];
130 | const double z_in_world = xyz_in_world[2];
131 |
132 | Matrix J1;
133 | Matrix J2;
134 |
135 | J1(0,0) = -fx * z_inv;
136 | J1(0,1) = 0.0;
137 | J1(0,2) = fx * x * z_inv_2;
138 |
139 | J1(1,0) = 0.0;
140 | J1(1,1) = -fx * z_inv;
141 | J1(1,2) = fx * y * z_inv_2;
142 |
143 | J2(0,0) = 1.0;
144 | J2(0,1) = 0.0;
145 | J2(0,2) = 0.0;
146 | J2(0,3) = 0.0;
147 | J2(0,4) = z_in_world;
148 | J2(0,5) = -y_in_world;
149 |
150 | J2(1,0) = 0.0;
151 | J2(1,1) = 1.0;
152 | J2(1,2) = 0.0;
153 | J2(1,3) = -z_in_world;
154 | J2(1,4) = 0.0;
155 | J2(1,5) = x_in_world;
156 |
157 | J2(2,0) = 0.0;
158 | J2(2,1) = 0.0;
159 | J2(2,2) = 1.0;
160 | J2(2,3) = y_in_world;
161 | J2(2,4) = -x_in_world;
162 | J2(2,5) = 0.0;
163 |
164 | J = J1 * T_ref_w.rotation_matrix() * J2;// * J2;
165 | }
166 |
167 | inline static void jacobian_xyz2uv(
168 | const Vector3d& xyz_in_f,
169 | Matrix& J)
170 | {
171 | const double x = xyz_in_f[0];
172 | const double y = xyz_in_f[1];
173 | const double z_inv = 1./xyz_in_f[2];
174 | const double z_inv_2 = z_inv*z_inv;
175 |
176 | J(0,0) = -z_inv; // -1/z
177 | J(0,1) = 0.0; // 0
178 | J(0,2) = x*z_inv_2; // x/z^2
179 | J(0,3) = y*J(0,2); // x*y/z^2
180 | J(0,4) = -(1.0 + x*J(0,2)); // -(1.0 + x^2/z^2)
181 | J(0,5) = y*z_inv; // y/z
182 |
183 | J(1,0) = 0.0; // 0
184 | J(1,1) = -z_inv; // -1/z
185 | J(1,2) = y*z_inv_2; // y/z^2
186 | J(1,3) = 1.0 + y*J(1,2); // 1.0 + y^2/z^2
187 | J(1,4) = -J(0,3); // -x*y/z^2
188 | J(1,5) = -x*z_inv; // x/z
189 | }
190 | };
191 |
192 | typedef std::shared_ptr FramePtr;
193 |
194 | /// Some helper functions for the frame object.
195 | namespace frame_utils {
196 |
197 | /// Creates an image pyramid of half-sampled images.
198 | void createImgPyramid(const cv::Mat& img_level_0, int n_levels, ImgPyr& pyr);
199 |
200 | /// Get the average depth of the features in the image.
201 | bool getSceneDepth(const Frame& frame, double& depth_mean, double& depth_min);
202 |
203 | } // namespace frame_utils
204 | } // namespace svo
205 |
206 | #endif // SVO_FRAME_H_
207 |
--------------------------------------------------------------------------------
/include/ikd-Tree/.vscode/c_cpp_properties.json:
--------------------------------------------------------------------------------
1 | {
2 | "configurations": [
3 | {
4 | "browse": {
5 | "databaseFilename": "",
6 | "limitSymbolsToIncludedHeaders": true
7 | },
8 | "includePath": [
9 | "/home/zqykawayi/FAST_LIO_L515_ws/devel/include/**",
10 | "/home/zqykawayi/livox_ws/devel/include/**",
11 | "/opt/ros/melodic/include/**",
12 | "/home/zqykawayi/FAST_LIO_L515_ws/src/FAST_LIO/include/**",
13 | "/home/zqykawayi/spatial_ws/src/spatial/include/**",
14 | "/home/zqykawayi/FAST_LIO_L515_ws/src/rpg_vikit/vikit_common/include/**",
15 | "/home/zqykawayi/FAST_LIO_L515_ws/src/rpg_vikit/vikit_ros/include/**",
16 | "/usr/include/**"
17 | ],
18 | "name": "ROS"
19 | }
20 | ],
21 | "version": 4
22 | }
--------------------------------------------------------------------------------
/include/ikd-Tree/.vscode/settings.json:
--------------------------------------------------------------------------------
1 | {
2 | "python.autoComplete.extraPaths": [
3 | "/home/zqykawayi/FAST_LIO_L515_ws/devel/lib/python2.7/dist-packages",
4 | "/home/zqykawayi/livox_ws/devel/lib/python2.7/dist-packages",
5 | "/opt/ros/melodic/lib/python2.7/dist-packages"
6 | ]
7 | }
--------------------------------------------------------------------------------
/include/ikd-Tree/README.md:
--------------------------------------------------------------------------------
1 | # ikd-Tree
2 | ikd-Tree is an incremental k-d tree for robotic applications.
3 |
--------------------------------------------------------------------------------
/include/ikd-Tree/ikd_Tree.h:
--------------------------------------------------------------------------------
1 | #pragma once
2 | #include
3 | #include
4 | #include
5 | #include
6 | #include
7 | #include
8 | #include
9 | #include
10 |
11 |
12 | #define EPSS 1e-6
13 | #define Minimal_Unbalanced_Tree_Size 10
14 | #define Multi_Thread_Rebuild_Point_Num 1500
15 | #define DOWNSAMPLE_SWITCH true
16 | #define ForceRebuildPercentage 0.2
17 | #define Q_LEN 1000000
18 |
19 | using namespace std;
20 |
21 | typedef pcl::PointXYZINormal PointType;
22 | typedef vector> PointVector;
23 |
24 | const PointType ZeroP;
25 |
26 | struct KD_TREE_NODE
27 | {
28 | PointType point;
29 | int division_axis;
30 | int TreeSize = 1;
31 | int invalid_point_num = 0;
32 | int down_del_num = 0;
33 | bool point_deleted = false;
34 | bool tree_deleted = false;
35 | bool point_downsample_deleted = false;
36 | bool tree_downsample_deleted = false;
37 | bool need_push_down_to_left = false;
38 | bool need_push_down_to_right = false;
39 | bool working_flag = false;
40 | pthread_mutex_t push_down_mutex_lock;
41 | float node_range_x[2], node_range_y[2], node_range_z[2];
42 | KD_TREE_NODE *left_son_ptr = nullptr;
43 | KD_TREE_NODE *right_son_ptr = nullptr;
44 | KD_TREE_NODE *father_ptr = nullptr;
45 | // For paper data record
46 | float alpha_del;
47 | float alpha_bal;
48 | };
49 |
50 | struct PointType_CMP{
51 | PointType point;
52 | float dist = 0.0;
53 | PointType_CMP (PointType p = ZeroP, float d = INFINITY){
54 | this->point = p;
55 | this->dist = d;
56 | };
57 | bool operator < (const PointType_CMP &a)const{
58 | if (fabs(dist - a.dist) < 1e-10) return point.x < a.point.x;
59 | else return dist < a.dist;
60 | }
61 | };
62 |
63 | struct BoxPointType{
64 | float vertex_min[3];
65 | float vertex_max[3];
66 | };
67 |
68 | enum operation_set {ADD_POINT, DELETE_POINT, DELETE_BOX, ADD_BOX, DOWNSAMPLE_DELETE, PUSH_DOWN};
69 |
70 | enum delete_point_storage_set {NOT_RECORD, DELETE_POINTS_REC, MULTI_THREAD_REC};
71 |
72 | struct Operation_Logger_Type{
73 | PointType point;
74 | BoxPointType boxpoint;
75 | bool tree_deleted, tree_downsample_deleted;
76 | operation_set op;
77 | };
78 |
79 | class MANUAL_Q{
80 | private:
81 | int head = 0,tail = 0, counter = 0;
82 | Operation_Logger_Type q[Q_LEN];
83 | bool is_empty;
84 | public:
85 | void pop();
86 | Operation_Logger_Type front();
87 | Operation_Logger_Type back();
88 | void clear();
89 | void push(Operation_Logger_Type op);
90 | bool empty();
91 | int size();
92 | };
93 |
94 | class MANUAL_HEAP
95 | {
96 | public:
97 | MANUAL_HEAP(int max_capacity = 100);
98 | ~MANUAL_HEAP();
99 | void pop();
100 | PointType_CMP top();
101 | void push(PointType_CMP point);
102 | int size();
103 | void clear();
104 | private:
105 | PointType_CMP * heap;
106 | void MoveDown(int heap_index);
107 | void FloatUp(int heap_index);
108 | int heap_size = 0;
109 | int cap = 0;
110 | };
111 |
112 |
113 | class KD_TREE
114 | {
115 | private:
116 | // Multi-thread Tree Rebuild
117 | bool termination_flag = false;
118 | bool rebuild_flag = false;
119 | pthread_t rebuild_thread;
120 | pthread_mutex_t termination_flag_mutex_lock, rebuild_ptr_mutex_lock, working_flag_mutex, search_flag_mutex;
121 | pthread_mutex_t rebuild_logger_mutex_lock, points_deleted_rebuild_mutex_lock;
122 | // queue Rebuild_Logger;
123 | MANUAL_Q Rebuild_Logger;
124 | PointVector Rebuild_PCL_Storage;
125 | KD_TREE_NODE ** Rebuild_Ptr;
126 | int search_mutex_counter = 0;
127 | static void * multi_thread_ptr(void *arg);
128 | void multi_thread_rebuild();
129 | void start_thread();
130 | void stop_thread();
131 | void run_operation(KD_TREE_NODE ** root, Operation_Logger_Type operation);
132 | // KD Tree Functions and augmented variables
133 | int Treesize_tmp = 0, Validnum_tmp = 0;
134 | float alpha_bal_tmp = 0.5, alpha_del_tmp = 0.0;
135 | float delete_criterion_param = 0.5f;
136 | float balance_criterion_param = 0.7f;
137 | float downsample_size = 0.2f;
138 | bool Delete_Storage_Disabled = false;
139 | KD_TREE_NODE * STATIC_ROOT_NODE = nullptr;
140 | PointVector Points_deleted;
141 | PointVector Downsample_Storage;
142 | PointVector Multithread_Points_deleted;
143 | void InitTreeNode(KD_TREE_NODE * root);
144 | void Test_Lock_States(KD_TREE_NODE *root);
145 | void BuildTree(KD_TREE_NODE ** root, int l, int r, PointVector & Storage);
146 | void Rebuild(KD_TREE_NODE ** root);
147 | int Delete_by_range(KD_TREE_NODE ** root, BoxPointType boxpoint, bool allow_rebuild, bool is_downsample);
148 | void Delete_by_point(KD_TREE_NODE ** root, PointType point, bool allow_rebuild);
149 | void Add_by_point(KD_TREE_NODE ** root, PointType point, bool allow_rebuild, int father_axis);
150 | void Add_by_range(KD_TREE_NODE ** root, BoxPointType boxpoint, bool allow_rebuild);
151 | void Search(KD_TREE_NODE * root, int k_nearest, PointType point, MANUAL_HEAP &q, double max_dist);//priority_queue
152 | void Search_by_range(KD_TREE_NODE *root, BoxPointType boxpoint, PointVector &Storage);
153 | bool Criterion_Check(KD_TREE_NODE * root);
154 | void Push_Down(KD_TREE_NODE * root);
155 | void Update(KD_TREE_NODE * root);
156 | void delete_tree_nodes(KD_TREE_NODE ** root);
157 | void downsample(KD_TREE_NODE ** root);
158 | bool same_point(PointType a, PointType b);
159 | float calc_dist(PointType a, PointType b);
160 | float calc_box_dist(KD_TREE_NODE * node, PointType point);
161 | static bool point_cmp_x(PointType a, PointType b);
162 | static bool point_cmp_y(PointType a, PointType b);
163 | static bool point_cmp_z(PointType a, PointType b);
164 | void print_treenode(KD_TREE_NODE * root, int index, FILE *fp, float x_min, float x_max, float y_min, float y_max, float z_min, float z_max);
165 |
166 | public:
167 | KD_TREE(float delete_param = 0.5, float balance_param = 0.6 , float box_length = 0.2);
168 | ~KD_TREE();
169 | void Set_delete_criterion_param(float delete_param);
170 | void Set_balance_criterion_param(float balance_param);
171 | void set_downsample_param(float box_length);
172 | void InitializeKDTree(float delete_param = 0.5, float balance_param = 0.7, float box_length = 0.2);
173 | int size();
174 | int validnum();
175 | void root_alpha(float &alpha_bal, float &alpha_del);
176 | void Build(PointVector point_cloud);
177 | void Nearest_Search(PointType point, int k_nearest, PointVector &Nearest_Points, vector & Point_Distance, double max_dist = INFINITY);
178 | int Add_Points(PointVector & PointToAdd, bool downsample_on);
179 | void Add_Point_Boxes(vector & BoxPoints);
180 | void Delete_Points(PointVector & PointToDel);
181 | int Delete_Point_Boxes(vector & BoxPoints);
182 | void flatten(KD_TREE_NODE * root, PointVector &Storage, delete_point_storage_set storage_type);
183 | void acquire_removed_points(PointVector & removed_points);
184 | void print_tree(int index, FILE *fp, float x_min, float x_max, float y_min, float y_max, float z_min, float z_max);
185 | BoxPointType tree_range();
186 | PointVector PCL_Storage;
187 | KD_TREE_NODE * Root_Node = nullptr;
188 | int max_queue_size = 0;
189 | };
190 |
--------------------------------------------------------------------------------
/include/lidar_selection.h:
--------------------------------------------------------------------------------
1 | #ifndef LIDAR_SELECTION_H_
2 | #define LIDAR_SELECTION_H_
3 |
4 | #include
5 | #include
6 | #include
7 | #include
8 | #include
9 | #include
10 | #include
11 | #include
12 | #include
13 | #include
14 | #include
15 | #include
16 | #include
17 |
18 | namespace lidar_selection {
19 |
20 | class LidarSelector {
21 | public:
22 | int grid_size;
23 | vk::AbstractCamera* cam;
24 | SparseMap* sparse_map;
25 | StatesGroup* state;
26 | StatesGroup* state_propagat;
27 | M3D Rli, Rci, Rcw, Jdphi_dR, Jdp_dt, Jdp_dR;
28 | V3D Pli, Pci, Pcw;
29 | int* align_flag;
30 | int* grid_num;
31 | int* map_index;
32 | float* map_dist;
33 | float* map_value;
34 | float* patch_cache;
35 | float* patch_with_border_;
36 | int width, height, grid_n_width, grid_n_height, length;
37 | SubSparseMap* sub_sparse_map;
38 | double fx,fy,cx,cy;
39 | bool ncc_en;
40 | int debug, patch_size, patch_size_total, patch_size_half;
41 | int count_img, MIN_IMG_COUNT;
42 | int NUM_MAX_ITERATIONS;
43 | vk::robust_cost::WeightFunctionPtr weight_function_;
44 | float weight_scale_;
45 | double img_point_cov, outlier_threshold, ncc_thre;
46 | size_t n_meas_; //!< Number of measurements
47 | deque< PointPtr > map_cur_frame_;
48 | deque< PointPtr > sub_map_cur_frame_;
49 | double computeH, ekf_time;
50 | double ave_total = 0.01;
51 | int frame_cont = 1;
52 | vk::robust_cost::ScaleEstimatorPtr scale_estimator_;
53 |
54 | Matrix G, H_T_H;
55 | MatrixXd H_sub, K;
56 | cv::flann::Index Kdtree;
57 |
58 | LidarSelector(const int grid_size, SparseMap* sparse_map);
59 |
60 | ~LidarSelector();
61 |
62 | void detect(cv::Mat img, PointCloudXYZI::Ptr pg);
63 | float CheckGoodPoints(cv::Mat img, V2D uv);
64 | void addFromSparseMap(cv::Mat img, PointCloudXYZI::Ptr pg);
65 | void addSparseMap(cv::Mat img, PointCloudXYZI::Ptr pg);
66 | void FeatureAlignment(cv::Mat img);
67 | void set_extrinsic(const V3D &transl, const M3D &rot);
68 | void init();
69 | void getpatch(cv::Mat img, V3D pg, float* patch_tmp, int level);
70 | void getpatch(cv::Mat img, V2D pc, float* patch_tmp, int level);
71 | void dpi(V3D p, MD(2,3)& J);
72 | float UpdateState(cv::Mat img, float total_residual, int level);
73 | double NCC(float* ref_patch, float* cur_patch, int patch_size);
74 |
75 | void ComputeJ(cv::Mat img);
76 | void reset_grid();
77 | void addObservation(cv::Mat img);
78 | void reset();
79 | bool initialization(FramePtr cur_frame, PointCloudXYZI::Ptr pg);
80 | void createPatchFromPatchWithBorder(float* patch_with_border, float* patch_ref);
81 | void getWarpMatrixAffine(
82 | const vk::AbstractCamera& cam,
83 | const Vector2d& px_ref,
84 | const Vector3d& f_ref,
85 | const double depth_ref,
86 | const SE3& T_cur_ref,
87 | const int level_ref, // px_ref对应特征点的金字塔层级
88 | const int pyramid_level,
89 | const int halfpatch_size,
90 | Matrix2d& A_cur_ref);
91 | bool align2D(
92 | const cv::Mat& cur_img,
93 | float* ref_patch_with_border,
94 | float* ref_patch,
95 | const int n_iter,
96 | Vector2d& cur_px_estimate,
97 | int index);
98 | void AddPoint(PointPtr pt_new);
99 | int getBestSearchLevel(const Matrix2d& A_cur_ref, const int max_level);
100 | void display_keypatch(double time);
101 | void updateFrameState(StatesGroup state);
102 | V3F getpixel(cv::Mat img, V2D pc);
103 |
104 | void warpAffine(
105 | const Matrix2d& A_cur_ref,
106 | const cv::Mat& img_ref,
107 | const Vector2d& px_ref,
108 | const int level_ref,
109 | const int search_level,
110 | const int pyramid_level,
111 | const int halfpatch_size,
112 | float* patch);
113 |
114 | PointCloudXYZI::Ptr Map_points;
115 | PointCloudXYZI::Ptr Map_points_output;
116 | PointCloudXYZI::Ptr pg_down;
117 | pcl::VoxelGrid downSizeFilter;
118 | unordered_map feat_map;
119 | unordered_map sub_feat_map; //timestamp
120 | unordered_map Warp_map; // reference frame id, A_cur_ref and search_level
121 |
122 | vector occupy_postions;
123 | set sub_postion;
124 | vector voxel_points_;
125 | vector add_voxel_points_;
126 |
127 |
128 | cv::Mat img_cp, img_rgb;
129 | std::vector overlap_kfs_;
130 | FramePtr new_frame_;
131 | FramePtr last_kf_;
132 | Map map_;
133 | enum Stage {
134 | STAGE_FIRST_FRAME,
135 | STAGE_DEFAULT_FRAME
136 | };
137 | Stage stage_;
138 | enum CellType {
139 | TYPE_MAP = 1,
140 | TYPE_POINTCLOUD,
141 | TYPE_UNKNOWN
142 | };
143 |
144 | private:
145 | struct Candidate
146 | {
147 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW
148 | PointPtr pt;
149 | Vector2d px;
150 | Candidate(PointPtr pt, Vector2d& px) : pt(pt), px(px) {}
151 | };
152 | typedef std::list Cell;
153 | typedef std::vector CandidateGrid;
154 |
155 | /// The grid stores a set of candidate matches. For every grid cell we try to find one match.
156 | struct Grid
157 | {
158 | CandidateGrid cells;
159 | vector cell_order;
160 | int cell_size;
161 | int grid_n_cols;
162 | int grid_n_rows;
163 | int cell_length;
164 | };
165 |
166 | Grid grid_;
167 | };
168 | typedef boost::shared_ptr LidarSelectorPtr;
169 |
170 | } // namespace lidar_detection
171 |
172 | #endif // LIDAR_SELECTION_H_
--------------------------------------------------------------------------------
/include/map.h:
--------------------------------------------------------------------------------
1 | // This file is part of SVO - Semi-direct Visual Odometry.
2 | //
3 | // Copyright (C) 2014 Christian Forster
4 | // (Robotics and Perception Group, University of Zurich, Switzerland).
5 | //
6 | // SVO is free software: you can redistribute it and/or modify it under the
7 | // terms of the GNU General Public License as published by the Free Software
8 | // Foundation, either version 3 of the License, or any later version.
9 | //
10 | // SVO is distributed in the hope that it will be useful, but WITHOUT ANY
11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
13 | //
14 | // You should have received a copy of the GNU General Public License
15 | // along with this program. If not, see .
16 |
17 | #ifndef SVO_MAP_H_
18 | #define SVO_MAP_H_
19 |
20 | #include
21 | #include
22 | #include
23 | #include
24 | #include
25 |
26 | namespace lidar_selection {
27 |
28 | class Seed;
29 |
30 | /// Container for converged 3D points that are not already assigned to two keyframes.
31 | class MapPointCandidates
32 | {
33 | public:
34 | typedef pair PointCandidate;
35 | typedef list PointCandidateList;
36 |
37 | /// The depth-filter is running in a parallel thread and fills the canidate list.
38 | /// This mutex controls concurrent access to point_candidates.
39 | boost::mutex mut_;
40 |
41 | /// Candidate points are created from converged seeds.
42 | /// Until the next keyframe, these points can be used for reprojection and pose optimization.
43 | PointCandidateList candidates_;
44 | list< PointPtr > trash_points_;
45 |
46 | MapPointCandidates();
47 | ~MapPointCandidates();
48 |
49 | PointCandidateList GetAllMapPoints() const{ return candidates_;}
50 |
51 | /// Add a candidate point.
52 | void newCandidatePoint(PointPtr point, double depth_sigma2);
53 |
54 | /// Adds the feature to the frame and deletes candidate from list.
55 | void addCandidatePointToFrame(FramePtr frame);
56 |
57 | /// Remove a candidate point from the list of candidates.
58 | bool deleteCandidatePoint(PointPtr point);
59 |
60 | /// Remove all candidates that belong to a frame.
61 | void removeFrameCandidates(FramePtr frame);
62 |
63 | /// Reset the candidate list, remove and delete all points.
64 | void reset();
65 |
66 | void deleteCandidate(PointCandidate& c);
67 |
68 | void emptyTrash();
69 | };
70 |
71 | /// Map object which saves all keyframes which are in a map.
72 | class Map : boost::noncopyable
73 | {
74 | public:
75 | list< FramePtr > keyframes_; //!< List of keyframes in the map.
76 | deque< PointPtr > map_points_;
77 | deque< float > values; //!< the score of every Point in cur frame.
78 | list< PointPtr > trash_points_; //!< A deleted point is moved to the trash bin. Now and then this is cleaned. One reason is that the visualizer must remove the points also.
79 | MapPointCandidates point_candidates_;
80 | int MaxKFid;
81 |
82 |
83 | Map();
84 | ~Map();
85 |
86 | list< FramePtr > getAllKeyframe() const{ return keyframes_; }
87 |
88 | void delete_points(int size);
89 | void clear();
90 |
91 |
92 | void addPoint(PointPtr point);
93 |
94 | /// Reset the map. Delete all keyframes and reset the frame and point counters.
95 | void reset();
96 |
97 | /// Delete a point in the map and remove all references in keyframes to it.
98 | void safeDeletePoint(PointPtr pt);
99 |
100 | /// Moves the point to the trash queue which is cleaned now and then.
101 | void deletePoint(PointPtr pt);
102 |
103 | /// Moves the frame to the trash queue which is cleaned now and then.
104 | bool safeDeleteFrame(FramePtr frame);
105 |
106 | /// Remove the references between a point and a frame.
107 | void removePtFrameRef(Frame* frame, FeaturePtr ftr);
108 |
109 | /// Add a new keyframe to the map.
110 | void addKeyframe(FramePtr new_keyframe);
111 |
112 | /// Given a frame, return all keyframes which have an overlapping field of view.
113 | void getCloseKeyframes(const FramePtr& frame, list< pair >& close_kfs) const;
114 |
115 | /// Return the keyframe which is spatially closest and has overlapping field of view.
116 | FramePtr getClosestKeyframe(const FramePtr& frame) const;
117 |
118 | /// Return the keyframe which is furthest apart from pos.
119 | FramePtr getFurthestKeyframe(const Vector3d& pos) const;
120 |
121 | bool getKeyframeById(const int id, FramePtr& frame) const;
122 |
123 | /// Transform the whole map with rotation R, translation t and scale s.
124 | void transform(const Matrix3d& R, const Vector3d& t, const double& s);
125 |
126 | /// Empty trash bin of deleted keyframes and map points. We don't delete the
127 | /// points immediately to ensure proper cleanup and to provide the visualizer
128 | /// a list of objects which must be removed.
129 | void emptyTrash();
130 |
131 | /// Return the keyframe which was last inserted in the map.
132 | inline FramePtr lastKeyframe() { return keyframes_.back(); }
133 |
134 | /// Return the number of keyframes in the map
135 | inline size_t size() const { return keyframes_.size(); }
136 | };
137 |
138 | /// A collection of debug functions to check the data consistency.
139 | namespace map_debug {
140 |
141 | void mapStatistics(Map* map);
142 | void mapValidation(Map* map, int id);
143 | void frameValidation(Frame* frame, int id);
144 | void pointValidation(PointPtr point, int id);
145 |
146 | } // namespace map_debug
147 | } // namespace lidar_selection
148 |
149 | #endif // SVO_MAP_H_
150 |
--------------------------------------------------------------------------------
/include/point.h:
--------------------------------------------------------------------------------
1 | // This file is part of SVO - Semi-direct Visual Odometry.
2 | //
3 | // Copyright (C) 2014 Christian Forster
4 | // (Robotics and Perception Group, University of Zurich, Switzerland).
5 | //
6 | // SVO is free software: you can redistribute it and/or modify it under the
7 | // terms of the GNU General Public License as published by the Free Software
8 | // Foundation, either version 3 of the License, or any later version.
9 | //
10 | // SVO is distributed in the hope that it will be useful, but WITHOUT ANY
11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
13 | //
14 | // You should have received a copy of the GNU General Public License
15 | // along with this program. If not, see .
16 |
17 | #ifndef SVO_POINT_H_
18 | #define SVO_POINT_H_
19 |
20 | #include
21 | #include
22 | #include
23 | #include
24 |
25 | namespace lidar_selection {
26 |
27 | typedef Matrix Matrix23d;
28 |
29 | /// A 3D point on the surface of the scene.
30 | class Point : boost::noncopyable
31 | {
32 | public:
33 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW
34 |
35 | // enum PointType {
36 | // TYPE_DELETED,
37 | // TYPE_CANDIDATE,
38 | // TYPE_UNKNOWN,
39 | // TYPE_GOOD
40 | // };
41 |
42 | static int point_counter_; //!< Counts the number of created points. Used to set the unique id.
43 | int id_; //!< Unique ID of the point.
44 | Vector3d pos_; //!< 3d pos of the point in the world coordinate frame.
45 | float x, y, z, intensity;
46 | Vector3d normal_; //!< Surface normal at point.
47 | Matrix3d normal_information_; //!< Inverse covariance matrix of normal estimation.
48 | bool normal_set_; //!< Flag whether the surface normal was estimated or not.
49 | list obs_; //!< References to keyframes which observe the point.
50 | size_t n_obs_; //!< Number of observations: Keyframes AND successful reprojections in intermediate frames.
51 | int last_published_ts_; //!< Timestamp of last publishing.
52 | int last_projected_kf_id_; //!< Flag for the reprojection: don't reproject a pt twice.
53 | // PointType type_; //!< Quality of the point.
54 | int n_failed_reproj_; //!< Number of failed reprojections. Used to assess the quality of the point.
55 | int n_succeeded_reproj_; //!< Number of succeeded reprojections. Used to assess the quality of the point.
56 | int last_structure_optim_; //!< Timestamp of last point optimization
57 | bool have_scaled;
58 | float value;
59 | Point(const Vector3d& pos);
60 | Point(const Vector3d& pos, FeaturePtr ftr);
61 | ~Point();
62 |
63 | void getFurthestViewObs(const Vector3d& framepos, FeaturePtr& ftr) const;
64 | void deleteFeatureRef(FeaturePtr ftr);
65 |
66 | /// Add a reference to a frame.
67 | void addFrameRef(FeaturePtr ftr);
68 |
69 | /// Remove reference to a frame.
70 | bool deleteFrameRef(Frame* frame);
71 |
72 | /// Initialize point normal. The inital estimate will point towards the frame.
73 | void initNormal();
74 |
75 | /// Check whether mappoint has reference to a frame.
76 | FeaturePtr findFrameRef(Frame* frame);
77 |
78 | bool getClosePose(const FramePtr& new_frame, FeaturePtr& ftr) const;
79 |
80 | /// Get Frame with similar viewpoint.
81 | bool getCloseViewObs(const Vector3d& pos, FeaturePtr& obs, const Vector2d& cur_px) const;
82 |
83 | bool getCloseViewObs_test(const Vector3d& pos, FeaturePtr& obs, const Vector2d& cur_px, double& min_cos_angle) const;
84 |
85 | /// Get number of observations.
86 | inline size_t nRefs() const { return obs_.size(); }
87 |
88 | /// Optimize point position through minimizing the reprojection error.
89 | void optimize(const size_t n_iter);
90 |
91 | /// Jacobian of point projection on unit plane (focal length = 1) in frame (f).
92 | inline static void jacobian_xyz2uv(
93 | const Vector3d& p_in_f,
94 | const Matrix3d& R_f_w,
95 | Matrix23d& point_jac)
96 | {
97 | const double z_inv = 1.0/p_in_f[2];
98 | const double z_inv_sq = z_inv*z_inv;
99 | point_jac(0, 0) = z_inv;
100 | point_jac(0, 1) = 0.0;
101 | point_jac(0, 2) = -p_in_f[0] * z_inv_sq;
102 | point_jac(1, 0) = 0.0;
103 | point_jac(1, 1) = z_inv;
104 | point_jac(1, 2) = -p_in_f[1] * z_inv_sq;
105 | point_jac = - point_jac * R_f_w;
106 | }
107 | };
108 |
109 | } // namespace lidar_selection
110 |
111 | #endif // SVO_POINT_H_
112 |
--------------------------------------------------------------------------------
/include/preprocess.h:
--------------------------------------------------------------------------------
1 | #include
2 | #include
3 | #include
4 | #include
5 |
6 | using namespace std;
7 |
8 | #define IS_VALID(a) ((abs(a)>1e8) ? true : false)
9 |
10 | typedef pcl::PointXYZINormal PointType;
11 | typedef pcl::PointCloud PointCloudXYZI;
12 |
13 | enum LID_TYPE{AVIA = 1, VELO16, OUST64}; //{1, 2, 3}
14 | enum Feature{Nor, Poss_Plane, Real_Plane, Edge_Jump, Edge_Plane, Wire, ZeroPoint};
15 | enum Surround{Prev, Next};
16 | enum E_jump{Nr_nor, Nr_zero, Nr_180, Nr_inf, Nr_blind};
17 |
18 | struct orgtype
19 | {
20 | double range;
21 | double dista;
22 | double angle[2];
23 | double intersect;
24 | E_jump edj[2];
25 | Feature ftype;
26 | orgtype()
27 | {
28 | range = 0;
29 | edj[Prev] = Nr_nor;
30 | edj[Next] = Nr_nor;
31 | ftype = Nor;
32 | intersect = 2;
33 | }
34 | };
35 |
36 | namespace velodyne_ros {
37 | struct EIGEN_ALIGN16 Point {
38 | PCL_ADD_POINT4D;
39 | float intensity;
40 | uint16_t ring;
41 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW
42 | };
43 | } // namespace velodyne_ros
44 | POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point,
45 | (float, x, x)
46 | (float, y, y)
47 | (float, z, z)
48 | (float, intensity, intensity)
49 | (uint16_t, ring, ring)
50 | )
51 |
52 | namespace ouster_ros {
53 | struct EIGEN_ALIGN16 Point {
54 | PCL_ADD_POINT4D;
55 | float intensity;
56 | uint32_t t;
57 | uint16_t reflectivity;
58 | uint8_t ring;
59 | uint16_t ambient;
60 | uint32_t range;
61 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW
62 | };
63 | } // namespace ouster_ros
64 |
65 | // clang-format off
66 | POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point,
67 | (float, x, x)
68 | (float, y, y)
69 | (float, z, z)
70 | (float, intensity, intensity)
71 | // use std::uint32_t to avoid conflicting with pcl::uint32_t
72 | (std::uint32_t, t, t)
73 | (std::uint16_t, reflectivity, reflectivity)
74 | (std::uint8_t, ring, ring)
75 | (std::uint16_t, ambient, ambient)
76 | (std::uint32_t, range, range)
77 | )
78 |
79 | class Preprocess
80 | {
81 | public:
82 | // EIGEN_MAKE_ALIGNED_OPERATOR_NEW
83 |
84 | Preprocess();
85 | ~Preprocess();
86 |
87 | void process(const livox_ros_driver2::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out);
88 | void process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out);
89 | void set(bool feat_en, int lid_type, double bld, int pfilt_num);
90 |
91 | // sensor_msgs::PointCloud2::ConstPtr pointcloud;
92 | PointCloudXYZI pl_full, pl_corn, pl_surf;
93 | PointCloudXYZI pl_buff[128]; //maximum 128 line lidar
94 | vector typess[128]; //maximum 128 line lidar
95 | int lidar_type, point_filter_num, N_SCANS;;
96 | double blind;
97 | bool feature_enabled;
98 | ros::Publisher pub_full, pub_surf, pub_corn;
99 |
100 |
101 | private:
102 | void avia_handler(const livox_ros_driver2::CustomMsg::ConstPtr &msg);
103 | void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
104 | void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
105 | void give_feature(PointCloudXYZI &pl, vector &types);
106 | void pub_func(PointCloudXYZI &pl, const ros::Time &ct);
107 | int plane_judge(const PointCloudXYZI &pl, vector &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct);
108 | bool small_plane(const PointCloudXYZI &pl, vector &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct);
109 | bool edge_jump_judge(const PointCloudXYZI &pl, vector &types, uint i, Surround nor_dir);
110 |
111 | int group_size;
112 | double disA, disB, inf_bound;
113 | double limit_maxmid, limit_midmin, limit_maxmin;
114 | double p2l_ratio;
115 | double jump_up_limit, jump_down_limit;
116 | double cos160;
117 | double edgea, edgeb;
118 | double smallp_intersect, smallp_ratio;
119 | double vx, vy, vz;
120 | };
--------------------------------------------------------------------------------
/include/so3_math.h:
--------------------------------------------------------------------------------
1 | #ifndef SO3_MATH_H
2 | #define SO3_MATH_H
3 |
4 | #include
5 | #include
6 |
7 | // #include
8 |
9 | #define SKEW_SYM_MATRX(v) 0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0
10 |
11 | template
12 | Eigen::Matrix Exp(const Eigen::Matrix &&ang)
13 | {
14 | T ang_norm = ang.norm();
15 | Eigen::Matrix Eye3 = Eigen::Matrix::Identity();
16 | if (ang_norm > 0.0000001)
17 | {
18 | Eigen::Matrix r_axis = ang / ang_norm;
19 | Eigen::Matrix K;
20 | K << SKEW_SYM_MATRX(r_axis);
21 | /// Roderigous Tranformation
22 | return Eye3 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K;
23 | }
24 | else
25 | {
26 | return Eye3;
27 | }
28 | }
29 |
30 | template
31 | Eigen::Matrix Exp(const Eigen::Matrix &ang_vel, const Ts &dt)
32 | {
33 | T ang_vel_norm = ang_vel.norm();
34 | Eigen::Matrix Eye3 = Eigen::Matrix::Identity();
35 |
36 | if (ang_vel_norm > 0.0000001)
37 | {
38 | Eigen::Matrix r_axis = ang_vel / ang_vel_norm;
39 | Eigen::Matrix K;
40 |
41 | K << SKEW_SYM_MATRX(r_axis);
42 |
43 | T r_ang = ang_vel_norm * dt;
44 |
45 | /// Roderigous Tranformation
46 | return Eye3 + std::sin(r_ang) * K + (1.0 - std::cos(r_ang)) * K * K;
47 | }
48 | else
49 | {
50 | return Eye3;
51 | }
52 | }
53 |
54 | template
55 | Eigen::Matrix Exp(const T &v1, const T &v2, const T &v3)
56 | {
57 | T &&norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3);
58 | Eigen::Matrix Eye3 = Eigen::Matrix::Identity();
59 | if (norm > 0.00001)
60 | {
61 | T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm};
62 | Eigen::Matrix K;
63 | K << SKEW_SYM_MATRX(r_ang);
64 |
65 | /// Roderigous Tranformation
66 | return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K;
67 | }
68 | else
69 | {
70 | return Eye3;
71 | }
72 | }
73 |
74 | /* Logrithm of a Rotation Matrix */
75 | template
76 | Eigen::Matrix Log(const Eigen::Matrix &R)
77 | {
78 | T theta = (R.trace() > 3.0 - 1e-6) ? 0.0 : std::acos(0.5 * (R.trace() - 1));
79 | Eigen::Matrix K(R(2,1) - R(1,2), R(0,2) - R(2,0), R(1,0) - R(0,1));
80 | return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K);
81 | }
82 |
83 | template
84 | Eigen::Matrix RotMtoEuler(const Eigen::Matrix &rot)
85 | {
86 | T sy = sqrt(rot(0,0)*rot(0,0) + rot(1,0)*rot(1,0));
87 | bool singular = sy < 1e-6;
88 | T x, y, z;
89 | if(!singular)
90 | {
91 | x = atan2(rot(2, 1), rot(2, 2));
92 | y = atan2(-rot(2, 0), sy);
93 | z = atan2(rot(1, 0), rot(0, 0));
94 | }
95 | else
96 | {
97 | x = atan2(-rot(1, 2), rot(1, 1));
98 | y = atan2(-rot(2, 0), sy);
99 | z = 0;
100 | }
101 | Eigen::Matrix ang(x, y, z);
102 | return ang;
103 | }
104 |
105 | #endif
106 |
--------------------------------------------------------------------------------
/include/use-ikfom.hpp:
--------------------------------------------------------------------------------
1 | #ifndef USE_IKFOM_H
2 | #define USE_IKFOM_H
3 |
4 | #include
5 |
6 | typedef MTK::vect<3, double> vect3;
7 | typedef MTK::SO3 SO3;
8 | typedef MTK::S2 S2;
9 | typedef MTK::vect<1, double> vect1;
10 | typedef MTK::vect<2, double> vect2;
11 |
12 | MTK_BUILD_MANIFOLD(state_ikfom,
13 | ((vect3, pos))
14 | ((SO3, rot))
15 | ((SO3, offset_R_L_I))
16 | ((vect3, offset_T_L_I))
17 | ((vect3, vel))
18 | ((vect3, bg))
19 | ((vect3, ba))
20 | ((S2, grav))
21 | );
22 |
23 | MTK_BUILD_MANIFOLD(input_ikfom,
24 | ((vect3, acc))
25 | ((vect3, gyro))
26 | );
27 |
28 | MTK_BUILD_MANIFOLD(process_noise_ikfom,
29 | ((vect3, ng))
30 | ((vect3, na))
31 | ((vect3, nbg))
32 | ((vect3, nba))
33 | );
34 |
35 | MTK::get_cov::type process_noise_cov()
36 | {
37 | MTK::get_cov::type cov = MTK::get_cov::type::Zero();
38 | MTK::setDiagonal(cov, &process_noise_ikfom::ng, 0.0001);// 0.03
39 | MTK::setDiagonal(cov, &process_noise_ikfom::na, 0.0001); // *dt 0.01 0.01 * dt * dt 0.05
40 | MTK::setDiagonal(cov, &process_noise_ikfom::nbg, 0.00001); // *dt 0.00001 0.00001 * dt *dt 0.3 //0.001 0.0001 0.01
41 | MTK::setDiagonal(cov, &process_noise_ikfom::nba, 0.00001); //0.001 0.05 0.0001/out 0.01
42 | return cov;
43 | }
44 |
45 | //double L_offset_to_I[3] = {0.04165, 0.02326, -0.0284}; // Avia
46 | //vect3 Lidar_offset_to_IMU(L_offset_to_I, 3);
47 | Eigen::Matrix get_f(state_ikfom &s, const input_ikfom &in)
48 | {
49 | Eigen::Matrix res = Eigen::Matrix::Zero();
50 | vect3 omega;
51 | in.gyro.boxminus(omega, s.bg);
52 | vect3 a_inertial = s.rot * (in.acc-s.ba);
53 | for(int i = 0; i < 3; i++ ){
54 | res(i) = s.vel[i];
55 | res(i + 3) = omega[i];
56 | res(i + 12) = a_inertial[i] + s.grav[i];
57 | }
58 | return res;
59 | }
60 |
61 | Eigen::Matrix df_dx(state_ikfom &s, const input_ikfom &in)
62 | {
63 | Eigen::Matrix cov = Eigen::Matrix::Zero();
64 | cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity();
65 | vect3 acc_;
66 | in.acc.boxminus(acc_, s.ba);
67 | vect3 omega;
68 | in.gyro.boxminus(omega, s.bg);
69 | cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix()*MTK::hat(acc_);
70 | cov.template block<3, 3>(12, 18) = -s.rot.toRotationMatrix();
71 | Eigen::Matrix vec = Eigen::Matrix::Zero();
72 | Eigen::Matrix grav_matrix;
73 | s.S2_Mx(grav_matrix, vec, 21);
74 | cov.template block<3, 2>(12, 21) = grav_matrix;
75 | cov.template block<3, 3>(3, 15) = -Eigen::Matrix3d::Identity();
76 | return cov;
77 | }
78 |
79 |
80 | Eigen::Matrix df_dw(state_ikfom &s, const input_ikfom &in)
81 | {
82 | Eigen::Matrix cov = Eigen::Matrix::Zero();
83 | cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix();
84 | cov.template block<3, 3>(3, 0) = -Eigen::Matrix3d::Identity();
85 | cov.template block<3, 3>(15, 6) = Eigen::Matrix3d::Identity();
86 | cov.template block<3, 3>(18, 9) = Eigen::Matrix3d::Identity();
87 | return cov;
88 | }
89 |
90 | vect3 SO3ToEuler(const SO3 &orient)
91 | {
92 | Eigen::Matrix _ang;
93 | Eigen::Vector4d q_data = orient.coeffs().transpose();
94 | //scalar w=orient.coeffs[3], x=orient.coeffs[0], y=orient.coeffs[1], z=orient.coeffs[2];
95 | double sqw = q_data[3]*q_data[3];
96 | double sqx = q_data[0]*q_data[0];
97 | double sqy = q_data[1]*q_data[1];
98 | double sqz = q_data[2]*q_data[2];
99 | double unit = sqx + sqy + sqz + sqw; // if normalized is one, otherwise is correction factor
100 | double test = q_data[3]*q_data[1] - q_data[2]*q_data[0];
101 |
102 | if (test > 0.49999*unit) { // singularity at north pole
103 |
104 | _ang << 2 * std::atan2(q_data[0], q_data[3]), M_PI/2, 0;
105 | double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3};
106 | vect3 euler_ang(temp, 3);
107 | return euler_ang;
108 | }
109 | if (test < -0.49999*unit) { // singularity at south pole
110 | _ang << -2 * std::atan2(q_data[0], q_data[3]), -M_PI/2, 0;
111 | double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3};
112 | vect3 euler_ang(temp, 3);
113 | return euler_ang;
114 | }
115 |
116 | _ang <<
117 | std::atan2(2*q_data[0]*q_data[3]+2*q_data[1]*q_data[2] , -sqx - sqy + sqz + sqw),
118 | std::asin (2*test/unit),
119 | std::atan2(2*q_data[2]*q_data[3]+2*q_data[1]*q_data[0] , sqx - sqy - sqz + sqw);
120 | double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3};
121 | vect3 euler_ang(temp, 3);
122 | // euler_ang[0] = roll, euler_ang[1] = pitch, euler_ang[2] = yaw
123 | return euler_ang;
124 | }
125 |
126 | #endif
--------------------------------------------------------------------------------
/launch/mapping_avia.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 |
9 |
10 |
11 |
12 |
13 |
14 |
15 |
16 |
17 |
18 |
19 |
20 |
21 | launch-prefix="gdb -ex run --args" launch-prefix="valgrind --leak-check=full --show-leak-kinds=all"
22 |
23 |
24 |
--------------------------------------------------------------------------------
/launch/mapping_avia_ntu.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 |
9 |
10 |
11 |
12 |
13 |
14 |
15 |
16 | launch-prefix="gdb -ex run --args"
17 |
18 |
--------------------------------------------------------------------------------
/launch/mapping_mid360.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 |
9 |
10 |
11 |
12 |
13 |
14 |
15 |
16 |
17 | launch-prefix="gdb -ex run --args" launch-prefix="valgrind --leak-check=full --show-leak-kinds=all"
18 |
19 |
20 |
--------------------------------------------------------------------------------
/launch/mapping_rviz.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 |
9 |
10 |
11 |
12 |
--------------------------------------------------------------------------------
/msg/Pose6D.msg:
--------------------------------------------------------------------------------
1 | # the preintegrated Lidar states at the time of IMU measurements in a frame
2 | float64 offset_time # the offset time of IMU measurement w.r.t the first lidar point
3 | float64[3] acc # the preintegrated total acceleration (global frame) at the Lidar origin
4 | float64[3] gyr # the unbiased angular velocity (body frame) at the Lidar origin
5 | float64[3] vel # the preintegrated velocity (global frame) at the Lidar origin
6 | float64[3] pos # the preintegrated position (global frame) at the Lidar origin
7 | float64[9] rot # the preintegrated rotation (global frame) at the Lidar origin
--------------------------------------------------------------------------------
/msg/States.msg:
--------------------------------------------------------------------------------
1 | Header header # timestamp of the first lidar in a frame
2 | float64[] rot_end # the estimated attitude (rotation matrix) at the end lidar point
3 | float64[] pos_end # the estimated position at the end lidar point (world frame)
4 | float64[] vel_end # the estimated velocity at the end lidar point (world frame)
5 | float64[] bias_gyr # gyroscope bias
6 | float64[] bias_acc # accelerator bias
7 | float64[] gravity # the estimated gravity acceleration
8 | float64[] cov # states covariance
9 | # Pose6D[] IMUpose # 6D pose at each imu measurements
--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 | fast_livo
4 | 0.0.0
5 |
6 |
7 | This is a modified version of FAST-LIO.
8 |
9 |
10 | claydergc
11 |
12 | BSD
13 |
14 | Chunran Zheng
15 |
16 | catkin
17 | geometry_msgs
18 | nav_msgs
19 | roscpp
20 | rospy
21 | std_msgs
22 | sensor_msgs
23 | tf
24 | pcl_ros
25 | livox_ros_driver2
26 | message_generation
27 | vikit_common
28 | vikit_ros
29 | cv_bridge
30 |
31 | geometry_msgs
32 | nav_msgs
33 | sensor_msgs
34 | roscpp
35 | rospy
36 | std_msgs
37 | tf
38 | pcl_ros
39 | livox_ros_driver2
40 | message_runtime
41 | vikit_common
42 | vikit_ros
43 | cv_bridge
44 | image_transport
45 | image_transport
46 | rostest
47 | rosbag
48 |
49 |
50 |
51 |
52 |
--------------------------------------------------------------------------------
/src/frame.cpp:
--------------------------------------------------------------------------------
1 | // This file is part of SVO - Semi-direct Visual Odometry.
2 | //
3 | // Copyright (C) 2014 Christian Forster
4 | // (Robotics and Perception Group, University of Zurich, Switzerland).
5 | //
6 | // SVO is free software: you can redistribute it and/or modify it under the
7 | // terms of the GNU General Public License as published by the Free Software
8 | // Foundation, either version 3 of the License, or any later version.
9 | //
10 | // SVO is distributed in the hope that it will be useful, but WITHOUT ANY
11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
13 | //
14 | // You should have received a copy of the GNU General Public License
15 | // along with this program. If not, see .
16 |
17 | #include
18 | #include
19 | #include
20 | #include
21 | #include
22 | #include
23 | #include
24 | #include
25 | // #include
26 |
27 | namespace lidar_selection {
28 |
29 | int Frame::frame_counter_ = 0;
30 |
31 | Frame::Frame(vk::AbstractCamera* cam, const cv::Mat& img) :
32 | id_(frame_counter_++),
33 | cam_(cam),
34 | key_pts_(5),
35 | is_keyframe_(false)
36 | {
37 | initFrame(img);
38 | }
39 |
40 | Frame::~Frame()
41 | {
42 | std::for_each(fts_.begin(), fts_.end(), [&](FeaturePtr i){i.reset();});
43 | }
44 |
45 | void Frame::initFrame(const cv::Mat& img)
46 | {
47 | // check image
48 | if(img.empty() || img.type() != CV_8UC1 || img.cols != cam_->width() || img.rows != cam_->height())
49 | throw std::runtime_error("Frame: provided image has not the same size as the camera model or image is not grayscale");
50 |
51 | // Set keypoints to nullptr
52 | std::for_each(key_pts_.begin(), key_pts_.end(), [&](FeaturePtr ftr){ ftr=nullptr; });
53 |
54 | ImgPyr ().swap(img_pyr_);
55 | img_pyr_.push_back(img);
56 | // Build Image Pyramid
57 | // frame_utils::createImgPyramid(img, max(Config::nPyrLevels(), Config::kltMaxLevel()+1), img_pyr_);
58 | // frame_utils::createImgPyramid(img, 5, img_pyr_);
59 | }
60 |
61 | void Frame::setKeyframe()
62 | {
63 | is_keyframe_ = true;
64 | setKeyPoints();
65 | }
66 |
67 | void Frame::addFeature(FeaturePtr ftr)
68 | {
69 | fts_.push_back(ftr);
70 | }
71 |
72 | void Frame::setKeyPoints()
73 | {
74 | for(size_t i = 0; i < 5; ++i)
75 | if(key_pts_[i] != nullptr)
76 | if(key_pts_[i]->point == nullptr)
77 | key_pts_[i] = nullptr;
78 | std::for_each(fts_.begin(), fts_.end(), [&](FeaturePtr ftr){ if(ftr->point != nullptr) checkKeyPoints(ftr); });
79 | }
80 |
81 | void Frame::checkKeyPoints(FeaturePtr ftr)
82 | {
83 | const int cu = cam_->width()/2;
84 | const int cv = cam_->height()/2;
85 |
86 | // center pixel
87 | if(key_pts_[0] == nullptr)
88 | key_pts_[0] = ftr;
89 |
90 | else if(std::max(std::fabs(ftr->px[0]-cu), std::fabs(ftr->px[1]-cv))
91 | < std::max(std::fabs(key_pts_[0]->px[0]-cu), std::fabs(key_pts_[0]->px[1]-cv)))
92 | key_pts_[0] = ftr;
93 |
94 | if(ftr->px[0] >= cu && ftr->px[1] >= cv)
95 | {
96 | if(key_pts_[1] == nullptr)
97 | key_pts_[1] = ftr;
98 | else if((ftr->px[0]-cu) * (ftr->px[1]-cv)
99 | > (key_pts_[1]->px[0]-cu) * (key_pts_[1]->px[1]-cv))
100 | key_pts_[1] = ftr;
101 | }
102 |
103 | if(ftr->px[0] >= cu && ftr->px[1] < cv)
104 | {
105 | if(key_pts_[2] == nullptr)
106 | key_pts_[2] = ftr;
107 | // else if((ftr->px[0]-cu) * (ftr->px[1]-cv)
108 | else if((ftr->px[0]-cu) * (cv-ftr->px[1])
109 | // > (key_pts_[2]->px[0]-cu) * (key_pts_[2]->px[1]-cv))
110 | > (key_pts_[2]->px[0]-cu) * (cv-key_pts_[2]->px[1]))
111 | key_pts_[2] = ftr;
112 | }
113 |
114 | if(ftr->px[0] < cu && ftr->px[1] < cv)
115 | {
116 | if(key_pts_[3] == nullptr)
117 | key_pts_[3] = ftr;
118 | else if((ftr->px[0]-cu) * (ftr->px[1]-cv)
119 | > (key_pts_[3]->px[0]-cu) * (key_pts_[3]->px[1]-cv))
120 | key_pts_[3] = ftr;
121 | }
122 |
123 | if(ftr->px[0] < cu && ftr->px[1] >= cv)
124 | // if(ftr->px[0] < cv && ftr->px[1] >= cv)
125 | {
126 | if(key_pts_[4] == nullptr)
127 | key_pts_[4] = ftr;
128 |
129 | else if(cu-(ftr->px[0]) * (ftr->px[1]-cv)
130 | > (cu-key_pts_[4]->px[0]) * (key_pts_[4]->px[1]-cv))
131 | key_pts_[4] = ftr;
132 | }
133 | }
134 |
135 | void Frame::removeKeyPoint(FeaturePtr ftr)
136 | {
137 | bool found = false;
138 | std::for_each(key_pts_.begin(), key_pts_.end(), [&](FeaturePtr& i){
139 | if(i == ftr) {
140 | i = nullptr;
141 | found = true;
142 | }
143 | });
144 | if(found)
145 | setKeyPoints();
146 | }
147 |
148 | bool Frame::isVisible(const Vector3d& xyz_w) const
149 | {
150 | Vector3d xyz_f = T_f_w_*xyz_w;
151 |
152 | if(xyz_f.z() < 0.0)
153 | return false; // point is behind the camera
154 | Vector2d px = f2c(xyz_f);
155 |
156 | if(px[0] >= 0.0 && px[1] >= 0.0 && px[0] < cam_->width() && px[1] < cam_->height())
157 | return true;
158 | return false;
159 | }
160 |
161 | /// Utility functions for the Frame class
162 | namespace frame_utils {
163 |
164 | void createImgPyramid(const cv::Mat& img_level_0, int n_levels, ImgPyr& pyr)
165 | {
166 | pyr.resize(n_levels);
167 | pyr[0] = img_level_0;
168 |
169 | for(int i=1; i depth_vec;
180 | depth_vec.reserve(frame.fts_.size());
181 | depth_min = std::numeric_limits