├── stampfly_firmware_0.0.1.bin ├── stampfly_firmware_0.0.1.2.bin ├── stampfly_firmware_0.0.1.3.bin ├── firmware ├── stampfly_firmware_v1.0.0_20240802.bin └── stampfly_firmware_v1.1.0_20240812.bin ├── src ├── button.hpp ├── buzzer.h ├── buzzer.cpp ├── telemetry.hpp ├── main.cpp ├── tof.hpp ├── imu.hpp ├── button.cpp ├── led.hpp ├── pid.hpp ├── rc.hpp ├── alt_kalman.hpp ├── pid.cpp ├── sensor.hpp ├── alt_kalman.cpp ├── imu.cpp ├── led.cpp └── flight_control.hpp ├── README.md ├── .github ├── workflows │ ├── Arduino-Lint-Check.yml │ └── clang-format-check.yml └── ISSUE_TEMPLATE │ └── bug-report.yml ├── .gitignore ├── lib ├── vl53l3c │ ├── vl53lx_silicon_core.h │ ├── vl53lx_hist_char.h │ ├── vl53lx_xtalk_private_structs.h │ ├── vl53lx_error_exceptions.h │ ├── vl53lx_dmax_structs.h │ ├── vl53lx_dmax.h │ ├── vl53lx_wait.h │ ├── vl53lx_wait copy.h │ ├── vl53lx_nvm_debug.h │ ├── vl53lx_dmax_private_structs.h │ ├── vl53lx_platform_ipp_imports.h │ ├── vl53lx_platform_ipp.h │ ├── vl53lx_hist_funcs.h │ ├── vl53lx_sigma_estimate.h │ ├── vl53lx_hist_map.h │ ├── vl53lx_platform_init.c │ ├── vl53lx_silicon_core.c │ ├── vl53lx_hist_algos_gen4.h │ ├── vl53lx_hist_core.h │ ├── vl53lx_platform_user_defines.h │ ├── mxconstants.h │ ├── vl53lx_preset_setup.h │ ├── vl53lx_api_calibration.h │ ├── vl53lx_platform_ipp.c │ ├── vl53lx_core_support.h │ ├── stm32xxx_hal.h │ ├── vl53lx_types.h │ ├── vl53lx_platform_init.h │ ├── vl53l1_platform_user_defines.h │ ├── vl53lx_hist_char.c │ ├── vl53l1_platform_user_config.h │ ├── vl53lx_platform_log.c │ ├── vl53lx_nvm.h │ ├── vl53lx_platform_user_data.h │ ├── vl53lx_xtalk.h │ ├── vl53lx_hist_algos_gen3.h │ ├── vl53lx_register_settings.h │ ├── vl53lx_platform_user_config.h │ ├── vl53lx_api_preset_modes.h │ ├── vl53lx_sigma_estimate.c │ ├── vl53lx_hist_private_structs.h │ ├── vl53lx_nvm_structs.h │ └── vl53lx_hist_structs.h ├── README ├── bmi270 │ ├── README.md │ ├── LICENSE │ ├── bmi270_maximum_fifo.h │ └── OIS_README.md └── MdgwickAHRS │ └── MadgwickAHRS.h ├── platformio.ini ├── generate_user_custom.py ├── LICENSE ├── include └── README └── .clang-format /stampfly_firmware_0.0.1.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/M5Fly-kanazawa/M5StampFly/HEAD/stampfly_firmware_0.0.1.bin -------------------------------------------------------------------------------- /stampfly_firmware_0.0.1.2.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/M5Fly-kanazawa/M5StampFly/HEAD/stampfly_firmware_0.0.1.2.bin -------------------------------------------------------------------------------- /stampfly_firmware_0.0.1.3.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/M5Fly-kanazawa/M5StampFly/HEAD/stampfly_firmware_0.0.1.3.bin -------------------------------------------------------------------------------- /firmware/stampfly_firmware_v1.0.0_20240802.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/M5Fly-kanazawa/M5StampFly/HEAD/firmware/stampfly_firmware_v1.0.0_20240802.bin -------------------------------------------------------------------------------- /firmware/stampfly_firmware_v1.1.0_20240812.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/M5Fly-kanazawa/M5StampFly/HEAD/firmware/stampfly_firmware_v1.1.0_20240812.bin -------------------------------------------------------------------------------- /src/button.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SPDX-FileCopyrightText: 2024 M5Stack Technology CO LTD 3 | * 4 | * SPDX-License-Identifier: MIT 5 | */ 6 | #ifndef BUTTON_HPP 7 | #define BUTTON_HPP 8 | 9 | bool init_button(void); 10 | 11 | #endif -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # StampFly 2 | 3 | ## Framework 4 | 5 | Platformio 6 | 7 | ## Base on project 8 | 9 | [M5Fly-kanazawa/StampFly2024June (github.com)](https://github.com/M5Fly-kanazawa/StampFly2024June) 10 | 11 | ## Product introduction 12 | 13 | [M5Stampfly](https://docs.m5stack.com/en/app/Stamp%20Fly) 14 | 15 | ## Third-party libraries 16 | 17 | fastled/FastLED 18 | 19 | tinyu-zhao/INA3221 20 | 21 | mathertel/OneButton @ ^2.5.0 -------------------------------------------------------------------------------- /.github/workflows/Arduino-Lint-Check.yml: -------------------------------------------------------------------------------- 1 | name: Arduino Lint Check 2 | on: 3 | push: 4 | branches: [ master ] 5 | pull_request: 6 | branches: [ master ] 7 | jobs: 8 | lint: 9 | name: Lint Check 10 | runs-on: ubuntu-latest 11 | steps: 12 | - uses: actions/checkout@v2 13 | - uses: arduino/arduino-lint-action@v1 14 | with: 15 | library-manager: update 16 | compliance: strict 17 | project-type: all -------------------------------------------------------------------------------- /src/buzzer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SPDX-FileCopyrightText: 2024 M5Stack Technology CO LTD 3 | * 4 | * SPDX-License-Identifier: MIT 5 | */ 6 | #ifndef BUZZER_H 7 | #define BUZZER_H 8 | 9 | #define NOTE_D1 294 10 | #define NOTE_D2 330 11 | #define NOTE_D3 350 12 | #define NOTE_D4 393 13 | #define NOTE_D5 441 14 | #define NOTE_D6 495 15 | #define NOTE_D7 556 16 | 17 | void setup_pwm_buzzer(void); 18 | void beep(void); 19 | void start_tone(void); 20 | void buzzer_sound(uint32_t frequency, uint32_t duration_ms); 21 | 22 | #endif 23 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | 34 | .pio 35 | build/ 36 | .vscode/.browse.c_cpp.db* 37 | .vscode/c_cpp_properties.json 38 | .vscode/launch.json 39 | .vscode/ipch 40 | .vscode 41 | .DS_Store 42 | src/.DS_Store 43 | lib/.DS_Store 44 | include/.DS_Store 45 | build/.DS_Store 46 | log/ -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_silicon_core.h: -------------------------------------------------------------------------------- 1 | 2 | /* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */ 3 | /****************************************************************************** 4 | * Copyright (c) 2020, STMicroelectronics - All Rights Reserved 5 | 6 | This file is part of VL53LX and is dual licensed, 7 | either GPL-2.0+ 8 | or 'BSD 3-clause "New" or "Revised" License' , at your option. 9 | ****************************************************************************** 10 | */ 11 | 12 | 13 | 14 | 15 | #ifndef _VL53LX_SILICON_CORE_H_ 16 | #define _VL53LX_SILICON_CORE_H_ 17 | 18 | #include "vl53lx_platform.h" 19 | 20 | #ifdef __cplusplus 21 | extern "C" { 22 | #endif 23 | 24 | 25 | 26 | 27 | VL53LX_Error VL53LX_is_firmware_ready_silicon( 28 | VL53LX_DEV Dev, 29 | uint8_t *pready); 30 | 31 | 32 | #ifdef __cplusplus 33 | } 34 | #endif 35 | 36 | #endif 37 | 38 | -------------------------------------------------------------------------------- /.github/workflows/clang-format-check.yml: -------------------------------------------------------------------------------- 1 | name: clang-format Check 2 | on: [push, pull_request] 3 | jobs: 4 | formatting-check: 5 | name: Formatting Check 6 | runs-on: ubuntu-latest 7 | strategy: 8 | matrix: 9 | path: 10 | - check: './' # path to include 11 | exclude: './lib' # path to exclude 12 | exclude: './include' # path to exclude 13 | # - check: 'src' 14 | # exclude: '(Fonts)' # Exclude file paths containing "Fonts" 15 | # - check: 'examples' 16 | # exclude: '' 17 | steps: 18 | - uses: actions/checkout@v3.1.0 19 | - name: Run clang-format style check for C/C++/Protobuf programs. 20 | uses: jidicula/clang-format-action@v4.9.0 21 | with: 22 | clang-format-version: '13' 23 | check-path: ${{ matrix.path['check'] }} 24 | exclude-regex: ${{ matrix.path['exclude'] }} 25 | -------------------------------------------------------------------------------- /platformio.ini: -------------------------------------------------------------------------------- 1 | ; PlatformIO Project Configuration File 2 | ; 3 | ; Build options: build flags, source filter 4 | ; Upload options: custom upload port, speed and extra flags 5 | ; Library options: dependencies, extra library storages 6 | ; Advanced options: extra scripting 7 | ; 8 | ; Please visit documentation for the other options and examples 9 | ; https://docs.platformio.org/page/projectconf.html 10 | 11 | [env] 12 | platform = espressif32 13 | board = esp32-s3-devkitc-1 14 | framework = arduino 15 | monitor_speed = 115200 16 | lib_deps = 17 | fastled/FastLED 18 | tinyu-zhao/INA3221 19 | mathertel/OneButton @ ^2.5.0 20 | 21 | [env:esp32-s3-devkitc-1] 22 | build_type = release 23 | build_flags = -DCORE_DEBUG_LEVEL=0 -O0 24 | extra_scripts = post:generate_user_custom.py 25 | custom_firmware_version = 0.0.1.3 26 | custom_firmware_name = stampfly_firmware 27 | custom_firmware_suffix = .bin 28 | custom_firmware_dir = ./ 29 | -------------------------------------------------------------------------------- /generate_user_custom.py: -------------------------------------------------------------------------------- 1 | Import("env") 2 | import os 3 | 4 | firmware_name = env.GetProjectOption("custom_firmware_name") 5 | firmware_version = env.GetProjectOption('custom_firmware_version') 6 | firmware_suffix = env.GetProjectOption('custom_firmware_suffix') 7 | firmware_dir = env.GetProjectOption('custom_firmware_dir') 8 | 9 | firmware_bin = '%s_%s%s' % (firmware_name, firmware_version, firmware_suffix) 10 | firmware_path = os.path.join(firmware_dir, firmware_bin) 11 | 12 | app_bin = os.path.join('$BUILD_DIR','${PROGNAME}%s' % firmware_suffix) 13 | 14 | env.AddCustomTarget( 15 | name="firmware", 16 | dependencies=['buildfs'], 17 | actions=[ 18 | '"$PYTHONEXE" "$UPLOADER" --chip esp32 merge_bin ' 19 | '-o %s %s $ESP32_APP_OFFSET %s' 20 | % (firmware_path, ' '.join([ 21 | addr + " " + name for addr, name in env['FLASH_EXTRA_IMAGES'] 22 | ]), app_bin) 23 | ], 24 | title="Generate User Custom") 25 | -------------------------------------------------------------------------------- /src/buzzer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SPDX-FileCopyrightText: 2024 M5Stack Technology CO LTD 3 | * 4 | * SPDX-License-Identifier: MIT 5 | */ 6 | #include "Arduino.h" 7 | #include 8 | #include "buzzer.h" 9 | 10 | const int buzzerPin = 40; 11 | const int channel = 5; 12 | 13 | void setup_pwm_buzzer(void) { 14 | ledcSetup(channel, 4000, 8); // 配置PWM通道：通道0，频率3000Hz，分辨率8位 15 | ledcAttachPin(buzzerPin, channel); // 将PWM通道绑定到GPIO 16 | } 17 | 18 | void buzzer_sound(uint32_t frequency, uint32_t duration_ms) { 19 | ledcWriteTone(channel, frequency); 20 | ledcWrite(channel, 127); 21 | 22 | vTaskDelay(duration_ms / portTICK_PERIOD_MS); 23 | 24 | ledcWriteTone(channel, 0); 25 | digitalWrite(channel, 0); 26 | } 27 | 28 | void beep(void) { 29 | buzzer_sound(4000, 100); 30 | } 31 | 32 | void start_tone(void) { 33 | buzzer_sound(NOTE_D1, 200); 34 | buzzer_sound(NOTE_D5, 200); 35 | buzzer_sound(NOTE_D3, 200); 36 | buzzer_sound(NOTE_D4, 200); 37 | } -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2024 M5Stack 4 | Copyright (c) 2024 Kouhei Ito 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | -------------------------------------------------------------------------------- /lib/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for project specific (private) libraries. 3 | PlatformIO will compile them to static libraries and link into executable file. 4 | 5 | The source code of each library should be placed in a an own separate directory 6 | ("lib/your_library_name/[here are source files]"). 7 | 8 | For example, see a structure of the following two libraries `Foo` and `Bar`: 9 | 10 | |--lib 11 | | | 12 | | |--Bar 13 | | | |--docs 14 | | | |--examples 15 | | | |--src 16 | | | |- Bar.c 17 | | | |- Bar.h 18 | | | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html 19 | | | 20 | | |--Foo 21 | | | |- Foo.c 22 | | | |- Foo.h 23 | | | 24 | | |- README --> THIS FILE 25 | | 26 | |- platformio.ini 27 | |--src 28 | |- main.c 29 | 30 | and a contents of `src/main.c`: 31 | ``` 32 | #include 33 | #include 34 | 35 | int main (void) 36 | { 37 | ... 38 | } 39 | 40 | ``` 41 | 42 | PlatformIO Library Dependency Finder will find automatically dependent 43 | libraries scanning project source files. 44 | 45 | More information about PlatformIO Library Dependency Finder 46 | - https://docs.platformio.org/page/librarymanager/ldf.html 47 | -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_hist_char.h: -------------------------------------------------------------------------------- 1 | 2 | /* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */ 3 | /****************************************************************************** 4 | * Copyright (c) 2020, STMicroelectronics - All Rights Reserved 5 | 6 | This file is part of VL53LX and is dual licensed, 7 | either GPL-2.0+ 8 | or 'BSD 3-clause "New" or "Revised" License' , at your option. 9 | ****************************************************************************** 10 | */ 11 | 12 | 13 | 14 | 15 | 16 | #ifndef _VL53LX_HIST_CHAR_H_ 17 | #define _VL53LX_HIST_CHAR_H_ 18 | 19 | #include "vl53lx_platform.h" 20 | 21 | #ifdef __cplusplus 22 | extern "C" 23 | { 24 | #endif 25 | 26 | 27 | 28 | 29 | VL53LX_Error VL53LX_set_calib_config( 30 | VL53LX_DEV Dev, 31 | uint8_t vcsel_delay__a0, 32 | uint8_t calib_1, 33 | uint8_t calib_2, 34 | uint8_t calib_3, 35 | uint8_t calib_2__a0, 36 | uint8_t spad_readout); 37 | 38 | 39 | 40 | 41 | VL53LX_Error VL53LX_set_hist_calib_pulse_delay( 42 | VL53LX_DEV Dev, 43 | uint8_t calib_delay); 44 | 45 | 46 | 47 | 48 | VL53LX_Error VL53LX_disable_calib_pulse_delay( 49 | VL53LX_DEV Dev); 50 | 51 | 52 | #ifdef __cplusplus 53 | } 54 | #endif 55 | 56 | #endif 57 | 58 | -------------------------------------------------------------------------------- /src/telemetry.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * MIT License 3 | * 4 | * Copyright (c) 2024 Kouhei Ito 5 | * Copyright (c) 2024 M5Stack 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the "Software"), to deal 9 | * in the Software without restriction, including without limitation the rights 10 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | * copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in all 15 | * copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | * SOFTWARE. 24 | */ 25 | 26 | #ifndef TELEMETRY_HPP 27 | #define TELEMETRY_HPP 28 | 29 | #include 30 | 31 | void telemetry(void); 32 | void telemetry_fast(void); 33 | 34 | #endif -------------------------------------------------------------------------------- /lib/bmi270/README.md: -------------------------------------------------------------------------------- 1 | BMI270 Sensor API 2 | 3 | > This package contains the sensor APIs for the BMI270 sensor 4 | 5 | ## Sensor Overview 6 | The BMI270 is a small, low power, low noise inertial measurement unit designed for use in mobile applications like augmented reality or indoor navigation which require highly accurate, real-time sensor data. 7 | 8 | ## Applications 9 | 10 | ### BMI270 (base) 11 | 12 | - Any motion, No motion, Significant motion detectors 13 | - Wrist worn Step counter and Step detector (Pedometer) 14 | - Activity change recognition 15 | - Still 16 | - Walking 17 | - Running 18 | - Wrist gestures 19 | - Push arm down 20 | - Pivot up 21 | - Wrist shake jiggle 22 | - Flick in 23 | - Flick out 24 | - Wrist wear wake up 25 | 26 | ### BMI270 Context 27 | 28 | - Step counter and Step detector (Pedometer) 29 | - Activity change recognition 30 | - Still 31 | - Walking 32 | - Running 33 | 34 | ### BMI270 Legacy 35 | 36 | - Any motion, No motion, Significant motion detector 37 | - Orientation detector (Advanced Potrait-Landscape) 38 | - High-G, Low-G (Freefall) detector 39 | - Flat detector 40 | - Tap detection (Single, Double, Triple taps) 41 | - Smartphone Step counter and Step detector (Pedometer) 42 | - Activity change recognition 43 | - Still 44 | - Walking 45 | - Running 46 | 47 | ### BMI270 Maximum FIFO 48 | 49 | - Supports a 6kB FIFO 50 | 51 | For more information refer product page [Link](https://www.bosch-sensortec.com/products/motion-sensors/imus/bmi270.html) 52 | 53 | --- -------------------------------------------------------------------------------- /include/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for project header files. 3 | 4 | A header file is a file containing C declarations and macro definitions 5 | to be shared between several project source files. You request the use of a 6 | header file in your project source file (C, C++, etc) located in `src` folder 7 | by including it, with the C preprocessing directive `#include'. 8 | 9 | ```src/main.c 10 | 11 | #include "header.h" 12 | 13 | int main (void) 14 | { 15 | ... 16 | } 17 | ``` 18 | 19 | Including a header file produces the same results as copying the header file 20 | into each source file that needs it. Such copying would be time-consuming 21 | and error-prone. With a header file, the related declarations appear 22 | in only one place. If they need to be changed, they can be changed in one 23 | place, and programs that include the header file will automatically use the 24 | new version when next recompiled. The header file eliminates the labor of 25 | finding and changing all the copies as well as the risk that a failure to 26 | find one copy will result in inconsistencies within a program. 27 | 28 | In C, the usual convention is to give header files names that end with `.h'. 29 | It is most portable to use only letters, digits, dashes, and underscores in 30 | header file names, and at most one dot. 31 | 32 | Read more about using header files in official GCC documentation: 33 | 34 | * Include Syntax 35 | * Include Operation 36 | * Once-Only Headers 37 | * Computed Includes 38 | 39 | https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html 40 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * MIT License 3 | * 4 | * Copyright (c) 2024 Kouhei Ito 5 | * Copyright (c) 2024 M5Stack 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the "Software"), to deal 9 | * in the Software without restriction, including without limitation the rights 10 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | * copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in all 15 | * copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | * SOFTWARE. 24 | */ 25 | 26 | #include 27 | #include 28 | #include "flight_control.hpp" 29 | 30 | // VL53L0X_ADDRESS 0x29 31 | // MPU6886_ADDRESS 0x68 32 | // BMP280_ADDRESS 0x76 33 | 34 | void setup() { 35 | init_copter(); 36 | delay(100); 37 | } 38 | 39 | void loop() { 40 | loop_400Hz(); 41 | } 42 | -------------------------------------------------------------------------------- /lib/bmi270/LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2023 Bosch Sensortec GmbH. All rights reserved. 2 | 3 | BSD-3-Clause 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in the 13 | documentation and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 25 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 27 | HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 28 | STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING 29 | IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | POSSIBILITY OF SUCH DAMAGE. -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_xtalk_private_structs.h: -------------------------------------------------------------------------------- 1 | 2 | /* SPDX-License-Identifier: BSD-3-Clause */ 3 | /****************************************************************************** 4 | * Copyright (c) 2020, STMicroelectronics - All Rights Reserved 5 | 6 | This file is part of VL53LX Protected and is dual licensed, 7 | either 'STMicroelectronics Proprietary license' 8 | or 'BSD 3-clause "New" or "Revised" License' , at your option. 9 | 10 | ****************************************************************************** 11 | 12 | 'STMicroelectronics Proprietary license' 13 | 14 | ****************************************************************************** 15 | 16 | License terms: STMicroelectronics Proprietary in accordance with licensing 17 | terms at www.st.com/sla0081 18 | 19 | ****************************************************************************** 20 | */ 21 | 22 | 23 | 24 | 25 | 26 | 27 | #ifndef _VL53LX_XTALK_PRIVATE_STRUCTS_H_ 28 | #define _VL53LX_XTALK_PRIVATE_STRUCTS_H_ 29 | 30 | #include "vl53lx_types.h" 31 | #include "vl53lx_hist_structs.h" 32 | 33 | 34 | #ifdef __cplusplus 35 | extern "C" 36 | { 37 | #endif 38 | 39 | #define VL53LX_D_012 4 40 | 41 | 42 | 43 | typedef struct { 44 | 45 | 46 | 47 | 48 | uint32_t VL53LX_p_061[VL53LX_D_012]; 49 | 50 | 51 | int16_t VL53LX_p_059; 52 | 53 | int16_t VL53LX_p_060; 54 | 55 | 56 | VL53LX_histogram_bin_data_t VL53LX_p_056; 57 | 58 | VL53LX_histogram_bin_data_t VL53LX_p_057; 59 | 60 | 61 | 62 | uint32_t VL53LX_p_058; 63 | 64 | 65 | uint32_t VL53LX_p_062[VL53LX_XTALK_HISTO_BINS]; 66 | 67 | 68 | } VL53LX_xtalk_algo_data_t; 69 | 70 | #ifdef __cplusplus 71 | } 72 | #endif 73 | 74 | #endif 75 | 76 | -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_error_exceptions.h: -------------------------------------------------------------------------------- 1 | 2 | /* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */ 3 | /****************************************************************************** 4 | * Copyright (c) 2020, STMicroelectronics - All Rights Reserved 5 | 6 | This file is part of VL53LX and is dual licensed, 7 | either GPL-2.0+ 8 | or 'BSD 3-clause "New" or "Revised" License' , at your option. 9 | ****************************************************************************** 10 | */ 11 | 12 | 13 | 14 | 15 | #ifndef _VL53LX_ERROR_EXCEPTIONS_H_ 16 | #define _VL53LX_ERROR_EXCEPTIONS_H_ 17 | 18 | #define IGNORE_DIVISION_BY_ZERO 0 19 | 20 | #define IGNORE_XTALK_EXTRACTION_NO_SAMPLE_FAIL 0 21 | #define IGNORE_XTALK_EXTRACTION_SIGMA_LIMIT_FAIL 0 22 | #define IGNORE_XTALK_EXTRACTION_NO_SAMPLE_FOR_GRADIENT_WARN 0 23 | #define IGNORE_XTALK_EXTRACTION_SIGMA_LIMIT_FOR_GRADIENT_WARN 0 24 | #define IGNORE_XTALK_EXTRACTION_MISSING_SAMPLES_WARN 0 25 | 26 | #define IGNORE_REF_SPAD_CHAR_NOT_ENOUGH_SPADS 0 27 | #define IGNORE_REF_SPAD_CHAR_RATE_TOO_HIGH 0 28 | #define IGNORE_REF_SPAD_CHAR_RATE_TOO_LOW 0 29 | 30 | #define IGNORE_OFFSET_CAL_MISSING_SAMPLES 0 31 | #define IGNORE_OFFSET_CAL_SIGMA_TOO_HIGH 0 32 | #define IGNORE_OFFSET_CAL_RATE_TOO_HIGH 0 33 | #define IGNORE_OFFSET_CAL_SPAD_COUNT_TOO_LOW 0 34 | 35 | #define IGNORE_ZONE_CAL_MISSING_SAMPLES 0 36 | #define IGNORE_ZONE_CAL_SIGMA_TOO_HIGH 0 37 | #define IGNORE_ZONE_CAL_RATE_TOO_HIGH 0 38 | 39 | #endif 40 | 41 | -------------------------------------------------------------------------------- /src/tof.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * MIT License 3 | * 4 | * Copyright (c) 2024 Kouhei Ito 5 | * Copyright (c) 2024 M5Stack 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the "Software"), to deal 9 | * in the Software without restriction, including without limitation the rights 10 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | * copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in all 15 | * copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | * SOFTWARE. 24 | */ 25 | 26 | #ifndef TOF_HPP 27 | #define TOF_HPP 28 | 29 | #include 30 | #include 31 | #include 32 | 33 | #define INT_BOTTOM 6 34 | #define XSHUT_BOTTOM 7 35 | #define INT_FRONT 8 36 | #define XSHUT_FRONT 9 37 | #define USER_A 0 38 | 39 | void tof_init(void); 40 | int16_t tof_range_get(VL53LX_DEV dev); 41 | void tof_test_ranging(VL53LX_DEV dev); 42 | int16_t tof_bottom_get_range(); 43 | int16_t tof_front_get_range(); 44 | 45 | #endif -------------------------------------------------------------------------------- /src/imu.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * MIT License 3 | * 4 | * Copyright (c) 2024 Kouhei Ito 5 | * Copyright (c) 2024 M5Stack 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the "Software"), to deal 9 | * in the Software without restriction, including without limitation the rights 10 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | * copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in all 15 | * copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | * SOFTWARE. 24 | */ 25 | 26 | #ifndef IMU_HPP 27 | #define IMU_HPP 28 | 29 | #include 30 | #include "common.h" 31 | #include "bmi2.h" 32 | 33 | #define DPS20002RAD 34.90658504 34 | #define DPS10002RAD 17.4532925199 35 | 36 | void imu_init(void); 37 | void imu_test(void); 38 | void imu_update(void); 39 | float imu_get_acc_x(void); 40 | float imu_get_acc_y(void); 41 | float imu_get_acc_z(void); 42 | float imu_get_gyro_x(void); 43 | float imu_get_gyro_y(void); 44 | float imu_get_gyro_z(void); 45 | 46 | #endif -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_dmax_structs.h: -------------------------------------------------------------------------------- 1 | 2 | /* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */ 3 | /****************************************************************************** 4 | * Copyright (c) 2020, STMicroelectronics - All Rights Reserved 5 | 6 | This file is part of VL53LX and is dual licensed, 7 | either GPL-2.0+ 8 | or 'BSD 3-clause "New" or "Revised" License' , at your option. 9 | ****************************************************************************** 10 | */ 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | #ifndef _VL53LX_DMAX_STRUCTS_H_ 19 | #define _VL53LX_DMAX_STRUCTS_H_ 20 | 21 | #include "vl53lx_types.h" 22 | 23 | #ifdef __cplusplus 24 | extern "C" 25 | { 26 | #endif 27 | 28 | 29 | #define VL53LX_MAX_AMBIENT_DMAX_VALUES 5 30 | 31 | 32 | 33 | 34 | 35 | typedef struct { 36 | 37 | 38 | 39 | uint16_t ref__actual_effective_spads; 40 | 41 | uint16_t ref__peak_signal_count_rate_mcps; 42 | 43 | uint16_t ref__distance_mm; 44 | 45 | uint16_t ref_reflectance_pc; 46 | 47 | 48 | 49 | 50 | uint16_t coverglass_transmission; 51 | 52 | 53 | } VL53LX_dmax_calibration_data_t; 54 | 55 | 56 | 57 | 58 | typedef struct { 59 | 60 | 61 | 62 | uint8_t signal_thresh_sigma; 63 | 64 | uint8_t ambient_thresh_sigma; 65 | 66 | int32_t min_ambient_thresh_events; 67 | 68 | int32_t signal_total_events_limit; 69 | 70 | 71 | uint16_t target_reflectance_for_dmax_calc[ 72 | VL53LX_MAX_AMBIENT_DMAX_VALUES]; 73 | 74 | uint16_t max_effective_spads; 75 | 76 | 77 | 78 | 79 | uint16_t dss_config__target_total_rate_mcps; 80 | 81 | uint8_t dss_config__aperture_attenuation; 82 | 83 | 84 | } VL53LX_hist_gen3_dmax_config_t; 85 | 86 | 87 | #ifdef __cplusplus 88 | } 89 | #endif 90 | 91 | #endif 92 | 93 | -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_dmax.h: -------------------------------------------------------------------------------- 1 | 2 | /* SPDX-License-Identifier: BSD-3-Clause */ 3 | /****************************************************************************** 4 | * Copyright (c) 2020, STMicroelectronics - All Rights Reserved 5 | 6 | This file is part of VL53LX Protected and is dual licensed, 7 | either 'STMicroelectronics Proprietary license' 8 | or 'BSD 3-clause "New" or "Revised" License' , at your option. 9 | 10 | ****************************************************************************** 11 | 12 | 'STMicroelectronics Proprietary license' 13 | 14 | ****************************************************************************** 15 | 16 | License terms: STMicroelectronics Proprietary in accordance with licensing 17 | terms at www.st.com/sla0081 18 | 19 | ****************************************************************************** 20 | */ 21 | 22 | 23 | 24 | 25 | #ifndef _VL53LX_DMAX_H_ 26 | #define _VL53LX_DMAX_H_ 27 | 28 | #include "vl53lx_types.h" 29 | #include "vl53lx_hist_structs.h" 30 | #include "vl53lx_dmax_private_structs.h" 31 | #include "vl53lx_error_codes.h" 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | 38 | 39 | 40 | VL53LX_Error VL53LX_f_001( 41 | uint16_t target_reflectance, 42 | VL53LX_dmax_calibration_data_t *pcal, 43 | VL53LX_hist_gen3_dmax_config_t *pcfg, 44 | VL53LX_histogram_bin_data_t *pbins, 45 | VL53LX_hist_gen3_dmax_private_data_t *pdata, 46 | int16_t *pambient_dmax_mm); 47 | 48 | 49 | 50 | 51 | uint32_t VL53LX_f_002( 52 | uint32_t events_threshold, 53 | uint32_t ref_signal_events, 54 | uint32_t ref_distance_mm, 55 | uint32_t signal_thresh_sigma); 56 | 57 | 58 | #ifdef __cplusplus 59 | } 60 | #endif 61 | 62 | #endif 63 | 64 | -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_wait.h: -------------------------------------------------------------------------------- 1 | 2 | /* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */ 3 | /****************************************************************************** 4 | * Copyright (c) 2020, STMicroelectronics - All Rights Reserved 5 | 6 | This file is part of VL53LX and is dual licensed, 7 | either GPL-2.0+ 8 | or 'BSD 3-clause "New" or "Revised" License' , at your option. 9 | ****************************************************************************** 10 | */ 11 | 12 | 13 | 14 | 15 | #ifndef _VL53LX_WAIT_H_ 16 | #define _VL53LX_WAIT_H_ 17 | 18 | #include "vl53lx_platform.h" 19 | 20 | #ifdef __cplusplus 21 | extern "C" { 22 | #endif 23 | 24 | 25 | 26 | 27 | VL53LX_Error VL53LX_wait_for_boot_completion( 28 | VL53LX_DEV Dev); 29 | 30 | 31 | 32 | 33 | VL53LX_Error VL53LX_wait_for_range_completion( 34 | VL53LX_DEV Dev); 35 | 36 | 37 | 38 | 39 | VL53LX_Error VL53LX_wait_for_test_completion( 40 | VL53LX_DEV Dev); 41 | 42 | 43 | 44 | 45 | 46 | 47 | VL53LX_Error VL53LX_is_boot_complete( 48 | VL53LX_DEV Dev, 49 | uint8_t *pready); 50 | 51 | 52 | 53 | VL53LX_Error VL53LX_is_firmware_ready( 54 | VL53LX_DEV Dev, 55 | uint8_t *pready); 56 | 57 | 58 | 59 | 60 | VL53LX_Error VL53LX_is_new_data_ready( 61 | VL53LX_DEV Dev, 62 | uint8_t *pready); 63 | 64 | 65 | 66 | 67 | 68 | 69 | VL53LX_Error VL53LX_poll_for_boot_completion( 70 | VL53LX_DEV Dev, 71 | uint32_t timeout_ms); 72 | 73 | 74 | 75 | 76 | VL53LX_Error VL53LX_poll_for_firmware_ready( 77 | VL53LX_DEV Dev, 78 | uint32_t timeout_ms); 79 | 80 | 81 | 82 | 83 | VL53LX_Error VL53LX_poll_for_range_completion( 84 | VL53LX_DEV Dev, 85 | uint32_t timeout_ms); 86 | 87 | 88 | 89 | #ifdef __cplusplus 90 | } 91 | #endif 92 | 93 | #endif 94 | 95 | -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_wait copy.h: -------------------------------------------------------------------------------- 1 | 2 | /* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */ 3 | /****************************************************************************** 4 | * Copyright (c) 2020, STMicroelectronics - All Rights Reserved 5 | 6 | This file is part of VL53LX and is dual licensed, 7 | either GPL-2.0+ 8 | or 'BSD 3-clause "New" or "Revised" License' , at your option. 9 | ****************************************************************************** 10 | */ 11 | 12 | 13 | 14 | 15 | #ifndef _VL53LX_WAIT_H_ 16 | #define _VL53LX_WAIT_H_ 17 | 18 | #include "vl53lx_platform.h" 19 | 20 | #ifdef __cplusplus 21 | extern "C" { 22 | #endif 23 | 24 | 25 | 26 | 27 | VL53LX_Error VL53LX_wait_for_boot_completion( 28 | VL53LX_DEV Dev); 29 | 30 | 31 | 32 | 33 | VL53LX_Error VL53LX_wait_for_range_completion( 34 | VL53LX_DEV Dev); 35 | 36 | 37 | 38 | 39 | VL53LX_Error VL53LX_wait_for_test_completion( 40 | VL53LX_DEV Dev); 41 | 42 | 43 | 44 | 45 | 46 | 47 | VL53LX_Error VL53LX_is_boot_complete( 48 | VL53LX_DEV Dev, 49 | uint8_t *pready); 50 | 51 | 52 | 53 | VL53LX_Error VL53LX_is_firmware_ready( 54 | VL53LX_DEV Dev, 55 | uint8_t *pready); 56 | 57 | 58 | 59 | 60 | VL53LX_Error VL53LX_is_new_data_ready( 61 | VL53LX_DEV Dev, 62 | uint8_t *pready); 63 | 64 | 65 | 66 | 67 | 68 | 69 | VL53LX_Error VL53LX_poll_for_boot_completion( 70 | VL53LX_DEV Dev, 71 | uint32_t timeout_ms); 72 | 73 | 74 | 75 | 76 | VL53LX_Error VL53LX_poll_for_firmware_ready( 77 | VL53LX_DEV Dev, 78 | uint32_t timeout_ms); 79 | 80 | 81 | 82 | 83 | VL53LX_Error VL53LX_poll_for_range_completion( 84 | VL53LX_DEV Dev, 85 | uint32_t timeout_ms); 86 | 87 | 88 | 89 | #ifdef __cplusplus 90 | } 91 | #endif 92 | 93 | #endif 94 | 95 | -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_nvm_debug.h: -------------------------------------------------------------------------------- 1 | 2 | /* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */ 3 | /****************************************************************************** 4 | * Copyright (c) 2020, STMicroelectronics - All Rights Reserved 5 | 6 | This file is part of VL53LX and is dual licensed, 7 | either GPL-2.0+ 8 | or 'BSD 3-clause "New" or "Revised" License' , at your option. 9 | ****************************************************************************** 10 | */ 11 | 12 | 13 | 14 | 15 | 16 | #ifndef _VL53LX_NVM_DEBUG_H_ 17 | #define _VL53LX_NVM_DEBUG_H_ 18 | 19 | #include "vl53lx_ll_def.h" 20 | #include "vl53lx_nvm_structs.h" 21 | 22 | 23 | 24 | #ifdef __cplusplus 25 | extern "C" 26 | { 27 | #endif 28 | 29 | #ifdef VL53LX_LOG_ENABLE 30 | 31 | 32 | 33 | void VL53LX_print_nvm_raw_data( 34 | uint8_t *pnvm_raw_data, 35 | uint32_t trace_flags); 36 | 37 | 38 | 39 | 40 | void VL53LX_print_decoded_nvm_data( 41 | VL53LX_decoded_nvm_data_t *pdata, 42 | char *pprefix, 43 | uint32_t trace_flags); 44 | 45 | 46 | 47 | 48 | void VL53LX_print_decoded_nvm_fmt_range_data( 49 | VL53LX_decoded_nvm_fmt_range_data_t *pdata, 50 | char *pprefix, 51 | uint32_t trace_flags); 52 | 53 | 54 | 55 | 56 | void VL53LX_print_decoded_nvm_fmt_info( 57 | VL53LX_decoded_nvm_fmt_info_t *pdata, 58 | char *pprefix, 59 | uint32_t trace_flags); 60 | 61 | 62 | 63 | void VL53LX_print_decoded_nvm_ews_info( 64 | VL53LX_decoded_nvm_ews_info_t *pdata, 65 | char *pprefix, 66 | uint32_t trace_flags); 67 | 68 | #endif 69 | 70 | #ifdef __cplusplus 71 | } 72 | #endif 73 | 74 | #endif 75 | 76 | -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_dmax_private_structs.h: -------------------------------------------------------------------------------- 1 | 2 | /* SPDX-License-Identifier: BSD-3-Clause */ 3 | /****************************************************************************** 4 | * Copyright (c) 2020, STMicroelectronics - All Rights Reserved 5 | 6 | This file is part of VL53LX Protected and is dual licensed, 7 | either 'STMicroelectronics Proprietary license' 8 | or 'BSD 3-clause "New" or "Revised" License' , at your option. 9 | 10 | ****************************************************************************** 11 | 12 | 'STMicroelectronics Proprietary license' 13 | 14 | ****************************************************************************** 15 | 16 | License terms: STMicroelectronics Proprietary in accordance with licensing 17 | terms at www.st.com/sla0081 18 | 19 | ****************************************************************************** 20 | */ 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | #ifndef _VL53LX_DMAX_PRIVATE_STRUCTS_H_ 29 | #define _VL53LX_DMAX_PRIVATE_STRUCTS_H_ 30 | 31 | #include "vl53lx_types.h" 32 | 33 | #ifdef __cplusplus 34 | extern "C" 35 | { 36 | #endif 37 | 38 | 39 | 40 | 41 | typedef struct { 42 | 43 | uint32_t VL53LX_p_037; 44 | 45 | 46 | uint8_t VL53LX_p_063; 47 | 48 | uint8_t VL53LX_p_064; 49 | 50 | 51 | uint16_t VL53LX_p_065; 52 | 53 | uint16_t VL53LX_p_066; 54 | 55 | uint16_t VL53LX_p_067; 56 | 57 | uint16_t VL53LX_p_038; 58 | 59 | 60 | uint32_t VL53LX_p_009; 61 | 62 | uint32_t VL53LX_p_033; 63 | 64 | 65 | uint16_t VL53LX_p_034; 66 | 67 | 68 | uint16_t VL53LX_p_004; 69 | 70 | 71 | uint32_t VL53LX_p_028; 72 | 73 | uint32_t VL53LX_p_035; 74 | 75 | 76 | int16_t VL53LX_p_036; 77 | 78 | int16_t VL53LX_p_022; 79 | 80 | 81 | } VL53LX_hist_gen3_dmax_private_data_t; 82 | 83 | 84 | #ifdef __cplusplus 85 | } 86 | #endif 87 | 88 | #endif 89 | 90 | -------------------------------------------------------------------------------- /src/button.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SPDX-FileCopyrightText: 2024 M5Stack Technology CO LTD 3 | * 4 | * SPDX-License-Identifier: MIT 5 | */ 6 | #include "button.hpp" 7 | #include "OneButton.h" 8 | #include "led.hpp" 9 | #include "buzzer.h" 10 | 11 | #define PIN_INPUT 0 12 | 13 | OneButton button(PIN_INPUT, true); 14 | 15 | uint8_t is_long_press = 0; 16 | 17 | void task_button_update(void *pvParameters); 18 | void LongPressStop(void *oneButton); 19 | void DuringLongPress(void *oneButton); 20 | void Click(void *oneButton); 21 | 22 | bool init_button(void) { 23 | // link functions to be called on events. 24 | button.attachClick(Click, &button); 25 | 26 | button.setLongPressIntervalMs(3000); 27 | 28 | xTaskCreatePinnedToCore(task_button_update, // 任务函数 29 | "TaskButtonUpdate", // 任务名称 30 | 1024 * 4, // 堆栈大小 31 | NULL, // 传递参数 32 | 0, // 任务优先级 33 | NULL, // 任务句柄 34 | tskNO_AFFINITY); // 无关联，不绑定在任何一个核上 35 | 36 | return true; 37 | } 38 | 39 | void task_button_update(void *pvParameters) { 40 | for (;;) { 41 | button.tick(); 42 | vTaskDelay(10 / portTICK_PERIOD_MS); 43 | } 44 | } 45 | 46 | // this function will be called when the button click. 47 | void Click(void *oneButton) { 48 | buzzer_sound(4000, 1000); 49 | esp_restart(); 50 | } 51 | 52 | // this function will be called when the button is released. 53 | void LongPressStop(void *oneButton) { 54 | is_long_press = 0; 55 | esp_restart(); 56 | } 57 | 58 | // this function will be called when the button is held down. 59 | void DuringLongPress(void *oneButton) { 60 | if (!is_long_press) { 61 | is_long_press = 1; 62 | } 63 | } -------------------------------------------------------------------------------- /src/led.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * MIT License 3 | * 4 | * Copyright (c) 2024 Kouhei Ito 5 | * Copyright (c) 2024 M5Stack 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the "Software"), to deal 9 | * in the Software without restriction, including without limitation the rights 10 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | * copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in all 15 | * copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | * SOFTWARE. 24 | */ 25 | 26 | #ifndef LED_HPP 27 | #define LED_HPP 28 | 29 | #include 30 | #include 31 | 32 | #define WHITE 0xffffff 33 | #define BLUE 0x0000ff 34 | #define RED 0xff0000 35 | #define YELLOW 0xffff00 36 | #define GREEN 0x00ff00 37 | #define PERPLE 0xff00ff 38 | #define POWEROFFCOLOR 0x18EBF9 39 | #define FLIPCOLOR 0xFF9933 40 | 41 | #define PIN_LED_ONBORD 39 42 | #define PIN_LED_ESP 21 43 | #define NUM_LEDS 1 44 | 45 | extern uint32_t Led_color; 46 | 47 | void led_init(void); 48 | void led_show(void); 49 | void led_drive(void); 50 | void onboard_led1(CRGB p, uint8_t state); 51 | void onboard_led2(CRGB p, uint8_t state); 52 | void esp_led(CRGB p, uint8_t state); 53 | 54 | #endif -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_platform_ipp_imports.h: -------------------------------------------------------------------------------- 1 | 2 | /* 3 | * This file is part of VL53LX Platform 4 | * 5 | * Copyright (c) 2016, STMicroelectronics - All Rights Reserved 6 | * 7 | * License terms: BSD 3-clause "New" or "Revised" License. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions are met: 11 | * 12 | * 1. Redistributions of source code must retain the above copyright notice, this 13 | * list of conditions and the following disclaimer. 14 | * 15 | * 2. Redistributions in binary form must reproduce the above copyright notice, 16 | * this list of conditions and the following disclaimer in the documentation 17 | * and/or other materials provided with the distribution. 18 | * 19 | * 3. Neither the name of the copyright holder nor the names of its contributors 20 | * may be used to endorse or promote products derived from this software 21 | * without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 24 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 25 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 26 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 27 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 28 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 29 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 31 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 32 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | */ 35 | 36 | #ifdef VL53LX_NEEDS_IPP 37 | # undef VL53LX_IPP_API 38 | # define VL53LX_IPP_API __declspec(dllimport) 39 | # pragma comment(lib, "EwokPlus25API_IPP") 40 | #endif 41 | -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_platform_ipp.h: -------------------------------------------------------------------------------- 1 | 2 | /* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */ 3 | /****************************************************************************** 4 | * Copyright (c) 2020, STMicroelectronics - All Rights Reserved 5 | 6 | This file is part of VL53LX and is dual licensed, 7 | either GPL-2.0+ 8 | or 'BSD 3-clause "New" or "Revised" License' , at your option. 9 | ****************************************************************************** 10 | */ 11 | 12 | 13 | 14 | #ifndef _VL53LX_PLATFORM_IPP_H_ 15 | #define _VL53LX_PLATFORM_IPP_H_ 16 | 17 | #include 18 | 19 | #ifdef __cplusplus 20 | extern "C" 21 | { 22 | #endif 23 | 24 | 25 | 26 | 27 | 28 | VL53LX_Error VL53LX_ipp_hist_process_data( 29 | VL53LX_DEV Dev, 30 | VL53LX_dmax_calibration_data_t *pdmax_cal, 31 | VL53LX_hist_gen3_dmax_config_t *pdmax_cfg, 32 | VL53LX_hist_post_process_config_t *ppost_cfg, 33 | VL53LX_histogram_bin_data_t *pbins, 34 | VL53LX_xtalk_histogram_data_t *pxtalk, 35 | uint8_t *pArea1, 36 | uint8_t *pArea2, 37 | uint8_t *phisto_merge_nb, 38 | VL53LX_range_results_t *presults); 39 | 40 | 41 | 42 | 43 | VL53LX_Error VL53LX_ipp_hist_ambient_dmax( 44 | VL53LX_DEV Dev, 45 | uint16_t target_reflectance, 46 | VL53LX_dmax_calibration_data_t *pdmax_cal, 47 | VL53LX_hist_gen3_dmax_config_t *pdmax_cfg, 48 | VL53LX_histogram_bin_data_t *pbins, 49 | int16_t *pambient_dmax_mm); 50 | 51 | 52 | 53 | 54 | VL53LX_Error VL53LX_ipp_xtalk_calibration_process_data( 55 | VL53LX_DEV Dev, 56 | VL53LX_xtalk_range_results_t *pxtalk_ranges, 57 | VL53LX_xtalk_histogram_data_t *pxtalk_shape, 58 | VL53LX_xtalk_calibration_results_t *pxtalk_cal); 59 | 60 | 61 | #ifdef __cplusplus 62 | } 63 | #endif 64 | 65 | #endif 66 | 67 | 68 | -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_hist_funcs.h: -------------------------------------------------------------------------------- 1 | 2 | /* SPDX-License-Identifier: BSD-3-Clause */ 3 | /****************************************************************************** 4 | * Copyright (c) 2020, STMicroelectronics - All Rights Reserved 5 | 6 | This file is part of VL53LX Protected and is dual licensed, 7 | either 'STMicroelectronics Proprietary license' 8 | or 'BSD 3-clause "New" or "Revised" License' , at your option. 9 | 10 | ****************************************************************************** 11 | 12 | 'STMicroelectronics Proprietary license' 13 | 14 | ****************************************************************************** 15 | 16 | License terms: STMicroelectronics Proprietary in accordance with licensing 17 | terms at www.st.com/sla0081 18 | 19 | ****************************************************************************** 20 | */ 21 | 22 | 23 | 24 | 25 | 26 | #ifndef _VL53LX_HIST_FUNCS_H_ 27 | #define _VL53LX_HIST_FUNCS_H_ 28 | 29 | #include "vl53lx_types.h" 30 | #include "vl53lx_ll_def.h" 31 | 32 | #ifdef __cplusplus 33 | extern "C" 34 | { 35 | #endif 36 | 37 | 38 | 39 | 40 | VL53LX_Error VL53LX_hist_process_data( 41 | VL53LX_dmax_calibration_data_t *pdmax_cal, 42 | VL53LX_hist_gen3_dmax_config_t *pdmax_cfg, 43 | VL53LX_hist_post_process_config_t *ppost_cfg, 44 | VL53LX_histogram_bin_data_t *pbins, 45 | VL53LX_xtalk_histogram_data_t *pxtalk, 46 | uint8_t *pArea1, 47 | uint8_t *pArea2, 48 | VL53LX_range_results_t *presults, 49 | uint8_t *HistMergeNumber); 50 | 51 | 52 | 53 | 54 | VL53LX_Error VL53LX_hist_ambient_dmax( 55 | uint16_t target_reflectance, 56 | VL53LX_dmax_calibration_data_t *pdmax_cal, 57 | VL53LX_hist_gen3_dmax_config_t *pdmax_cfg, 58 | VL53LX_histogram_bin_data_t *pbins, 59 | int16_t *pambient_dmax_mm); 60 | 61 | 62 | #ifdef __cplusplus 63 | } 64 | #endif 65 | 66 | #endif 67 | 68 | -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_sigma_estimate.h: -------------------------------------------------------------------------------- 1 | 2 | /* SPDX-License-Identifier: BSD-3-Clause */ 3 | /****************************************************************************** 4 | * Copyright (c) 2020, STMicroelectronics - All Rights Reserved 5 | 6 | This file is part of VL53LX Protected and is dual licensed, 7 | either 'STMicroelectronics Proprietary license' 8 | or 'BSD 3-clause "New" or "Revised" License' , at your option. 9 | 10 | ****************************************************************************** 11 | 12 | 'STMicroelectronics Proprietary license' 13 | 14 | ****************************************************************************** 15 | 16 | License terms: STMicroelectronics Proprietary in accordance with licensing 17 | terms at www.st.com/sla0081 18 | 19 | ****************************************************************************** 20 | */ 21 | 22 | 23 | 24 | 25 | #ifndef _VL53LX_SIGMA_ESTIMATE_H_ 26 | #define _VL53LX_SIGMA_ESTIMATE_H_ 27 | 28 | #include "vl53lx_types.h" 29 | #include "vl53lx_ll_def.h" 30 | 31 | #ifdef __cplusplus 32 | extern "C" { 33 | #endif 34 | 35 | #define VL53LX_D_002 0xFFFF 36 | 37 | 38 | #define VL53LX_D_008 0xFFFF 39 | #define VL53LX_D_003 0xFFFFFF 40 | #define VL53LX_D_007 0xFFFFFFFF 41 | #define VL53LX_D_005 0x7FFFFFFFFF 42 | #define VL53LX_D_009 0xFFFFFFFFFF 43 | #define VL53LX_D_010 0xFFFFFFFFFFFF 44 | #define VL53LX_D_004 0xFFFFFFFFFFFFFF 45 | #define VL53LX_D_006 0x7FFFFFFFFFFFFFFF 46 | #define VL53LX_D_011 0xFFFFFFFFFFFFFFFF 47 | 48 | 49 | 50 | 51 | 52 | 53 | VL53LX_Error VL53LX_f_023( 54 | uint8_t sigma_estimator__sigma_ref_mm, 55 | uint32_t VL53LX_p_007, 56 | uint32_t VL53LX_p_032, 57 | uint32_t VL53LX_p_001, 58 | uint32_t a_zp, 59 | uint32_t c_zp, 60 | uint32_t bx, 61 | uint32_t ax_zp, 62 | uint32_t cx_zp, 63 | uint32_t VL53LX_p_028, 64 | uint16_t fast_osc_frequency, 65 | uint16_t *psigma_est); 66 | 67 | 68 | 69 | #ifdef __cplusplus 70 | } 71 | #endif 72 | 73 | #endif 74 | 75 | -------------------------------------------------------------------------------- /src/pid.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * MIT License 3 | * 4 | * Copyright (c) 2024 Kouhei Ito 5 | * Copyright (c) 2024 M5Stack 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the "Software"), to deal 9 | * in the Software without restriction, including without limitation the rights 10 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | * copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in all 15 | * copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | * SOFTWARE. 24 | */ 25 | 26 | #ifndef PID_HPP 27 | #define PID_HPP 28 | 29 | class PID { 30 | private: 31 | float m_kp; 32 | float m_ti; 33 | float m_td; 34 | float m_eta; 35 | float m_err, m_err2, m_err3; 36 | float m_h; 37 | 38 | public: 39 | float m_differential; 40 | float m_integral; 41 | PID(); 42 | void set_parameter(float kp, float ti, float td, float eta, float h); 43 | void reset(void); 44 | void i_reset(void); 45 | void printGain(void); 46 | void set_error(float err); 47 | float update(float err, float h); 48 | }; 49 | 50 | class Filter { 51 | private: 52 | float m_state; 53 | float m_T; 54 | float m_h; 55 | 56 | public: 57 | float m_out; 58 | Filter(); 59 | void set_parameter(float T, float h); 60 | void reset(void); 61 | float update(float u, float h); 62 | }; 63 | 64 | #endif -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_hist_map.h: -------------------------------------------------------------------------------- 1 | 2 | /* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */ 3 | /****************************************************************************** 4 | * Copyright (c) 2020, STMicroelectronics - All Rights Reserved 5 | 6 | This file is part of VL53LX and is dual licensed, 7 | either GPL-2.0+ 8 | or 'BSD 3-clause "New" or "Revised" License' , at your option. 9 | ****************************************************************************** 10 | */ 11 | 12 | 13 | 14 | 15 | 16 | #ifndef _VL53LX_HIST_MAP_H_ 17 | #define _VL53LX_HIST_MAP_H_ 18 | 19 | #include "vl53lx_register_map.h" 20 | 21 | #ifdef __cplusplus 22 | extern "C" 23 | { 24 | #endif 25 | 26 | 27 | 28 | 29 | #define VL53LX_HISTOGRAM_CONFIG__OPCODE_SEQUENCE_0 \ 30 | VL53LX_SIGMA_ESTIMATOR__EFFECTIVE_PULSE_WIDTH_NS 31 | 32 | #define VL53LX_HISTOGRAM_CONFIG__OPCODE_SEQUENCE_1 \ 33 | VL53LX_SIGMA_ESTIMATOR__EFFECTIVE_AMBIENT_WIDTH_NS 34 | 35 | #define VL53LX_HISTOGRAM_CONFIG__OPCODE_SEQUENCE_2 \ 36 | VL53LX_SIGMA_ESTIMATOR__SIGMA_REF_MM 37 | 38 | #define VL53LX_HISTOGRAM_CONFIG__AMB_THRESH_HIGH \ 39 | VL53LX_ALGO__RANGE_IGNORE_THRESHOLD_MCPS 40 | 41 | 42 | 43 | 44 | #define VL53LX_RESULT__HISTOGRAM_BIN_0_2 0x008E 45 | #define VL53LX_RESULT__HISTOGRAM_BIN_0_1 0x008F 46 | #define VL53LX_RESULT__HISTOGRAM_BIN_0_0 0x0090 47 | 48 | #define VL53LX_RESULT__HISTOGRAM_BIN_23_2 0x00D3 49 | #define VL53LX_RESULT__HISTOGRAM_BIN_23_1 0x00D4 50 | #define VL53LX_RESULT__HISTOGRAM_BIN_23_0 0x00D5 51 | 52 | #define VL53LX_RESULT__HISTOGRAM_BIN_23_0_MSB 0x00D9 53 | #define VL53LX_RESULT__HISTOGRAM_BIN_23_0_LSB 0x00DA 54 | 55 | 56 | 57 | #define VL53LX_HISTOGRAM_BIN_DATA_I2C_INDEX \ 58 | VL53LX_RESULT__INTERRUPT_STATUS 59 | #define VL53LX_HISTOGRAM_BIN_DATA_I2C_SIZE_BYTES \ 60 | (VL53LX_RESULT__HISTOGRAM_BIN_23_0_LSB - \ 61 | VL53LX_RESULT__INTERRUPT_STATUS + 1) 62 | 63 | #ifdef __cplusplus 64 | } 65 | #endif 66 | 67 | #endif 68 | 69 | -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_platform_init.c: -------------------------------------------------------------------------------- 1 | 2 | // SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause 3 | /****************************************************************************** 4 | * Copyright (c) 2020, STMicroelectronics - All Rights Reserved 5 | 6 | This file is part of VL53LX and is dual licensed, 7 | either GPL-2.0+ 8 | or 'BSD 3-clause "New" or "Revised" License' , at your option. 9 | ****************************************************************************** 10 | */ 11 | 12 | 13 | 14 | 15 | 16 | #include "vl53lx_platform.h" 17 | #include 18 | #include "vl53lx_ll_def.h" 19 | 20 | 21 | VL53LX_Error VL53LX_platform_init( 22 | VL53LX_Dev_t *pdev, 23 | uint8_t i2c_slave_address, 24 | uint8_t comms_type, 25 | uint16_t comms_speed_khz) 26 | { 27 | 28 | 29 | VL53LX_Error status = VL53LX_ERROR_NONE; 30 | 31 | 32 | 33 | pdev->i2c_slave_address = i2c_slave_address; 34 | pdev->comms_type = comms_type; 35 | pdev->comms_speed_khz = comms_speed_khz; 36 | 37 | if (status == VL53LX_ERROR_NONE) 38 | status = 39 | VL53LX_CommsInitialise( 40 | pdev, 41 | pdev->comms_type, 42 | pdev->comms_speed_khz); 43 | 44 | 45 | if (status == VL53LX_ERROR_NONE) 46 | status = VL53LX_GpioXshutdown(0); 47 | 48 | 49 | if (status == VL53LX_ERROR_NONE) 50 | status = VL53LX_GpioPowerEnable(0); 51 | 52 | 53 | if (status == VL53LX_ERROR_NONE) 54 | status = VL53LX_GpioCommsSelect(0); 55 | 56 | 57 | if (status == VL53LX_ERROR_NONE) 58 | status = VL53LX_WaitUs(pdev, 1000); 59 | 60 | 61 | if (status == VL53LX_ERROR_NONE) 62 | status = VL53LX_GpioPowerEnable(1); 63 | 64 | 65 | if (status == VL53LX_ERROR_NONE) 66 | status = VL53LX_WaitUs(pdev, 1000); 67 | 68 | 69 | if (status == VL53LX_ERROR_NONE) 70 | status = VL53LX_GpioXshutdown(1); 71 | 72 | 73 | if (status == VL53LX_ERROR_NONE) 74 | status = VL53LX_WaitUs(pdev, 100); 75 | 76 | return status; 77 | } 78 | 79 | 80 | VL53LX_Error VL53LX_platform_terminate( 81 | VL53LX_Dev_t *pdev) 82 | { 83 | 84 | 85 | VL53LX_Error status = VL53LX_ERROR_NONE; 86 | 87 | 88 | if (status == VL53LX_ERROR_NONE) 89 | status = VL53LX_GpioXshutdown(0); 90 | 91 | 92 | if (status == VL53LX_ERROR_NONE) 93 | status = VL53LX_GpioPowerEnable(0); 94 | 95 | 96 | if (status == VL53LX_ERROR_NONE) 97 | status = VL53LX_CommsClose(pdev); 98 | 99 | return status; 100 | } 101 | 102 | 103 | 104 | -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_silicon_core.c: -------------------------------------------------------------------------------- 1 | 2 | // SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause 3 | /****************************************************************************** 4 | * Copyright (c) 2020, STMicroelectronics - All Rights Reserved 5 | 6 | This file is part of VL53LX and is dual licensed, 7 | either GPL-2.0+ 8 | or 'BSD 3-clause "New" or "Revised" License' , at your option. 9 | ****************************************************************************** 10 | */ 11 | 12 | 13 | 14 | 15 | 16 | #include "vl53lx_platform.h" 17 | #include "vl53lx_ll_def.h" 18 | #include "vl53lx_register_map.h" 19 | #include "vl53lx_core.h" 20 | #include "vl53lx_silicon_core.h" 21 | 22 | 23 | #define LOG_FUNCTION_START(fmt, ...) \ 24 | _LOG_FUNCTION_START(VL53LX_TRACE_MODULE_CORE, fmt, ##__VA_ARGS__) 25 | #define LOG_FUNCTION_END(status, ...) \ 26 | _LOG_FUNCTION_END(VL53LX_TRACE_MODULE_CORE, status, ##__VA_ARGS__) 27 | #define LOG_FUNCTION_END_FMT(status, fmt, ...) \ 28 | _LOG_FUNCTION_END_FMT(VL53LX_TRACE_MODULE_CORE,\ 29 | status, fmt, ##__VA_ARGS__) 30 | 31 | 32 | VL53LX_Error VL53LX_is_firmware_ready_silicon( 33 | VL53LX_DEV Dev, 34 | uint8_t *pready) 35 | { 36 | 37 | 38 | VL53LX_Error status = VL53LX_ERROR_NONE; 39 | VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev); 40 | 41 | uint8_t comms_buffer[5]; 42 | 43 | LOG_FUNCTION_START(""); 44 | 45 | 46 | 47 | status = VL53LX_ReadMulti( 48 | Dev, 49 | VL53LX_INTERRUPT_MANAGER__ENABLES, 50 | comms_buffer, 51 | 5); 52 | 53 | if (status != VL53LX_ERROR_NONE) 54 | goto ENDFUNC; 55 | 56 | pdev->dbg_results.interrupt_manager__enables = 57 | comms_buffer[0]; 58 | pdev->dbg_results.interrupt_manager__clear = 59 | comms_buffer[1]; 60 | pdev->dbg_results.interrupt_manager__status = 61 | comms_buffer[2]; 62 | pdev->dbg_results.mcu_to_host_bank__wr_access_en = 63 | comms_buffer[3]; 64 | pdev->dbg_results.power_management__go1_reset_status = 65 | comms_buffer[4]; 66 | 67 | if ((pdev->sys_ctrl.power_management__go1_power_force & 0x01) 68 | == 0x01) { 69 | 70 | if (((pdev->dbg_results.interrupt_manager__enables & 71 | 0x1F) == 0x1F) && 72 | ((pdev->dbg_results.interrupt_manager__clear 73 | & 0x1F) == 0x1F)) 74 | *pready = 0x01; 75 | else 76 | *pready = 0x00; 77 | 78 | } else { 79 | 80 | 81 | if ((pdev->dbg_results.power_management__go1_reset_status 82 | & 0x01) == 0x00) 83 | *pready = 0x01; 84 | else 85 | *pready = 0x00; 86 | } 87 | 88 | 89 | ENDFUNC: 90 | LOG_FUNCTION_END(status); 91 | 92 | return status; 93 | } 94 | 95 | -------------------------------------------------------------------------------- /src/rc.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * MIT License 3 | * 4 | * Copyright (c) 2024 Kouhei Ito 5 | * Copyright (c) 2024 M5Stack 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the "Software"), to deal 9 | * in the Software without restriction, including without limitation the rights 10 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | * copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in all 15 | * copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | * SOFTWARE. 24 | */ 25 | 26 | #ifndef RC_HPP 27 | #define RC_HPP 28 | 29 | #include 30 | #include 31 | 32 | // #define MINIJOYC 33 | 34 | #define CHANNEL 13 35 | 36 | #define RUDDER 0 37 | #define ELEVATOR 1 38 | #define THROTTLE 2 39 | #define AILERON 3 40 | #define LOG 4 41 | #define DPAD_UP 5 42 | #define DPAD_DOWN 6 43 | #define DPAD_LEFT 7 44 | #define DPAD_RIGHT 8 45 | #define BUTTON_ARM 9 46 | #define BUTTON_FLIP 10 47 | #define CONTROLMODE 11 48 | #define ALTCONTROLMODE 12 49 | 50 | #define RUDDER_MAX 511 51 | #define RUDDER_MIN -512 52 | #define ELEVATOR_MAX 127 53 | #define ELEVATOR_MIN -128 54 | #define THROTTLE_MAX 511 55 | #define THROTTLE_MIN -512 56 | #define AILERON_MAX 127 57 | #define AILERON_MIN -128 58 | 59 | #define LOG_MAX 1 60 | #define LOG_MIN 0 61 | #define CH6MAX 127 62 | #define CH6MIN -128 63 | 64 | #define RUDDER_MAX_JOYC 100 65 | #define ELEVATOR_MAX 127 66 | #define THROTTLE_MAX_JOYC 100 67 | 68 | void rc_init(void); 69 | void rc_demo(void); 70 | void rc_end(void); 71 | uint8_t rc_isconnected(void); 72 | uint8_t telemetry_send(uint8_t* data, uint16_t datalen); 73 | void send_peer_info(void); 74 | 75 | extern volatile float Stick[16]; 76 | extern volatile uint8_t Rc_err_flag; 77 | extern volatile uint8_t MyMacAddr[6]; 78 | extern volatile uint8_t Recv_MAC[3]; 79 | extern volatile uint16_t Connect_flag; 80 | #endif -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_hist_algos_gen4.h: -------------------------------------------------------------------------------- 1 | 2 | /* SPDX-License-Identifier: BSD-3-Clause */ 3 | /****************************************************************************** 4 | * Copyright (c) 2020, STMicroelectronics - All Rights Reserved 5 | 6 | This file is part of VL53LX Protected and is dual licensed, 7 | either 'STMicroelectronics Proprietary license' 8 | or 'BSD 3-clause "New" or "Revised" License' , at your option. 9 | 10 | ****************************************************************************** 11 | 12 | 'STMicroelectronics Proprietary license' 13 | 14 | ****************************************************************************** 15 | 16 | License terms: STMicroelectronics Proprietary in accordance with licensing 17 | terms at www.st.com/sla0081 18 | 19 | ****************************************************************************** 20 | */ 21 | 22 | 23 | 24 | 25 | 26 | #ifndef _VL53LX_HIST_ALGOS_GEN4_H_ 27 | #define _VL53LX_HIST_ALGOS_GEN4_H_ 28 | 29 | #include "vl53lx_types.h" 30 | #include "vl53lx_ll_def.h" 31 | 32 | #include "vl53lx_hist_private_structs.h" 33 | #include "vl53lx_dmax_private_structs.h" 34 | 35 | 36 | #ifdef __cplusplus 37 | extern "C" 38 | { 39 | #endif 40 | 41 | 42 | 43 | 44 | VL53LX_Error VL53LX_f_025( 45 | VL53LX_dmax_calibration_data_t *pdmax_cal, 46 | VL53LX_hist_gen3_dmax_config_t *pdmax_cfg, 47 | VL53LX_hist_post_process_config_t *ppost_cfg, 48 | VL53LX_histogram_bin_data_t *pbins, 49 | VL53LX_histogram_bin_data_t *pxtalk, 50 | VL53LX_hist_gen3_algo_private_data_t *palgo, 51 | VL53LX_hist_gen4_algo_filtered_data_t *pfiltered, 52 | VL53LX_hist_gen3_dmax_private_data_t *pdmax_algo, 53 | VL53LX_range_results_t *presults, 54 | uint8_t histo_merge_nb); 55 | 56 | 57 | 58 | 59 | VL53LX_Error VL53LX_f_026( 60 | uint8_t pulse_no, 61 | VL53LX_histogram_bin_data_t *ppulse, 62 | VL53LX_hist_gen3_algo_private_data_t *palgo, 63 | VL53LX_hist_gen4_algo_filtered_data_t *pfiltered); 64 | 65 | 66 | 67 | 68 | VL53LX_Error VL53LX_f_027( 69 | uint8_t pulse_no, 70 | uint16_t noise_threshold, 71 | VL53LX_hist_gen4_algo_filtered_data_t *pfiltered, 72 | VL53LX_hist_gen3_algo_private_data_t *palgo); 73 | 74 | 75 | 76 | 77 | VL53LX_Error VL53LX_f_028( 78 | uint8_t bin, 79 | int32_t VL53LX_p_007, 80 | int32_t VL53LX_p_032, 81 | int32_t VL53LX_p_001, 82 | int32_t ax, 83 | int32_t bx, 84 | int32_t cx, 85 | int32_t VL53LX_p_028, 86 | uint8_t VL53LX_p_030, 87 | uint32_t *pmedian_phase); 88 | 89 | 90 | #ifdef __cplusplus 91 | } 92 | #endif 93 | 94 | #endif 95 | 96 | -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_hist_core.h: -------------------------------------------------------------------------------- 1 | 2 | /* SPDX-License-Identifier: BSD-3-Clause */ 3 | /****************************************************************************** 4 | * Copyright (c) 2020, STMicroelectronics - All Rights Reserved 5 | 6 | This file is part of VL53LX Protected and is dual licensed, 7 | either 'STMicroelectronics Proprietary license' 8 | or 'BSD 3-clause "New" or "Revised" License' , at your option. 9 | 10 | ****************************************************************************** 11 | 12 | 'STMicroelectronics Proprietary license' 13 | 14 | ****************************************************************************** 15 | 16 | License terms: STMicroelectronics Proprietary in accordance with licensing 17 | terms at www.st.com/sla0081 18 | 19 | ****************************************************************************** 20 | */ 21 | 22 | 23 | 24 | 25 | 26 | #ifndef _VL53LX_HIST_CORE_H_ 27 | #define _VL53LX_HIST_CORE_H_ 28 | 29 | #include "vl53lx_types.h" 30 | #include "vl53lx_ll_def.h" 31 | #include "vl53lx_hist_private_structs.h" 32 | 33 | #ifdef __cplusplus 34 | extern "C" 35 | { 36 | #endif 37 | 38 | 39 | 40 | 41 | void VL53LX_f_022( 42 | uint8_t VL53LX_p_032, 43 | uint8_t filter_woi, 44 | VL53LX_histogram_bin_data_t *pbins, 45 | int32_t *pa, 46 | int32_t *pb, 47 | int32_t *pc); 48 | 49 | 50 | 51 | 52 | VL53LX_Error VL53LX_f_018( 53 | uint16_t vcsel_width, 54 | uint16_t fast_osc_frequency, 55 | uint32_t total_periods_elapsed, 56 | uint16_t VL53LX_p_004, 57 | VL53LX_range_data_t *pdata, 58 | uint8_t histo_merge_nb); 59 | 60 | 61 | 62 | 63 | void VL53LX_f_019( 64 | uint16_t gain_factor, 65 | int16_t range_offset_mm, 66 | VL53LX_range_data_t *pdata); 67 | 68 | 69 | 70 | 71 | void VL53LX_f_029( 72 | VL53LX_histogram_bin_data_t *pdata, 73 | int32_t ambient_estimate_counts_per_bin); 74 | 75 | 76 | 77 | 78 | void VL53LX_f_005( 79 | VL53LX_histogram_bin_data_t *pxtalk, 80 | VL53LX_histogram_bin_data_t *pbins, 81 | VL53LX_histogram_bin_data_t *pxtalk_realigned); 82 | 83 | 84 | 85 | int8_t VL53LX_f_030( 86 | VL53LX_histogram_bin_data_t *pdata1, 87 | VL53LX_histogram_bin_data_t *pdata2); 88 | 89 | 90 | 91 | VL53LX_Error VL53LX_f_031( 92 | VL53LX_histogram_bin_data_t *pidata, 93 | VL53LX_histogram_bin_data_t *podata); 94 | 95 | #ifdef __cplusplus 96 | } 97 | #endif 98 | 99 | #endif 100 | 101 | -------------------------------------------------------------------------------- /src/alt_kalman.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * MIT License 3 | * 4 | * Copyright (c) 2024 Kouhei Ito 5 | * Copyright (c) 2024 M5Stack 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the "Software"), to deal 9 | * in the Software without restriction, including without limitation the rights 10 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | * copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in all 15 | * copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | * SOFTWARE. 24 | */ 25 | 26 | #ifndef ALT_KALMAN_HPP 27 | #define ALT_KALMAN_HPP 28 | 29 | class Mat {}; 30 | 31 | class Alt_kalman { 32 | // accel 33 | float accel = 0.0; 34 | 35 | // state 36 | float velocity = 0.0, altitude = 0.0, bias = 0.0; 37 | float velocity_, altitude_, bias_; 38 | float true_v; 39 | 40 | // Sensor 41 | float z_sens; 42 | 43 | // Bias beta 44 | float beta = -0.01; 45 | 46 | // Kalman Gain 47 | float k1, k2, k3; 48 | 49 | // F 50 | float f11 = 1.0, f12 = 0.0, f13 = -step; 51 | float f21 = step, f22 = 1.0, f23 = 0.0; 52 | float f31 = 0.0, f32 = 0.0, f33 = 1 + beta * step; 53 | 54 | // B 55 | float b11 = step, b12 = 0.0; 56 | float b21 = 0.0, b22 = 0.0; 57 | float b31 = 0.0, b32 = step; 58 | 59 | // H 60 | float h1 = 0.0, h2 = 1.0, h3 = 0.0; 61 | ; 62 | 63 | // P 64 | float p11 = 100.0, p12 = 0.0, p13 = 0.0; 65 | float p21 = 0.0, p22 = 100.0, p23 = 0.0; 66 | float p31 = 0.0, p32 = 0.0, p33 = 100.0; 67 | 68 | float p11_, p12_, p13_; 69 | float p21_, p22_, p23_; 70 | float p31_, p32_, p33_; 71 | 72 | // Q 73 | float q1 = 0.1 * 0.1, q2 = (1.0) * (1.0); // q1=1.0*1.0 q2=1.0*1.0 74 | 75 | // R 76 | // float R = 0.004*0.004; 77 | float R = 0.004 * 0.004; 78 | 79 | public: 80 | // step 81 | float step = 1.0 / 400.0; 82 | // state 83 | float Velocity = 0.0, Altitude = 0.0, Bias = 0.0; 84 | 85 | // Method 86 | Alt_kalman(); 87 | void update(float z_sens, float accel, float h); 88 | void set_vel(float v); 89 | void reset(void); 90 | }; 91 | 92 | #endif -------------------------------------------------------------------------------- /lib/MdgwickAHRS/MadgwickAHRS.h: -------------------------------------------------------------------------------- 1 | //============================================================================================= 2 | // MadgwickAHRS.h 3 | //============================================================================================= 4 | // 5 | // Implementation of Madgwick's IMU and AHRS algorithms. 6 | // See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/ 7 | // 8 | // From the x-io website "Open-source resources available on this website are 9 | // provided under the GNU General Public Licence unless an alternative licence 10 | // is provided in source." 11 | // 12 | // Date Author Notes 13 | // 29/09/2011 SOH Madgwick Initial release 14 | // 02/10/2011 SOH Madgwick Optimised for reduced CPU load 15 | // 16 | //============================================================================================= 17 | #ifndef MadgwickAHRS_h 18 | #define MadgwickAHRS_h 19 | #include 20 | 21 | //-------------------------------------------------------------------------------------------- 22 | // Variable declaration 23 | class Madgwick{ 24 | private: 25 | static float invSqrt(float x); 26 | float beta; // algorithm gain 27 | float q0; 28 | float q1; 29 | float q2; 30 | float q3; // quaternion of sensor frame relative to auxiliary frame 31 | float invSampleFreq; 32 | float roll; 33 | float pitch; 34 | float yaw; 35 | float betaDef = 0.1f; 36 | char anglesComputed; 37 | void computeAngles(); 38 | 39 | //------------------------------------------------------------------------------------------- 40 | // Function declarations 41 | public: 42 | Madgwick(void); 43 | void reset(void); 44 | void set_beta(float beta) { betaDef = beta;} 45 | void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; } 46 | void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); 47 | void updateIMU(float gx, float gy, float gz, float ax, float ay, float az); 48 | //float getPitch(){return atan2f(2.0f * q2 * q3 - 2.0f * q0 * q1, 2.0f * q0 * q0 + 2.0f * q3 * q3 - 1.0f);}; 49 | //float getRoll(){return -1.0f * asinf(2.0f * q1 * q3 + 2.0f * q0 * q2);}; 50 | //float getYaw(){return atan2f(2.0f * q1 * q2 - 2.0f * q0 * q3, 2.0f * q0 * q0 + 2.0f * q1 * q1 - 1.0f);}; 51 | float getRoll() { 52 | if (!anglesComputed) computeAngles(); 53 | return roll * 57.29578f; 54 | } 55 | float getPitch() { 56 | if (!anglesComputed) computeAngles(); 57 | return pitch * 57.29578f; 58 | } 59 | float getYaw() { 60 | if (!anglesComputed) computeAngles(); 61 | return yaw * 57.29578f + 180.0f; 62 | } 63 | float getRollRadians() { 64 | if (!anglesComputed) computeAngles(); 65 | return roll; 66 | } 67 | float getPitchRadians() { 68 | if (!anglesComputed) computeAngles(); 69 | return pitch; 70 | } 71 | float getYawRadians() { 72 | if (!anglesComputed) computeAngles(); 73 | return yaw; 74 | } 75 | }; 76 | #endif 77 | 78 | -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_platform_user_defines.h: -------------------------------------------------------------------------------- 1 | 2 | /* 3 | * This file is part of VL53LX Platform 4 | * 5 | * Copyright (c) 2016, STMicroelectronics - All Rights Reserved 6 | * 7 | * License terms: BSD 3-clause "New" or "Revised" License. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions are met: 11 | * 12 | * 1. Redistributions of source code must retain the above copyright notice, this 13 | * list of conditions and the following disclaimer. 14 | * 15 | * 2. Redistributions in binary form must reproduce the above copyright notice, 16 | * this list of conditions and the following disclaimer in the documentation 17 | * and/or other materials provided with the distribution. 18 | * 19 | * 3. Neither the name of the copyright holder nor the names of its contributors 20 | * may be used to endorse or promote products derived from this software 21 | * without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 24 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 25 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 26 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 27 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 28 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 29 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 31 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 32 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | */ 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | #ifndef _VL53LX_PLATFORM_USER_DEFINES_H_ 66 | #define _VL53LX_PLATFORM_USER_DEFINES_H_ 67 | 68 | #ifdef __cplusplus 69 | extern "C" 70 | { 71 | #endif 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | #define do_division_u(dividend, divisor) (dividend / divisor) 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | #define do_division_s(dividend, divisor) (dividend / divisor) 100 | 101 | 102 | #define WARN_OVERRIDE_STATUS(__X__)\ 103 | trace_print(VL53LX_TRACE_LEVEL_WARNING, #__X__); 104 | 105 | #ifdef _MSC_VER 106 | #define DISABLE_WARNINGS() { \ 107 | __pragma(warning(push)); \ 108 | __pragma(warning(disable:4127)); \ 109 | } 110 | #define ENABLE_WARNINGS() { \ 111 | __pragma(warning( pop )); \ 112 | } 113 | #else 114 | 115 | 116 | #define DISABLE_WARNINGS() 117 | #define ENABLE_WARNINGS() 118 | #endif 119 | 120 | 121 | #ifdef __cplusplus 122 | } 123 | #endif 124 | 125 | #endif 126 | 127 | 128 | -------------------------------------------------------------------------------- /lib/vl53l3c/mxconstants.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * File Name : mxconstants.h 4 | * Description : This file contains the common defines of the application 5 | ****************************************************************************** 6 | * 7 | * COPYRIGHT(c) 2015 STMicroelectronics 8 | * 9 | * Redistribution and use in source and binary forms, with or without modification, 10 | * are permitted provided that the following conditions are met: 11 | * 1. Redistributions of source code must retain the above copyright notice, 12 | * this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 3. Neither the name of STMicroelectronics nor the names of its contributors 17 | * may be used to endorse or promote products derived from this software 18 | * without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | * 31 | ****************************************************************************** 32 | */ 33 | /* Includes ------------------------------------------------------------------*/ 34 | 35 | /* USER CODE BEGIN Includes */ 36 | 37 | /* USER CODE END Includes */ 38 | 39 | /* Private define ------------------------------------------------------------*/ 40 | 41 | #define B1_Pin GPIO_PIN_13 42 | #define B1_GPIO_Port GPIOC 43 | #define USART_TX_Pin GPIO_PIN_2 44 | #define USART_TX_GPIO_Port GPIOA 45 | #define USART_RX_Pin GPIO_PIN_3 46 | #define USART_RX_GPIO_Port GPIOA 47 | #define LD2_Pin GPIO_PIN_5 48 | #define LD2_GPIO_Port GPIOA 49 | #define TMS_Pin GPIO_PIN_13 50 | #define TMS_GPIO_Port GPIOA 51 | #define TCK_Pin GPIO_PIN_14 52 | #define TCK_GPIO_Port GPIOA 53 | #define SWO_Pin GPIO_PIN_3 54 | #define SWO_GPIO_Port GPIOB 55 | #define VL53L0_OsDelay() HAL_Delay(1) 56 | /* USER CODE BEGIN Private defines */ 57 | 58 | /* USER CODE END Private defines */ 59 | 60 | /** 61 | * @} 62 | */ 63 | 64 | /** 65 | * @} 66 | */ 67 | 68 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 69 | -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_preset_setup.h: -------------------------------------------------------------------------------- 1 | 2 | /* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */ 3 | /****************************************************************************** 4 | * Copyright (c) 2020, STMicroelectronics - All Rights Reserved 5 | 6 | This file is part of VL53LX and is dual licensed, 7 | either GPL-2.0+ 8 | or 'BSD 3-clause "New" or "Revised" License' , at your option. 9 | ****************************************************************************** 10 | */ 11 | 12 | #ifndef _VL53LX_PRESET_SETUP_H_ 13 | #define _VL53LX_PRESET_SETUP_H_ 14 | 15 | #ifdef __cplusplus 16 | extern "C" 17 | { 18 | #endif 19 | 20 | /* indexes for the bare driver tuning setting API function */ 21 | enum VL53LX_Tuning_t { 22 | VL53LX_TUNING_VERSION = 0, 23 | VL53LX_TUNING_PROXY_MIN, 24 | VL53LX_TUNING_SINGLE_TARGET_XTALK_TARGET_DISTANCE_MM, 25 | VL53LX_TUNING_SINGLE_TARGET_XTALK_SAMPLE_NUMBER, 26 | VL53LX_TUNING_MIN_AMBIENT_DMAX_VALID, 27 | VL53LX_TUNING_MAX_SIMPLE_OFFSET_CALIBRATION_SAMPLE_NUMBER, 28 | VL53LX_TUNING_XTALK_FULL_ROI_TARGET_DISTANCE_MM, 29 | VL53LX_TUNING_SIMPLE_OFFSET_CALIBRATION_REPEAT, 30 | VL53LX_TUNING_XTALK_FULL_ROI_BIN_SUM_MARGIN, 31 | VL53LX_TUNING_XTALK_FULL_ROI_DEFAULT_OFFSET, 32 | VL53LX_TUNING_ZERO_DISTANCE_OFFSET_NON_LINEAR_FACTOR, 33 | VL53LX_TUNING_MAX_TUNABLE_KEY 34 | }; 35 | 36 | /* default values for the tuning settings parameters */ 37 | #define TUNING_VERSION 0x0007 38 | 39 | #define TUNING_PROXY_MIN -30 /* min distance in mm */ 40 | #define TUNING_SINGLE_TARGET_XTALK_TARGET_DISTANCE_MM 600 41 | /* Target distance in mm for single target Xtalk */ 42 | #define TUNING_SINGLE_TARGET_XTALK_SAMPLE_NUMBER 50 43 | /* Number of sample used for single target Xtalk */ 44 | #define TUNING_MIN_AMBIENT_DMAX_VALID 8 45 | /* Minimum ambient level to state the Dmax returned by the device is valid */ 46 | #ifdef SMALL_FOOTPRINT 47 | #define TUNING_MAX_SIMPLE_OFFSET_CALIBRATION_SAMPLE_NUMBER 50 48 | #else 49 | #define TUNING_MAX_SIMPLE_OFFSET_CALIBRATION_SAMPLE_NUMBER 10 50 | #endif 51 | /* Maximum loops to perform simple offset calibration */ 52 | #define TUNING_XTALK_FULL_ROI_TARGET_DISTANCE_MM 600 53 | /* Target distance in mm for target Xtalk from Bins method*/ 54 | #ifdef SMALL_FOOTPRINT 55 | #define TUNING_SIMPLE_OFFSET_CALIBRATION_REPEAT 1 56 | #else 57 | #define TUNING_SIMPLE_OFFSET_CALIBRATION_REPEAT 3 58 | #endif 59 | /* Number of loops done during the simple offset calibration*/ 60 | #define TUNING_ZERO_DISTANCE_OFFSET_NON_LINEAR_FACTOR_DEFAULT 9 61 | /* zero distance offset calibration non linear compensation default value */ 62 | 63 | /* The following settings are related to the fix for ticket EwokP #558410 */ 64 | #define TUNING_XTALK_FULL_ROI_BIN_SUM_MARGIN 24 65 | /* Acceptance margin for the xtalk_shape bin_data sum computation */ 66 | #define TUNING_XTALK_FULL_ROI_DEFAULT_OFFSET 50 67 | /* Recovery value for Xtalk compensation plane offset in kcps */ 68 | /* 50 stands for ~0.10 kcps cover glass in 7.9 format */ 69 | /* End of settings related to the fix for ticket EwokP #558410 */ 70 | 71 | #ifdef __cplusplus 72 | } 73 | #endif 74 | 75 | #endif /* _VL53LX_PRESET_SETUP_H_ */ 76 | -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_api_calibration.h: -------------------------------------------------------------------------------- 1 | 2 | /* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */ 3 | /****************************************************************************** 4 | * Copyright (c) 2020, STMicroelectronics - All Rights Reserved 5 | 6 | This file is part of VL53LX and is dual licensed, 7 | either GPL-2.0+ 8 | or 'BSD 3-clause "New" or "Revised" License' , at your option. 9 | ****************************************************************************** 10 | */ 11 | 12 | 13 | 14 | 15 | #ifndef _VL53LX_API_CALIBRATION_H_ 16 | #define _VL53LX_API_CALIBRATION_H_ 17 | 18 | #include "vl53lx_platform.h" 19 | 20 | #ifdef __cplusplus 21 | extern "C" { 22 | #endif 23 | 24 | 25 | 26 | 27 | VL53LX_Error VL53LX_run_ref_spad_char(VL53LX_DEV Dev, 28 | VL53LX_Error * pcal_status); 29 | 30 | 31 | 32 | 33 | VL53LX_Error VL53LX_run_device_test( 34 | VL53LX_DEV Dev, 35 | VL53LX_DeviceTestMode device_test_mode); 36 | 37 | 38 | 39 | 40 | VL53LX_Error VL53LX_get_and_avg_xtalk_samples( 41 | VL53LX_DEV Dev, 42 | uint8_t num_of_samples, 43 | uint8_t measurement_mode, 44 | int16_t xtalk_filter_thresh_max_mm, 45 | int16_t xtalk_filter_thresh_min_mm, 46 | uint16_t xtalk_max_valid_rate_kcps, 47 | uint8_t xtalk_result_id, 48 | uint8_t xtalk_histo_id, 49 | VL53LX_xtalk_range_results_t *pxtalk_results, 50 | VL53LX_histogram_bin_data_t *psum_histo, 51 | VL53LX_histogram_bin_data_t *pavg_histo); 52 | 53 | 54 | 55 | 56 | 57 | VL53LX_Error VL53LX_run_phasecal_average( 58 | VL53LX_DEV Dev, 59 | uint8_t measurement_mode, 60 | uint8_t phasecal_result__vcsel_start, 61 | uint16_t phasecal_num_of_samples, 62 | VL53LX_range_results_t *prange_results, 63 | uint16_t *pphasecal_result__reference_phase, 64 | uint16_t *pzero_distance_phase); 65 | 66 | 67 | 68 | 69 | void VL53LX_hist_xtalk_extract_data_init( 70 | VL53LX_hist_xtalk_extract_data_t *pxtalk_data); 71 | 72 | 73 | 74 | VL53LX_Error VL53LX_hist_xtalk_extract_update( 75 | int16_t target_distance_mm, 76 | uint16_t target_width_oversize, 77 | VL53LX_histogram_bin_data_t *phist_bins, 78 | VL53LX_hist_xtalk_extract_data_t *pxtalk_data); 79 | 80 | 81 | 82 | VL53LX_Error VL53LX_hist_xtalk_extract_fini( 83 | VL53LX_histogram_bin_data_t *phist_bins, 84 | VL53LX_hist_xtalk_extract_data_t *pxtalk_data, 85 | VL53LX_xtalk_calibration_results_t *pxtalk_cal, 86 | VL53LX_xtalk_histogram_shape_t *pxtalk_shape); 87 | 88 | 89 | 90 | 91 | VL53LX_Error VL53LX_run_hist_xtalk_extraction( 92 | VL53LX_DEV Dev, 93 | int16_t cal_distance_mm, 94 | VL53LX_Error *pcal_status); 95 | 96 | 97 | #ifdef __cplusplus 98 | } 99 | #endif 100 | 101 | #endif 102 | 103 | -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_platform_ipp.c: -------------------------------------------------------------------------------- 1 | 2 | // SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause 3 | /****************************************************************************** 4 | * Copyright (c) 2020, STMicroelectronics - All Rights Reserved 5 | 6 | This file is part of VL53LX and is dual licensed, 7 | either GPL-2.0+ 8 | or 'BSD 3-clause "New" or "Revised" License' , at your option. 9 | ****************************************************************************** 10 | */ 11 | 12 | 13 | 14 | 15 | #include "vl53lx_platform_ipp.h" 16 | #include "vl53lx_ll_def.h" 17 | #include "vl53lx_hist_structs.h" 18 | #include "vl53lx_hist_funcs.h" 19 | #include "vl53lx_xtalk.h" 20 | 21 | 22 | #define LOG_FUNCTION_START(fmt, ...) \ 23 | _LOG_FUNCTION_START(VL53LX_TRACE_MODULE_CORE, fmt, ##__VA_ARGS__) 24 | #define LOG_FUNCTION_END(status, ...) \ 25 | _LOG_FUNCTION_END(VL53LX_TRACE_MODULE_CORE, status, ##__VA_ARGS__) 26 | 27 | 28 | VL53LX_Error VL53LX_ipp_hist_process_data( 29 | VL53LX_DEV Dev, 30 | VL53LX_dmax_calibration_data_t *pdmax_cal, 31 | VL53LX_hist_gen3_dmax_config_t *pdmax_cfg, 32 | VL53LX_hist_post_process_config_t *ppost_cfg, 33 | VL53LX_histogram_bin_data_t *pbins, 34 | VL53LX_xtalk_histogram_data_t *pxtalk, 35 | uint8_t *pArea1, 36 | uint8_t *pArea2, 37 | uint8_t *phisto_merge_nb, 38 | VL53LX_range_results_t *presults) 39 | { 40 | 41 | 42 | 43 | VL53LX_Error status = VL53LX_ERROR_NONE; 44 | 45 | SUPPRESS_UNUSED_WARNING(Dev); 46 | 47 | status = 48 | VL53LX_hist_process_data( 49 | pdmax_cal, 50 | pdmax_cfg, 51 | ppost_cfg, 52 | pbins, 53 | pxtalk, 54 | pArea1, 55 | pArea2, 56 | presults, 57 | phisto_merge_nb); 58 | 59 | return status; 60 | } 61 | 62 | 63 | VL53LX_Error VL53LX_ipp_hist_ambient_dmax( 64 | VL53LX_DEV Dev, 65 | uint16_t target_reflectance, 66 | VL53LX_dmax_calibration_data_t *pdmax_cal, 67 | VL53LX_hist_gen3_dmax_config_t *pdmax_cfg, 68 | VL53LX_histogram_bin_data_t *pbins, 69 | int16_t *pambient_dmax_mm) 70 | { 71 | 72 | 73 | 74 | VL53LX_Error status = VL53LX_ERROR_NONE; 75 | 76 | SUPPRESS_UNUSED_WARNING(Dev); 77 | 78 | status = 79 | VL53LX_hist_ambient_dmax( 80 | target_reflectance, 81 | pdmax_cal, 82 | pdmax_cfg, 83 | pbins, 84 | pambient_dmax_mm); 85 | 86 | return status; 87 | } 88 | 89 | 90 | VL53LX_Error VL53LX_ipp_xtalk_calibration_process_data( 91 | VL53LX_DEV Dev, 92 | VL53LX_xtalk_range_results_t *pxtalk_ranges, 93 | VL53LX_xtalk_histogram_data_t *pxtalk_shape, 94 | VL53LX_xtalk_calibration_results_t *pxtalk_cal) 95 | { 96 | 97 | 98 | 99 | VL53LX_Error status = VL53LX_ERROR_NONE; 100 | 101 | SUPPRESS_UNUSED_WARNING(Dev); 102 | 103 | status = 104 | VL53LX_xtalk_calibration_process_data( 105 | pxtalk_ranges, 106 | pxtalk_shape, 107 | pxtalk_cal); 108 | 109 | return status; 110 | } 111 | 112 | 113 | 114 | -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_core_support.h: -------------------------------------------------------------------------------- 1 | 2 | /* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */ 3 | /****************************************************************************** 4 | * Copyright (c) 2020, STMicroelectronics - All Rights Reserved 5 | 6 | This file is part of VL53LX and is dual licensed, 7 | either GPL-2.0+ 8 | or 'BSD 3-clause "New" or "Revised" License' , at your option. 9 | ****************************************************************************** 10 | */ 11 | 12 | 13 | 14 | 15 | #ifndef _VL53LX_CORE_SUPPORT_H_ 16 | #define _VL53LX_CORE_SUPPORT_H_ 17 | 18 | #include "vl53lx_types.h" 19 | #include "vl53lx_hist_structs.h" 20 | 21 | #ifdef __cplusplus 22 | extern "C" { 23 | #endif 24 | 25 | 26 | 27 | 28 | uint32_t VL53LX_calc_pll_period_us( 29 | uint16_t fast_osc_frequency); 30 | 31 | 32 | 33 | 34 | 35 | uint32_t VL53LX_duration_maths( 36 | uint32_t pll_period_us, 37 | uint32_t vcsel_parm_pclks, 38 | uint32_t window_vclks, 39 | uint32_t periods_elapsed_mclks); 40 | 41 | 42 | 43 | uint32_t VL53LX_events_per_spad_maths( 44 | int32_t VL53LX_p_010, 45 | uint16_t num_spads, 46 | uint32_t duration); 47 | 48 | 49 | 50 | 51 | uint32_t VL53LX_isqrt( 52 | uint32_t num); 53 | 54 | 55 | 56 | 57 | void VL53LX_hist_calc_zero_distance_phase( 58 | VL53LX_histogram_bin_data_t *pdata); 59 | 60 | 61 | 62 | 63 | void VL53LX_hist_estimate_ambient_from_thresholded_bins( 64 | int32_t ambient_threshold_sigma, 65 | VL53LX_histogram_bin_data_t *pdata); 66 | 67 | 68 | 69 | 70 | void VL53LX_hist_remove_ambient_bins( 71 | VL53LX_histogram_bin_data_t *pdata); 72 | 73 | 74 | 75 | 76 | uint32_t VL53LX_calc_pll_period_mm( 77 | uint16_t fast_osc_frequency); 78 | 79 | 80 | 81 | 82 | uint16_t VL53LX_rate_maths( 83 | int32_t VL53LX_p_018, 84 | uint32_t time_us); 85 | 86 | 87 | 88 | 89 | uint16_t VL53LX_rate_per_spad_maths( 90 | uint32_t frac_bits, 91 | uint32_t peak_count_rate, 92 | uint16_t num_spads, 93 | uint32_t max_output_value); 94 | 95 | 96 | 97 | 98 | int32_t VL53LX_range_maths( 99 | uint16_t fast_osc_frequency, 100 | uint16_t VL53LX_p_014, 101 | uint16_t zero_distance_phase, 102 | uint8_t fractional_bits, 103 | int32_t gain_factor, 104 | int32_t range_offset_mm); 105 | 106 | 107 | 108 | 109 | uint8_t VL53LX_decode_vcsel_period( 110 | uint8_t vcsel_period_reg); 111 | 112 | 113 | 114 | void VL53LX_copy_xtalk_bin_data_to_histogram_data_struct( 115 | VL53LX_xtalk_histogram_shape_t *pxtalk, 116 | VL53LX_histogram_bin_data_t *phist); 117 | 118 | 119 | 120 | 121 | void VL53LX_init_histogram_bin_data_struct( 122 | int32_t bin_value, 123 | uint16_t VL53LX_p_021, 124 | VL53LX_histogram_bin_data_t *pdata); 125 | 126 | 127 | 128 | 129 | void VL53LX_decode_row_col( 130 | uint8_t spad_number, 131 | uint8_t *prow, 132 | uint8_t *pcol); 133 | 134 | 135 | 136 | 137 | void VL53LX_hist_find_min_max_bin_values( 138 | VL53LX_histogram_bin_data_t *pdata); 139 | 140 | 141 | 142 | 143 | void VL53LX_hist_estimate_ambient_from_ambient_bins( 144 | VL53LX_histogram_bin_data_t *pdata); 145 | 146 | 147 | #ifdef __cplusplus 148 | } 149 | #endif 150 | 151 | #endif 152 | 153 | -------------------------------------------------------------------------------- /lib/vl53l3c/stm32xxx_hal.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32xxx_hal.h 4 | * @date 10/03/2015 18:07:07 5 | * @brief This file allows to select the right stm32 hal file 6 | ****************************************************************************** 7 | * 8 | * COPYRIGHT(c) 2015 STMicroelectronics 9 | * 10 | * Redistribution and use in source and binary forms, with or without modification, 11 | * are permitted provided that the following conditions are met: 12 | * 1. Redistributions of source code must retain the above copyright notice, 13 | * this list of conditions and the following disclaimer. 14 | * 2. Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 3. Neither the name of STMicroelectronics nor the names of its contributors 18 | * may be used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************** 33 | */ 34 | 35 | /* Define to prevent recursive inclusion -------------------------------------*/ 36 | #ifndef __STM32xxx_HAL_H 37 | #define __STM32xxx_HAL_H 38 | 39 | 40 | #ifdef __cplusplus 41 | extern "C" { 42 | #endif 43 | 44 | /* Includes ------------------------------------------------------------------*/ 45 | #include "mxconstants.h" 46 | 47 | #ifdef STM32F401xE 48 | #include "stm32f4xx_hal.h" 49 | #define MCU_NAME "STM32F401xE" 50 | #endif 51 | #ifdef STM32L476xx 52 | #include "stm32l4xx_hal.h" 53 | #define MCU_NAME "STM32L476" 54 | #endif 55 | /* Exported types ------------------------------------------------------------*/ 56 | /* Exported constants --------------------------------------------------------*/ 57 | /* Exported macro ------------------------------------------------------------*/ 58 | #if TRACE_UART 59 | # define trace_printf uart_printf 60 | extern int uart_printf(const char *msg, ...); 61 | #else 62 | # define trace_printf(...) (void)0 63 | # define uart_printf(...) (void)0 64 | #endif 65 | /* Exported functions ------------------------------------------------------- */ 66 | 67 | #ifdef __cplusplus 68 | } 69 | #endif 70 | 71 | #endif /* __STM32xxx_HAL_H */ 72 | 73 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 74 | -------------------------------------------------------------------------------- /src/pid.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * MIT License 3 | * 4 | * Copyright (c) 2024 Kouhei Ito 5 | * Copyright (c) 2024 M5Stack 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the "Software"), to deal 9 | * in the Software without restriction, including without limitation the rights 10 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | * copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in all 15 | * copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | * SOFTWARE. 24 | */ 25 | 26 | #include 27 | #include "pid.hpp" 28 | 29 | PID::PID() { 30 | m_kp = 1.0e-8f; 31 | m_ti = 1.0e8f; 32 | m_td = 0.0f; 33 | m_eta = 0.01; 34 | m_integral = 0.0f; 35 | m_differential = 0.0f; 36 | m_err = 0.0f; 37 | m_h = 0.01f; 38 | } 39 | 40 | void PID::set_parameter(float kp, float ti, float td, float eta, float h) { 41 | m_kp = kp; 42 | m_ti = ti; 43 | m_td = td; 44 | m_eta = eta; 45 | m_h = h; 46 | } 47 | 48 | void PID::reset(void) { 49 | m_integral = 0.0f; 50 | m_differential = 0.0f; 51 | m_err = 0.0f; 52 | m_err2 = 0.0f; 53 | m_err3 = 0.0f; 54 | } 55 | 56 | void PID::i_reset(void) { 57 | m_integral = 0.0f; 58 | } 59 | void PID::printGain(void) { 60 | Serial.printf("#Kp:%8.4f Ti:%8.4f Td:%8.4f Eta:%8.4f h:%8.4f\r\n", m_kp, m_ti, m_td, m_eta, m_h); 61 | } 62 | 63 | void PID::set_error(float err) { 64 | m_err = err; 65 | } 66 | 67 | float PID::update(float err, float h) { 68 | float d; 69 | m_h = h; 70 | 71 | // 積分 72 | m_integral = m_integral + m_h * (err + m_err) / 2 / m_ti; 73 | if (m_integral > 30000.0f) m_integral = 30000.0f; 74 | if (m_integral < -30000.0f) m_integral = -30000.0f; 75 | // 不完全微分 76 | m_differential = (2 * m_eta * m_td - m_h) * m_differential / (2 * m_eta * m_td + m_h) + 77 | 2 * m_td * (err - m_err) / (2 * m_eta * m_td + m_h); 78 | m_err = err; 79 | return m_kp * (err + m_integral + m_differential); 80 | } 81 | 82 | Filter::Filter() { 83 | m_state = 0.0f; 84 | m_T = 0.0025f; 85 | m_h = 0.0025f; 86 | } 87 | 88 | void Filter::reset(void) { 89 | m_state = 0.0f; 90 | } 91 | 92 | void Filter::set_parameter(float T, float h) { 93 | m_T = T; 94 | m_h = h; 95 | } 96 | 97 | float Filter::update(float u, float h) { 98 | m_h = h; 99 | m_state = m_state * m_T / (m_T + m_h) + u * m_h / (m_T + m_h); 100 | m_out = m_state; 101 | return m_out; 102 | } 103 | -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_types.h: -------------------------------------------------------------------------------- 1 | 2 | /* 3 | * This file is part of VL53LX Platform 4 | * 5 | * Copyright (c) 2016, STMicroelectronics - All Rights Reserved 6 | * 7 | * License terms: BSD 3-clause "New" or "Revised" License. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions are met: 11 | * 12 | * 1. Redistributions of source code must retain the above copyright notice, this 13 | * list of conditions and the following disclaimer. 14 | * 15 | * 2. Redistributions in binary form must reproduce the above copyright notice, 16 | * this list of conditions and the following disclaimer in the documentation 17 | * and/or other materials provided with the distribution. 18 | * 19 | * 3. Neither the name of the copyright holder nor the names of its contributors 20 | * may be used to endorse or promote products derived from this software 21 | * without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 24 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 25 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 26 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 27 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 28 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 29 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 31 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 32 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | */ 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | #ifndef _VL53LX_TYPES_H_ 70 | #define _VL53LX_TYPES_H_ 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | #include 84 | #include 85 | #include 86 | #include 87 | #include 88 | 89 | #ifndef NULL 90 | #error "Error NULL definition should be done. Please add required include " 91 | #endif 92 | 93 | 94 | #if !defined(STDINT_H) && !defined(_STDINT_H) && !defined(_GCC_STDINT_H) && !defined(__STDINT_DECLS) && !defined(_GCC_WRAP_STDINT_H) && !defined(_STDINT) 95 | 96 | #pragma message("Please review type definition of STDINT define for your platform and add to list above ") 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | typedef unsigned long long uint64_t; 112 | 113 | 114 | 115 | 116 | 117 | 118 | typedef unsigned int uint32_t; 119 | 120 | 121 | 122 | 123 | 124 | typedef int int32_t; 125 | 126 | 127 | 128 | 129 | 130 | typedef unsigned short uint16_t; 131 | 132 | 133 | 134 | 135 | 136 | typedef short int16_t; 137 | 138 | 139 | 140 | 141 | 142 | typedef unsigned char uint8_t; 143 | 144 | 145 | 146 | 147 | 148 | typedef signed char int8_t; 149 | 150 | 151 | 152 | #endif 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | typedef uint32_t FixPoint1616_t; 161 | 162 | #endif 163 | 164 | -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_platform_init.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | Copyright (C) 2016, STMicroelectronics International N.V. 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of STMicroelectronics nor the 13 | names of its contributors may be used to endorse or promote products 14 | derived from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND 19 | NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED. 20 | IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | ******************************************************************************/ 28 | 29 | 30 | #ifndef _VL53LX_PLATFORM_INIT_H_ 31 | #define _VL53LX_PLATFORM_INIT_H_ 32 | 33 | #include 34 | 35 | #ifdef __cplusplus 36 | extern "C" 37 | { 38 | #endif 39 | 40 | /** 41 | * @file VL53LX_platform_init.h 42 | * 43 | * @brief EwokPlus25 comms and GPIO init 44 | */ 45 | 46 | 47 | /** 48 | * @brief Initialise platform comms, GPIO and reset device 49 | * 50 | * Initialises comms, sets the states of GPIO (xshutdown, ncs, 51 | * EVK device power regulator enable) and resets the device 52 | * 53 | * @param[in] pdev : pointer to device structure (device handle) 54 | * @param[in] i2c_slave_address : I2C slave address 55 | * @param[in] comms_type : Comms type: VL53LX_I2C or VL53LX_SPI 56 | * @param[in] comms_speed_khz : 400kHz recommended for I2C 57 | * 58 | * @return VL53LX_ERROR_NONE Success 59 | * @return "Other error code" See ::VL53LX_Error 60 | */ 61 | 62 | VL53LX_Error VL53LX_platform_init( 63 | VL53LX_Dev_t *pdev, 64 | uint8_t i2c_slave_address, 65 | uint8_t comms_type, 66 | uint16_t comms_speed_khz); 67 | 68 | 69 | /** 70 | * @brief Close platform comms and GPIO 71 | * 72 | * Puts the device into reset, disables the EVK device power regulator 73 | * and closes comms 74 | * 75 | * @param[in] pdev : pointer to device structure (device handle) 76 | * 77 | * @return VL53LX_ERROR_NONE Success 78 | * @return "Other error code" See ::VL53LX_Error 79 | */ 80 | 81 | VL53LX_Error VL53LX_platform_terminate( 82 | VL53LX_Dev_t *pdev); 83 | 84 | 85 | #ifdef __cplusplus 86 | } 87 | #endif 88 | 89 | #endif 90 | 91 | -------------------------------------------------------------------------------- /lib/vl53l3c/vl53l1_platform_user_defines.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | Copyright (C) 2016, STMicroelectronics International N.V. 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of STMicroelectronics nor the 13 | names of its contributors may be used to endorse or promote products 14 | derived from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND 19 | NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED. 20 | IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | ******************************************************************************/ 28 | 29 | 30 | #ifndef _VL53L1_PLATFORM_USER_DEFINES_H_ 31 | #define _VL53L1_PLATFORM_USER_DEFINES_H_ 32 | 33 | #ifdef __cplusplus 34 | extern "C" 35 | { 36 | #endif 37 | 38 | /** 39 | * @file vl53l1_platform_user_defines.h 40 | * 41 | * @brief All end user OS/platform/application definitions 42 | */ 43 | 44 | 45 | /** 46 | * @def do_division_u 47 | * @brief customer supplied division operation - 64-bit unsigned 48 | * 49 | * @param dividend unsigned 64-bit numerator 50 | * @param divisor unsigned 64-bit denominator 51 | */ 52 | #define do_division_u(dividend, divisor) (dividend / divisor) 53 | 54 | 55 | /** 56 | * @def do_division_s 57 | * @brief customer supplied division operation - 64-bit signed 58 | * 59 | * @param dividend signed 64-bit numerator 60 | * @param divisor signed 64-bit denominator 61 | */ 62 | #define do_division_s(dividend, divisor) (dividend / divisor) 63 | 64 | 65 | /** 66 | * @def WARN_OVERRIDE_STATUS 67 | * @brief customer supplied macro to optionally output info when a specific 68 | error has been overridden with success within the EwokPlus driver 69 | * 70 | * @param __X__ the macro which enabled the suppression 71 | */ 72 | #define WARN_OVERRIDE_STATUS(__X__)\ 73 | trace_print (VL53L1_TRACE_LEVEL_WARNING, #__X__); 74 | 75 | 76 | #ifdef _MSC_VER 77 | #define DISABLE_WARNINGS() { \ 78 | __pragma (warning (push)); \ 79 | __pragma (warning (disable:4127)); \ 80 | } 81 | #define ENABLE_WARNINGS() { \ 82 | __pragma (warning (pop)); \ 83 | } 84 | #else 85 | #define DISABLE_WARNINGS() 86 | #define ENABLE_WARNINGS() 87 | #endif 88 | 89 | 90 | #ifdef __cplusplus 91 | } 92 | #endif 93 | 94 | #endif // _VL53L1_PLATFORM_USER_DEFINES_H_ 95 | 96 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/bug-report.yml: -------------------------------------------------------------------------------- 1 | # Source: 2 | # https://github.com/Tinyu-Zhao/M5-Depends/blob/main/.github/ISSUE_TEMPLATE/bug-report.yml 3 | # See: 4 | # https://docs.github.com/communities/using-templates-to-encourage-useful-issues-and-pull-requests/syntax-for-issue-forms 5 | 6 | name: Bug report 7 | description: Report a problem with the code in this repository. 8 | labels: 9 | - "type: bug" 10 | body: 11 | - type: markdown 12 | attributes: 13 | value: "Thank you for opening an issue on an M5Stack Arduino library repository.\n\ 14 | To improve the speed of resolution please review the following guidelines and common troubleshooting steps below before creating the issue:\n\ 15 | If you have any UIFLOW questions you can ask in the [special board](https://community.m5stack.com/category/5/uiflow), there will be many enthusiastic friends to help you.\n\ 16 | Do not use GitHub issues for troubleshooting projects and issues. Instead use the forums at https://community.m5stack.com to ask questions and troubleshoot why something isn't working as expected. In many cases the problem is a common issue that you will more quickly receive help from the forum community. GitHub issues are meant for known defects in the code. If you don't know if there is a defect in the code then start with troubleshooting on the forum first." 17 | - type: textarea 18 | id: description 19 | attributes: 20 | label: Describe the bug 21 | description: A clear and concise description of what the bug is. 22 | validations: 23 | required: true 24 | - type: textarea 25 | id: reproduce 26 | attributes: 27 | label: To reproduce 28 | description: Provide the specific set of steps we can follow to reproduce the problem. 29 | placeholder: | 30 | 1. In this environment... 31 | 2. With this config... 32 | 3. Run '...' 33 | 4. See error... 34 | validations: 35 | required: true 36 | - type: textarea 37 | id: expected 38 | attributes: 39 | label: Expected behavior 40 | description: What would you expect to happen after following those instructions? 41 | validations: 42 | required: true 43 | - type: textarea 44 | id: screenshots 45 | attributes: 46 | label: Screenshots 47 | description: If applicable, add screenshots to help explain your problem. 48 | validations: 49 | required: false 50 | - type: textarea 51 | id: information 52 | attributes: 53 | label: Environment 54 | description: | 55 | If applicable, add screenshots to help explain your problem. 56 | examples: 57 | - **OS**: Ubuntu 20.04 58 | - **IDE & IDE Version**: Arduino 1.8.19 Or Platform IO v2.5.0 59 | - **Repository Version**: 0.4.0 60 | value: | 61 | - OS: 62 | - IDE &IDE Version: 63 | - Repository Version: 64 | validations: 65 | required: false 66 | - type: textarea 67 | id: additional 68 | attributes: 69 | label: Additional context 70 | description: Add any additional information here. 71 | validations: 72 | required: false 73 | - type: checkboxes 74 | id: checklist 75 | attributes: 76 | label: Issue checklist 77 | description: Please double-check that you have done each of the following things before submitting the issue. 78 | options: 79 | - label: I searched for previous reports in [the issue tracker](https://github.com/m5stack/M5Stack/issues?q=) 80 | required: true 81 | - label: My report contains all necessary details 82 | required: true 83 | -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_hist_char.c: -------------------------------------------------------------------------------- 1 | 2 | // SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause 3 | /****************************************************************************** 4 | * Copyright (c) 2020, STMicroelectronics - All Rights Reserved 5 | 6 | This file is part of VL53LX and is dual licensed, 7 | either GPL-2.0+ 8 | or 'BSD 3-clause "New" or "Revised" License' , at your option. 9 | ****************************************************************************** 10 | */ 11 | 12 | 13 | 14 | 15 | 16 | #include 17 | #include 18 | 19 | 20 | 21 | #include "vl53lx_core.h" 22 | #include "vl53lx_register_settings.h" 23 | #include "vl53lx_hist_char.h" 24 | 25 | #define LOG_FUNCTION_START(fmt, ...) \ 26 | _LOG_FUNCTION_START(VL53LX_TRACE_MODULE_HISTOGRAM, fmt, ##__VA_ARGS__) 27 | #define LOG_FUNCTION_END(status, ...) \ 28 | _LOG_FUNCTION_END(VL53LX_TRACE_MODULE_HISTOGRAM, status, ##__VA_ARGS__) 29 | #define LOG_FUNCTION_END_FMT(status, fmt, ...) \ 30 | _LOG_FUNCTION_END_FMT(VL53LX_TRACE_MODULE_HISTOGRAM,\ 31 | status, fmt, ##__VA_ARGS__) 32 | 33 | 34 | VL53LX_Error VL53LX_set_calib_config( 35 | VL53LX_DEV Dev, 36 | uint8_t vcsel_delay__a0, 37 | uint8_t calib_1, 38 | uint8_t calib_2, 39 | uint8_t calib_3, 40 | uint8_t calib_2__a0, 41 | uint8_t spad_readout) 42 | { 43 | 44 | 45 | VL53LX_Error status = VL53LX_ERROR_NONE; 46 | uint8_t comms_buffer[3]; 47 | 48 | LOG_FUNCTION_START(""); 49 | 50 | 51 | 52 | status = VL53LX_enable_powerforce(Dev); 53 | 54 | 55 | if (status == VL53LX_ERROR_NONE) 56 | status = VL53LX_disable_firmware(Dev); 57 | 58 | 59 | 60 | 61 | if (status == VL53LX_ERROR_NONE) { 62 | status = VL53LX_WrByte( 63 | Dev, 64 | VL53LX_RANGING_CORE__VCSEL_DELAY__A0, 65 | vcsel_delay__a0); 66 | } 67 | 68 | 69 | 70 | if (status == VL53LX_ERROR_NONE) { 71 | 72 | 73 | comms_buffer[0] = calib_1; 74 | comms_buffer[1] = calib_2; 75 | comms_buffer[2] = calib_3; 76 | 77 | status = VL53LX_WriteMulti( 78 | Dev, 79 | VL53LX_RANGING_CORE__CALIB_1, 80 | comms_buffer, 81 | 3); 82 | } 83 | 84 | 85 | 86 | if (status == VL53LX_ERROR_NONE) 87 | status = VL53LX_WrByte( 88 | Dev, 89 | VL53LX_RANGING_CORE__CALIB_2__A0, 90 | calib_2__a0); 91 | 92 | 93 | 94 | if (status == VL53LX_ERROR_NONE) 95 | status = VL53LX_WrByte( 96 | Dev, 97 | VL53LX_RANGING_CORE__SPAD_READOUT, 98 | spad_readout); 99 | 100 | 101 | 102 | if (status == VL53LX_ERROR_NONE) 103 | status = VL53LX_enable_firmware(Dev); 104 | 105 | LOG_FUNCTION_END(status); 106 | 107 | return status; 108 | } 109 | 110 | 111 | 112 | VL53LX_Error VL53LX_set_hist_calib_pulse_delay( 113 | VL53LX_DEV Dev, 114 | uint8_t calib_delay) 115 | { 116 | 117 | 118 | VL53LX_Error status = VL53LX_ERROR_NONE; 119 | 120 | LOG_FUNCTION_START(""); 121 | 122 | status = 123 | VL53LX_set_calib_config( 124 | Dev, 125 | 0x01, 126 | calib_delay, 127 | 0x04, 128 | 0x08, 129 | 0x14, 130 | VL53LX_RANGING_CORE__SPAD_READOUT__CALIB_PULSES); 131 | 132 | LOG_FUNCTION_END(status); 133 | 134 | return status; 135 | } 136 | 137 | 138 | VL53LX_Error VL53LX_disable_calib_pulse_delay( 139 | VL53LX_DEV Dev) 140 | { 141 | 142 | 143 | VL53LX_Error status = VL53LX_ERROR_NONE; 144 | 145 | LOG_FUNCTION_START(""); 146 | 147 | status = 148 | VL53LX_set_calib_config( 149 | Dev, 150 | 0x00, 151 | 0x00, 152 | 0x00, 153 | 0x00, 154 | 0x00, 155 | VL53LX_RANGING_CORE__SPAD_READOUT__STANDARD); 156 | 157 | LOG_FUNCTION_END(status); 158 | 159 | return status; 160 | } 161 | 162 | 163 | -------------------------------------------------------------------------------- /lib/vl53l3c/vl53l1_platform_user_config.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | Copyright (C) 2015, STMicroelectronics International N.V. 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of STMicroelectronics nor the 13 | names of its contributors may be used to endorse or promote products 14 | derived from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND 19 | NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED. 20 | IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | ********************************************************************************/ 28 | 29 | /** 30 | * @file vl53l1_platform_user_config.h 31 | * 32 | * @brief EwokPlus compile time user modifiable configuration 33 | */ 34 | 35 | 36 | #ifndef _VL53L1_PLATFORM_USER_CONFIG_H_ 37 | #define _VL53L1_PLATFORM_USER_CONFIG_H_ 38 | 39 | #define VL53L1_BYTES_PER_WORD 2 40 | #define VL53L1_BYTES_PER_DWORD 4 41 | 42 | /* Define polling delays */ 43 | #define VL53L1_BOOT_COMPLETION_POLLING_TIMEOUT_MS 500 44 | #define VL53L1_RANGE_COMPLETION_POLLING_TIMEOUT_MS 2000 45 | #define VL53L1_TEST_COMPLETION_POLLING_TIMEOUT_MS 60000 46 | 47 | #define VL53L1_POLLING_DELAY_MS 1 48 | 49 | /* Define LLD TuningParms Page Base Address 50 | * - Part of Patch_AddedTuningParms_11761 51 | */ 52 | #define VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS 0x8000 53 | #define VL53L1_TUNINGPARM_PRIVATE_PAGE_BASE_ADDRESS 0xC000 54 | 55 | #define VL53L1_GAIN_FACTOR__STANDARD_DEFAULT 0x0800 56 | /*!< Default standard ranging gain correction factor 57 | 1.11 format. 1.0 = 0x0800, 0.980 = 0x07D7 */ 58 | 59 | #define VL53L1_OFFSET_CAL_MIN_EFFECTIVE_SPADS 0x0500 60 | /*!< Lower Limit for the MM1 effective SPAD count during offset 61 | calibration Format 8.8 0x0500 -> 5.0 effective SPADs */ 62 | 63 | #define VL53L1_OFFSET_CAL_MAX_PRE_PEAK_RATE_MCPS 0x1900 64 | /*!< Max Limit for the pre range peak rate during offset 65 | calibration Format 9.7 0x1900 -> 50.0 Mcps. 66 | If larger then in pile up */ 67 | 68 | #define VL53L1_OFFSET_CAL_MAX_SIGMA_MM 0x0040 69 | /*!< Max sigma estimate limit during offset calibration 70 | Check applies to pre-range, mm1 and mm2 ranges 71 | Format 14.2 0x0040 -> 16.0mm. */ 72 | 73 | #define VL53L1_MAX_USER_ZONES 169 74 | /*!< Max number of user Zones - maximal limitation from 75 | FW stream divide - value of 254 */ 76 | 77 | #define VL53L1_MAX_RANGE_RESULTS 2 78 | /*!< Allocates storage for return and reference restults */ 79 | 80 | 81 | #define VL53L1_MAX_STRING_LENGTH 512 82 | 83 | #endif /* _VL53L1_PLATFORM_USER_CONFIG_H_ */ 84 | 85 | -------------------------------------------------------------------------------- /src/sensor.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * MIT License 3 | * 4 | * Copyright (c) 2024 Kouhei Ito 5 | * Copyright (c) 2024 M5Stack 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the "Software"), to deal 9 | * in the Software without restriction, including without limitation the rights 10 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | * copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in all 15 | * copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | * SOFTWARE. 24 | */ 25 | 26 | #ifndef SENSOR_HPP 27 | #define SENSOR_HPP 28 | 29 | #include 30 | #include "flight_control.hpp" 31 | #include "pid.hpp" 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include "alt_kalman.hpp" 37 | #include 38 | #include "driver/gpio.h" 39 | #include "sdkconfig.h" 40 | 41 | #define SDA_PIN (3) 42 | #define SCL_PIN (4) 43 | #define PIN_NUM_MISO (43) 44 | #define PIN_NUM_MOSI (14) 45 | #define PIN_NUM_CLK (44) 46 | #define PIN_CS (46) 47 | 48 | typedef struct { 49 | spi_host_device_t host; ///< The SPI host used, set before calling `spi_eeprom_init()` 50 | gpio_num_t cs_io; ///< CS gpio number, set before calling `spi_eeprom_init()` 51 | gpio_num_t miso_io; ///< MISO gpio number, set before calling `spi_eeprom_init()` 52 | bool intr_used; ///< Whether to use polling or interrupt when waiting for write to be done. Set before calling 53 | ///< `spi_eeprom_init()`. 54 | } eeprom_config_t; 55 | 56 | typedef struct eeprom_context_t* eeprom_handle_t; 57 | 58 | typedef struct { 59 | float q0; 60 | float q1; 61 | float q2; 62 | float q3; 63 | } quat_t; 64 | 65 | typedef struct { 66 | uint16_t distance; 67 | uint16_t cnt; 68 | } distance_t; 69 | 70 | // Sensor data 71 | // extern volatile float Ax,Ay,Az,Wp,Wq,Wr,Mx,My,Mz,Mx0,My0,Mz0,Mx_ave,My_ave,Mz_ave; 72 | extern volatile float Roll_angle, Pitch_angle, Yaw_angle; 73 | extern volatile float Roll_rate, Pitch_rate, Yaw_rate; 74 | extern volatile float Accel_x_raw, Accel_y_raw, Accel_z_raw; 75 | extern volatile float Accel_x, Accel_y, Accel_z; 76 | extern volatile float Accel_z_d; 77 | extern volatile int16_t RawRange; 78 | extern volatile int16_t Range; 79 | extern volatile float Altitude; 80 | extern volatile float Altitude2; 81 | extern volatile float Alt_velocity; 82 | extern volatile uint8_t Alt_control_ok; 83 | extern volatile float Voltage; 84 | extern float Acc_norm; 85 | extern quat_t Quat; 86 | extern float Over_g, Over_rate; 87 | extern uint8_t OverG_flag; 88 | extern uint8_t Range0flag; 89 | extern volatile uint8_t Under_voltage_flag; 90 | extern volatile uint8_t ToF_bottom_data_ready_flag; 91 | extern volatile float Az; 92 | extern volatile float Az_bias; 93 | extern Alt_kalman EstimatedAltitude; 94 | extern volatile int16_t RawRangeFront; 95 | extern volatile int16_t RangeFront; 96 | 97 | void sensor_init(void); 98 | float sensor_read(void); 99 | void sensor_reset_offset(void); 100 | void sensor_calc_offset_avarage(void); 101 | void ahrs_reset(void); 102 | uint8_t scan_i2c(void); 103 | 104 | #endif -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_platform_log.c: -------------------------------------------------------------------------------- 1 | /* 2 | * COPYRIGHT (C) STMicroelectronics 2015. All rights reserved. 3 | * 4 | * This software is the confidential and proprietary information of 5 | * STMicroelectronics ("Confidential Information"). You shall not 6 | * disclose such Confidential Information and shall use it only in 7 | * accordance with the terms of the license agreement you entered into 8 | * with STMicroelectronics 9 | * 10 | * Programming Golden Rule: Keep it Simple! 11 | * 12 | */ 13 | 14 | /** 15 | * @file VL53LX_platform_log.c 16 | * 17 | * @brief Code function definitions for EwokPlus25 Platform Logging Layer 18 | */ 19 | 20 | #include // sprintf(), vsnprintf(), printf() 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | 28 | #ifdef VL53LX_LOG_ENABLE 29 | 30 | char * _trace_filename = NULL; 31 | FILE *_tracefile = NULL; 32 | 33 | uint32_t _trace_level = VL53LX_TRACE_LEVEL_ALL; 34 | uint32_t _trace_modules = VL53LX_TRACE_MODULE_ALL; 35 | uint32_t _trace_functions = VL53LX_TRACE_FUNCTION_ALL; 36 | 37 | int8_t VL53LX_trace_config( 38 | char *filename, 39 | uint32_t modules, 40 | uint32_t level, 41 | uint32_t functions) 42 | { 43 | int8_t status = 0; 44 | 45 | // // Something in the commented out code below causes ncsim to crash! 46 | // 47 | // if (((_trace_filename != NULL) && (_trace_filename != filename)) ||strcmp(filename,"")==0) 48 | // { 49 | // if ( _tracefile != NULL ) 50 | // { 51 | // fclose(_tracefile); 52 | // _tracefile = NULL; 53 | // } 54 | // free(_trace_filename); 55 | // _trace_filename = NULL; 56 | // } 57 | // 58 | 59 | if (((filename != NULL) && (_tracefile == NULL)) && strcmp(filename,"")) 60 | { 61 | _tracefile = fopen(filename, "w+"); 62 | 63 | //TODO: Add time and header banner to the log file to indicate we've just opened a new log session 64 | 65 | if ( _tracefile != NULL ) 66 | { 67 | _trace_filename = (char*)malloc((strlen(filename) + 1) * sizeof(char)); 68 | strcpy(_trace_filename, filename); 69 | } 70 | else 71 | { 72 | printf("VL53LX_trace_config(): failed to open log file (%s)\n", filename); 73 | status = 1; 74 | } 75 | } 76 | 77 | _trace_modules = modules; 78 | _trace_level = level; 79 | _trace_functions = functions; 80 | 81 | return status; 82 | } 83 | 84 | void VL53LX_trace_print_module_function(uint32_t module, uint32_t level, uint32_t function, const char *format, ...) 85 | { 86 | if ( ((level <=_trace_level) && ((module & _trace_modules) > 0)) 87 | || ((function & _trace_functions) > 0) ) 88 | { 89 | va_list arg_list; 90 | char message[VL53LX_MAX_STRING_LENGTH]; 91 | 92 | va_start(arg_list, format); 93 | vsnprintf(message, VL53LX_MAX_STRING_LENGTH-1, format, arg_list); /*lint !e534 ignore return*/ 94 | va_end(arg_list); 95 | 96 | if (_tracefile != NULL) 97 | { 98 | fprintf(_tracefile, message); /*lint !e592 ignore format specifier*/ 99 | } 100 | else 101 | { 102 | CH_printf(CH_ID_TRACE, message); /*lint !e592 ignore format specifier*/ 103 | } 104 | 105 | // if (_tracefile != NULL) 106 | // fprintf(_tracefile, message); 107 | // else 108 | // printf(message); 109 | } /*lint !e438 ignore issues with arg_list*/ 110 | } 111 | 112 | 113 | uint32_t VL53LX_get_trace_functions(void) 114 | { 115 | return _trace_functions; 116 | } 117 | 118 | 119 | void VL53LX_set_trace_functions(uint32_t function) 120 | { 121 | _trace_functions = function; 122 | } 123 | 124 | 125 | uint32_t VL53LX_clock(void) 126 | { 127 | /* Returns current tick count in [ms] */ 128 | uint32_t tick_count_ms = (uint32_t)clock(); 129 | return tick_count_ms; 130 | } 131 | #endif // VL53LX_LOG_ENABLE 132 | -------------------------------------------------------------------------------- /src/alt_kalman.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * MIT License 3 | * 4 | * Copyright (c) 2024 Kouhei Ito 5 | * Copyright (c) 2024 M5Stack 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the "Software"), to deal 9 | * in the Software without restriction, including without limitation the rights 10 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | * copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in all 15 | * copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | * SOFTWARE. 24 | */ 25 | 26 | #include "alt_kalman.hpp" 27 | // #include 28 | // #include 29 | // #include 30 | #include 31 | 32 | Alt_kalman::Alt_kalman() {}; 33 | 34 | void Alt_kalman::update(float z_sens, float accel, float h) { 35 | step = h; 36 | 37 | // x:estimate x_:predict 38 | 39 | // Matrix update 40 | // f13 = -step; 41 | // f21 = step; 42 | // f33 = 1 + beta*step; 43 | // b11 = step; 44 | // b32 = step; 45 | 46 | // predict state 47 | velocity_ = velocity + (accel - bias) * step; 48 | altitude_ = altitude + velocity * step; 49 | bias_ = bias * (1 + step * beta); 50 | // velocity = velocity_; 51 | // altitude = altitude_; 52 | 53 | // predict P 54 | p11_ = p11 - step * (p31 + p13) + step * step * p33 + step * step * q1; 55 | p12_ = step * (p11 - p32) - step * step * p31 + p12; 56 | p13_ = (1 + beta * step) * (p13 - step * p33); 57 | 58 | p21_ = step * (p11 - p23) - step * step * p13 + p21; 59 | p22_ = step * step * p11 + step * (p21 + p12) + p22; 60 | p23_ = (1 + beta * step) * (p23 + step * p13); 61 | 62 | p31_ = (1 + beta * step) * (p31 - step * p33); 63 | p32_ = (1 + beta * step) * (p32 + step * p31); 64 | p33_ = (1 + beta * step) * (1 + beta * step) * p33 + step * step * q2; 65 | // USBSerial.printf("%f %f %f %f %f %f %f %f %f\n\r",p11_,p12_,p13,p21_,p22_,p23_,p31_,p32_,p33_); 66 | 67 | // update kalman gain 68 | float s = p22_ + R; 69 | k1 = p12_ / s; 70 | k2 = p22_ / s; 71 | k3 = p32_ / s; 72 | 73 | // inovation 74 | float e = z_sens - altitude_; 75 | 76 | // estimate state 77 | velocity = velocity_ + k1 * e; 78 | altitude = altitude_ + k2 * e; 79 | bias = bias_ + k3 * e; 80 | 81 | // Estimated state output 82 | Velocity = velocity; 83 | Altitude = altitude; 84 | Bias = bias; 85 | // printf("%11.3f %11.4f %11.4f %11.4f %11.4f ", t+step, velocity, altitude, z_sens, true_v); 86 | 87 | // estimate P 88 | p11 = p11_ - k1 * p21_; 89 | p12 = p12_ - k1 * p22_; 90 | p13 = p13_ - k1 * p23_; 91 | p21 = p21_ * (1 - k2); 92 | p22 = p22_ * (1 - k2); 93 | p23 = p23_ * (1 - k2); 94 | p31 = -k3 * p21_ + p31_; 95 | p32 = -k3 * p22_ + p32_; 96 | p33 = -k3 * p23_ + p33_; 97 | 98 | // printf("%11.4f %11.4f %11.4f %11.4f\n", p11, p12, p21, p22); 99 | } 100 | 101 | void Alt_kalman::set_vel(float v) { 102 | velocity = v; 103 | } 104 | 105 | void Alt_kalman::reset(void) { 106 | // P 107 | p11 = 100.0, p12 = 0.0, p13 = 0.0; 108 | p21 = 0.0, p22 = 100.0, p23 = 0.0; 109 | p31 = 0.0, p32 = 0.0, p33 = 100.0; 110 | 111 | Velocity = 0.0, Altitude = 0.0, Bias = 0.0; 112 | } 113 | 114 | void mat_times(Mat A, Mat B) { 115 | } 116 | -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_nvm.h: -------------------------------------------------------------------------------- 1 | 2 | /* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */ 3 | /****************************************************************************** 4 | * Copyright (c) 2020, STMicroelectronics - All Rights Reserved 5 | 6 | This file is part of VL53LX and is dual licensed, 7 | either GPL-2.0+ 8 | or 'BSD 3-clause "New" or "Revised" License' , at your option. 9 | ****************************************************************************** 10 | */ 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | #ifndef _VL53LX_NVM_H_ 19 | #define _VL53LX_NVM_H_ 20 | 21 | #include "vl53lx_platform.h" 22 | #include "vl53lx_ll_def.h" 23 | 24 | #ifdef __cplusplus 25 | extern "C" 26 | { 27 | #endif 28 | 29 | #define VL53LX_NVM_POWER_UP_DELAY_US 50 30 | #define VL53LX_NVM_READ_TRIGGER_DELAY_US 5 31 | 32 | 33 | 34 | VL53LX_Error VL53LX_nvm_enable( 35 | VL53LX_DEV Dev, 36 | uint16_t nvm_ctrl_pulse_width, 37 | int32_t nvm_power_up_delay_us); 38 | 39 | 40 | 41 | 42 | VL53LX_Error VL53LX_nvm_read( 43 | VL53LX_DEV Dev, 44 | uint8_t start_address, 45 | uint8_t count, 46 | uint8_t *pdata); 47 | 48 | 49 | 50 | 51 | VL53LX_Error VL53LX_nvm_disable( 52 | VL53LX_DEV Dev); 53 | 54 | 55 | 56 | 57 | VL53LX_Error VL53LX_nvm_format_decode( 58 | uint16_t buf_size, 59 | uint8_t *pbuffer, 60 | VL53LX_decoded_nvm_data_t *pdata); 61 | 62 | 63 | 64 | 65 | VL53LX_Error VL53LX_nvm_decode_optical_centre( 66 | uint16_t buf_size, 67 | uint8_t *pbuffer, 68 | VL53LX_optical_centre_t *pdata); 69 | 70 | 71 | 72 | 73 | VL53LX_Error VL53LX_nvm_decode_cal_peak_rate_map( 74 | uint16_t buf_size, 75 | uint8_t *pbuffer, 76 | VL53LX_cal_peak_rate_map_t *pdata); 77 | 78 | 79 | 80 | 81 | VL53LX_Error VL53LX_nvm_decode_additional_offset_cal_data( 82 | uint16_t buf_size, 83 | uint8_t *pbuffer, 84 | VL53LX_additional_offset_cal_data_t *pdata); 85 | 86 | 87 | 88 | 89 | VL53LX_Error VL53LX_nvm_decode_fmt_range_results_data( 90 | uint16_t buf_size, 91 | uint8_t *pbuffer, 92 | VL53LX_decoded_nvm_fmt_range_data_t *pdata); 93 | 94 | 95 | 96 | 97 | VL53LX_Error VL53LX_nvm_decode_fmt_info( 98 | uint16_t buf_size, 99 | uint8_t *pbuffer, 100 | VL53LX_decoded_nvm_fmt_info_t *pdata); 101 | 102 | 103 | 104 | 105 | VL53LX_Error VL53LX_nvm_decode_ews_info( 106 | uint16_t buf_size, 107 | uint8_t *pbuffer, 108 | VL53LX_decoded_nvm_ews_info_t *pdata); 109 | 110 | 111 | 112 | 113 | void VL53LX_nvm_format_encode( 114 | VL53LX_decoded_nvm_data_t *pnvm_info, 115 | uint8_t *pnvm_data); 116 | 117 | 118 | 119 | 120 | VL53LX_Error VL53LX_read_nvm_raw_data( 121 | VL53LX_DEV Dev, 122 | uint8_t start_address, 123 | uint8_t count, 124 | uint8_t *pnvm_raw_data); 125 | 126 | 127 | 128 | 129 | VL53LX_Error VL53LX_read_nvm( 130 | VL53LX_DEV Dev, 131 | uint8_t nvm_format, 132 | VL53LX_decoded_nvm_data_t *pnvm_info); 133 | 134 | 135 | 136 | 137 | VL53LX_Error VL53LX_read_nvm_optical_centre( 138 | VL53LX_DEV Dev, 139 | VL53LX_optical_centre_t *pcentre); 140 | 141 | 142 | 143 | 144 | VL53LX_Error VL53LX_read_nvm_cal_peak_rate_map( 145 | VL53LX_DEV Dev, 146 | VL53LX_cal_peak_rate_map_t *pcal_data); 147 | 148 | 149 | 150 | 151 | VL53LX_Error VL53LX_read_nvm_additional_offset_cal_data( 152 | VL53LX_DEV Dev, 153 | VL53LX_additional_offset_cal_data_t *pcal_data); 154 | 155 | 156 | 157 | 158 | VL53LX_Error VL53LX_read_nvm_fmt_range_results_data( 159 | VL53LX_DEV Dev, 160 | uint16_t range_results_select, 161 | VL53LX_decoded_nvm_fmt_range_data_t *prange_data); 162 | 163 | 164 | #ifdef __cplusplus 165 | } 166 | #endif 167 | 168 | #endif 169 | 170 | -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_platform_user_data.h: -------------------------------------------------------------------------------- 1 | 2 | /* 3 | * This file is part of VL53LX Platform 4 | * 5 | * Copyright (c) 2016, STMicroelectronics - All Rights Reserved 6 | * 7 | * License terms: BSD 3-clause "New" or "Revised" License. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions are met: 11 | * 12 | * 1. Redistributions of source code must retain the above copyright notice, this 13 | * list of conditions and the following disclaimer. 14 | * 15 | * 2. Redistributions in binary form must reproduce the above copyright notice, 16 | * this list of conditions and the following disclaimer in the documentation 17 | * and/or other materials provided with the distribution. 18 | * 19 | * 3. Neither the name of the copyright holder nor the names of its contributors 20 | * may be used to endorse or promote products derived from this software 21 | * without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 24 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 25 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 26 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 27 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 28 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 29 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 31 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 32 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | */ 35 | 36 | 37 | 38 | 39 | #include "stm32xxx_hal.h" 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | #ifndef _VL53LX_PLATFORM_USER_DATA_H_ 66 | #define _VL53LX_PLATFORM_USER_DATA_H_ 67 | 68 | #ifndef __KERNEL__ 69 | #include 70 | #endif 71 | 72 | #include "vl53lx_def.h" 73 | 74 | #ifdef __cplusplus 75 | extern "C" 76 | { 77 | #endif 78 | 79 | 80 | typedef struct { 81 | VL53LX_DevData_t Data; 82 | /*!< Low Level Driver data structure */ 83 | uint8_t i2c_slave_address; 84 | uint8_t comms_type; 85 | uint16_t comms_speed_khz; 86 | I2C_HandleTypeDef *I2cHandle; 87 | uint8_t I2cDevAddr; 88 | int Present; 89 | int Enabled; 90 | int LoopState; 91 | int FirstStreamCountZero; 92 | int Idle; 93 | int Ready; 94 | uint8_t RangeStatus; 95 | FixPoint1616_t SignalRateRtnMegaCps; 96 | VL53LX_DeviceState device_state; /*!< Device State */ 97 | } VL53LX_Dev_t; 98 | 99 | typedef VL53LX_Dev_t* VL53LX_DEV; 100 | 101 | /** 102 | * @def VL53LXDevDataGet 103 | * @brief Get ST private structure @a VL53LX_DevData_t data access 104 | * 105 | * @param Dev Device Handle 106 | * @param field ST structure field name 107 | * It maybe used and as real data "ref" not just as "get" for sub-structure item 108 | * like VL53L1DevDataGet(FilterData.field)[i] or 109 | * VL53L1DevDataGet(FilterData.MeasurementIndex)++ 110 | */ 111 | #define VL53LXDevDataGet(Dev, field) (Dev->Data.field) 112 | 113 | 114 | /** 115 | * @def VL53LXDevDataSet(Dev, field, data) 116 | * @brief Set ST private structure @a VL53LX_DevData_t data field 117 | * @param Dev Device Handle 118 | * @param field ST structure field name 119 | * @param data Data to be set 120 | */ 121 | #define VL53LXDevDataSet(Dev, field, data) ((Dev->Data.field) = (data)) 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | #define PALDevDataGet(Dev, field) (Dev->Data.field) 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | #define PALDevDataSet(Dev, field, VL53LX_PRM_00005) (Dev->Data.field)=(VL53LX_PRM_00005) 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | #define VL53LXDevStructGetLLDriverHandle(Dev) (&Dev->Data.LLData) 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | #define VL53LXDevStructGetLLResultsHandle(Dev) (&Dev->Data.llresults) 166 | 167 | 168 | 169 | #ifdef __cplusplus 170 | } 171 | #endif 172 | 173 | #endif 174 | 175 | -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_xtalk.h: -------------------------------------------------------------------------------- 1 | 2 | /* SPDX-License-Identifier: BSD-3-Clause */ 3 | /****************************************************************************** 4 | * Copyright (c) 2020, STMicroelectronics - All Rights Reserved 5 | 6 | This file is part of VL53LX Protected and is dual licensed, 7 | either 'STMicroelectronics Proprietary license' 8 | or 'BSD 3-clause "New" or "Revised" License' , at your option. 9 | 10 | ****************************************************************************** 11 | 12 | 'STMicroelectronics Proprietary license' 13 | 14 | ****************************************************************************** 15 | 16 | License terms: STMicroelectronics Proprietary in accordance with licensing 17 | terms at www.st.com/sla0081 18 | 19 | ****************************************************************************** 20 | */ 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | #ifndef _VL53LX_XTALK_H_ 29 | #define _VL53LX_XTALK_H_ 30 | 31 | #include "vl53lx_types.h" 32 | #include "vl53lx_ll_def.h" 33 | #include "vl53lx_xtalk_private_structs.h" 34 | 35 | #ifdef __cplusplus 36 | extern "C" { 37 | #endif 38 | 39 | 40 | 41 | 42 | VL53LX_Error VL53LX_xtalk_calibration_process_data( 43 | VL53LX_xtalk_range_results_t *pxtalk_ranges, 44 | VL53LX_xtalk_histogram_data_t *pxtalk_shape, 45 | VL53LX_xtalk_calibration_results_t *pxtalk_cal); 46 | 47 | 48 | 49 | 50 | VL53LX_Error VL53LX_f_041( 51 | VL53LX_histogram_bin_data_t *pavg_bins, 52 | VL53LX_xtalk_algo_data_t *pdebug, 53 | VL53LX_xtalk_range_data_t *pxtalk_data, 54 | uint8_t histogram__window_start, 55 | uint8_t histogram__window_end, 56 | VL53LX_xtalk_histogram_shape_t *pxtalk_shape); 57 | 58 | 59 | 60 | VL53LX_Error VL53LX_f_039( 61 | VL53LX_xtalk_range_results_t *pxtalk_results, 62 | VL53LX_xtalk_algo_data_t *pdebug, 63 | int16_t *xgradient, 64 | int16_t *ygradient); 65 | 66 | 67 | 68 | 69 | VL53LX_Error VL53LX_f_040( 70 | VL53LX_xtalk_range_data_t *pxtalk_data, 71 | VL53LX_xtalk_algo_data_t *pdebug, 72 | uint32_t *xtalk_mean_offset_kcps); 73 | 74 | 75 | 76 | 77 | VL53LX_Error VL53LX_f_045( 78 | VL53LX_histogram_bin_data_t *phist_data, 79 | VL53LX_xtalk_range_data_t *pxtalk_data, 80 | VL53LX_xtalk_algo_data_t *pdebug, 81 | VL53LX_xtalk_histogram_shape_t *pxtalk_histo); 82 | 83 | 84 | 85 | 86 | 87 | VL53LX_Error VL53LX_f_032( 88 | uint32_t mean_offset, 89 | int16_t xgradient, 90 | int16_t ygradient, 91 | int8_t centre_offset_x, 92 | int8_t centre_offset_y, 93 | uint16_t roi_effective_spads, 94 | uint8_t roi_centre_spad, 95 | uint8_t roi_xy_size, 96 | uint32_t *xtalk_rate_kcps); 97 | 98 | 99 | 100 | 101 | VL53LX_Error VL53LX_f_033( 102 | VL53LX_histogram_bin_data_t *phist_data, 103 | VL53LX_xtalk_histogram_shape_t *pxtalk_data, 104 | uint32_t xtalk_rate_kcps, 105 | VL53LX_histogram_bin_data_t *pxtalkcount_data); 106 | 107 | 108 | 109 | 110 | VL53LX_Error VL53LX_f_047( 111 | VL53LX_histogram_bin_data_t *phist_data, 112 | VL53LX_histogram_bin_data_t *pxtalk_data, 113 | uint8_t xtalk_bin_offset); 114 | 115 | 116 | 117 | VL53LX_Error VL53LX_f_044( 118 | VL53LX_histogram_bin_data_t *pxtalk_data, 119 | uint32_t amb_threshold, 120 | uint8_t VL53LX_p_019, 121 | uint8_t VL53LX_p_024); 122 | 123 | 124 | 125 | VL53LX_Error VL53LX_f_046( 126 | VL53LX_customer_nvm_managed_t *pcustomer, 127 | VL53LX_dynamic_config_t *pdyn_cfg, 128 | VL53LX_xtalk_histogram_data_t *pxtalk_shape, 129 | VL53LX_histogram_bin_data_t *pip_hist_data, 130 | VL53LX_histogram_bin_data_t *pop_hist_data, 131 | VL53LX_histogram_bin_data_t *pxtalk_count_data); 132 | 133 | 134 | 135 | 136 | VL53LX_Error VL53LX_f_043( 137 | uint8_t sigma_mult, 138 | int32_t VL53LX_p_028, 139 | uint32_t *ambient_noise); 140 | 141 | 142 | #ifdef __cplusplus 143 | } 144 | #endif 145 | 146 | #endif 147 | 148 | -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_hist_algos_gen3.h: -------------------------------------------------------------------------------- 1 | 2 | /* SPDX-License-Identifier: BSD-3-Clause */ 3 | /****************************************************************************** 4 | * Copyright (c) 2020, STMicroelectronics - All Rights Reserved 5 | 6 | This file is part of VL53LX Protected and is dual licensed, 7 | either 'STMicroelectronics Proprietary license' 8 | or 'BSD 3-clause "New" or "Revised" License' , at your option. 9 | 10 | ****************************************************************************** 11 | 12 | 'STMicroelectronics Proprietary license' 13 | 14 | ****************************************************************************** 15 | 16 | License terms: STMicroelectronics Proprietary in accordance with licensing 17 | terms at www.st.com/sla0081 18 | 19 | ****************************************************************************** 20 | */ 21 | 22 | 23 | 24 | 25 | 26 | #ifndef _VL53LX_HIST_ALGOS_GEN3_H_ 27 | #define _VL53LX_HIST_ALGOS_GEN3_H_ 28 | 29 | #include "vl53lx_types.h" 30 | #include "vl53lx_ll_def.h" 31 | 32 | #include "vl53lx_hist_private_structs.h" 33 | #include "vl53lx_dmax_private_structs.h" 34 | 35 | #ifdef __cplusplus 36 | extern "C" 37 | { 38 | #endif 39 | 40 | 41 | 42 | 43 | void VL53LX_f_003( 44 | VL53LX_hist_gen3_algo_private_data_t *palgo); 45 | 46 | 47 | 48 | 49 | 50 | 51 | VL53LX_Error VL53LX_f_006( 52 | uint16_t ambient_threshold_events_scaler, 53 | int32_t ambient_threshold_sigma, 54 | int32_t min_ambient_threshold_events, 55 | uint8_t algo__crosstalk_compensation_enable, 56 | VL53LX_histogram_bin_data_t *pbins, 57 | VL53LX_histogram_bin_data_t *pxtalk, 58 | VL53LX_hist_gen3_algo_private_data_t *palgo); 59 | 60 | 61 | 62 | 63 | 64 | 65 | VL53LX_Error VL53LX_f_007( 66 | VL53LX_hist_gen3_algo_private_data_t *palgo); 67 | 68 | 69 | 70 | 71 | VL53LX_Error VL53LX_f_008( 72 | VL53LX_hist_gen3_algo_private_data_t *palgo); 73 | 74 | 75 | 76 | 77 | VL53LX_Error VL53LX_f_009( 78 | VL53LX_hist_gen3_algo_private_data_t *palgo); 79 | 80 | 81 | 82 | 83 | VL53LX_Error VL53LX_f_016( 84 | VL53LX_HistTargetOrder target_order, 85 | VL53LX_hist_gen3_algo_private_data_t *palgo); 86 | 87 | 88 | 89 | 90 | VL53LX_Error VL53LX_f_010( 91 | uint8_t pulse_no, 92 | VL53LX_histogram_bin_data_t *pbins, 93 | VL53LX_hist_gen3_algo_private_data_t *palgo); 94 | 95 | 96 | 97 | VL53LX_Error VL53LX_f_015( 98 | uint8_t pulse_no, 99 | uint8_t clip_events, 100 | VL53LX_histogram_bin_data_t *pbins, 101 | VL53LX_hist_gen3_algo_private_data_t *palgo); 102 | 103 | 104 | 105 | 106 | VL53LX_Error VL53LX_f_020( 107 | int16_t VL53LX_p_019, 108 | int16_t VL53LX_p_024, 109 | uint8_t VL53LX_p_030, 110 | uint8_t clip_events, 111 | VL53LX_histogram_bin_data_t *pbins, 112 | uint32_t *pphase); 113 | 114 | 115 | 116 | 117 | VL53LX_Error VL53LX_f_011( 118 | uint8_t pulse_no, 119 | VL53LX_histogram_bin_data_t *pbins, 120 | VL53LX_hist_gen3_algo_private_data_t *palgo, 121 | int32_t pad_value, 122 | VL53LX_histogram_bin_data_t *ppulse); 123 | 124 | 125 | 126 | 127 | VL53LX_Error VL53LX_f_014( 128 | uint8_t bin, 129 | uint8_t sigma_estimator__sigma_ref_mm, 130 | uint8_t VL53LX_p_030, 131 | uint8_t VL53LX_p_051, 132 | uint8_t crosstalk_compensation_enable, 133 | VL53LX_histogram_bin_data_t *phist_data_ap, 134 | VL53LX_histogram_bin_data_t *phist_data_zp, 135 | VL53LX_histogram_bin_data_t *pxtalk_hist, 136 | uint16_t *psigma_est); 137 | 138 | 139 | 140 | 141 | void VL53LX_f_017( 142 | uint8_t range_id, 143 | uint8_t valid_phase_low, 144 | uint8_t valid_phase_high, 145 | uint16_t sigma_thres, 146 | VL53LX_histogram_bin_data_t *pbins, 147 | VL53LX_hist_pulse_data_t *ppulse, 148 | VL53LX_range_data_t *pdata); 149 | 150 | 151 | #ifdef __cplusplus 152 | } 153 | #endif 154 | 155 | #endif 156 | 157 | -------------------------------------------------------------------------------- /src/imu.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * MIT License 3 | * 4 | * Copyright (c) 2024 Kouhei Ito 5 | * Copyright (c) 2024 M5Stack 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the "Software"), to deal 9 | * in the Software without restriction, including without limitation the rights 10 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | * copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in all 15 | * copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | * SOFTWARE. 24 | */ 25 | 26 | #include 27 | #include "common.h" 28 | #include "bmi2.h" 29 | #include "imu.hpp" 30 | #include 31 | 32 | float acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z; 33 | struct bmi2_sens_data imu_data; 34 | 35 | void imu_init(void) { 36 | int8_t st; 37 | uint8_t data = 0; 38 | 39 | USBSerial.printf("Start IMU Initialize!\n\r"); 40 | 41 | pinMode(46, OUTPUT); // CSを設定 42 | digitalWrite(46, 1); // CSをHIGH 43 | pinMode(12, OUTPUT); // CSを設定 44 | digitalWrite(12, 1); // CSをHIGH 45 | delay(5); 46 | USBSerial.printf("SPI Initilize status:%d\n\r", spi_init()); 47 | 48 | // BMI270 Init 49 | bmi270_dev_init(); 50 | st = bmi270_init(pBmi270); 51 | USBSerial.printf("#INIT Status:%d\n\r", st); 52 | if (st != 0) { 53 | USBSerial.printf("BMI270 INIT Fail!\n\r"); 54 | while (1); 55 | } 56 | USBSerial.printf("#Chip ID DEV:%02X\n\r", Bmi270.chip_id); 57 | USBSerial.printf("#APP_STATUS:%02X\n\r", Bmi270.aps_status); 58 | 59 | USBSerial.printf("#INIT_STATUS Read:%d\n\r", bmi2_get_regs(0x21, &data, 1, pBmi270)); 60 | USBSerial.printf("#INIT_STATUS:%02X\n\r", data); 61 | // IMU Config 62 | USBSerial.printf("#Config Status:%d\n\r", set_accel_gyro_config(pBmi270)); 63 | uint8_t sensor_list[2] = {BMI2_ACCEL, BMI2_GYRO}; 64 | USBSerial.printf("#Sensor enable Status:%d\n\r", bmi2_sensor_enable(sensor_list, 2, pBmi270)); 65 | } 66 | 67 | void imu_update(void) { 68 | bmi2_get_sensor_data(&imu_data, pBmi270); 69 | } 70 | 71 | float imu_get_acc_x(void) { 72 | return lsb_to_mps2(imu_data.acc.x, 8.0, 16) / GRAVITY_EARTH; 73 | } 74 | 75 | float imu_get_acc_y(void) { 76 | return lsb_to_mps2(imu_data.acc.y, 8.0, 16) / GRAVITY_EARTH; 77 | } 78 | 79 | float imu_get_acc_z(void) { 80 | return lsb_to_mps2(imu_data.acc.z, 8.0, 16) / GRAVITY_EARTH; 81 | } 82 | 83 | float imu_get_gyro_x(void) { 84 | return lsb_to_rps(imu_data.gyr.x, DPS20002RAD, 16); 85 | } 86 | 87 | float imu_get_gyro_y(void) { 88 | return lsb_to_rps(imu_data.gyr.y, DPS20002RAD, 16); 89 | } 90 | 91 | float imu_get_gyro_z(void) { 92 | return lsb_to_rps(imu_data.gyr.z, DPS20002RAD, 16); 93 | } 94 | 95 | void imu_test(void) { 96 | u_long st, now, old, end; 97 | uint16_t count; 98 | uint8_t ret; 99 | st = micros(); 100 | now = st; 101 | old = st; 102 | struct bmi2_sens_data imu_data; 103 | usleep(1000 * 5000); 104 | 105 | while (1) { 106 | old = now; 107 | now = micros(); 108 | ret = bmi2_get_sensor_data(&imu_data, pBmi270); 109 | // USBSerial.printf("%d\n\r", ret); 110 | acc_x = lsb_to_mps2(imu_data.acc.x, 8.0, 16); 111 | acc_y = lsb_to_mps2(imu_data.acc.y, 8.0, 16); 112 | acc_z = lsb_to_mps2(imu_data.acc.z, 8.0, 16); 113 | gyro_x = lsb_to_rps(imu_data.gyr.x, DPS10002RAD, 16); 114 | gyro_y = lsb_to_rps(imu_data.gyr.y, DPS10002RAD, 16); 115 | gyro_z = lsb_to_rps(imu_data.gyr.z, DPS10002RAD, 16); 116 | #if 1 117 | USBSerial.printf("%8.4f %7.5f %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f %d\n\r", (float)(now - st) * 1.0e-6, 118 | (float)(now - old) * 1.0e-6, acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z, ret); 119 | #endif 120 | } 121 | } 122 | -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_register_settings.h: -------------------------------------------------------------------------------- 1 | 2 | /* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */ 3 | /****************************************************************************** 4 | * Copyright (c) 2020, STMicroelectronics - All Rights Reserved 5 | 6 | This file is part of VL53LX and is dual licensed, 7 | either GPL-2.0+ 8 | or 'BSD 3-clause "New" or "Revised" License' , at your option. 9 | ****************************************************************************** 10 | */ 11 | 12 | 13 | 14 | 15 | #ifndef _VL53LX_REGISTER_SETTINGS_H_ 16 | #define _VL53LX_REGISTER_SETTINGS_H_ 17 | 18 | 19 | 20 | 21 | 22 | 23 | #define VL53LX_DEVICESCHEDULERMODE_PSEUDO_SOLO 0x00 24 | #define VL53LX_DEVICESCHEDULERMODE_STREAMING 0x01 25 | #define VL53LX_DEVICESCHEDULERMODE_HISTOGRAM 0x02 26 | 27 | 28 | 29 | 30 | 31 | #define VL53LX_DEVICEREADOUTMODE_SINGLE_SD (0x00 << 2) 32 | #define VL53LX_DEVICEREADOUTMODE_DUAL_SD (0x01 << 2) 33 | #define VL53LX_DEVICEREADOUTMODE_SPLIT_READOUT (0x02 << 2) 34 | #define VL53LX_DEVICEREADOUTMODE_SPLIT_MANUAL (0x03 << 2) 35 | 36 | 37 | 38 | 39 | 40 | 41 | #define VL53LX_DEVICEMEASUREMENTMODE_MODE_MASK 0xF0 42 | #define VL53LX_DEVICEMEASUREMENTMODE_STOP_MASK 0x0F 43 | 44 | #define VL53LX_GROUPEDPARAMETERHOLD_ID_MASK 0x02 45 | 46 | 47 | 48 | #define VL53LX_EWOK_I2C_DEV_ADDR_DEFAULT 0x29 49 | 50 | #define VL53LX_OSC_FREQUENCY 0x00 51 | #define VL53LX_OSC_TRIM_DEFAULT 0x00 52 | #define VL53LX_OSC_FREQ_SET_DEFAULT 0x00 53 | 54 | #define VL53LX_RANGE_HISTOGRAM_REF 0x08 55 | #define VL53LX_RANGE_HISTOGRAM_RET 0x10 56 | #define VL53LX_RANGE_HISTOGRAM_BOTH 0x18 57 | #define VL53LX_RANGE_HISTOGRAM_INIT 0x20 58 | #define VL53LX_RANGE_VHV_INIT 0x40 59 | 60 | 61 | #define VL53LX_RESULT_RANGE_STATUS 0x1F 62 | 63 | 64 | #define VL53LX_SYSTEM__SEED_CONFIG__MANUAL 0x00 65 | #define VL53LX_SYSTEM__SEED_CONFIG__STANDARD 0x01 66 | #define VL53LX_SYSTEM__SEED_CONFIG__EVEN_UPDATE_ONLY 0x02 67 | 68 | 69 | #define VL53LX_INTERRUPT_CONFIG_LEVEL_LOW 0x00 70 | #define VL53LX_INTERRUPT_CONFIG_LEVEL_HIGH 0x01 71 | #define VL53LX_INTERRUPT_CONFIG_OUT_OF_WINDOW 0x02 72 | #define VL53LX_INTERRUPT_CONFIG_IN_WINDOW 0x03 73 | #define VL53LX_INTERRUPT_CONFIG_NEW_SAMPLE_READY 0x20 74 | 75 | 76 | #define VL53LX_CLEAR_RANGE_INT 0x01 77 | #define VL53LX_CLEAR_ERROR_INT 0x02 78 | 79 | 80 | #define VL53LX_SEQUENCE_VHV_EN 0x01 81 | #define VL53LX_SEQUENCE_PHASECAL_EN 0x02 82 | #define VL53LX_SEQUENCE_REFERENCE_PHASE_EN 0x04 83 | #define VL53LX_SEQUENCE_DSS1_EN 0x08 84 | #define VL53LX_SEQUENCE_DSS2_EN 0x10 85 | #define VL53LX_SEQUENCE_MM1_EN 0x20 86 | #define VL53LX_SEQUENCE_MM2_EN 0x40 87 | #define VL53LX_SEQUENCE_RANGE_EN 0x80 88 | 89 | 90 | #define VL53LX_DSS_CONTROL__ROI_SUBTRACT 0x20 91 | #define VL53LX_DSS_CONTROL__ROI_INTERSECT 0x10 92 | 93 | #define VL53LX_DSS_CONTROL__MODE_DISABLED 0x00 94 | #define VL53LX_DSS_CONTROL__MODE_TARGET_RATE 0x01 95 | #define VL53LX_DSS_CONTROL__MODE_EFFSPADS 0x02 96 | #define VL53LX_DSS_CONTROL__MODE_BLOCKSELECT 0x03 97 | 98 | 99 | 100 | #define VL53LX_RANGING_CORE__SPAD_READOUT__STANDARD 0x45 101 | #define VL53LX_RANGING_CORE__SPAD_READOUT__RETURN_ARRAY_ONLY 0x05 102 | #define VL53LX_RANGING_CORE__SPAD_READOUT__REFERENCE_ARRAY_ONLY 0x55 103 | #define VL53LX_RANGING_CORE__SPAD_READOUT__RETURN_SPLIT_ARRAY 0x25 104 | #define VL53LX_RANGING_CORE__SPAD_READOUT__CALIB_PULSES 0xF5 105 | 106 | 107 | #define VL53LX_LASER_SAFETY__KEY_VALUE 0x6C 108 | 109 | 110 | 111 | #define VL53LX_RANGE_STATUS__RANGE_STATUS_MASK 0x1F 112 | #define VL53LX_RANGE_STATUS__MAX_THRESHOLD_HIT_MASK 0x20 113 | #define VL53LX_RANGE_STATUS__MIN_THRESHOLD_HIT_MASK 0x40 114 | #define VL53LX_RANGE_STATUS__GPH_ID_RANGE_STATUS_MASK 0x80 115 | 116 | 117 | 118 | #define VL53LX_INTERRUPT_STATUS__INT_STATUS_MASK 0x07 119 | #define VL53LX_INTERRUPT_STATUS__INT_ERROR_STATUS_MASK 0x18 120 | #define VL53LX_INTERRUPT_STATUS__GPH_ID_INT_STATUS_MASK 0x20 121 | 122 | 123 | 124 | 125 | #endif 126 | 127 | 128 | 129 | 130 | 131 | -------------------------------------------------------------------------------- /lib/bmi270/bmi270_maximum_fifo.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (c) 2023 Bosch Sensortec GmbH. All rights reserved. 3 | * 4 | * BSD-3-Clause 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 12 | * 2. Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived from 18 | * this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 26 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 28 | * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 29 | * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING 30 | * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * @file bmi270_maximum_fifo.h 34 | * @date 2023-05-03 35 | * @version v2.86.1 36 | * 37 | */ 38 | 39 | /** 40 | * \ingroup bmi2xy 41 | * \defgroup bmi270_maximum_fifo BMI270_MAXIMUM_FIFO 42 | * @brief Sensor driver for BMI270_MAXIMUM_FIFO sensor 43 | */ 44 | 45 | #ifndef BMI270_MAXIMUM_FIFO_H_ 46 | #define BMI270_MAXIMUM_FIFO_H_ 47 | 48 | /*! CPP guard */ 49 | #ifdef __cplusplus 50 | extern "C" { 51 | #endif 52 | 53 | /***************************************************************************/ 54 | 55 | /*! Header files 56 | ****************************************************************************/ 57 | #include "bmi2.h" 58 | 59 | /***************************************************************************/ 60 | 61 | /*! Macro definitions 62 | ****************************************************************************/ 63 | 64 | /*! @name BMI270 Chip identifier */ 65 | #define BMI270_MAXIMUM_FIFO_CHIP_ID UINT8_C(0x24) 66 | 67 | /*! @name Defines maximum number of pages */ 68 | #define BMI270_MAXIMUM_FIFO_MAX_PAGE_NUM UINT8_C(0) 69 | 70 | /*! @name Defines maximum number of feature input configurations */ 71 | #define BMI270_MAXIMUM_FIFO_MAX_FEAT_IN UINT8_C(0) 72 | 73 | /*! @name Defines maximum number of feature outputs */ 74 | #define BMI270_MAXIMUM_FIFO_MAX_FEAT_OUT UINT8_C(0) 75 | 76 | /*! @name Mask definitions for feature interrupt status bits */ 77 | 78 | /***************************************************************************/ 79 | 80 | /*! BMI270 User Interface function prototypes 81 | ****************************************************************************/ 82 | 83 | /** 84 | * \ingroup bmi270_maximum_fifo 85 | * \defgroup bmi270_maximum_fifoApiInit Initialization 86 | * @brief Initialize the sensor and device structure 87 | */ 88 | 89 | /*! 90 | * \ingroup bmi270_maximum_fifoApiInit 91 | * \page bmi270_maximum_fifo_api_bmi270_maximum_fifo_init bmi270_maximum_fifo_init 92 | * \code 93 | * int8_t bmi270_maximum_fifo_init(struct bmi2_dev *dev); 94 | * \endcode 95 | * @details This API: 96 | * 1) updates the device structure with address of the configuration file. 97 | * 2) Initializes BMI270 sensor. 98 | * 3) Writes the configuration file. 99 | * 4) Updates the feature offset parameters in the device structure. 100 | * 5) Updates the maximum number of pages, in the device structure. 101 | * 102 | * @param[in, out] dev : Structure instance of bmi2_dev. 103 | * 104 | * @return Result of API execution status 105 | * @retval 0 -> Success 106 | * @retval < 0 -> Fail 107 | */ 108 | int8_t bmi270_maximum_fifo_init(struct bmi2_dev *dev); 109 | 110 | /******************************************************************************/ 111 | /*! @name C++ Guard Macros */ 112 | /******************************************************************************/ 113 | #ifdef __cplusplus 114 | } 115 | #endif /* End of CPP guard */ 116 | 117 | #endif /* BMI270_MAXIMUM_FIFO_H_ */ 118 | -------------------------------------------------------------------------------- /src/led.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * MIT License 3 | * 4 | * Copyright (c) 2024 Kouhei Ito 5 | * Copyright (c) 2024 M5Stack 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the "Software"), to deal 9 | * in the Software without restriction, including without limitation the rights 10 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | * copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in all 15 | * copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | * SOFTWARE. 24 | */ 25 | 26 | #include "led.hpp" 27 | #include "sensor.hpp" 28 | #include "rc.hpp" 29 | #include "flight_control.hpp" 30 | 31 | uint32_t Led_color = 0x000000; 32 | uint32_t Led_color2 = 255; 33 | uint32_t Led_color3 = 0x000000; 34 | uint16_t LedBlinkCounter = 0; 35 | CRGB led_esp[1]; 36 | CRGB led_onboard[2]; 37 | 38 | void led_drive(void); 39 | void onboard_led1(CRGB p, uint8_t state); 40 | void onboard_led2(CRGB p, uint8_t state); 41 | void esp_led(CRGB p, uint8_t state); 42 | 43 | void led_init(void) { 44 | FastLED.addLeds(led_onboard, 2); 45 | FastLED.addLeds(led_esp, 1); 46 | FastLED.setBrightness(15); 47 | } 48 | 49 | void led_show(void) { 50 | FastLED.show(32); 51 | } 52 | 53 | void led_drive(void) { 54 | if (Mode == AVERAGE_MODE) { 55 | onboard_led1(PERPLE, 1); 56 | onboard_led2(PERPLE, 1); 57 | } else if (Mode == AUTO_LANDING_MODE) { 58 | onboard_led1(GREEN, 1); 59 | onboard_led2(GREEN, 1); 60 | } else if (Mode == FLIGHT_MODE) { 61 | if (Control_mode == ANGLECONTROL) { 62 | if (Flip_flag == 0) 63 | Led_color = YELLOW; // スタビライズモード・マニュアル飛行では黄色 64 | else 65 | Led_color = 0xFF9933; // 宙返りではオレンジ？ 66 | } else 67 | Led_color = 0xDC669B; // アクロモード 68 | 69 | if (Throttle_control_mode == 1) Led_color = 0xc71585; // 高度制御初期 70 | if (Alt_flag >= 1) Led_color = 0x331155; // 高度制御モードではピンク 71 | if (Rc_err_flag == 1) Led_color = 0xff0000; 72 | 73 | if (Under_voltage_flag < UNDER_VOLTAGE_COUNT) { 74 | onboard_led1(Led_color, 1); 75 | onboard_led2(Led_color, 1); 76 | } else { 77 | onboard_led1(POWEROFFCOLOR, 1); 78 | onboard_led2(Led_color, 1); 79 | } 80 | } else if (Mode == PARKING_MODE) { 81 | if (Under_voltage_flag < UNDER_VOLTAGE_COUNT) { 82 | // イルミネーション 83 | if (LedBlinkCounter == 0) { //<10 84 | if (Led_color2 & 0x800000) 85 | Led_color2 = (Led_color2 << 1) | 1; 86 | else 87 | Led_color2 = Led_color2 << 1; 88 | onboard_led1(Led_color2, 1); 89 | onboard_led2(Led_color2, 1); 90 | // if (Under_voltage_flag < UNDER_VOLTAGE_COUNT) {onboard_led1(Led_color2, 1);onboard_led2(Led_color2, 91 | // 1);} else onboard_led(POWEROFFCOLOR,1); 92 | LedBlinkCounter++; 93 | } 94 | LedBlinkCounter++; 95 | if (LedBlinkCounter > 20) LedBlinkCounter = 0; 96 | } else { 97 | // 水色点滅 98 | if (LedBlinkCounter < 10) { 99 | onboard_led1(POWEROFFCOLOR, 1); 100 | onboard_led2(POWEROFFCOLOR, 1); 101 | } else if (LedBlinkCounter < 200) { 102 | onboard_led1(POWEROFFCOLOR, 0); 103 | onboard_led2(POWEROFFCOLOR, 0); 104 | } else 105 | LedBlinkCounter = 0; 106 | LedBlinkCounter++; 107 | } 108 | } 109 | 110 | // LED show 111 | led_show(); 112 | // FastLED.show(128); 113 | } 114 | 115 | void onboard_led1(CRGB p, uint8_t state) { 116 | if (state == 1) { 117 | led_onboard[0] = p; 118 | } else { 119 | led_onboard[0] = 0; 120 | } 121 | return; 122 | } 123 | 124 | void onboard_led2(CRGB p, uint8_t state) { 125 | if (state == 1) { 126 | led_onboard[1] = p; 127 | } else { 128 | led_onboard[1] = 0; 129 | } 130 | return; 131 | } 132 | 133 | void esp_led(CRGB p, uint8_t state) { 134 | if (state == 1) 135 | led_esp[0] = p; 136 | else 137 | led_esp[0] = 0; 138 | return; 139 | } 140 | -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_platform_user_config.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | Copyright (C) 2015, STMicroelectronics International N.V. 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of STMicroelectronics nor the 13 | names of its contributors may be used to endorse or promote products 14 | derived from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND 19 | NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED. 20 | IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | ********************************************************************************/ 28 | 29 | /** 30 | * @file VL53LX_platform_user_config.h 31 | * 32 | * @brief EwokPlus compile time user modifiable configuration 33 | */ 34 | 35 | 36 | #ifndef _VL53LX_PLATFORM_USER_CONFIG_H_ 37 | #define _VL53LX_PLATFORM_USER_CONFIG_H_ 38 | 39 | #define VL53LX_BYTES_PER_WORD 2 40 | #define VL53LX_BYTES_PER_DWORD 4 41 | 42 | /* Define polling delays */ 43 | #define VL53LX_BOOT_COMPLETION_POLLING_TIMEOUT_MS 500 44 | #define VL53LX_RANGE_COMPLETION_POLLING_TIMEOUT_MS 2000 45 | #define VL53LX_TEST_COMPLETION_POLLING_TIMEOUT_MS 10000 46 | 47 | #define VL53LX_POLLING_DELAY_MS 1 48 | 49 | /* Define LLD TuningParms Page Base Address 50 | * - Part of Patch_AddedTuningParms_11761 51 | */ 52 | #define VL53LX_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS 0x8000 53 | #define VL53LX_TUNINGPARM_PRIVATE_PAGE_BASE_ADDRESS 0xC000 54 | 55 | #define VL53LX_OFFSET_CAL_MIN_MM1_EFFECTIVE_SPADS 0x0500 56 | /*!< Lower Limit for the MM1 effective SPAD count during offset 57 | calibration Format 8.8 0x0500 -> 5.0 effective SPADs */ 58 | 59 | #define VL53LX_GAIN_FACTOR__STANDARD_DEFAULT 0x0800 60 | /*!< Default standard ranging gain correction factor 61 | 1.11 format. 1.0 = 0x0800, 0.980 = 0x07D7 */ 62 | #define VL53LX_GAIN_FACTOR__HISTOGRAM_DEFAULT 0x0800 63 | /*!< Default histogram ranging gain correction factor 64 | 1.11 format. 1.0 = 0x0800, 0.975 = 0x07CC */ 65 | 66 | 67 | #define VL53LX_OFFSET_CAL_MIN_EFFECTIVE_SPADS 0x0500 68 | /*!< Lower Limit for the MM1 effective SPAD count during offset 69 | calibration Format 8.8 0x0500 -> 5.0 effective SPADs */ 70 | 71 | #define VL53LX_OFFSET_CAL_MAX_PRE_PEAK_RATE_MCPS 0x1900 72 | /*!< Max Limit for the pre range preak rate during offset 73 | calibration Format 9.7 0x1900 -> 50.0 Mcps. 74 | If larger then in pile up */ 75 | 76 | #define VL53LX_OFFSET_CAL_MAX_SIGMA_MM 0x0040 77 | /*!< Max sigma estimate limit during offset calibration 78 | Check applies to pre-range, mm1 and mm2 ranges 79 | Format 14.2 0x0040 -> 16.0mm. */ 80 | 81 | #define VL53LX_ZONE_CAL_MAX_PRE_PEAK_RATE_MCPS 0x1900 82 | /*!< Max Peak Rate Limit for the during zone calibration 83 | Format 9.7 0x1900 -> 50.0 Mcps. 84 | If larger then in pile up */ 85 | 86 | #define VL53LX_ZONE_CAL_MAX_SIGMA_MM 0x0040 87 | /*!< Max sigma estimate limit during zone calibration 88 | Format 14.2 0x0040 -> 16.0mm. */ 89 | 90 | 91 | #define VL53LX_XTALK_EXTRACT_MAX_SIGMA_MM 0x008C 92 | /*!< Max Sigma value allowed for a successful xtalk extraction 93 | Format 14.2 0x008C -> 35.0 mm.*/ 94 | 95 | #ifndef VL53LX_MAX_USER_ZONES 96 | #define VL53LX_MAX_USER_ZONES 16 97 | /*!< Max number of user Zones - maximal limitation from 98 | FW stream divide - value of 254 */ 99 | #endif 100 | 101 | #define VL53LX_MAX_RANGE_RESULTS 4 102 | #define VL53LX_BUFFER_SIZE 5 103 | 104 | /*!< Sets the maximum number of targets distances the histogram 105 | post processing can generate */ 106 | 107 | #define VL53LX_MAX_STRING_LENGTH 512 108 | /*!< Sets the maximum string length */ 109 | 110 | #endif /* _VL53LX_PLATFORM_USER_CONFIG_H_ */ 111 | 112 | -------------------------------------------------------------------------------- /src/flight_control.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * MIT License 3 | * 4 | * Copyright (c) 2024 Kouhei Ito 5 | * Copyright (c) 2024 M5Stack 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the "Software"), to deal 9 | * in the Software without restriction, including without limitation the rights 10 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | * copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in all 15 | * copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | * SOFTWARE. 24 | */ 25 | 26 | #ifndef CONTROL_HPP 27 | #define CONTROL_HPP 28 | 29 | // #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #define BATTERY_VOLTAGE (3.7) 36 | #define PIN_BUTTON 0 37 | #define AVERAGENUM 800 38 | 39 | #define INIT_MODE 0 40 | #define AVERAGE_MODE 1 41 | #define FLIGHT_MODE 2 42 | #define PARKING_MODE 3 43 | #define LOG_MODE 4 44 | #define AUTO_LANDING_MODE 5 45 | #define FLIP_MODE 6 46 | 47 | #define POWER_LIMIT 3.34 48 | #define UNDER_VOLTAGE_COUNT 100 49 | 50 | #define ANGLECONTROL 0 51 | #define RATECONTROL 1 52 | 53 | #define AUTO_ALT 4 54 | #define MANUAL_ALT 5 55 | 56 | #define ALT_LIMIT (2.0) 57 | #define ALT_REF_MIN (0.05) 58 | #define ALT_REF_MAX (1.8) 59 | 60 | #define RNAGE0FLAG_MAX (20) 61 | 62 | #define RATE_RATE (70.0f) 63 | #define RATE_MAX (1600.0f) 64 | #define RATE_EXPO (0.5f) 65 | 66 | // グローバル関数の宣言 67 | void init_copter(void); 68 | void loop_400Hz(void); 69 | void set_duty_fr(float duty); 70 | void set_duty_fl(float duty); 71 | void set_duty_rr(float duty); 72 | void set_duty_rl(float duty); 73 | 74 | // グローバル変数 75 | extern volatile uint8_t Mode; 76 | extern volatile uint8_t Loop_flag; 77 | extern float Control_period; 78 | extern volatile float Elapsed_time; 79 | 80 | // PID Gain 81 | // Rate control PID gain 82 | extern const float Roll_rate_kp; 83 | extern const float Roll_rate_ti; 84 | extern const float Roll_rate_td; 85 | extern const float Roll_rate_eta; 86 | 87 | extern const float Pitch_rate_kp; 88 | extern const float Pitch_rate_ti; 89 | extern const float Pitch_rate_td; 90 | extern const float Pitch_rate_eta; 91 | 92 | extern const float Yaw_rate_kp; 93 | extern const float Yaw_rate_ti; 94 | extern const float Yaw_rate_td; 95 | extern const float Yaw_rate_eta; 96 | 97 | // Angle control PID gain 98 | extern const float Rall_angle_kp; 99 | extern const float Rall_angle_ti; 100 | extern const float Rall_angle_td; 101 | extern const float Rall_angle_eta; 102 | 103 | extern const float Pitch_angle_kp; 104 | extern const float Pitch_angle_ti; 105 | extern const float Pitch_angle_td; 106 | extern const float Pitch_angle_eta; 107 | 108 | // Altitude control PID gain 109 | extern const float alt_kp; 110 | extern const float alt_ti; 111 | extern const float alt_td; 112 | extern const float alt_eta; 113 | extern const float alt_period; 114 | 115 | extern volatile float Interval_time; 116 | 117 | // Offset 118 | extern volatile float Roll_angle_offset, Pitch_angle_offset, Yaw_angle_offset; 119 | extern volatile float Elevator_center, Aileron_center, Rudder_center; 120 | 121 | // 制御目標 122 | // PID Control reference 123 | // 角速度目標値 124 | // Rate reference 125 | extern volatile float Roll_rate_reference, Pitch_rate_reference, Yaw_rate_reference; 126 | // 角度目標値 127 | // Angle reference 128 | extern volatile float Roll_angle_reference, Pitch_angle_reference, Yaw_angle_reference; 129 | // 舵角指令値 130 | // Commanad 131 | // スロットル指令値 132 | // Throttle 133 | extern volatile float Thrust_command; 134 | // 角速度指令値 135 | // Rate command 136 | extern volatile float Roll_rate_command, Pitch_rate_command, Yaw_rate_command; 137 | // 角度指令値 138 | // Angle comannd 139 | extern volatile float Roll_angle_command, Pitch_angle_command, Yaw_angle_command; 140 | // 高度目標 141 | extern volatile float Alt_ref; 142 | // Motor Duty 143 | extern volatile float FrontRight_motor_duty; 144 | extern volatile float FrontLeft_motor_duty; 145 | extern volatile float RearRight_motor_duty; 146 | extern volatile float RearLeft_motor_duty; 147 | // 速度目標Z 148 | extern float Z_dot_ref; 149 | 150 | extern uint8_t Control_mode; 151 | extern uint8_t Flip_flag; 152 | extern uint8_t Alt_flag; 153 | extern uint8_t Throttle_control_mode; 154 | 155 | extern uint8_t ahrs_reset_flag; 156 | extern uint8_t last_ahrs_reset_flag; 157 | #endif 158 | -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_api_preset_modes.h: -------------------------------------------------------------------------------- 1 | 2 | /* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */ 3 | /****************************************************************************** 4 | * Copyright (c) 2020, STMicroelectronics - All Rights Reserved 5 | 6 | This file is part of VL53LX and is dual licensed, 7 | either GPL-2.0+ 8 | or 'BSD 3-clause "New" or "Revised" License' , at your option. 9 | ****************************************************************************** 10 | */ 11 | 12 | 13 | 14 | 15 | #ifndef _VL53LX_API_PRESET_MODES_H_ 16 | #define _VL53LX_API_PRESET_MODES_H_ 17 | 18 | #include "vl53lx_ll_def.h" 19 | #include "vl53lx_dmax_structs.h" 20 | 21 | #ifdef __cplusplus 22 | extern "C" { 23 | #endif 24 | 25 | 26 | 27 | 28 | VL53LX_Error VL53LX_init_refspadchar_config_struct( 29 | VL53LX_refspadchar_config_t *pdata); 30 | 31 | 32 | 33 | 34 | VL53LX_Error VL53LX_init_ssc_config_struct( 35 | VL53LX_ssc_config_t *pdata); 36 | 37 | 38 | 39 | 40 | VL53LX_Error VL53LX_init_xtalk_config_struct( 41 | VL53LX_customer_nvm_managed_t *pnvm, 42 | VL53LX_xtalk_config_t *pdata); 43 | 44 | 45 | 46 | VL53LX_Error VL53LX_init_xtalk_extract_config_struct( 47 | VL53LX_xtalkextract_config_t *pdata); 48 | 49 | 50 | 51 | VL53LX_Error VL53LX_init_offset_cal_config_struct( 52 | VL53LX_offsetcal_config_t *pdata); 53 | 54 | 55 | 56 | VL53LX_Error VL53LX_init_zone_cal_config_struct( 57 | VL53LX_zonecal_config_t *pdata); 58 | 59 | 60 | 61 | VL53LX_Error VL53LX_init_hist_post_process_config_struct( 62 | uint8_t xtalk_compensation_enable, 63 | VL53LX_hist_post_process_config_t *pdata); 64 | 65 | 66 | 67 | 68 | VL53LX_Error VL53LX_init_dmax_calibration_data_struct( 69 | VL53LX_dmax_calibration_data_t *pdata); 70 | 71 | 72 | 73 | 74 | VL53LX_Error VL53LX_init_tuning_parm_storage_struct( 75 | VL53LX_tuning_parm_storage_t *pdata); 76 | 77 | 78 | 79 | VL53LX_Error VL53LX_init_hist_gen3_dmax_config_struct( 80 | VL53LX_hist_gen3_dmax_config_t *pdata); 81 | 82 | 83 | 84 | 85 | VL53LX_Error VL53LX_preset_mode_standard_ranging( 86 | VL53LX_static_config_t *pstatic, 87 | VL53LX_histogram_config_t *phistogram, 88 | VL53LX_general_config_t *pgeneral, 89 | VL53LX_timing_config_t *ptiming, 90 | VL53LX_dynamic_config_t *pdynamic, 91 | VL53LX_system_control_t *psystem, 92 | VL53LX_tuning_parm_storage_t *ptuning_parms, 93 | VL53LX_zone_config_t *pzone_cfg); 94 | 95 | 96 | 97 | 98 | VL53LX_Error VL53LX_preset_mode_histogram_ranging( 99 | VL53LX_hist_post_process_config_t *phistpostprocess, 100 | VL53LX_static_config_t *pstatic, 101 | VL53LX_histogram_config_t *phistogram, 102 | VL53LX_general_config_t *pgeneral, 103 | VL53LX_timing_config_t *ptiming, 104 | VL53LX_dynamic_config_t *pdynamic, 105 | VL53LX_system_control_t *psystem, 106 | VL53LX_tuning_parm_storage_t *ptuning_parms, 107 | VL53LX_zone_config_t *pzone_cfg); 108 | 109 | 110 | 111 | 112 | VL53LX_Error VL53LX_preset_mode_histogram_long_range( 113 | VL53LX_hist_post_process_config_t *phistpostprocess, 114 | VL53LX_static_config_t *pstatic, 115 | VL53LX_histogram_config_t *phistogram, 116 | VL53LX_general_config_t *pgeneral, 117 | VL53LX_timing_config_t *ptiming, 118 | VL53LX_dynamic_config_t *pdynamic, 119 | VL53LX_system_control_t *psystem, 120 | VL53LX_tuning_parm_storage_t *ptuning_parms, 121 | VL53LX_zone_config_t *pzone_cfg); 122 | 123 | 124 | 125 | 126 | VL53LX_Error VL53LX_preset_mode_histogram_medium_range( 127 | VL53LX_hist_post_process_config_t *phistpostprocess, 128 | VL53LX_static_config_t *pstatic, 129 | VL53LX_histogram_config_t *phistogram, 130 | VL53LX_general_config_t *pgeneral, 131 | VL53LX_timing_config_t *ptiming, 132 | VL53LX_dynamic_config_t *pdynamic, 133 | VL53LX_system_control_t *psystem, 134 | VL53LX_tuning_parm_storage_t *ptuning_parms, 135 | VL53LX_zone_config_t *pzone_cfg); 136 | 137 | 138 | 139 | 140 | VL53LX_Error VL53LX_preset_mode_histogram_short_range( 141 | VL53LX_hist_post_process_config_t *phistpostprocess, 142 | VL53LX_static_config_t *pstatic, 143 | VL53LX_histogram_config_t *phistogram, 144 | VL53LX_general_config_t *pgeneral, 145 | VL53LX_timing_config_t *ptiming, 146 | VL53LX_dynamic_config_t *pdynamic, 147 | VL53LX_system_control_t *psystem, 148 | VL53LX_tuning_parm_storage_t *ptuning_parms, 149 | VL53LX_zone_config_t *pzone_cfg); 150 | 151 | 152 | 153 | 154 | void VL53LX_copy_hist_cfg_to_static_cfg( 155 | VL53LX_histogram_config_t *phistogram, 156 | VL53LX_static_config_t *pstatic, 157 | VL53LX_general_config_t *pgeneral, 158 | VL53LX_timing_config_t *ptiming, 159 | VL53LX_dynamic_config_t *pdynamic); 160 | 161 | 162 | 163 | void VL53LX_copy_hist_bins_to_static_cfg( 164 | VL53LX_histogram_config_t *phistogram, 165 | VL53LX_static_config_t *pstatic, 166 | VL53LX_timing_config_t *ptiming); 167 | 168 | #ifdef __cplusplus 169 | } 170 | #endif 171 | 172 | #endif 173 | 174 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | BasedOnStyle: Google 4 | AccessModifierOffset: -1 5 | AlignAfterOpenBracket: Align 6 | AlignConsecutiveMacros: true 7 | AlignConsecutiveAssignments: true 8 | AlignConsecutiveDeclarations: false 9 | AlignEscapedNewlines: Left 10 | AlignOperands: true 11 | AlignTrailingComments: true 12 | AllowAllArgumentsOnNextLine: true 13 | AllowAllConstructorInitializersOnNextLine: true 14 | AllowAllParametersOfDeclarationOnNextLine: true 15 | AllowShortBlocksOnASingleLine: Never 16 | AllowShortCaseLabelsOnASingleLine: false 17 | AllowShortFunctionsOnASingleLine: false 18 | AllowShortLambdasOnASingleLine: All 19 | AllowShortIfStatementsOnASingleLine: WithoutElse 20 | AllowShortLoopsOnASingleLine: true 21 | AlwaysBreakAfterDefinitionReturnType: None 22 | AlwaysBreakAfterReturnType: None 23 | AlwaysBreakBeforeMultilineStrings: true 24 | AlwaysBreakTemplateDeclarations: Yes 25 | BinPackArguments: true 26 | BinPackParameters: true 27 | BraceWrapping: 28 | AfterCaseLabel: false 29 | AfterClass: false 30 | AfterControlStatement: false 31 | AfterEnum: false 32 | AfterFunction: false 33 | AfterNamespace: false 34 | AfterObjCDeclaration: false 35 | AfterStruct: false 36 | AfterUnion: false 37 | AfterExternBlock: false 38 | BeforeCatch: false 39 | BeforeElse: false 40 | IndentBraces: false 41 | SplitEmptyFunction: true 42 | SplitEmptyRecord: true 43 | SplitEmptyNamespace: true 44 | BreakBeforeBinaryOperators: None 45 | BreakBeforeBraces: Attach 46 | BreakBeforeInheritanceComma: false 47 | BreakInheritanceList: BeforeColon 48 | BreakBeforeTernaryOperators: true 49 | BreakConstructorInitializersBeforeComma: false 50 | BreakConstructorInitializers: BeforeColon 51 | BreakAfterJavaFieldAnnotations: false 52 | BreakStringLiterals: true 53 | ColumnLimit: 120 54 | CommentPragmas: '^ IWYU pragma:' 55 | CompactNamespaces: false 56 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 57 | ConstructorInitializerIndentWidth: 4 58 | ContinuationIndentWidth: 4 59 | Cpp11BracedListStyle: true 60 | DeriveLineEnding: true 61 | DerivePointerAlignment: true 62 | DisableFormat: false 63 | ExperimentalAutoDetectBinPacking: false 64 | FixNamespaceComments: true 65 | ForEachMacros: 66 | - foreach 67 | - Q_FOREACH 68 | - BOOST_FOREACH 69 | IncludeBlocks: Regroup 70 | IncludeCategories: 71 | - Regex: '^' 72 | Priority: 2 73 | SortPriority: 0 74 | - Regex: '^<.*\.h>' 75 | Priority: 1 76 | SortPriority: 0 77 | - Regex: '^<.*' 78 | Priority: 2 79 | SortPriority: 0 80 | - Regex: '.*' 81 | Priority: 3 82 | SortPriority: 0 83 | IncludeIsMainRegex: '([-_](test|unittest))?$' 84 | IncludeIsMainSourceRegex: '' 85 | IndentCaseLabels: true 86 | IndentGotoLabels: true 87 | IndentPPDirectives: None 88 | IndentWidth: 4 89 | IndentWrappedFunctionNames: false 90 | JavaScriptQuotes: Leave 91 | JavaScriptWrapImports: true 92 | KeepEmptyLinesAtTheStartOfBlocks: false 93 | MacroBlockBegin: '' 94 | MacroBlockEnd: '' 95 | MaxEmptyLinesToKeep: 1 96 | NamespaceIndentation: None 97 | ObjCBinPackProtocolList: Never 98 | ObjCBlockIndentWidth: 2 99 | ObjCSpaceAfterProperty: false 100 | ObjCSpaceBeforeProtocolList: true 101 | PenaltyBreakAssignment: 2 102 | PenaltyBreakBeforeFirstCallParameter: 1 103 | PenaltyBreakComment: 300 104 | PenaltyBreakFirstLessLess: 120 105 | PenaltyBreakString: 1000 106 | PenaltyBreakTemplateDeclaration: 10 107 | PenaltyExcessCharacter: 1000000 108 | PenaltyReturnTypeOnItsOwnLine: 200 109 | PointerAlignment: Left 110 | RawStringFormats: 111 | - Language: Cpp 112 | Delimiters: 113 | - cc 114 | - CC 115 | - cpp 116 | - Cpp 117 | - CPP 118 | - 'c++' 119 | - 'C++' 120 | CanonicalDelimiter: '' 121 | BasedOnStyle: google 122 | - Language: TextProto 123 | Delimiters: 124 | - pb 125 | - PB 126 | - proto 127 | - PROTO 128 | EnclosingFunctions: 129 | - EqualsProto 130 | - EquivToProto 131 | - PARSE_PARTIAL_TEXT_PROTO 132 | - PARSE_TEST_PROTO 133 | - PARSE_TEXT_PROTO 134 | - ParseTextOrDie 135 | - ParseTextProtoOrDie 136 | CanonicalDelimiter: '' 137 | BasedOnStyle: google 138 | ReflowComments: true 139 | SortIncludes: false 140 | SortUsingDeclarations: true 141 | SpaceAfterCStyleCast: false 142 | SpaceAfterLogicalNot: false 143 | SpaceAfterTemplateKeyword: true 144 | SpaceBeforeAssignmentOperators: true 145 | SpaceBeforeCpp11BracedList: false 146 | SpaceBeforeCtorInitializerColon: true 147 | SpaceBeforeInheritanceColon: true 148 | SpaceBeforeParens: ControlStatements 149 | SpaceBeforeRangeBasedForLoopColon: true 150 | SpaceInEmptyBlock: false 151 | SpaceInEmptyParentheses: false 152 | SpacesBeforeTrailingComments: 2 153 | SpacesInAngles: false 154 | SpacesInConditionalStatement: false 155 | SpacesInContainerLiterals: true 156 | SpacesInCStyleCastParentheses: false 157 | SpacesInParentheses: false 158 | SpacesInSquareBrackets: false 159 | SpaceBeforeSquareBrackets: false 160 | Standard: Auto 161 | StatementMacros: 162 | - Q_UNUSED 163 | - QT_REQUIRE_VERSION 164 | TabWidth: 4 165 | UseCRLF: false 166 | UseTab: Never 167 | ... 168 | -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_sigma_estimate.c: -------------------------------------------------------------------------------- 1 | 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | /****************************************************************************** 4 | * Copyright (c) 2020, STMicroelectronics - All Rights Reserved 5 | 6 | This file is part of VL53LX Protected and is dual licensed, 7 | either 'STMicroelectronics Proprietary license' 8 | or 'BSD 3-clause "New" or "Revised" License' , at your option. 9 | 10 | ****************************************************************************** 11 | 12 | 'STMicroelectronics Proprietary license' 13 | 14 | ****************************************************************************** 15 | 16 | License terms: STMicroelectronics Proprietary in accordance with licensing 17 | terms at www.st.com/sla0081 18 | 19 | ****************************************************************************** 20 | */ 21 | 22 | 23 | 24 | 25 | #include 26 | #include 27 | #include "vl53lx_core_support.h" 28 | #include "vl53lx_error_codes.h" 29 | 30 | #include "vl53lx_sigma_estimate.h" 31 | 32 | 33 | #define LOG_FUNCTION_START(fmt, ...) \ 34 | _LOG_FUNCTION_START(VL53LX_TRACE_MODULE_PROTECTED, fmt, ##__VA_ARGS__) 35 | #define LOG_FUNCTION_END(status, ...) \ 36 | _LOG_FUNCTION_END(VL53LX_TRACE_MODULE_PROTECTED, status, ##__VA_ARGS__) 37 | #define LOG_FUNCTION_END_FMT(status, fmt, ...) \ 38 | _LOG_FUNCTION_END_FMT(VL53LX_TRACE_MODULE_PROTECTED, \ 39 | status, fmt, ##__VA_ARGS__) 40 | 41 | #define trace_print(level, ...) \ 42 | _LOG_TRACE_PRINT(VL53LX_TRACE_MODULE_PROTECTED, \ 43 | level, VL53LX_TRACE_FUNCTION_NONE, ##__VA_ARGS__) 44 | 45 | 46 | 47 | VL53LX_Error VL53LX_f_023( 48 | uint8_t sigma_estimator__sigma_ref_mm, 49 | uint32_t VL53LX_p_007, 50 | uint32_t VL53LX_p_032, 51 | uint32_t VL53LX_p_001, 52 | uint32_t a_zp, 53 | uint32_t c_zp, 54 | uint32_t bx, 55 | uint32_t ax_zp, 56 | uint32_t cx_zp, 57 | uint32_t VL53LX_p_028, 58 | uint16_t fast_osc_frequency, 59 | uint16_t *psigma_est) 60 | { 61 | 62 | 63 | VL53LX_Error status = VL53LX_ERROR_DIVISION_BY_ZERO; 64 | uint32_t sigma_int = VL53LX_D_002; 65 | 66 | uint32_t pll_period_mm = 0; 67 | 68 | uint64_t tmp0 = 0; 69 | uint64_t tmp1 = 0; 70 | uint64_t b_minus_amb = 0; 71 | uint64_t VL53LX_p_055 = 0; 72 | 73 | *psigma_est = VL53LX_D_002; 74 | 75 | 76 | 77 | if (fast_osc_frequency != 0) { 78 | 79 | 80 | 81 | pll_period_mm = VL53LX_calc_pll_period_mm(fast_osc_frequency); 82 | 83 | 84 | 85 | if (VL53LX_p_028 > VL53LX_p_032) 86 | b_minus_amb = (uint64_t)VL53LX_p_028 - 87 | (uint64_t)VL53LX_p_032; 88 | else 89 | b_minus_amb = (uint64_t)VL53LX_p_032 - 90 | (uint64_t)VL53LX_p_028; 91 | 92 | 93 | 94 | if (VL53LX_p_007 > VL53LX_p_001) 95 | VL53LX_p_055 = (uint64_t)VL53LX_p_007 - 96 | (uint64_t)VL53LX_p_001; 97 | else 98 | VL53LX_p_055 = (uint64_t)VL53LX_p_001 - 99 | (uint64_t)VL53LX_p_007; 100 | 101 | 102 | 103 | if (b_minus_amb != 0) { 104 | 105 | 106 | 107 | 108 | tmp0 = (uint64_t)VL53LX_p_032 + (uint64_t)bx + 109 | (uint64_t)VL53LX_p_028; 110 | if (tmp0 > VL53LX_D_003) 111 | tmp0 = VL53LX_D_003; 112 | 113 | 114 | 115 | tmp1 = (uint64_t)VL53LX_p_055 * (uint64_t)VL53LX_p_055; 116 | tmp1 = tmp1 << 8; 117 | 118 | 119 | if (tmp1 > VL53LX_D_004) 120 | tmp1 = VL53LX_D_004; 121 | 122 | 123 | tmp1 = do_division_u(tmp1, b_minus_amb); 124 | tmp1 = do_division_u(tmp1, b_minus_amb); 125 | 126 | 127 | if (tmp1 > (uint64_t)VL53LX_D_005) 128 | tmp1 = (uint64_t)VL53LX_D_005; 129 | 130 | 131 | tmp0 = tmp1 * tmp0; 132 | 133 | 134 | tmp1 = (uint64_t)c_zp + (uint64_t)cx_zp + 135 | (uint64_t)a_zp + (uint64_t)ax_zp; 136 | 137 | 138 | if (tmp1 > (uint64_t)VL53LX_D_003) 139 | tmp1 = (uint64_t)VL53LX_D_003; 140 | 141 | tmp1 = tmp1 << 8; 142 | 143 | 144 | tmp0 = tmp1 + tmp0; 145 | if (tmp0 > (uint64_t)VL53LX_D_006) 146 | tmp0 = (uint64_t)VL53LX_D_006; 147 | 148 | 149 | 150 | 151 | 152 | 153 | if (tmp0 > (uint64_t)VL53LX_D_007) { 154 | tmp0 = do_division_u(tmp0, b_minus_amb); 155 | tmp0 = tmp0 * pll_period_mm; 156 | } else { 157 | tmp0 = tmp0 * pll_period_mm; 158 | tmp0 = do_division_u(tmp0, b_minus_amb); 159 | } 160 | 161 | 162 | if (tmp0 > (uint64_t)VL53LX_D_006) 163 | tmp0 = (uint64_t)VL53LX_D_006; 164 | 165 | 166 | 167 | if (tmp0 > (uint64_t)VL53LX_D_007) { 168 | tmp0 = do_division_u(tmp0, b_minus_amb); 169 | tmp0 = do_division_u(tmp0, 4); 170 | tmp0 = tmp0 * pll_period_mm; 171 | } else { 172 | tmp0 = tmp0 * pll_period_mm; 173 | tmp0 = do_division_u(tmp0, b_minus_amb); 174 | tmp0 = do_division_u(tmp0, 4); 175 | } 176 | 177 | 178 | if (tmp0 > (uint64_t)VL53LX_D_006) 179 | tmp0 = (uint64_t)VL53LX_D_006; 180 | 181 | 182 | tmp0 = tmp0 >> 2; 183 | 184 | 185 | if (tmp0 > (uint64_t)VL53LX_D_007) 186 | tmp0 = (uint64_t)VL53LX_D_007; 187 | 188 | 189 | tmp1 = (uint64_t)sigma_estimator__sigma_ref_mm << 7; 190 | tmp1 = tmp1 * tmp1; 191 | tmp0 = tmp0 + tmp1; 192 | 193 | 194 | if (tmp0 > (uint64_t)VL53LX_D_007) 195 | tmp0 = (uint64_t)VL53LX_D_007; 196 | 197 | 198 | sigma_int = VL53LX_isqrt((uint32_t)tmp0); 199 | 200 | *psigma_est = (uint16_t)sigma_int; 201 | 202 | status = VL53LX_ERROR_NONE; 203 | } 204 | 205 | } 206 | 207 | return status; 208 | } 209 | 210 | 211 | 212 | 213 | -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_hist_private_structs.h: -------------------------------------------------------------------------------- 1 | 2 | /* SPDX-License-Identifier: BSD-3-Clause */ 3 | /****************************************************************************** 4 | * Copyright (c) 2020, STMicroelectronics - All Rights Reserved 5 | 6 | This file is part of VL53LX Protected and is dual licensed, 7 | either 'STMicroelectronics Proprietary license' 8 | or 'BSD 3-clause "New" or "Revised" License' , at your option. 9 | 10 | ****************************************************************************** 11 | 12 | 'STMicroelectronics Proprietary license' 13 | 14 | ****************************************************************************** 15 | 16 | License terms: STMicroelectronics Proprietary in accordance with licensing 17 | terms at www.st.com/sla0081 18 | 19 | ****************************************************************************** 20 | */ 21 | 22 | 23 | 24 | 25 | 26 | #ifndef _VL53LX_HIST_PRIVATE_STRUCTS_H_ 27 | #define _VL53LX_HIST_PRIVATE_STRUCTS_H_ 28 | 29 | #include "vl53lx_types.h" 30 | #include "vl53lx_hist_structs.h" 31 | 32 | #define VL53LX_D_001 8 33 | 34 | #ifdef __cplusplus 35 | extern "C" { 36 | #endif 37 | 38 | 39 | 40 | 41 | typedef struct { 42 | 43 | uint8_t VL53LX_p_019; 44 | 45 | uint8_t VL53LX_p_020; 46 | 47 | uint8_t VL53LX_p_021; 48 | 49 | uint8_t VL53LX_p_029; 50 | 51 | int32_t VL53LX_p_016; 52 | 53 | 54 | int32_t VL53LX_p_043[VL53LX_HISTOGRAM_BUFFER_SIZE]; 55 | int32_t VL53LX_p_068[VL53LX_HISTOGRAM_BUFFER_SIZE]; 56 | 57 | uint8_t VL53LX_p_040[VL53LX_HISTOGRAM_BUFFER_SIZE]; 58 | 59 | int32_t VL53LX_p_018[VL53LX_HISTOGRAM_BUFFER_SIZE]; 60 | uint16_t VL53LX_p_014[VL53LX_HISTOGRAM_BUFFER_SIZE]; 61 | uint16_t VL53LX_p_008[VL53LX_HISTOGRAM_BUFFER_SIZE]; 62 | 63 | } VL53LX_hist_gen1_algo_private_data_t; 64 | 65 | 66 | 67 | 68 | typedef struct { 69 | 70 | uint8_t VL53LX_p_019; 71 | 72 | uint8_t VL53LX_p_020; 73 | 74 | uint8_t VL53LX_p_021; 75 | 76 | uint16_t VL53LX_p_015; 77 | 78 | uint8_t VL53LX_p_005; 79 | 80 | uint8_t VL53LX_p_029; 81 | 82 | int32_t VL53LX_p_028; 83 | 84 | int32_t VL53LX_p_016; 85 | 86 | 87 | int32_t VL53LX_p_007[VL53LX_HISTOGRAM_BUFFER_SIZE]; 88 | 89 | int32_t VL53LX_p_032[VL53LX_HISTOGRAM_BUFFER_SIZE]; 90 | 91 | int32_t VL53LX_p_001[VL53LX_HISTOGRAM_BUFFER_SIZE]; 92 | 93 | 94 | int32_t VL53LX_p_018[VL53LX_HISTOGRAM_BUFFER_SIZE]; 95 | 96 | int32_t VL53LX_p_055[VL53LX_HISTOGRAM_BUFFER_SIZE]; 97 | 98 | int32_t VL53LX_p_053[VL53LX_HISTOGRAM_BUFFER_SIZE]; 99 | 100 | int32_t VL53LX_p_054[VL53LX_HISTOGRAM_BUFFER_SIZE]; 101 | 102 | 103 | } VL53LX_hist_gen2_algo_filtered_data_t; 104 | 105 | 106 | 107 | 108 | typedef struct { 109 | 110 | uint8_t VL53LX_p_019; 111 | 112 | uint8_t VL53LX_p_020; 113 | 114 | uint8_t VL53LX_p_021; 115 | 116 | int32_t VL53LX_p_031; 117 | 118 | 119 | uint8_t VL53LX_p_069[VL53LX_HISTOGRAM_BUFFER_SIZE]; 120 | 121 | uint8_t VL53LX_p_070[VL53LX_HISTOGRAM_BUFFER_SIZE]; 122 | 123 | 124 | uint32_t VL53LX_p_014[VL53LX_HISTOGRAM_BUFFER_SIZE]; 125 | 126 | uint16_t VL53LX_p_008[VL53LX_HISTOGRAM_BUFFER_SIZE]; 127 | 128 | 129 | uint8_t VL53LX_p_040[VL53LX_HISTOGRAM_BUFFER_SIZE]; 130 | 131 | 132 | } VL53LX_hist_gen2_algo_detection_data_t; 133 | 134 | 135 | 136 | 137 | typedef struct { 138 | 139 | uint8_t VL53LX_p_012; 140 | 141 | uint8_t VL53LX_p_019; 142 | 143 | uint8_t VL53LX_p_023; 144 | 145 | uint8_t VL53LX_p_024; 146 | 147 | uint8_t VL53LX_p_013; 148 | 149 | 150 | uint8_t VL53LX_p_025; 151 | 152 | uint8_t VL53LX_p_051; 153 | 154 | 155 | int32_t VL53LX_p_016; 156 | 157 | int32_t VL53LX_p_017; 158 | 159 | int32_t VL53LX_p_010; 160 | 161 | 162 | uint32_t VL53LX_p_026; 163 | 164 | uint32_t VL53LX_p_011; 165 | 166 | uint32_t VL53LX_p_027; 167 | 168 | 169 | uint16_t VL53LX_p_002; 170 | 171 | 172 | } VL53LX_hist_pulse_data_t; 173 | 174 | 175 | 176 | 177 | typedef struct { 178 | 179 | uint8_t VL53LX_p_019; 180 | 181 | uint8_t VL53LX_p_020; 182 | 183 | uint8_t VL53LX_p_021; 184 | 185 | uint8_t VL53LX_p_030; 186 | 187 | uint8_t VL53LX_p_039; 188 | 189 | int32_t VL53LX_p_028; 190 | 191 | int32_t VL53LX_p_031; 192 | 193 | 194 | uint8_t VL53LX_p_040[VL53LX_HISTOGRAM_BUFFER_SIZE]; 195 | 196 | uint8_t VL53LX_p_041[VL53LX_HISTOGRAM_BUFFER_SIZE]; 197 | 198 | uint8_t VL53LX_p_042[VL53LX_HISTOGRAM_BUFFER_SIZE]; 199 | 200 | 201 | int32_t VL53LX_p_052[VL53LX_HISTOGRAM_BUFFER_SIZE]; 202 | 203 | int32_t VL53LX_p_043[VL53LX_HISTOGRAM_BUFFER_SIZE]; 204 | 205 | int32_t VL53LX_p_018[VL53LX_HISTOGRAM_BUFFER_SIZE]; 206 | 207 | 208 | uint8_t VL53LX_p_044; 209 | 210 | uint8_t VL53LX_p_045; 211 | 212 | uint8_t VL53LX_p_046; 213 | 214 | 215 | VL53LX_hist_pulse_data_t VL53LX_p_003[VL53LX_D_001]; 216 | 217 | 218 | 219 | 220 | VL53LX_histogram_bin_data_t VL53LX_p_006; 221 | 222 | VL53LX_histogram_bin_data_t VL53LX_p_047; 223 | 224 | VL53LX_histogram_bin_data_t VL53LX_p_048; 225 | 226 | VL53LX_histogram_bin_data_t VL53LX_p_049; 227 | 228 | VL53LX_histogram_bin_data_t VL53LX_p_050; 229 | 230 | 231 | 232 | 233 | } VL53LX_hist_gen3_algo_private_data_t; 234 | 235 | 236 | 237 | 238 | typedef struct { 239 | 240 | uint8_t VL53LX_p_019; 241 | 242 | uint8_t VL53LX_p_020; 243 | 244 | uint8_t VL53LX_p_021; 245 | 246 | 247 | int32_t VL53LX_p_007[VL53LX_HISTOGRAM_BUFFER_SIZE]; 248 | 249 | int32_t VL53LX_p_032[VL53LX_HISTOGRAM_BUFFER_SIZE]; 250 | 251 | int32_t VL53LX_p_001[VL53LX_HISTOGRAM_BUFFER_SIZE]; 252 | 253 | 254 | int32_t VL53LX_p_053[VL53LX_HISTOGRAM_BUFFER_SIZE]; 255 | 256 | int32_t VL53LX_p_054[VL53LX_HISTOGRAM_BUFFER_SIZE]; 257 | 258 | 259 | uint8_t VL53LX_p_040[VL53LX_HISTOGRAM_BUFFER_SIZE]; 260 | 261 | 262 | } VL53LX_hist_gen4_algo_filtered_data_t; 263 | 264 | #ifdef __cplusplus 265 | } 266 | #endif 267 | 268 | #endif 269 | 270 | -------------------------------------------------------------------------------- /lib/bmi270/OIS_README.md: -------------------------------------------------------------------------------- 1 | # Sensor API for the BMI2's OIS interface 2 | 3 | ## Table of Contents 4 | - [Introduction](#Intro) 5 | - [Integration details](#Integration) 6 | - [Driver files information](#file) 7 | - [Sensor interfaces](#interface) 8 | - [Integration Examples](#examples) 9 | 10 | ### Introduction 11 | This package contains Bosch Sensortec's BMI2 Sensor API. 12 | 13 | ### Integration details 14 | - Integrate _bmi2.c_, _bmi2.h_, _bmi2_ois.c_, _bmi2_ois.h_, _bmi2_defs.h_ and the required variant files in your project. 15 | - User has to include _bmi2_ois.h_ in the code to call OIS related APIs and a _variant header_ for initialization as 16 | well as BMI2 related API calls, as shown below: 17 | ``` c 18 | #include "bmi261.h" 19 | #include "bmi2_ois.h" 20 | ```` 21 | ### Driver files information 22 | - *_bmi2_ois.c_* 23 | * This file has function definitions of OIS related API interfaces. 24 | - *_bmi2_ois.h_* 25 | * This header file has necessary include files, function declarations, required to make OIS related API calls. 26 | 27 | ### Sensor interfaces 28 | #### _Host Interface_ 29 | - I2C interface 30 | - SPI interface 31 | _Note: By default, the interface is I2C._ 32 | 33 | #### _OIS Interface_ 34 | - SPI interface 35 | 36 | ### Integration examples 37 | #### Configuring SPI/I2C for host interface. 38 | To configure host interface, an instance of the bmi2_dev structure should be 39 | created for initializing BMI2 sensor. "_Refer **README** for initializing BMI2 40 | sensor._" 41 | 42 | #### Configuring SPI for OIS interface. 43 | To configure OIS interface, an instance of the bmi2_ois_dev structure should be 44 | created. The following parameters are required to be updated in the structure, 45 | by the user. 46 | 47 | Parameters | Details 48 | --------------|----------------------------------- 49 | _intf_ptr_ | device address reference of SPI interface 50 | _ois_read_ | read through SPI interface 51 | _ois_write_ | read through SPI interface 52 | _ois_delay_us_| delay in micro seconds 53 | _acc_en_ | for enabling accelerometer 54 | _gyr_en_ | for enabling gyroscope 55 | 56 | ``` c 57 | int8_t rslt = 0; 58 | 59 | struct bmi2_ois_dev ois_dev = { 60 | .intf_ptr = intf_ptr will contain the chip selection info of SPI CS pin, 61 | .ois_read = user_spi_reg_read, 62 | .ois_write = user_spi_reg_write, 63 | .ois_delay_us = user_delay_us 64 | }; 65 | ``` 66 | >**_Important Note_**: For initializing and configuring BMI2 sensors, which is 67 | done through host interface, the API's are to be used from bmi2.c file. Rest 68 | of the API's, for OIS configurations and the reading of OIS data, which is done 69 | through OIS interface, are to be used from bmi2_ois.c file. 70 | 71 | ##### Get accelerometer and gyroscope data through OIS interface 72 | ``` c 73 | int8_t rslt; 74 | /* Array to enable sensor through host interface */ 75 | uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO}; 76 | /* Array to enable sensor through OIS interface */ 77 | uint8_t sens_sel[2] = {BMI2_OIS_ACCEL, BMI2_OIS_GYRO}; 78 | /* Initialize the configuration structure */ 79 | struct bmi2_sens_config sens_cfg = {0}; 80 | 81 | /* Initialize BMI2 */ 82 | rslt = bmi2_init(&dev); 83 | if (rslt != BMI2_OK) { 84 | printf("Error: %d\n", rslt); 85 | return; 86 | } 87 | 88 | /* Enable accelerometer and gyroscope through host interface */ 89 | rslt = bmi2_sensor_enable(sens_list, 2, &dev); 90 | if (rslt != BMI2_OK) { 91 | printf("Error: %d\n", rslt); 92 | return; 93 | } 94 | 95 | /* Setting of OIS Range is done through host interface */ 96 | /* Select the gyroscope sensor for OIS Range configuration */ 97 | sens_cfg.type = BMI2_GYRO; 98 | 99 | /* Get gyroscope configuration */ 100 | rslt = bmi2_get_sensor_config(&sens_cfg, 1, &dev); 101 | if (rslt != BMI2_OK) { 102 | printf("Error: %d\n", rslt); 103 | return; 104 | } 105 | 106 | /* Set the desired OIS Range */ 107 | sens_cfg.cfg.gyr.ois_range = BMI2_GYR_OIS_2000; 108 | 109 | /* Set gyroscope configuration for default values */ 110 | rslt = bmi2_set_sensor_config(&sens_cfg, 1, &dev); 111 | if (rslt != BMI2_OK) { 112 | printf("Error: %d\n", rslt); 113 | return; 114 | } 115 | 116 | /* Enable OIS through host interface */ 117 | rslt = bmi2_set_ois_interface(BMI2_ENABLE, &dev); 118 | if (rslt != BMI2_OK) { 119 | printf("Error: %d\n", rslt); 120 | return; 121 | } 122 | 123 | /* Disable Advance Power Save Mode through host interface */ 124 | rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &dev); 125 | if (rslt != BMI2_OK) { 126 | printf("Error: %d\n", rslt); 127 | return; 128 | } 129 | 130 | /* Get configurations for OIS through OIS interface for default values */ 131 | rslt = bmi2_ois_get_config(&ois_dev); 132 | if (rslt != BMI2_OK) { 133 | printf("Error: %d\n", rslt); 134 | return; 135 | } 136 | 137 | /* Enable accelerometer and gyroscope for reading OIS data */ 138 | ois_dev.acc_en = BMI2_ENABLE; 139 | ois_dev.gyr_en = BMI2_ENABLE; 140 | 141 | /* Set configurations for OIS through OIS interface */ 142 | rslt = bmi2_ois_set_config(&ois_dev); 143 | if (rslt == BMI2_OK) { 144 | /* Get OIS accelerometer and gyroscope data through OIS interface */ 145 | rslt = bmi2_ois_read_data(sens_sel, 2, &ois_dev); 146 | if (rslt == BMI2_OK) { 147 | /* Print accelerometer data */ 148 | printf("OIS Accel x-axis = %d\t", ois_dev.acc_data.x); 149 | printf("OIS Accel y-axis= %d\t", ois_dev.acc_data.y); 150 | printf("OIS Accel z-axis = %d\r\n", ois_dev.acc_data.z); 151 | 152 | /* Print gyroscope data */ 153 | printf("OIS Gyro x-axis = %d\t", ois_dev.gyr_data.x); 154 | printf("OIS Gyro y-axis= %d\t", ois_dev.gyr_data.y); 155 | printf("OIS Gyro z-axis = %d\r\n", ois_dev.gyr_data.z); 156 | } 157 | } 158 | 159 | if (rslt != BMI2_OK) { 160 | printf("Error code: %d\n", rslt); 161 | return; 162 | } 163 | 164 | /* Enable Advance Power Save Mode through host interface */ 165 | rslt = bmi2_set_adv_power_save(BMI2_ENABLE, &dev); 166 | if (rslt != BMI2_OK) { 167 | printf("Error: %d\n", rslt); 168 | return; 169 | } 170 | ``` 171 | 172 | -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_nvm_structs.h: -------------------------------------------------------------------------------- 1 | 2 | /* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */ 3 | /****************************************************************************** 4 | * Copyright (c) 2020, STMicroelectronics - All Rights Reserved 5 | 6 | This file is part of VL53LX and is dual licensed, 7 | either GPL-2.0+ 8 | or 'BSD 3-clause "New" or "Revised" License' , at your option. 9 | ****************************************************************************** 10 | */ 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | #ifndef _VL53LX_NVM_STRUCTS_H_ 19 | #define _VL53LX_NVM_STRUCTS_H_ 20 | 21 | 22 | #ifdef __cplusplus 23 | extern "C" 24 | { 25 | #endif 26 | 27 | #include "vl53lx_platform.h" 28 | #include "vl53lx_ll_def.h" 29 | 30 | 31 | typedef struct { 32 | 33 | uint16_t result__actual_effective_rtn_spads; 34 | uint8_t ref_spad_array__num_requested_ref_spads; 35 | uint8_t ref_spad_array__ref_location; 36 | uint16_t result__peak_signal_count_rate_rtn_mcps; 37 | uint16_t result__ambient_count_rate_rtn_mcps; 38 | uint16_t result__peak_signal_count_rate_ref_mcps; 39 | uint16_t result__ambient_count_rate_ref_mcps; 40 | uint16_t measured_distance_mm; 41 | uint16_t measured_distance_stdev_mm; 42 | } VL53LX_decoded_nvm_fmt_range_data_t; 43 | 44 | 45 | typedef struct { 46 | 47 | char nvm__fmt__fgc[19]; 48 | uint8_t nvm__fmt__test_program_major; 49 | uint8_t nvm__fmt__test_program_minor; 50 | uint8_t nvm__fmt__map_major; 51 | uint8_t nvm__fmt__map_minor; 52 | uint8_t nvm__fmt__year; 53 | uint8_t nvm__fmt__month; 54 | uint8_t nvm__fmt__day; 55 | uint8_t nvm__fmt__module_date_phase; 56 | uint16_t nvm__fmt__time; 57 | uint8_t nvm__fmt__tester_id; 58 | uint8_t nvm__fmt__site_id; 59 | uint8_t nvm__ews__test_program_major; 60 | uint8_t nvm__ews__test_program_minor; 61 | uint8_t nvm__ews__probe_card_major; 62 | uint8_t nvm__ews__probe_card_minor; 63 | uint8_t nvm__ews__tester_id; 64 | char nvm__ews__lot[8]; 65 | uint8_t nvm__ews__wafer; 66 | uint8_t nvm__ews__xcoord; 67 | uint8_t nvm__ews__ycoord; 68 | } VL53LX_decoded_nvm_fmt_info_t; 69 | 70 | 71 | typedef struct { 72 | 73 | uint8_t nvm__ews__test_program_major; 74 | uint8_t nvm__ews__test_program_minor; 75 | uint8_t nvm__ews__probe_card_major; 76 | uint8_t nvm__ews__probe_card_minor; 77 | uint8_t nvm__ews__tester_id; 78 | char nvm__ews__lot[8]; 79 | uint8_t nvm__ews__wafer; 80 | uint8_t nvm__ews__xcoord; 81 | uint8_t nvm__ews__ycoord; 82 | } VL53LX_decoded_nvm_ews_info_t; 83 | 84 | 85 | typedef struct { 86 | uint8_t nvm__identification_model_id; 87 | uint8_t nvm__identification_module_type; 88 | uint8_t nvm__identification_revision_id; 89 | uint16_t nvm__identification_module_id; 90 | uint8_t nvm__i2c_valid; 91 | uint8_t nvm__i2c_device_address_ews; 92 | uint16_t nvm__ews__fast_osc_frequency; 93 | uint8_t nvm__ews__fast_osc_trim_max; 94 | uint8_t nvm__ews__fast_osc_freq_set; 95 | uint16_t nvm__ews__slow_osc_calibration; 96 | uint16_t nvm__fmt__fast_osc_frequency; 97 | uint8_t nvm__fmt__fast_osc_trim_max; 98 | uint8_t nvm__fmt__fast_osc_freq_set; 99 | uint16_t nvm__fmt__slow_osc_calibration; 100 | uint8_t nvm__vhv_config_unlock; 101 | uint8_t nvm__ref_selvddpix; 102 | uint8_t nvm__ref_selvquench; 103 | uint8_t nvm__regavdd1v2_sel; 104 | uint8_t nvm__regdvdd1v2_sel; 105 | uint8_t nvm__vhv_timeout__macrop; 106 | uint8_t nvm__vhv_loop_bound; 107 | uint8_t nvm__vhv_count_threshold; 108 | uint8_t nvm__vhv_offset; 109 | uint8_t nvm__vhv_init_enable; 110 | uint8_t nvm__vhv_init_value; 111 | uint8_t nvm__laser_safety_vcsel_trim_ll; 112 | uint8_t nvm__laser_safety_vcsel_selion_ll; 113 | uint8_t nvm__laser_safety_vcsel_selion_max_ll; 114 | uint8_t nvm__laser_safety_mult_ll; 115 | uint8_t nvm__laser_safety_clip_ll; 116 | uint8_t nvm__laser_safety_vcsel_trim_ld; 117 | uint8_t nvm__laser_safety_vcsel_selion_ld; 118 | uint8_t nvm__laser_safety_vcsel_selion_max_ld; 119 | uint8_t nvm__laser_safety_mult_ld; 120 | uint8_t nvm__laser_safety_clip_ld; 121 | uint8_t nvm__laser_safety_lock_byte; 122 | uint8_t nvm__laser_safety_unlock_byte; 123 | uint8_t nvm__ews__spad_enables_rtn[VL53LX_RTN_SPAD_BUFFER_SIZE]; 124 | uint8_t nvm__ews__spad_enables_ref__loc1[VL53LX_REF_SPAD_BUFFER_SIZE]; 125 | uint8_t nvm__ews__spad_enables_ref__loc2[VL53LX_REF_SPAD_BUFFER_SIZE]; 126 | uint8_t nvm__ews__spad_enables_ref__loc3[VL53LX_REF_SPAD_BUFFER_SIZE]; 127 | uint8_t nvm__fmt__spad_enables_rtn[VL53LX_RTN_SPAD_BUFFER_SIZE]; 128 | uint8_t nvm__fmt__spad_enables_ref__loc1[VL53LX_REF_SPAD_BUFFER_SIZE]; 129 | uint8_t nvm__fmt__spad_enables_ref__loc2[VL53LX_REF_SPAD_BUFFER_SIZE]; 130 | uint8_t nvm__fmt__spad_enables_ref__loc3[VL53LX_REF_SPAD_BUFFER_SIZE]; 131 | uint8_t nvm__fmt__roi_config__mode_roi_centre_spad; 132 | uint8_t nvm__fmt__roi_config__mode_roi_x_size; 133 | uint8_t nvm__fmt__roi_config__mode_roi_y_size; 134 | uint8_t nvm__fmt__ref_spad_apply__num_requested_ref_spad; 135 | uint8_t nvm__fmt__ref_spad_man__ref_location; 136 | uint16_t nvm__fmt__mm_config__inner_offset_mm; 137 | uint16_t nvm__fmt__mm_config__outer_offset_mm; 138 | uint16_t nvm__fmt__algo_part_to_part_range_offset_mm; 139 | uint16_t nvm__fmt__algo__crosstalk_compensation_plane_offset_kcps; 140 | uint16_t nvm__fmt__algo__crosstalk_compensation_x_plane_gradient_kcps; 141 | uint16_t nvm__fmt__algo__crosstalk_compensation_y_plane_gradient_kcps; 142 | uint8_t nvm__fmt__spare__host_config__nvm_config_spare_0; 143 | uint8_t nvm__fmt__spare__host_config__nvm_config_spare_1; 144 | uint8_t nvm__customer_space_programmed; 145 | uint8_t nvm__cust__i2c_device_address; 146 | uint8_t nvm__cust__ref_spad_apply__num_requested_ref_spad; 147 | uint8_t nvm__cust__ref_spad_man__ref_location; 148 | uint16_t nvm__cust__mm_config__inner_offset_mm; 149 | uint16_t nvm__cust__mm_config__outer_offset_mm; 150 | uint16_t nvm__cust__algo_part_to_part_range_offset_mm; 151 | uint16_t nvm__cust__algo__crosstalk_compensation_plane_offset_kcps; 152 | uint16_t nvm__cust__algo__crosstalk_compensation_x_plane_gradient_kcps; 153 | uint16_t nvm__cust__algo__crosstalk_compensation_y_plane_gradient_kcps; 154 | uint8_t nvm__cust__spare__host_config__nvm_config_spare_0; 155 | uint8_t nvm__cust__spare__host_config__nvm_config_spare_1; 156 | VL53LX_optical_centre_t fmt_optical_centre; 157 | VL53LX_cal_peak_rate_map_t fmt_peak_rate_map; 158 | VL53LX_additional_offset_cal_data_t fmt_add_offset_data; 159 | 160 | VL53LX_decoded_nvm_fmt_range_data_t 161 | fmt_range_data[VL53LX_NVM_MAX_FMT_RANGE_DATA]; 162 | 163 | VL53LX_decoded_nvm_fmt_info_t fmt_info; 164 | VL53LX_decoded_nvm_ews_info_t ews_info; 165 | 166 | } VL53LX_decoded_nvm_data_t; 167 | 168 | 169 | 170 | #ifdef __cplusplus 171 | } 172 | #endif 173 | 174 | #endif 175 | 176 | -------------------------------------------------------------------------------- /lib/vl53l3c/vl53lx_hist_structs.h: -------------------------------------------------------------------------------- 1 | 2 | /* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */ 3 | /****************************************************************************** 4 | * Copyright (c) 2020, STMicroelectronics - All Rights Reserved 5 | 6 | This file is part of VL53LX and is dual licensed, 7 | either GPL-2.0+ 8 | or 'BSD 3-clause "New" or "Revised" License' , at your option. 9 | ****************************************************************************** 10 | */ 11 | 12 | 13 | 14 | 15 | 16 | #ifndef _VL53LX_HIST_STRUCTS_H_ 17 | #define _VL53LX_HIST_STRUCTS_H_ 18 | 19 | #include "vl53lx_ll_device.h" 20 | #include "vl53lx_dmax_structs.h" 21 | 22 | #ifdef __cplusplus 23 | extern "C" 24 | { 25 | #endif 26 | 27 | #define VL53LX_MAX_BIN_SEQUENCE_LENGTH 6 28 | #define VL53LX_MAX_BIN_SEQUENCE_CODE 15 29 | #define VL53LX_HISTOGRAM_BUFFER_SIZE 24 30 | #define VL53LX_XTALK_HISTO_BINS 12 31 | 32 | 33 | 34 | typedef struct { 35 | 36 | uint8_t histogram_config__spad_array_selection; 37 | 38 | uint8_t histogram_config__low_amb_even_bin_0_1; 39 | uint8_t histogram_config__low_amb_even_bin_2_3; 40 | uint8_t histogram_config__low_amb_even_bin_4_5; 41 | 42 | uint8_t histogram_config__low_amb_odd_bin_0_1; 43 | uint8_t histogram_config__low_amb_odd_bin_2_3; 44 | uint8_t histogram_config__low_amb_odd_bin_4_5; 45 | 46 | uint8_t histogram_config__mid_amb_even_bin_0_1; 47 | uint8_t histogram_config__mid_amb_even_bin_2_3; 48 | uint8_t histogram_config__mid_amb_even_bin_4_5; 49 | 50 | uint8_t histogram_config__mid_amb_odd_bin_0_1; 51 | uint8_t histogram_config__mid_amb_odd_bin_2; 52 | uint8_t histogram_config__mid_amb_odd_bin_3_4; 53 | uint8_t histogram_config__mid_amb_odd_bin_5; 54 | 55 | uint8_t histogram_config__user_bin_offset; 56 | 57 | uint8_t histogram_config__high_amb_even_bin_0_1; 58 | uint8_t histogram_config__high_amb_even_bin_2_3; 59 | uint8_t histogram_config__high_amb_even_bin_4_5; 60 | 61 | uint8_t histogram_config__high_amb_odd_bin_0_1; 62 | uint8_t histogram_config__high_amb_odd_bin_2_3; 63 | uint8_t histogram_config__high_amb_odd_bin_4_5; 64 | 65 | uint16_t histogram_config__amb_thresh_low; 66 | 67 | uint16_t histogram_config__amb_thresh_high; 68 | 69 | 70 | } VL53LX_histogram_config_t; 71 | 72 | 73 | 74 | 75 | typedef struct { 76 | 77 | VL53LX_HistAlgoSelect hist_algo_select; 78 | 79 | 80 | VL53LX_HistTargetOrder hist_target_order; 81 | 82 | 83 | uint8_t filter_woi0; 84 | 85 | uint8_t filter_woi1; 86 | 87 | 88 | VL53LX_HistAmbEstMethod hist_amb_est_method; 89 | 90 | uint8_t ambient_thresh_sigma0; 91 | 92 | uint8_t ambient_thresh_sigma1; 93 | 94 | 95 | 96 | uint16_t ambient_thresh_events_scaler; 97 | 98 | 99 | 100 | int32_t min_ambient_thresh_events; 101 | 102 | uint16_t noise_threshold; 103 | 104 | 105 | int32_t signal_total_events_limit; 106 | 107 | uint8_t sigma_estimator__sigma_ref_mm; 108 | 109 | uint16_t sigma_thresh; 110 | 111 | int16_t range_offset_mm; 112 | 113 | uint16_t gain_factor; 114 | 115 | 116 | uint8_t valid_phase_low; 117 | 118 | uint8_t valid_phase_high; 119 | 120 | uint8_t algo__consistency_check__phase_tolerance; 121 | 122 | uint8_t algo__consistency_check__event_sigma; 123 | 124 | 125 | 126 | uint16_t algo__consistency_check__event_min_spad_count; 127 | 128 | 129 | 130 | uint16_t algo__consistency_check__min_max_tolerance; 131 | 132 | 133 | uint8_t algo__crosstalk_compensation_enable; 134 | 135 | uint32_t algo__crosstalk_compensation_plane_offset_kcps; 136 | 137 | int16_t algo__crosstalk_compensation_x_plane_gradient_kcps; 138 | 139 | int16_t algo__crosstalk_compensation_y_plane_gradient_kcps; 140 | 141 | 142 | int16_t algo__crosstalk_detect_min_valid_range_mm; 143 | 144 | int16_t algo__crosstalk_detect_max_valid_range_mm; 145 | 146 | uint16_t algo__crosstalk_detect_max_valid_rate_kcps; 147 | 148 | uint16_t algo__crosstalk_detect_max_sigma_mm; 149 | 150 | 151 | 152 | uint8_t algo__crosstalk_detect_event_sigma; 153 | 154 | 155 | 156 | uint16_t algo__crosstalk_detect_min_max_tolerance; 157 | 158 | 159 | } VL53LX_hist_post_process_config_t; 160 | 161 | 162 | 163 | typedef struct { 164 | 165 | 166 | VL53LX_DeviceState cfg_device_state; 167 | 168 | VL53LX_DeviceState rd_device_state; 169 | 170 | 171 | uint8_t zone_id; 172 | 173 | uint32_t time_stamp; 174 | 175 | 176 | uint8_t VL53LX_p_019; 177 | 178 | uint8_t VL53LX_p_020; 179 | 180 | uint8_t VL53LX_p_021; 181 | 182 | uint8_t number_of_ambient_bins; 183 | 184 | uint8_t bin_seq[VL53LX_MAX_BIN_SEQUENCE_LENGTH]; 185 | 186 | uint8_t bin_rep[VL53LX_MAX_BIN_SEQUENCE_LENGTH]; 187 | 188 | int32_t bin_data[VL53LX_HISTOGRAM_BUFFER_SIZE]; 189 | 190 | 191 | uint8_t result__interrupt_status; 192 | 193 | uint8_t result__range_status; 194 | 195 | uint8_t result__report_status; 196 | 197 | uint8_t result__stream_count; 198 | 199 | uint16_t result__dss_actual_effective_spads; 200 | 201 | 202 | uint16_t phasecal_result__reference_phase; 203 | 204 | uint8_t phasecal_result__vcsel_start; 205 | 206 | uint8_t cal_config__vcsel_start; 207 | 208 | uint16_t vcsel_width; 209 | 210 | uint8_t VL53LX_p_005; 211 | 212 | uint16_t VL53LX_p_015; 213 | 214 | uint32_t total_periods_elapsed; 215 | 216 | 217 | uint32_t peak_duration_us; 218 | 219 | uint32_t woi_duration_us; 220 | 221 | 222 | int32_t min_bin_value; 223 | 224 | int32_t max_bin_value; 225 | 226 | 227 | uint16_t zero_distance_phase; 228 | 229 | uint8_t number_of_ambient_samples; 230 | 231 | int32_t ambient_events_sum; 232 | 233 | int32_t VL53LX_p_028; 234 | 235 | 236 | uint8_t roi_config__user_roi_centre_spad; 237 | 238 | uint8_t roi_config__user_roi_requested_global_xy_size; 239 | 240 | 241 | } VL53LX_histogram_bin_data_t; 242 | 243 | 244 | 245 | 246 | typedef struct { 247 | 248 | 249 | uint8_t zone_id; 250 | 251 | uint32_t time_stamp; 252 | 253 | 254 | uint8_t VL53LX_p_019; 255 | 256 | uint8_t VL53LX_p_020; 257 | 258 | uint8_t VL53LX_p_021; 259 | 260 | uint32_t bin_data[VL53LX_XTALK_HISTO_BINS]; 261 | 262 | 263 | 264 | uint16_t phasecal_result__reference_phase; 265 | 266 | uint8_t phasecal_result__vcsel_start; 267 | 268 | uint8_t cal_config__vcsel_start; 269 | 270 | uint16_t vcsel_width; 271 | 272 | uint16_t VL53LX_p_015; 273 | 274 | uint16_t zero_distance_phase; 275 | 276 | 277 | } VL53LX_xtalk_histogram_shape_t; 278 | 279 | 280 | 281 | 282 | typedef struct { 283 | 284 | 285 | VL53LX_xtalk_histogram_shape_t xtalk_shape; 286 | 287 | VL53LX_histogram_bin_data_t xtalk_hist_removed; 288 | 289 | } VL53LX_xtalk_histogram_data_t; 290 | 291 | 292 | 293 | 294 | #ifdef __cplusplus 295 | } 296 | #endif 297 | 298 | #endif 299 | 300 | --------------------------------------------------------------------------------