├── .gitignore ├── BrakeModule_sw03 ├── .gitkeep ├── .vscode │ ├── arduino.json │ └── c_cpp_properties.json ├── ADC_DMA.h ├── BrakeModule_sw03.ino ├── STM32F1_CAN.h └── build_opt.h ├── Pics ├── 20220817_162326.jpg ├── ABSunit.PNG ├── BLS_logik.PNG ├── BM.jpg ├── BM0.2.PNG ├── BM03.PNG ├── BMMain.png ├── Bosch57cars.PNG ├── Brake light switch.JPG ├── Bus Topology I -K-M-P-Can-Diagnostic.jpg ├── Charge_pump_operation.png ├── DSCIII.PNG ├── DSC_VOLT.png ├── PCB_032.PNG ├── Pump.PNG ├── PumpFETs.PNG ├── PumpSch.PNG └── Readme.md ├── README.md ├── Safety ├── README.md └── misra │ ├── __pycache__ │ └── cppcheckdata.cpython-38.pyc │ ├── code_tests.sh │ ├── cppcheckdata.py │ ├── misra.json │ ├── misra.py │ ├── misra_rules.txt │ ├── misra_violations_output.txt │ └── suppressions.txt └── dsc_system.pdf /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | -------------------------------------------------------------------------------- /BrakeModule_sw03/.gitkeep: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /BrakeModule_sw03/.vscode/arduino.json: -------------------------------------------------------------------------------- 1 | { 2 | "configuration": "pnum=BLUEPILL_F103C8,upload_method=serialMethod,xserial=generic,usb=none,xusb=FS,opt=osstd,dbg=none,rtlib=nano", 3 | "board": "STMicroelectronics:stm32:GenF1", 4 | "sketch": "BrakeModule_sw03.ino", 5 | "port": "/dev/ttyUSB0" 6 | } -------------------------------------------------------------------------------- /BrakeModule_sw03/.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "version": 4, 3 | "configurations": [ 4 | { 5 | "name": "Arduino", 6 | "compilerPath": "/home/goran/.arduino15/packages/STMicroelectronics/tools/xpack-arm-none-eabi-gcc/10.3.1-2.3/bin/arm-none-eabi-g++", 7 | "compilerArgs": [ 8 | "-mcpu=cortex-m3", 9 | "-mthumb", 10 | "-Wall", 11 | "-Wextra", 12 | "-std=gnu++14", 13 | "-ffunction-sections", 14 | "-fdata-sections", 15 | "-nostdlib", 16 | "-fno-threadsafe-statics", 17 | "--param", 18 | "-fno-rtti", 19 | "-fno-exceptions", 20 | "-fno-use-cxa-atexit" 21 | ], 22 | "intelliSenseMode": "gcc-x64", 23 | "includePath": [ 24 | "/home/goran/git/BrakeModule/BrakeModule_sw03", 25 | "/home/goran/.arduino15/packages/STMicroelectronics/hardware/stm32/2.2.0/cores/arduino/avr", 26 | "/home/goran/.arduino15/packages/STMicroelectronics/hardware/stm32/2.2.0/cores/arduino/stm32", 27 | "/home/goran/.arduino15/packages/STMicroelectronics/hardware/stm32/2.2.0/cores/arduino/stm32/LL", 28 | "/home/goran/.arduino15/packages/STMicroelectronics/hardware/stm32/2.2.0/cores/arduino/stm32/usb", 29 | "/home/goran/.arduino15/packages/STMicroelectronics/hardware/stm32/2.2.0/cores/arduino/stm32/OpenAMP", 30 | "/home/goran/.arduino15/packages/STMicroelectronics/hardware/stm32/2.2.0/cores/arduino/stm32/usb/hid", 31 | "/home/goran/.arduino15/packages/STMicroelectronics/hardware/stm32/2.2.0/cores/arduino/stm32/usb/cdc", 32 | "/home/goran/.arduino15/packages/STMicroelectronics/hardware/stm32/2.2.0/system/Drivers/STM32F1xx_HAL_Driver/Inc", 33 | "/home/goran/.arduino15/packages/STMicroelectronics/hardware/stm32/2.2.0/system/Drivers/STM32F1xx_HAL_Driver/Src", 34 | "/home/goran/.arduino15/packages/STMicroelectronics/hardware/stm32/2.2.0/system/STM32F1xx", 35 | "/home/goran/.arduino15/packages/STMicroelectronics/hardware/stm32/2.2.0/system/Middlewares/ST/STM32_USB_Device_Library/Core/Inc", 36 | "/home/goran/.arduino15/packages/STMicroelectronics/hardware/stm32/2.2.0/system/Middlewares/ST/STM32_USB_Device_Library/Core/Src", 37 | "/home/goran/.arduino15/packages/STMicroelectronics/hardware/stm32/2.2.0/system/Middlewares/OpenAMP", 38 | "/home/goran/.arduino15/packages/STMicroelectronics/hardware/stm32/2.2.0/system/Middlewares/OpenAMP/open-amp/lib/include", 39 | "/home/goran/.arduino15/packages/STMicroelectronics/hardware/stm32/2.2.0/system/Middlewares/OpenAMP/libmetal/lib/include", 40 | "/home/goran/.arduino15/packages/STMicroelectronics/hardware/stm32/2.2.0/system/Middlewares/OpenAMP/virtual_driver", 41 | "/home/goran/.arduino15/packages/STMicroelectronics/tools/CMSIS/5.7.0/CMSIS/Core/Include/", 42 | "/home/goran/.arduino15/packages/STMicroelectronics/hardware/stm32/2.2.0/system/Drivers/CMSIS/Device/ST/STM32F1xx/Include/", 43 | "/home/goran/.arduino15/packages/STMicroelectronics/hardware/stm32/2.2.0/system/Drivers/CMSIS/Device/ST/STM32F1xx/Source/Templates/gcc/", 44 | "/home/goran/.arduino15/packages/STMicroelectronics/tools/CMSIS/5.7.0/CMSIS/DSP/Include", 45 | "/home/goran/.arduino15/packages/STMicroelectronics/tools/CMSIS/5.7.0/CMSIS/DSP/PrivateInclude", 46 | "/home/goran/.arduino15/packages/STMicroelectronics/hardware/stm32/2.2.0/cores/arduino", 47 | "/home/goran/.arduino15/packages/STMicroelectronics/hardware/stm32/2.2.0/variants/STM32F1xx/F103C8T_F103CB(T-U)", 48 | "/home/goran/.arduino15/packages/STMicroelectronics/hardware/stm32/2.2.0/libraries/IWatchdog/src", 49 | "/home/goran/.arduino15/packages/STMicroelectronics/hardware/stm32/2.2.0/libraries/SrcWrapper/src", 50 | "/home/goran/.arduino15/packages/STMicroelectronics/tools/xpack-arm-none-eabi-gcc/10.3.1-2.3/arm-none-eabi/include/c++/10.3.1", 51 | "/home/goran/.arduino15/packages/STMicroelectronics/tools/xpack-arm-none-eabi-gcc/10.3.1-2.3/arm-none-eabi/include/c++/10.3.1/arm-none-eabi", 52 | "/home/goran/.arduino15/packages/STMicroelectronics/tools/xpack-arm-none-eabi-gcc/10.3.1-2.3/arm-none-eabi/include/c++/10.3.1/backward", 53 | "/home/goran/.arduino15/packages/STMicroelectronics/tools/xpack-arm-none-eabi-gcc/10.3.1-2.3/lib/gcc/arm-none-eabi/10.3.1/include", 54 | "/home/goran/.arduino15/packages/STMicroelectronics/tools/xpack-arm-none-eabi-gcc/10.3.1-2.3/lib/gcc/arm-none-eabi/10.3.1/include-fixed", 55 | "/home/goran/.arduino15/packages/STMicroelectronics/tools/xpack-arm-none-eabi-gcc/10.3.1-2.3/arm-none-eabi/include" 56 | ], 57 | "forcedInclude": [ 58 | "/home/goran/.arduino15/packages/STMicroelectronics/hardware/stm32/2.2.0/cores/arduino/Arduino.h" 59 | ], 60 | "cStandard": "c11", 61 | "cppStandard": "c++14", 62 | "defines": [ 63 | "USE_FULL_LL_DRIVER", 64 | "STM32F1xx", 65 | "ARDUINO=10819", 66 | "ARDUINO_BLUEPILL_F103C8", 67 | "ARDUINO_ARCH_STM32", 68 | "BOARD_NAME=\"BLUEPILL_F103C8\"", 69 | "VARIANT_H=\"variant_PILL_F103Cx.h\"", 70 | "STM32F103xB", 71 | "HAL_UART_MODULE_ENABLED", 72 | "__DBL_MIN_EXP__=(-1021)", 73 | "__HQ_FBIT__=15", 74 | "__cpp_attributes=200809L", 75 | "__UINT_LEAST16_MAX__=0xffff", 76 | "__ARM_SIZEOF_WCHAR_T=4", 77 | "__ATOMIC_ACQUIRE=2", 78 | "__SFRACT_IBIT__=0", 79 | "__FLT_MIN__=1.1754943508222875e-38F", 80 | "__GCC_IEC_559_COMPLEX=0", 81 | "__cpp_aggregate_nsdmi=201304L", 82 | "__UFRACT_MAX__=0XFFFFP-16UR", 83 | "__UINT_LEAST8_TYPE__=unsigned char", 84 | "__DQ_FBIT__=63", 85 | "__INTMAX_C(c)=c ## LL", 86 | "__ULFRACT_FBIT__=32", 87 | "__CHAR_BIT__=8", 88 | "__USQ_IBIT__=0", 89 | "__UINT8_MAX__=0xff", 90 | "__ACCUM_FBIT__=15", 91 | "__WINT_MAX__=0xffffffffU", 92 | "__FLT32_MIN_EXP__=(-125)", 93 | "__cpp_static_assert=200410L", 94 | "__USFRACT_FBIT__=8", 95 | "__ORDER_LITTLE_ENDIAN__=1234", 96 | "__SIZE_MAX__=0xffffffffU", 97 | "__ARM_ARCH_ISA_ARM=1", 98 | "__WCHAR_MAX__=0xffffffffU", 99 | "__LACCUM_IBIT__=32", 100 | "__DBL_DENORM_MIN__=double(4.9406564584124654e-324L)", 101 | "__GCC_ATOMIC_CHAR_LOCK_FREE=1", 102 | "__GCC_IEC_559=0", 103 | "__FLT32X_DECIMAL_DIG__=17", 104 | "__FLT_EVAL_METHOD__=0", 105 | "__TQ_IBIT__=0", 106 | "__cpp_binary_literals=201304L", 107 | "__LLACCUM_MAX__=0X7FFFFFFFFFFFFFFFP-31LLK", 108 | "__FLT64_DECIMAL_DIG__=17", 109 | "__GCC_ATOMIC_CHAR32_T_LOCK_FREE=1", 110 | "__cpp_variadic_templates=200704L", 111 | "__UINT_FAST64_MAX__=0xffffffffffffffffULL", 112 | "__SIG_ATOMIC_TYPE__=int", 113 | "__DBL_MIN_10_EXP__=(-307)", 114 | "__FINITE_MATH_ONLY__=0", 115 | "__ARMEL__=1", 116 | "__cpp_variable_templates=201304L", 117 | "__FLT32X_MAX_EXP__=1024", 118 | "__LFRACT_IBIT__=0", 119 | "__GNUC_PATCHLEVEL__=1", 120 | "__FLT32_HAS_DENORM__=1", 121 | "__LFRACT_MAX__=0X7FFFFFFFP-31LR", 122 | "__USA_FBIT__=16", 123 | "__UINT_FAST8_MAX__=0xffffffffU", 124 | "__cpp_rvalue_reference=200610L", 125 | "__FLT32_MAX_10_EXP__=38", 126 | "__ARM_ARCH_4T__=1", 127 | "__INT8_C(c)=c", 128 | "__INT_LEAST8_WIDTH__=8", 129 | "__UINT_LEAST64_MAX__=0xffffffffffffffffULL", 130 | "__SA_FBIT__=15", 131 | "__SHRT_MAX__=0x7fff", 132 | "__LDBL_MAX__=1.7976931348623157e+308L", 133 | "__FRACT_MAX__=0X7FFFP-15R", 134 | "__UFRACT_FBIT__=16", 135 | "__UFRACT_MIN__=0.0UR", 136 | "__UINT_LEAST8_MAX__=0xff", 137 | "__GCC_ATOMIC_BOOL_LOCK_FREE=1", 138 | "__UINTMAX_TYPE__=long long unsigned int", 139 | "__LLFRACT_EPSILON__=0x1P-63LLR", 140 | "__FLT_EVAL_METHOD_TS_18661_3__=0", 141 | "__CHAR_UNSIGNED__=1", 142 | "__UINT32_MAX__=0xffffffffUL", 143 | "__GXX_EXPERIMENTAL_CXX0X__=1", 144 | "__ULFRACT_MAX__=0XFFFFFFFFP-32ULR", 145 | "__TA_IBIT__=64", 146 | "__LDBL_MAX_EXP__=1024", 147 | "__WINT_MIN__=0U", 148 | "__INT_LEAST16_WIDTH__=16", 149 | "__ULLFRACT_MIN__=0.0ULLR", 150 | "__SCHAR_MAX__=0x7f", 151 | "__WCHAR_MIN__=0U", 152 | "__INT64_C(c)=c ## LL", 153 | "__GCC_ATOMIC_POINTER_LOCK_FREE=1", 154 | "__LLACCUM_MIN__=(-0X1P31LLK-0X1P31LLK)", 155 | "__SIZEOF_INT__=4", 156 | "__FLT32X_MANT_DIG__=53", 157 | "__GCC_ATOMIC_CHAR16_T_LOCK_FREE=1", 158 | "__USACCUM_IBIT__=8", 159 | "__USER_LABEL_PREFIX__", 160 | "__STDC_HOSTED__=1", 161 | "__LFRACT_MIN__=(-0.5LR-0.5LR)", 162 | "__HA_IBIT__=8", 163 | "__cpp_decltype_auto=201304L", 164 | "__DBL_DIG__=15", 165 | "__FLT32_DIG__=6", 166 | "__FLT_EPSILON__=1.1920928955078125e-7F", 167 | "__APCS_32__=1", 168 | "__GXX_WEAK__=1", 169 | "__SHRT_WIDTH__=16", 170 | "__USFRACT_IBIT__=0", 171 | "__LDBL_MIN__=2.2250738585072014e-308L", 172 | "__FRACT_MIN__=(-0.5R-0.5R)", 173 | "__cpp_threadsafe_static_init=200806L", 174 | "__DA_IBIT__=32", 175 | "__ARM_SIZEOF_MINIMAL_ENUM=1", 176 | "__FLT32X_HAS_INFINITY__=1", 177 | "__INT32_MAX__=0x7fffffffL", 178 | "__UQQ_FBIT__=8", 179 | "__INT_WIDTH__=32", 180 | "__SIZEOF_LONG__=4", 181 | "__UACCUM_MAX__=0XFFFFFFFFP-16UK", 182 | "__UINT16_C(c)=c", 183 | "__DECIMAL_DIG__=17", 184 | "__LFRACT_EPSILON__=0x1P-31LR", 185 | "__FLT64_EPSILON__=2.2204460492503131e-16F64", 186 | "__ULFRACT_MIN__=0.0ULR", 187 | "__INT16_MAX__=0x7fff", 188 | "__FLT64_MIN_EXP__=(-1021)", 189 | "__LDBL_HAS_QUIET_NAN__=1", 190 | "__ULACCUM_IBIT__=32", 191 | "__FLT64_MANT_DIG__=53", 192 | "__UACCUM_EPSILON__=0x1P-16UK", 193 | "__GNUC__=10", 194 | "__ULLACCUM_MAX__=0XFFFFFFFFFFFFFFFFP-32ULLK", 195 | "__GXX_RTTI=1", 196 | "__HQ_IBIT__=0", 197 | "__FLT_HAS_DENORM__=1", 198 | "__SIZEOF_LONG_DOUBLE__=8", 199 | "__SA_IBIT__=16", 200 | "__BIGGEST_ALIGNMENT__=8", 201 | "__STDC_UTF_16__=1", 202 | "__FLT64_MAX_10_EXP__=308", 203 | "__GNUC_STDC_INLINE__=1", 204 | "__DQ_IBIT__=0", 205 | "__cpp_delegating_constructors=200604L", 206 | "__FLT32_HAS_INFINITY__=1", 207 | "__DBL_MAX__=double(1.7976931348623157e+308L)", 208 | "__ULFRACT_IBIT__=0", 209 | "__cpp_raw_strings=200710L", 210 | "__INT_FAST32_MAX__=0x7fffffff", 211 | "__DBL_HAS_INFINITY__=1", 212 | "__HAVE_SPECULATION_SAFE_VALUE=1", 213 | "__ACCUM_IBIT__=16", 214 | "__THUMB_INTERWORK__=1", 215 | "__INTPTR_WIDTH__=32", 216 | "__UINT_LEAST32_MAX__=0xffffffffUL", 217 | "__ULLACCUM_IBIT__=32", 218 | "__LACCUM_MAX__=0X7FFFFFFFFFFFFFFFP-31LK", 219 | "__FLT32X_HAS_DENORM__=1", 220 | "__INT_FAST16_TYPE__=int", 221 | "__LDBL_HAS_DENORM__=1", 222 | "__cplusplus=201402L", 223 | "__cpp_ref_qualifiers=200710L", 224 | "__INT_LEAST32_MAX__=0x7fffffffL", 225 | "__ARM_PCS=1", 226 | "__ACCUM_MAX__=0X7FFFFFFFP-15K", 227 | "__DEPRECATED=1", 228 | "__cpp_rvalue_references=200610L", 229 | "__DBL_MAX_EXP__=1024", 230 | "__USACCUM_EPSILON__=0x1P-8UHK", 231 | "__WCHAR_WIDTH__=32", 232 | "__FLT32_MAX__=3.4028234663852886e+38F32", 233 | "__GCC_ATOMIC_LONG_LOCK_FREE=1", 234 | "__SFRACT_MAX__=0X7FP-7HR", 235 | "__FRACT_IBIT__=0", 236 | "__PTRDIFF_MAX__=0x7fffffff", 237 | "__UACCUM_MIN__=0.0UK", 238 | "__UACCUM_IBIT__=16", 239 | "__FLT32_HAS_QUIET_NAN__=1", 240 | "__GNUG__=10", 241 | "__LONG_LONG_MAX__=0x7fffffffffffffffLL", 242 | "__ULACCUM_MAX__=0XFFFFFFFFFFFFFFFFP-32ULK", 243 | "__cpp_nsdmi=200809L", 244 | "__SIZEOF_WINT_T__=4", 245 | "__LONG_LONG_WIDTH__=64", 246 | "__cpp_initializer_lists=200806L", 247 | "__FLT32_MAX_EXP__=128", 248 | "__ULLACCUM_MIN__=0.0ULLK", 249 | "__cpp_hex_float=201603L", 250 | "__GXX_ABI_VERSION=1014", 251 | "__UTA_FBIT__=64", 252 | "__FLT_MIN_EXP__=(-125)", 253 | "__UFRACT_IBIT__=0", 254 | "__cpp_lambdas=200907L", 255 | "__INT_FAST64_TYPE__=long long int", 256 | "__FLT64_DENORM_MIN__=4.9406564584124654e-324F64", 257 | "__DBL_MIN__=double(2.2250738585072014e-308L)", 258 | "__SIZEOF_POINTER__=4", 259 | "__SIZE_TYPE__=unsigned int", 260 | "__DBL_HAS_QUIET_NAN__=1", 261 | "__FLT32X_EPSILON__=2.2204460492503131e-16F32x", 262 | "__LACCUM_MIN__=(-0X1P31LK-0X1P31LK)", 263 | "__FRACT_FBIT__=15", 264 | "__ULLACCUM_FBIT__=32", 265 | "__GXX_TYPEINFO_EQUALITY_INLINE=0", 266 | "__FLT64_MIN_10_EXP__=(-307)", 267 | "__ULLFRACT_EPSILON__=0x1P-64ULLR", 268 | "__USES_INITFINI__=1", 269 | "__REGISTER_PREFIX__", 270 | "__UINT16_MAX__=0xffff", 271 | "__ACCUM_MIN__=(-0X1P15K-0X1P15K)", 272 | "__SQ_IBIT__=0", 273 | "__FLT32_MIN__=1.1754943508222875e-38F32", 274 | "__UINT8_TYPE__=unsigned char", 275 | "__UHA_FBIT__=8", 276 | "__FLT_DIG__=6", 277 | "__NO_INLINE__=1", 278 | "__SFRACT_MIN__=(-0.5HR-0.5HR)", 279 | "__UTQ_FBIT__=128", 280 | "__DEC_EVAL_METHOD__=2", 281 | "__FLT_MANT_DIG__=24", 282 | "__LDBL_DECIMAL_DIG__=17", 283 | "__VERSION__=\"10.3.1 20210824 (release)\"", 284 | "__UINT64_C(c)=c ## ULL", 285 | "__ULLFRACT_FBIT__=64", 286 | "__cpp_unicode_characters=200704L", 287 | "__SOFTFP__=1", 288 | "__FRACT_EPSILON__=0x1P-15R", 289 | "__ULACCUM_MIN__=0.0ULK", 290 | "__UDA_FBIT__=32", 291 | "__LLACCUM_EPSILON__=0x1P-31LLK", 292 | "__GCC_ATOMIC_INT_LOCK_FREE=1", 293 | "__FLOAT_WORD_ORDER__=__ORDER_LITTLE_ENDIAN__", 294 | "__USFRACT_MIN__=0.0UHR", 295 | "__FLT32_MANT_DIG__=24", 296 | "__UQQ_IBIT__=0", 297 | "__USFRACT_MAX__=0XFFP-8UHR", 298 | "__SCHAR_WIDTH__=8", 299 | "__INT32_C(c)=c ## L", 300 | "__ORDER_PDP_ENDIAN__=3412", 301 | "__UHQ_FBIT__=16", 302 | "__LLACCUM_FBIT__=31", 303 | "__INT_FAST32_TYPE__=int", 304 | "__UINT_LEAST16_TYPE__=short unsigned int", 305 | "__DBL_HAS_DENORM__=1", 306 | "__cpp_rtti=199711L", 307 | "__UINT64_MAX__=0xffffffffffffffffULL", 308 | "__UDQ_FBIT__=64", 309 | "__INT8_TYPE__=signed char", 310 | "__cpp_digit_separators=201309L", 311 | "__ELF__=1", 312 | "__GCC_ASM_FLAG_OUTPUTS__=1", 313 | "__SACCUM_EPSILON__=0x1P-7HK", 314 | "__ULFRACT_EPSILON__=0x1P-32ULR", 315 | "__LLFRACT_FBIT__=63", 316 | "__FLT_RADIX__=2", 317 | "__INT_LEAST16_TYPE__=short int", 318 | "__LDBL_EPSILON__=2.2204460492503131e-16L", 319 | "__UINTMAX_C(c)=c ## ULL", 320 | "__SACCUM_MAX__=0X7FFFP-7HK", 321 | "__FLT32X_MIN__=2.2250738585072014e-308F32x", 322 | "__SIG_ATOMIC_MAX__=0x7fffffff", 323 | "__UACCUM_FBIT__=16", 324 | "__GCC_ATOMIC_WCHAR_T_LOCK_FREE=1", 325 | "__VFP_FP__=1", 326 | "__SIZEOF_PTRDIFF_T__=4", 327 | "__LACCUM_EPSILON__=0x1P-31LK", 328 | "__LDBL_DIG__=15", 329 | "__FLT32X_MIN_EXP__=(-1021)", 330 | "__INT_FAST16_MAX__=0x7fffffff", 331 | "__FLT64_DIG__=15", 332 | "__UINT_FAST32_MAX__=0xffffffffU", 333 | "__UINT_LEAST64_TYPE__=long long unsigned int", 334 | "__SFRACT_EPSILON__=0x1P-7HR", 335 | "__FLT_HAS_QUIET_NAN__=1", 336 | "__FLT_MAX_10_EXP__=38", 337 | "__LONG_MAX__=0x7fffffffL", 338 | "__SIZEOF_SIZE_T__=4", 339 | "__FLT_HAS_INFINITY__=1", 340 | "__cpp_unicode_literals=200710L", 341 | "__UINT_FAST16_TYPE__=unsigned int", 342 | "__ARM_32BIT_STATE=1", 343 | "__INT_FAST32_WIDTH__=32", 344 | "__CHAR16_TYPE__=short unsigned int", 345 | "__PRAGMA_REDEFINE_EXTNAME=1", 346 | "__SIZE_WIDTH__=32", 347 | "__INT_LEAST16_MAX__=0x7fff", 348 | "__INT64_MAX__=0x7fffffffffffffffLL", 349 | "__SACCUM_FBIT__=7", 350 | "__FLT32_DENORM_MIN__=1.4012984643248171e-45F32", 351 | "__SIG_ATOMIC_WIDTH__=32", 352 | "__INT_LEAST64_TYPE__=long long int", 353 | "__INT16_TYPE__=short int", 354 | "__INT_LEAST8_TYPE__=signed char", 355 | "__SQ_FBIT__=31", 356 | "__ARM_ARCH_ISA_THUMB=1", 357 | "__INT_FAST8_MAX__=0x7fffffff", 358 | "__ARM_ARCH=4", 359 | "__INTPTR_MAX__=0x7fffffff", 360 | "__cpp_sized_deallocation=201309L", 361 | "__QQ_FBIT__=7", 362 | "__UTA_IBIT__=64", 363 | "__FLT64_HAS_QUIET_NAN__=1", 364 | "__FLT32_MIN_10_EXP__=(-37)", 365 | "__EXCEPTIONS=1", 366 | "__PTRDIFF_WIDTH__=32", 367 | "__LDBL_MANT_DIG__=53", 368 | "__SFRACT_FBIT__=7", 369 | "__cpp_range_based_for=200907L", 370 | "__SACCUM_MIN__=(-0X1P7HK-0X1P7HK)", 371 | "__FLT64_HAS_INFINITY__=1", 372 | "__SIG_ATOMIC_MIN__=(-__SIG_ATOMIC_MAX__ - 1)", 373 | "__cpp_return_type_deduction=201304L", 374 | "__INTPTR_TYPE__=int", 375 | "__UINT16_TYPE__=short unsigned int", 376 | "__WCHAR_TYPE__=unsigned int", 377 | "__SIZEOF_FLOAT__=4", 378 | "__TQ_FBIT__=127", 379 | "__USQ_FBIT__=32", 380 | "__UINTPTR_MAX__=0xffffffffU", 381 | "__INT_FAST64_WIDTH__=64", 382 | "__cpp_decltype=200707L", 383 | "__FLT32_DECIMAL_DIG__=9", 384 | "__INT_FAST64_MAX__=0x7fffffffffffffffLL", 385 | "__GCC_ATOMIC_TEST_AND_SET_TRUEVAL=1", 386 | "__FLT_NORM_MAX__=3.4028234663852886e+38F", 387 | "__UINT_FAST64_TYPE__=long long unsigned int", 388 | "__INT_MAX__=0x7fffffff", 389 | "__LACCUM_FBIT__=31", 390 | "__USACCUM_MIN__=0.0UHK", 391 | "__UHA_IBIT__=8", 392 | "__INT64_TYPE__=long long int", 393 | "__FLT_MAX_EXP__=128", 394 | "__UTQ_IBIT__=0", 395 | "__DBL_MANT_DIG__=53", 396 | "__cpp_inheriting_constructors=201511L", 397 | "__INT_LEAST64_MAX__=0x7fffffffffffffffLL", 398 | "__WINT_TYPE__=unsigned int", 399 | "__UINT_LEAST32_TYPE__=long unsigned int", 400 | "__SIZEOF_SHORT__=2", 401 | "__ULLFRACT_IBIT__=0", 402 | "__FLT32_NORM_MAX__=3.4028234663852886e+38F32", 403 | "__LDBL_MIN_EXP__=(-1021)", 404 | "__arm__=1", 405 | "__FLT64_MAX__=1.7976931348623157e+308F64", 406 | "__UDA_IBIT__=32", 407 | "__WINT_WIDTH__=32", 408 | "__INT_LEAST8_MAX__=0x7f", 409 | "__INT_LEAST64_WIDTH__=64", 410 | "__FLT32X_MAX_10_EXP__=308", 411 | "__LFRACT_FBIT__=31", 412 | "__WCHAR_UNSIGNED__=1", 413 | "__LDBL_MAX_10_EXP__=308", 414 | "__ATOMIC_RELAXED=0", 415 | "__DBL_EPSILON__=double(2.2204460492503131e-16L)", 416 | "__UINT8_C(c)=c", 417 | "__FLT64_MAX_EXP__=1024", 418 | "__INT_LEAST32_TYPE__=long int", 419 | "__SIZEOF_WCHAR_T__=4", 420 | "__LLFRACT_MAX__=0X7FFFFFFFFFFFFFFFP-63LLR", 421 | "__FLT64_NORM_MAX__=1.7976931348623157e+308F64", 422 | "__INTMAX_MAX__=0x7fffffffffffffffLL", 423 | "__INT_FAST8_TYPE__=int", 424 | "__ULLACCUM_EPSILON__=0x1P-32ULLK", 425 | "__USACCUM_MAX__=0XFFFFP-8UHK", 426 | "__LDBL_HAS_INFINITY__=1", 427 | "__UHQ_IBIT__=0", 428 | "__ARM_FEATURE_COPROC=1", 429 | "__LLACCUM_IBIT__=32", 430 | "__FLT64_HAS_DENORM__=1", 431 | "__FLT32_EPSILON__=1.1920928955078125e-7F32", 432 | "__DBL_DECIMAL_DIG__=17", 433 | "__STDC_UTF_32__=1", 434 | "__INT_FAST8_WIDTH__=32", 435 | "__FLT32X_MAX__=1.7976931348623157e+308F32x", 436 | "__TA_FBIT__=63", 437 | "__DBL_NORM_MAX__=double(1.7976931348623157e+308L)", 438 | "__BYTE_ORDER__=__ORDER_LITTLE_ENDIAN__", 439 | "__UDQ_IBIT__=0", 440 | "__INTMAX_WIDTH__=64", 441 | "__ORDER_BIG_ENDIAN__=4321", 442 | "__cpp_runtime_arrays=198712L", 443 | "__UINT64_TYPE__=long long unsigned int", 444 | "__ACCUM_EPSILON__=0x1P-15K", 445 | "__UINT32_C(c)=c ## UL", 446 | "__cpp_alias_templates=200704L", 447 | "__FLT_DENORM_MIN__=1.4012984643248171e-45F", 448 | "__LLFRACT_IBIT__=0", 449 | "__INT8_MAX__=0x7f", 450 | "__LONG_WIDTH__=32", 451 | "__UINT_FAST32_TYPE__=unsigned int", 452 | "__FLT32X_NORM_MAX__=1.7976931348623157e+308F32x", 453 | "__CHAR32_TYPE__=long unsigned int", 454 | "__FLT_MAX__=3.4028234663852886e+38F", 455 | "__cpp_constexpr=201304L", 456 | "__USACCUM_FBIT__=8", 457 | "__INT32_TYPE__=long int", 458 | "__SIZEOF_DOUBLE__=8", 459 | "__cpp_exceptions=199711L", 460 | "__FLT_MIN_10_EXP__=(-37)", 461 | "__UFRACT_EPSILON__=0x1P-16UR", 462 | "__FLT64_MIN__=2.2250738585072014e-308F64", 463 | "__INT_LEAST32_WIDTH__=32", 464 | "__INTMAX_TYPE__=long long int", 465 | "__FLT32X_HAS_QUIET_NAN__=1", 466 | "__ATOMIC_CONSUME=1", 467 | "__GNUC_MINOR__=3", 468 | "__INT_FAST16_WIDTH__=32", 469 | "__UINTMAX_MAX__=0xffffffffffffffffULL", 470 | "__FLT32X_DENORM_MIN__=4.9406564584124654e-324F32x", 471 | "__HA_FBIT__=7", 472 | "__DBL_MAX_10_EXP__=308", 473 | "__LDBL_DENORM_MIN__=4.9406564584124654e-324L", 474 | "__INT16_C(c)=c", 475 | "__STDC__=1", 476 | "__FLT32X_DIG__=15", 477 | "__PTRDIFF_TYPE__=int", 478 | "__LLFRACT_MIN__=(-0.5LLR-0.5LLR)", 479 | "__ATOMIC_SEQ_CST=5", 480 | "__DA_FBIT__=31", 481 | "__UINT32_TYPE__=long unsigned int", 482 | "__FLT32X_MIN_10_EXP__=(-307)", 483 | "__UINTPTR_TYPE__=unsigned int", 484 | "__USA_IBIT__=16", 485 | "__ARM_EABI__=1", 486 | "__LDBL_MIN_10_EXP__=(-307)", 487 | "__cpp_generic_lambdas=201304L", 488 | "__SIZEOF_LONG_LONG__=8", 489 | "__ULACCUM_EPSILON__=0x1P-32ULK", 490 | "__cpp_user_defined_literals=200809L", 491 | "__SACCUM_IBIT__=8", 492 | "__GCC_ATOMIC_LLONG_LOCK_FREE=1", 493 | "__FLT_DECIMAL_DIG__=9", 494 | "__UINT_FAST16_MAX__=0xffffffffU", 495 | "__LDBL_NORM_MAX__=1.7976931348623157e+308L", 496 | "__GCC_ATOMIC_SHORT_LOCK_FREE=1", 497 | "__ULLFRACT_MAX__=0XFFFFFFFFFFFFFFFFP-64ULLR", 498 | "__UINT_FAST8_TYPE__=unsigned int", 499 | "__USFRACT_EPSILON__=0x1P-8UHR", 500 | "__ULACCUM_FBIT__=32", 501 | "__QQ_IBIT__=0", 502 | "__cpp_init_captures=201304L", 503 | "__ATOMIC_ACQ_REL=4", 504 | "__ATOMIC_RELEASE=3", 505 | "USBCON" 506 | ] 507 | } 508 | ] 509 | } -------------------------------------------------------------------------------- /BrakeModule_sw03/ADC_DMA.h: -------------------------------------------------------------------------------- 1 | 2 | // Code from https://www.stm32duino.com/viewtopic.php?f=41&t=110 3 | 4 | /* Private define ------------------------------------------------------------*/ 5 | #define ADC_BUFFER_SIZE ((uint32_t) 6) /* Size of array containing ADC converted values */ 6 | #define RANGE_12BITS ((uint32_t) 4095) /* Max value with a full range of 12 bits */ 7 | 8 | /* Private variables ---------------------------------------------------------*/ 9 | /* ADC handler declaration */ 10 | ADC_HandleTypeDef AdcHandle; 11 | /* Variable containing ADC conversions results */ 12 | __IO uint16_t ADCValues[ADC_BUFFER_SIZE]; 13 | /* Variable to report ADC analog watchdog status: */ 14 | /* RESET <=> voltage into AWD window */ 15 | /* SET <=> voltage out of AWD window */ 16 | uint8_t ubAnalogWatchdogStatus = RESET; /* Set into analog watchdog interrupt callback */ 17 | 18 | /** 19 | @brief This function handles ADC interrupt request. 20 | @param None 21 | @retval None 22 | */ 23 | extern "C" void ADC1_2_IRQHandler(void) 24 | { 25 | HAL_ADC_IRQHandler(&AdcHandle); 26 | } 27 | 28 | /** 29 | @brief This function handles DMA interrupt request. 30 | @param None 31 | @retval None 32 | */ 33 | extern "C" void DMA1_Channel1_IRQHandler(void) 34 | { 35 | HAL_DMA_IRQHandler(AdcHandle.DMA_Handle); 36 | } 37 | 38 | /** 39 | @brief ADC MSP initialization 40 | This function configures the hardware resources used in this example: 41 | - Enable clock of ADC peripheral 42 | - Configure the GPIO associated to the peripheral channels 43 | - Configure the DMA associated to the peripheral 44 | - Configure the NVIC associated to the peripheral interruptions 45 | @param hadc: ADC handle pointer 46 | @retval None 47 | */ 48 | extern "C" void HAL_ADC_MspInit(ADC_HandleTypeDef *hadc) 49 | { 50 | GPIO_InitTypeDef GPIO_InitStruct; 51 | static DMA_HandleTypeDef DmaHandle; 52 | RCC_PeriphCLKInitTypeDef PeriphClkInit; 53 | 54 | /*##-1- Enable peripherals and GPIO Clocks #################################*/ 55 | /* Enable clock of GPIO associated to the peripheral channels */ 56 | // __HAL_RCC_GPIOA_CLK_ENABLE(); 57 | 58 | /* Enable clock of ADCx peripheral */ 59 | __HAL_RCC_ADC1_CLK_ENABLE(); 60 | 61 | /* Configure ADCx clock prescaler */ 62 | /* Caution: On STM32F1, ADC clock frequency max is 14MHz (refer to device */ 63 | /* datasheet). */ 64 | /* Therefore, ADC clock prescaler must be configured in function */ 65 | /* of ADC clock source frequency to remain below this maximum */ 66 | /* frequency. */ 67 | PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_ADC; 68 | PeriphClkInit.AdcClockSelection = RCC_ADCPCLK2_DIV6; 69 | HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit); 70 | 71 | /* Enable clock of DMA associated to the peripheral */ 72 | __HAL_RCC_DMA1_CLK_ENABLE(); 73 | 74 | /*##-2- Configure peripheral GPIO ##########################################*/ 75 | /* Configure GPIO pin of the selected ADC channel */ 76 | GPIO_InitStruct.Pin = ADC_CHANNEL_0; 77 | GPIO_InitStruct.Mode = GPIO_MODE_ANALOG; 78 | GPIO_InitStruct.Pull = GPIO_PIN_0; 79 | HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); 80 | 81 | GPIO_InitStruct.Pin = ADC_CHANNEL_1; 82 | GPIO_InitStruct.Mode = GPIO_MODE_ANALOG; 83 | GPIO_InitStruct.Pull = GPIO_PIN_0; 84 | HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); 85 | 86 | GPIO_InitStruct.Pin = ADC_CHANNEL_5; 87 | GPIO_InitStruct.Mode = GPIO_MODE_ANALOG; 88 | GPIO_InitStruct.Pull = GPIO_PIN_5; 89 | HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); 90 | 91 | GPIO_InitStruct.Pin = ADC_CHANNEL_6; 92 | GPIO_InitStruct.Mode = GPIO_MODE_ANALOG; 93 | GPIO_InitStruct.Pull = GPIO_PIN_6; 94 | HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); 95 | 96 | /*##-3- Configure the DMA ##################################################*/ 97 | /* Configure DMA parameters */ 98 | DmaHandle.Instance = DMA1_Channel1; 99 | 100 | DmaHandle.Init.Direction = DMA_PERIPH_TO_MEMORY; 101 | DmaHandle.Init.PeriphInc = DMA_PINC_DISABLE; 102 | DmaHandle.Init.MemInc = DMA_MINC_ENABLE; 103 | DmaHandle.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD; /* Transfer from ADC by half-word to match with ADC configuration: ADC resolution 10 or 12 bits */ 104 | DmaHandle.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD; /* Transfer to memory by half-word to match with buffer variable type: half-word */ 105 | DmaHandle.Init.Mode = DMA_CIRCULAR; /* DMA in circular mode to match with ADC configuration: DMA continuous requests */ 106 | DmaHandle.Init.Priority = DMA_PRIORITY_HIGH; 107 | 108 | /* Deinitialize & Initialize the DMA for new transfer */ 109 | HAL_DMA_DeInit(&DmaHandle); 110 | HAL_DMA_Init(&DmaHandle); 111 | 112 | /* Associate the initialized DMA handle to the ADC handle */ 113 | __HAL_LINKDMA(hadc, DMA_Handle, DmaHandle); 114 | 115 | /*##-4- Configure the NVIC #################################################*/ 116 | 117 | /* NVIC configuration for DMA interrupt (transfer completion or error) */ 118 | /* Priority: high-priority */ 119 | HAL_NVIC_SetPriority(DMA1_Channel1_IRQn, 1, 0); 120 | HAL_NVIC_EnableIRQ(DMA1_Channel1_IRQn); 121 | 122 | 123 | /* NVIC configuration for ADC interrupt */ 124 | /* Priority: high-priority */ 125 | HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); 126 | HAL_NVIC_EnableIRQ(ADC1_2_IRQn); 127 | } 128 | 129 | /** 130 | @brief ADC MSP de-initialization 131 | This function frees the hardware resources used in this example: 132 | - Disable clock of ADC peripheral 133 | - Revert GPIO associated to the peripheral channels to their default state 134 | - Revert DMA associated to the peripheral to its default state 135 | - Revert NVIC associated to the peripheral interruptions to its default state 136 | @param hadc: ADC handle pointer 137 | @retval None 138 | */ 139 | extern "C" void HAL_ADC_MspDeInit(ADC_HandleTypeDef *hadc) 140 | { 141 | /*##-1- Reset peripherals ##################################################*/ 142 | __HAL_RCC_ADC1_FORCE_RESET(); 143 | __HAL_RCC_ADC1_RELEASE_RESET(); 144 | 145 | /*##-2- Disable peripherals and GPIO Clocks ################################*/ 146 | /* De-initialize GPIO pin of the selected ADC channel */ 147 | HAL_GPIO_DeInit(GPIOA, GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_5 | GPIO_PIN_6); 148 | 149 | /*##-3- Disable the DMA ####################################################*/ 150 | /* De-Initialize the DMA associated to the peripheral */ 151 | if (hadc->DMA_Handle != NULL) 152 | { 153 | HAL_DMA_DeInit(hadc->DMA_Handle); 154 | } 155 | 156 | /*##-4- Disable the NVIC ###################################################*/ 157 | /* Disable the NVIC configuration for DMA interrupt */ 158 | HAL_NVIC_DisableIRQ(DMA1_Channel1_IRQn); 159 | 160 | /* Disable the NVIC configuration for ADC interrupt */ 161 | HAL_NVIC_DisableIRQ(ADC1_2_IRQn); 162 | } 163 | 164 | /** 165 | @brief ADC configuration 166 | @param None 167 | @retval None 168 | */ 169 | static void ADC_Config(void) 170 | { 171 | ADC_ChannelConfTypeDef sConfig; 172 | ADC_AnalogWDGConfTypeDef AnalogWDGConfig; 173 | 174 | /* Configuration of ADCx init structure: ADC parameters and regular group */ 175 | AdcHandle.Instance = ADC1; 176 | 177 | //AdcHandle.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV8; 178 | //AdcHandle.Init.Resolution = ADC_RESOLUTION_12B; 179 | AdcHandle.Init.ScanConvMode = ADC_SCAN_ENABLE; 180 | //AdcHandle.Init.ScanConvMode = ADC_SCAN_DISABLE; 181 | AdcHandle.Init.ContinuousConvMode = ENABLE; 182 | AdcHandle.Init.DiscontinuousConvMode = DISABLE; 183 | AdcHandle.Init.ExternalTrigConv = ADC_SOFTWARE_START; 184 | AdcHandle.Init.DataAlign = ADC_DATAALIGN_RIGHT; 185 | AdcHandle.Init.NbrOfConversion = 6; 186 | //AdcHandle.Init.NbrOfConversion = 1; 187 | 188 | if (HAL_ADC_Init(&AdcHandle) != HAL_OK) 189 | { 190 | Error_Handler(); 191 | } 192 | /** Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time. 193 | */ 194 | sConfig.Channel = ADC_CHANNEL_0; 195 | sConfig.Rank = ADC_REGULAR_RANK_1; 196 | sConfig.SamplingTime = ADC_SAMPLETIME_239CYCLES_5; 197 | if (HAL_ADC_ConfigChannel(&AdcHandle, &sConfig) != HAL_OK) 198 | { 199 | Error_Handler(); 200 | } 201 | /** Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time. 202 | */ 203 | sConfig.Channel = ADC_CHANNEL_1; 204 | sConfig.Rank = ADC_REGULAR_RANK_2; 205 | sConfig.SamplingTime = ADC_SAMPLETIME_239CYCLES_5; 206 | if (HAL_ADC_ConfigChannel(&AdcHandle, &sConfig) != HAL_OK) 207 | { 208 | Error_Handler(); 209 | } 210 | /** Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time. 211 | */ 212 | sConfig.Channel = ADC_CHANNEL_5; 213 | sConfig.Rank = ADC_REGULAR_RANK_3; 214 | sConfig.SamplingTime = ADC_SAMPLETIME_239CYCLES_5; 215 | if (HAL_ADC_ConfigChannel(&AdcHandle, &sConfig) != HAL_OK) 216 | { 217 | Error_Handler(); 218 | } 219 | /** Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time. 220 | */ 221 | sConfig.Channel = ADC_CHANNEL_6; 222 | sConfig.Rank = ADC_REGULAR_RANK_4; 223 | sConfig.SamplingTime = ADC_SAMPLETIME_239CYCLES_5; 224 | if (HAL_ADC_ConfigChannel(&AdcHandle, &sConfig) != HAL_OK) 225 | { 226 | Error_Handler(); 227 | } 228 | /** Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time. 229 | */ 230 | sConfig.Channel = ADC_CHANNEL_VREFINT; 231 | sConfig.Rank = ADC_REGULAR_RANK_5; 232 | sConfig.SamplingTime = ADC_SAMPLETIME_239CYCLES_5; 233 | if (HAL_ADC_ConfigChannel(&AdcHandle, &sConfig) != HAL_OK) 234 | { 235 | Error_Handler(); 236 | } 237 | /** Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time. 238 | */ 239 | sConfig.Channel = ADC_CHANNEL_TEMPSENSOR; 240 | sConfig.Rank = ADC_REGULAR_RANK_6; 241 | sConfig.SamplingTime = ADC_SAMPLETIME_239CYCLES_5; 242 | if (HAL_ADC_ConfigChannel(&AdcHandle, &sConfig) != HAL_OK) 243 | { 244 | Error_Handler(); 245 | } 246 | 247 | /* Set analog watchdog thresholds in order to be between steps of DAC */ 248 | /* voltage. */ 249 | /* - High threshold: between DAC steps 1/2 and 3/4 of full range: */ 250 | /* 5/8 of full range (4095 <=> Vdda=3.3V): 2559<=> 2.06V */ 251 | /* - Low threshold: between DAC steps 0 and 1/4 of full range: */ 252 | /* 1/8 of full range (4095 <=> Vdda=3.3V): 512 <=> 0.41V */ 253 | 254 | /* Analog watchdog 1 configuration */ 255 | AnalogWDGConfig.WatchdogMode = ADC_ANALOGWATCHDOG_ALL_REG; 256 | AnalogWDGConfig.Channel = ADC_CHANNEL_0; 257 | AnalogWDGConfig.ITMode = ENABLE; 258 | AnalogWDGConfig.HighThreshold = (RANGE_12BITS * 5U / 8U); 259 | AnalogWDGConfig.LowThreshold = (RANGE_12BITS * 1U / 8U); 260 | if (HAL_ADC_AnalogWDGConfig(&AdcHandle, &AnalogWDGConfig) != HAL_OK) 261 | { 262 | /* Channel Configuration Error */ 263 | Error_Handler(); 264 | } 265 | } 266 | 267 | /** 268 | @brief Analog watchdog callback in non blocking mode. 269 | @param hadc: ADC handle 270 | @retval None 271 | */ 272 | extern "C" void HAL_ADC_LevelOutOfWindowCallback(ADC_HandleTypeDef * ) { 273 | /* Set variable to report analog watchdog out of window status to main */ 274 | /* program. */ 275 | ubAnalogWatchdogStatus = SET; 276 | } 277 | 278 | /** 279 | @brief ADC error callback in non blocking mode 280 | (ADC conversion with interruption or transfer by DMA) 281 | @param hadc: ADC handle 282 | @retval None 283 | */ 284 | extern "C" void HAL_ADC_ErrorCallback(ADC_HandleTypeDef * ) 285 | { 286 | /* In case of ADC error, call main error handler */ 287 | Error_Handler(); 288 | } 289 | -------------------------------------------------------------------------------- /BrakeModule_sw03/BrakeModule_sw03.ino: -------------------------------------------------------------------------------- 1 | 2 | /* 3 | Software for BrakeModule v0.3.2 4 | Software version 0.3 5 | 6 | The Main purpose of the module is to control charge pump so that the car will brake wo any errors on the car side and be an CC for OP so that it can controlled from the steering wheel buttons. 7 | Functionalities in software version 0.1: 8 | - Read CAN data: 9 | - Car speed 10 | - Brakelight signal 11 | - BMW original cruise state and cruise button presses 12 | - OP brake demand and cancel request 13 | - Send CAN data: 14 | - Emulate Toyota cruise messages 0x1D2 and 0x1D3 to control OP 15 | - Disconnect DSC module from charge pump with relays 16 | - Control charge pump + side with relay and - side with PWR MOSFET 17 | - Control cars two brake light signal with relay and P-channel FET 18 | - Use ADC with DMA to read: 19 | - DSC line voltage through voltage divider 20 | - Brakelight BLTS signal (12V) through voltage divider 21 | 22 | Remember that in STM32F103xC max currents are (datasheet p. 43): 23 | - Total current into VDD /V DDA power lines (source) 150 mA 24 | - Output current sunk by any I/O and control pin 25 mA 25 | - Output current source by any I/Os and control pin 25 mA 26 | - Injected current on five volt tolerant pins -5/+0 mA 27 | - Injected current on any other pin +/- 5 mA 28 | - Total injected current (sum of all I/O and control pins) +/- 25 mA 29 | 30 | v0.2 will try to: 31 | - Fix bug where DSC gives DTC when harshly braking and OP disconnect (I believe it is caused by brake light signal going to zero w high brakeline pressure (zeroing problem) -> Maybe below solutions solved this 32 | - Add brakepressure reading from 0x77F to CAN read -> DONE 33 | - Add if statement not to disengage brakelight signal if brakepressure over threshold -> DONE 34 | - Make change to OP disengage logik so that it will disconnect only when brake pedal is pressed and OP want's to accelerate, not only for brakelight signal -> DONE 35 | - Add GAS_COMMAND from 0x200 to CAN read -> DONE 36 | - Some code cleanup 37 | - Delete STM32 uncommented variables -> DONE 38 | - Uncomment vout and temp variables -> DONE 39 | - Delete uncommented old MyTim->setPWM functions 40 | 41 | v0.3 will try to: 42 | - Make relays state changes to functions -> DONE 43 | - Use CMSIS calls instead of arduino functions -> DONE 44 | - Add safety measures 45 | - Verify GPIO OUTPUT state (IDR is updated every APB2 bus clock cycle (on datasheet APB2 bus speed is 72 MHz so check this) so there might be some delay in pin state change detection) 46 | -> FUNCTION made 47 | - Lock pin configuration in GPIOx_LCKR register, so that our pin counfigurations won't change for accidential bit flip (have to check if this is configuration lock or pin state lock) http://slemi.info/2017/04/16/configuring-io-pins/ 48 | -> DONE? 49 | - Panic at high STM32 chip temperature -> DONE 50 | - IWDG -> DONE before 51 | - Detect ADC freeze (see if there is some normal fluctuation on values) -> DONE 52 | - Make debugBlinky -> DONE 53 | - Make explanations for the fail_flags to debug what caused the failure 54 | - If there is sent fail (CAN BUS fail, the loop speed decreases drastically), check this! 55 | - ADC averaging (suppress one off) (or max difference?) 56 | - ADC min/max values (analogWatchdog?) / max ROC / not possible values 57 | - CAN msg CRC check (can be done in HW? and can this be implement somehow to other critical data?) -> DONE? 58 | - Add bit and operation for CAN msg byte relevant bits (decrease possibility of bit flip messing the wanted value) 59 | - CAN bus fail detection -> It is DONE in STM32F1_CAN.h and if not 0x343 is correctly received in x amount cycles 60 | - Brake demand boundaries -> DONE? 61 | - SetSpeed min/max values -> DONE? 62 | - Read DSC state from CAN and if not on disengage 63 | - Read and compare: (maybe think if this is wise, maybe only if brake pressure then pedal must be pressed (is it needed bc this is done in DSC)) 64 | - Brake pressure from CAN 65 | - Brake pedal state from CAN 66 | - Compare pump line voltage to DSC line voltage when -> NOT POSSIBLE AT THIS HW -> Maybe in test HW :> 67 | - Think how transition from BM ON state to OFF state should be done so that it would be most unlikely that the BM will trigger DTC in DSC 68 | - Is it possible to DSC to run CP when brake pedal (or brakepressure) press is detected 69 | - If PMP line voltage is detected wo brake demand don't switch to OFF state 70 | - How would brake pressure zeroing problem would be tackled when transitioning from ON to OFF state in panic mode 71 | - Is this problem in DSC event 72 | - Is best strategy to change BM state or track DSC brake demand and run BM accordingly 73 | 74 | 75 | TODO: 76 | - Clean the AVR/STM32 parallel code 77 | - Test if releasetimer could be replaced with brakepressure 78 | - Add 1M looptyme to CAN 79 | - Don't connect pump back to DSC if brake pressure is above certain threshold (try to prevent bug that triggers DSC DTC if braked hard and pump is running and then connects back to DSC) 80 | - Clean readSerial() stuff -> Did some cleaning -> More? 81 | - Have multiple readings of each ADC channel 82 | - Implement BLS LOW side detection (is it possible w this HW?) 83 | 84 | */ 85 | 86 | #include "STM32F1_CAN.h" 87 | #include "ADC_DMA.h" 88 | #include 89 | 90 | // #define FAN_CTRL // If defined use FAN for PWR MOSFET temperature control 91 | // #define DEBUG // If defined use serial for debugging -> This is defined in "STM32F1_CAN.h" 92 | 93 | #define RAMP_VALUE 60U // Interval time between multiple button presses detections in milliseconds 94 | #define INTERVAL 30U // Interval time at which to send 0x1D2 and 0x1D3 messages (milliseconds) 95 | #define UP 32U // This is UP button press value for button press byte (BTN_CMD) 96 | #define DWN 64U // This is DOWN button press value for button press byte (BTN_CMD) 97 | #define RSM 96U // This is RESUME button press value for button press byte (BTN_CMD) 98 | 99 | #define FROZEN_THRESHOLD 10 // Freezed ADC detection threshold value 100 | #define DSC_THRESHOLD 3000U // Value that trigger DSC pump demand 101 | #define DSC_MAX 3500U // MAX value that should be detected in DSC_volt 102 | 103 | #define pwm_pin PA8 104 | #define max(a,b) ((a)>(b)?(a):(b)) 105 | #define min(a,b) ((a)<(b)?(a):(b)) 106 | 107 | uint32_t dallasTyme = 0; // 5 sec interval time value for reading DS18B20 and some other stuff 108 | uint32_t loopTyme = 0; // Time value for every 1 million loop cycles 109 | uint32_t currentMillis = 0; // Looptime 110 | uint32_t previousMillis1 = 0; // Time value for last sent 0x1D2 msg 111 | uint32_t previousMillis2 = 15; // Time value for last sent 0x1D3 msg 112 | uint32_t timerRelease = 0; // Time value for delaying the transition from direct drive of the charge pump back to ABS control 113 | uint32_t rampTyme = 0; // Time value for interval value for detecting multiple set speed up or down value 114 | uint32_t loopydoo = 0; // Time difference value for every 1 million loop cycles 115 | uint32_t blinkyTyme = 0; // Time difference value for blnkyLed 116 | uint32_t counter = 0; // Count value for counting 1 million loops 117 | uint32_t CAN_count = 0; // Count value for number of CAN msg reads 118 | 119 | float STMtemp; // Internal temperature of the STM32F1 chip 120 | float Vsense; // Internal reference voltage of the ADC 121 | int32_t iSTMtemp; // Internal temperature of the STM32F1 chip 122 | int32_t iVsense; // Internal reference voltage of the ADC 123 | 124 | int16_t TMP36 = 0; // TMP36 temperature read in desicelsius (200 = 20 degrees C) 125 | int16_t BRK_CMD = 0; // Deceleration value (wanted braking) from OP read from CAN 126 | uint16_t errcounter = 0; // Errorcounter for disabling OP if non 0x343 is received at 65000 cycletimes 127 | uint16_t CNL_REQ_CNT = 0; // Counter for detecting BrakeModule cancel request (disable module) 128 | uint16_t BRK_ST_CNT = 0; // Counter for delaying BRK_ST_OP happening so that OP wont disengage unnecessarily 129 | uint16_t DSC_volt = 0; // Value of voltage from DSC+ line in milliVolts (this has to be int if used in minus calculation) 130 | uint16_t previousDSC_volt = 0; // Save previous DSC_volt for freezed ADC detection (this has to be int if used in minus calculation) 131 | uint16_t car_speed = 0; // Value of the read car speed 132 | uint16_t ADC_frozen = 0; // Counter for frozen ADC values 133 | uint16_t ledInterval = 2000; // Interval value for blinkyLed 134 | 135 | uint8_t set_speed = 0; // Speed value sent to OP to be the set speed of the cruise control 136 | uint8_t BRK_ST = 0; // Brake pedal state from CAN 137 | uint8_t BRK_ST_OP = 0; // Brake pedal state send for OP by CAN 138 | uint8_t BTN_CMD = 0; // Steering wheel button state from CAN 139 | uint8_t OCC = 0; // Car cruise control pre-enable state from CAN 140 | uint8_t CNL_REQ = 0; // Cancel request (disable) to BrakeModule sent by OP through CAN 141 | uint8_t BRK_CMD_CRC = 0; // 0x343 CRC calculation 142 | uint8_t CC_ST_OP = 0; // State of emulation of TOYOTA cruise control sent to OP 143 | 144 | bool BRK_FLG = false; // Braking logik flags, flags true when OP starts to brake 145 | bool RLS_FLG = false; // Braking logik flags, flag for intermediate step for going back from OP control to ABS control 146 | bool FAN_ON = false; // Flag for FAN control 147 | bool BTN_PRS_FLG = false; // Flag for Steering wheel button presses 148 | // bool fail_flag = false; // Flag for any failure that would prevent OPENPILOT to engage and BrakeModule to turn ON 149 | 150 | //Interpolation shit, this is done BC relationship of PWM and brake pressure is not linear 151 | 152 | // in[] holds the read values from BRK_CMD so are the INPUT values for the interpolation for OUTPUTing the PWM values regarding of the INPUT 153 | // note: the in array should have increasing values 154 | int cmd_in[] = {-1000, -900, -800, -700, -600, -500, -400, -300, -200, -100, -50}; 155 | // out[] holds the values wanted in OUTPUT regarding to INPUT in[], these are PWM duty cycle% values that are interpolated from BRK_CMD values 156 | int pwm_out[] = {100, 97, 93, 88, 81, 73, 67, 59, 54, 39, 20}; // This is STM323F1 values 157 | 158 | uint16_t freq = 15000; 159 | uint16_t BLTS_volt = 0; // Brake-light test switch circuit read 160 | uint16_t BRK_PRS = 0; 161 | uint16_t ACC_CMD = 0; 162 | 163 | // uint16_t id; // This is unnecessary? Why don't just use CAN_RX_msg.id 164 | // uint8_t dlc; 165 | // uint8_t arry[8]; 166 | 167 | int16_t PWM = 0; 168 | 169 | // Setup Hardware timer 170 | //HardwareTimer *MyTim; 171 | TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(digitalPinToPinName(pwm_pin), PinMap_PWM); 172 | uint32_t channel = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(pwm_pin), PinMap_PWM)); 173 | HardwareTimer *MyTim = new HardwareTimer(Instance); 174 | 175 | 176 | // ******************************************************************************************************* 177 | // *********************************************** SETUP ************************************************* 178 | // ******************************************************************************************************* 179 | void setup() { 180 | 181 | #ifdef DEBUG 182 | // initialize serial: 183 | Serial.begin(230400); // I use this w STM32 184 | #endif 185 | 186 | // Setup pins HAL configuration GPIO stuff https://simonmartin.ch/resources/stm32/dl/STM32%20Tutorial%2001%20-%20GPIO%20Operations%20using%20HAL%20(and%20FreeRTOS).pdf 187 | pinMode(PA7, OUTPUT); // 12V relay 188 | pinMode(PB0, OUTPUT); // PMP+ relay 189 | pinMode(PB1, OUTPUT); // PMP- relay 190 | pinMode(PB12, OUTPUT); // BLS_HIGH driver 191 | pinMode(PB13, OUTPUT); // BLS_LOW relay driver 192 | pinMode(PB14, OUTPUT); // FAN driver 193 | pinMode(PC13, OUTPUT); // Blinky LED 194 | 195 | // This is to make sure that brakelight signal transistors are in right state and not floating (really needed?) 196 | digitalWrite(PB12, HIGH); 197 | digitalWrite(PB13, HIGH); 198 | digitalWrite(PB12, LOW); 199 | digitalWrite(PB13, LOW); 200 | 201 | // Setup the PWM 202 | MyTim->setPWM(channel, pwm_pin, freq, 0); 203 | //MyTim->setCaptureCompare(channel, 0, PERCENT_COMPARE_FORMAT); 204 | 205 | // Setup ADC 206 | // Used and modded ADC config for mulitple channels: https://github.com/MahdiKarimian/Multiple-ADC-with-DMA-in-STM32-without-interrupt/blob/main/main.c 207 | // Great tutorial for CMSIS ADC DMA config https://www.youtube.com/watch?v=Gn1UhtXqiaI&t=61s 208 | 209 | /* Configure the ADC peripheral */ 210 | ADC_Config(); 211 | 212 | /* Run the ADC calibration */ 213 | if (HAL_ADCEx_Calibration_Start(&AdcHandle) != HAL_OK) { 214 | /* Calibration Error */ 215 | Error_Handler(); 216 | } 217 | /* Start ADC conversion on regular group with transfer by DMA */ 218 | if (HAL_ADC_Start_DMA(&AdcHandle, (uint32_t *)ADCValues, ADC_BUFFER_SIZE) != HAL_OK) { 219 | /* Start Error */ 220 | Error_Handler(); 221 | } 222 | 223 | // Setup CAN 224 | bool ret = CANInit(CAN_500KBPS, 2); // CAN_RX mapped to PB8, CAN_TX mapped to PB9 225 | if (!ret) 226 | { 227 | while(true) {} 228 | } 229 | 230 | // LOCK pin configurations after intialization 231 | port_lock_conf(); 232 | 233 | // README for IWacthdog library https://github.com/stm32duino/Arduino_Core_STM32/blob/main/libraries/IWatchdog/README.md 234 | // Initialize the IWDG with 4 seconds timeout. 235 | // This would cause a CPU reset if the IWDG timer 236 | // is not reloaded in approximately 400 milliseconds. 237 | IWatchdog.begin(400000); 238 | 239 | } 240 | 241 | 242 | 243 | // ******************************************************************************************************* 244 | // ********************************************* LOOPYDOO ************************************************ 245 | // ******************************************************************************************************* 246 | 247 | void loop() { 248 | 249 | currentMillis = millis(); 250 | 251 | // This is to show how fast is the looptyme, the value given is average loop execution time per 1 million loopcycles in nanoseconds 252 | if (counter > 1000000U) { 253 | loopydoo = currentMillis - loopTyme; 254 | #ifdef DEBUG 255 | Serial.println(); 256 | Serial.print("Loop tyme was in 1M cycles: "); 257 | Serial.println(loopydoo); 258 | Serial.println(); 259 | #endif 260 | counter = 0; 261 | loopTyme = millis(); 262 | } 263 | 264 | 265 | // ******************************* READ SERIAL FOR INPUT ***************************************** 266 | 267 | #ifdef DEBUG 268 | readSerial(); 269 | #endif 270 | 271 | 272 | // ************************************** ADC STUFF ********************************************** 273 | 274 | // ADC bit resolution is 12 275 | // ADC clock is set 12 MHz 276 | // Sampling time is set to 239.5 cycles per sample 277 | // ADC values are read from 6 channels 278 | // ADC conversion time is: ( 1.5 cycles + 239.5 cycles ) / 12 cycles per uS = 20,8 uS 279 | // When all 6 has been read, one channel will update every 125 uS 280 | DSC_volt = ADCValues[0]; 281 | BLTS_volt = ADCValues[2]; 282 | 283 | // Check if the ADC value is changing by more than the threshold 284 | // over multiple readings 285 | //if (abs(DSC_volt - previousDSC_volt) < FROZEN_THRESHOLD) { 286 | if ((abs(DSC_volt - previousDSC_volt) < FROZEN_THRESHOLD) && (DSC_volt > 0U)) { // Maybe this is just used for testing, test if 0 value check is necessary in production 287 | // The ADC is frozen, so do something about it (e.g. try restarting it) 288 | ADC_frozen++; // Counter for not triggering the failure with few same values (see ADC update interval) 289 | if (ADC_frozen > 25000U) { // TODO: Calculate if this value changes per loop and how long this takes if frozen (if ~20 uS/loop, trigger time is 500 millisecond) 290 | fail_flag = true; 291 | #ifdef DEBUG 292 | Serial.println("ADC frozed"); // If ADC Frozed this was printed way too often 293 | #endif 294 | } 295 | } 296 | else { 297 | ADC_frozen = 0; 298 | } 299 | 300 | // Save the current ADC value for the next iteration 301 | previousDSC_volt = DSC_volt; 302 | 303 | // Detect not possible value from DSC 304 | if (DSC_volt > DSC_MAX) { 305 | fail_flag = true; 306 | } 307 | 308 | // ************************************* READ CAN STUFF ****************************************** 309 | 310 | CAN_msg_t CAN_RX_msg; 311 | 312 | // if(CANMsgAvail()) { 313 | if(CANMsgAvail() != 0U) { 314 | CANReceive(&CAN_RX_msg); 315 | //id = CAN_RX_msg.id; 316 | 317 | // Put needed CAN DATA into variables 318 | //if (id == 0x153) { // Read speed from 0x153 and do some conversion 319 | if (CAN_RX_msg.id == 0x153) { // Read speed from 0x153 and do some conversion 320 | //car_speed = (((CAN_RX_msg.data[2] << 3) + (CAN_RX_msg.data[1] >> 5)) * 0.25) - 2.64; 321 | car_speed = ((((CAN_RX_msg.data[2] & B00011111) << 3) + ((CAN_RX_msg.data[1] & B11100000) >> 5)) * 0.25) - 2.64; 322 | //Serial.println("0x153"); 323 | CAN_count++; 324 | } 325 | //if (id == 0x200) { // Read GAS_COMMAND request value from OP 326 | if (CAN_RX_msg.id == 0x200) { // Read GAS_COMMAND request value from OP 327 | ACC_CMD = ((CAN_RX_msg.data[0] << 8) | CAN_RX_msg.data[1]); 328 | //Serial.println("0x200"); 329 | CAN_count++; 330 | } 331 | //if (id == 0x329) { 332 | if (CAN_RX_msg.id == 0x329) { 333 | BTN_CMD = (CAN_RX_msg.data[3] & B01100000); // &B01100000 is to make sure that other bits in the byte don't bother 334 | BRK_ST = (CAN_RX_msg.data[6] & B00000001); // Read brake pedal switch state 335 | BRK_ST_CNT++; // Use this for not triggering BRK_ST_OP too early after OP has stopped braking 336 | //Serial.println("0x329"); 337 | CAN_count++; 338 | } 339 | //if (id == 0x343) { 340 | if (CAN_RX_msg.id == 0x343) { 341 | uint8_t cksum = can_cksum(CAN_RX_msg.data, 7, 0x343); // Calculate TOYOTA checksum 342 | if (cksum == CAN_RX_msg.data[7]) { 343 | BRK_CMD = ((CAN_RX_msg.data[0] << 8) | CAN_RX_msg.data[1]); // Read brake request value from OP 344 | BRK_CMD = max(-3500, BRK_CMD); // Is these values clipped already in interpolation 345 | BRK_CMD = min(150, BRK_CMD); // Is these values clipped already in interpolation 346 | CNL_REQ = (CAN_RX_msg.data[3] & B00000001); // Read cancel request flag from OP 347 | //Serial.println("0x343"); 348 | CAN_count++; 349 | errcounter = 0; // If 0x343 read and checksum OK, set error counter to 0 350 | } 351 | else { 352 | fail_flag = true; // If 0x343 checksum does not match raise fail_flag to disable OP 353 | } 354 | } 355 | //if (id == 0x545) { 356 | if (CAN_RX_msg.id == 0x545) { 357 | OCC = (CAN_RX_msg.data[0] & B00001000); // If OCC == 0x08 BMW original cruise is pre-enabled (cruise light is ON) and OP should not engage! 358 | //Serial.println("0x545"); 359 | CAN_count++; 360 | } 361 | //if (id == 0x77F) { // Read brakepressure from 0x77F in hehtopascals 362 | if (CAN_RX_msg.id == 0x77F) { // Read brakepressure from 0x77F in hehtopascals 363 | //BRK_PRS = (CAN_RX_msg.data[7] << 6 | CAN_RX_msg.data[6] >> 2); 364 | BRK_PRS = ((CAN_RX_msg.data[7] & B00000011) << 6) | ((CAN_RX_msg.data[6] & B11111100) >> 2); 365 | //Serial.println("0x77F"); 366 | CAN_count++; 367 | } 368 | } 369 | 370 | 371 | 372 | 373 | // ******************************** OP LOGIK STUFF ********************************************** 374 | 375 | if ((BTN_CMD == RSM) && (BTN_PRS_FLG == false)) { // Engage/disengage OP if resume button press is detected, BTN_PRS_FLG prevents multiple button presses 376 | if (CC_ST_OP == false) { // If OP cruise state is false 377 | set_speed = car_speed; // When OP CC activation is set, save car_speed to set_speed 378 | CNL_REQ_CNT = 0; 379 | #ifdef DEBUG 380 | Serial.println("Speed setted"); 381 | #endif 382 | } 383 | CC_ST_OP = !CC_ST_OP; // If resume button pressed change OP cruise control state flag 384 | } 385 | 386 | if ((BTN_CMD == UP) && (currentMillis - rampTyme) > RAMP_VALUE) { // Ramp up set speed in orderly manner, 60 ms delay on incrementing set speed value so that change isn't too fast 387 | set_speed++; 388 | set_speed = min(set_speed, 135U); // Restrict max value of set_speed to 135 kph 389 | rampTyme = currentMillis; 390 | } 391 | if ((BTN_CMD == DWN) && (currentMillis - rampTyme) > RAMP_VALUE) { // Ramp down set speed in orderly manner, 60 ms delay on incrementing set speed value so that change isn't too fast 392 | set_speed--; 393 | set_speed = max(set_speed, 0U); // Restrict min value of set_speed to 0 394 | rampTyme = currentMillis; 395 | } 396 | 397 | if (BTN_CMD == RSM) { // If resume cruise button press is detected, set RSM_PRS_FLG to avoid multiple button press detections 398 | BTN_PRS_FLG = true; // This should avoid making multiple button presses at one long button press if (BTN_PRS_FLG == false) is used 399 | } 400 | else { 401 | BTN_PRS_FLG = false; 402 | } 403 | 404 | // // Option 2: Tested and worked! 405 | // if (BRK_ST == 1 && BRK_FLG == false) { // If brake pedal pressed and OP is no braking, trigger brake flag for OP to know after consective 5 msgs 406 | // if (BRK_ST_CNT > 5) { // If 0x153 interval is 10 ms, BRK_ST_OP it will be triggered after 60 ms (or is it 70 ms?) 407 | // BRK_ST_OP = 1; 408 | // BRK_ST_CNT = 0; 409 | // #ifdef DEBUG 410 | // //Serial.println("BRAKE!"); 411 | // #endif 412 | // } 413 | // } 414 | // else { 415 | // BRK_ST_OP = 0; 416 | // //BRK_ST_CNT = 0; // Maybe if this is deleted, BRK_ST_OP is triggered faster BC counter is most probably > 5 417 | // } 418 | 419 | 420 | // New OP brake signal logik based on reading OP acceleration demand and brake pedal state. NOTE TO SELF is this good logik? 421 | if ((BLTS_volt > 1500U) && (ACC_CMD > 500U)) { // 422 | BRK_ST_OP = 1; 423 | } 424 | else { 425 | BRK_ST_OP = 0; 426 | } 427 | 428 | if (errcounter == 0U) { // If 0x343 msg is received, this is triggered BC errcounter is set to 0 429 | CNL_REQ_CNT++; 430 | if (CNL_REQ_CNT > 5U) { // 0x343 interval is 10 ms so this is triggered every 60 ms (or is it 70 ms?) 431 | if ((CNL_REQ == 1U) || (OCC == 8U)) { // If OP has sent cancel request or BMW original CC is pre-enabled, set TOYOTA CC ACTIVE flag to false 432 | CC_ST_OP = false; 433 | CNL_REQ_CNT = 0; 434 | } 435 | } 436 | } 437 | 438 | 439 | // ************************************** BRAKE LOGIK ******************************************* 440 | 441 | // After braking event disconnect charge pump from main voltage before connecting it back to DSC-module 442 | if ((BRK_CMD > -30) && (BRK_FLG == true) && (RLS_FLG == false)) { // This is when we wanna go back to normal, isolate pump from 12V (relay & MOSFET) and start releasetimer 443 | // digitalWrite(PA7, LOW); // 12V to PUMP relay LOW == disconnect 444 | pwr_relay_off(); 445 | MyTim->setCaptureCompare(channel, 100, PERCENT_COMPARE_FORMAT); // Set PWM duty cycle to 0% (B- LOW) COMMENT! MAYBE disable this, could to be better for the FLYBACKING voltage to get DOWN, do it @ releasetimer 446 | timerRelease = currentMillis; // Set timer for releasing the control back to DSC after settling time (if switch is done too fast car gives DSC error) 447 | RLS_FLG = true; // Set brakingReleaseFlag to true 448 | } 449 | 450 | // If upper function was excecuted and we went back to braking territory, set BRK_FLG and RLS_FLG false so we can start braking before timerRelease (lower func) is excecuted 451 | if ((BRK_CMD < -50) && (RLS_FLG == true)) { 452 | MyTim->setCaptureCompare(channel, 0, PERCENT_COMPARE_FORMAT); 453 | BRK_FLG = false; 454 | RLS_FLG = false; 455 | } 456 | 457 | // 600 ms after the pump was disconnected from 12V, turn OFF brakelights and turn OFF DSC relays so that the pump is controlled by DSC-module 458 | //if (((currentMillis - timerRelease) > 600) && RLS_FLG == true) { // Original value 400 459 | if (((currentMillis - timerRelease) > 600U) && (RLS_FLG == true) && (BRK_PRS < 6U)) { // Test high brakepressure vs brakelight signal fix 460 | MyTim->setCaptureCompare(channel, 0, PERCENT_COMPARE_FORMAT); // Set PWM duty cycle to 0% // Is this needed to do again? 461 | relays_off(); 462 | 463 | // digitalWrite(PB13, LOW); // Set BLS LOW side OFF 464 | // digitalWrite(PB12, LOW); // Set BLS HIGH side OFF 465 | // digitalWrite(PB0, LOW); // Set DSC relays LOW 466 | // digitalWrite(PB1, LOW); // Set DSC relays LOW 467 | 468 | RLS_FLG = false; 469 | BRK_FLG = false; 470 | BRK_ST_CNT = 0; // This is needed BC otherwise BRK_ST -> BRK_ST_OP might be triggered too early 471 | #ifdef DEBUG 472 | Serial.println("Not braking"); 473 | #endif 474 | } 475 | 476 | // This is when we want to start braking 477 | if ((BRK_CMD < -50) && (BRK_FLG == false) && (fail_flag == false)) { 478 | // digitalWrite(PB0, HIGH); 479 | // digitalWrite(PB1, HIGH); 480 | 481 | // digitalWrite(PB13, HIGH); // Set BLS LOW side ON 482 | // digitalWrite(PB12, HIGH); // Set BLS HIGH side ON 483 | 484 | // digitalWrite(PA7, HIGH); // 12V to PUMP relay HIGH == connect 485 | 486 | relays_on(); 487 | BRK_FLG = true; 488 | #ifdef DEBUG 489 | Serial.println("Braking"); 490 | #endif 491 | } 492 | 493 | //if (BRK_FLG == true && RLS_FLG == false){ // This is done this way BC then upper func wont be excecuted in every loop, only this 494 | if ((BRK_FLG == true) && (RLS_FLG == false) && (errcounter == 0U)) { // This is done this way BC so that the PWM wont be done at every cycle loop but every 10 ms (change of 0x343) 495 | // Check if DSC line has drive voltage and drive the FET accordingly (this is safety feature) 496 | if (DSC_volt > DSC_THRESHOLD) { 497 | PWM = 100; 498 | } 499 | else { 500 | PWM = multiMap(BRK_CMD, cmd_in, pwm_out, 11); 501 | } 502 | MyTim->setCaptureCompare(channel, PWM, PERCENT_COMPARE_FORMAT); 503 | } 504 | 505 | 506 | // ************************************* SEND CAN STUFF ***************************************** 507 | 508 | // This is emulating TOYOTA cruise controller messages sent to OP, TOYOTA CC is used BC of legacy reasons :) 509 | CAN_msg_t CAN_TX_msg; 510 | 511 | // Send 0x1D2 msg PCM_CRUISE 512 | if ((currentMillis - previousMillis1) >= INTERVAL) { 513 | // Save the last time you send message 514 | previousMillis1 = currentMillis; 515 | 516 | CAN_TX_msg.type = DATA_FRAME; 517 | CAN_TX_msg.format = STANDARD_FORMAT; 518 | 519 | CAN_TX_msg.id = 0x1D2; 520 | CAN_TX_msg.len = 8; 521 | CAN_TX_msg.data[0] = (CC_ST_OP << 5) & 0x20U; // The wanted state of the OP cruise control (CC_ST_OP) 522 | CAN_TX_msg.data[1] = 0x0; 523 | CAN_TX_msg.data[2] = 0x0; 524 | CAN_TX_msg.data[3] = 0x0; 525 | CAN_TX_msg.data[4] = BRK_ST_OP & 0x01U; // Brake pedal state sent to OP (BRK_ST_OP) 526 | CAN_TX_msg.data[5] = 0x0; 527 | CAN_TX_msg.data[6] = (CC_ST_OP << 7) & 0x80U; // The wanted state of the OP cruise control (CC_ST_OP) 528 | CAN_TX_msg.data[7] = can_cksum(CAN_TX_msg.data, 7, 0x1D2); 529 | CANSend(&CAN_TX_msg); 530 | // This is to flush data from CAN_TX_msg struct, this should avoid that data is "transferred" to other CAN sends 531 | //memset(&CAN_TX_msg, 0, sizeof(CAN_TX_msg);) 532 | } 533 | 534 | // Send 0x1D3 msg PCM_CRUISE_2 535 | if ((currentMillis - previousMillis2) >= INTERVAL) { 536 | // Save the last time you send message 537 | previousMillis2 = currentMillis; 538 | 539 | CAN_TX_msg.id = 0x1D3; 540 | CAN_TX_msg.len = 8; 541 | CAN_TX_msg.data[0] = PWM; // Debugging msg, PWM value 542 | CAN_TX_msg.data[1] = 0xA0; // 0xA0 is for setting the pre-enable state of the TOYOTA cruise control (MAIN ON and LOW_SPEED_LOCKDOWN = 1) 543 | CAN_TX_msg.data[2] = set_speed; // Speed value sent to OP to set the cruise controller set speed 544 | CAN_TX_msg.data[3] = (BLTS_volt >> 8); // Debugging msg, BLTS wire voltage in milliVolts 545 | CAN_TX_msg.data[4] = BLTS_volt; // Debugging msg, BLTS wire voltage in milliVolts 546 | CAN_TX_msg.data[5] = (DSC_volt >> 8); // Debugging msg, DSC+ wire voltage in milliVolts 547 | CAN_TX_msg.data[6] = DSC_volt; // Debugging msg, DSC+ wire voltage in milliVolts 548 | CAN_TX_msg.data[7] = can_cksum(CAN_TX_msg.data, 7, 0x1D3); 549 | CANSend(&CAN_TX_msg); 550 | // This is to flush data from CAN_TX_msg struct, this should avoid that data is "transferred" to other CAN sends 551 | //memset(&CAN_TX_msg, 0, sizeof(CAN_TX_msg);) 552 | } 553 | 554 | 555 | // ********************************** DEBUGGING ***************************************** 556 | 557 | // Do this every 1 sec so it wont bother other stuff so much 558 | if ((currentMillis - dallasTyme) > 1000U) { 559 | #ifdef DEBUG 560 | //digitalWrite(PC13, !digitalRead(PC13)); // Chanced to own function for deBug 561 | Serial.print("ADC[0] (PA0) value is "); 562 | Serial.println(DSC_volt); 563 | Serial.print("ADC[1] (TMP36) value is "); 564 | Serial.println(ADCValues[1]); 565 | Serial.print("ADC[2] value (PA5) is "); 566 | Serial.println(ADCValues[2]); 567 | Serial.print("VREFINT is "); 568 | Serial.println(ADCValues[4]); 569 | Serial.print("INT TEMP ADC is "); 570 | Serial.println(ADCValues[5]); 571 | Serial.print("ADC watchdog status is "); 572 | Serial.println(ubAnalogWatchdogStatus); 573 | Serial.print("Brake pressure is "); 574 | Serial.println(BRK_PRS); 575 | Serial.print("BRK_CMD is "); 576 | Serial.println(BRK_CMD); 577 | Serial.println(PWM); 578 | Serial.print("Counter is "); 579 | Serial.println(counter); 580 | Serial.print("errCounter is "); 581 | Serial.println(errcounter); 582 | Serial.print("fail_flag is "); 583 | Serial.println(fail_flag); 584 | Serial.print("TMP36 TEMP is "); 585 | Serial.println(TMP36); 586 | Serial.print("Vsense is "); 587 | Serial.println(iVsense); 588 | Serial.print("STM32 chip temp is "); 589 | Serial.println(iSTMtemp); 590 | #endif 591 | 592 | dallasTyme = currentMillis; 593 | } 594 | 595 | // Do blinkyLed stuff for debugging 596 | if ((currentMillis - blinkyTyme) > ledInterval) { 597 | digitalWrite(PC13, !digitalRead(PC13)); 598 | blinkyTyme = currentMillis; 599 | } 600 | 601 | // Failure detected, make blinkyLed blink more faster 602 | if (fail_flag == true) { 603 | ledInterval = 200; 604 | } 605 | else { 606 | ledInterval = 2000; 607 | } 608 | 609 | // ********************************** TEMPERATURE MONITORING n CONTROL ************************************** 610 | 611 | // This gives POWER MOSFET temperature in desicelsius 612 | TMP36 = (((ADCValues[1] * 3300) / 4096) - 500); 613 | 614 | // This gives the internal temperature of the STM32 chip 615 | // Vsense = (1.2 * ADCValues[5]) / ADCValues[4]; 616 | // STMtemp = ((1.43 - Vsense ) / 0.0043) + 50; 617 | 618 | // This is ~4 times faster than above 619 | iVsense = (120000 * ADCValues[5]) / ADCValues[4]; 620 | iSTMtemp = ((143000 - iVsense ) / 43) + 500; 621 | 622 | #ifdef FAN_CTRL 623 | if ((TMP36 > 450) && !FAN_ON) { 624 | digitalWrite(PB14, HIGH); // Turn FAN pin HIGH when FET temp over 45 degrees 625 | FAN_ON = 1; 626 | } 627 | if ((TMP36 < 450) && FAN_ON) { 628 | digitalWrite(PB14, LOW);; // Turn FAN pin LOW when FET temp under 45 degrees 629 | FAN_ON = 0; 630 | } 631 | #endif 632 | 633 | if (TMP36 > 800 || iSTMtemp > 500) { // If FET temp over 80 degrees or STM32 internal temp over 50, disable OP 634 | fail_flag = true; 635 | } 636 | 637 | 638 | if ((errcounter > 65000U) || (fail_flag == true)) { // errcounter func is triggered every (65000 x ~9us =) 585 ms if no 0x343 msg is detected which should arrive every 10 ms 639 | CC_ST_OP = false; 640 | CNL_REQ = 1; 641 | errcounter = 0; 642 | // Make this tstuff so that it will disconnect properly, and does not cause DTC 643 | MyTim->setCaptureCompare(channel, 0, PERCENT_COMPARE_FORMAT); 644 | digitalWrite(PA7, LOW); // 12V to PUMP relay HIGH == connect 645 | digitalWrite(PB0, LOW); 646 | digitalWrite(PB1, LOW); 647 | digitalWrite(PB13, LOW); // Set BLS LOW side OFF 648 | digitalWrite(PB12, LOW); // Set BLS HIGH side OFF 649 | } 650 | 651 | errcounter++; // Add errcounter value, this value is set to zero allways when 0x343 is detected 652 | counter++; // Add counter value, this is for counting loops for measuring average looptyme 653 | ubAnalogWatchdogStatus = RESET; // This will set WatchdogStatus to zero when analogvalues are inside bounds 654 | 655 | // make sure the code in this loop is executed in 656 | // less than 4 milliseconds to leave 50% headroom for 657 | // the timer reload. 658 | IWatchdog.reload(); 659 | } 660 | 661 | // ******************************************************************************************************* 662 | // ***************************************** END of LOOPYDOO ********************************************* 663 | // ******************************************************************************************************* 664 | 665 | // ******************************************** FUNCTIONS ************************************************ 666 | 667 | // TOYOTA CAN CHECKSUM 668 | // I think this is written by wocsor, same as the TOYOTA cruise CAN send stuff 669 | int can_cksum (uint8_t *dat, uint8_t len, uint16_t addr) { 670 | uint8_t checksum = 0; 671 | checksum = ((addr & 0xFF00U) >> 8) + (addr & 0x00FFU) + len + 1U; 672 | for (uint8_t i = 0; i < len; i++) { 673 | checksum += (dat[i]); 674 | } 675 | return checksum; 676 | } 677 | 678 | 679 | // Interpolation shit 680 | // This is direct copy from https://github.com/RobTillaart/MultiMap 681 | // note: the _in array should have increasing values 682 | int multiMap(int val, int* in, int* out, uint8_t size) 683 | { 684 | // take care the value is within range 685 | // val = constrain(val, _in[0], _in[size-1]); 686 | if (val <= in[0]) 687 | { 688 | return out[0]; 689 | } 690 | 691 | if (val >= in[size-1U]) 692 | { 693 | return out[size-1U]; 694 | } 695 | 696 | // search right interval 697 | uint8_t pos = 1; // _in[0] allready tested 698 | while(val > in[pos]) 699 | { 700 | pos++; 701 | } 702 | 703 | // this will handle all exact "points" in the _in array 704 | if (val == in[pos]) 705 | { 706 | return out[pos]; 707 | } 708 | 709 | // interpolate in the right segment for the rest 710 | return (val - in[pos-1U]) * (out[pos] - out[pos-1U]) / (in[pos] - in[pos-1U]) + out[pos-1U]; 711 | } 712 | 713 | 714 | // Use faster CMISS defines in pin state changes, using BSRR instead of ODR and BRR u can use atomic instead of or configuration (faster) 715 | // https://www.youtube.com/watch?v=CyfaDirhp9M 716 | // BrakeModule relays state ON 717 | void relays_on() 718 | { 719 | // PMP- relay to ON (PB0) 720 | // PMP+ relay to ON (PB1) 721 | // Set BLS LOW side ON (PB13) 722 | // Set BLS HIGH side ON (PB12) 723 | // Set pins 0, 1, 12, 13 HIGH on bit set/reset register on port GPIOB 724 | GPIOB -> BSRR = GPIO_BSRR_BS0 | GPIO_BSRR_BS1 | GPIO_BSRR_BS12 | GPIO_BSRR_BS13; 725 | 726 | // 12V to PUMP relay (PA7) 727 | // Set pin 7 HIGH on bit set/reset register on port GPIOA 728 | GPIOA -> BSRR = GPIO_BSRR_BS7; 729 | } 730 | 731 | 732 | // BrakeModule 12V relay to charge pump OFF 733 | void pwr_relay_off() 734 | { 735 | // 12V to PUMP relay (PA7) 736 | // Set pin 7 LOW on bit set/reset register on port GPIOA 737 | GPIOA -> BSRR = GPIO_BSRR_BR7; 738 | } 739 | 740 | // BrakeModule 12V relay to charge pump OFF 741 | void relays_off() 742 | { 743 | // Set pins 0, 1, 12, 13 LOW on bit set/reset register on port GPIOB 744 | GPIOB -> BSRR = GPIO_BSRR_BR0 | GPIO_BSRR_BR1 | GPIO_BSRR_BR12 | GPIO_BSRR_BR13; 745 | } 746 | 747 | // Read pin OUTPUT states from input data register IDR (STM32F1 RM0008 p. 163, 172) 748 | bool read_output_states() 749 | { 750 | /* read PC13 */ 751 | //if(GPIOC -> IDR & GPIO_PIN_13) 752 | if((GPIOB -> IDR & (GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_12 | GPIO_PIN_13)) && (GPIOA -> IDR & GPIO_PIN_7)) 753 | { 754 | // Stuff 755 | return true; 756 | } 757 | return false; 758 | } 759 | 760 | // LOCK pin configuration (STM32F1 RM0008 p. 174) 761 | // Once the pin configuration is locked, it cannot be changed until the next reset. 762 | // This can help prevent accidental or malicious changes to the pin configuration, which could cause your application to malfunction. 763 | void port_lock_conf() 764 | { 765 | /* LOCK pin configuration of PA7, PB0, PB1, PB12, P13 */ 766 | HAL_GPIO_LockPin(GPIOA, GPIO_PIN_7); 767 | HAL_GPIO_LockPin(GPIOB, GPIO_PIN_0); 768 | HAL_GPIO_LockPin(GPIOB, GPIO_PIN_1); 769 | HAL_GPIO_LockPin(GPIOB, GPIO_PIN_12); 770 | HAL_GPIO_LockPin(GPIOB, GPIO_PIN_13); 771 | 772 | /* LOCK pin configuration of PA0, PA1, PA5, PA6 */ 773 | HAL_GPIO_LockPin(GPIOA, GPIO_PIN_0); 774 | HAL_GPIO_LockPin(GPIOA, GPIO_PIN_1); 775 | HAL_GPIO_LockPin(GPIOA, GPIO_PIN_5); 776 | HAL_GPIO_LockPin(GPIOA, GPIO_PIN_6); 777 | } 778 | 779 | // Read STM32F103 internal temperature 780 | int read_STM_temp(void) 781 | { 782 | float Vsense; 783 | float iTemp; 784 | float vdd; 785 | vdd = 1.20 * (4096.0 / ADCValues[3]); // Is this input voltage compared to 1.2 reference voltage 786 | Vsense = vdd/4096.0 * ADCValues[4]; 787 | iTemp = ((1.43 - Vsense ) / 0.0043) + 50; 788 | return iTemp; 789 | } 790 | 791 | // Serial read function for DEBUGGING 792 | void readSerial() 793 | { 794 | // if there's any serial available, read it: 795 | while (Serial.available() > 0) { 796 | // look for the next valid integer in the incoming serial stream: 797 | int16_t input = Serial.parseInt(); 798 | 799 | // look for the newline. That's the end of your sentence: 800 | if (Serial.read() == '\n') { 801 | Serial.print("Input is: "); 802 | Serial.println(input); 803 | 804 | if ((input > 1) && (input < 101)) { 805 | MyTim->setCaptureCompare(channel, input, PERCENT_COMPARE_FORMAT); 806 | Serial.print("PWM input is "); 807 | Serial.println(input); 808 | Serial.println(); 809 | 810 | } 811 | else if (input == 101) { 812 | Serial.println(); 813 | Serial.print("CAN count was "); 814 | Serial.println(CAN_count); 815 | Serial.println(); 816 | CAN_count = 0; 817 | } 818 | else if (input == 200) { 819 | digitalWrite(PA7, LOW); // 12V to PUMP relay HIGH == connect 820 | digitalWrite(PB0, LOW); // PMP+ 821 | digitalWrite(PB1, LOW); // PMP- 822 | digitalWrite(PB13, LOW); // Set BLS LOW side OFF 823 | digitalWrite(PB12, LOW); // Set BLS HIGH side OFF 824 | Serial.println("Braking mode was disabled"); 825 | } 826 | else if (input == 201) { 827 | digitalWrite(PB0, HIGH); // PMP+ 828 | digitalWrite(PB1, HIGH); // PMP- 829 | digitalWrite(PB13, HIGH); // Set BLS LOW side OFF 830 | digitalWrite(PB12, HIGH); // Set BLS HIGH side OFF 831 | digitalWrite(PA7, HIGH); // 12V to PUMP relay HIGH == connect 832 | Serial.println("Braking mode was enabled"); 833 | } 834 | else if (input == 666) { 835 | digitalWrite(PB13, !digitalRead(PB13)); 836 | Serial.println("BLS_LOW pin was toggled"); 837 | } 838 | else if (input == 667) { 839 | digitalWrite(PB12, !digitalRead(PB12)); 840 | Serial.println("BLS_HIGH was toggled"); 841 | } 842 | else if (input == 668) { 843 | digitalWrite(PB1, !digitalRead(PB1)); 844 | Serial.println("PMP- pin was toggled"); 845 | } 846 | else if (input == 669) { 847 | digitalWrite(PB14, !digitalRead(PB14)); 848 | Serial.println("FAN pin was toggled"); 849 | } 850 | else if (input == 700) { 851 | digitalWrite(PB0, !digitalRead(PB0)); 852 | Serial.println("PMP+ pin was toggled"); 853 | } 854 | else if (input == 701) { 855 | digitalWrite(PA7, !digitalRead(PA7)); 856 | Serial.println("12V pin was toggled"); 857 | } 858 | else if (input == 702) { 859 | digitalWrite(PA7, HIGH); 860 | digitalWrite(PB0, HIGH); 861 | digitalWrite(PB1, HIGH); 862 | Serial.println("Brake sequence is ON"); 863 | } 864 | else if (input == 703) { 865 | digitalWrite(PA7, LOW); 866 | digitalWrite(PB0, LOW); 867 | digitalWrite(PB1, LOW); 868 | Serial.println("Brake sequence is OFF"); 869 | } 870 | else if (input < -10) { 871 | BRK_CMD = input; 872 | Serial.print("BRK_CMD has been set to value "); 873 | Serial.println(BRK_CMD); 874 | } 875 | else{ 876 | MyTim->setCaptureCompare(channel, 0, PERCENT_COMPARE_FORMAT); 877 | } 878 | } 879 | } 880 | } 881 | -------------------------------------------------------------------------------- /BrakeModule_sw03/STM32F1_CAN.h: -------------------------------------------------------------------------------- 1 | 2 | // Original code from https://github.com/nopnop2002/Arduino-STM32-CAN/tree/master/stm32f103 3 | // Code updated on 12.12.2022 4 | 5 | 6 | #include 7 | 8 | #define DEBUG 1 9 | 10 | bool fail_flag = false; // Flag for any failure that would prevent OPENPILOT to engage and BrakeModule to turn ON 11 | 12 | /* Symbolic names for bit rate of CAN message */ 13 | typedef enum {CAN_50KBPS, CAN_100KBPS, CAN_125KBPS, CAN_250KBPS, CAN_500KBPS, CAN_1000KBPS} BITRATE; 14 | 15 | /* Real speed for bit rate of CAN message */ 16 | uint32_t SPEED[6] = {50*1000, 100*1000, 125*1000, 250*1000, 500*1000, 1000*1000}; 17 | 18 | /* Symbolic names for formats of CAN message */ 19 | typedef enum {STANDARD_FORMAT = 0, EXTENDED_FORMAT} CAN_FORMAT; 20 | 21 | /* Symbolic names for type of CAN message */ 22 | typedef enum {DATA_FRAME = 0, REMOTE_FRAME} CAN_FRAME; 23 | 24 | 25 | typedef struct 26 | { 27 | uint32_t id; /* 29 bit identifier */ 28 | uint8_t data[8]; /* Data field */ 29 | uint8_t len; /* Length of data field in bytes */ 30 | uint8_t ch; /* Object channel(Not use) */ 31 | uint8_t format; /* 0 - STANDARD, 1- EXTENDED IDENTIFIER */ 32 | uint8_t type; /* 0 - DATA FRAME, 1 - REMOTE FRAME */ 33 | } CAN_msg_t; 34 | 35 | typedef struct 36 | { 37 | uint16_t baud_rate_prescaler; /// [1 to 1024] 38 | uint8_t time_segment_1; /// [1 to 16] 39 | uint8_t time_segment_2; /// [1 to 8] 40 | uint8_t resynchronization_jump_width; /// [1 to 4] (recommended value is 1) 41 | } CAN_bit_timing_config_t; 42 | 43 | #define CAN_STM32_ERROR_UNSUPPORTED_BIT_RATE 1000 44 | #define CAN_STM32_ERROR_MSR_INAK_NOT_SET 1001 45 | #define CAN_STM32_ERROR_MSR_INAK_NOT_CLEARED 1002 46 | #define CAN_STM32_ERROR_UNSUPPORTED_FRAME_FORMAT 1003 47 | 48 | /* 49 | * Calculation of bit timing dependent on peripheral clock rate 50 | */ 51 | int16_t ComputeCANTimings(const uint32_t peripheral_clock_rate, 52 | const uint32_t target_bitrate, 53 | CAN_bit_timing_config_t* const out_timings) 54 | { 55 | if (target_bitrate < 1000U) 56 | { 57 | return -CAN_STM32_ERROR_UNSUPPORTED_BIT_RATE; 58 | } 59 | 60 | assert(out_timings != NULL); // NOLINT 61 | memset(out_timings, 0, sizeof(*out_timings)); 62 | 63 | /* 64 | * Hardware configuration 65 | */ 66 | static const uint8_t MaxBS1 = 16; 67 | static const uint8_t MaxBS2 = 8; 68 | 69 | /* 70 | * Ref. "Automatic Baudrate Detection in CANopen Networks", U. Koppe, MicroControl GmbH & Co. KG 71 | * CAN in Automation, 2003 72 | * 73 | * According to the source, optimal quanta per bit are: 74 | * Bitrate Optimal Maximum 75 | * 1000 kbps 8 10 76 | * 500 kbps 16 17 77 | * 250 kbps 16 17 78 | * 125 kbps 16 17 79 | */ 80 | const uint8_t max_quanta_per_bit = (uint8_t)((target_bitrate >= 1000000U) ? 10U : 17U); // NOLINT 81 | assert(max_quanta_per_bit <= (MaxBS1 + MaxBS2)); 82 | 83 | static const uint16_t MaxSamplePointLocationPermill = 900; 84 | 85 | /* 86 | * Computing (prescaler * BS): 87 | * BITRATE = 1 / (PRESCALER * (1 / PCLK) * (1 + BS1 + BS2)) -- See the Reference Manual 88 | * BITRATE = PCLK / (PRESCALER * (1 + BS1 + BS2)) -- Simplified 89 | * let: 90 | * BS = 1 + BS1 + BS2 -- Number of time quanta per bit 91 | * PRESCALER_BS = PRESCALER * BS 92 | * ==> 93 | * PRESCALER_BS = PCLK / BITRATE 94 | */ 95 | const uint32_t prescaler_bs = peripheral_clock_rate / target_bitrate; 96 | 97 | /* 98 | * Searching for such prescaler value so that the number of quanta per bit is highest. 99 | */ 100 | uint8_t bs1_bs2_sum = (uint8_t)(max_quanta_per_bit - 1U); // NOLINT 101 | 102 | while ((prescaler_bs % (1U + bs1_bs2_sum)) != 0U) 103 | { 104 | if (bs1_bs2_sum <= 2U) 105 | { 106 | return -CAN_STM32_ERROR_UNSUPPORTED_BIT_RATE; // No solution 107 | } 108 | bs1_bs2_sum--; 109 | } 110 | 111 | const uint32_t prescaler = prescaler_bs / (1U + bs1_bs2_sum); 112 | if ((prescaler < 1U) || (prescaler > 1024U)) 113 | { 114 | return -CAN_STM32_ERROR_UNSUPPORTED_BIT_RATE; // No solution 115 | } 116 | 117 | /* 118 | * Now we have a constraint: (BS1 + BS2) == bs1_bs2_sum. 119 | * We need to find such values so that the sample point is as close as possible to the optimal value, 120 | * which is 87.5%, which is 7/8. 121 | * 122 | * Solve[(1 + bs1)/(1 + bs1 + bs2) == 7/8, bs2] (* Where 7/8 is 0.875, the recommended sample point location *) 123 | * {{bs2 -> (1 + bs1)/7}} 124 | * 125 | * Hence: 126 | * bs2 = (1 + bs1) / 7 127 | * bs1 = (7 * bs1_bs2_sum - 1) / 8 128 | * 129 | * Sample point location can be computed as follows: 130 | * Sample point location = (1 + bs1) / (1 + bs1 + bs2) 131 | * 132 | * Since the optimal solution is so close to the maximum, we prepare two solutions, and then pick the best one: 133 | * - With rounding to nearest 134 | * - With rounding to zero 135 | */ 136 | uint8_t bs1 = (uint8_t)(((7U * bs1_bs2_sum - 1U) + 4U) / 8U); // Trying rounding to nearest first // NOLINT 137 | uint8_t bs2 = (uint8_t)(bs1_bs2_sum - bs1); // NOLINT 138 | assert(bs1_bs2_sum > bs1); 139 | 140 | { 141 | const uint16_t sample_point_permill = (uint16_t)(1000U * (1U + bs1) / (1U + bs1 + bs2)); // NOLINT 142 | 143 | if (sample_point_permill > MaxSamplePointLocationPermill) // Strictly more! 144 | { 145 | bs1 = (uint8_t)((7U * bs1_bs2_sum - 1U) / 8U); // Nope, too far; now rounding to zero 146 | bs2 = (uint8_t)(bs1_bs2_sum - bs1); 147 | } 148 | } 149 | 150 | const bool valid = (bs1 >= 1U) && (bs1 <= MaxBS1) && (bs2 >= 1U) && (bs2 <= MaxBS2); 151 | 152 | /* 153 | * Final validation 154 | * Helpful Python: 155 | * def sample_point_from_btr(x): 156 | * assert 0b0011110010000000111111000000000 & x == 0 157 | * ts2,ts1,brp = (x>>20)&7, (x>>16)&15, x&511 158 | * return (1+ts1+1)/(1+ts1+1+ts2+1) 159 | */ 160 | if ((target_bitrate != (peripheral_clock_rate / (prescaler * (1U + bs1 + bs2)))) || 161 | !valid) 162 | { 163 | // This actually means that the algorithm has a logic error, hence assert(0). 164 | assert(0); // NOLINT 165 | return -CAN_STM32_ERROR_UNSUPPORTED_BIT_RATE; 166 | } 167 | 168 | out_timings->baud_rate_prescaler = (uint16_t) prescaler; 169 | out_timings->resynchronization_jump_width = 1; // One is recommended by UAVCAN, CANOpen, and DeviceNet 170 | out_timings->time_segment_1 = bs1; 171 | out_timings->time_segment_2 = bs2; 172 | 173 | if (DEBUG) { 174 | Serial.print("target_bitrate="); 175 | Serial.println(target_bitrate); 176 | Serial.print("peripheral_clock_rate="); 177 | Serial.println(peripheral_clock_rate); 178 | 179 | Serial.print("timings.baud_rate_prescaler="); 180 | Serial.println(out_timings->baud_rate_prescaler); 181 | Serial.print("timings.time_segment_1="); 182 | Serial.println(out_timings->time_segment_1); 183 | Serial.print("timings.time_segment_2="); 184 | Serial.println(out_timings->time_segment_2); 185 | Serial.print("timings.resynchronization_jump_width="); 186 | Serial.println(out_timings->resynchronization_jump_width); 187 | } 188 | return 0; 189 | } 190 | 191 | /** 192 | * Print registers. 193 | */ 194 | void printRegister(const char * buf, uint32_t reg) { 195 | if (DEBUG == 0) return; 196 | Serial.print(buf); 197 | Serial.print(reg, HEX); 198 | Serial.println(); 199 | } 200 | 201 | /** 202 | * Initializes the CAN filter registers. 203 | * 204 | * @preconditions - This register can be written only when the filter initialization mode is set (FINIT=1) in the CAN_FMR register. 205 | * @params: index - Specified filter index. index 27:14 are available in connectivity line devices only. 206 | * @params: scale - Select filter scale. 207 | * 0: Dual 16-bit scale configuration 208 | * 1: Single 32-bit scale configuration 209 | * @params: mode - Select filter mode. 210 | * 0: Two 32-bit registers of filter bank x are in Identifier Mask mode 211 | * 1: Two 32-bit registers of filter bank x are in Identifier List mode 212 | * @params: fifo - Select filter assigned. 213 | * 0: Filter assigned to FIFO 0 214 | * 1: Filter assigned to FIFO 1 215 | * @params: bank1 - Filter bank register 1 216 | * @params: bank2 - Filter bank register 2 217 | * 218 | */ 219 | void CANSetFilter(uint8_t index, uint8_t scale, uint8_t mode, uint8_t fifo, uint32_t bank1, uint32_t bank2) { 220 | if (index > 27U) return; 221 | 222 | CAN1->FA1R &= ~(0x1UL<FS1R &= ~(0x1UL<FS1R |= (0x1UL<FM1R &= ~(0x1UL<FM1R |= (0x1UL<FFA1R &= ~(0x1UL<FFA1R |= (0x1UL<sFilterRegister[index].FR1 = bank1; // Set filter bank registers1 242 | CAN1->sFilterRegister[index].FR2 = bank2; // Set filter bank registers2 243 | 244 | CAN1->FA1R |= (0x1UL<APB1ENR |= 0x2000000UL; // Enable CAN clock 265 | RCC->APB2ENR |= 0x1UL; // Enable AFIO clock 266 | AFIO->MAPR &= 0xFFFF9FFF; // reset CAN remap 267 | // CAN_RX mapped to PA11, CAN_TX mapped to PA12 268 | 269 | if (remap == 0) { 270 | RCC->APB2ENR |= 0x4UL; // Enable GPIOA clock 271 | GPIOA->CRH &= ~(0xFF000UL); // Configure PA12(0b0000) and PA11(0b0000) 272 | // 0b0000 273 | // MODE=00(Input mode) 274 | // CNF=00(Analog mode) 275 | 276 | GPIOA->CRH |= 0xB8FFFUL; // Configure PA12(0b1011) and PA11(0b1000) 277 | // 0b1011 278 | // MODE=11(Output mode, max speed 50 MHz) 279 | // CNF=10(Alternate function output Push-pull 280 | // 0b1000 281 | // MODE=00(Input mode) 282 | // CNF=10(Input with pull-up / pull-down) 283 | 284 | GPIOA->ODR |= 0x1UL << 12; // PA12 Upll-up 285 | 286 | } 287 | 288 | if (remap == 2) { 289 | AFIO->MAPR |= 0x00004000; // set CAN remap 290 | // CAN_RX mapped to PB8, CAN_TX mapped to PB9 (not available on 36-pin package) 291 | 292 | RCC->APB2ENR |= 0x8UL; // Enable GPIOB clock 293 | GPIOB->CRH &= ~(0xFFUL); // Configure PB9(0b0000) and PB8(0b0000) 294 | // 0b0000 295 | // MODE=00(Input mode) 296 | // CNF=00(Analog mode) 297 | 298 | GPIOB->CRH |= 0xB8UL; // Configure PB9(0b1011) and PB8(0b1000) 299 | // 0b1011 300 | // MODE=11(Output mode, max speed 50 MHz) 301 | // CNF=10(Alternate function output Push-pull 302 | // 0b1000 303 | // MODE=00(Input mode) 304 | // CNF=10(Input with pull-up / pull-down) 305 | 306 | GPIOB->ODR |= 0x1UL << 8; // PB8 Upll-up 307 | } 308 | 309 | if (remap == 3) { 310 | AFIO->MAPR |= 0x00005000; // set CAN remap 311 | // CAN_RX mapped to PD0, CAN_TX mapped to PD1 (available on 100-pin and 144-pin package) 312 | 313 | RCC->APB2ENR |= 0x20UL; // Enable GPIOD clock 314 | GPIOD->CRL &= ~(0xFFUL); // Configure PD1(0b0000) and PD0(0b0000) 315 | // 0b0000 316 | // MODE=00(Input mode) 317 | // CNF=00(Analog mode) 318 | 319 | GPIOD->CRH |= 0xB8UL; // Configure PD1(0b1011) and PD0(0b1000) 320 | // 0b1000 321 | // MODE=00(Input mode) 322 | // CNF=10(Input with pull-up / pull-down) 323 | // 0b1011 324 | // MODE=11(Output mode, max speed 50 MHz) 325 | // CNF=10(Alternate function output Push-pull 326 | 327 | GPIOD->ODR |= 0x1UL << 0; // PD0 Upll-up 328 | } 329 | 330 | CAN1->MCR |= 0x1UL; // Require CAN1 to Initialization mode 331 | while (!(CAN1->MSR & 0x1UL)); // Wait for Initialization mode 332 | 333 | //CAN1->MCR = 0x51UL; // Hardware initialization(No automatic retransmission) 334 | CAN1->MCR = 0x41UL; // Hardware initialization(With automatic retransmission) 335 | 336 | // Set bit timing register 337 | CAN_bit_timing_config_t timings; 338 | Serial.print("bitrate="); 339 | Serial.println(bitrate); 340 | uint32_t target_bitrate = SPEED[bitrate]; 341 | Serial.print("target_bitrate="); 342 | Serial.println(target_bitrate); 343 | int result = ComputeCANTimings(HAL_RCC_GetPCLK1Freq(), target_bitrate, &timings); 344 | Serial.print("ComputeCANTimings result="); 345 | Serial.println(result); 346 | if (result != 0) while(true); 347 | CAN1->BTR = (((timings.resynchronization_jump_width - 1U) & 3U) << 24U) | 348 | (((timings.time_segment_1 - 1U) & 15U) << 16U) | 349 | (((timings.time_segment_2 - 1U) & 7U) << 20U) | 350 | ((timings.baud_rate_prescaler - 1U) & 1023U); 351 | 352 | // Configure Filters to default values 353 | CAN1->FMR |= 0x1UL; // Set to filter initialization mode 354 | CAN1->FMR &= 0xFFFFC0FF; // Clear CAN2 start bank 355 | 356 | // bxCAN has 28 filters. 357 | // These filters are shared by both CAN1 and CAN2. 358 | // STM32F103 has only CAN1, so all 28 are used for CAN1 359 | CAN1->FMR |= 0x1CU << 8; // Assign all filters to CAN1 360 | 361 | // Set fileter 0 362 | // Single 32-bit scale configuration 363 | // Two 32-bit registers of filter bank x are in Identifier Mask mode 364 | // Filter assigned to FIFO 0 365 | // Filter bank register to all 0 366 | CANSetFilter(0, 1, 0, 0, 0x0UL, 0x0UL); 367 | 368 | CAN1->FMR &= ~(0x1UL); // Deactivate initialization mode 369 | 370 | uint16_t TimeoutMilliseconds = 1000; 371 | bool can1 = false; 372 | CAN1->MCR &= ~(0x1UL); // Require CAN1 to normal mode 373 | 374 | // Wait for normal mode 375 | // If the connection is not correct, it will not return to normal mode. 376 | for (uint16_t wait_ack = 0; wait_ack < TimeoutMilliseconds; wait_ack++) { 377 | if ((CAN1->MSR & 0x1UL) == 0) { 378 | can1 = true; 379 | break; 380 | } 381 | delayMicroseconds(1000); 382 | } 383 | //Serial.print("can1="); 384 | //Serial.println(can1); 385 | if (can1) { 386 | Serial.println("CAN1 initialize ok"); 387 | } else { 388 | Serial.println("CAN1 initialize fail!!"); 389 | return false; 390 | } 391 | return true; 392 | } 393 | 394 | 395 | #define STM32_CAN_TIR_TXRQ (1U << 0U) // Bit 0: Transmit Mailbox Request 396 | #define STM32_CAN_RIR_RTR (1U << 1U) // Bit 1: Remote Transmission Request 397 | #define STM32_CAN_RIR_IDE (1U << 2U) // Bit 2: Identifier Extension 398 | #define STM32_CAN_TIR_RTR (1U << 1U) // Bit 1: Remote Transmission Request 399 | #define STM32_CAN_TIR_IDE (1U << 2U) // Bit 2: Identifier Extension 400 | 401 | #define CAN_EXT_ID_MASK 0x1FFFFFFFU 402 | #define CAN_STD_ID_MASK 0x000007FFU 403 | 404 | /** 405 | * Decodes CAN messages from the data registers and populates a 406 | * CAN message struct with the data fields. 407 | * 408 | * @preconditions A valid CAN message is received 409 | * @params CAN_rx_msg - CAN message structure for reception 410 | * 411 | */ 412 | void CANReceive(CAN_msg_t* CAN_rx_msg) 413 | { 414 | uint32_t id = CAN1->sFIFOMailBox[0].RIR; 415 | if ((id & STM32_CAN_RIR_IDE) == 0U) { // Standard frame format 416 | CAN_rx_msg->format = STANDARD_FORMAT;; 417 | CAN_rx_msg->id = (CAN_STD_ID_MASK & (id >> 21U)); 418 | } 419 | else { // Extended frame format 420 | CAN_rx_msg->format = EXTENDED_FORMAT;; 421 | CAN_rx_msg->id = (CAN_EXT_ID_MASK & (id >> 3U)); 422 | } 423 | 424 | if ((id & STM32_CAN_RIR_RTR) == 0U) { // Data frame 425 | CAN_rx_msg->type = DATA_FRAME; 426 | } 427 | else { // Remote frame 428 | CAN_rx_msg->type = REMOTE_FRAME; 429 | } 430 | 431 | 432 | CAN_rx_msg->len = (CAN1->sFIFOMailBox[0].RDTR) & 0xFUL; 433 | 434 | CAN_rx_msg->data[0] = 0xFFUL & CAN1->sFIFOMailBox[0].RDLR; 435 | CAN_rx_msg->data[1] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDLR >> 8); 436 | CAN_rx_msg->data[2] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDLR >> 16); 437 | CAN_rx_msg->data[3] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDLR >> 24); 438 | CAN_rx_msg->data[4] = 0xFFUL & CAN1->sFIFOMailBox[0].RDHR; 439 | CAN_rx_msg->data[5] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDHR >> 8); 440 | CAN_rx_msg->data[6] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDHR >> 16); 441 | CAN_rx_msg->data[7] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDHR >> 24); 442 | 443 | // Release FIFO 0 output mailbox. 444 | // Make the next incoming message available. 445 | CAN1->RF0R |= 0x20UL; 446 | } 447 | 448 | /** 449 | * Encodes CAN messages using the CAN message struct and populates the 450 | * data registers with the sent. 451 | * 452 | * @params CAN_tx_msg - CAN message structure for transmission 453 | * 454 | */ 455 | void CANSend(CAN_msg_t* CAN_tx_msg) 456 | { 457 | volatile int count = 0; 458 | 459 | uint32_t out = 0; 460 | if (CAN_tx_msg->format == EXTENDED_FORMAT) { // Extended frame format 461 | out = ((CAN_tx_msg->id & CAN_EXT_ID_MASK) << 3U) | STM32_CAN_TIR_IDE; 462 | } 463 | else { // Standard frame format 464 | out = ((CAN_tx_msg->id & CAN_STD_ID_MASK) << 21U); 465 | } 466 | 467 | // Remote frame 468 | if (CAN_tx_msg->type == REMOTE_FRAME) { 469 | out |= STM32_CAN_TIR_RTR; 470 | } 471 | 472 | CAN1->sTxMailBox[0].TDTR &= ~(0xF); 473 | CAN1->sTxMailBox[0].TDTR |= CAN_tx_msg->len & 0xFUL; 474 | 475 | CAN1->sTxMailBox[0].TDLR = (((uint32_t) CAN_tx_msg->data[3] << 24) | 476 | ((uint32_t) CAN_tx_msg->data[2] << 16) | 477 | ((uint32_t) CAN_tx_msg->data[1] << 8) | 478 | ((uint32_t) CAN_tx_msg->data[0] )); 479 | CAN1->sTxMailBox[0].TDHR = (((uint32_t) CAN_tx_msg->data[7] << 24) | 480 | ((uint32_t) CAN_tx_msg->data[6] << 16) | 481 | ((uint32_t) CAN_tx_msg->data[5] << 8) | 482 | ((uint32_t) CAN_tx_msg->data[4] )); 483 | 484 | // Send Go 485 | CAN1->sTxMailBox[0].TIR = out | STM32_CAN_TIR_TXRQ; 486 | 487 | // Wait until the mailbox is empty 488 | //while(((CAN1->sTxMailBox[0].TIR & 0x1UL) != 0) && (count++ < 1000000)); 489 | // Test if this work like above and solves the MISRA2012:13.5 violation 490 | while(((CAN1->sTxMailBox[0].TIR & 0x1UL) != 0) && (count < 1000000)) { 491 | count++; 492 | } 493 | 494 | // The mailbox don't becomes empty while loop 495 | //if (CAN1->sTxMailBox[0].TIR & 0x1UL) { 496 | if ((CAN1->sTxMailBox[0].TIR & 0x1UL) != 0U) { 497 | Serial.println("Send Fail"); 498 | Serial.println(CAN1->ESR); 499 | Serial.println(CAN1->MSR); 500 | Serial.println(CAN1->TSR); 501 | fail_flag = true; 502 | } 503 | 504 | } 505 | 506 | /** 507 | * Returns whether there are CAN messages available. 508 | * 509 | * @returns If pending CAN messages are in the CAN controller 510 | * 511 | */ 512 | uint8_t CANMsgAvail(void) 513 | { 514 | // Check for pending FIFO 0 messages 515 | return CAN1->RF0R & 0x3UL; 516 | } 517 | -------------------------------------------------------------------------------- /BrakeModule_sw03/build_opt.h: -------------------------------------------------------------------------------- 1 | -DHAL_ADC_MODULE_ONLY 2 | -------------------------------------------------------------------------------- /Pics/20220817_162326.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/killinen/BrakeModule/52e397301addb98a9d2e167176d838cb5e5ef9ed/Pics/20220817_162326.jpg -------------------------------------------------------------------------------- /Pics/ABSunit.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/killinen/BrakeModule/52e397301addb98a9d2e167176d838cb5e5ef9ed/Pics/ABSunit.PNG -------------------------------------------------------------------------------- /Pics/BLS_logik.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/killinen/BrakeModule/52e397301addb98a9d2e167176d838cb5e5ef9ed/Pics/BLS_logik.PNG -------------------------------------------------------------------------------- /Pics/BM.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/killinen/BrakeModule/52e397301addb98a9d2e167176d838cb5e5ef9ed/Pics/BM.jpg -------------------------------------------------------------------------------- /Pics/BM0.2.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/killinen/BrakeModule/52e397301addb98a9d2e167176d838cb5e5ef9ed/Pics/BM0.2.PNG -------------------------------------------------------------------------------- /Pics/BM03.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/killinen/BrakeModule/52e397301addb98a9d2e167176d838cb5e5ef9ed/Pics/BM03.PNG -------------------------------------------------------------------------------- /Pics/BMMain.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/killinen/BrakeModule/52e397301addb98a9d2e167176d838cb5e5ef9ed/Pics/BMMain.png -------------------------------------------------------------------------------- /Pics/Bosch57cars.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/killinen/BrakeModule/52e397301addb98a9d2e167176d838cb5e5ef9ed/Pics/Bosch57cars.PNG -------------------------------------------------------------------------------- /Pics/Brake light switch.JPG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/killinen/BrakeModule/52e397301addb98a9d2e167176d838cb5e5ef9ed/Pics/Brake light switch.JPG -------------------------------------------------------------------------------- /Pics/Bus Topology I -K-M-P-Can-Diagnostic.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/killinen/BrakeModule/52e397301addb98a9d2e167176d838cb5e5ef9ed/Pics/Bus Topology I -K-M-P-Can-Diagnostic.jpg -------------------------------------------------------------------------------- /Pics/Charge_pump_operation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/killinen/BrakeModule/52e397301addb98a9d2e167176d838cb5e5ef9ed/Pics/Charge_pump_operation.png -------------------------------------------------------------------------------- /Pics/DSCIII.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/killinen/BrakeModule/52e397301addb98a9d2e167176d838cb5e5ef9ed/Pics/DSCIII.PNG -------------------------------------------------------------------------------- /Pics/DSC_VOLT.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/killinen/BrakeModule/52e397301addb98a9d2e167176d838cb5e5ef9ed/Pics/DSC_VOLT.png -------------------------------------------------------------------------------- /Pics/PCB_032.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/killinen/BrakeModule/52e397301addb98a9d2e167176d838cb5e5ef9ed/Pics/PCB_032.PNG -------------------------------------------------------------------------------- /Pics/Pump.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/killinen/BrakeModule/52e397301addb98a9d2e167176d838cb5e5ef9ed/Pics/Pump.PNG -------------------------------------------------------------------------------- /Pics/PumpFETs.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/killinen/BrakeModule/52e397301addb98a9d2e167176d838cb5e5ef9ed/Pics/PumpFETs.PNG -------------------------------------------------------------------------------- /Pics/PumpSch.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/killinen/BrakeModule/52e397301addb98a9d2e167176d838cb5e5ef9ed/Pics/PumpSch.PNG -------------------------------------------------------------------------------- /Pics/Readme.md: -------------------------------------------------------------------------------- 1 | This just contains pics that are used in the main README.md 2 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # BrakeModule 2 | --- 3 | 4 | This project was created to solve the need for a MY99 BMW 540i (E39) to be able to brake using the semi-autonomous driving software OPENPILOT. 5 | 6 | This project has ended for me because, unfortunately, I no longer have the car. However, this was definitely fun to do. 7 | 8 | --- 9 | 10 | ### Disclaimer 11 | 12 | The software and hardware of this project has been done by fellow that have no knowledge of any kind on automotive, software or hardware developing stuff nor electrical engineering. I just like to get things done. So if some ever implements this, keep in mind that there is some change that the BrakeModule will behave unwantedly or will damage your cars electrical system! If you have better knowledge than I, please contribute on developing this. 13 | 14 | --- 15 | 16 | ## Backround 17 | 18 | This module has been designed for use with a car equipped with the BOSCH ABS 5.7 Dynamic Stability Control system 3 (DSCIII). It is possible that this module could be adapted for use with other cars that use this system, but modifications to the software may be required. The description of the system provided here is based on the author's observations and assumptions, and may not be accurate for all cars that use this system. There might also be a change this could be used on BOSCH 5.3 system, if DSCIII (or similar) is implemented. 19 | 20 | List of cars that use BOSCH 5.7 system found on the internet. 21 | 26 | | Brand | Model and year | | | | 27 | |------------------|------------------------|---------------------|--------------------------|--------------------| 28 | | Acura | RL, 2000-2004 | | | | 29 | | Audi | A4, 2000-2004 | A4, 2004-2008 | A6, 1997-2004 | A7, 2001-2005 | 30 | | Audi | A8, 2002-2010 | S4, 2004-2006 | S6, 2002-2003 | RS6, 2003 | 31 | | Alfa Romeo | 148, 2001-2010 | 156, 1997-2005 | 166, 1998-2007 | GT, 2003-2010 | 32 | | Bentley | Continental, 2004-2008 | | | | 33 | | BMW | E39, 1995-2003 | E46, 1998-2005 | E38, 1994-2001 | E65 E66, 2001-2011 | 34 | | BMW | E53, 2000-2007 | E52, 2000-2004 | | | 35 | | Citroen | C8, 2002-> | Xsara, 1997-2005 | Xsara Picasso, 1999-2010 | | 36 | | Dodge | Sprinter, 2003-2006 | | | | 37 | | Ferrari | F430, 2005 | | | | 38 | | Fiat | Stilo, 2001-2008 | Ulysse, 2002-2011 | | | 39 | | Ford | Mondeo, 2000-2007 | | | | 40 | | Jaguar | X-type, 2001-2009 | | | | 41 | | Lancia | Phedra, 2002-2010 | | | | 42 | | Land/Range Rover | 2002-2012 | | | | 43 | | Maserati | Quattroporte, 2004-> | | | | 44 | | Mercedes-Benz | Sprinter, 1995-2006 | Viano, 2003-> | V-Class, 1996-2003 | | 45 | | Opel/Vauxhall | Vectra, 1998-2001 | | | | 46 | | Peugeot | 307, 2000-2008 | 807, 2002-> | | | 47 | | Porsche | 911, 1997-2005 | 996, 2002-2004 | | | 48 | | Renault | Avantime, 2001-2003 | Clio, 1998-2005 | Clio, 2005-2012 | Megane, 2002-2008 | 49 | | Rover | 75, 1999-2005 | | | | 50 | | Seat | Cordoba, 2002-2009 | Ibiza, 1999-2002 | | | 51 | | Skoda | Fabia, 1999-2007 | Felicia, 1994-1998 | Superb, 2002-2008 | | 52 | | Saab | 9-3, 1999-2002 | 9-5, 2002-2004 | | | 53 | | Smart | City-Coupe, 1998-2007 | Roadster, 2003-2005 | | | 54 | | Subaru | Outback, 2003-2009 | | | | 55 | | Toyota | Avensis, 1997-2003 | Avensis, 2003-2008 | Corolla, 2001-2007 | Verso, 2001-2007 | 56 | | Volkswagen | Passat, 2000-2005 | Phaeton, 2002-> | Polo, 2001-2009 | | 57 | 58 | Likely there are more, found also these: 59 | - Ford Freestyle 60 | - Honda Civic, Passport, and Pilot 61 | - Lincoln Town Car 62 | - Mercury Grand Marquis and Sable 63 | - Subaru Baja, Forester, Impreza, Legacy 64 | 65 | I cannot guarantee the accuracy of this list, and I'm uncertain if all the mentioned cars are compatible or equipped with the necessary charge pump for braking purposes. 66 | 67 | The BrakeModule has been designed to receive brake request values from OPENPILOT via the CAN bus. OPENPILOT is an open source driver assistance system that offers Automated Lane Centering and Adaptive Cruise Control for over 200 supported car makes and models. To function properly, OPENPILOT needs to be able to control the longitudinal (gas and brake) and lateral (steering) movements of the car using the CAN bus. For more information, see https://comma.ai/ and https://github.com/commaai/openpilot. 68 | 69 | Video introducing OPENPILOT. 70 | 71 | [![](https://i3.ytimg.com/vi/NmBfgOanCyk/maxresdefault.jpg)](https://youtu.be/NmBfgOanCyk) 72 | 73 | At the moment, my implementation of the OPENPILOT only utilizes vision-based ACC (no radar) with stop and go capability, achieved using the BrakeModule and a device similar to the [comma](https://youtu.be/3z-Izl-ve5o?si=7oRMfNvz600YJCA-) [pedal](https://github.com/commaai/openpilot/wiki/comma-pedal). Lateral control is a [work in progress](https://github.com/killinen/E39steerModule). The top left of the screen shows the set speed of 87 km/h, and the top center shows the current speed as reduced by the vision system that detects the distance, speed, and acceleration/deceleration of the lead car. 74 | 75 | 76 | https://user-images.githubusercontent.com/37126045/204889544-91e67136-c381-4791-b589-27530b2b94af.mp4 77 | 78 | The following video demonstrates the performance of OPENPILOT and BrakeModule together. The pink graph shows brake pressure in bar, the purple graph shows gas pedal position (controlled by OPENPILOT, not the driver), the blue graph shows car speed in km/h, and the turquoise graph shows the distance to the lead car in meters. 79 | 80 | https://user-images.githubusercontent.com/37126045/205499143-c1d0632b-0707-4776-ab54-dc6cb979e526.mp4 81 | 82 | --- 83 | 84 | ## Working principle 85 | ### Abstract of BOSCH 5.7/DSCIII system 86 | The system can be roughly divided into few subsystems: 87 | - control module (ECU) that is responsible all of the logic and electrical stuff 88 | - valves that control the brake fluid flow 89 | - ABS relief pump 90 | - charge pump (also known as pre-charge pump) 91 | - sensors that monitor the system and the trajectory of the car. 92 | 93 | The ABS module monitors the car's state using its sensors, and determines if ABS or DSC intervention is required. In such cases, the module will generally take the following actions: 94 | - reduce motor torque requesting it via sending CAN messages to the DME (motor ECU) 95 | - relief pressure of individual brakes by controlling the valves and running the ABS relief pump (ABS event). 96 | - apply pressure to individual brakes by controlling the valves and running the charge pump (DSC event). 97 | 98 | Picture of BOSCH 5.7 with DSCIII. 99 |

100 | 101 |

102 | 103 | By controlling the charge pump, which is responsible for raising brake pressure in the system (10-15 bar), we can also control the car's braking. The charge pump increases brake pressure on both the front and rear axles. Below is snippet from DSC manual that explains how the charge pump control the brakes. 104 | 105 |

106 | 107 |

108 | 109 | The charge pump is controlled by two N-channel MOSFETs inside the control module, in a half-bridge configuration. 110 | 111 | Pic of the 2 FETs inside the module, right one is damaged. These are not the regular type black thingies (type [TO-220](https://en.wikipedia.org/wiki/TO-220)) that you likely see as MOSFETs. 112 |

113 | 114 |

115 | 116 | Handwritten schematic of the driving circuit of the charge pump. 117 |

118 | 119 |

120 | 121 | If someone want's to deep dive on Bosch DSC 5.7 ABS Module Diagnosis and Repair read this great post: https://www.bimmerfest.com/threads/bosch-dsc-5-7-abs-module-diagnosis-and-repair.822139/#post-8854110 (the pics are stolen from it). 122 | 123 | Further knowledge on how BOSCH 5.7/DSCIII works, look at https://github.com/killinen/BrakeModule/blob/main/dsc_system.pdf in the repo. 124 | 125 | Control module/hydraulic unit shown in M54 engine bay. 126 |

127 | 128 |

129 | 130 | Charge pump shown in M62TU engine bay. 131 |

132 | 133 |

134 | 135 | ## The name of the Man-in-the-middle is BrakeModule 136 | The concept behind gaining control of the ABS/DSC unit's braking ability is to use a man-in-the-middle tactic (https://en.wikipedia.org/wiki/Man-in-the-middle_attack). This can be achieved by taking control of the charge pump from the ABS module, preventing any diagnostic systems from being confused. To take control of the ABS/DSC unit's braking ability, the following steps can be taken: 137 | 1. Disconnect the BOSCH control module from the charge pump. 138 | 2. Reconnect the charge pump wires to a resistor so that the control module "thinks" that the pump is still connected. If the wires are disconnected, the module will throw a charge pump error because it will detect an open circuit via the feedback lines (f/b), shown in handwritten schematic. 139 | 3. Connecting 12V to the pump and use N-channel power MOSFET to adjust to charge pump load (brake pressure). 140 | 4. Manipulating the cars brake light (pedal) switch signal wires, as increased brake pressure in the system without detection of the brake pedal being pressed will cause a brake pressure sensor defekt error. 141 | 5. The BrakeModule determines its operations by reading messages from the car's CAN bus, where OPENPILOT is also connected. 142 | 143 | Main principle of driving charge pump and brakelight switch with BrakeModule. 144 |

145 | 146 |

147 | 148 | Brake light switch has 4 wires: 12V, ground, signal LOW, signal 2 HIGH. Signal LOW is grounded and signal HIGH is floating when pedal not pressed. When pedal is pressed, signal LOW is floating and signal HIGH is connected to 12V. Those lines are in BMW lingo S_BLS (brake-light switch) S_BLTS (brake-light test switch). 149 |

150 | 151 |

152 | 153 | In E39 the DME control unit evaluates the signals for the purpose of registering brake operation. The brake-light switch connects to earth (B-), the brake-light test switch connects to B+. And uses this truth table for operation: 154 |

155 | 156 |

157 | 158 | The BrakeModule (as well OPENPILOT) is connected to BMW's CAN bus which have following modules attached: 159 | - Instrument Cluster 160 | - LEW (steering angle sensor) 161 | - DSCIII (BOSCH ABS control module) 162 | - DME (engine control module) 163 | 164 | Whole bus topology of my car: 165 |

166 | 167 |

168 | 169 | The CAN bus used here, should NOT be confused to diagnostic bus which is exposed in OBD ports. 170 | 171 | --- 172 | 173 | ## BrakeModule HW 174 | 175 | The BrakeModule has three main functionalities: 176 | 1. Three relays that switch the charge pump in line with the DSC system or the BrakeModule. 177 | 2. A power MOSFET that controls the charge pump. 178 | 3. A temperature measurement system to monitor the power MOSFET temperature, along with a FAN to cool it down. 179 | 180 | Additionally, the BrakeModule uses a CAN module for communication and a voltage divider to measure the DSC+ line voltage and the car's brake light switch. 181 | 182 | The board size is 110x82mm and it contains. 183 | - Blue Pill development board containing STM32F103 MCU. 184 | - MCP2551 high-speed CAN protocol controller bus interface module for commucation. 185 | - 3 SMIH-12VDC-SL-C relays for switching the charge pump in/offline. 186 | - TC1413N DIP8 mosfet driver. 187 | - POWERPAK SO-8L MOSFET. 188 | - 12V to 5V buck converter module. 189 | - BS250P for driving the brakelight switch high side. 190 | - HK19F-DC12V-SHG relay for driving the brakelight switch low side. 191 | - TMP36 temperature sensor for MOSFET. 192 | - SOT23-NPN transistors for common switching purposes, eg. possible fan. 193 | - TO92-NPN transistors for driving the SMIH-12VDC-SL-C relays 194 | - USB to UART (FTDI) connector for updating the software and debugging. 195 | - Voltage dividers for sensing brake pedal high side (S_BLTS) and DSC charge pump driver state (is voltage fed to CP via DSC). 196 | 197 | 198 | Picture of PCB of BrakeModule v0.3.2. 199 |

200 | 201 |

202 | 203 | 204 | Picture of BrakeModule inside 3D-printed case. 205 |

206 | 207 |

208 | 209 | --- 210 | 211 | ## BrakeModule SW 212 | 213 | The software has been developed for the STM32F103 chip using the Arduino IDE, with some STM32Cube HAL functionality added. 214 | 215 | Normally he charge pump is connected to the BOSCH control module, with the relays in the normally-closed mode. However, when a braking demand is detected in the 0x343 CAN message (BRK_CMD) from OPENPILOT, the BOSCH module is disconnected from the pump and the pump is instead controlled by a 15 kHz PWM signal from the power MOSFET, with the relays switched to the OFF position from the normally-closed mode. Additionally, the brake light signal lines are driven to produce correct HIGH (S_BLTS) and LOW (S_BLS) signals, indicating to the car that the brake pedal has been pressed. When BRK_CMD demand is no longer detected, first 12V line and ground (PWR MOSFET) will be disconnected from the pump and after 600 ms BOSCH control module is switch back inline with the pump. Also brake light switch is turned OFF. This delay is because if the transition from pump activated with BrakeModule back to DSC module is too fast, BOSCH module will give error code. I think this is caused of pump still rotating (generating voltage to pump wires) and you will connect the pump to BOSCH module, modules feedback lines detects voltage at the pump when it shouldn't and gives an error or brake pressure is seen in brake circuit and brake light switch is turned off. 216 | 217 | The speed value (car_speed) is read from message 0x153 and sent to OPENPILOT as the ACC set speed when OPENPILOT is engaged (set_speed). 218 | 219 | The brake pedal state determined by reading voltage from the brake light switch HIGH signal line (S_BLTS), and when there is no braking demand in message 0x343 (BRK_CMD) from OPENPILOT, an active brake pedal pressed state (BRK_ST_OP) is sent to OPENPILOT in order to disengage it. The BMW cruise control button presses (BTN_CMD) are detected on message 0x329. If BTN_CMD contains a RESUME button press, it will engage or disengage OPENPILOT's ACC depending on its current state. BTN_CMD also includes cruise controls + and - button presses, which adjust the set speed of OPENPILOT's ACC accordingly when pressed. The BMW's cruise control state (OCC) is detected on 0x545, and if it is on, OPENPILOT will not engage to prevent the original CC and OPENPILOT from simultaneously controlling the longitudinal direction. 220 | 221 | 222 | 223 | The BrakeModule is used to simulate the cruise control system of a TOYOTA Corolla because the OPENPILOT fork identifies the car as a TOYOTA. As a result, data is sent in 0x1D2 and 0x1D3 CAN messages to OPENPILOT, which are typically used by the cruise control system of TOYOTA vehicles. The use of TOYOTA in OPENPILOT is due to legacy reasons, as the first person to use OPENPILOT on an older car implemented it on a TOYOTA Celica. My code is simply a revised version of that original implementation. 224 | 225 | If DEBUG is #defined in software you can control the board via serial (look at the readSerial() function) + some debugging messages are shown. 226 | 227 | If FAN_CTRL is #defined in the software and the temperature measured by the TMP36 sensor (not implemented in the v0.3 code) exceeds 45 degrees, a small NPN transistor is pulled low, which is connected to the FAN connector (designed to cool the MOSFET). Additionally, if the temperature exceeds 80 degrees, the BrakeModule will disable OPENPILOT and prevent it from engaging until the temperature falls below that threshold (this shabang might is not necessary at least haven't been for me). 228 | 229 | 230 | 231 | --- 232 | 233 | ## OT of CAN bus messages 234 | Some of the decoded CAN messages that is on E39 CAN bus can be found here: https://github.com/killinen/opendbc/blob/master/BMW_E39.dbc (I'm sure this is not perfect, would be cool if someone else would like to look into it). It is in openDBC format, here's couple good reads what DBC is. https://github.com/stefanhoelzl/CANpy/blob/master/docs/DBC_Specification.md http://socialledge.com/sjsu/index.php/DBC_Format. Great insipiration for reverse engineering the CAN messages was this thread relating to E46 CAN bus messages: https://www.bimmerforums.com/forum/showthread.php?1887229-E46-Can-bus-project. Also for some other chassis CAN msgs see dzid's issue https://github.com/killinen/BrakeModule/issues/1. Python library for en/decoding DBC's https://cantools.readthedocs.io/en/latest/. 235 | 236 | --- 237 | 238 | ## Next iteration 239 | 240 | If I have the inspiration to make of BM v0.3.3 I would: 241 | - Fix HW bugs (funny I found few). 242 | - ~~Reduce part count (simplify).~~ 243 | - Get rid of extra DSC voltage divider circuit in the upper right corner of PCB (useless). 244 | - Maybe add (working) brake light switch low side sensing (redundancy). 245 | - ~~Maybe 4 layer PCB design (could lower the power tracing resistance?)~~ 246 | - ~~Maybe use of different kind of power MOSFET (is the footprint right).~~ 247 | 248 | Follow up to above: Ditch (partly) the 0.3.3 went for 0.3.4 where the design changes will include: 249 | - Safety related HW (this just went overkill but going to test stuff and probably will revert some stuff later on) 250 | - Integrate Panda, steering angle sensor (connects to cars CAN bus) and [StepperServoCAN](https://github.com/dzid26/StepperServo-hardware) wirings 251 | - Do some simplifying and part changes 252 | - Add testing points for easier development 253 | - Add brownout detection 254 | - Add few new HW bugs 255 | 256 | ### Possible v0.4 257 | 258 | It may be possible to ditch the pump-side relays and use a similar MOSFET configuration as in the DSC module. This would allow the system to sense the DSC wires and drive the pump accordingly. The below picture shows the voltage in millivolts (DSC_VOLTAGE) measured from the DSC+ wire. There appears to be some sort of bootstrapping cycle, and the voltage is increased when the pump is running and driven by the BrakeModule. The running of the pump can be observed from the PWM value (2047 = 100% duty cycle), and the brake pressure (in bars) is shown for reference. The pressure reading without the pump being driven is the result of me pressing the brake pedal :) 259 | 260 |

261 | 262 |

263 | 264 | The unfortunate thing in these voltage spikes are IMO that if you would like to eliminate the change of charge pump not running when DSC module is demanding it but BrakeModule controlling the pump it will result of somewhat of an lag if you are waiting to see that is this voltage spike bootstrapping cycle or an real pump control demand. Will this result an real world meaningful lag, I can't really say (this doesn't affect only w v0.4 but current system also). 265 | 266 | Benefits for this hardware design would be reduced complexity and part count. This is likely to result in simpler software as well. Relays switching noise should be reduced. Overall these are quite minor improvements. 267 | 268 | In older HW v0.2 I have used (tested) relay or N-channel MOSFET for controlling the LOW side BLS signal line (S_BLS). I somehow prefer the use of NC-relay, but maybe transfer to some other type of solution in the future. Dual channel MOSFET IC maybe or something. 269 | 270 | --- 271 | 272 | ### Problems that I know of 273 | What I have understand there are standards and guidelines for automotive hardware and software design, this project does not follow any of those. 274 | 275 | My main concern is that if a stability control event occurs while the BrakeModule is controlling the charge pump, there is a non-trivial chance that the stability control system will not function as intended. I have not yet experienced any problems with this, but I understand that more testing is needed to be confident that this will not happen. I plan to conduct further testing to address this concern in the future. 276 | 277 | If the ABS control unit is damaged, it can be difficult and costly to repair. I cannot guarantee that using the BrakeModule will not cause any damage, but so far, I have not experienced any problems with my unit, even though it has had quite a bit of rough love. This the quality of the German engineering ;) 278 | 279 | I am not aware of the maximum capabilities of the charge pump, such as whether it can overheat and brake if run for too long. The software does not have any restrictions in place to prevent this. I have run the charge pump for extended periods in red lights in Nordic summer (best outside 25-30°C), and so far it has not failed. However, on hotter days, I have observed a decrease in the maximum brake pressure that the pump can produce, which I assume is due to the brake fluid getting hot. 280 | 281 | If brake pedal is pressed hard and BrakeModule is controlling the pump and switched back to "normal mode", BOSCH module will give an error (this might be solved). 282 | 283 | A well-designed approach to installing the module would likely involve using a professional case to protect the components and provide integrated heat dispersion. This would prevent the components from being exposed and improve the overall performance and reliability of the system. 284 | 285 | --- 286 | 287 | ### Someone might think 288 | 289 | That wouldn't it be best if you could control the BOSCH control module using CAN to access the charge pump, but I do not have the knowledge or expertise to do so. This approach may be feasible because the same unit is used with ACC systems, and the only way that I can imagine to apply the brakes is to use the charge pump. 290 | 291 | The benefits in my mind of the BrakeModule to latter is to have full control of braking (my understanding is that OEM system won't brake below certain speed in ACC mode). Scalability is also benefit because you probably won't need to reverse engineer all the possible messages/programs that are implemented on different car brands and models. And lastly this is more fun :) 292 | 293 | --- 294 | -------------------------------------------------------------------------------- /Safety/README.md: -------------------------------------------------------------------------------- 1 | ## Safety aspect 2 | 3 | In critical systems, it is important to adhere to the principle of rigorous code testing. The BrakeModule code has undergone this process through the use of static analysis tools such as Cppcheck and the Misra addon. 4 | 5 | Cppcheck is a static analysis tool for C/C++ code that is designed to detect bugs and undefined behavior. It has been used to check the BrakeModule code for any issues. 6 | 7 | The Misra addon is a layer on top of Cppcheck that performs rule checking according to the MISRA-C:2012 standard. The BrakeModule code has passed the Cppcheck test and is currently being checked for MISRA compliance. MISRA violations can be seen on misra/misra_violations_output.txt. For making those tests there is shell script code_tests.sh in misra folder. 8 | 9 | During compilation, the ```-warnings=all``` flag is also set to highlight any potential problems. 10 | 11 | ### General safety filosophy 12 | The BrakeModule aims to minimize the impact on the original ABS/DSC system's functionality and safety features. 13 | 14 | TODO: Conduct a systematic analysis of logic or component failures and their effect on the BrakeModule and the rest of the system. 15 | 16 | ### Software design 17 | Some of the funtional safety stuff that has been implemented in software: 18 | - STM32F1 internal temperature monitoring and and OP disengaged if temperature above certain threshold. 19 | - POWER MOSFET temperature monitoring and and OP disengaged if temperature above certain threshold. 20 | - Check for freezed ADC values. 21 | - IDWG for restarting the program if chip freezes. 22 | - CAN bus failure detection. 23 | - Check that OP related CAN BUS msg is present in the BUS. 24 | - Lock pin configuration after initialization. 25 | 26 | ### Hardware design 27 | Here text what safety has tried to be implemented in hardware 28 | 29 | It is important to note that these safety measures do not guarantee the safety of the BrakeModule. Further analysis may be necessary to fully assess the system's safety. 30 | -------------------------------------------------------------------------------- /Safety/misra/__pycache__/cppcheckdata.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/killinen/BrakeModule/52e397301addb98a9d2e167176d838cb5e5ef9ed/Safety/misra/__pycache__/cppcheckdata.cpython-38.pyc -------------------------------------------------------------------------------- /Safety/misra/code_tests.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # cppcheck --addon=misra.json --suppressions-list=suppressions.txt BrakeModule_STM32F1_sw03_misra.ino 4 | # cppcheck --addon=misra.json --suppressions-list=suppressions.txt $1 5 | # cppcheck --addon=./misra/misra.json --suppressions-list=./misra/suppressions.txt $1 6 | 7 | source_folder="$(find ../.. -maxdepth 1 -name 'BrakeModule*')" 8 | cppcheck_out_file="misra_violations_output.txt" 9 | FILE="$(find ${source_folder}/*.ino)" 10 | 11 | # This is not used, maybe in future, this is here to give inspiration :) 12 | cppcheck_parameters=( --inline-suppr 13 | --language=c++ 14 | --addon="$script_folder/misra.json" 15 | --suppressions-list="$script_folder/suppressions.txt" 16 | --platform=avr8 17 | --cppcheck-build-dir="$out_folder" 18 | -j "$num_cores" 19 | -DCORE_AVR=1 20 | -D__AVR_ATmega2560__ 21 | # This is defined in the AVR headers, which aren't included. 22 | # cppcheck will not do type checking on unknown types. 23 | # It's used a lot and it's unsigned, which can trigger a lot 24 | # of type mismatch violations. 25 | -Dbyte=uint8_t 26 | # All violations from included libraries (*src* folders) are ignored 27 | --suppress="*:*src*" 28 | # No libdivide - analysis takes too long 29 | -UUSE_LIBDIVIDE 30 | # Don't parse the /src folder 31 | -i "$source_folder/src" 32 | "$source_folder/**.ino" 33 | "$source_folder/**.cpp") 34 | 35 | 36 | 37 | #printf "\nThen make MISRA-c2012 check for $FILE file in current folder\n" 38 | printf "Make CPPCHECK and MISRA-c2012 check for $FILE and save the output into misra_violations_output.txt\n\n" 39 | sleep 5 40 | 41 | cppcheck --addon=misra.json --suppressions-list=suppressions.txt $FILE |& tee $cppcheck_out_file 42 | 43 | # Count lines for Mandatory or Required rules 44 | error_count=`grep -i "Mandatory - \|Required - " < "$cppcheck_out_file" | wc -l` 45 | printf "*** MISRA violations count was $error_count ***\n" |& tee -a $cppcheck_out_file 46 | -------------------------------------------------------------------------------- /Safety/misra/cppcheckdata.py: -------------------------------------------------------------------------------- 1 | """ 2 | cppcheckdata 3 | 4 | This is a Python module that helps you access Cppcheck dump data. 5 | 6 | License: No restrictions, use this as you need. 7 | """ 8 | 9 | from xml.etree import ElementTree 10 | import argparse 11 | from fnmatch import fnmatch 12 | import json 13 | import sys 14 | 15 | 16 | class Directive: 17 | """ 18 | Directive class. Contains information about each preprocessor directive in the source code. 19 | 20 | Attributes: 21 | str The directive line, with all C or C++ comments removed 22 | file Name of (possibly included) file where directive is defined 23 | linenr Line number in (possibly included) file where directive is defined 24 | 25 | To iterate through all directives use such code: 26 | @code 27 | data = cppcheckdata.parsedump(...) 28 | for cfg in data.configurations: 29 | for directive in cfg.directives: 30 | print(directive.str) 31 | @endcode 32 | """ 33 | 34 | str = None 35 | file = None 36 | linenr = None 37 | column = 0 38 | 39 | def __init__(self, element): 40 | self.str = element.get('str') 41 | self.file = element.get('file') 42 | self.linenr = int(element.get('linenr')) 43 | 44 | def __repr__(self): 45 | attrs = ["str", "file", "linenr"] 46 | return "{}({})".format( 47 | "Directive", 48 | ", ".join(("{}={}".format(a, repr(getattr(self, a))) for a in attrs)) 49 | ) 50 | 51 | 52 | class ValueType: 53 | """ 54 | ValueType class. Contains (promoted) type information for each node in the AST. 55 | """ 56 | 57 | type = None 58 | sign = None 59 | bits = 0 60 | constness = 0 61 | pointer = 0 62 | typeScopeId = None 63 | typeScope = None 64 | originalTypeName = None 65 | 66 | def __init__(self, element): 67 | self.type = element.get('valueType-type') 68 | self.sign = element.get('valueType-sign') 69 | bits = element.get('valueType-bits') 70 | if bits: 71 | self.bits = int(bits) 72 | self.typeScopeId = element.get('valueType-typeScope') 73 | self.originalTypeName = element.get('valueType-originalTypeName') 74 | constness = element.get('valueType-constness') 75 | if constness: 76 | self.constness = int(constness) 77 | else: 78 | self.constness = 0 79 | pointer = element.get('valueType-pointer') 80 | if pointer: 81 | self.pointer = int(pointer) 82 | else: 83 | self.pointer = 0 84 | 85 | def __repr__(self): 86 | attrs = ["type", "sign", "bits", "typeScopeId", "originalTypeName", 87 | "constness", "constness", "pointer"] 88 | return "{}({})".format( 89 | "ValueType", 90 | ", ".join(("{}={}".format(a, repr(getattr(self, a))) for a in attrs)) 91 | ) 92 | 93 | 94 | def setId(self, IdMap): 95 | self.typeScope = IdMap[self.typeScopeId] 96 | 97 | def isIntegral(self): 98 | return self.type in {'bool', 'char', 'short', 'int', 'long', 'long long'} 99 | 100 | def isFloat(self): 101 | return self.type in {'float', 'double', 'long double'} 102 | 103 | def isEnum(self): 104 | return self.typeScope and self.typeScope.type == "Enum" 105 | 106 | 107 | class Token: 108 | """ 109 | Token class. Contains information about each token in the source code. 110 | 111 | The CppcheckData.tokenlist is a list of Token items 112 | 113 | C++ class: http://cppcheck.net/devinfo/doxyoutput/classToken.html 114 | 115 | Attributes: 116 | str Token string 117 | next Next token in tokenlist. For last token, next is None. 118 | previous Previous token in tokenlist. For first token, previous is None. 119 | link Linked token in tokenlist. Each '(', '[' and '{' are linked to the 120 | corresponding '}', ']' and ')'. For templates, the '<' is linked to 121 | the corresponding '>'. 122 | scope Scope information for this token. See the Scope class. 123 | isName Is this token a symbol name 124 | isNumber Is this token a number, for example 123, 12.34 125 | isInt Is this token a int value such as 1234 126 | isFloat Is this token a int value such as 12.34 127 | isString Is this token a string literal such as "hello" 128 | strlen string length for string literal 129 | isChar Is this token a char literal such as 'x' 130 | isOp Is this token a operator 131 | isArithmeticalOp Is this token a arithmetic operator 132 | isAssignmentOp Is this token a assignment operator 133 | isComparisonOp Is this token a comparison operator 134 | isLogicalOp Is this token a logical operator: && || 135 | isUnsigned Is this token a unsigned type 136 | isSigned Is this token a signed type 137 | isExpandedMacro Is this token a expanded macro token 138 | varId varId for token, each variable has a unique non-zero id 139 | variable Variable information for this token. See the Variable class. 140 | function If this token points at a function call, this attribute has the Function 141 | information. See the Function class. 142 | values Possible values of token 143 | valueType type information 144 | typeScope type scope (token->type()->classScope) 145 | astParent ast parent 146 | astOperand1 ast operand1 147 | astOperand2 ast operand2 148 | file file name 149 | linenr line number 150 | column column 151 | 152 | To iterate through all tokens use such code: 153 | @code 154 | data = cppcheckdata.parsedump(...) 155 | for cfg in data.configurations: 156 | code = '' 157 | for token in cfg.tokenlist: 158 | code = code + token.str + ' ' 159 | print(code) 160 | @endcode 161 | """ 162 | 163 | Id = None 164 | str = None 165 | next = None 166 | previous = None 167 | linkId = None 168 | link = None 169 | scopeId = None 170 | scope = None 171 | isName = False 172 | isNumber = False 173 | isInt = False 174 | isFloat = False 175 | isString = False 176 | strlen = None 177 | isChar = False 178 | isOp = False 179 | isArithmeticalOp = False 180 | isAssignmentOp = False 181 | isComparisonOp = False 182 | isLogicalOp = False 183 | isUnsigned = False 184 | isSigned = False 185 | isExpandedMacro = False 186 | varId = None 187 | variableId = None 188 | variable = None 189 | functionId = None 190 | function = None 191 | valuesId = None 192 | values = None 193 | valueType = None 194 | 195 | typeScopeId = None 196 | typeScope = None 197 | 198 | astParentId = None 199 | astParent = None 200 | astOperand1Id = None 201 | astOperand1 = None 202 | astOperand2Id = None 203 | astOperand2 = None 204 | 205 | file = None 206 | linenr = None 207 | column = None 208 | 209 | def __init__(self, element): 210 | self.Id = element.get('id') 211 | self.str = element.get('str') 212 | self.next = None 213 | self.previous = None 214 | self.scopeId = element.get('scope') 215 | self.scope = None 216 | type = element.get('type') 217 | if type == 'name': 218 | self.isName = True 219 | if element.get('isUnsigned'): 220 | self.isUnsigned = True 221 | if element.get('isSigned'): 222 | self.isSigned = True 223 | elif type == 'number': 224 | self.isNumber = True 225 | if element.get('isInt'): 226 | self.isInt = True 227 | elif element.get('isFloat'): 228 | self.isFloat = True 229 | elif type == 'string': 230 | self.isString = True 231 | self.strlen = int(element.get('strlen')) 232 | elif type == 'char': 233 | self.isChar = True 234 | elif type == 'op': 235 | self.isOp = True 236 | if element.get('isArithmeticalOp'): 237 | self.isArithmeticalOp = True 238 | elif element.get('isAssignmentOp'): 239 | self.isAssignmentOp = True 240 | elif element.get('isComparisonOp'): 241 | self.isComparisonOp = True 242 | elif element.get('isLogicalOp'): 243 | self.isLogicalOp = True 244 | if element.get('isExpandedMacro'): 245 | self.isExpandedMacro = True 246 | self.linkId = element.get('link') 247 | self.link = None 248 | if element.get('varId'): 249 | self.varId = int(element.get('varId')) 250 | self.variableId = element.get('variable') 251 | self.variable = None 252 | self.functionId = element.get('function') 253 | self.function = None 254 | self.valuesId = element.get('values') 255 | self.values = None 256 | if element.get('valueType-type'): 257 | self.valueType = ValueType(element) 258 | else: 259 | self.valueType = None 260 | self.typeScopeId = element.get('type-scope') 261 | self.typeScope = None 262 | self.astParentId = element.get('astParent') 263 | self.astParent = None 264 | self.astOperand1Id = element.get('astOperand1') 265 | self.astOperand1 = None 266 | self.astOperand2Id = element.get('astOperand2') 267 | self.astOperand2 = None 268 | self.file = element.get('file') 269 | self.linenr = int(element.get('linenr')) 270 | self.column = int(element.get('column')) 271 | 272 | def __repr__(self): 273 | attrs = ["Id", "str", "scopeId", "isName", "isUnsigned", "isSigned", 274 | "isNumber", "isInt", "isFloat", "isString", "strlen", 275 | "isChar", "isOp", "isArithmeticalOp", "isComparisonOp", 276 | "isLogicalOp", "isExpandedMacro", "linkId", "varId", 277 | "variableId", "functionId", "valuesId", "valueType", 278 | "typeScopeId", "astParentId", "astOperand1Id", "file", 279 | "linenr", "column"] 280 | return "{}({})".format( 281 | "Token", 282 | ", ".join(("{}={}".format(a, repr(getattr(self, a))) for a in attrs)) 283 | ) 284 | 285 | def setId(self, IdMap): 286 | self.scope = IdMap[self.scopeId] 287 | self.link = IdMap[self.linkId] 288 | self.variable = IdMap[self.variableId] 289 | self.function = IdMap[self.functionId] 290 | self.values = IdMap[self.valuesId] 291 | self.typeScope = IdMap[self.typeScopeId] 292 | self.astParent = IdMap[self.astParentId] 293 | self.astOperand1 = IdMap[self.astOperand1Id] 294 | self.astOperand2 = IdMap[self.astOperand2Id] 295 | if self.valueType: 296 | self.valueType.setId(IdMap) 297 | 298 | def getValue(self, v): 299 | """ 300 | Get value if it exists 301 | Returns None if it doesn't exist 302 | """ 303 | 304 | if not self.values: 305 | return None 306 | for value in self.values: 307 | if value.intvalue == v: 308 | return value 309 | return None 310 | 311 | 312 | class Scope: 313 | """ 314 | Scope. Information about global scope, function scopes, class scopes, inner scopes, etc. 315 | C++ class: http://cppcheck.net/devinfo/doxyoutput/classScope.html 316 | 317 | Attributes 318 | bodyStart The { Token for this scope 319 | bodyEnd The } Token for this scope 320 | className Name of this scope. 321 | For a function scope, this is the function name; 322 | For a class scope, this is the class name. 323 | function If this scope belongs at a function call, this attribute 324 | has the Function information. See the Function class. 325 | type Type of scope: Global, Function, Class, If, While 326 | """ 327 | 328 | Id = None 329 | bodyStartId = None 330 | bodyStart = None 331 | bodyEndId = None 332 | bodyEnd = None 333 | className = None 334 | functionId = None 335 | function = None 336 | nestedInId = None 337 | nestedIn = None 338 | type = None 339 | isExecutable = None 340 | 341 | def __init__(self, element): 342 | self.Id = element.get('id') 343 | self.className = element.get('className') 344 | self.functionId = element.get('function') 345 | self.function = None 346 | self.bodyStartId = element.get('bodyStart') 347 | self.bodyStart = None 348 | self.bodyEndId = element.get('bodyEnd') 349 | self.bodyEnd = None 350 | self.nestedInId = element.get('nestedIn') 351 | self.nestedIn = None 352 | self.type = element.get('type') 353 | self.isExecutable = (self.type in ('Function', 'If', 'Else', 'For', 'While', 'Do', 354 | 'Switch', 'Try', 'Catch', 'Unconditional', 'Lambda')) 355 | 356 | def __repr__(self): 357 | attrs = ["Id", "className", "functionId", "bodyStartId", "bodyEndId", 358 | "nestedInId", "nestedIn", "type", "isExecutable"] 359 | return "{}({})".format( 360 | "Scope", 361 | ", ".join(("{}={}".format(a, repr(getattr(self, a))) for a in attrs)) 362 | ) 363 | 364 | def setId(self, IdMap): 365 | self.bodyStart = IdMap[self.bodyStartId] 366 | self.bodyEnd = IdMap[self.bodyEndId] 367 | self.nestedIn = IdMap[self.nestedInId] 368 | self.function = IdMap[self.functionId] 369 | 370 | 371 | class Function: 372 | """ 373 | Information about a function 374 | C++ class: 375 | http://cppcheck.net/devinfo/doxyoutput/classFunction.html 376 | 377 | Attributes 378 | argument Argument list 379 | tokenDef Token in function definition 380 | isVirtual Is this function is virtual 381 | isImplicitlyVirtual Is this function is virtual this in the base classes 382 | isStatic Is this function is static 383 | """ 384 | 385 | Id = None 386 | argument = None 387 | argumentId = None 388 | tokenDef = None 389 | tokenDefId = None 390 | name = None 391 | type = None 392 | isVirtual = None 393 | isImplicitlyVirtual = None 394 | isStatic = None 395 | 396 | def __init__(self, element): 397 | self.Id = element.get('id') 398 | self.tokenDefId = element.get('tokenDef') 399 | self.name = element.get('name') 400 | self.type = element.get('type') 401 | isVirtual = element.get('isVirtual') 402 | self.isVirtual = (isVirtual and isVirtual == 'true') 403 | isImplicitlyVirtual = element.get('isImplicitlyVirtual') 404 | self.isImplicitlyVirtual = (isImplicitlyVirtual and isImplicitlyVirtual == 'true') 405 | isStatic = element.get('isStatic') 406 | self.isStatic = (isStatic and isStatic == 'true') 407 | 408 | self.argument = {} 409 | self.argumentId = {} 410 | for arg in element: 411 | self.argumentId[int(arg.get('nr'))] = arg.get('variable') 412 | 413 | def __repr__(self): 414 | attrs = ["Id", "tokenDefId", "name", "type", "isVirtual", 415 | "isImplicitlyVirtual", "isStatic", "argumentId"] 416 | return "{}({})".format( 417 | "Function", 418 | ", ".join(("{}={}".format(a, repr(getattr(self, a))) for a in attrs)) 419 | ) 420 | 421 | def setId(self, IdMap): 422 | for argnr, argid in self.argumentId.items(): 423 | self.argument[argnr] = IdMap[argid] 424 | self.tokenDef = IdMap[self.tokenDefId] 425 | 426 | 427 | class Variable: 428 | """ 429 | Information about a variable 430 | C++ class: 431 | http://cppcheck.net/devinfo/doxyoutput/classVariable.html 432 | 433 | Attributes: 434 | nameToken Name token in variable declaration 435 | typeStartToken Start token of variable declaration 436 | typeEndToken End token of variable declaration 437 | access Global/Local/Namespace/Public/Protected/Public/Throw/Argument 438 | scope Variable scope 439 | isArgument Is this variable a function argument? 440 | isArray Is this variable an array? 441 | isClass Is this variable a class or struct? 442 | isConst Is this variable a const variable? 443 | isGlobal Is this variable a global variable? 444 | isExtern Is this variable an extern variable? 445 | isLocal Is this variable a local variable? 446 | isPointer Is this variable a pointer 447 | isReference Is this variable a reference 448 | isStatic Is this variable static? 449 | constness Variable constness (same encoding as ValueType::constness) 450 | """ 451 | 452 | Id = None 453 | nameTokenId = None 454 | nameToken = None 455 | typeStartTokenId = None 456 | typeStartToken = None 457 | typeEndTokenId = None 458 | typeEndToken = None 459 | access = None 460 | scopeId = None 461 | scope = None 462 | isArgument = False 463 | isArray = False 464 | isClass = False 465 | isConst = False 466 | isExtern = False 467 | isGlobal = False 468 | isLocal = False 469 | isPointer = False 470 | isReference = False 471 | isStatic = False 472 | constness = 0 473 | 474 | def __init__(self, element): 475 | self.Id = element.get('id') 476 | self.nameTokenId = element.get('nameToken') 477 | self.nameToken = None 478 | self.typeStartTokenId = element.get('typeStartToken') 479 | self.typeStartToken = None 480 | self.typeEndTokenId = element.get('typeEndToken') 481 | self.typeEndToken = None 482 | self.access = element.get('access') 483 | self.scopeId = element.get('scope') 484 | self.scope = None 485 | self.isArgument = element.get('isArgument') == 'true' 486 | self.isArray = element.get('isArray') == 'true' 487 | self.isClass = element.get('isClass') == 'true' 488 | self.isConst = element.get('isConst') == 'true' 489 | self.isGlobal = element.get('access') == 'Global' 490 | self.isExtern = element.get('isExtern') == 'true' 491 | self.isLocal = element.get('isLocal') == 'true' 492 | self.isPointer = element.get('isPointer') == 'true' 493 | self.isReference = element.get('isReference') == 'true' 494 | self.isStatic = element.get('isStatic') == 'true' 495 | self.constness = element.get('constness') 496 | if self.constness: 497 | self.constness = int(self.constness) 498 | 499 | def __repr__(self): 500 | attrs = ["Id", "nameTokenId", "typeStartTokenId", "typeEndTokenId", 501 | "access", "scopeId", "isArgument", "isArray", "isClass", 502 | "isConst", "isGlobal", "isExtern", "isLocal", "isPointer", 503 | "isReference", "isStatic", "constness", ] 504 | return "{}({})".format( 505 | "Variable", 506 | ", ".join(("{}={}".format(a, repr(getattr(self, a))) for a in attrs)) 507 | ) 508 | 509 | def setId(self, IdMap): 510 | self.nameToken = IdMap[self.nameTokenId] 511 | self.typeStartToken = IdMap[self.typeStartTokenId] 512 | self.typeEndToken = IdMap[self.typeEndTokenId] 513 | self.scope = IdMap[self.scopeId] 514 | 515 | 516 | class ValueFlow: 517 | """ 518 | ValueFlow::Value class 519 | Each possible value has a ValueFlow::Value item. 520 | Each ValueFlow::Value either has a intvalue or tokvalue 521 | C++ class: 522 | http://cppcheck.net/devinfo/doxyoutput/classValueFlow_1_1Value.html 523 | 524 | Attributes: 525 | values Possible values 526 | """ 527 | 528 | Id = None 529 | values = None 530 | 531 | class Value: 532 | """ 533 | Value class 534 | 535 | Attributes: 536 | intvalue integer value 537 | tokvalue token value 538 | floatvalue float value 539 | containerSize container size 540 | condition condition where this Value comes from 541 | valueKind 'known' or 'possible' 542 | inconclusive Is value inconclusive? 543 | """ 544 | 545 | intvalue = None 546 | tokvalue = None 547 | floatvalue = None 548 | containerSize = None 549 | condition = None 550 | valueKind = None 551 | inconclusive = False 552 | 553 | def isKnown(self): 554 | return self.valueKind and self.valueKind == 'known' 555 | 556 | def isPossible(self): 557 | return self.valueKind and self.valueKind == 'possible' 558 | 559 | def __init__(self, element): 560 | self.intvalue = element.get('intvalue') 561 | if self.intvalue: 562 | self.intvalue = int(self.intvalue) 563 | self.tokvalue = element.get('tokvalue') 564 | self.floatvalue = element.get('floatvalue') 565 | self.containerSize = element.get('container-size') 566 | self.condition = element.get('condition-line') 567 | if self.condition: 568 | self.condition = int(self.condition) 569 | if element.get('known'): 570 | self.valueKind = 'known' 571 | elif element.get('possible'): 572 | self.valueKind = 'possible' 573 | if element.get('inconclusive'): 574 | self.inconclusive = True 575 | 576 | def __repr__(self): 577 | attrs = ["intvalue", "tokvalue", "floatvalue", "containerSize", 578 | "condition", "valueKind", "inconclusive"] 579 | return "{}({})".format( 580 | "Value", 581 | ", ".join(("{}={}".format(a, repr(getattr(self, a))) for a in attrs)) 582 | ) 583 | 584 | def __init__(self, element): 585 | self.Id = element.get('id') 586 | self.values = [] 587 | for value in element: 588 | self.values.append(ValueFlow.Value(value)) 589 | 590 | def __repr__(self): 591 | attrs = ["Id", "values"] 592 | return "{}({})".format( 593 | "ValueFlow", 594 | ", ".join(("{}={}".format(a, repr(getattr(self, a))) for a in attrs)) 595 | ) 596 | 597 | 598 | class Suppression: 599 | """ 600 | Suppression class 601 | This class contains a suppression entry to suppress a warning. 602 | 603 | Attributes 604 | errorId The id string of the error to suppress, can be a wildcard 605 | fileName The name of the file to suppress warnings for, can include wildcards 606 | lineNumber The number of the line to suppress warnings from, can be 0 to represent any line 607 | symbolName The name of the symbol to match warnings for, can include wildcards 608 | """ 609 | 610 | errorId = None 611 | fileName = None 612 | lineNumber = None 613 | symbolName = None 614 | 615 | def __init__(self, element): 616 | self.errorId = element.get('errorId') 617 | self.fileName = element.get('fileName') 618 | self.lineNumber = element.get('lineNumber') 619 | self.symbolName = element.get('symbolName') 620 | 621 | def __repr__(self): 622 | attrs = ['errorId' , "fileName", "lineNumber", "symbolName"] 623 | return "{}({})".format( 624 | "Suppression", 625 | ", ".join(("{}={}".format(a, repr(getattr(self, a))) for a in attrs)) 626 | ) 627 | 628 | def isMatch(self, file, line, message, errorId): 629 | if ((self.fileName is None or fnmatch(file, self.fileName)) 630 | and (self.lineNumber is None or line == self.lineNumber) 631 | and (self.symbolName is None or fnmatch(message, '*'+self.symbolName+'*')) 632 | and fnmatch(errorId, self.errorId)): 633 | return True 634 | else: 635 | return False 636 | 637 | 638 | class Configuration: 639 | """ 640 | Configuration class 641 | This class contains the directives, tokens, scopes, functions, 642 | variables, value flows, and suppressions for one configuration. 643 | 644 | Attributes: 645 | name Name of the configuration, "" for default 646 | directives List of Directive items 647 | tokenlist List of Token items 648 | scopes List of Scope items 649 | functions List of Function items 650 | variables List of Variable items 651 | valueflow List of ValueFlow values 652 | """ 653 | 654 | name = '' 655 | directives = [] 656 | tokenlist = [] 657 | scopes = [] 658 | functions = [] 659 | variables = [] 660 | valueflow = [] 661 | 662 | def __init__(self, confignode): 663 | self.name = confignode.get('cfg') 664 | self.directives = [] 665 | self.tokenlist = [] 666 | self.scopes = [] 667 | self.functions = [] 668 | self.variables = [] 669 | self.valueflow = [] 670 | arguments = [] 671 | 672 | for element in confignode: 673 | if element.tag == "standards": 674 | self.standards = Standards(element) 675 | 676 | if element.tag == 'directivelist': 677 | for directive in element: 678 | self.directives.append(Directive(directive)) 679 | 680 | if element.tag == 'tokenlist': 681 | for token in element: 682 | self.tokenlist.append(Token(token)) 683 | 684 | # set next/previous.. 685 | prev = None 686 | for token in self.tokenlist: 687 | token.previous = prev 688 | if prev: 689 | prev.next = token 690 | prev = token 691 | if element.tag == 'scopes': 692 | for scope in element: 693 | self.scopes.append(Scope(scope)) 694 | for functionList in scope: 695 | if functionList.tag == 'functionList': 696 | for function in functionList: 697 | self.functions.append(Function(function)) 698 | if element.tag == 'variables': 699 | for variable in element: 700 | var = Variable(variable) 701 | if var.nameTokenId: 702 | self.variables.append(var) 703 | else: 704 | arguments.append(var) 705 | if element.tag == 'valueflow': 706 | for values in element: 707 | self.valueflow.append(ValueFlow(values)) 708 | 709 | IdMap = {None: None, '0': None, '00000000': None, '0000000000000000': None} 710 | for token in self.tokenlist: 711 | IdMap[token.Id] = token 712 | for scope in self.scopes: 713 | IdMap[scope.Id] = scope 714 | for function in self.functions: 715 | IdMap[function.Id] = function 716 | for variable in self.variables: 717 | IdMap[variable.Id] = variable 718 | for variable in arguments: 719 | IdMap[variable.Id] = variable 720 | for values in self.valueflow: 721 | IdMap[values.Id] = values.values 722 | 723 | for token in self.tokenlist: 724 | token.setId(IdMap) 725 | for scope in self.scopes: 726 | scope.setId(IdMap) 727 | for function in self.functions: 728 | function.setId(IdMap) 729 | for variable in self.variables: 730 | variable.setId(IdMap) 731 | for variable in arguments: 732 | variable.setId(IdMap) 733 | 734 | def __repr__(self): 735 | attrs = ["name"] 736 | return "{}({})".format( 737 | "Configuration", 738 | ", ".join(("{}={}".format(a, repr(getattr(self, a))) for a in attrs)) 739 | ) 740 | 741 | 742 | class Platform: 743 | """ 744 | Platform class 745 | This class contains type sizes 746 | 747 | Attributes: 748 | name Name of the platform 749 | char_bit CHAR_BIT value 750 | short_bit SHORT_BIT value 751 | int_bit INT_BIT value 752 | long_bit LONG_BIT value 753 | long_long_bit LONG_LONG_BIT value 754 | pointer_bit POINTER_BIT value 755 | """ 756 | 757 | name = '' 758 | char_bit = 0 759 | short_bit = 0 760 | int_bit = 0 761 | long_bit = 0 762 | long_long_bit = 0 763 | pointer_bit = 0 764 | 765 | def __init__(self, platformnode): 766 | self.name = platformnode.get('name') 767 | self.char_bit = int(platformnode.get('char_bit')) 768 | self.short_bit = int(platformnode.get('short_bit')) 769 | self.int_bit = int(platformnode.get('int_bit')) 770 | self.long_bit = int(platformnode.get('long_bit')) 771 | self.long_long_bit = int(platformnode.get('long_long_bit')) 772 | self.pointer_bit = int(platformnode.get('pointer_bit')) 773 | 774 | def __repr__(self): 775 | attrs = ["name", "char_bit", "short_bit", "int_bit", 776 | "long_bit", "long_long_bit", "pointer_bit"] 777 | return "{}({})".format( 778 | "Platform", 779 | ", ".join(("{}={}".format(a, repr(getattr(self, a))) for a in attrs)) 780 | ) 781 | 782 | 783 | class Standards: 784 | """ 785 | Standards class 786 | This class contains versions of standards that were used for the cppcheck 787 | 788 | Attributes: 789 | c C Standard used 790 | cpp C++ Standard used 791 | posix If Posix was used 792 | """ 793 | 794 | def __init__(self, standardsnode): 795 | self.c = standardsnode.find("c").get("version") 796 | self.cpp = standardsnode.find("cpp").get("version") 797 | self.posix = standardsnode.find("posix") is not None 798 | 799 | def __repr__(self): 800 | attrs = ["c", "cpp", "posix"] 801 | return "{}({})".format( 802 | "Standards", 803 | ", ".join(("{}={}".format(a, repr(getattr(self, a))) for a in attrs)) 804 | ) 805 | 806 | 807 | class CppcheckData: 808 | """ 809 | Class that makes cppcheck dump data available 810 | Contains a list of Configuration instances 811 | 812 | Attributes: 813 | configurations List of Configurations 814 | 815 | To iterate through all configurations use such code: 816 | @code 817 | data = cppcheckdata.parsedump(...) 818 | for cfg in data.configurations: 819 | print('cfg: ' + cfg.name) 820 | @endcode 821 | 822 | To iterate through all tokens in each configuration use such code: 823 | @code 824 | data = cppcheckdata.parsedump(...) 825 | for cfg in data.configurations: 826 | print('cfg: ' + cfg.name) 827 | code = '' 828 | for token in cfg.tokenlist: 829 | code = code + token.str + ' ' 830 | print(' ' + code) 831 | @endcode 832 | 833 | To iterate through all scopes (functions, types, etc) use such code: 834 | @code 835 | data = cppcheckdata.parsedump(...) 836 | for cfg in data.configurations: 837 | print('cfg: ' + cfg.name) 838 | for scope in cfg.scopes: 839 | print(' type:' + scope.type + ' name:' + scope.className) 840 | @endcode 841 | """ 842 | 843 | rawTokens = [] 844 | platform = None 845 | configurations = [] 846 | suppressions = [] 847 | 848 | def __init__(self, filename): 849 | self.configurations = [] 850 | 851 | data = ElementTree.parse(filename) 852 | 853 | for platformNode in data.getroot(): 854 | if platformNode.tag == 'platform': 855 | self.platform = Platform(platformNode) 856 | 857 | for rawTokensNode in data.getroot(): 858 | if rawTokensNode.tag != 'rawtokens': 859 | continue 860 | files = [] 861 | for node in rawTokensNode: 862 | if node.tag == 'file': 863 | files.append(node.get('name')) 864 | elif node.tag == 'tok': 865 | tok = Token(node) 866 | tok.file = files[int(node.get('fileIndex'))] 867 | self.rawTokens.append(tok) 868 | for i in range(len(self.rawTokens) - 1): 869 | self.rawTokens[i + 1].previous = self.rawTokens[i] 870 | self.rawTokens[i].next = self.rawTokens[i + 1] 871 | 872 | for suppressionsNode in data.getroot(): 873 | if suppressionsNode.tag == "suppressions": 874 | for suppression in suppressionsNode: 875 | self.suppressions.append(Suppression(suppression)) 876 | 877 | # root is 'dumps' node, each config has its own 'dump' subnode. 878 | for cfgnode in data.getroot(): 879 | if cfgnode.tag == 'dump': 880 | self.configurations.append(Configuration(cfgnode)) 881 | 882 | def __repr__(self): 883 | attrs = ["configurations", "platform"] 884 | return "{}({})".format( 885 | "CppcheckData", 886 | ", ".join(("{}={}".format(a, repr(getattr(self, a))) for a in attrs)) 887 | ) 888 | 889 | 890 | # Get function arguments 891 | def getArgumentsRecursive(tok, arguments): 892 | if tok is None: 893 | return 894 | if tok.str == ',': 895 | getArgumentsRecursive(tok.astOperand1, arguments) 896 | getArgumentsRecursive(tok.astOperand2, arguments) 897 | else: 898 | arguments.append(tok) 899 | 900 | 901 | def getArguments(ftok): 902 | if (not ftok.isName) or (ftok.next is None) or ftok.next.str != '(': 903 | return None 904 | args = [] 905 | getArgumentsRecursive(ftok.next.astOperand2, args) 906 | return args 907 | 908 | 909 | def parsedump(filename): 910 | """ 911 | parse a cppcheck dump file 912 | """ 913 | return CppcheckData(filename) 914 | 915 | 916 | def astIsFloat(token): 917 | """ 918 | Check if type of ast node is float/double 919 | """ 920 | 921 | if not token: 922 | return False 923 | if token.str == '.': 924 | return astIsFloat(token.astOperand2) 925 | if token.str in '+-*/%': 926 | return astIsFloat(token.astOperand1) or astIsFloat(token.astOperand2) 927 | if not token.variable: 928 | # float literal? 929 | if token.str[0].isdigit(): 930 | for c in token.str: 931 | if c == 'f' or c == '.' or c == 'E': 932 | return True 933 | return False 934 | typeToken = token.variable.typeStartToken 935 | endToken = token.variable.typeEndToken 936 | while typeToken != endToken: 937 | if typeToken.str == 'float' or typeToken.str == 'double': 938 | return True 939 | typeToken = typeToken.next 940 | if typeToken.str == 'float' or typeToken.str == 'double': 941 | return True 942 | return False 943 | 944 | 945 | class CppCheckFormatter(argparse.HelpFormatter): 946 | """ 947 | Properly formats multiline argument helps 948 | """ 949 | def _split_lines(self, text, width): 950 | # this is the RawTextHelpFormatter._split_lines 951 | if text.startswith('R|'): 952 | return text[2:].splitlines() 953 | return argparse.HelpFormatter._split_lines(self, text, width) 954 | 955 | 956 | def ArgumentParser(): 957 | """ 958 | Returns an argparse argument parser with an already-added 959 | argument definition for -t/--template 960 | """ 961 | parser = argparse.ArgumentParser(formatter_class=CppCheckFormatter) 962 | parser.add_argument('-t', '--template', metavar='', 963 | default='{callstack}: ({severity}) {message}', 964 | help="R|Format the error messages. E.g.\n" 965 | "'{file}:{line},{severity},{id},{message}' or\n" 966 | "'{file}({line}):({severity}) {message}' or\n" 967 | "'{callstack} {message}'\n" 968 | "Pre-defined templates: gcc, vs, edit") 969 | parser.add_argument("dumpfile", nargs='*', 970 | help="Path of dump files from cppcheck.") 971 | parser.add_argument("--cli", 972 | help="Addon is executed from Cppcheck", 973 | action="store_true") 974 | parser.add_argument("-q", "--quiet", 975 | help='do not print "Checking ..." lines', 976 | action="store_true") 977 | return parser 978 | 979 | 980 | def simpleMatch(token, pattern): 981 | for p in pattern.split(' '): 982 | if not token or token.str != p: 983 | return False 984 | token = token.next 985 | return True 986 | 987 | 988 | def reportError(location, severity, message, addon, errorId, extra=''): 989 | if '--cli' in sys.argv: 990 | msg = { 'file': location.file, 991 | 'linenr': location.linenr, 992 | 'column': location.column, 993 | 'severity': severity, 994 | 'message': message, 995 | 'addon': addon, 996 | 'errorId': errorId, 997 | 'extra': extra} 998 | sys.stdout.write(json.dumps(msg) + '\n') 999 | else: 1000 | loc = '[%s:%i]' % (location.file, location.linenr) 1001 | if len(extra) > 0: 1002 | message += ' (' + extra + ')' 1003 | sys.stderr.write('%s (%s) %s [%s-%s]\n' % (loc, severity, message, addon, errorId)) 1004 | -------------------------------------------------------------------------------- /Safety/misra/misra.json: -------------------------------------------------------------------------------- 1 | { 2 | "script": "misra", 3 | "args": ["--rule-texts=misra_rules.txt"] 4 | } 5 | -------------------------------------------------------------------------------- /Safety/misra/misra_rules.txt: -------------------------------------------------------------------------------- 1 | Appendix A Summary of guidelines 2 | Rule 1.1 3 | C Syntax violation 4 | Rule 1.2 5 | txt rule 1.2 6 | Rule 1.3 7 | No text specified 8 | Rule 2.1 9 | No text specified 10 | Rule 2.2 11 | No text specified 12 | Rule 2.3 13 | Advisory - All defined types must be used. IE No unused types should be defined. 14 | Rule 2.4 15 | No text specified 16 | Rule 2.5 17 | Advisory - Macro declared but not used 18 | Rule 2.6 19 | No text specified 20 | Rule 2.7 21 | Advisory - All function parameters must be used 22 | Rule 3.1 23 | Required - /* and // cannot used within a comment (ie No nested comments) 24 | Rule 3.2 25 | No text specified 26 | Rule 4.1 27 | No text specified 28 | Rule 4.2 29 | No text specified 30 | Rule 5.1 31 | No text specified 32 | Rule 5.2 33 | No text specified 34 | Rule 5.3 35 | Required - An identifier (variable) in an outer scope shall not be redfined within an inner scope 36 | Rule 5.4 37 | Required - Macro names must be unique 38 | Rule 5.5 39 | No text specified 40 | Rule 5.6 41 | No text specified 42 | Rule 5.7 43 | No text specified 44 | Rule 5.8 45 | Required - All identifiers must be unique 46 | Rule 5.9 47 | No text specified 48 | Rule 6.1 49 | No text specified 50 | Rule 6.2 51 | No text specified 52 | Rule 7.1 53 | No text specified 54 | Rule 7.2 55 | Required - All unsigned integer constants must have a 'u' or 'U' suffix 56 | Rule 7.3 57 | No text specified 58 | Rule 7.4 59 | No text specified 60 | Rule 8.1 61 | No text specified 62 | Rule 8.2 63 | Required - Function prototypes must have named parameters or void if none 64 | Rule 8.3 65 | No text specified 66 | Rule 8.4 67 | Required - Functions must have a prototype and prototype arguements/return types must match 68 | Rule 8.5 69 | Required - An object or function must only be declared once 70 | Rule 8.6 71 | Required - Externally identified object must be defined once and only once (Object has not been defined or was defined in multiple places) 72 | Rule 8.7 73 | Advisory - Objects and functions should only be made available externally if required 74 | Rule 8.8 75 | No text specified 76 | Rule 8.9 77 | No text specified 78 | Rule 8.10 79 | Required - All inline functions should be declared static 80 | Rule 8.11 81 | Advisory - Externally available arrays must have an explicitely declared size 82 | Rule 8.12 83 | No text specified 84 | Rule 8.13 85 | No text specified 86 | Rule 8.14 87 | No text specified 88 | Rule 9.1 89 | No text specified 90 | Rule 9.2 91 | No text specified 92 | Rule 9.3 93 | No text specified 94 | Rule 9.4 95 | No text specified 96 | Rule 9.5 97 | No text specified 98 | Rule 10.1 99 | Required - Operations must be of an essentially correct type (Eg no shift on signed values, comparison is not boolean, incrementing/decrementing a bool etc) 100 | Rule 10.2 101 | No text specified 102 | Rule 10.3 103 | Required - The value from an expression must only be assigned to an object with the same or larger essential type 104 | Rule 10.4 105 | Required - The target of an operation must be of an appropriate type. Eg No implicit conversion of signed/unsigned. 106 | Rule 10.5 107 | No text specified 108 | Rule 10.6 109 | Required - An expression should not assign a value to a variable of a narrower or essentially different type 110 | Rule 10.7 111 | Required - Complex int expression requires cast when widening objects 112 | Rule 10.8 113 | Required - A composite expression value should not be cast to a wider type or a different essential type 114 | Rule 11.1 115 | No text specified 116 | Rule 11.2 117 | No text specified 118 | Rule 11.3 119 | Required - A cast shall not be performed between a pointer to object type and a pointer to a different object type 120 | Rule 11.4 121 | Advisory - Object pointers should not be treated as or converted to integers 122 | Rule 11.5 123 | Advisory - A void pointer should not be converted to an object pointer 124 | Rule 11.6 125 | No text specified 126 | Rule 11.7 127 | No text specified 128 | Rule 11.8 129 | No text specified 130 | Rule 11.9 131 | Required - An integer null pointer shall have no value assigned other than NULL macro 132 | Rule 12.1 133 | Advisory - Order of operations within an expression must be explicit. Multiple conditions in a logical operation should have brackets around them. 134 | Rule 12.2 135 | Required - Right hand side of a shift operation must not exceed the width of the essential value. Potential shift too far or negative shift. 136 | Rule 12.3 137 | Advisory - The comma operator is not permitted. 138 | Rule 12.4 139 | No text specified 140 | Rule 13.1 141 | No text specified 142 | Rule 13.2 143 | No text specified 144 | Rule 13.3 145 | No text specified 146 | Rule 13.4 147 | Advisory - Assignment operation should not be used in an expression (Check for = when == should have been used) 148 | Rule 13.5 149 | Required - The right hand operand of a logical && or || operator shall not contain persistent side effects. 150 | Rule 13.6 151 | No text specified 152 | Rule 14.1 153 | No text specified 154 | Rule 14.2 155 | No text specified 156 | Rule 14.3 157 | No text specified 158 | Rule 14.4 159 | Required - Non-Boolean type expression used in an if statement or the conditional iteration statement of a loop 160 | Rule 15.1 161 | No text specified 162 | Rule 15.2 163 | No text specified 164 | Rule 15.3 165 | No text specified 166 | Rule 15.4 167 | No text specified 168 | Rule 15.5 169 | Advisory - A function should only have a single return point 170 | Rule 15.6 171 | Required - Loops, switch and if/else statements must have brackets around their body 172 | Rule 15.7 173 | Required - 'else if' statements must terminate with a final 'else' 174 | Rule 16.1 175 | No text specified 176 | Rule 16.2 177 | No text specified 178 | Rule 16.3 179 | Required - All cases within switches must have an unconditional break statement 180 | Rule 16.4 181 | Required - All switch statements must have a default case 182 | Rule 16.5 183 | No text specified 184 | Rule 16.6 185 | No text specified 186 | Rule 16.7 187 | No text specified 188 | Rule 17.1 189 | No text specified 190 | Rule 17.2 191 | Required - Functions shall not call themselves, either directly or indirectly 192 | Rule 17.3 193 | Mandatory - No implicit function declarations 194 | Rule 17.4 195 | No text specified 196 | Rule 17.5 197 | No text specified 198 | Rule 17.6 199 | No text specified 200 | Rule 17.7 201 | Required - The value returned by a function having non-void return type shall be used 202 | Rule 17.8 203 | Advisory - An argument to a function should be treated as read-only 204 | Rule 18.1 205 | No text specified 206 | Rule 18.2 207 | No text specified 208 | Rule 18.3 209 | No text specified 210 | Rule 18.4 211 | Advisory - The +, -, += and -= operators should not be applied to an expression of pointer type 212 | Rule 18.5 213 | No text specified 214 | Rule 18.6 215 | No text specified 216 | Rule 18.7 217 | No text specified 218 | Rule 18.8 219 | No text specified 220 | Rule 19.1 221 | No text specified 222 | Rule 19.2 223 | No text specified 224 | Rule 20.1 225 | Required - Only comments and prepropccesor directives can come before #include statements 226 | Rule 20.2 227 | No text specified 228 | Rule 20.3 229 | Required - #include statements must be followed by a filename 230 | Rule 20.4 231 | No text specified 232 | Rule 20.5 233 | Advisory - Use of #undef is not permitted 234 | Rule 20.6 235 | No text specified 236 | Rule 20.7 237 | Required - Macro expressions and parameters must be enclosed in parentheses 238 | Rule 20.8 239 | No text specified 240 | Rule 20.9 241 | No text specified 242 | Rule 20.10 243 | No text specified 244 | Rule 20.11 245 | No text specified 246 | Rule 20.12 247 | No text specified 248 | Rule 20.13 249 | No text specified 250 | Rule 20.14 251 | No text specified 252 | Rule 21.1 253 | No text specified 254 | Rule 21.2 255 | No text specified 256 | Rule 21.3 257 | Required - Memory allocation functions (Eg malloc(), talloc() etc) shall not be used 258 | Rule 21.4 259 | No text specified 260 | Rule 21.5 261 | No text specified 262 | Rule 21.6 263 | No text specified 264 | Rule 21.7 265 | No text specified 266 | Rule 21.8 267 | Required - Termination functions from stdlib (Eg exit, abort etc) shall not be used 268 | Rule 21.9 269 | No text specified 270 | Rule 21.10 271 | No text specified 272 | Rule 21.11 273 | No text specified 274 | Rule 21.12 275 | No text specified 276 | Rule 22.1 277 | No text specified 278 | Rule 22.2 279 | No text specified 280 | Rule 22.3 281 | No text specified 282 | Rule 22.4 283 | No text specified 284 | Rule 22.5 285 | No text specified 286 | Rule 22.6 287 | No text specified 288 | -------------------------------------------------------------------------------- /Safety/misra/misra_violations_output.txt: -------------------------------------------------------------------------------- 1 | Checking ../../BrakeModule_sw03/BrakeModule_sw03.ino ... 2 | Checking ../../BrakeModule_sw03/BrakeModule_sw03.ino: DEBUG... 3 | Checking ../../BrakeModule_sw03/BrakeModule_sw03.ino: FAN_CTRL... 4 | ../../BrakeModule_sw03/BrakeModule_sw03.ino:2:1: style: Required - /* and // cannot used within a comment (ie No nested comments) [misra-c2012-3.1] 5 | /* 6 | ^ 7 | ../../BrakeModule_sw03/STM32F1_CAN.h:61:11: style: Required - The value returned by a function having non-void return type shall be used [misra-c2012-17.7] 8 | memset(out_timings, 0, sizeof(*out_timings)); 9 | ^ 10 | *** MISRA violations count was 2 *** 11 | -------------------------------------------------------------------------------- /Safety/misra/suppressions.txt: -------------------------------------------------------------------------------- 1 | #All Advisory rules are suppressed: 2 | misra-c2012-1.2 3 | misra-c2012-2.3 4 | misra-c2012-2.4 5 | misra-c2012-2.5 6 | misra-c2012-2.6 7 | misra-c2012-2.7 8 | misra-c2012-4.2 9 | misra-c2012-5.9 10 | misra-c2012-8.7 11 | misra-c2012-8.9 12 | misra-c2012-8.13 13 | misra-c2012-10.5 14 | misra-c2012-11.4 15 | misra-c2012-11.5 16 | misra-c2012-12.1 17 | misra-c2012-12.3 18 | misra-c2012-12.4 19 | misra-c2012-13.3 20 | misra-c2012-13.4 21 | misra-c2012-15.1 22 | misra-c2012-15.4 23 | misra-c2012-15.5 24 | misra-c2012-17.5 25 | misra-c2012-17.8 26 | misra-c2012-18.4 27 | misra-c2012-18.5 28 | misra-c2012-19.2 29 | misra-c2012-20.1 30 | misra-c2012-20.5 31 | misra-c2012-20.10 32 | misra-c2012-21.12 33 | -------------------------------------------------------------------------------- /dsc_system.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/killinen/BrakeModule/52e397301addb98a9d2e167176d838cb5e5ef9ed/dsc_system.pdf --------------------------------------------------------------------------------