├── .github └── workflows │ └── platformio.yml ├── .gitignore ├── LICENSE ├── README.md ├── bin ├── bbread.py └── recv.sh ├── docker-compose.yml ├── docker └── Dockerfile ├── docs ├── calculations.ods ├── cli.md ├── connections.md ├── development.md ├── dshot_gcr_dump.txt ├── esp-fc-esp32-i2c.fzz ├── esp-fc-esp32-spi.fzz ├── esp-fc-esp8266-i2c.fzz ├── images │ ├── bfc │ │ ├── bfc_cli_pins.png │ │ ├── bfc_configuration.png │ │ └── bfc_options.png │ ├── esp-fc-esp32_i2c_wiring.png │ ├── esp-fc-esp32_spi_wiring.png │ ├── espfc_flashing.png │ ├── espfc_receiver.png │ ├── espfc_wemos_d1_mini_schematic.png │ ├── espfc_wemos_d1_mini_wiring.png │ ├── espfc_wemos_mini32_wiring.png │ ├── espfc_wifi_ap_connect.png │ ├── espfc_wifi_ap_enable.png │ ├── espfc_wiring_combined.png │ ├── esptool-js-flash-connect.png │ └── esptool-js-flash-flashed.png ├── pyDrone_dump.txt ├── setup.md ├── wireless.md └── wiring.md ├── include └── .gitkeep ├── lib ├── AHRS │ ├── library.json │ └── src │ │ ├── Kalman.cpp │ │ ├── Kalman.h │ │ ├── Madgwick.cpp │ │ ├── Madgwick.h │ │ ├── Mahony.cpp │ │ ├── Mahony.h │ │ └── helper_3dmath.h ├── EscDriver │ ├── library.json │ └── src │ │ ├── EscDriver.h │ │ ├── EscDriverBase.cpp │ │ ├── EscDriverBase.hpp │ │ ├── EscDriverEsp32.cpp │ │ ├── EscDriverEsp32.h │ │ ├── EscDriverEsp32c3.cpp │ │ ├── EscDriverEsp32c3.h │ │ ├── EscDriverEsp8266.cpp │ │ ├── EscDriverEsp8266.h │ │ ├── EscDriverRP2040.cpp │ │ └── EscDriverRP2040.h ├── EspWire │ ├── library.json │ └── src │ │ ├── EspWire.cpp │ │ ├── EspWire.h │ │ ├── esp_twi.cpp │ │ ├── esp_twi.h │ │ ├── keywords.txt │ │ └── library.properties ├── Espfc │ ├── library.json │ └── src │ │ ├── Blackbox │ │ ├── Blackbox.cpp │ │ ├── Blackbox.h │ │ ├── BlackboxBridge.cpp │ │ ├── BlackboxBridge.h │ │ ├── BlackboxFlashfs.cpp │ │ └── BlackboxSerialBuffer.h │ │ ├── Connect │ │ ├── Buzzer.cpp │ │ ├── Buzzer.hpp │ │ ├── Cli.cpp │ │ ├── Cli.hpp │ │ ├── Msp.cpp │ │ ├── Msp.hpp │ │ ├── MspParser.cpp │ │ ├── MspParser.hpp │ │ ├── MspProcessor.cpp │ │ ├── MspProcessor.hpp │ │ ├── StatusLed.cpp │ │ ├── StatusLed.hpp │ │ ├── Vtx.cpp │ │ └── Vtx.hpp │ │ ├── Control │ │ ├── Actuator.cpp │ │ ├── Actuator.h │ │ ├── Controller.cpp │ │ ├── Controller.h │ │ ├── Fusion.cpp │ │ ├── Fusion.h │ │ ├── Pid.cpp │ │ ├── Pid.h │ │ ├── Rates.cpp │ │ └── Rates.h │ │ ├── Debug_Espfc.cpp │ │ ├── Debug_Espfc.h │ │ ├── Device │ │ ├── BaroBMP085.h │ │ ├── BaroBMP280.h │ │ ├── BaroDevice.cpp │ │ ├── BaroDevice.h │ │ ├── BaroSPL06.h │ │ ├── BusAwareDevice.h │ │ ├── BusDevice.cpp │ │ ├── BusDevice.h │ │ ├── BusI2C.cpp │ │ ├── BusI2C.h │ │ ├── BusSPI.cpp │ │ ├── BusSPI.h │ │ ├── BusSlave.cpp │ │ ├── BusSlave.h │ │ ├── FlashDevice.h │ │ ├── GyroBMI160.h │ │ ├── GyroDevice.cpp │ │ ├── GyroDevice.h │ │ ├── GyroICM20602.h │ │ ├── GyroLSM6DSO.h │ │ ├── GyroMPU6050.h │ │ ├── GyroMPU6500.h │ │ ├── GyroMPU9250.h │ │ ├── Input │ │ │ ├── InputButton.cpp │ │ │ └── InputButton.hpp │ │ ├── InputCRSF.cpp │ │ ├── InputCRSF.h │ │ ├── InputDevice.h │ │ ├── InputEspNow.cpp │ │ ├── InputEspNow.h │ │ ├── InputIBUS.cpp │ │ ├── InputIBUS.hpp │ │ ├── InputPPM.cpp │ │ ├── InputPPM.h │ │ ├── InputSBUS.cpp │ │ ├── InputSBUS.h │ │ ├── MagAK8963.h │ │ ├── MagDevice.cpp │ │ ├── MagDevice.h │ │ ├── MagHMC5338L.h │ │ ├── MagQMC5338L.h │ │ ├── SerialDevice.h │ │ └── SerialDeviceAdapter.h │ │ ├── Espfc.cpp │ │ ├── Espfc.h │ │ ├── Hal │ │ ├── Gpio.cpp │ │ ├── Gpio.h │ │ └── Pgm.h │ │ ├── Hardware.cpp │ │ ├── Hardware.h │ │ ├── Input.cpp │ │ ├── Input.h │ │ ├── Model.h │ │ ├── ModelConfig.h │ │ ├── ModelState.h │ │ ├── Output │ │ ├── Mixer.cpp │ │ ├── Mixer.h │ │ ├── Mixers.h │ │ └── OutputIBUS.hpp │ │ ├── Rc │ │ ├── Crsf.cpp │ │ └── Crsf.h │ │ ├── Sensor │ │ ├── AccelSensor.cpp │ │ ├── AccelSensor.h │ │ ├── BaroSensor.cpp │ │ ├── BaroSensor.h │ │ ├── BaseSensor.cpp │ │ ├── BaseSensor.h │ │ ├── GpsSensor.cpp │ │ ├── GpsSensor.hpp │ │ ├── GyroSensor.cpp │ │ ├── GyroSensor.h │ │ ├── MagSensor.cpp │ │ ├── MagSensor.h │ │ ├── VoltageSensor.cpp │ │ └── VoltageSensor.h │ │ ├── SensorManager.cpp │ │ ├── SensorManager.h │ │ ├── SerialManager.cpp │ │ ├── SerialManager.h │ │ ├── Target │ │ ├── Queue.cpp │ │ ├── Queue.h │ │ ├── QueueAtomic.cpp │ │ ├── QueueAtomic.h │ │ ├── QueueFreeRTOS.cpp │ │ ├── QueueRP2040.cpp │ │ ├── Target.h │ │ ├── TargetESP32.h │ │ ├── TargetESP32c3.h │ │ ├── TargetESP32s2.h │ │ ├── TargetESP32s3.h │ │ ├── TargetESP8266.h │ │ ├── TargetEsp32Common.h │ │ ├── TargetRP2040.h │ │ └── TargetUnitTest.h │ │ ├── Telemetry │ │ ├── TelemetryCRSF.h │ │ └── TelemetryText.h │ │ ├── TelemetryManager.cpp │ │ ├── TelemetryManager.h │ │ ├── Utils │ │ ├── Bits.hpp │ │ ├── Crc.cpp │ │ ├── Crc.hpp │ │ ├── FFTAnalyzer.hpp │ │ ├── FFTAnalyzer.ipp │ │ ├── Filter.cpp │ │ ├── Filter.h │ │ ├── FilterHelper.cpp │ │ ├── FilterHelper.h │ │ ├── FreqAnalyzer.cpp │ │ ├── FreqAnalyzer.hpp │ │ ├── Logger.h │ │ ├── Math.hpp │ │ ├── MemoryHelper.h │ │ ├── RingBuf.h │ │ ├── Sma.hpp │ │ ├── Sma.ipp │ │ ├── Stats.cpp │ │ ├── Stats.h │ │ ├── Storage.cpp │ │ ├── Storage.h │ │ ├── Timer.cpp │ │ └── Timer.h │ │ ├── Wireless.cpp │ │ └── Wireless.h ├── Gps │ ├── library.json │ └── src │ │ ├── Gps.hpp │ │ ├── GpsParser.hpp │ │ └── GpsProtocol.hpp ├── MultiButton │ ├── library.json │ └── src │ │ └── MultiButton.h ├── betaflight │ ├── library.json │ └── src │ │ ├── blackbox │ │ ├── blackbox.c │ │ ├── blackbox.h │ │ ├── blackbox_encoding.c │ │ ├── blackbox_encoding.h │ │ ├── blackbox_fielddefs.h │ │ ├── blackbox_io.c │ │ └── blackbox_io.h │ │ ├── build │ │ ├── build_config.h │ │ ├── debug.h │ │ └── version.h │ │ ├── cli │ │ └── settings.h │ │ ├── common │ │ ├── axis.h │ │ ├── encoding.h │ │ ├── maths.h │ │ ├── printf.h │ │ ├── time.h │ │ └── utils.h │ │ ├── config │ │ ├── config.h │ │ ├── feature.h │ │ ├── parameter_group.h │ │ └── parameter_group_ids.h │ │ ├── drivers │ │ ├── buf_writer.h │ │ ├── compass │ │ │ └── compass.h │ │ ├── dshot.h │ │ ├── io.h │ │ ├── io_types.h │ │ ├── light_led.h │ │ ├── sdcard.h │ │ ├── sensor.h │ │ ├── serial.h │ │ ├── time.h │ │ └── timer.h │ │ ├── extract_headers.sh │ │ ├── fc │ │ ├── board_info.h │ │ ├── config.h │ │ ├── controlrate_profile.h │ │ ├── parameter_names.h │ │ ├── rc.h │ │ ├── rc_controls.h │ │ ├── rc_modes.h │ │ └── runtime_config.h │ │ ├── flight │ │ ├── failsafe.h │ │ ├── gps_rescue.h │ │ ├── mixer.h │ │ ├── navigation.h │ │ ├── pid.h │ │ ├── position.h │ │ ├── rpm_filter.h │ │ └── servos.h │ │ ├── io │ │ ├── asyncfatfs │ │ │ └── asyncfatfs.h │ │ ├── beeper.h │ │ ├── flashfs.h │ │ ├── gps.h │ │ ├── serial.h │ │ ├── serial_4way.c │ │ ├── serial_4way.h │ │ ├── serial_4way_avrootloader.c │ │ ├── serial_4way_avrootloader.h │ │ ├── serial_4way_impl.h │ │ ├── serial_4way_stk500v2.c │ │ └── serial_4way_stk500v2.h │ │ ├── msp │ │ ├── msp_protocol.h │ │ ├── msp_protocol_v2_betaflight.h │ │ ├── msp_protocol_v2_common.h │ │ └── msp_serial.h │ │ ├── pg │ │ ├── motor.h │ │ ├── pg.h │ │ ├── pg_ids.h │ │ └── rx.h │ │ ├── platform.c │ │ ├── platform.h │ │ ├── platform_cpp.cpp │ │ ├── rx │ │ └── rx.h │ │ └── sensors │ │ ├── acceleration.h │ │ ├── barometer.h │ │ ├── battery.h │ │ ├── compass.h │ │ ├── gyro.h │ │ ├── rangefinder.h │ │ └── sonar.h ├── printf │ ├── library.json │ └── src │ │ ├── printf.c │ │ └── printf.h └── readme.txt ├── merge_firmware.py ├── partitions_4M_nota.csv ├── platformio.ini ├── src └── main.cpp └── test ├── test_esc └── test_esc.cpp ├── test_fc └── test_fc.cpp ├── test_input_crsf └── test_input_crsf.cpp ├── test_math └── test_math.cpp └── test_msp └── test_msp.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Precompiled Headers 8 | *.gch 9 | *.pch 10 | 11 | # Compiled Dynamic libraries 12 | *.so 13 | *.dylib 14 | *.dll 15 | 16 | # Fortran module files 17 | *.mod 18 | *.smod 19 | 20 | # Compiled Static libraries 21 | *.lai 22 | *.la 23 | *.a 24 | *.lib 25 | 26 | # Executables 27 | *.exe 28 | *.out 29 | *.app 30 | *.elf 31 | .pioenvs 32 | .clang_complete 33 | .gcc-flags.json 34 | 35 | *.BFL 36 | *.log 37 | .directory 38 | /tmp 39 | .vscode/ 40 | .pio/ 41 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2016 rtlopez 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /bin/bbread.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import serial 3 | import signal 4 | from datetime import datetime 5 | 6 | port = "/dev/ttyUSB1" 7 | #speed = 230400 8 | speed = 500000 9 | 10 | now = datetime.now() # current date and time 11 | filename = now.strftime("LOG_%y%m%d_%H%M%S.BFL") 12 | 13 | f = open(filename, "wb") 14 | print("File opened for write:", filename) 15 | 16 | ser = serial.Serial( 17 | port=port, 18 | baudrate=speed, 19 | parity=serial.PARITY_NONE, 20 | stopbits=serial.STOPBITS_ONE, 21 | bytesize=serial.EIGHTBITS, 22 | timeout=0.2 23 | ) 24 | print("Serial opened for read:", port, speed) 25 | 26 | stopProcessing = False 27 | 28 | def handler(signum, frame): 29 | global stopProcessing 30 | stopProcessing = True 31 | print("Exiting") 32 | 33 | signal.signal(signal.SIGINT, handler) 34 | print("Press CTRL^C to stop") 35 | 36 | while True: 37 | if stopProcessing: break 38 | x = ser.read() 39 | f.write(x) 40 | 41 | ser.close() 42 | f.close() 43 | -------------------------------------------------------------------------------- /bin/recv.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash -ex 2 | 3 | PORT="/dev/ttyUSB1" 4 | SPEED="115200" 5 | SPEED="230400" 6 | #SPEED="250000" 7 | #SPEED="500000" 8 | 9 | LOGFILE="LOG_$(date +%y%m%d%H%M).BFL" 10 | 11 | stty -F $PORT $SPEED 12 | cat < $PORT > $LOGFILE 13 | 14 | -------------------------------------------------------------------------------- /docker-compose.yml: -------------------------------------------------------------------------------- 1 | version: '3.5' 2 | 3 | services: 4 | pio: 5 | image: pio 6 | build: 7 | context: docker 8 | hostname: pio 9 | network_mode: "host" 10 | volumes: 11 | - ./:/app 12 | -------------------------------------------------------------------------------- /docker/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ubuntu:22.04 2 | 3 | RUN apt-get update && DEBIAN_FRONTEND=noninteractive apt-get install -y --no-install-recommends \ 4 | python3 python3-pip build-essential mc git curl wget \ 5 | && apt-get clean && rm -rf /var/lib/apt/lists/* \ 6 | && groupadd -g 1000 pio && useradd -m -u 1000 -g 1000 -s /bin/bash pio \ 7 | && mkdir -p /home/pio/.platformio \ 8 | && chown pio:pio /home/pio/.platformio 9 | 10 | RUN pip3 install -U platformio 11 | 12 | USER 1000:1000 13 | 14 | WORKDIR /app 15 | 16 | -------------------------------------------------------------------------------- /docs/calculations.ods: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/docs/calculations.ods -------------------------------------------------------------------------------- /docs/development.md: -------------------------------------------------------------------------------- 1 | # Development 2 | 3 | ## Compile code 4 | 5 | ``` 6 | pio run -e lolin32 7 | ``` 8 | 9 | ## Merge image 10 | 11 | This step is only required to prepare single image to upload through User Interface, like Web UI or Firmware flasher for ESP32 based chips. 12 | ``` 13 | python3 ~/.platformio/packages/tool-esptoolpy/esptool.py --chip ESP32 merge_bin -o .pio/build/lolin32/firmware_full.bin --target-offset 0x1000 --flash_mode dio --flash_freq 80m --flash_size 4MB 0x1000 .pio/build/lolin32/bootloader.bin 0x8000 .pio/build/lolin32/partitions.bin 0xe000 ~/.platformio/packages/framework-arduinoespressif32/tools/partitions/boot_app0.bin 0x10000 .pio/build/lolin32/firmware.bin 14 | ``` 15 | 16 | ## Flash device 17 | 18 | ``` 19 | pio run -e lolin32 -t upload 20 | ``` 21 | 22 | For ESP32 chip it expands to: 23 | ``` 24 | python3 ~/.platformio/packages/tool-esptoolpy/esptool.py --chip esp32 --port "/dev/ttyUSB0" --baud 921600 --before default_reset --after hard_reset write_flash -z --flash_mode dio --flash_freq 80m --flash_size 4MB 0x1000 .pio/build/lolin32/bootloader.bin 0x8000 .pio/build/lolin32/partitions.bin 0xe000 ~/.platformio/packages/framework-arduinoespressif32/tools/partitions/boot_app0.bin 0x10000 .pio/build/lolin32/firmware.bin 25 | ``` 26 | 27 | To flash merged file 28 | 29 | ``` 30 | python3 ~/.platformio/packages/tool-esptoolpy/esptool.py --chip esp32 --port "/dev/ttyUSB0" --baud 921600 --before default_reset --after hard_reset write_flash -z --flash_mode dio --flash_freq 80m --flash_size 4MB 0x1000 .pio/build/lolin32/firmware_full.bin 31 | ``` 32 | 33 | ## Run unit tests 34 | 35 | ``` 36 | pio test -e native 37 | ``` 38 | 39 | ## Docker 40 | 41 | If you don't want to install PlatformIO 42 | 43 | ``` 44 | docker-compose run pio 45 | ``` 46 | 47 | examples 48 | 49 | ``` 50 | docker-compose run pio pio run -e lolin32 51 | docker-compose run pio pio test -e native 52 | ``` 53 | Keep in mind, that to be able to flash device, additional effort is required as by default docker is isolated enviroment devices are't accessible. 54 | 55 | ## VSCode 56 | 57 | This project is based on [platformio](https://platformio.org/), it is recommended to install it as VSCode IDE extension. 58 | 59 | 1. if you don't have VSCode yet? visit https://code.visualstudio.com/download 60 | 2. then install https://platformio.org/install/ide?install=vscode 61 | -------------------------------------------------------------------------------- /docs/esp-fc-esp32-i2c.fzz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/docs/esp-fc-esp32-i2c.fzz -------------------------------------------------------------------------------- /docs/esp-fc-esp32-spi.fzz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/docs/esp-fc-esp32-spi.fzz -------------------------------------------------------------------------------- /docs/esp-fc-esp8266-i2c.fzz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/docs/esp-fc-esp8266-i2c.fzz -------------------------------------------------------------------------------- /docs/images/bfc/bfc_cli_pins.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/docs/images/bfc/bfc_cli_pins.png -------------------------------------------------------------------------------- /docs/images/bfc/bfc_configuration.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/docs/images/bfc/bfc_configuration.png -------------------------------------------------------------------------------- /docs/images/bfc/bfc_options.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/docs/images/bfc/bfc_options.png -------------------------------------------------------------------------------- /docs/images/esp-fc-esp32_i2c_wiring.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/docs/images/esp-fc-esp32_i2c_wiring.png -------------------------------------------------------------------------------- /docs/images/esp-fc-esp32_spi_wiring.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/docs/images/esp-fc-esp32_spi_wiring.png -------------------------------------------------------------------------------- /docs/images/espfc_flashing.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/docs/images/espfc_flashing.png -------------------------------------------------------------------------------- /docs/images/espfc_receiver.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/docs/images/espfc_receiver.png -------------------------------------------------------------------------------- /docs/images/espfc_wemos_d1_mini_schematic.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/docs/images/espfc_wemos_d1_mini_schematic.png -------------------------------------------------------------------------------- /docs/images/espfc_wemos_d1_mini_wiring.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/docs/images/espfc_wemos_d1_mini_wiring.png -------------------------------------------------------------------------------- /docs/images/espfc_wemos_mini32_wiring.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/docs/images/espfc_wemos_mini32_wiring.png -------------------------------------------------------------------------------- /docs/images/espfc_wifi_ap_connect.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/docs/images/espfc_wifi_ap_connect.png -------------------------------------------------------------------------------- /docs/images/espfc_wifi_ap_enable.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/docs/images/espfc_wifi_ap_enable.png -------------------------------------------------------------------------------- /docs/images/espfc_wiring_combined.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/docs/images/espfc_wiring_combined.png -------------------------------------------------------------------------------- /docs/images/esptool-js-flash-connect.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/docs/images/esptool-js-flash-connect.png -------------------------------------------------------------------------------- /docs/images/esptool-js-flash-flashed.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/docs/images/esptool-js-flash-flashed.png -------------------------------------------------------------------------------- /docs/wireless.md: -------------------------------------------------------------------------------- 1 | # ESP-FC Wireless functions 2 | 3 | Espressif modules have a built-in WiFi module that can be used for configuration and control. 4 | 5 | ## WiFi configuration 6 | 7 | > [!NOTE] 8 | > To be able to configure device using WiFi connection, you need to enable `SOFTSERIAL` function. WiFi function can only be used to configure device. Whilst WiFi is active, it is not possible to ARM controller and reboot is required. 9 | 10 | ![Enable WiFi](/docs/images/espfc_wifi_ap_enable.png) 11 | 12 | WiFi will start its own Access-Point if board will not detect receiver signal for at least 30 seconds (stay in failsafe mode since boot). Name of this AP is `ESP-FC` and it is open network. If you connect to this AP, then choose `Manual Selection` and type `tcp://192.168.4.1:1111` 13 | 14 | ![Connect to ESP-FC AP](/docs/images/espfc_wifi_ap_connect.png) 15 | 16 | You can also automatically connect ESP-FC to your home network. To achive that, go to the CLI tab, and enter your network name and secret. 17 | ``` 18 | set wifi_ssid MY-HOME-NET 19 | set wifi_pass MY-HOME-PASS 20 | ``` 21 | > [!NOTE] 22 | > Network name and pass must not contains spaces. 23 | 24 | then you can check status of connection by typing `wifi` in CLI tab. 25 | ``` 26 | wifi 27 | ST IP4: tcp://0.0.0.0:1111 28 | ST MAC: 30:30:F9:6E:10:74 29 | AP IP4: tcp://192.168.4.1:1111 30 | AP MAC: 32:30:F9:6E:10:74 31 | ``` 32 | In this mode you need to discover IP address granted by DHCP. In line `ST IP4` is the address that you need to use in configurator. If there is `0.0.0.0`, that means that FC was not able to connect to home network. 33 | 34 | ## ESP-NOW control 35 | 36 | ESP-NOW is a proprietary wireless communication protocol defined by Espressif, which enables the direct, quick and low-power control of smart devices, without the need of a router. 37 | 38 | As there are no real transmitters usng this protocol on the market, you have to build your own transmitting module first. If you alredy own RC transmitter with JR bay, you can use another ESP32 module. In this case follow [espnow-rclink-tx](https://github.com/rtlopez/espnow-rclink-tx) instructions. 39 | 40 | In ESP-FC you have to choose `SPI Rx (e.g. built-in Rx)` as Receiver mode in Receiver tab. 41 | 42 | ![ESP-FC ESP-NOW Reciever](/docs/images/espfc_receiver.png) 43 | 44 | Transmitter and receiver binds automatically after power up, you don't need to do anything. Recomended startup procedure is: 45 | 1. turn on transmitter first 46 | 2. next power up receiver/flight controller 47 | -------------------------------------------------------------------------------- /include/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/include/.gitkeep -------------------------------------------------------------------------------- /lib/AHRS/library.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "AHRS", 3 | "version": "1.0.0" 4 | } 5 | -------------------------------------------------------------------------------- /lib/AHRS/src/Kalman.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. 2 | 3 | This software may be distributed and modified under the terms of the GNU 4 | General Public License version 2 (GPL2) as published by the Free Software 5 | Foundation and appearing in the file GPL2.TXT included in the packaging of 6 | this file. Please note that GPL2 Section 2[b] requires that all works based 7 | on this software must also be made publicly available under the terms of 8 | the GPL2 ("Copyleft"). 9 | 10 | Contact information 11 | ------------------- 12 | 13 | Kristian Lauszus, TKJ Electronics 14 | Web : http://www.tkjelectronics.com 15 | e-mail : kristianl@tkjelectronics.com 16 | */ 17 | 18 | #ifndef _Kalman_h_ 19 | #define _Kalman_h_ 20 | 21 | class Kalman { 22 | public: 23 | Kalman(); 24 | 25 | // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds 26 | float getAngle(float newAngle, float newRate, float dt); 27 | 28 | void setAngle(float angle); // Used to set angle, this should be set as the starting angle 29 | float getRate(); // Return the unbiased rate 30 | 31 | /* These are used to tune the Kalman filter */ 32 | void setQangle(float Q_angle); 33 | void setQbias(float Q_bias); 34 | void setRmeasure(float R_measure); 35 | 36 | float getQangle(); 37 | float getQbias(); 38 | float getRmeasure(); 39 | 40 | private: 41 | /* Kalman filter variables */ 42 | float Q_angle; // Process noise variance for the accelerometer 43 | float Q_bias; // Process noise variance for the gyro bias 44 | float R_measure; // Measurement noise variance - this is actually the variance of the measurement noise 45 | 46 | float angle; // The angle calculated by the Kalman filter - part of the 2x1 state vector 47 | float bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector 48 | float rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate 49 | 50 | float P[2][2]; // Error covariance matrix - This is a 2x2 matrix 51 | }; 52 | 53 | #endif 54 | -------------------------------------------------------------------------------- /lib/AHRS/src/Madgwick.h: -------------------------------------------------------------------------------- 1 | //============================================================================================= 2 | // Madgwick.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 Madgwick_h 18 | #define Madgwick_h 19 | 20 | #include "helper_3dmath.h" 21 | 22 | //-------------------------------------------------------------------------------------------- 23 | // Variable declaration 24 | class Madgwick { 25 | private: 26 | float q0, q1, q2, q3; // quaternion of sensor frame relative to auxiliary frame 27 | float beta; // algorithm gain 28 | float invSampleFreq; 29 | float roll, pitch, yaw; 30 | bool anglesComputed; 31 | 32 | void computeAngles(); 33 | 34 | //------------------------------------------------------------------------------------------- 35 | // Function declarations 36 | public: 37 | Madgwick(); 38 | void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; } 39 | 40 | void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); 41 | void update(float gx, float gy, float gz, float ax, float ay, float az); 42 | 43 | void setKp(float p) { 44 | beta = p; 45 | } 46 | void setKi(float i) { 47 | (void)i; 48 | } 49 | const Quaternion getQuaternion() const { 50 | return Quaternion(q0, q1, q2, q3); 51 | } 52 | const VectorFloat getEuler() { 53 | if (!anglesComputed) computeAngles(); 54 | return VectorFloat(roll, pitch, yaw); 55 | } 56 | }; 57 | #endif 58 | -------------------------------------------------------------------------------- /lib/AHRS/src/Mahony.h: -------------------------------------------------------------------------------- 1 | //============================================================================================= 2 | // Mahony.h 3 | //============================================================================================= 4 | // 5 | // Madgwick's implementation of Mayhony's AHRS algorithm. 6 | // See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/ 7 | // 8 | // Date Author Notes 9 | // 29/09/2011 SOH Madgwick Initial release 10 | // 02/10/2011 SOH Madgwick Optimised for reduced CPU load 11 | // 12 | //============================================================================================= 13 | #ifndef Mahony_h 14 | #define Mahony_h 15 | 16 | #include "helper_3dmath.h" 17 | 18 | //-------------------------------------------------------------------------------------------- 19 | // Variable declaration 20 | 21 | class Mahony { 22 | private: 23 | float q0, q1, q2, q3; // quaternion of sensor frame relative to auxiliary frame 24 | float twoKp; // 2 * proportional gain (Kp) 25 | float twoKi; // 2 * integral gain (Ki) 26 | float integralFBx, integralFBy, integralFBz; // integral error terms scaled by Ki 27 | float invSampleFreq; 28 | float roll, pitch, yaw; 29 | bool anglesComputed; 30 | 31 | void computeAngles(); 32 | 33 | //------------------------------------------------------------------------------------------- 34 | // Function declarations 35 | 36 | public: 37 | Mahony(); 38 | void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; } 39 | 40 | void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); 41 | void update(float gx, float gy, float gz, float ax, float ay, float az); 42 | 43 | void setKp(float p) { 44 | twoKp = p; 45 | } 46 | void setKi(float i) { 47 | twoKi = i; 48 | } 49 | const Quaternion getQuaternion() const { 50 | return Quaternion(q0, q1, q2, q3); 51 | } 52 | const VectorFloat getEuler() { 53 | if (!anglesComputed) computeAngles(); 54 | return VectorFloat(roll, pitch, yaw); 55 | } 56 | }; 57 | 58 | #endif 59 | -------------------------------------------------------------------------------- /lib/EscDriver/library.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "EscDriver", 3 | "version": "1.0.0" 4 | } 5 | -------------------------------------------------------------------------------- /lib/EscDriver/src/EscDriver.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "EscDriverBase.hpp" 4 | 5 | #if defined(ESP8266) 6 | 7 | #define ESC_CHANNEL_COUNT 4 8 | #include "EscDriverEsp8266.h" 9 | #define EscDriver EscDriverEsp8266 10 | 11 | #define ESC_DRIVER_MOTOR_TIMER ESC_DRIVER_TIMER1 12 | #define ESC_DRIVER_SERVO_TIMER ESC_DRIVER_TIMER2 13 | 14 | #elif defined(ESP32C3) 15 | 16 | #define ESC_CHANNEL_COUNT 4 17 | #include "EscDriverEsp32c3.h" 18 | #define EscDriver EscDriverEsp32c3 19 | 20 | #define ESC_DRIVER_MOTOR_TIMER ESC_DRIVER_TIMER0 21 | #define ESC_DRIVER_SERVO_TIMER ESC_DRIVER_TIMER1 22 | 23 | #elif defined(ESP32S3) || defined(ESP32S2) 24 | 25 | #define ESC_CHANNEL_COUNT 4 26 | #include "EscDriverEsp32.h" 27 | #define EscDriver EscDriverEsp32 28 | 29 | #define ESC_DRIVER_MOTOR_TIMER 0 30 | #define ESC_DRIVER_SERVO_TIMER 1 31 | 32 | #elif defined(ESP32) 33 | 34 | #define ESC_CHANNEL_COUNT RMT_CHANNEL_MAX 35 | #include "EscDriverEsp32.h" 36 | #define EscDriver EscDriverEsp32 37 | 38 | #define ESC_DRIVER_MOTOR_TIMER 0 39 | #define ESC_DRIVER_SERVO_TIMER 1 40 | 41 | #elif defined(ARCH_RP2040) 42 | 43 | #define ESC_CHANNEL_COUNT 8 44 | #include "EscDriverRP2040.h" 45 | #define EscDriver EscDriverRP2040 46 | 47 | #define ESC_DRIVER_MOTOR_TIMER ESC_DRIVER_TIMER0 48 | #define ESC_DRIVER_SERVO_TIMER ESC_DRIVER_TIMER1 49 | 50 | #elif defined(UNIT_TEST) 51 | 52 | #define ESC_CHANNEL_COUNT 4 53 | #define EscDriver EscDriverBase 54 | 55 | #define ESC_DRIVER_MOTOR_TIMER 0 56 | #define ESC_DRIVER_SERVO_TIMER 1 57 | 58 | #else 59 | 60 | #error "Unsupported platform" 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /lib/EscDriver/src/EscDriverBase.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | enum EscProtocol { 7 | ESC_PROTOCOL_PWM, 8 | ESC_PROTOCOL_ONESHOT125, 9 | ESC_PROTOCOL_ONESHOT42, 10 | ESC_PROTOCOL_MULTISHOT, 11 | ESC_PROTOCOL_BRUSHED, 12 | ESC_PROTOCOL_DSHOT150, 13 | ESC_PROTOCOL_DSHOT300, 14 | ESC_PROTOCOL_DSHOT600, 15 | ESC_PROTOCOL_PROSHOT, 16 | ESC_PROTOCOL_DISABLED, 17 | ESC_PROTOCOL_COUNT 18 | }; 19 | 20 | struct EscConfig 21 | { 22 | int timer; 23 | EscProtocol protocol; 24 | int rate; 25 | bool async; 26 | bool dshotTelemetry; 27 | }; 28 | 29 | #define PWM_TO_DSHOT(v) (((v - 1000) * 2) + 47) 30 | #define ESC_PROTOCOL_SANITIZE(p) (p > ESC_PROTOCOL_DSHOT600 && p != ESC_PROTOCOL_DISABLED ? ESC_PROTOCOL_DSHOT600 : p) 31 | 32 | class EscDriverBase 33 | { 34 | public: 35 | static constexpr size_t DSHOT_BIT_COUNT = 16; 36 | static constexpr uint32_t INVALID_TELEMETRY_VALUE = 0xffff; 37 | static constexpr int SECONDS_PER_MINUTE = 60; 38 | static constexpr float ERPM_PER_LSB = 100.0f; 39 | 40 | static uint16_t dshotConvert(uint16_t pulse); 41 | static uint16_t dshotEncode(uint16_t value, bool inverted = false); 42 | 43 | /** 44 | * @param data expected data layout (bits): duration0(15), level0(1), duration(15), level1(1) 45 | * @param len number of data items 46 | * @param bitLen duration of single bit 47 | * @return uint32_t raw gcr value 48 | */ 49 | static uint32_t extractTelemetryGcr(uint32_t* data, size_t len, uint32_t bitLen); 50 | static uint32_t durationToBitLen(uint32_t duration, uint32_t len); 51 | static uint32_t pushBits(uint32_t value, uint32_t bitVal, size_t bitLen); 52 | static uint32_t gcrToRawValue(uint32_t value); 53 | 54 | static float getErpmToHzRatio(int poles); 55 | static uint32_t convertToErpm(uint32_t value); 56 | static uint32_t convertToValue(uint32_t value); 57 | 58 | static const char * const * getProtocolNames(); 59 | static const char * const getProtocolName(EscProtocol protocol); 60 | 61 | #if defined(UNIT_TEST) 62 | int begin(const EscConfig& conf) { return 1; } 63 | void end() {} 64 | int attach(size_t channel, int pin, int pulse) { return 1; } 65 | int write(size_t channel, int pulse) { return 1; } 66 | void apply() {} 67 | int pin(size_t channel) const { return -1; } 68 | uint32_t telemetry(size_t channel) const { return 0; } 69 | #endif 70 | }; 71 | -------------------------------------------------------------------------------- /lib/EscDriver/src/EscDriverEsp32c3.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #if defined(ESP32C3) 4 | 5 | #include "EscDriver.h" 6 | 7 | enum EscDriverTimer 8 | { 9 | ESC_DRIVER_TIMER0, 10 | ESC_DRIVER_TIMER1, 11 | }; 12 | 13 | class EscDriverEsp32c3: public EscDriverBase 14 | { 15 | public: 16 | class Slot 17 | { 18 | public: 19 | Slot(): pin(-1), pulse(0) {} 20 | int pin; 21 | int pulse; 22 | bool operator<(const Slot& rhs) const 23 | { 24 | if(!active()) return false; 25 | else if(!rhs.active()) return true; 26 | return this->pulse < rhs.pulse; 27 | } 28 | inline bool active() const { return pin != -1; } 29 | }; 30 | 31 | class Item 32 | { 33 | public: 34 | Item(): set_mask(0), clr_mask(0), ticks(0), last(0) {} 35 | uint32_t set_mask; // pin set mask 36 | uint32_t clr_mask; // pin clear mask 37 | int32_t ticks; // delay to next cycle 38 | uint32_t last; 39 | void reset() { *this = Item(); } 40 | }; 41 | 42 | using mask_t = uint32_t; 43 | 44 | EscDriverEsp32c3(); 45 | 46 | int begin(const EscConfig& conf); 47 | void end(); 48 | int attach(size_t channel, int pin, int pulse); 49 | int write(size_t channel, int pulse); 50 | void apply(); 51 | int pin(size_t channel) const; 52 | uint32_t telemetry(size_t channel) const; 53 | static bool handle(void * p); 54 | 55 | private: 56 | void commit(); 57 | uint32_t usToTicks(uint32_t us); 58 | uint32_t usToTicksReal(EscDriverTimer timer, uint32_t us); 59 | int32_t minTicks(EscDriverTimer timer); 60 | void dshotWrite(); 61 | 62 | static void _isr_init(EscDriverTimer timer, void * driver); 63 | static bool _isr_wait(EscDriverTimer timer, const uint32_t ticks); 64 | static void _isr_start(EscDriverTimer timer); 65 | static void _isr_end(EscDriverTimer timer, void* p); 66 | 67 | volatile bool _busy; 68 | bool _async; 69 | EscProtocol _protocol; 70 | int _rate; 71 | EscDriverTimer _timer; 72 | int32_t _interval; 73 | int32_t _intervalMin; // for brushed 74 | int32_t _intervalMax; // for brushed 75 | 76 | int _dh; 77 | int _dl; 78 | 79 | Slot _slots[ESC_CHANNEL_COUNT]; 80 | Item _items[ESC_CHANNEL_COUNT * 2]; 81 | 82 | volatile Item * _it; 83 | const Item * _end; 84 | 85 | mask_t dshotSetMask[DSHOT_BIT_COUNT]; 86 | mask_t dshotClrMask[DSHOT_BIT_COUNT * 2]; 87 | 88 | //static EscDriverEsp32c3 * _instance; 89 | }; 90 | 91 | #endif // ESP32C3 92 | -------------------------------------------------------------------------------- /lib/EscDriver/src/EscDriverEsp8266.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #if defined(ESP8266) 4 | 5 | #include "EscDriver.h" 6 | 7 | enum EscDriverTimer 8 | { 9 | ESC_DRIVER_TIMER0, 10 | ESC_DRIVER_TIMER1, 11 | ESC_DRIVER_TIMER2 12 | }; 13 | 14 | #define ETS_FRC_TIMER2_INUM 10 15 | 16 | #define DELTA_TICKS_MAX ((APB_CLK_FREQ / 1000000L) * 50000L) 17 | #define DELTA_TICKS_MIN 5 18 | 19 | class EscDriverEsp8266: public EscDriverBase 20 | { 21 | public: 22 | class Slot 23 | { 24 | public: 25 | Slot(): pin(-1), pulse(0) {} 26 | int pin; 27 | int pulse; 28 | bool operator<(const Slot& rhs) const 29 | { 30 | if(!active()) return false; 31 | else if(!rhs.active()) return true; 32 | return this->pulse < rhs.pulse; 33 | } 34 | inline bool active() const { return pin != -1; } 35 | }; 36 | 37 | class Item 38 | { 39 | public: 40 | Item(): set_mask(0), clr_mask(0), ticks(0), last(0) {} 41 | uint32_t set_mask; // pin set mask 42 | uint32_t clr_mask; // pin clear mask 43 | int32_t ticks; // delay to next cycle 44 | uint32_t last; 45 | void reset() { *this = Item(); } 46 | }; 47 | 48 | using mask_t = uint32_t; 49 | 50 | EscDriverEsp8266(); 51 | 52 | int begin(const EscConfig& conf); 53 | void end(); 54 | int attach(size_t channel, int pin, int pulse); 55 | int write(size_t channel, int pulse); 56 | void apply(); 57 | int pin(size_t channel) const; 58 | uint32_t telemetry(size_t channel) const; 59 | static void handle(void * p, void * x); 60 | 61 | private: 62 | void commit(); 63 | uint32_t usToTicks(uint32_t us); 64 | uint32_t usToTicksReal(EscDriverTimer timer, uint32_t us); 65 | int32_t minTicks(EscDriverTimer timer); 66 | void dshotWrite(); 67 | 68 | static void _isr_init(EscDriverTimer timer, void * driver); 69 | static void _isr_begin(EscDriverTimer timer); 70 | static bool _isr_wait(EscDriverTimer timer, const uint32_t ticks); 71 | static void _isr_start(EscDriverTimer timer); 72 | static void _isr_reboot(void* p); 73 | static void _isr_end(EscDriverTimer timer, void* p); 74 | 75 | volatile bool _busy; 76 | bool _async; 77 | EscProtocol _protocol; 78 | int _rate; 79 | EscDriverTimer _timer; 80 | int32_t _interval; 81 | int32_t _intervalMin; // for brushed 82 | int32_t _intervalMax; // for brushed 83 | 84 | int _dh; 85 | int _dm; 86 | int _dl; 87 | 88 | Slot _slots[ESC_CHANNEL_COUNT]; 89 | Item _items[ESC_CHANNEL_COUNT * 2]; 90 | 91 | volatile Item * _it; 92 | const Item * _end; 93 | }; 94 | 95 | #endif // ESP8266 96 | -------------------------------------------------------------------------------- /lib/EscDriver/src/EscDriverRP2040.h: -------------------------------------------------------------------------------- 1 | #ifndef _ESC_DRIVER_RP2040_H_ 2 | #define _ESC_DRIVER_RP2040_H_ 3 | 4 | #if defined(ARCH_RP2040) 5 | 6 | #include "EscDriver.h" 7 | #include 8 | 9 | // TODO: 10 | // https://cocode.se/linux/raspberry/pwm.html 11 | // https://forums.raspberrypi.com/viewtopic.php?t=305969 (irq) 12 | // https://github.com/raspberrypi/pico-examples/blob/master/pwm/led_fade/pwm_led_fade.c (irq) 13 | 14 | enum EscDriverTimer 15 | { 16 | ESC_DRIVER_TIMER0, 17 | ESC_DRIVER_TIMER1 18 | }; 19 | 20 | class EscDriverRP2040: public EscDriverBase 21 | { 22 | public: 23 | class Slot 24 | { 25 | public: 26 | Slot(): pin(-1), pulse(0), slice(0), channel(0), drive(false) {} 27 | int pin; 28 | int pulse; 29 | int slice; 30 | int channel; 31 | bool drive; 32 | int pwm_dma_chan; 33 | dma_channel_config dma_config; 34 | inline bool active() const { return pin != -1; } 35 | }; 36 | 37 | using mask_t = uint32_t; 38 | 39 | EscDriverRP2040(); 40 | 41 | int begin(const EscConfig& conf); 42 | void end(); 43 | int attach(size_t channel, int pin, int pulse); 44 | int write(size_t channel, int pulse); 45 | int pin(size_t channel) const; 46 | uint32_t telemetry(size_t channel) const; 47 | void apply(); 48 | 49 | private: 50 | uint32_t usToTicks(uint32_t us); 51 | uint32_t usToTicksReal(uint32_t us); 52 | uint32_t nsToDshotTicks(uint32_t ns); 53 | void dshotWriteDMA(); 54 | bool isSliceDriven(int slice); 55 | void clearDmaBuffer(); 56 | 57 | EscProtocol _protocol; 58 | bool _async; 59 | int _rate; 60 | EscDriverTimer _timer; 61 | uint16_t _divider; 62 | uint32_t _interval; 63 | 64 | Slot _slots[ESC_CHANNEL_COUNT]; 65 | 66 | int _dh; 67 | int _dl; 68 | int _dt; 69 | 70 | uint32_t _dma_buffer[NUM_PWM_SLICES][DSHOT_BIT_COUNT + 1]; 71 | }; 72 | 73 | #endif // ARCH_RP2040 74 | 75 | #endif 76 | -------------------------------------------------------------------------------- /lib/EspWire/library.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "EspWire", 3 | "version": "1.0.0" 4 | } 5 | -------------------------------------------------------------------------------- /lib/EspWire/src/EspWire.h: -------------------------------------------------------------------------------- 1 | /* 2 | EspTwoWire.h - TWI/I2C library for Arduino & Wiring 3 | Copyright (c) 2006 Nicholas Zambetti. All right reserved. 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | 19 | Modified 2012 by Todd Krein (todd@krein.org) to implement repeated starts 20 | Modified December 2014 by Ivan Grokhotkov (ivan@esp8266.com) - esp8266 support 21 | Modified April 2015 by Hrsto Gochkov (ficeto@ficeto.com) - alternative esp8266 support 22 | */ 23 | 24 | #ifndef EspTwoWire_h 25 | #define EspTwoWire_h 26 | 27 | #include 28 | #include 29 | 30 | #define ESPWIRE_BUFFER_LENGTH 64 31 | 32 | class EspTwoWire : public Stream 33 | { 34 | private: 35 | static uint8_t rxBuffer[]; 36 | static uint8_t rxBufferIndex; 37 | static uint8_t rxBufferLength; 38 | 39 | static uint8_t txAddress; 40 | static uint8_t txBuffer[]; 41 | static uint8_t txBufferIndex; 42 | static uint8_t txBufferLength; 43 | 44 | static uint8_t transmitting; 45 | static void (*user_onRequest)(void); 46 | static void (*user_onReceive)(int); 47 | static void onRequestService(void); 48 | static void onReceiveService(uint8_t*, int); 49 | public: 50 | EspTwoWire(); 51 | void begin(int sda, int scl, uint32_t frequency = 0); 52 | void pins(int sda, int scl) __attribute__((deprecated)); // use begin(sda, scl) in new code 53 | void begin(); 54 | void begin(uint8_t); 55 | void begin(int); 56 | void setClock(uint32_t); 57 | void setClockStretchLimit(uint32_t); 58 | void beginTransmission(uint8_t)/* IRAM_ATTR */; 59 | void beginTransmission(int); 60 | uint8_t endTransmission(void) /* IRAM_ATTR */; 61 | uint8_t endTransmission(uint8_t); 62 | size_t requestFrom(uint8_t address, size_t size, bool sendStop); 63 | uint8_t status(); 64 | 65 | uint8_t requestFrom(uint8_t, uint8_t) /* IRAM_ATTR */; 66 | uint8_t requestFrom(uint8_t, uint8_t, uint8_t); 67 | uint8_t requestFrom(int, int); 68 | uint8_t requestFrom(int, int, int); 69 | 70 | virtual size_t write(uint8_t) /* IRAM_ATTR */; 71 | virtual size_t write(const uint8_t *, size_t); 72 | virtual int available(void) /* IRAM_ATTR */; 73 | virtual int read(void) /* IRAM_ATTR */; 74 | virtual int peek(void); 75 | virtual void flush(void); 76 | void onReceive( void (*)(int) ); 77 | void onRequest( void (*)(void) ); 78 | 79 | inline size_t write(unsigned long n) { return write((uint8_t)n); } 80 | inline size_t write(long n) { return write((uint8_t)n); } 81 | inline size_t write(unsigned int n) { return write((uint8_t)n); } 82 | inline size_t write(int n) { return write((uint8_t)n); } 83 | using Print::write; 84 | }; 85 | 86 | #if !defined(NO_GLOBAL_INSTANCES) && !defined(NO_GLOBAL_TWOWIRE) 87 | extern EspTwoWire EspWire; 88 | #endif 89 | 90 | #endif 91 | -------------------------------------------------------------------------------- /lib/EspWire/src/esp_twi.h: -------------------------------------------------------------------------------- 1 | /* 2 | twi.h - Software I2C library for esp8266 3 | 4 | Copyright (c) 2015 Hristo Gochkov. All rights reserved. 5 | This file is part of the esp8266 core for Arduino environment. 6 | 7 | This library is free software; you can redistribute it and/or 8 | modify it under the terms of the GNU Lesser General Public 9 | License as published by the Free Software Foundation; either 10 | version 2.1 of the License, or (at your option) any later version. 11 | 12 | This library is distributed in the hope that it will be useful, 13 | but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | Lesser General Public License for more details. 16 | 17 | You should have received a copy of the GNU Lesser General Public 18 | License along with this library; if not, write to the Free Software 19 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 20 | */ 21 | #ifndef ESP_TWI_H 22 | #define ESP_TWI_H 23 | 24 | #ifdef __cplusplus 25 | extern "C" { 26 | #endif 27 | 28 | #define I2C_OK 0 29 | #define I2C_SCL_HELD_LOW 1 30 | #define I2C_SCL_HELD_LOW_AFTER_READ 2 31 | #define I2C_SDA_HELD_LOW 3 32 | #define I2C_SDA_HELD_LOW_AFTER_INIT 4 33 | 34 | void esp_twi_init(unsigned char sda, unsigned char scl); 35 | void esp_twi_stop(void); 36 | void esp_twi_setClock(unsigned int freq); 37 | void esp_twi_setClockStretchLimit(uint32_t limit); 38 | uint8_t esp_twi_writeTo(unsigned char address, unsigned char * buf, unsigned int len, unsigned char sendStop); 39 | uint8_t esp_twi_readFrom(unsigned char address, unsigned char * buf, unsigned int len, unsigned char sendStop); 40 | uint8_t esp_twi_status(); 41 | 42 | #ifdef __cplusplus 43 | } 44 | #endif 45 | 46 | #endif 47 | -------------------------------------------------------------------------------- /lib/EspWire/src/keywords.txt: -------------------------------------------------------------------------------- 1 | ####################################### 2 | # Syntax Coloring Map For Wire 3 | ####################################### 4 | 5 | ####################################### 6 | # Datatypes (KEYWORD1) 7 | ####################################### 8 | 9 | ####################################### 10 | # Methods and Functions (KEYWORD2) 11 | ####################################### 12 | 13 | begin KEYWORD2 14 | setClock KEYWORD2 15 | setClockStretchLimit KEYWORD2 16 | beginTransmission KEYWORD2 17 | endTransmission KEYWORD2 18 | requestFrom KEYWORD2 19 | send KEYWORD2 20 | receive KEYWORD2 21 | onReceive KEYWORD2 22 | onRequest KEYWORD2 23 | 24 | ####################################### 25 | # Instances (KEYWORD2) 26 | ####################################### 27 | 28 | Wire KEYWORD2 29 | 30 | ####################################### 31 | # Constants (LITERAL1) 32 | ####################################### 33 | 34 | -------------------------------------------------------------------------------- /lib/EspWire/src/library.properties: -------------------------------------------------------------------------------- 1 | name=Wire 2 | version=1.1 3 | author=Arduino 4 | maintainer=Ivan Grokhotkov 5 | sentence=Allows the communication between devices or sensors connected via Two Wire Interface Bus. For esp8266 boards. 6 | paragraph= 7 | category=Signal Input/Output 8 | url=http://arduino.cc/en/Reference/Wire 9 | architectures=esp8266 10 | -------------------------------------------------------------------------------- /lib/Espfc/library.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "Espfc", 3 | "version": "0.2.0" 4 | } 5 | -------------------------------------------------------------------------------- /lib/Espfc/src/Blackbox/Blackbox.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Model.h" 4 | #include "Device/SerialDevice.h" 5 | #include "BlackboxSerialBuffer.h" 6 | extern "C" { 7 | #include 8 | } 9 | 10 | namespace Espfc { 11 | 12 | namespace Blackbox { 13 | 14 | class Blackbox 15 | { 16 | public: 17 | Blackbox(Model& model); 18 | int begin(); 19 | int update(); 20 | 21 | private: 22 | void updateData(); 23 | void updateArmed(); 24 | void updateMode(); 25 | 26 | Model& _model; 27 | pidProfile_s _pidProfile; 28 | Device::SerialDevice * _serial; 29 | BlackboxSerialBuffer _buffer; 30 | }; 31 | 32 | } 33 | 34 | } 35 | -------------------------------------------------------------------------------- /lib/Espfc/src/Blackbox/BlackboxBridge.cpp: -------------------------------------------------------------------------------- 1 | #include "BlackboxBridge.h" 2 | 3 | static Espfc::Model * _model_ptr = nullptr; 4 | 5 | void initBlackboxModel(Espfc::Model * m) 6 | { 7 | _model_ptr = m; 8 | } 9 | 10 | uint16_t getBatteryVoltageLatest(void) 11 | { 12 | if(!_model_ptr) return 0; 13 | float v = (*_model_ptr).state.battery.voltageUnfiltered; 14 | return constrain(lrintf(v * 100.0f), 0, 32000); 15 | } 16 | 17 | int32_t getAmperageLatest(void) 18 | { 19 | if(!_model_ptr) return 0; 20 | float v = (*_model_ptr).state.battery.currentUnfiltered; 21 | return constrain(lrintf(v * 100.0f), 0, 32000); 22 | } 23 | 24 | bool rxIsReceivingSignal(void) 25 | { 26 | if(!_model_ptr) return false; 27 | return !((*_model_ptr).state.input.rxLoss || (*_model_ptr).state.input.rxFailSafe); 28 | } 29 | 30 | bool isRssiConfigured(void) 31 | { 32 | if(!_model_ptr) return false; 33 | return (*_model_ptr).config.input.rssiChannel > 0; 34 | } 35 | 36 | uint16_t getRssi(void) 37 | { 38 | if(!_model_ptr) return 0; 39 | return (*_model_ptr).getRssi(); 40 | } 41 | 42 | failsafePhase_e failsafePhase() 43 | { 44 | if(!_model_ptr) return ::FAILSAFE_IDLE; 45 | return (failsafePhase_e)(*_model_ptr).state.failsafe.phase; 46 | } 47 | 48 | static uint32_t enabledSensors = 0; 49 | 50 | bool featureIsEnabled(uint32_t mask) 51 | { 52 | return featureConfigMutable()->enabledFeatures & mask; 53 | } 54 | 55 | void sensorsSet(uint32_t mask) 56 | { 57 | enabledSensors |= mask; 58 | } 59 | 60 | bool sensors(uint32_t mask) 61 | { 62 | return enabledSensors & mask; 63 | } 64 | 65 | float pidGetPreviousSetpoint(int axis) 66 | { 67 | return Espfc::Utils::toDeg(_model_ptr->state.setpoint.rate[axis]); 68 | } 69 | 70 | float mixerGetThrottle(void) 71 | { 72 | return (_model_ptr->state.output.ch[Espfc::AXIS_THRUST] + 1.0f) * 0.5f; 73 | } 74 | 75 | int16_t getMotorOutputLow() 76 | { 77 | return _model_ptr->state.mixer.digitalOutput ? PWM_TO_DSHOT(1000) : 1000; 78 | } 79 | 80 | int16_t getMotorOutputHigh() 81 | { 82 | return _model_ptr->state.mixer.digitalOutput ? PWM_TO_DSHOT(2000) : 2000; 83 | } 84 | 85 | bool areMotorsRunning(void) 86 | { 87 | return _model_ptr->areMotorsRunning() || _model_ptr->state.mode.isLongClickActive(); 88 | } 89 | 90 | uint16_t getDshotErpm(uint8_t i) 91 | { 92 | return _model_ptr->state.output.telemetry.erpm[i]; 93 | } 94 | -------------------------------------------------------------------------------- /lib/Espfc/src/Blackbox/BlackboxBridge.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Model.h" 4 | #include 5 | extern "C" { 6 | #include 7 | #include 8 | bool blackboxShouldLogPFrame(void); 9 | bool blackboxShouldLogIFrame(void); 10 | } 11 | 12 | void initBlackboxModel(Espfc::Model * m); 13 | -------------------------------------------------------------------------------- /lib/Espfc/src/Blackbox/BlackboxSerialBuffer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Target/Target.h" 4 | #include "Device/SerialDevice.h" 5 | 6 | namespace Espfc { 7 | 8 | namespace Blackbox { 9 | 10 | class BlackboxSerialBuffer: public Device::SerialDevice 11 | { 12 | public: 13 | static constexpr size_t SIZE = SERIAL_TX_FIFO_SIZE;//128; 14 | 15 | BlackboxSerialBuffer(): _dev(nullptr), _idx(0), _data(nullptr) {} 16 | 17 | ~BlackboxSerialBuffer() 18 | { 19 | if(!_data) return; 20 | 21 | delete[] _data; 22 | _data = nullptr; 23 | } 24 | 25 | void updateBaudRate(int baud) override { }; 26 | 27 | void wrap(Espfc::Device::SerialDevice * s) 28 | { 29 | _dev = s; 30 | _data = new uint8_t[SIZE]; 31 | } 32 | 33 | void begin(const Espfc::SerialDeviceConfig& conf) override 34 | { 35 | //_dev->begin(conf); 36 | } 37 | 38 | size_t write(uint8_t c) override 39 | { 40 | _data[_idx++] = c; 41 | if(_idx >= SIZE) flush(); 42 | return 1; 43 | } 44 | 45 | void flush() override 46 | { 47 | if(_dev) _dev->write(_data, _idx); 48 | _idx = 0; 49 | } 50 | 51 | int availableForWrite() override 52 | { 53 | //return _dev->availableForWrite(); 54 | return SIZE - _idx; 55 | } 56 | 57 | bool isTxFifoEmpty() override 58 | { 59 | //return _dev->isTxFifoEmpty(); 60 | return _idx == 0; 61 | } 62 | 63 | int available() override { return _dev->available(); } 64 | int read() override { return _dev->read(); } 65 | size_t readMany(uint8_t * c, size_t l) override { 66 | #if defined(ARCH_RP2040) 67 | size_t count = std::min(l, (size_t)available()); 68 | for(size_t i = 0; i < count; i++) 69 | { 70 | c[i] = read(); 71 | } 72 | return count; 73 | #else 74 | return _dev->readMany(c, l); 75 | #endif 76 | } 77 | int peek() override { return _dev->peek(); } 78 | 79 | size_t write(const uint8_t * c, size_t l) override 80 | { 81 | for(size_t i = 0; i < l; i++) 82 | { 83 | write(c[i]); 84 | } 85 | return l; 86 | } 87 | bool isSoft() const override { return false; }; 88 | operator bool() const override { return (bool)(*_dev); } 89 | 90 | Device::SerialDevice * _dev; 91 | size_t _idx; 92 | uint8_t* _data; 93 | }; 94 | 95 | } 96 | 97 | } 98 | -------------------------------------------------------------------------------- /lib/Espfc/src/Connect/Buzzer.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Model.h" 4 | 5 | namespace Espfc { 6 | 7 | namespace Connect { 8 | 9 | enum BuzzerPlayStatus 10 | { 11 | BUZZER_STATUS_IDLE, 12 | BUZZER_STATUS_TEST, 13 | BUZZER_STATUS_ON, 14 | BUZZER_STATUS_OFF 15 | }; 16 | 17 | class Buzzer 18 | { 19 | public: 20 | Buzzer(Model& model); 21 | int begin(); 22 | int update(); 23 | 24 | private: 25 | void _play(bool v, int time, BuzzerPlayStatus s); 26 | void _write(bool v); 27 | void _delay(int time); 28 | 29 | static const uint8_t** schemes(); 30 | 31 | Model& _model; 32 | 33 | BuzzerPlayStatus _status; 34 | uint32_t _wait; 35 | const uint8_t * _scheme; 36 | BuzzerEvent _e; 37 | }; 38 | 39 | } 40 | 41 | } 42 | -------------------------------------------------------------------------------- /lib/Espfc/src/Connect/Msp.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include "Hal/Pgm.h" 6 | 7 | namespace Espfc { 8 | 9 | namespace Connect { 10 | 11 | constexpr size_t MSP_BUF_SIZE = 192; 12 | constexpr size_t MSP_BUF_OUT_SIZE = 240; 13 | 14 | enum MspState { 15 | MSP_STATE_IDLE, 16 | MSP_STATE_HEADER_START, 17 | 18 | MSP_STATE_HEADER_M, 19 | MSP_STATE_HEADER_V1, 20 | MSP_STATE_PAYLOAD_V1, 21 | MSP_STATE_CHECKSUM_V1, 22 | 23 | MSP_STATE_HEADER_X, 24 | MSP_STATE_HEADER_V2, 25 | MSP_STATE_PAYLOAD_V2, 26 | MSP_STATE_CHECKSUM_V2, 27 | 28 | MSP_STATE_RECEIVED, 29 | }; 30 | 31 | enum MspType { 32 | MSP_TYPE_CMD, 33 | MSP_TYPE_REPLY 34 | }; 35 | 36 | enum MspVersion { 37 | MSP_V1, 38 | MSP_V2 39 | }; 40 | 41 | struct MspHeaderV1 { 42 | uint8_t size; 43 | uint8_t cmd; 44 | } __attribute__((packed)); 45 | 46 | struct MspHeaderV2 { 47 | uint8_t flags; 48 | uint16_t cmd; 49 | uint16_t size; 50 | } __attribute__((packed)); 51 | 52 | class MspMessage 53 | { 54 | public: 55 | MspMessage(); 56 | bool isReady() const; 57 | bool isCmd() const; 58 | bool isIdle() const; 59 | int remain() const; 60 | void advance(size_t size); 61 | uint8_t readU8(); 62 | uint16_t readU16(); 63 | uint32_t readU32(); 64 | 65 | MspState state; 66 | MspType dir; 67 | MspVersion version; 68 | uint8_t flags; 69 | uint16_t cmd; 70 | uint16_t expected; 71 | uint16_t received; 72 | uint16_t read; 73 | uint8_t checksum; 74 | uint8_t checksum2; 75 | uint8_t buffer[MSP_BUF_SIZE]; 76 | }; 77 | 78 | class MspResponse 79 | { 80 | public: 81 | MspResponse(); 82 | 83 | MspVersion version; 84 | uint16_t cmd; 85 | int8_t result; 86 | uint8_t direction; 87 | uint16_t len; 88 | uint8_t data[MSP_BUF_OUT_SIZE]; 89 | 90 | int remain() const; 91 | void advance(size_t size); 92 | void writeData(const char * v, int size); 93 | void writeString(const char * v); 94 | void writeString(const __FlashStringHelper *ifsh); 95 | void writeU8(uint8_t v); 96 | void writeU16(uint16_t v); 97 | void writeU32(uint32_t v); 98 | size_t serialize(uint8_t * buff, size_t len_max) const; 99 | size_t serializeV1(uint8_t * buff, size_t len_max) const; 100 | size_t serializeV2(uint8_t * buff, size_t len_max) const; 101 | }; 102 | 103 | } 104 | 105 | } 106 | -------------------------------------------------------------------------------- /lib/Espfc/src/Connect/MspParser.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Connect/Msp.hpp" 4 | 5 | namespace Espfc { 6 | 7 | namespace Connect { 8 | 9 | class MspParser 10 | { 11 | public: 12 | MspParser(); 13 | void parse(char c, MspMessage& msg); 14 | }; 15 | 16 | } 17 | 18 | } 19 | -------------------------------------------------------------------------------- /lib/Espfc/src/Connect/MspProcessor.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Model.h" 4 | #include "Connect/Msp.hpp" 5 | #include "Connect/MspParser.hpp" 6 | #include "Device/SerialDevice.h" 7 | #include 8 | 9 | namespace Espfc { 10 | 11 | namespace Connect { 12 | 13 | class MspProcessor 14 | { 15 | public: 16 | MspProcessor(Model& model); 17 | bool parse(char c, MspMessage& msg); 18 | void processCommand(MspMessage& m, MspResponse& r, Device::SerialDevice& s); 19 | void processEsc4way(); 20 | void processRestart(); 21 | void serializeFlashData(MspResponse& r, uint32_t address, const uint16_t size, bool useLegacyFormat, bool allowCompression); 22 | 23 | void sendResponse(MspResponse& r, Device::SerialDevice& s); 24 | void postCommand(); 25 | bool debugSkip(uint8_t cmd); 26 | void debugMessage(const MspMessage& m); 27 | void debugResponse(const MspResponse& r); 28 | 29 | private: 30 | Model& _model; 31 | MspParser _parser; 32 | std::function _postCommand; 33 | }; 34 | 35 | } 36 | 37 | } 38 | -------------------------------------------------------------------------------- /lib/Espfc/src/Connect/StatusLed.cpp: -------------------------------------------------------------------------------- 1 | #include "StatusLed.hpp" 2 | #include 3 | 4 | namespace Espfc::Connect 5 | { 6 | 7 | static int LED_OFF_PATTERN[] = {0}; 8 | static int LED_OK_PATTERN[] = {100, 900, 0}; 9 | static int LED_ERROR_PATTERN[] = {100, 100, 100, 100, 100, 1500, 0}; 10 | static int LED_ON_PATTERN[] = {100, 0}; 11 | 12 | StatusLed::StatusLed() : _pin(-1), _invert(0), _status(LED_OFF), _next(0), _state(LOW), _step(0), _pattern(LED_OFF_PATTERN) {} 13 | 14 | void StatusLed::begin(int8_t pin, uint8_t invert) 15 | { 16 | if(pin == -1) return; 17 | _pin = pin; 18 | _invert = invert; 19 | pinMode(_pin, OUTPUT); 20 | setStatus(LED_ON, true); 21 | } 22 | 23 | void StatusLed::setStatus(LedStatus newStatus, bool force) 24 | { 25 | if(_pin == -1) return; 26 | if(!force && newStatus == _status) return; 27 | 28 | _status = newStatus; 29 | _state = LOW; 30 | _step = 0; 31 | _next = millis(); 32 | 33 | switch (_status) 34 | { 35 | case LED_OK: 36 | _pattern = LED_OK_PATTERN; 37 | break; 38 | case LED_ERROR: 39 | _pattern = LED_ERROR_PATTERN; 40 | break; 41 | case LED_ON: 42 | _pattern = LED_ON_PATTERN; 43 | _state = HIGH; 44 | break; 45 | case LED_OFF: 46 | default: 47 | _pattern = LED_OFF_PATTERN; 48 | break; 49 | } 50 | _write(_state); 51 | } 52 | 53 | void StatusLed::update() 54 | { 55 | if(_pin == -1 || !_pattern) return; 56 | 57 | uint32_t now = millis(); 58 | 59 | if(now < _next) return; 60 | 61 | if (!_pattern[_step]) 62 | { 63 | _step = 0; 64 | _next = now + 20; 65 | return; 66 | } 67 | 68 | _state = !(_step & 1); 69 | _write(_state); 70 | 71 | _next = now + _pattern[_step]; 72 | _step++; 73 | } 74 | 75 | void StatusLed::_write(uint8_t val) 76 | { 77 | digitalWrite(_pin, val ^ _invert); 78 | } 79 | 80 | } 81 | -------------------------------------------------------------------------------- /lib/Espfc/src/Connect/StatusLed.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | namespace Espfc::Connect 6 | { 7 | 8 | enum LedStatus 9 | { 10 | LED_OFF, 11 | LED_OK, 12 | LED_ERROR, 13 | LED_ON 14 | }; 15 | 16 | class StatusLed 17 | { 18 | 19 | public: 20 | StatusLed(); 21 | void begin(int8_t pin, uint8_t invert); 22 | void update(); 23 | void setStatus(LedStatus newStatus, bool force = false); 24 | 25 | private: 26 | void _write(uint8_t val); 27 | int8_t _pin; 28 | int8_t _invert; 29 | LedStatus _status; 30 | uint32_t _next; 31 | bool _state; 32 | size_t _step; 33 | int * _pattern; 34 | }; 35 | 36 | } -------------------------------------------------------------------------------- /lib/Espfc/src/Connect/Vtx.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Device/SerialDevice.h" 4 | #include "Utils/Timer.h" 5 | #include "Model.h" 6 | 7 | namespace Espfc::Connect { 8 | 9 | enum VtxDeviceType { 10 | VTXDEV_UNSUPPORTED = 0, // reserved for MSP 11 | VTXDEV_RTC6705 = 1, 12 | // 2 reserved 13 | VTXDEV_SMARTAUDIO = 3, 14 | VTXDEV_TRAMP = 4, 15 | VTXDEV_MSP = 5, 16 | VTXDEV_UNKNOWN = 0xFF, 17 | }; 18 | 19 | enum State { 20 | INACTIVE, 21 | INIT, 22 | SET_POWER, 23 | SET_CHANNEL, 24 | IDLE, 25 | }; 26 | 27 | class Vtx 28 | { 29 | public: 30 | Vtx(Model& model): _serial(NULL), _model(model) {} 31 | 32 | int begin(Device::SerialDevice * serial); 33 | int update(); 34 | int setChannel(); 35 | int setPower(); 36 | Connect::VtxDeviceType type; 37 | 38 | private: 39 | Device::SerialDevice* _serial; 40 | Model& _model; 41 | State _state = State::INACTIVE; 42 | bool _armed = false; 43 | Utils::Timer _timer; 44 | }; 45 | 46 | } 47 | -------------------------------------------------------------------------------- /lib/Espfc/src/Control/Actuator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Model.h" 4 | 5 | namespace Espfc::Control { 6 | 7 | class Actuator 8 | { 9 | public: 10 | Actuator(Model& model); 11 | 12 | int begin(); 13 | int update(); 14 | 15 | #ifndef UNIT_TEST 16 | private: 17 | #endif 18 | 19 | void updateScaler(); 20 | void updateArmingDisabled(); 21 | void updateModeMask(); 22 | bool canActivateMode(FlightMode mode); 23 | void updateArmed(); 24 | void updateAirMode(); 25 | void updateBuzzer(); 26 | void updateDynLpf(); 27 | void updateRescueConfig(); 28 | void updateLed(); 29 | 30 | Model& _model; 31 | }; 32 | 33 | } 34 | -------------------------------------------------------------------------------- /lib/Espfc/src/Control/Controller.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Model.h" 4 | #include "Control/Rates.h" 5 | 6 | namespace Espfc { 7 | 8 | namespace Control { 9 | 10 | class Controller 11 | { 12 | public: 13 | Controller(Model& model); 14 | int begin(); 15 | int update(); 16 | 17 | void outerLoopRobot(); 18 | void innerLoopRobot(); 19 | void outerLoop(); 20 | void innerLoop(); 21 | 22 | inline float getTpaFactor() const; 23 | inline void resetIterm(); 24 | float calculateSetpointRate(int axis, float input); 25 | 26 | private: 27 | Model& _model; 28 | Rates _rates; 29 | Utils::Filter _speedFilter; 30 | }; 31 | 32 | } 33 | 34 | } 35 | -------------------------------------------------------------------------------- /lib/Espfc/src/Control/Fusion.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Model.h" 4 | #include 5 | #include 6 | 7 | namespace Espfc { 8 | 9 | namespace Control { 10 | 11 | class Fusion 12 | { 13 | public: 14 | Fusion(Model& model); 15 | int begin(); 16 | void restoreGain(); 17 | int update(); 18 | 19 | void experimentalFusion(); 20 | void simpleFusion(); 21 | void kalmanFusion(); 22 | void complementaryFusion(); 23 | void complementaryFusionOld(); 24 | void rtqfFusion(); 25 | void updatePoseFromAccelMag(); 26 | 27 | // experimental 28 | void lerpFusion(); 29 | void madgwickFusion(); 30 | void mahonyFusion(); 31 | 32 | private: 33 | Model& _model; 34 | bool _first; 35 | Madgwick _madgwick; 36 | Mahony _mahony; 37 | }; 38 | 39 | } 40 | 41 | } 42 | -------------------------------------------------------------------------------- /lib/Espfc/src/Control/Pid.cpp: -------------------------------------------------------------------------------- 1 | #include "Pid.h" 2 | #include "Utils/Math.hpp" 3 | #include "Utils/MemoryHelper.h" 4 | 5 | namespace Espfc { 6 | 7 | namespace Control { 8 | 9 | Pid::Pid(): 10 | rate(1.0f), dt(1.0f), Kp(0.1), Ki(0.f), Kd(0.f), Kf(0.0f), iLimit(0.3f), oLimit(1.f), 11 | pScale(1.f), iScale(1.f), dScale(1.f), fScale(1.f), 12 | error(0.f), iTermError(0.f), 13 | pTerm(0.f), iTerm(0.f), dTerm(0.f), fTerm(0.f), 14 | prevMeasurement(0.f), prevError(0.f), prevSetpoint(0.f), 15 | outputSaturated(false), 16 | itermRelax(ITERM_RELAX_OFF), itermRelaxFactor(1.0f), itermRelaxBase(0.f) 17 | {} 18 | 19 | void Pid::begin() 20 | { 21 | dt = 1.f / rate; 22 | } 23 | 24 | float FAST_CODE_ATTR Pid::update(float setpoint, float measurement) 25 | { 26 | error = setpoint - measurement; 27 | 28 | // P-term 29 | pTerm = Kp * error * pScale; 30 | pTerm = ptermFilter.update(pTerm); 31 | 32 | // I-term 33 | iTermError = error; 34 | if(Ki > 0.f && iScale > 0.f) 35 | { 36 | if(!outputSaturated) 37 | { 38 | // I-term relax 39 | if(itermRelax) 40 | { 41 | const bool increasing = (iTerm > 0 && iTermError > 0) || (iTerm < 0 && iTermError < 0); 42 | const bool incrementOnly = itermRelax == ITERM_RELAX_RP_INC || itermRelax == ITERM_RELAX_RPY_INC; 43 | itermRelaxBase = setpoint - itermRelaxFilter.update(setpoint); 44 | itermRelaxFactor = std::max(0.0f, 1.0f - std::abs(Utils::toDeg(itermRelaxBase)) * 0.025f); // (itermRelaxBase / 40) 45 | if(!incrementOnly || increasing) iTermError *= itermRelaxFactor; 46 | } 47 | iTerm += Ki * iScale * iTermError * dt; 48 | iTerm = Utils::clamp(iTerm, -iLimit, iLimit); 49 | } 50 | } 51 | else 52 | { 53 | iTerm = 0; // zero integral 54 | } 55 | 56 | // D-term 57 | if(Kd > 0.f && dScale > 0.f) 58 | { 59 | //dTerm = (Kd * dScale * (((error - prevError) * dGamma) + (prevMeasurement - measure) * (1.f - dGamma)) / dt); 60 | dTerm = Kd * dScale * ((prevMeasurement - measurement) * rate); 61 | dTerm = dtermNotchFilter.update(dTerm); 62 | dTerm = dtermFilter.update(dTerm); 63 | dTerm = dtermFilter2.update(dTerm); 64 | } 65 | else 66 | { 67 | dTerm = 0; 68 | } 69 | 70 | // F-term 71 | if(Kf > 0.f && fScale > 0.f) 72 | { 73 | fTerm = Kf * fScale * (setpoint - prevSetpoint) * rate; 74 | fTerm = ftermFilter.update(fTerm); 75 | } 76 | else 77 | { 78 | fTerm = 0; 79 | } 80 | 81 | prevMeasurement = measurement; 82 | prevError = error; 83 | prevSetpoint = setpoint; 84 | 85 | return Utils::clamp(pTerm + iTerm + dTerm + fTerm, -oLimit, oLimit); 86 | } 87 | 88 | } 89 | 90 | } 91 | -------------------------------------------------------------------------------- /lib/Espfc/src/Control/Pid.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "Utils/Filter.h" 5 | #include "Utils/Math.hpp" 6 | 7 | namespace Espfc { 8 | 9 | // bataflight scalers 10 | constexpr float PTERM_SCALE_BETAFLIGHT = 0.032029f; 11 | constexpr float ITERM_SCALE_BETAFLIGHT = 0.244381f; 12 | constexpr float DTERM_SCALE_BETAFLIGHT = 0.000529f; 13 | constexpr float FTERM_SCALE_BETAFLIGHT = 0.00013754f; 14 | 15 | constexpr float PTERM_SCALE = PTERM_SCALE_BETAFLIGHT * Utils::toDeg(1.0f) * 0.001f; // ~ 0.00183 = 0.032029f * 57.29 / 1000 16 | constexpr float ITERM_SCALE = ITERM_SCALE_BETAFLIGHT * Utils::toDeg(1.0f) * 0.001f; // ~ 0.014f 17 | constexpr float DTERM_SCALE = DTERM_SCALE_BETAFLIGHT * Utils::toDeg(1.0f) * 0.001f; // ~ 0.0000303f 18 | constexpr float FTERM_SCALE = FTERM_SCALE_BETAFLIGHT * Utils::toDeg(1.0f) * 0.001f; // ~ 0.00000788f 19 | 20 | constexpr float LEVEL_PTERM_SCALE = 0.1f; // 1/10 21 | constexpr float LEVEL_ITERM_SCALE = 0.1f; // 1/10 22 | constexpr float LEVEL_DTERM_SCALE = 0.001f; // 1/1000 23 | constexpr float LEVEL_FTERM_SCALE = 0.001f; // 1/1000 24 | 25 | enum ItermRelaxType { 26 | ITERM_RELAX_OFF, 27 | ITERM_RELAX_RP, 28 | ITERM_RELAX_RPY, 29 | ITERM_RELAX_RP_INC, 30 | ITERM_RELAX_RPY_INC, 31 | ITERM_RELAX_COUNT, 32 | }; 33 | 34 | namespace Control { 35 | 36 | class Pid 37 | { 38 | public: 39 | Pid(); 40 | void begin(); 41 | float update(float setpoint, float measure); 42 | 43 | float rate; 44 | float dt; 45 | 46 | float Kp; 47 | float Ki; 48 | float Kd; 49 | float Kf; 50 | 51 | float iLimit; 52 | float oLimit; 53 | 54 | float pScale; 55 | float iScale; 56 | float dScale; 57 | float fScale; 58 | 59 | float error; 60 | float iTermError; 61 | 62 | float pTerm; 63 | float iTerm; 64 | float dTerm; 65 | float fTerm; 66 | 67 | Utils::Filter dtermFilter; 68 | Utils::Filter dtermFilter2; 69 | Utils::Filter dtermNotchFilter; 70 | Utils::Filter ptermFilter; 71 | Utils::Filter ftermFilter; 72 | Utils::Filter itermRelaxFilter; 73 | 74 | float prevMeasurement; 75 | float prevError; 76 | float prevSetpoint; 77 | 78 | bool outputSaturated; 79 | int8_t itermRelax; 80 | float itermRelaxFactor; 81 | float itermRelaxBase; 82 | }; 83 | 84 | } 85 | 86 | } 87 | -------------------------------------------------------------------------------- /lib/Espfc/src/Control/Rates.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Utils/Math.hpp" 4 | #include "ModelConfig.h" 5 | 6 | namespace Espfc 7 | { 8 | 9 | enum RateType { 10 | RATES_TYPE_BETAFLIGHT = 0, 11 | RATES_TYPE_RACEFLIGHT, 12 | RATES_TYPE_KISS, 13 | RATES_TYPE_ACTUAL, 14 | RATES_TYPE_QUICK, 15 | }; 16 | 17 | namespace Control { 18 | 19 | class Rates 20 | { 21 | public: 22 | void begin(const InputConfig& config); 23 | float getSetpoint(const int axis, float input) const; 24 | 25 | private: 26 | float betaflight(const int axis, float rcCommandf, const float rcCommandfAbs) const; 27 | float raceflight(const int axis, float rcCommandf, const float rcCommandfAbs) const; 28 | float kiss(const int axis, float rcCommandf, const float rcCommandfAbs) const; 29 | float actual(const int axis, float rcCommandf, const float rcCommandfAbs) const; 30 | float quick(const int axis, float rcCommandf, const float rcCommandfAbs) const; 31 | 32 | inline float power3(float x) const 33 | { 34 | return x * x * x; 35 | } 36 | 37 | inline float power5(float x) const 38 | { 39 | return x * x * x * x * x; 40 | } 41 | 42 | inline float constrainf(float x, float l, float h) const 43 | { 44 | return Utils::clamp(x, l, h); 45 | } 46 | 47 | private: 48 | RateType rateType; 49 | uint8_t rcExpo[3]; 50 | uint8_t rcRates[3]; 51 | uint8_t rates[3]; 52 | int16_t rateLimit[3]; 53 | }; 54 | 55 | } 56 | 57 | } 58 | -------------------------------------------------------------------------------- /lib/Espfc/src/Debug_Espfc.cpp: -------------------------------------------------------------------------------- 1 | #include "Debug_Espfc.h" 2 | 3 | namespace Espfc 4 | { 5 | 6 | #ifdef ESPFC_DEBUG_SERIAL 7 | Stream * _debugStream = nullptr; 8 | #endif 9 | 10 | } 11 | -------------------------------------------------------------------------------- /lib/Espfc/src/Debug_Espfc.h: -------------------------------------------------------------------------------- 1 | #ifndef _ESPFC_DEBUG_H_ 2 | #define _ESPFC_DEBUG_H_ 3 | 4 | #include 5 | #include "Hal/Gpio.h" 6 | 7 | #ifdef ESPFC_DEBUG_PIN 8 | #define PIN_DEBUG(v) ::Espfc::Hal::Gpio::digitalWrite(ESPFC_DEBUG_PIN, v) 9 | #define PIN_DEBUG_INIT() ::Espfc::Hal::Gpio::pinMode(ESPFC_DEBUG_PIN, OUTPUT) 10 | #else 11 | #define PIN_DEBUG(v) 12 | #define PIN_DEBUG_INIT() 13 | #endif 14 | 15 | namespace Espfc 16 | { 17 | 18 | #ifdef ESPFC_DEBUG_SERIAL 19 | extern Stream * _debugStream; 20 | 21 | static inline void initDebugStream(Stream * p) { _debugStream = p; } 22 | 23 | #define LOG_SERIAL_INIT(p) _debugStream = p; 24 | #define LOG_SERIAL_DEBUG(v) if(_debugStream) { _debugStream->print(v); } 25 | #define LOG_SERIAL_DEBUG_HEX(v) if(_debugStream) { _debugStream->print(v, HEX); } 26 | 27 | template 28 | void D(T t) 29 | { 30 | if(!_debugStream) return; 31 | _debugStream->println(t); 32 | } 33 | 34 | template 35 | void D(T t, Args... args) // recursive variadic function 36 | { 37 | if(!_debugStream) return; 38 | _debugStream->print(t); 39 | _debugStream->print(' '); 40 | D(args...); 41 | } 42 | 43 | #else 44 | 45 | static inline void initDebugStream(Stream * p) {} 46 | 47 | #define LOG_SERIAL_INIT(p) 48 | #define LOG_SERIAL_DEBUG(v) 49 | #define LOG_SERIAL_DEBUG_HEX(v) 50 | #define D(...) 51 | 52 | #endif 53 | 54 | } 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /lib/Espfc/src/Device/BaroDevice.cpp: -------------------------------------------------------------------------------- 1 | #include "Device/BaroDevice.h" 2 | #include "Hal/Pgm.h" 3 | #include 4 | 5 | namespace Espfc::Device { 6 | 7 | const char ** BaroDevice::getNames() 8 | { 9 | static const char* devChoices[] = { PSTR("AUTO"), PSTR("NONE"), PSTR("BMP085"), PSTR("MS5611"), PSTR("BMP280"), PSTR("SPL06-001"), NULL }; 10 | return devChoices; 11 | } 12 | 13 | const char * BaroDevice::getName(DeviceType type) 14 | { 15 | if(type >= BARO_MAX) return PSTR("?"); 16 | return getNames()[type]; 17 | } 18 | 19 | } 20 | -------------------------------------------------------------------------------- /lib/Espfc/src/Device/BaroDevice.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "BusDevice.h" 4 | #include "BusAwareDevice.h" 5 | 6 | namespace Espfc { 7 | 8 | enum BaroDeviceType { 9 | BARO_DEFAULT = 0, 10 | BARO_NONE = 1, 11 | BARO_BMP085 = 2, 12 | BARO_MS5611 = 3, 13 | BARO_BMP280 = 4, 14 | BARO_SPL06 = 5, 15 | BARO_MAX 16 | }; 17 | 18 | enum BaroDeviceMode { 19 | BARO_MODE_TEMP, 20 | BARO_MODE_PRESS, 21 | }; 22 | 23 | namespace Device { 24 | 25 | class BaroDevice: public BusAwareDevice 26 | { 27 | public: 28 | typedef BaroDeviceType DeviceType; 29 | 30 | virtual int begin(BusDevice * bus) = 0; 31 | virtual int begin(BusDevice * bus, uint8_t addr) = 0; 32 | 33 | virtual DeviceType getType() const = 0; 34 | 35 | virtual float readTemperature() = 0; 36 | virtual float readPressure() = 0; 37 | virtual int getDelay(BaroDeviceMode mode) const = 0; 38 | virtual void setMode(BaroDeviceMode mode) = 0; 39 | 40 | virtual bool testConnection() = 0; 41 | 42 | static const char ** getNames(); 43 | static const char * getName(DeviceType type); 44 | }; 45 | 46 | } 47 | 48 | } 49 | -------------------------------------------------------------------------------- /lib/Espfc/src/Device/BusAwareDevice.h: -------------------------------------------------------------------------------- 1 | #ifndef _ESPFC_DEVICE_BUS_AWARE_DEVICE_H_ 2 | #define _ESPFC_DEVICE_BUS_AWARE_DEVICE_H_ 3 | 4 | #include "BusDevice.h" 5 | 6 | namespace Espfc { 7 | 8 | namespace Device { 9 | 10 | class BusAwareDevice 11 | { 12 | public: 13 | void setBus(BusDevice * bus, uint8_t addr) 14 | { 15 | _bus = bus; 16 | _addr = addr; 17 | } 18 | 19 | const BusDevice * getBus() const 20 | { 21 | return _bus; 22 | } 23 | 24 | uint8_t getAddress() const 25 | { 26 | return _addr; 27 | } 28 | 29 | protected: 30 | BusDevice * _bus; 31 | uint8_t _addr; 32 | }; 33 | 34 | } 35 | 36 | } 37 | 38 | #endif -------------------------------------------------------------------------------- /lib/Espfc/src/Device/BusDevice.cpp: -------------------------------------------------------------------------------- 1 | #include "Device/BusDevice.h" 2 | #include "Hal/Pgm.h" 3 | #include 4 | 5 | namespace Espfc::Device { 6 | 7 | const char ** BusDevice::getNames() 8 | { 9 | static const char* busDevChoices[] = { PSTR("NONE"), PSTR("AUTO"), PSTR("I2C"), PSTR("SPI"), PSTR("SLV"), NULL }; 10 | return busDevChoices; 11 | } 12 | 13 | const char * BusDevice::getName(BusType type) 14 | { 15 | if(type >= BUS_MAX) return PSTR("?"); 16 | return getNames()[type]; 17 | } 18 | 19 | } 20 | -------------------------------------------------------------------------------- /lib/Espfc/src/Device/BusI2C.cpp: -------------------------------------------------------------------------------- 1 | #include "Target/Target.h" 2 | 3 | #if defined(ESPFC_I2C_0) 4 | 5 | #include "BusI2C.h" 6 | 7 | namespace Espfc { 8 | 9 | namespace Device { 10 | 11 | BusI2C::BusI2C(WireClass& i2c): _dev(i2c) {} 12 | 13 | BusType BusI2C::getType() const { return BUS_I2C; } 14 | 15 | int BusI2C::begin(int sda, int scl, uint32_t speed) 16 | { 17 | if(sda == -1 || scl == -1) return 0; 18 | 19 | targetI2CInit(_dev, sda, scl, speed); 20 | 21 | return 1; 22 | } 23 | 24 | int8_t FAST_CODE_ATTR BusI2C::readFast(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) 25 | { 26 | return read(devAddr, regAddr, length, data); 27 | } 28 | 29 | int8_t FAST_CODE_ATTR BusI2C::read(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) 30 | { 31 | int8_t count = 0; 32 | uint32_t t1 = millis(); 33 | 34 | //D("i2c:r0", devAddr, regAddr, length); 35 | 36 | _dev.beginTransmission(devAddr); 37 | _dev.write(regAddr); 38 | _dev.endTransmission(); 39 | _dev.requestFrom(devAddr, length); 40 | 41 | for (; _dev.available() && (_timeout == 0 || millis() - t1 < _timeout); count++) 42 | { 43 | data[count] = _dev.read(); 44 | //D("i2c:r1", count, data[count]); 45 | } 46 | 47 | //D("i2c:r3", length, count); 48 | if (_timeout > 0 && millis() - t1 >= _timeout && count < length) count = -1; // timeout 49 | 50 | if(onError && count != length) onError(); 51 | 52 | return count; 53 | } 54 | 55 | bool BusI2C::write(uint8_t devAddr, uint8_t regAddr, uint8_t length, const uint8_t* data) 56 | { 57 | //Serial.print("I2C W "); Serial.print(devAddr, HEX); Serial.print(' '); Serial.print(regAddr, HEX); Serial.print(' '); Serial.println(length); 58 | 59 | _dev.beginTransmission(devAddr); 60 | _dev.write((uint8_t) regAddr); // send address 61 | for (uint8_t i = 0; i < length; i++) 62 | { 63 | _dev.write(data[i]); 64 | //Serial.print("I2C W "); Serial.print(i); Serial.print(' '); Serial.println(data[i], HEX); 65 | } 66 | uint8_t status = _dev.endTransmission(); 67 | 68 | if(onError && status != 0) onError(); 69 | 70 | return status == 0; 71 | } 72 | 73 | } 74 | 75 | } 76 | 77 | #endif 78 | -------------------------------------------------------------------------------- /lib/Espfc/src/Device/BusI2C.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "BusDevice.h" 4 | 5 | namespace Espfc { 6 | 7 | namespace Device { 8 | 9 | class BusI2C: public BusDevice 10 | { 11 | public: 12 | BusI2C(WireClass& i2c); 13 | BusType getType() const override; 14 | 15 | int begin(int sda, int scl, uint32_t speed); 16 | int8_t readFast(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) override; 17 | int8_t read(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) override; 18 | bool write(uint8_t devAddr, uint8_t regAddr, uint8_t length, const uint8_t* data) override; 19 | 20 | private: 21 | WireClass& _dev; 22 | }; 23 | 24 | } 25 | 26 | } 27 | -------------------------------------------------------------------------------- /lib/Espfc/src/Device/BusSPI.cpp: -------------------------------------------------------------------------------- 1 | #include "Target/Target.h" 2 | 3 | #if defined(ESPFC_SPI_0) 4 | 5 | #include "BusSPI.h" 6 | #include 7 | 8 | namespace Espfc { 9 | 10 | namespace Device { 11 | 12 | BusSPI::BusSPI(ESPFC_SPI_0_DEV_T& spi): _dev(spi) {} 13 | 14 | BusType BusSPI::getType() const { return BUS_SPI; } 15 | 16 | int BusSPI::begin(int8_t sck, int8_t mosi, int8_t miso, int8_t ss) 17 | { 18 | if(sck == -1 || miso == -1 || mosi == -1) return 0; 19 | 20 | targetSPIInit(_dev, sck, mosi, miso, ss); 21 | 22 | return 1; 23 | } 24 | 25 | int8_t BusSPI::read(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) 26 | { 27 | //D("spi:r", regAddr, length); 28 | transfer(devAddr, regAddr | SPI_READ, length, NULL, data, SPI_SPEED_NORMAL); 29 | return length; 30 | } 31 | 32 | int8_t FAST_CODE_ATTR BusSPI::readFast(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) 33 | { 34 | //D("spi:r", regAddr, length); 35 | transfer(devAddr, regAddr | SPI_READ, length, NULL, data, SPI_SPEED_FAST); 36 | return length; 37 | } 38 | 39 | bool BusSPI::write(uint8_t devAddr, uint8_t regAddr, uint8_t length, const uint8_t* data) 40 | { 41 | //D("spi:w", regAddr, length, *data); 42 | transfer(devAddr, regAddr & SPI_WRITE, length, data, NULL, SPI_SPEED_NORMAL); 43 | return true; 44 | } 45 | 46 | void FAST_CODE_ATTR BusSPI::transfer(uint8_t devAddr, uint8_t regAddr, uint8_t length, const uint8_t *in, uint8_t *out, uint32_t speed) 47 | { 48 | _dev.beginTransaction(SPISettings(speed, MSBFIRST, SPI_MODE0)); 49 | Hal::Gpio::digitalWrite(devAddr, LOW); 50 | #if defined (ARCH_RP2040) 51 | _dev.transfer(regAddr); 52 | _dev.transfer(in, out, length); 53 | #else 54 | _dev.transfer(regAddr); 55 | _dev.transferBytes(in, out, length); 56 | #endif 57 | Hal::Gpio::digitalWrite(devAddr, HIGH); 58 | _dev.endTransaction(); 59 | } 60 | 61 | } 62 | 63 | } 64 | 65 | #endif -------------------------------------------------------------------------------- /lib/Espfc/src/Device/BusSPI.h: -------------------------------------------------------------------------------- 1 | #ifndef _ESPFC_DEVICE_BUSSPI_H_ 2 | #define _ESPFC_DEVICE_BUSSPI_H_ 3 | 4 | #include "BusDevice.h" 5 | 6 | namespace Espfc { 7 | 8 | namespace Device { 9 | 10 | class BusSPI: public BusDevice 11 | { 12 | public: 13 | BusSPI(ESPFC_SPI_0_DEV_T& spi); 14 | 15 | static constexpr uint8_t SPI_READ = 0x80; 16 | static constexpr uint8_t SPI_WRITE = 0x7f; 17 | 18 | static constexpr uint32_t SPI_SPEED_NORMAL = 1000000; 19 | static constexpr uint32_t SPI_SPEED_FAST = 16000000; 20 | 21 | BusType getType() const override; 22 | 23 | int begin(int8_t sck = -1, int8_t mosi = -1, int8_t miso = -1, int8_t ss = -1); 24 | 25 | int8_t read(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) override; 26 | 27 | int8_t readFast(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) override; 28 | 29 | bool write(uint8_t devAddr, uint8_t regAddr, uint8_t length, const uint8_t* data) override; 30 | 31 | private: 32 | void transfer(uint8_t devAddr, uint8_t regAddr, uint8_t length, const uint8_t *in, uint8_t *out, uint32_t speed); 33 | 34 | ESPFC_SPI_0_DEV_T& _dev; 35 | }; 36 | 37 | } 38 | 39 | } 40 | 41 | #endif 42 | -------------------------------------------------------------------------------- /lib/Espfc/src/Device/BusSlave.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "BusSlave.h" 3 | 4 | #define MPU6050_I2C_SLV0_ADDR 0x25 5 | #define MPU6050_I2C_SLV0_REG 0x26 6 | #define MPU6050_I2C_SLV0_DO 0x63 7 | #define MPU6050_I2C_SLV0_CTRL 0x27 8 | #define MPU6050_I2C_SLV0_EN 0x80 9 | #define MPU6050_EXT_SENS_DATA_00 0x49 10 | 11 | namespace Espfc { 12 | 13 | namespace Device { 14 | 15 | BusSlave::BusSlave() {} 16 | 17 | int BusSlave::begin(BusDevice * dev, int addr) 18 | { 19 | setBus(dev, addr); 20 | 21 | return 1; 22 | } 23 | 24 | BusType BusSlave::getType() const { return BUS_SLV; } 25 | 26 | int8_t BusSlave::read(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) 27 | { 28 | // set slave 0 to the AK8963 and set for read 29 | if(!writeMaster(MPU6050_I2C_SLV0_ADDR, devAddr | 0x80)) { 30 | return 0; 31 | } 32 | // set the register to the desired AK8963 sub address 33 | if(!writeMaster(MPU6050_I2C_SLV0_REG, regAddr)) { 34 | return 0; 35 | } 36 | // enable I2C and request the bytes 37 | if(!writeMaster(MPU6050_I2C_SLV0_CTRL, MPU6050_I2C_SLV0_EN | length)) { 38 | return 0; 39 | } 40 | 41 | // takes some time for these registers to fill 42 | delay(1); 43 | 44 | // read the bytes off the EXT_SENS_DATA registers 45 | int8_t res = readMaster(MPU6050_EXT_SENS_DATA_00, length, data); 46 | 47 | return res; 48 | } 49 | 50 | // readFast() ignores devAddr and regAddr args and read ext sensor data reg from master 51 | int8_t IRAM_ATTR BusSlave::readFast(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) 52 | { 53 | return _bus->readFast(_addr, MPU6050_EXT_SENS_DATA_00, length, data); 54 | } 55 | 56 | // writes only one byte, length is ignored 57 | bool BusSlave::write(uint8_t devAddr, uint8_t regAddr, uint8_t length, const uint8_t* data) 58 | { 59 | // set slave 0 to the AK8963 and set for write 60 | if(!writeMaster(MPU6050_I2C_SLV0_ADDR, devAddr)) { 61 | return false; 62 | } 63 | // set the register to the desired AK8963 sub address 64 | if(!writeMaster(MPU6050_I2C_SLV0_REG, regAddr)) { 65 | return false; 66 | } 67 | // store the data for write 68 | if(!writeMaster(MPU6050_I2C_SLV0_DO, *data)) { 69 | return false; 70 | } 71 | // enable I2C and send 1 byte 72 | if(!writeMaster(MPU6050_I2C_SLV0_CTRL, MPU6050_I2C_SLV0_EN | 0x01)) { 73 | return false; 74 | } 75 | 76 | return true; 77 | } 78 | 79 | int8_t BusSlave::writeMaster(uint8_t regAddr, uint8_t data) 80 | { 81 | int8_t res = _bus->write(_addr, regAddr, 1, &data); 82 | delay(10); 83 | return res; 84 | } 85 | 86 | int8_t BusSlave::readMaster(uint8_t regAddr, uint8_t length, uint8_t *data) 87 | { 88 | return _bus->read(_addr, regAddr, length, data); 89 | } 90 | 91 | } 92 | 93 | } 94 | -------------------------------------------------------------------------------- /lib/Espfc/src/Device/BusSlave.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "BusDevice.h" 5 | #include "BusAwareDevice.h" 6 | 7 | namespace Espfc { 8 | 9 | namespace Device { 10 | 11 | class BusSlave: public BusDevice, public BusAwareDevice 12 | { 13 | public: 14 | BusSlave(); 15 | 16 | int begin(BusDevice * dev, int addr); 17 | 18 | BusType getType() const override; 19 | 20 | virtual int8_t read(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) override; 21 | 22 | // readFast() ignores devAddr and regAddr args and read ext sensor data reg from master 23 | virtual int8_t readFast(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) override; 24 | 25 | // writes only one byte, length is ignored 26 | virtual bool write(uint8_t devAddr, uint8_t regAddr, uint8_t length, const uint8_t* data) override; 27 | 28 | int8_t writeMaster(uint8_t regAddr, uint8_t data); 29 | 30 | int8_t readMaster(uint8_t regAddr, uint8_t length, uint8_t *data); 31 | }; 32 | 33 | } 34 | 35 | } 36 | -------------------------------------------------------------------------------- /lib/Espfc/src/Device/FlashDevice.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace Espfc { 8 | 9 | namespace Device { 10 | 11 | class FlashPartition 12 | { 13 | public: 14 | FlashPartition(esp_partition_t* p): _p(p) {} 15 | 16 | size_t read(uint32_t address, uint8_t * data, size_t len) 17 | { 18 | len = std::min((uint32_t)len, _p->size - address); 19 | if(esp_partition_read_raw(_p, address, data, len) == ESP_OK) 20 | { 21 | return len; 22 | } 23 | return 0; 24 | } 25 | 26 | size_t write(uint32_t address, const uint8_t * data, size_t len) 27 | { 28 | esp_partition_write_raw(_p, address, data, len); 29 | return len; 30 | } 31 | 32 | private: 33 | esp_partition_t* _p; 34 | }; 35 | 36 | class FlashDevice 37 | { 38 | public: 39 | static void partitions(Stream& s) 40 | { 41 | s.printf("ESP32 Partition table:\r\n"); 42 | s.printf("| Type | Sub | Offset | Size | Label |\r\n"); 43 | s.printf("| ---- | --- | -------- | -------- | ---------------- |\r\n"); 44 | 45 | esp_partition_iterator_t pi = esp_partition_find(ESP_PARTITION_TYPE_ANY, ESP_PARTITION_SUBTYPE_ANY, nullptr); 46 | if (pi != NULL) { 47 | do { 48 | const esp_partition_t* p = esp_partition_get(pi); 49 | s.printf("| %02x | %02x | 0x%06X | 0x%06X | %-16s |\r\n", 50 | p->type, p->subtype, p->address, p->size, p->label); 51 | } while ((pi = esp_partition_next(pi))); 52 | } 53 | } 54 | 55 | static const esp_partition_t* findPartition() 56 | { 57 | return esp_partition_find_first(ESP_PARTITION_TYPE_DATA, ESP_PARTITION_SUBTYPE_DATA_SPIFFS, nullptr); 58 | } 59 | 60 | static void journal(Stream& s) 61 | { 62 | FlashfsJournalItem journal[8]; 63 | flashfsJournalLoad(journal, 0, 8); 64 | for(size_t i = 0; i < 8; i++) 65 | { 66 | const auto& it = journal[i]; 67 | s.printf("%08x => %08x\r\n", it.logBegin, it.logEnd); 68 | } 69 | } 70 | 71 | }; 72 | 73 | } 74 | 75 | } -------------------------------------------------------------------------------- /lib/Espfc/src/Device/GyroDevice.cpp: -------------------------------------------------------------------------------- 1 | #include "Device/GyroDevice.h" 2 | #include "Hal/Pgm.h" 3 | #include 4 | 5 | namespace Espfc::Device { 6 | 7 | const char ** GyroDevice::getNames() 8 | { 9 | static const char* devChoices[] = { PSTR("AUTO"), PSTR("NONE"), PSTR("MPU6000"), PSTR("MPU6050"), PSTR("MPU6500"), PSTR("MPU9250"), PSTR("LSM6DSO"), PSTR("ICM20602"),PSTR("BMI160"), NULL }; 10 | return devChoices; 11 | } 12 | 13 | const char * GyroDevice::getName(DeviceType type) 14 | { 15 | if(type >= GYRO_MAX) return PSTR("?"); 16 | return getNames()[type]; 17 | } 18 | 19 | } 20 | -------------------------------------------------------------------------------- /lib/Espfc/src/Device/GyroDevice.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "BusDevice.h" 5 | #include "BusAwareDevice.h" 6 | 7 | namespace Espfc { 8 | 9 | enum GyroDeviceType { 10 | GYRO_AUTO = 0, 11 | GYRO_NONE = 1, 12 | GYRO_MPU6000 = 2, 13 | GYRO_MPU6050 = 3, 14 | GYRO_MPU6500 = 4, 15 | GYRO_MPU9250 = 5, 16 | GYRO_LSM6DSO = 6, 17 | GYRO_ICM20602 = 7, 18 | GYRO_BMI160 = 8, 19 | GYRO_MAX 20 | }; 21 | 22 | namespace Device { 23 | 24 | class GyroDevice: public BusAwareDevice 25 | { 26 | public: 27 | typedef GyroDeviceType DeviceType; 28 | 29 | virtual int begin(BusDevice * bus) = 0; 30 | virtual int begin(BusDevice * bus, uint8_t addr) = 0; 31 | 32 | virtual DeviceType getType() const = 0; 33 | 34 | virtual int readGyro(VectorInt16& v) = 0; 35 | virtual int readAccel(VectorInt16& v) = 0; 36 | 37 | virtual void setDLPFMode(uint8_t mode) = 0; 38 | virtual int getRate() const = 0; 39 | virtual void setRate(int rate) = 0; 40 | 41 | virtual bool testConnection() = 0; 42 | 43 | static const char ** getNames(); 44 | static const char * getName(DeviceType type); 45 | }; 46 | 47 | } 48 | 49 | } 50 | -------------------------------------------------------------------------------- /lib/Espfc/src/Device/GyroICM20602.h: -------------------------------------------------------------------------------- 1 | #ifndef _ESPFC_DEVICE_GYRO_ICM20602_H_ 2 | #define _ESPFC_DEVICE_GYRO_ICM20602_H_ 3 | 4 | #include "BusDevice.h" 5 | #include "GyroMPU6050.h" 6 | #include "helper_3dmath.h" 7 | #include "Debug_Espfc.h" 8 | 9 | #define ICM20602_RA_ACCEL2_CONFIG 0x1D 10 | #define ICM20602_WHOAMI_DEFAULT_VALUE 0x12 11 | 12 | namespace Espfc { 13 | 14 | namespace Device { 15 | 16 | class GyroICM20602: public GyroMPU6050 17 | { 18 | public: 19 | GyroDeviceType getType() const override 20 | { 21 | return GYRO_ICM20602; 22 | } 23 | 24 | void setDLPFMode(uint8_t mode) override 25 | { 26 | GyroMPU6050::setDLPFMode(mode); 27 | _bus->writeByte(_addr, ICM20602_RA_ACCEL2_CONFIG, mode); 28 | } 29 | 30 | bool testConnection() override 31 | { 32 | uint8_t whoami = 0; 33 | _bus->readByte(_addr, MPU6050_RA_WHO_AM_I, &whoami); 34 | //D("icm20602:whoami", _addr, whoami); 35 | return whoami == ICM20602_WHOAMI_DEFAULT_VALUE; 36 | } 37 | }; 38 | 39 | } 40 | 41 | } 42 | 43 | #endif 44 | -------------------------------------------------------------------------------- /lib/Espfc/src/Device/GyroMPU6500.h: -------------------------------------------------------------------------------- 1 | #ifndef _ESPFC_DEVICE_GYRO_MPU6500_H_ 2 | #define _ESPFC_DEVICE_GYRO_MPU6500_H_ 3 | 4 | #include "BusDevice.h" 5 | #include "GyroMPU6050.h" 6 | #include "helper_3dmath.h" 7 | #include "Debug_Espfc.h" 8 | 9 | #define MPU6500_ACCEL_CONF2 0x1D 10 | #define MPU6500_WHOAMI_DEFAULT_VALUE 0x70 11 | #define MPU6500_WHOAMI_ALT_VALUE 0x75 12 | 13 | namespace Espfc { 14 | 15 | namespace Device { 16 | 17 | class GyroMPU6500: public GyroMPU6050 18 | { 19 | public: 20 | GyroDeviceType getType() const override 21 | { 22 | return GYRO_MPU6500; 23 | } 24 | 25 | void setDLPFMode(uint8_t mode) override 26 | { 27 | GyroMPU6050::setDLPFMode(mode); 28 | _bus->writeByte(_addr, MPU6500_ACCEL_CONF2, mode); 29 | } 30 | 31 | bool testConnection() override 32 | { 33 | uint8_t whoami = 0; 34 | uint8_t len = _bus->readByte(_addr, MPU6050_RA_WHO_AM_I, &whoami); 35 | //D("MPU6500:whoami", _addr, whoami); 36 | return len == 1 && (whoami == MPU6500_WHOAMI_DEFAULT_VALUE || whoami == MPU6500_WHOAMI_ALT_VALUE); 37 | } 38 | }; 39 | 40 | } 41 | 42 | } 43 | 44 | #endif 45 | -------------------------------------------------------------------------------- /lib/Espfc/src/Device/GyroMPU9250.h: -------------------------------------------------------------------------------- 1 | #ifndef _ESPFC_DEVICE_GYRO_MPU9250_H_ 2 | #define _ESPFC_DEVICE_GYRO_MPU9250_H_ 3 | 4 | #include "BusDevice.h" 5 | #include "GyroMPU6050.h" 6 | #include "helper_3dmath.h" 7 | #include "Debug_Espfc.h" 8 | 9 | #define MPU9250_ACCEL_CONF2 0x1D 10 | #define MPU9250_WHOAMI_DEFAULT_VALUE 0x71 11 | #define MPU9250_WHOAMI_ALT_VALUE 0x73 12 | 13 | namespace Espfc { 14 | 15 | namespace Device { 16 | 17 | class GyroMPU9250: public GyroMPU6050 18 | { 19 | public: 20 | GyroDeviceType getType() const override 21 | { 22 | return GYRO_MPU9250; 23 | } 24 | 25 | void setDLPFMode(uint8_t mode) override 26 | { 27 | GyroMPU6050::setDLPFMode(mode); 28 | _bus->writeByte(_addr, MPU9250_ACCEL_CONF2, mode); 29 | } 30 | 31 | bool testConnection() override 32 | { 33 | uint8_t whoami = 0; 34 | uint8_t len = _bus->readByte(_addr, MPU6050_RA_WHO_AM_I, &whoami); 35 | //D("mpu9250:whoami", _addr, whoami); 36 | return len == 1 && (whoami == MPU9250_WHOAMI_DEFAULT_VALUE || whoami == MPU9250_WHOAMI_ALT_VALUE); 37 | } 38 | }; 39 | 40 | } 41 | 42 | } 43 | 44 | #endif 45 | -------------------------------------------------------------------------------- /lib/Espfc/src/Device/Input/InputButton.cpp: -------------------------------------------------------------------------------- 1 | #include "Device/Input/InputButton.hpp" 2 | #include "Hal/Gpio.h" 3 | #include 4 | #include 5 | 6 | // https://github.com/poelstra/arduino-multi-button 7 | namespace { 8 | 9 | const static MultiButtonConfig conf = { 20, 250, 300 }; 10 | static MultiButton btn(&conf); 11 | 12 | } 13 | 14 | namespace Espfc::Device::Input { 15 | 16 | int InputButton::begin(int pin, bool triggerLow) 17 | { 18 | _pin = pin; 19 | _triggerLow = triggerLow; 20 | 21 | if(_pin == -1) return 0; 22 | 23 | Hal::Gpio::pinMode(_pin, INPUT_PULLUP); 24 | 25 | return 1; 26 | } 27 | 28 | int InputButton::update() 29 | { 30 | if(_pin == -1) return 0; 31 | 32 | bool pressed = Hal::Gpio::digitalRead(_pin) ^ _triggerLow; 33 | 34 | btn.update(pressed); 35 | 36 | if (btn.isSingleClick()) 37 | { 38 | _result ^= 1 << 0; // toggle bit 0 39 | } 40 | 41 | if (btn.isDoubleClick()) 42 | { 43 | _result ^= 1 << 1; // toggle bit 1 44 | } 45 | 46 | if (btn.isLongClick()) 47 | { 48 | _result ^= 1 << 2; // toggle bit 2 49 | } 50 | 51 | return _result; 52 | } 53 | 54 | } 55 | -------------------------------------------------------------------------------- /lib/Espfc/src/Device/Input/InputButton.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace Espfc::Device::Input { 6 | 7 | class InputButton 8 | { 9 | public: 10 | int begin(int pin, bool triggerLow = true); 11 | int update(); 12 | 13 | private: 14 | int _result = 0; // bit 0: one click, 1: double click, 2: long press 15 | int _pin = -1; 16 | bool _triggerLow = true; 17 | }; 18 | 19 | } 20 | -------------------------------------------------------------------------------- /lib/Espfc/src/Device/InputCRSF.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Device/SerialDevice.h" 4 | #include "Device/InputDevice.h" 5 | #include "Rc/Crsf.h" 6 | #include "TelemetryManager.h" 7 | 8 | // https://github.com/CapnBry/CRServoF/blob/master/lib/CrsfSerial/crsf_protocol.h 9 | // https://github.com/AlessioMorale/crsf_parser/tree/master 10 | // https://github.com/betaflight/betaflight/blob/master/src/main/rx/crsf.c 11 | 12 | namespace Espfc { 13 | 14 | namespace Device { 15 | 16 | class InputCRSF: public InputDevice 17 | { 18 | public: 19 | enum CrsfState { 20 | CRSF_ADDR, 21 | CRSF_SIZE, 22 | CRSF_TYPE, 23 | CRSF_DATA, 24 | CRSF_CRC 25 | }; 26 | 27 | InputCRSF(); 28 | 29 | int begin(Device::SerialDevice * serial, TelemetryManager * telemetry); 30 | virtual InputStatus update() override; 31 | virtual uint16_t get(uint8_t i) const override; 32 | virtual void get(uint16_t * data, size_t len) const override; 33 | virtual size_t getChannelCount() const override; 34 | virtual bool needAverage() const override; 35 | 36 | void print(char c) const; 37 | void parse(Rc::CrsfMessage& frame, int d); 38 | 39 | private: 40 | void reset(); 41 | void apply(const Rc::CrsfMessage& msg); 42 | void applyLinkStats(const Rc::CrsfMessage& msg); 43 | void applyChannels(const Rc::CrsfMessage& msg); 44 | void applyMspReq(const Rc::CrsfMessage& msg); 45 | 46 | static constexpr size_t CHANNELS = 16; 47 | static constexpr size_t TELEMETRY_INTERVAL = 20000; 48 | 49 | Device::SerialDevice * _serial; 50 | TelemetryManager * _telemetry; 51 | CrsfState _state; 52 | uint8_t _idx; 53 | bool _new_data; 54 | Rc::CrsfMessage _frame; 55 | uint16_t _channels[CHANNELS]; 56 | uint32_t _telemetry_next; 57 | }; 58 | 59 | } 60 | 61 | } 62 | -------------------------------------------------------------------------------- /lib/Espfc/src/Device/InputDevice.h: -------------------------------------------------------------------------------- 1 | #ifndef _ESPFC_DEVICE_INPUT_DEVICE_H_ 2 | #define _ESPFC_DEVICE_INPUT_DEVICE_H_ 3 | 4 | #include 5 | #include 6 | 7 | namespace Espfc { 8 | 9 | enum InputStatus { 10 | INPUT_IDLE, 11 | INPUT_RECEIVED, 12 | INPUT_LOST, 13 | INPUT_FAILSAFE 14 | }; 15 | 16 | namespace Device { 17 | 18 | class InputDevice 19 | { 20 | public: 21 | virtual InputStatus update() = 0; 22 | virtual uint16_t get(uint8_t channel) const = 0; 23 | virtual void get(uint16_t * data, size_t len) const = 0; 24 | virtual size_t getChannelCount() const = 0; 25 | virtual bool needAverage() const = 0; 26 | }; 27 | 28 | } 29 | 30 | } 31 | 32 | #endif 33 | -------------------------------------------------------------------------------- /lib/Espfc/src/Device/InputEspNow.cpp: -------------------------------------------------------------------------------- 1 | #if defined(ESP32) || defined(ESP8266) 2 | 3 | #include "InputEspNow.h" 4 | #include "Utils/MemoryHelper.h" 5 | 6 | namespace Espfc { 7 | 8 | namespace Device { 9 | 10 | int InputEspNow::begin(void) 11 | { 12 | for(size_t i = 0; i < CHANNELS; i++) 13 | { 14 | _channels[i] = i == 2 ? 1000 : 1500; 15 | } 16 | return _rx.begin(); 17 | } 18 | 19 | InputStatus FAST_CODE_ATTR InputEspNow::update() 20 | { 21 | _rx.update(); 22 | if(_rx.available()) 23 | { 24 | for(size_t i = 0; i < CHANNELS; i++) 25 | { 26 | _channels[i] = _rx.getChannel(i); 27 | } 28 | return INPUT_RECEIVED; 29 | } 30 | return INPUT_IDLE; 31 | } 32 | 33 | uint16_t FAST_CODE_ATTR InputEspNow::get(uint8_t i) const 34 | { 35 | return _channels[i]; 36 | } 37 | 38 | void FAST_CODE_ATTR InputEspNow::get(uint16_t * data, size_t len) const 39 | { 40 | const uint16_t * src = _channels; 41 | while(len--) 42 | { 43 | *data++ = *src++; 44 | } 45 | } 46 | 47 | size_t InputEspNow::getChannelCount() const { return CHANNELS; } 48 | 49 | bool InputEspNow::needAverage() const { return false; } 50 | 51 | } 52 | 53 | } 54 | 55 | #endif 56 | -------------------------------------------------------------------------------- /lib/Espfc/src/Device/InputEspNow.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #if defined(ESP32) || defined(ESP8266) 4 | 5 | #include "Device/InputDevice.h" 6 | #include 7 | #include 8 | #include 9 | 10 | namespace Espfc { 11 | 12 | namespace Device { 13 | 14 | class InputEspNow: public InputDevice 15 | { 16 | public: 17 | int begin(void); 18 | InputStatus update() override; 19 | uint16_t get(uint8_t i) const override; 20 | void get(uint16_t * data, size_t len) const override; 21 | size_t getChannelCount() const override; 22 | bool needAverage() const override; 23 | 24 | private: 25 | EspNowRcLink::Receiver _rx; 26 | static constexpr size_t CHANNELS = EspNowRcLink::RC_CHANNEL_MAX + 1; 27 | uint16_t _channels[CHANNELS]; 28 | }; 29 | 30 | } 31 | 32 | } 33 | 34 | #endif -------------------------------------------------------------------------------- /lib/Espfc/src/Device/InputIBUS.cpp: -------------------------------------------------------------------------------- 1 | #include "InputIBUS.hpp" 2 | #include "Utils/MemoryHelper.h" 3 | #include 4 | 5 | namespace Espfc::Device 6 | { 7 | 8 | InputIBUS::InputIBUS() : _serial(NULL), _state(IBUS_LENGTH), _idx(0), _new_data(false) {} 9 | 10 | int InputIBUS::begin(Device::SerialDevice *serial) 11 | { 12 | _serial = serial; 13 | 14 | std::fill_n((uint8_t*)&_data, IBUS_FRAME_SIZE, 0); 15 | std::fill_n(_channels, CHANNELS, 0); 16 | 17 | return 1; 18 | } 19 | 20 | InputStatus FAST_CODE_ATTR InputIBUS::update() 21 | { 22 | if (!_serial) return INPUT_IDLE; 23 | 24 | uint8_t buff[64] = {0}; 25 | size_t len = std::min((size_t)_serial->available(), (size_t)sizeof(buff)); 26 | 27 | if (len) 28 | { 29 | _serial->readMany(buff, len); 30 | uint8_t* ptr = buff; 31 | while(len--) 32 | { 33 | parse(_data, *ptr++); 34 | } 35 | } 36 | 37 | if (_new_data) 38 | { 39 | _new_data = false; 40 | return INPUT_RECEIVED; 41 | } 42 | return INPUT_IDLE; 43 | } 44 | 45 | void FAST_CODE_ATTR InputIBUS::parse(IBusData& frameData, int d) 46 | { 47 | uint8_t* data = reinterpret_cast(&frameData); 48 | uint8_t c = d & 0xff; 49 | switch(_state) 50 | { 51 | case IBUS_LENGTH: 52 | if(c == IBUS_FRAME_SIZE) 53 | { 54 | data[_idx++] = c; 55 | _state = IBUS_CMD; 56 | } 57 | break; 58 | case IBUS_CMD: 59 | if(c == IBUS_COMMAND) 60 | { 61 | data[_idx++] = c; 62 | _state = IBUS_DATA; 63 | } 64 | else 65 | { 66 | _state = IBUS_LENGTH; 67 | } 68 | break; 69 | case IBUS_DATA: 70 | data[_idx] = c; 71 | if(++_idx >= IBUS_FRAME_SIZE - 2) 72 | { 73 | _state = IBUS_CRC_LO; 74 | } 75 | break; 76 | case IBUS_CRC_LO: 77 | data[_idx++] = c; 78 | _state = IBUS_CRC_HI; 79 | break; 80 | case IBUS_CRC_HI: 81 | data[_idx++] = c; 82 | uint16_t csum = 0xffff; 83 | for(size_t i = 0; i < IBUS_FRAME_SIZE - 2; i++) 84 | { 85 | csum -= data[i]; 86 | } 87 | if(frameData.checksum == csum) apply(frameData); 88 | _state = IBUS_LENGTH; 89 | _idx = 0; 90 | break; 91 | } 92 | } 93 | 94 | void FAST_CODE_ATTR InputIBUS::apply(IBusData& data) 95 | { 96 | for(size_t i = 0; i < CHANNELS; i++) 97 | { 98 | _channels[i] = data.ch[i]; 99 | } 100 | _new_data = true; 101 | } 102 | 103 | uint16_t FAST_CODE_ATTR InputIBUS::get(uint8_t i) const 104 | { 105 | return _channels[i]; 106 | } 107 | 108 | void FAST_CODE_ATTR InputIBUS::get(uint16_t *data, size_t len) const 109 | { 110 | const uint16_t *src = _channels; 111 | while (len--) 112 | { 113 | *data++ = *src++; 114 | } 115 | } 116 | 117 | size_t InputIBUS::getChannelCount() const { return CHANNELS; } 118 | 119 | bool InputIBUS::needAverage() const { return false; } 120 | 121 | } -------------------------------------------------------------------------------- /lib/Espfc/src/Device/InputIBUS.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Device/SerialDevice.h" 4 | #include "Device/InputDevice.h" 5 | 6 | namespace Espfc::Device 7 | { 8 | 9 | class InputIBUS : public InputDevice 10 | { 11 | public: 12 | struct IBusData 13 | { 14 | uint8_t len; 15 | uint8_t cmd; 16 | uint16_t ch[14]; 17 | uint16_t checksum; 18 | } __attribute__((__packed__)); 19 | 20 | InputIBUS(); 21 | 22 | int begin(Device::SerialDevice *serial); 23 | InputStatus update() override; 24 | uint16_t get(uint8_t i) const override; 25 | void get(uint16_t *data, size_t len) const override; 26 | size_t getChannelCount() const override; 27 | bool needAverage() const override; 28 | 29 | void parse(IBusData& frame, int d); 30 | 31 | private: 32 | enum IbusState 33 | { 34 | IBUS_LENGTH, 35 | IBUS_CMD, 36 | IBUS_DATA, 37 | IBUS_CRC_LO, 38 | IBUS_CRC_HI, 39 | }; 40 | 41 | void apply(IBusData& frame); 42 | 43 | static constexpr size_t IBUS_FRAME_SIZE = sizeof(IBusData); 44 | static constexpr uint8_t IBUS_COMMAND = 0x40; 45 | static constexpr size_t CHANNELS = 14; 46 | 47 | Device::SerialDevice *_serial; 48 | IbusState _state; 49 | uint8_t _idx = 0; 50 | bool _new_data; 51 | 52 | IBusData _data; 53 | uint16_t _channels[CHANNELS]; 54 | }; 55 | 56 | } 57 | -------------------------------------------------------------------------------- /lib/Espfc/src/Device/InputPPM.cpp: -------------------------------------------------------------------------------- 1 | #include "Device/InputPPM.h" 2 | #include 3 | #include "Utils/MemoryHelper.h" 4 | 5 | namespace Espfc { 6 | 7 | namespace Device { 8 | 9 | void InputPPM::begin(uint8_t pin, int mode) 10 | { 11 | if(_pin != -1) 12 | { 13 | detachInterrupt(_pin); 14 | _pin = -1; 15 | } 16 | if(pin != -1) 17 | { 18 | _pin = pin; 19 | _channel = 0; 20 | _last_tick = micros(); 21 | for(size_t i = 0; i < CHANNELS; i++) 22 | { 23 | _channels[i] = (i == 2) ? 1000 : 1500; // throttle 24 | } 25 | pinMode(_pin, INPUT); 26 | #if defined(UNIT_TEST) 27 | // no mock available 28 | #elif defined(ARCH_RP2040) 29 | attachInterruptParam(_pin, InputPPM::handle_isr, (PinStatus)mode, this); 30 | #else 31 | attachInterruptArg(_pin, InputPPM::handle_isr, this, mode); 32 | #endif 33 | } 34 | } 35 | 36 | InputStatus FAST_CODE_ATTR InputPPM::update() 37 | { 38 | if(_new_data) 39 | { 40 | _new_data = false; 41 | return INPUT_RECEIVED; 42 | } 43 | return INPUT_IDLE; 44 | } 45 | 46 | uint16_t FAST_CODE_ATTR InputPPM::get(uint8_t i) const 47 | { 48 | return _channels[i]; 49 | } 50 | 51 | void FAST_CODE_ATTR InputPPM::get(uint16_t * data, size_t len) const 52 | { 53 | const uint16_t * src = const_cast(_channels); 54 | while(len--) 55 | { 56 | *data++ = *src++; 57 | } 58 | } 59 | 60 | size_t InputPPM::getChannelCount() const { return CHANNELS; } 61 | 62 | bool InputPPM::needAverage() const { return true; } 63 | 64 | void IRAM_ATTR InputPPM::handle() 65 | { 66 | uint32_t now = micros(); 67 | uint32_t width = now - _last_tick; 68 | 69 | _last_tick = now; 70 | 71 | if(width > 3000) // sync 72 | { 73 | _channel = 0; 74 | return; 75 | } 76 | 77 | if(_channel < CHANNELS) // ignore exceding channels 78 | { 79 | _channels[_channel] = width; 80 | } 81 | if(_channel == 3) 82 | { 83 | _new_data = true; // increase responsivnes for sticks channels 84 | } 85 | _channel++; 86 | } 87 | 88 | void IRAM_ATTR InputPPM::handle_isr(void* args) 89 | { 90 | if(args) reinterpret_cast(args)->handle(); 91 | } 92 | 93 | } 94 | 95 | } 96 | -------------------------------------------------------------------------------- /lib/Espfc/src/Device/InputPPM.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include "InputDevice.h" 6 | 7 | namespace Espfc { 8 | 9 | enum PPMMode { 10 | PPM_MODE_NORMAL = 0x01, // RISING edge 11 | PPM_MODE_INVERTED = 0x02 // FALLING edge 12 | }; 13 | 14 | namespace Device { 15 | 16 | class InputPPM: public InputDevice 17 | { 18 | public: 19 | void begin(uint8_t pin, int mode = PPM_MODE_NORMAL); 20 | InputStatus update() override; 21 | uint16_t get(uint8_t i) const override; 22 | void get(uint16_t * data, size_t len) const override; 23 | size_t getChannelCount() const override; 24 | bool needAverage() const override; 25 | 26 | private: 27 | static constexpr size_t CHANNELS = 16; 28 | static constexpr uint32_t BROKEN_LINK_US = 100000UL; // 100ms 29 | 30 | void handle(); 31 | static void handle_isr(void* args); 32 | 33 | volatile uint16_t _channels[CHANNELS]; 34 | volatile uint32_t _last_tick; 35 | volatile uint8_t _channel; 36 | volatile bool _new_data; 37 | uint8_t _pin; 38 | }; 39 | 40 | } 41 | 42 | } 43 | -------------------------------------------------------------------------------- /lib/Espfc/src/Device/InputSBUS.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Device/SerialDevice.h" 4 | #include "Device/InputDevice.h" 5 | #include 6 | #include 7 | 8 | namespace Espfc { 9 | 10 | namespace Device { 11 | 12 | struct SbusData 13 | { 14 | uint8_t syncByte; 15 | // 176 bits of data (11 bits per channel * 16 channels) = 22 bytes. 16 | unsigned int chan0 : 11; 17 | unsigned int chan1 : 11; 18 | unsigned int chan2 : 11; 19 | unsigned int chan3 : 11; 20 | unsigned int chan4 : 11; 21 | unsigned int chan5 : 11; 22 | unsigned int chan6 : 11; 23 | unsigned int chan7 : 11; 24 | unsigned int chan8 : 11; 25 | unsigned int chan9 : 11; 26 | unsigned int chan10 : 11; 27 | unsigned int chan11 : 11; 28 | unsigned int chan12 : 11; 29 | unsigned int chan13 : 11; 30 | unsigned int chan14 : 11; 31 | unsigned int chan15 : 11; 32 | uint8_t flags; 33 | /** 34 | * The endByte is 0x00 on FrSky and some futaba RX's, on Some SBUS2 RX's the value indicates the telemetry byte that is sent after every 4th sbus frame. 35 | * See https://github.com/cleanflight/cleanflight/issues/590#issuecomment-101027349 36 | * and https://github.com/cleanflight/cleanflight/issues/590#issuecomment-101706023 37 | */ 38 | uint8_t endByte; 39 | } __attribute__ ((__packed__)); 40 | 41 | #define SBUS_FLAG_SIGNAL_LOSS (1 << 2) 42 | #define SBUS_FLAG_FAILSAFE_ACTIVE (1 << 3) 43 | 44 | class InputSBUS: public InputDevice 45 | { 46 | public: 47 | enum SbusState { 48 | SBUS_START, 49 | SBUS_DATA, 50 | SBUS_END 51 | }; 52 | 53 | InputSBUS(); 54 | 55 | int begin(Device::SerialDevice * serial); 56 | InputStatus update() override; 57 | uint16_t get(uint8_t i) const override; 58 | void get(uint16_t * data, size_t len) const override; 59 | size_t getChannelCount() const override; 60 | bool needAverage() const override; 61 | 62 | private: 63 | void parse(int d); 64 | void apply(); 65 | uint16_t convert(int v); 66 | 67 | static constexpr size_t SBUS_FRAME_SIZE = sizeof(SbusData); 68 | static constexpr size_t CHANNELS = 16; 69 | 70 | Device::SerialDevice * _serial; 71 | SbusState _state; 72 | uint8_t _idx = 0; 73 | bool _new_data; 74 | 75 | uint8_t _data[SBUS_FRAME_SIZE]; 76 | uint16_t _channels[CHANNELS]; 77 | uint8_t _flags; 78 | }; 79 | 80 | } 81 | 82 | } 83 | -------------------------------------------------------------------------------- /lib/Espfc/src/Device/MagDevice.cpp: -------------------------------------------------------------------------------- 1 | #include "Device/MagDevice.h" 2 | #include "Hal/Pgm.h" 3 | #include 4 | 5 | namespace Espfc::Device { 6 | 7 | const char ** MagDevice::getNames() 8 | { 9 | static const char* devChoices[] = { PSTR("AUTO"), PSTR("NONE"), PSTR("HMC5883L"), PSTR("AK8975"), PSTR("AK8963"), PSTR("QMC5883L"),NULL }; 10 | return devChoices; 11 | } 12 | 13 | const char * MagDevice::getName(DeviceType type) 14 | { 15 | if(type >= MAG_MAX) return PSTR("?"); 16 | return getNames()[type]; 17 | } 18 | 19 | } -------------------------------------------------------------------------------- /lib/Espfc/src/Device/MagDevice.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "helper_3dmath.h" 4 | #include "BusAwareDevice.h" 5 | 6 | namespace Espfc { 7 | 8 | enum MagDeviceType { 9 | MAG_DEFAULT = 0, 10 | MAG_NONE = 1, 11 | MAG_HMC5883 = 2, 12 | MAG_AK8975 = 3, 13 | MAG_AK8963 = 4, 14 | MAG_QMC5883 = 5, 15 | MAG_MAX 16 | }; 17 | 18 | namespace Device { 19 | 20 | class MagDevice: public BusAwareDevice 21 | { 22 | public: 23 | typedef MagDeviceType DeviceType; 24 | 25 | virtual int begin(BusDevice * bus) = 0; 26 | virtual int begin(BusDevice * bus, uint8_t addr) = 0; 27 | 28 | virtual DeviceType getType() const = 0; 29 | 30 | virtual int readMag(VectorInt16& v) = 0; 31 | virtual const VectorFloat convert(const VectorInt16& v) const = 0; 32 | virtual int getRate() const = 0; 33 | 34 | virtual bool testConnection() = 0; 35 | 36 | static const char ** getNames(); 37 | static const char * getName(DeviceType type); 38 | }; 39 | 40 | } 41 | 42 | } 43 | -------------------------------------------------------------------------------- /lib/Espfc/src/Device/SerialDeviceAdapter.h: -------------------------------------------------------------------------------- 1 | #ifndef _ESPFC_SERIAL_DEVICE_ADAPTER_H_ 2 | #define _ESPFC_SERIAL_DEVICE_ADAPTER_H_ 3 | 4 | #include "Device/SerialDevice.h" 5 | #ifdef ESPFC_SERIAL_SOFT_0_WIFI 6 | #include 7 | #endif 8 | 9 | namespace Espfc { 10 | 11 | namespace Device { 12 | 13 | template 14 | class SerialDeviceAdapter: public SerialDevice 15 | { 16 | public: 17 | SerialDeviceAdapter(T& dev): _dev(dev) {} 18 | void begin(const SerialDeviceConfig& conf) override { targetSerialInit(_dev, conf); } 19 | void updateBaudRate(int baud) override { _dev.updateBaudRate(baud); }; 20 | int available() override { return _dev.available(); } 21 | int read() override { return _dev.read(); } 22 | size_t readMany(uint8_t * c, size_t l) override { 23 | #if defined(ARCH_RP2040) 24 | size_t count = std::min(l, (size_t)available()); 25 | for(size_t i = 0; i < count; i++) 26 | { 27 | c[i] = read(); 28 | } 29 | return count; 30 | #else 31 | return _dev.read(c, l); 32 | #endif 33 | } 34 | int peek() override { return _dev.peek(); } 35 | void flush() override { _dev.flush(); } 36 | size_t write(uint8_t c) override { return _dev.write(c); } 37 | size_t write(const uint8_t * c, size_t l) override { return _dev.write(c, l); } 38 | bool isSoft() const override { return false; }; 39 | int availableForWrite() override { return _dev.availableForWrite(); } 40 | bool isTxFifoEmpty() override { return _dev.availableForWrite() >= SERIAL_TX_FIFO_SIZE; } 41 | operator bool() const override { return (bool)_dev; } 42 | private: 43 | T& _dev; 44 | }; 45 | 46 | // WiFiClient specializations 47 | #ifdef ESPFC_SERIAL_SOFT_0_WIFI 48 | template<> 49 | inline void SerialDeviceAdapter::begin(const SerialDeviceConfig& conf) 50 | { 51 | } 52 | 53 | template<> 54 | inline int SerialDeviceAdapter::availableForWrite() 55 | { 56 | return SERIAL_TX_FIFO_SIZE; 57 | } 58 | 59 | template<> 60 | inline bool SerialDeviceAdapter::isTxFifoEmpty() 61 | { 62 | return true; 63 | } 64 | 65 | template<> 66 | inline void SerialDeviceAdapter::updateBaudRate(int baud) {} 67 | 68 | #endif 69 | 70 | #if defined(ESP32C3) || defined(ESP32S3) 71 | template<> 72 | inline void SerialDeviceAdapter::updateBaudRate(int baud) {} 73 | #endif 74 | 75 | #if defined(ESP32S2) 76 | template<> 77 | inline void SerialDeviceAdapter::updateBaudRate(int baud) {} 78 | #endif 79 | 80 | #if defined(ARCH_RP2040) 81 | template<> 82 | inline void SerialDeviceAdapter::updateBaudRate(int baud) 83 | { 84 | _dev.begin(baud); 85 | } 86 | 87 | template<> 88 | inline void SerialDeviceAdapter::updateBaudRate(int baud) {} 89 | #endif 90 | 91 | } 92 | 93 | } 94 | 95 | #endif -------------------------------------------------------------------------------- /lib/Espfc/src/Espfc.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Model.h" 4 | #include "Hardware.h" 5 | #include "Control/Controller.h" 6 | #include "Input.h" 7 | #include "Control/Actuator.h" 8 | #include "SensorManager.h" 9 | #include "TelemetryManager.h" 10 | #include "SerialManager.h" 11 | #include "Output/Mixer.h" 12 | #include "Blackbox/Blackbox.h" 13 | #include "Connect/Buzzer.hpp" 14 | 15 | namespace Espfc { 16 | 17 | class Espfc 18 | { 19 | public: 20 | Espfc(); 21 | 22 | int load(); 23 | int begin(); 24 | int update(bool externalTrigger = false); 25 | int updateOther(); 26 | 27 | int getGyroInterval() const 28 | { 29 | return _model.state.gyro.timer.interval; 30 | } 31 | 32 | private: 33 | Model _model; 34 | Hardware _hardware; 35 | Control::Controller _controller; 36 | TelemetryManager _telemetry; 37 | Input _input; 38 | Control::Actuator _actuator; 39 | SensorManager _sensor; 40 | Output::Mixer _mixer; 41 | Blackbox::Blackbox _blackbox; 42 | Connect::Buzzer _buzzer; 43 | SerialManager _serial; 44 | uint32_t _loop_next; 45 | }; 46 | 47 | } 48 | -------------------------------------------------------------------------------- /lib/Espfc/src/Hal/Gpio.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "Gpio.h" 3 | #include "Utils/MemoryHelper.h" 4 | #if defined(ESP32) 5 | #include "hal/gpio_ll.h" 6 | #endif 7 | 8 | namespace Espfc { 9 | 10 | namespace Hal { 11 | 12 | void FAST_CODE_ATTR Gpio::digitalWrite(uint8_t pin, pin_status_t val) 13 | { 14 | #if defined(ESP8266) 15 | if (pin < 16) 16 | { 17 | if (val) 18 | GPOS = (1 << pin); 19 | else 20 | GPOC = (1 << pin); 21 | } 22 | else if (pin == 16) 23 | { 24 | if (val) 25 | GP16O |= 1; 26 | else 27 | GP16O &= ~1; 28 | } 29 | #elif defined(ESP32) 30 | //::gpio_set_level((gpio_num_t)pin, val); 31 | gpio_ll_set_level(&GPIO, (gpio_num_t)pin, val); 32 | #elif defined(UNIT_TEST) 33 | // do nothing 34 | #else 35 | ::digitalWrite(pin, val); 36 | #endif 37 | } 38 | 39 | pin_status_t FAST_CODE_ATTR Gpio::digitalRead(uint8_t pin) 40 | { 41 | #if defined(ESP8266) 42 | if (pin < 16) 43 | { 44 | return GPIP(pin); 45 | } 46 | else if (pin == 16) 47 | { 48 | return GP16I & 0x01; 49 | } 50 | return 0; 51 | #elif defined(ESP32) 52 | // return ::gpio_get_level((gpio_num_t)pin); 53 | return ::gpio_ll_get_level(&GPIO, (gpio_num_t)pin); 54 | #elif defined(UNIT_TEST) 55 | return 0; 56 | // do nothing 57 | #else 58 | return ::digitalRead(pin); 59 | #endif 60 | } 61 | 62 | void FAST_CODE_ATTR Gpio::pinMode(uint8_t pin, pin_mode_t mode) 63 | { 64 | #if defined(UNIT_TEST) 65 | return; 66 | // do nothing 67 | #else 68 | ::pinMode(pin, mode); 69 | #endif 70 | } 71 | 72 | } 73 | 74 | } 75 | -------------------------------------------------------------------------------- /lib/Espfc/src/Hal/Gpio.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #if defined(ARCH_RP2040) 7 | using pin_status_t = PinStatus; 8 | using pin_mode_t = PinMode; 9 | #else 10 | using pin_status_t = uint8_t; 11 | using pin_mode_t = uint8_t; 12 | #endif 13 | 14 | namespace Espfc { 15 | 16 | namespace Hal { 17 | 18 | class Gpio 19 | { 20 | public: 21 | static void digitalWrite(uint8_t pin, pin_status_t val); 22 | static pin_status_t digitalRead(uint8_t pin); 23 | static void pinMode(uint8_t pin, pin_mode_t mode); 24 | }; 25 | 26 | } 27 | 28 | } 29 | -------------------------------------------------------------------------------- /lib/Espfc/src/Hal/Pgm.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #ifndef UNIT_TEST 4 | 5 | #include 6 | 7 | #else 8 | 9 | #ifndef PSTR 10 | #define PSTR(s) (s) 11 | #endif 12 | 13 | #ifndef FPSTR 14 | #define FPSTR(s) (s) 15 | #endif 16 | 17 | #ifndef F 18 | #define F(s) (s) 19 | #endif 20 | 21 | #define PGM_P const char * 22 | #define pgm_read_byte(addr) (*(const unsigned char *)(addr)) 23 | 24 | #endif // UNIT_TEST 25 | 26 | #ifdef ARCH_RP2040 27 | namespace arduino { 28 | class __FlashStringHelper; 29 | } 30 | using __FlashStringHelper = arduino::__FlashStringHelper; 31 | #else 32 | class __FlashStringHelper; 33 | #endif 34 | -------------------------------------------------------------------------------- /lib/Espfc/src/Hardware.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Model.h" 4 | #if defined(ESPFC_I2C_0) 5 | #include "Device/BusI2C.h" 6 | #endif 7 | #if defined(ESPFC_SPI_0) 8 | #include "Device/BusSPI.h" 9 | #endif 10 | #include "Device/BusSlave.h" 11 | 12 | namespace Espfc { 13 | 14 | class Hardware 15 | { 16 | public: 17 | Hardware(Model& model); 18 | int begin(); 19 | void onI2CError(); 20 | 21 | void initBus(); 22 | void detectGyro(); 23 | void detectMag(); 24 | void detectBaro(); 25 | 26 | #if defined(ESPFC_SPI_0) 27 | template 28 | bool detectDevice(Dev& dev, Device::BusSPI& bus, int cs) 29 | { 30 | typename Dev::DeviceType type = dev.getType(); 31 | bool status = dev.begin(&bus, cs); 32 | _model.logger.info().log(F("SPI")).log(FPSTR(Dev::getName(type))).logln(status ? "Y" : ""); 33 | return status; 34 | } 35 | #endif 36 | 37 | #if defined(ESPFC_I2C_0) 38 | template 39 | bool detectDevice(Dev& dev, Device::BusI2C& bus) 40 | { 41 | typename Dev::DeviceType type = dev.getType(); 42 | bool status = dev.begin(&bus); 43 | _model.logger.info().log(F("I2C")).log(FPSTR(Dev::getName(type))).logln(status ? "Y" : ""); 44 | return status; 45 | } 46 | #endif 47 | 48 | template 49 | bool detectDevice(Dev& dev, Device::BusSlave& bus) 50 | { 51 | typename Dev::DeviceType type = dev.getType(); 52 | bool status = dev.begin(&bus); 53 | _model.logger.info().log(F("SLV")).log(FPSTR(Dev::getName(type))).logln(status ? "Y" : ""); 54 | return status; 55 | } 56 | 57 | static void restart(const Model& model); 58 | 59 | private: 60 | Model& _model; 61 | }; 62 | 63 | } 64 | -------------------------------------------------------------------------------- /lib/Espfc/src/Input.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Model.h" 4 | #include "Utils/Math.hpp" 5 | #include "Device/InputDevice.h" 6 | #include "Device/InputPPM.h" 7 | #include "Device/InputIBUS.hpp" 8 | #include "Device/InputSBUS.h" 9 | #include "Device/InputCRSF.h" 10 | #include "TelemetryManager.h" 11 | #if defined(ESPFC_ESPNOW) 12 | #include "Device/InputEspNow.h" 13 | #endif 14 | 15 | namespace Espfc { 16 | 17 | enum FailsafeChannelMode { 18 | FAILSAFE_MODE_AUTO, 19 | FAILSAFE_MODE_HOLD, 20 | FAILSAFE_MODE_SET, 21 | FAILSAFE_MODE_INVALID 22 | }; 23 | 24 | enum InputPwmRange { 25 | PWM_RANGE_MIN = 1000, 26 | PWM_RANGE_MID = 1500, 27 | PWM_RANGE_MAX = 2000 28 | }; 29 | 30 | class Input 31 | { 32 | public: 33 | Input(Model& model, TelemetryManager& telemetry); 34 | 35 | int begin(); 36 | int update(); 37 | 38 | int16_t getFailsafeValue(uint8_t c); 39 | void setInput(Axis i, float v, bool newFrame, bool noFilter = false); 40 | 41 | InputStatus readInputs(); 42 | void processInputs(); 43 | 44 | bool failsafe(InputStatus status); 45 | void failsafeIdle(); 46 | void failsafeStage1(); 47 | void failsafeStage2(); 48 | void filterInputs(InputStatus status); 49 | 50 | void updateFrameRate(); 51 | Device::InputDevice * getInputDevice(); 52 | 53 | private: 54 | inline float _interpolate(float left, float right, float step) 55 | { 56 | return (left * (1.f - step) + right * step); 57 | } 58 | 59 | Model& _model; 60 | TelemetryManager& _telemetry; 61 | Device::InputDevice * _device; 62 | Utils::Filter _filter[INPUT_CHANNELS]; 63 | float _step; 64 | Device::InputPPM _ppm; 65 | Device::InputIBUS _ibus; 66 | Device::InputSBUS _sbus; 67 | Device::InputCRSF _crsf; 68 | #if defined(ESPFC_ESPNOW) 69 | Device::InputEspNow _espnow; 70 | #endif 71 | 72 | static constexpr uint32_t TENTH_TO_US = 100000UL; // 1_000_000 / 10; 73 | static constexpr uint32_t FRAME_TIME_DEFAULT_US = 23000; // 23 ms 74 | }; 75 | 76 | } 77 | -------------------------------------------------------------------------------- /lib/Espfc/src/Output/Mixer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Model.h" 4 | #include "EscDriver.h" 5 | 6 | namespace Espfc { 7 | 8 | namespace Output { 9 | 10 | class Mixer 11 | { 12 | public: 13 | Mixer(Model& model); 14 | int begin(); 15 | int update(); 16 | 17 | void updateMixer(const MixerConfig& mixer, float * outputs); 18 | float limitThrust(float thrust, ThrottleLimitType type, int8_t limit); 19 | float limitOutput(float output, const OutputChannelConfig& occ, int limit); 20 | void writeOutput(const MixerConfig& mixer, float * out); 21 | void readTelemetry(); 22 | float inline erpmToHz(float erpm); 23 | float inline erpmToRpm(float erpm); 24 | bool inline _stop(void); 25 | 26 | private: 27 | Model& _model; 28 | EscDriver * _motor; 29 | EscDriver * _servo; 30 | 31 | EscDriver escMotor; 32 | EscDriver escServo; 33 | uint32_t _statsCounter; 34 | uint32_t _statsCounterMax; 35 | float _erpmToHz; 36 | }; 37 | 38 | } 39 | 40 | } 41 | -------------------------------------------------------------------------------- /lib/Espfc/src/Output/OutputIBUS.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Device/SerialDevice.h" 4 | #include "Utils/Timer.h" 5 | 6 | namespace Espfc::Output { 7 | 8 | class OutputIBUS 9 | { 10 | public: 11 | OutputIBUS() {} 12 | 13 | int begin(Device::SerialDevice* serial) 14 | { 15 | _serial = serial; 16 | _timer.setInterval(7000); // 7ms 17 | 18 | return 1; 19 | } 20 | 21 | int update() 22 | { 23 | if(!_timer.check()) return 0; 24 | 25 | // const uint8_t data[] = { 26 | // 0x20, 0x40, // preambule (len, cmd) 27 | // 0xDC, 0x05, 0xDC, 0x05, 0xBE, 0x05, 0xDC, 0x05, // channel 1-4 28 | // 0xD0, 0x07, 0xD0, 0x07, 0xDC, 0x05, 0xDC, 0x05, // channel 5-8 29 | // 0xDC, 0x05, 0xDC, 0x05, 0xDC, 0x05, 0xDC, 0x05, // channel 9-12 30 | // 0xDC, 0x05, 0xDC, 0x05, // channel 13-14 31 | // 0x83, 0xF3 // checksum 32 | // }; 33 | 34 | const uint8_t data[] = { 35 | 0x20, 0x40, 36 | 0xDB, 0x05, 0xDC, 0x05, 0x54, 0x05, 0xDC, 0x05, 0xE8, 0x03, 0xD0, 0x07, 0xD2, 0x05, 0xE8, 0x03, 37 | 0xDC, 0x05, 0xDC, 0x05, 0xDC, 0x05, 0xDC, 0x05, 0xDC, 0x05, 0xDC, 0x05, 38 | 0xDA, 0xF3, 39 | }; 40 | 41 | _serial->write(data, sizeof(data)); 42 | 43 | return 1; 44 | } 45 | 46 | private: 47 | Device::SerialDevice* _serial; 48 | Utils::Timer _timer; 49 | }; 50 | 51 | } 52 | -------------------------------------------------------------------------------- /lib/Espfc/src/Sensor/AccelSensor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Model.h" 4 | #include "BaseSensor.h" 5 | #include "Device/GyroDevice.h" 6 | 7 | namespace Espfc { 8 | 9 | namespace Sensor { 10 | 11 | class AccelSensor: public BaseSensor 12 | { 13 | public: 14 | AccelSensor(Model& model); 15 | 16 | int begin(); 17 | int update(); 18 | int read(); 19 | int filter(); 20 | 21 | private: 22 | void calibrate(); 23 | 24 | Model& _model; 25 | Device::GyroDevice * _gyro; 26 | Utils::Filter _filter[AXIS_COUNT_RPY]; 27 | }; 28 | 29 | } 30 | 31 | } 32 | -------------------------------------------------------------------------------- /lib/Espfc/src/Sensor/BaroSensor.h: -------------------------------------------------------------------------------- 1 | #ifndef _ESPFC_SENSOR_BARO_SENSOR_H_ 2 | #define _ESPFC_SENSOR_BARO_SENSOR_H_ 3 | 4 | #include "BaseSensor.h" 5 | #include "Model.h" 6 | #include "Device/BaroDevice.h" 7 | #include "Utils/Filter.h" 8 | 9 | namespace Espfc { 10 | 11 | namespace Sensor { 12 | 13 | class BaroSensor: public BaseSensor 14 | { 15 | public: 16 | enum BaroState 17 | { 18 | BARO_STATE_INIT, 19 | BARO_STATE_TEMP_GET, 20 | BARO_STATE_PRESS_GET, 21 | }; 22 | 23 | BaroSensor(Model& model); 24 | 25 | int begin(); 26 | int update(); 27 | int read(); 28 | 29 | private: 30 | void readTemperature(); 31 | void readPressure(); 32 | void updateAltitude(); 33 | 34 | Model& _model; 35 | Device::BaroDevice * _baro; 36 | BaroState _state; 37 | Utils::Filter _temperatureFilter; 38 | Utils::Filter _pressureFilter; 39 | Utils::Filter _altitudeFilter; 40 | uint32_t _wait; 41 | int32_t _counter; 42 | }; 43 | 44 | } 45 | 46 | } 47 | #endif -------------------------------------------------------------------------------- /lib/Espfc/src/Sensor/BaseSensor.cpp: -------------------------------------------------------------------------------- 1 | #include "BaseSensor.h" 2 | #include "ModelConfig.h" 3 | #include "Utils/MemoryHelper.h" 4 | 5 | namespace Espfc { 6 | 7 | namespace Sensor { 8 | 9 | void FAST_CODE_ATTR BaseSensor::align(VectorFloat& dest, uint8_t rotation) 10 | { 11 | const float x = dest.x; 12 | const float y = dest.y; 13 | const float z = dest.z; 14 | 15 | switch(rotation) 16 | { 17 | default: 18 | case ALIGN_CW0_DEG: 19 | dest.x = x; 20 | dest.y = y; 21 | dest.z = z; 22 | break; 23 | case ALIGN_CW90_DEG: 24 | dest.x = y; 25 | dest.y = -x; 26 | dest.z = z; 27 | break; 28 | case ALIGN_CW180_DEG: 29 | dest.x = -x; 30 | dest.y = -y; 31 | dest.z = z; 32 | break; 33 | case ALIGN_CW270_DEG: 34 | dest.x = -y; 35 | dest.y = x; 36 | dest.z = z; 37 | break; 38 | case ALIGN_CW0_DEG_FLIP: 39 | dest.x = -x; 40 | dest.y = y; 41 | dest.z = -z; 42 | break; 43 | case ALIGN_CW90_DEG_FLIP: 44 | dest.x = y; 45 | dest.y = x; 46 | dest.z = -z; 47 | break; 48 | case ALIGN_CW180_DEG_FLIP: 49 | dest.x = x; 50 | dest.y = -y; 51 | dest.z = -z; 52 | break; 53 | case ALIGN_CW270_DEG_FLIP: 54 | dest.x = -y; 55 | dest.y = -x; 56 | dest.z = -z; 57 | break; 58 | } 59 | } 60 | 61 | void FAST_CODE_ATTR BaseSensor::toVector(VectorInt16& v, uint8_t * buf) 62 | { 63 | v.x = (int16_t)(((uint16_t)buf[0] << 8) | (uint16_t)buf[1]); 64 | v.y = (int16_t)(((uint16_t)buf[2] << 8) | (uint16_t)buf[3]); 65 | v.z = (int16_t)(((uint16_t)buf[4] << 8) | (uint16_t)buf[5]); 66 | } 67 | 68 | } 69 | 70 | } 71 | -------------------------------------------------------------------------------- /lib/Espfc/src/Sensor/BaseSensor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace Espfc::Sensor { 7 | 8 | class BaseSensor 9 | { 10 | public: 11 | void align(VectorFloat& dest, uint8_t rotation); 12 | void toVector(VectorInt16& v, uint8_t * buf); 13 | }; 14 | 15 | } 16 | -------------------------------------------------------------------------------- /lib/Espfc/src/Sensor/GpsSensor.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Model.h" 4 | #include "Device/SerialDevice.h" 5 | #include "Utils/Timer.h" 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace Espfc::Sensor { 12 | 13 | class GpsSensor 14 | { 15 | public: 16 | GpsSensor(Model& model); 17 | 18 | int begin(Device::SerialDevice* port, int baud); 19 | 20 | int update(); 21 | 22 | private: 23 | enum State { 24 | DETECT_BAUD, 25 | SET_BAUD, 26 | DISABLE_NMEA, 27 | GET_VERSION, 28 | ENABLE_UBX, 29 | ENABLE_NAV5, 30 | ENABLE_SBAS, 31 | SET_RATE, 32 | WAIT, 33 | RECEIVE, 34 | ERROR, 35 | }; 36 | 37 | void handle(); 38 | 39 | bool processUbx(uint8_t c); 40 | void processNmea(uint8_t c); 41 | void setBaud(int baud); 42 | 43 | void onMessage(); 44 | 45 | template 46 | void send(const MsgType m, State ackState, State timeoutState = ERROR) 47 | { 48 | Gps::UbxFrame frame{m}; 49 | const uint8_t* ptr = reinterpret_cast(&frame); 50 | _port->write(ptr, sizeof(frame)); 51 | 52 | setState(WAIT, ackState, timeoutState); 53 | } 54 | 55 | void setState(State state, State ackState, State timeoutState); 56 | 57 | void setState(State state); 58 | 59 | void handleError() const; 60 | void handleNavPvt() const; 61 | void handleNavSat() const; 62 | void handleVersion() const; 63 | void checkSupport(const char* payload) const; 64 | 65 | static constexpr uint32_t TIMEOUT = 300000; 66 | static constexpr uint32_t DETECT_TIMEOUT = 2200000; 67 | 68 | Model& _model; 69 | 70 | State _state = WAIT; 71 | State _ackState = WAIT; 72 | State _timeoutState = DETECT_BAUD; 73 | size_t _counter = 0; 74 | uint32_t _timeout = 0; 75 | int _currentBaud = 0; 76 | int _targetBaud = 0; 77 | 78 | Gps::UbxParser _ubxParser; 79 | Gps::UbxMessage _ubxMsg; 80 | 81 | Gps::NmeaParser _nmeaParser; 82 | Gps::NmeaMessage _nmeaMsg; 83 | 84 | Device::SerialDevice* _port; 85 | Utils::Timer _timer; 86 | }; 87 | 88 | } 89 | -------------------------------------------------------------------------------- /lib/Espfc/src/Sensor/GyroSensor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "BaseSensor.h" 4 | #include "Model.h" 5 | #include "Device/GyroDevice.h" 6 | #include "Utils/Sma.hpp" 7 | #ifdef ESPFC_DSP 8 | #include "Utils/FFTAnalyzer.hpp" 9 | #else 10 | #include "Utils/FreqAnalyzer.hpp" 11 | #endif 12 | 13 | #define ESPFC_FUZZY_ACCEL_ZERO 0.05 14 | #define ESPFC_FUZZY_GYRO_ZERO 0.20 15 | 16 | namespace Espfc { 17 | 18 | namespace Sensor { 19 | 20 | class GyroSensor: public BaseSensor 21 | { 22 | public: 23 | GyroSensor(Model& model); 24 | ~GyroSensor(); 25 | 26 | int begin(); 27 | int read(); 28 | int filter(); 29 | void postLoop(); 30 | void rpmFilterUpdate(); 31 | void dynNotchFilterUpdate(); 32 | 33 | private: 34 | void calibrate(); 35 | 36 | Utils::Sma _sma; 37 | Utils::Sma _dyn_notch_sma; 38 | size_t _dyn_notch_denom; 39 | size_t _dyn_notch_count; 40 | bool _dyn_notch_enabled; 41 | bool _dyn_notch_debug; 42 | bool _rpm_enabled; 43 | size_t _rpm_motor_index; 44 | float _rpm_weights[3]; 45 | float _rpm_fade_inv; 46 | float _rpm_min_freq; 47 | float _rpm_max_freq; 48 | float _rpm_q; 49 | 50 | Model& _model; 51 | Device::GyroDevice * _gyro; 52 | 53 | #ifdef ESPFC_DSP 54 | Utils::FFTAnalyzer<128> _fft[3]; 55 | #else 56 | Utils::FreqAnalyzer _freqAnalyzer[3]; 57 | #endif 58 | 59 | }; 60 | 61 | } 62 | 63 | } 64 | -------------------------------------------------------------------------------- /lib/Espfc/src/Sensor/MagSensor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Model.h" 4 | #include "BaseSensor.h" 5 | #include "Device/MagDevice.h" 6 | 7 | namespace Espfc { 8 | 9 | namespace Sensor { 10 | 11 | class MagSensor: public BaseSensor 12 | { 13 | public: 14 | MagSensor(Model& model); 15 | 16 | int begin(); 17 | int update(); 18 | int read(); 19 | int filter(); 20 | 21 | private: 22 | void calibrate(); 23 | void resetCalibration(); 24 | void updateCalibration(); 25 | void applyCalibration(); 26 | 27 | Model& _model; 28 | Device::MagDevice * _mag; 29 | }; 30 | 31 | } 32 | 33 | } 34 | -------------------------------------------------------------------------------- /lib/Espfc/src/Sensor/VoltageSensor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Model.h" 4 | #include "BaseSensor.h" 5 | #include "Utils/Filter.h" 6 | 7 | namespace Espfc { 8 | 9 | namespace Sensor { 10 | 11 | class VoltageSensor: public BaseSensor 12 | { 13 | public: 14 | enum State { 15 | VBAT, 16 | IBAT 17 | }; 18 | VoltageSensor(Model& model); 19 | int begin(); 20 | int update(); 21 | int readVbat(); 22 | int readIbat(); 23 | 24 | private: 25 | Model& _model; 26 | Utils::Filter _vFilterFast; 27 | Utils::Filter _vFilter; 28 | Utils::Filter _iFilterFast; 29 | Utils::Filter _iFilter; 30 | State _state; 31 | }; 32 | 33 | } 34 | 35 | } 36 | -------------------------------------------------------------------------------- /lib/Espfc/src/SensorManager.cpp: -------------------------------------------------------------------------------- 1 | #include "SensorManager.h" 2 | 3 | namespace Espfc { 4 | 5 | SensorManager::SensorManager(Model& model): _model(model), _gyro(model), _accel(model), _mag(model), _baro(model), _voltage(model), _fusion(model), _fusionUpdate(false) {} 6 | 7 | int SensorManager::begin() 8 | { 9 | _gyro.begin(); 10 | _accel.begin(); 11 | _mag.begin(); 12 | _baro.begin(); 13 | _voltage.begin(); 14 | _fusion.begin(); 15 | _button.begin(_model.config.pin[PIN_BUTTON]); 16 | 17 | return 1; 18 | } 19 | 20 | int FAST_CODE_ATTR SensorManager::read() 21 | { 22 | _gyro.read(); 23 | 24 | if(_model.state.loopTimer.syncTo(_model.state.gyro.timer)) 25 | { 26 | _model.state.appQueue.send(Event(EVENT_GYRO_READ)); 27 | } 28 | 29 | if(_model.state.accel.timer.syncTo(_model.state.gyro.timer)) 30 | { 31 | _accel.update(); 32 | _model.state.appQueue.send(Event(EVENT_ACCEL_READ)); 33 | _model.state.mode.button = _button.update(); 34 | return 1; 35 | } 36 | 37 | if(_mag.update()) return 1; 38 | 39 | if(_baro.update()) return 1; 40 | 41 | if(_voltage.update()) return 1; 42 | 43 | return 0; 44 | } 45 | 46 | int FAST_CODE_ATTR SensorManager::preLoop() 47 | { 48 | _gyro.filter(); 49 | if(_model.state.gyro.biasSamples == 0) 50 | { 51 | _model.state.gyro.biasSamples = -1; 52 | _fusion.restoreGain(); 53 | } 54 | return 1; 55 | } 56 | 57 | int SensorManager::postLoop() 58 | { 59 | _gyro.postLoop(); 60 | return 1; 61 | } 62 | 63 | int FAST_CODE_ATTR SensorManager::fusion() 64 | { 65 | return _fusion.update(); 66 | } 67 | 68 | // main task 69 | int FAST_CODE_ATTR SensorManager::update() 70 | { 71 | _gyro.read(); 72 | return preLoop(); 73 | } 74 | 75 | // sub task 76 | int SensorManager::updateDelayed() 77 | { 78 | _gyro.postLoop(); 79 | 80 | // update at most one sensor besides gyro 81 | int status = 0; 82 | if(_model.state.accel.timer.syncTo(_model.state.gyro.timer)) 83 | { 84 | _accel.update(); 85 | _model.state.mode.button = _button.update(); 86 | status = 1; 87 | } 88 | 89 | // delay imu update to next cycle 90 | if(_fusionUpdate) 91 | { 92 | _fusionUpdate = false; 93 | _fusion.update(); 94 | } 95 | _fusionUpdate = status; 96 | 97 | if(status) return 1; 98 | 99 | if(_mag.update()) return 1; 100 | 101 | if(_baro.update()) return 1; 102 | 103 | if(_voltage.update()) return 0; 104 | 105 | return 0; 106 | } 107 | 108 | } 109 | -------------------------------------------------------------------------------- /lib/Espfc/src/SensorManager.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Model.h" 4 | #include "Control/Fusion.h" 5 | #include "Sensor/GyroSensor.h" 6 | #include "Sensor/AccelSensor.h" 7 | #include "Sensor/MagSensor.h" 8 | #include "Sensor/BaroSensor.h" 9 | #include "Sensor/VoltageSensor.h" 10 | #include "Device/Input/InputButton.hpp" 11 | 12 | namespace Espfc { 13 | 14 | class SensorManager 15 | { 16 | public: 17 | SensorManager(Model& model); 18 | 19 | int begin(); 20 | int read(); 21 | int preLoop(); 22 | int postLoop(); 23 | int fusion(); 24 | // main task 25 | int update(); 26 | // sub task 27 | int updateDelayed(); 28 | 29 | private: 30 | Model& _model; 31 | Sensor::GyroSensor _gyro; 32 | Sensor::AccelSensor _accel; 33 | Sensor::MagSensor _mag; 34 | Sensor::BaroSensor _baro; 35 | Sensor::VoltageSensor _voltage; 36 | Control::Fusion _fusion; 37 | bool _fusionUpdate; 38 | Device::Input::InputButton _button; 39 | }; 40 | 41 | } 42 | -------------------------------------------------------------------------------- /lib/Espfc/src/SerialManager.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Model.h" 4 | #include "Device/SerialDevice.h" 5 | #include "Connect/MspProcessor.hpp" 6 | #include "Connect/Vtx.hpp" 7 | #include "Connect/Cli.hpp" 8 | #include "TelemetryManager.h" 9 | #include "Output/OutputIBUS.hpp" 10 | #include "Sensor/GpsSensor.hpp" 11 | #ifdef ESPFC_SERIAL_SOFT_0_WIFI 12 | #include "Wireless.h" 13 | #endif 14 | 15 | namespace Espfc { 16 | 17 | class SerialManager 18 | { 19 | public: 20 | SerialManager(Model& model, TelemetryManager& telemetry); 21 | 22 | int begin(); 23 | int update(); 24 | 25 | private: 26 | static Device::SerialDevice * getSerialPortById(SerialPort portId); 27 | void processMsp(SerialPortState& ss); 28 | 29 | void next() 30 | { 31 | _current++; 32 | if(_current >= SERIAL_UART_COUNT) _current = 0; 33 | } 34 | 35 | Model& _model; 36 | size_t _current; 37 | 38 | Connect::MspProcessor _msp; 39 | Connect::Cli _cli; 40 | Connect::Vtx _vtx; 41 | TelemetryManager& _telemetry; 42 | Output::OutputIBUS _ibus; 43 | Sensor::GpsSensor _gps; 44 | #ifdef ESPFC_SERIAL_SOFT_0_WIFI 45 | Wireless _wireless; 46 | #endif 47 | }; 48 | 49 | } 50 | -------------------------------------------------------------------------------- /lib/Espfc/src/Target/Queue.cpp: -------------------------------------------------------------------------------- 1 | #include "Target.h" 2 | 3 | #if defined(UNIT_TEST) || !defined(ESPFC_MULTI_CORE) 4 | 5 | #include "Queue.h" 6 | 7 | namespace Espfc { 8 | 9 | namespace Target { 10 | 11 | void Queue::begin() {} 12 | 13 | void FAST_CODE_ATTR Queue::send(const Event& e) { (void)e; } 14 | 15 | Event FAST_CODE_ATTR Queue::receive() { return Event(); } 16 | 17 | bool FAST_CODE_ATTR Queue::isEmpty() const { return true; } 18 | 19 | bool FAST_CODE_ATTR Queue::isFull() const { return false; } 20 | 21 | } 22 | 23 | } 24 | 25 | #endif -------------------------------------------------------------------------------- /lib/Espfc/src/Target/Queue.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // https://www.raspberrypi.com/documentation/pico-sdk/high_level.html#queue 4 | // https://techtutorialsx.com/2017/08/20/esp32-arduino-freertos-queues/ 5 | // https://www.freertos.org/a00116.html 6 | 7 | namespace Espfc { 8 | 9 | enum EventType 10 | { 11 | EVENT_IDLE, 12 | EVENT_GYRO_READ, 13 | EVENT_ACCEL_READ, 14 | EVENT_DISARM, 15 | }; 16 | 17 | class Event 18 | { 19 | public: 20 | Event(): type(EVENT_IDLE) {} 21 | Event(EventType t): type(t) {} 22 | Event(const Event& e) = default; 23 | public: 24 | EventType type; 25 | }; 26 | 27 | } 28 | 29 | #if defined(ARCH_RP2040) 30 | #include 31 | using TargetQueueHandle = queue_t; 32 | #elif defined(ESPFC_FREE_RTOS_QUEUE) 33 | #include 34 | using TargetQueueHandle = QueueHandle_t; 35 | #elif defined(ESPFC_ATOMIC_QUEUE) 36 | #include "QueueAtomic.h" 37 | using TargetQueueHandle = Espfc::QueueAtomic; 38 | #elif defined(UNIT_TEST) || !defined(ESPFC_MULTI_CORE) 39 | using TargetQueueHandle = int; 40 | #else 41 | #error "Not yet implelented multicore queue" 42 | #endif 43 | 44 | namespace Espfc { 45 | 46 | namespace Target { 47 | 48 | class Queue 49 | { 50 | public: 51 | void begin(); 52 | void send(const Event& e); 53 | Event receive(); 54 | bool isEmpty() const; 55 | bool isFull() const; 56 | 57 | private: 58 | TargetQueueHandle _q; 59 | }; 60 | 61 | } 62 | 63 | } 64 | -------------------------------------------------------------------------------- /lib/Espfc/src/Target/QueueAtomic.cpp: -------------------------------------------------------------------------------- 1 | #include "Target.h" 2 | 3 | #if defined(ESPFC_ATOMIC_QUEUE) 4 | 5 | #include "Queue.h" 6 | 7 | namespace Espfc { 8 | 9 | namespace Target { 10 | 11 | void Queue::begin() 12 | { 13 | } 14 | 15 | void FAST_CODE_ATTR Queue::send(const Event& e) 16 | { 17 | if(isFull()) return; 18 | _q.push(e); 19 | } 20 | 21 | Event FAST_CODE_ATTR Queue::receive() 22 | { 23 | Event e; 24 | _q.pop(e); 25 | return e; 26 | } 27 | 28 | bool FAST_CODE_ATTR Queue::isEmpty() const 29 | { 30 | return _q.isEmpty(); 31 | } 32 | 33 | bool FAST_CODE_ATTR Queue::isFull() const 34 | { 35 | return _q.isFull(); 36 | } 37 | 38 | } 39 | 40 | } 41 | 42 | #endif 43 | -------------------------------------------------------------------------------- /lib/Espfc/src/Target/QueueAtomic.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #if defined(ESPFC_ATOMIC_QUEUE) || defined(UNIT_TEST) 4 | 5 | #include 6 | #include 7 | 8 | namespace Espfc { 9 | 10 | template 11 | class QueueAtomic 12 | { 13 | public: 14 | enum { Capacity = Size + 1 }; 15 | 16 | QueueAtomic(): _tail(0), _head(0) {} 17 | ~QueueAtomic() {} 18 | 19 | bool push(const Element& item) 20 | { 21 | auto current_tail = _tail.load(); 22 | auto next_tail = increment(current_tail); 23 | if(next_tail != _head.load()) 24 | { 25 | _array[current_tail] = item; 26 | _tail.store(next_tail); 27 | return true; 28 | } 29 | return false; // full queue 30 | } 31 | 32 | bool pop(Element& item) 33 | { 34 | const auto current_head = _head.load(); 35 | if(current_head == _tail.load()) return false; // empty queue 36 | 37 | item = _array[current_head]; 38 | _head.store(increment(current_head)); 39 | return true; 40 | } 41 | 42 | bool isEmpty() const { return (_head.load() == _tail.load()); } 43 | 44 | bool isFull() const 45 | { 46 | const auto next_tail = increment(_tail.load()); 47 | return (next_tail == _head.load()); 48 | } 49 | bool isLockFree() const { return std::atomic{}.is_lock_free(); } 50 | 51 | private: 52 | size_t increment(size_t idx) const { return (idx + 1) % Capacity; } 53 | 54 | std::atomic _tail; 55 | std::atomic _head; 56 | Element _array[Capacity]; 57 | }; 58 | 59 | } 60 | 61 | #endif 62 | -------------------------------------------------------------------------------- /lib/Espfc/src/Target/QueueFreeRTOS.cpp: -------------------------------------------------------------------------------- 1 | #include "Target.h" 2 | 3 | #ifdef ESPFC_FREE_RTOS_QUEUE 4 | 5 | #include "Queue.h" 6 | 7 | namespace Espfc { 8 | 9 | namespace Target { 10 | 11 | void Queue::begin() 12 | { 13 | _q = xQueueCreate(64, sizeof(Event)); 14 | } 15 | 16 | void Queue::send(const Event& e) 17 | { 18 | if(isFull()) return; 19 | xQueueSend(_q, &e, (TickType_t)0); 20 | } 21 | 22 | Event Queue::receive() 23 | { 24 | Event e; 25 | xQueueReceive(_q, &e, portMAX_DELAY); 26 | return e; 27 | } 28 | 29 | bool Queue::isEmpty() const 30 | { 31 | return uxQueueMessagesWaiting(_q) == 0; 32 | } 33 | 34 | bool Queue::isFull() const 35 | { 36 | return uxQueueMessagesWaiting(_q) == 64; 37 | } 38 | 39 | } 40 | 41 | } 42 | 43 | #endif 44 | -------------------------------------------------------------------------------- /lib/Espfc/src/Target/QueueRP2040.cpp: -------------------------------------------------------------------------------- 1 | #include "Target.h" 2 | 3 | #if defined(ARCH_RP2040) 4 | 5 | #include "Queue.h" 6 | 7 | namespace Espfc { 8 | 9 | namespace Target { 10 | 11 | void Queue::begin() 12 | { 13 | queue_init(&_q, sizeof(Event), 128); 14 | } 15 | 16 | void Queue::send(const Event& e) 17 | { 18 | if(isFull()) return; 19 | //Serial1.write((uint8_t)e.type); 20 | queue_add_blocking(&_q, &e); 21 | } 22 | 23 | Event Queue::receive() 24 | { 25 | Event e; 26 | queue_remove_blocking(&_q, &e); 27 | return e; 28 | } 29 | 30 | bool Queue::isEmpty() const 31 | { 32 | return queue_is_empty(const_cast(&_q)); 33 | } 34 | 35 | bool Queue::isFull() const 36 | { 37 | return queue_is_full(const_cast(&_q)); 38 | } 39 | 40 | } 41 | 42 | } 43 | 44 | #endif 45 | -------------------------------------------------------------------------------- /lib/Espfc/src/Target/Target.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #if defined(ESP32C3) 4 | #include "TargetESP32c3.h" 5 | #elif defined(ESP32S2) 6 | #include "TargetESP32s2.h" 7 | #elif defined(ESP32S3) 8 | #include "TargetESP32s3.h" 9 | #elif defined(ESP32) 10 | #include "TargetESP32.h" 11 | #elif defined(ESP8266) 12 | #include "TargetESP8266.h" 13 | #elif defined(ARCH_RP2040) 14 | #include "TargetRP2040.h" 15 | #elif defined(UNIT_TEST) 16 | #include "TargetUnitTest.h" 17 | #else 18 | #error "Unsupported platform!" 19 | #endif 20 | 21 | #include "Queue.h" 22 | #include "Utils/MemoryHelper.h" 23 | 24 | #if defined(ESPFC_I2C_0) 25 | #if defined(ESPFC_I2C_0_SOFT) 26 | #include "EspWire.h" 27 | #define WireClass EspTwoWire 28 | #define WireInstance EspWire 29 | #else 30 | #include 31 | #define WireClass TwoWire 32 | #define WireInstance Wire 33 | #endif 34 | #if defined(NO_GLOBAL_INSTANCES) || defined(NO_GLOBAL_TWOWIRE) 35 | WireClass WireInstance; 36 | #endif 37 | #endif 38 | 39 | #if defined(ESPFC_SPI_0) 40 | #include 41 | #if !defined(ESPFC_SPI_0_DEV) 42 | #define ESPFC_SPI_0_DEV SPI 43 | #endif 44 | 45 | #if !defined(ESPFC_SPI_0_DEV_T) 46 | #define ESPFC_SPI_0_DEV_T SPIClass 47 | #endif 48 | #if defined(NO_GLOBAL_INSTANCES) || defined(NO_GLOBAL_SPI) 49 | #if !defined(ARCH_RP2040) 50 | ESPFC_SPI_0_DEV_T ESPFC_SPI_0_DEV; 51 | #endif 52 | #endif 53 | #endif 54 | 55 | namespace Espfc { 56 | 57 | enum SerialPort { 58 | #ifdef ESPFC_SERIAL_USB 59 | SERIAL_USB, 60 | #endif 61 | #ifdef ESPFC_SERIAL_0 62 | SERIAL_UART_0, 63 | #endif 64 | #ifdef ESPFC_SERIAL_1 65 | SERIAL_UART_1, 66 | #endif 67 | #ifdef ESPFC_SERIAL_2 68 | SERIAL_UART_2, 69 | #endif 70 | #ifdef ESPFC_SERIAL_SOFT_0 71 | SERIAL_SOFT_0, 72 | #endif 73 | SERIAL_UART_COUNT 74 | }; 75 | 76 | } 77 | -------------------------------------------------------------------------------- /lib/Espfc/src/Target/TargetESP32.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #define ESPFC_INPUT 4 | #define ESPFC_INPUT_PIN 35 // ppm 5 | 6 | #define ESPFC_OUTPUT_COUNT 8 7 | #define ESPFC_OUTPUT_0 27 8 | #define ESPFC_OUTPUT_1 25 9 | #define ESPFC_OUTPUT_2 4 10 | #define ESPFC_OUTPUT_3 12 11 | #define ESPFC_OUTPUT_4 -1 12 | #define ESPFC_OUTPUT_5 -1 13 | #define ESPFC_OUTPUT_6 -1 14 | #define ESPFC_OUTPUT_7 -1 15 | 16 | #define ESPFC_SERIAL_0 17 | #define ESPFC_SERIAL_0_DEV Serial 18 | #define ESPFC_SERIAL_0_DEV_T HardwareSerial 19 | #define ESPFC_SERIAL_0_TX 1 20 | #define ESPFC_SERIAL_0_RX 3 21 | #define ESPFC_SERIAL_0_FN (SERIAL_FUNCTION_MSP) 22 | #define ESPFC_SERIAL_0_BAUD (SERIAL_SPEED_115200) 23 | #define ESPFC_SERIAL_0_BBAUD (SERIAL_SPEED_NONE) 24 | 25 | #define ESPFC_SERIAL_1 26 | #define ESPFC_SERIAL_1_DEV Serial1 27 | #define ESPFC_SERIAL_1_DEV_T HardwareSerial 28 | #define ESPFC_SERIAL_1_TX 33 29 | #define ESPFC_SERIAL_1_RX 32 30 | #define ESPFC_SERIAL_1_FN (SERIAL_FUNCTION_MSP) 31 | #define ESPFC_SERIAL_1_BAUD (SERIAL_SPEED_115200) 32 | #define ESPFC_SERIAL_1_BBAUD (SERIAL_SPEED_NONE) 33 | 34 | #define ESPFC_SERIAL_2 35 | #define ESPFC_SERIAL_2_DEV Serial2 36 | #define ESPFC_SERIAL_2_DEV_T HardwareSerial 37 | #define ESPFC_SERIAL_2_TX 17 38 | #define ESPFC_SERIAL_2_RX 16 39 | #define ESPFC_SERIAL_2_FN (SERIAL_FUNCTION_RX_SERIAL) 40 | #define ESPFC_SERIAL_2_BAUD (SERIAL_SPEED_115200) 41 | #define ESPFC_SERIAL_2_BBAUD (SERIAL_SPEED_NONE) 42 | 43 | #define ESPFC_SERIAL_SOFT_0 44 | #define ESPFC_SERIAL_SOFT_0_FN (SERIAL_FUNCTION_MSP) 45 | #define ESPFC_SERIAL_SOFT_0_WIFI 46 | 47 | #define ESPFC_SERIAL_REMAP_PINS 48 | #define ESPFC_SERIAL_DEBUG_PORT SERIAL_UART_0 49 | #define SERIAL_TX_FIFO_SIZE 0xFF 50 | 51 | #define ESPFC_SPI_0 52 | #define ESPFC_SPI_0_DEV SPI1 53 | #define ESPFC_SPI_0_SCK 18 54 | #define ESPFC_SPI_0_MOSI 23 55 | #define ESPFC_SPI_0_MISO 19 56 | 57 | #define ESPFC_SPI_CS_GYRO 5 58 | #define ESPFC_SPI_CS_BARO 13 59 | 60 | #define ESPFC_I2C_0 61 | #define ESPFC_I2C_0_SCL 22 62 | #define ESPFC_I2C_0_SDA 21 63 | #define ESPFC_I2C_0_SOFT 64 | 65 | #define ESPFC_BUZZER_PIN 26 66 | #define ESPFC_BUTTON_PIN 0 67 | #define ESPFC_LED_PIN 2 68 | 69 | #define ESPFC_ADC_0 70 | #define ESPFC_ADC_0_PIN 36 71 | 72 | #define ESPFC_ADC_1 73 | #define ESPFC_ADC_1_PIN 39 74 | 75 | #define ESPFC_ADC_SCALE (3.3f / 4096) 76 | 77 | #define ESPFC_FEATURE_MASK (FEATURE_RX_SERIAL | FEATURE_DYNAMIC_FILTER) 78 | 79 | #define ESPFC_GYRO_I2C_RATE_MAX 2000 80 | #define ESPFC_GYRO_SPI_RATE_MAX 4000 81 | 82 | #define ESPFC_DSHOT_TELEMETRY 83 | 84 | #define ESPFC_FREE_RTOS 85 | #ifndef CONFIG_FREERTOS_UNICORE 86 | #define ESPFC_MULTI_CORE 87 | #endif 88 | 89 | //#define ESPFC_FREE_RTOS_QUEUE 90 | #define ESPFC_ATOMIC_QUEUE 91 | 92 | #define ESPFC_DSP 93 | 94 | #include "./TargetEsp32Common.h" 95 | -------------------------------------------------------------------------------- /lib/Espfc/src/Target/TargetESP32c3.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Esp.h" 4 | #include "Debug_Espfc.h" 5 | // flash pins: 12-17 (reserved) 6 | // usb pins: 18,19 7 | // strapping pins: 2,8,9, must be high on boot 8 | #define ESPFC_INPUT 9 | #define ESPFC_INPUT_PIN -1 // ppm 10 | 11 | #define ESPFC_OUTPUT_COUNT ESC_CHANNEL_COUNT 12 | #define ESPFC_OUTPUT_0 2 13 | #define ESPFC_OUTPUT_1 3 14 | #define ESPFC_OUTPUT_2 4 15 | #define ESPFC_OUTPUT_3 5 16 | 17 | #define ESPFC_SERIAL_0 18 | #define ESPFC_SERIAL_0_DEV Serial0 19 | #define ESPFC_SERIAL_0_DEV_T HardwareSerial 20 | #define ESPFC_SERIAL_0_TX 21 21 | #define ESPFC_SERIAL_0_RX 20 22 | #define ESPFC_SERIAL_0_FN (SERIAL_FUNCTION_MSP) 23 | #define ESPFC_SERIAL_0_BAUD (SERIAL_SPEED_115200) 24 | #define ESPFC_SERIAL_0_BBAUD (SERIAL_SPEED_NONE) 25 | 26 | #define ESPFC_SERIAL_1 27 | #define ESPFC_SERIAL_1_DEV Serial1 28 | #define ESPFC_SERIAL_1_DEV_T HardwareSerial 29 | #define ESPFC_SERIAL_1_TX -1 30 | #define ESPFC_SERIAL_1_RX -1 31 | #define ESPFC_SERIAL_1_FN (SERIAL_FUNCTION_MSP) 32 | #define ESPFC_SERIAL_1_BAUD (SERIAL_SPEED_115200) 33 | #define ESPFC_SERIAL_1_BBAUD (SERIAL_SPEED_NONE) 34 | 35 | #define ESPFC_SERIAL_USB 36 | #define ESPFC_SERIAL_USB_DEV Serial 37 | #define ESPFC_SERIAL_USB_DEV_T HWCDC 38 | #define ESPFC_SERIAL_USB_FN (SERIAL_FUNCTION_MSP) 39 | 40 | #define ESPFC_SERIAL_SOFT_0 41 | #define ESPFC_SERIAL_SOFT_0_FN (SERIAL_FUNCTION_MSP) 42 | #define ESPFC_SERIAL_SOFT_0_WIFI 43 | 44 | #define ESPFC_SERIAL_REMAP_PINS 45 | #define ESPFC_SERIAL_DEBUG_PORT SERIAL_UART_0 46 | #define SERIAL_TX_FIFO_SIZE 0xFF 47 | 48 | #define ESPFC_SPI_0 49 | #define ESPFC_SPI_0_DEV SPI1 50 | #define ESPFC_SPI_0_SCK -1 51 | #define ESPFC_SPI_0_MOSI -1 52 | #define ESPFC_SPI_0_MISO -1 53 | 54 | #define ESPFC_SPI_CS_GYRO -1 55 | #define ESPFC_SPI_CS_BARO -1 56 | 57 | #define ESPFC_I2C_0 58 | #define ESPFC_I2C_0_SCL 6 59 | #define ESPFC_I2C_0_SDA 8 60 | #define ESPFC_I2C_0_SOFT 61 | 62 | #define ESPFC_BUZZER_PIN -1 63 | #define ESPFC_BUTTON_PIN -1 64 | #define ESPFC_LED_PIN -1 65 | 66 | #define ESPFC_ADC_0 67 | #define ESPFC_ADC_0_PIN 0 68 | 69 | #define ESPFC_ADC_1 70 | #define ESPFC_ADC_1_PIN 1 71 | 72 | #define ESPFC_ADC_SCALE (3.3f / 4096) 73 | 74 | #define ESPFC_FEATURE_MASK (FEATURE_RX_SERIAL | FEATURE_DYNAMIC_FILTER) 75 | 76 | #define ESPFC_GYRO_I2C_RATE_MAX 1000 77 | #define ESPFC_GYRO_SPI_RATE_MAX 2000 78 | 79 | #define ESPFC_DSP 80 | 81 | #include "Device/SerialDevice.h" 82 | 83 | #include "Target/TargetEsp32Common.h" 84 | 85 | namespace Espfc { 86 | 87 | template<> 88 | inline int targetSerialInit(HWCDC& dev, const SerialDeviceConfig& conf) 89 | { 90 | dev.begin(conf.baud); 91 | //while(!dev) delay(10); 92 | return 1; 93 | } 94 | 95 | } 96 | -------------------------------------------------------------------------------- /lib/Espfc/src/Target/TargetESP32s2.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Esp.h" 4 | #include "Debug_Espfc.h" 5 | 6 | #define ESPFC_INPUT 7 | #define ESPFC_INPUT_PIN 6 // ppm 8 | 9 | #define ESPFC_OUTPUT_COUNT 4 10 | #define ESPFC_OUTPUT_0 39 11 | #define ESPFC_OUTPUT_1 40 12 | #define ESPFC_OUTPUT_2 41 13 | #define ESPFC_OUTPUT_3 42 14 | 15 | #define ESPFC_SERIAL_0 16 | #define ESPFC_SERIAL_0_DEV Serial0 17 | #define ESPFC_SERIAL_0_DEV_T HardwareSerial 18 | #define ESPFC_SERIAL_0_TX 43 19 | #define ESPFC_SERIAL_0_RX 44 20 | #define ESPFC_SERIAL_0_FN (SERIAL_FUNCTION_MSP) 21 | #define ESPFC_SERIAL_0_BAUD (SERIAL_SPEED_115200) 22 | #define ESPFC_SERIAL_0_BBAUD (SERIAL_SPEED_NONE) 23 | 24 | #define ESPFC_SERIAL_1 25 | #define ESPFC_SERIAL_1_DEV Serial1 26 | #define ESPFC_SERIAL_1_DEV_T HardwareSerial 27 | #define ESPFC_SERIAL_1_TX 16 28 | #define ESPFC_SERIAL_1_RX 15 29 | #define ESPFC_SERIAL_1_FN (SERIAL_FUNCTION_RX_SERIAL) 30 | #define ESPFC_SERIAL_1_BAUD (SERIAL_SPEED_115200) 31 | #define ESPFC_SERIAL_1_BBAUD (SERIAL_SPEED_NONE) 32 | 33 | #define ESPFC_SERIAL_USB 34 | #define ESPFC_SERIAL_USB_DEV Serial 35 | #define ESPFC_SERIAL_USB_DEV_T USBCDC 36 | #define ESPFC_SERIAL_USB_FN (SERIAL_FUNCTION_MSP) 37 | 38 | #define ESPFC_SERIAL_SOFT_0 39 | #define ESPFC_SERIAL_SOFT_0_FN (SERIAL_FUNCTION_MSP) 40 | #define ESPFC_SERIAL_SOFT_0_WIFI 41 | 42 | #define ESPFC_SERIAL_REMAP_PINS 43 | #define ESPFC_SERIAL_DEBUG_PORT SERIAL_UART_0 44 | #define SERIAL_TX_FIFO_SIZE 0xFF 45 | 46 | #define ESPFC_SPI_0 47 | #define ESPFC_SPI_0_SCK 12 48 | #define ESPFC_SPI_0_MOSI 11 49 | #define ESPFC_SPI_0_MISO 13 50 | 51 | #define ESPFC_SPI_CS_GYRO 8 52 | #define ESPFC_SPI_CS_BARO 7 53 | 54 | #define ESPFC_I2C_0 55 | #define ESPFC_I2C_0_SCL 10 56 | #define ESPFC_I2C_0_SDA 9 57 | #define ESPFC_I2C_0_SOFT 58 | 59 | #define ESPFC_BUZZER_PIN 5 60 | #define ESPFC_BUTTON_PIN -1 61 | #define ESPFC_LED_PIN -1 62 | 63 | #define ESPFC_ADC_0 64 | #define ESPFC_ADC_0_PIN 1 65 | 66 | #define ESPFC_ADC_1 67 | #define ESPFC_ADC_1_PIN 4 68 | 69 | #define ESPFC_ADC_SCALE (3.3f / 4096) 70 | 71 | #define ESPFC_FEATURE_MASK (FEATURE_RX_SERIAL | FEATURE_DYNAMIC_FILTER) 72 | 73 | #define ESPFC_GYRO_I2C_RATE_MAX 2000 74 | #define ESPFC_GYRO_SPI_RATE_MAX 2000 75 | 76 | #define ESPFC_DSHOT_TELEMETRY 77 | 78 | #define ESPFC_FREE_RTOS 79 | #ifndef CONFIG_FREERTOS_UNICORE 80 | #define ESPFC_MULTI_CORE 81 | #endif 82 | 83 | //#define ESPFC_FREE_RTOS_QUEUE 84 | //#define ESPFC_ATOMIC_QUEUE 85 | 86 | #define ESPFC_DSP 87 | 88 | #include "Device/SerialDevice.h" 89 | 90 | #include "Target/TargetEsp32Common.h" 91 | 92 | namespace Espfc { 93 | 94 | template<> 95 | inline int targetSerialInit(USBCDC& dev, const SerialDeviceConfig& conf) 96 | { 97 | dev.begin(conf.baud); 98 | //while(!dev) delay(10); 99 | return 1; 100 | } 101 | 102 | } 103 | -------------------------------------------------------------------------------- /lib/Espfc/src/Target/TargetESP32s3.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Esp.h" 4 | #include "Debug_Espfc.h" 5 | 6 | // pins to avoid: 7 | // strapping: 0, 3, 45, 46 8 | // flash/psram: 26-37 (reserved) 9 | // usb/jtag: 19, 20 10 | 11 | #define ESPFC_INPUT 12 | #define ESPFC_INPUT_PIN 6 // ppm 13 | 14 | #define ESPFC_OUTPUT_COUNT 4 15 | #define ESPFC_OUTPUT_0 39 16 | #define ESPFC_OUTPUT_1 40 17 | #define ESPFC_OUTPUT_2 41 18 | #define ESPFC_OUTPUT_3 42 19 | 20 | #define ESPFC_SERIAL_0 21 | #define ESPFC_SERIAL_0_DEV Serial0 22 | #define ESPFC_SERIAL_0_DEV_T HardwareSerial 23 | #define ESPFC_SERIAL_0_TX 43 24 | #define ESPFC_SERIAL_0_RX 44 25 | #define ESPFC_SERIAL_0_FN (SERIAL_FUNCTION_MSP) 26 | #define ESPFC_SERIAL_0_BAUD (SERIAL_SPEED_115200) 27 | #define ESPFC_SERIAL_0_BBAUD (SERIAL_SPEED_NONE) 28 | 29 | #define ESPFC_SERIAL_1 30 | #define ESPFC_SERIAL_1_DEV Serial1 31 | #define ESPFC_SERIAL_1_DEV_T HardwareSerial 32 | #define ESPFC_SERIAL_1_TX 16 33 | #define ESPFC_SERIAL_1_RX 15 34 | #define ESPFC_SERIAL_1_FN (SERIAL_FUNCTION_MSP) 35 | #define ESPFC_SERIAL_1_BAUD (SERIAL_SPEED_115200) 36 | #define ESPFC_SERIAL_1_BBAUD (SERIAL_SPEED_NONE) 37 | 38 | #define ESPFC_SERIAL_2 39 | #define ESPFC_SERIAL_2_DEV Serial2 40 | #define ESPFC_SERIAL_2_DEV_T HardwareSerial 41 | #define ESPFC_SERIAL_2_TX 18 42 | #define ESPFC_SERIAL_2_RX 17 43 | #define ESPFC_SERIAL_2_FN (SERIAL_FUNCTION_RX_SERIAL) 44 | #define ESPFC_SERIAL_2_BAUD (SERIAL_SPEED_115200) 45 | #define ESPFC_SERIAL_2_BBAUD (SERIAL_SPEED_NONE) 46 | 47 | #define ESPFC_SERIAL_USB 48 | #define ESPFC_SERIAL_USB_DEV Serial 49 | #define ESPFC_SERIAL_USB_DEV_T HWCDC 50 | #define ESPFC_SERIAL_USB_FN (SERIAL_FUNCTION_MSP) 51 | 52 | #define ESPFC_SERIAL_SOFT_0 53 | #define ESPFC_SERIAL_SOFT_0_FN (SERIAL_FUNCTION_MSP) 54 | #define ESPFC_SERIAL_SOFT_0_WIFI 55 | 56 | #define ESPFC_SERIAL_REMAP_PINS 57 | #define ESPFC_SERIAL_DEBUG_PORT SERIAL_USB 58 | #define SERIAL_TX_FIFO_SIZE 0xFF 59 | 60 | #define ESPFC_SPI_0 61 | #define ESPFC_SPI_0_DEV SPI1 62 | #define ESPFC_SPI_0_SCK 12 63 | #define ESPFC_SPI_0_MOSI 11 64 | #define ESPFC_SPI_0_MISO 13 65 | 66 | #define ESPFC_SPI_CS_GYRO 8 67 | #define ESPFC_SPI_CS_BARO 7 68 | 69 | #define ESPFC_I2C_0 70 | #define ESPFC_I2C_0_SCL 10 71 | #define ESPFC_I2C_0_SDA 9 72 | #define ESPFC_I2C_0_SOFT 73 | 74 | #define ESPFC_BUZZER_PIN 5 75 | #define ESPFC_BUTTON_PIN -1 76 | #define ESPFC_LED_PIN -1 77 | 78 | #define ESPFC_ADC_0 79 | #define ESPFC_ADC_0_PIN 1 80 | 81 | #define ESPFC_ADC_1 82 | #define ESPFC_ADC_1_PIN 4 83 | 84 | #define ESPFC_ADC_SCALE (3.3f / 4096) 85 | 86 | #define ESPFC_FEATURE_MASK (FEATURE_RX_SERIAL | FEATURE_DYNAMIC_FILTER) 87 | 88 | #define ESPFC_GYRO_I2C_RATE_MAX 2000 89 | #define ESPFC_GYRO_SPI_RATE_MAX 4000 90 | 91 | #define ESPFC_DSHOT_TELEMETRY 92 | 93 | #define ESPFC_FREE_RTOS 94 | #ifndef CONFIG_FREERTOS_UNICORE 95 | #define ESPFC_MULTI_CORE 96 | #endif 97 | 98 | //#define ESPFC_FREE_RTOS_QUEUE 99 | #define ESPFC_ATOMIC_QUEUE 100 | 101 | #define ESPFC_DSP 102 | 103 | #include "Device/SerialDevice.h" 104 | 105 | #include "Target/TargetEsp32Common.h" 106 | 107 | namespace Espfc { 108 | 109 | template<> 110 | inline int targetSerialInit(HWCDC& dev, const SerialDeviceConfig& conf) 111 | { 112 | dev.begin(conf.baud); 113 | //dev.setTxTimeoutMs(10); 114 | //while(!dev) delay(10); 115 | return 1; 116 | } 117 | 118 | } 119 | -------------------------------------------------------------------------------- /lib/Espfc/src/Target/TargetEsp32Common.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Esp.h" 4 | #include "Debug_Espfc.h" 5 | #include "Device/SerialDevice.h" 6 | 7 | #define SERIAL_UART_PARITY_NONE 0B00000000 8 | #define SERIAL_UART_PARITY_EVEN 0B00000010 9 | #define SERIAL_UART_PARITY_ODD 0B00000011 10 | 11 | #define SERIAL_UART_NB_BIT_5 0B00000000 12 | #define SERIAL_UART_NB_BIT_6 0B00000100 13 | #define SERIAL_UART_NB_BIT_7 0B00001000 14 | #define SERIAL_UART_NB_BIT_8 0B00001100 15 | 16 | #define SERIAL_UART_NB_STOP_BIT_0 0B00000000 17 | #define SERIAL_UART_NB_STOP_BIT_1 0B00010000 18 | #define SERIAL_UART_NB_STOP_BIT_15 0B00100000 19 | #define SERIAL_UART_NB_STOP_BIT_2 0B00110000 20 | 21 | #define ESPFC_WIFI 22 | #define ESPFC_ESPNOW 23 | 24 | namespace Espfc { 25 | 26 | template 27 | inline int targetSerialInit(T& dev, const SerialDeviceConfig& conf) 28 | { 29 | uint32_t sc = 0x8000000; 30 | switch(conf.data_bits) 31 | { 32 | case 8: sc |= SERIAL_UART_NB_BIT_8; break; 33 | case 7: sc |= SERIAL_UART_NB_BIT_7; break; 34 | case 6: sc |= SERIAL_UART_NB_BIT_6; break; 35 | case 5: sc |= SERIAL_UART_NB_BIT_5; break; 36 | default: sc |= SERIAL_UART_NB_BIT_8; break; 37 | } 38 | switch(conf.parity) 39 | { 40 | case SDC_SERIAL_PARITY_EVEN: sc |= SERIAL_UART_PARITY_EVEN; break; 41 | case SDC_SERIAL_PARITY_ODD: sc |= SERIAL_UART_PARITY_ODD; break; 42 | default: sc |= SERIAL_UART_PARITY_NONE; break; 43 | } 44 | switch(conf.stop_bits) 45 | { 46 | case SDC_SERIAL_STOP_BITS_2: sc |= SERIAL_UART_NB_STOP_BIT_2; break; 47 | case SDC_SERIAL_STOP_BITS_15: sc |= SERIAL_UART_NB_STOP_BIT_15; break; 48 | case SDC_SERIAL_STOP_BITS_1: sc |= SERIAL_UART_NB_STOP_BIT_1; break; 49 | default: break; 50 | } 51 | if(dev) dev.end(); 52 | dev.setTxBufferSize(SERIAL_TX_FIFO_SIZE); 53 | dev.begin(conf.baud, sc, conf.rx_pin, conf.tx_pin, conf.inverted); 54 | return 1; 55 | } 56 | 57 | template 58 | inline int targetSPIInit(T& dev, int8_t sck, int8_t mosi, int8_t miso, int8_t ss) 59 | { 60 | dev.begin(sck, miso, mosi, ss); 61 | return 1; 62 | } 63 | 64 | template 65 | inline int targetI2CInit(T& dev, int8_t sda, int8_t scl, uint32_t speed) 66 | { 67 | dev.begin(sda, scl, speed); 68 | dev.setTimeout(50); 69 | return 1; 70 | } 71 | 72 | inline uint32_t getBoardId0() 73 | { 74 | const int64_t mac = ESP.getEfuseMac(); 75 | return (uint32_t)mac; 76 | } 77 | 78 | inline uint32_t getBoardId1() 79 | { 80 | const int64_t mac = ESP.getEfuseMac(); 81 | return (uint32_t)(mac >> 32); 82 | } 83 | 84 | inline uint32_t getBoardId2() 85 | { 86 | return 0; 87 | } 88 | 89 | inline void targetReset() 90 | { 91 | ESP.restart(); 92 | while(1) {} 93 | } 94 | 95 | inline uint32_t targetCpuFreq() 96 | { 97 | return ESP.getCpuFreqMHz(); 98 | } 99 | 100 | inline uint32_t targetFreeHeap() 101 | { 102 | return ESP.getFreeHeap(); 103 | } 104 | 105 | }; 106 | -------------------------------------------------------------------------------- /lib/Espfc/src/Target/TargetUnitTest.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #define ESPFC_INPUT 6 | #define ESPFC_INPUT_PIN 0 7 | 8 | #define ESPFC_OUTPUT_COUNT 4 // 4 is minimum 9 | #define ESPFC_OUTPUT_0 1 10 | #define ESPFC_OUTPUT_1 2 11 | #define ESPFC_OUTPUT_2 3 12 | #define ESPFC_OUTPUT_3 4 13 | 14 | #define ESPFC_OUTPUT_PROTOCOL ESC_PROTOCOL_DISABLED 15 | #define ESPFC_FEATURE_MASK (0) 16 | 17 | #define ESPFC_GYRO_I2C_RATE_MAX 2000 18 | #define ESPFC_GYRO_SPI_RATE_MAX 8000 19 | 20 | #define SERIAL_TX_FIFO_SIZE 0xFF 21 | 22 | #define ESPFC_SERIAL_DEBUG_PORT 0 23 | #define ESPFC_BUZZER_PIN -1 24 | #define ESPFC_BUTTON_PIN -1 25 | #define ESPFC_LED_PIN -1 26 | 27 | inline void targetReset() 28 | { 29 | } 30 | 31 | inline uint32_t getBoardId0() 32 | { 33 | return 0; 34 | } 35 | 36 | inline uint32_t getBoardId1() 37 | { 38 | return 0; 39 | } 40 | 41 | inline uint32_t getBoardId2() 42 | { 43 | return 0; 44 | } 45 | 46 | inline uint32_t targetCpuFreq() 47 | { 48 | return 1; 49 | } 50 | 51 | inline uint32_t targetFreeHeap() 52 | { 53 | return 1; 54 | } -------------------------------------------------------------------------------- /lib/Espfc/src/Telemetry/TelemetryText.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Model.h" 4 | 5 | namespace Espfc { 6 | 7 | namespace Telemetry { 8 | 9 | class TelemetryText 10 | { 11 | public: 12 | TelemetryText(Model& model): _model(model) {} 13 | 14 | int process(Stream& s) const 15 | { 16 | //print(s, _model.state.gyro.adc.x, 3); 17 | //println(s); 18 | 19 | return 1; 20 | } 21 | 22 | private: 23 | template 24 | void print(Stream& s, const T& v) const 25 | { 26 | s.print(v); 27 | s.print(' '); 28 | } 29 | 30 | void print(Stream& s, const long& v) const 31 | { 32 | s.print(v); 33 | s.print(' '); 34 | } 35 | 36 | void print(Stream& s, float& v, int len) const 37 | { 38 | s.print(v, len); 39 | s.print(' '); 40 | } 41 | 42 | void print(Stream& s, const VectorFloat& v) const 43 | { 44 | print(s, v.x); 45 | print(s, v.y); 46 | print(s, v.z); 47 | } 48 | 49 | void print(Stream& s, const VectorInt16& v) const 50 | { 51 | print(s, v.x); 52 | print(s, v.y); 53 | print(s, v.z); 54 | } 55 | 56 | void print(Stream& s, const Quaternion& v) const 57 | { 58 | print(s, v.w); 59 | print(s, v.x); 60 | print(s, v.y); 61 | print(s, v.z); 62 | } 63 | 64 | void println(Stream& s) const 65 | { 66 | s.println(); 67 | } 68 | 69 | Model& _model; 70 | }; 71 | 72 | } 73 | 74 | } 75 | -------------------------------------------------------------------------------- /lib/Espfc/src/TelemetryManager.cpp: -------------------------------------------------------------------------------- 1 | #include "TelemetryManager.h" 2 | 3 | namespace Espfc { 4 | 5 | TelemetryManager::TelemetryManager(Model& model): _model(model), _msp(model), _text(model), _crsf(model) {} 6 | 7 | int TelemetryManager::process(Device::SerialDevice& s, TelemetryProtocol protocol) const 8 | { 9 | Utils::Stats::Measure measure(_model.state.stats, COUNTER_TELEMETRY); 10 | 11 | switch(protocol) 12 | { 13 | case TELEMETRY_PROTOCOL_TEXT: 14 | _text.process(s); 15 | break; 16 | case TELEMETRY_PROTOCOL_CRSF: 17 | _crsf.process(s); 18 | break; 19 | } 20 | 21 | return 1; 22 | } 23 | 24 | int TelemetryManager::processMsp(Device::SerialDevice& s, TelemetryProtocol protocol, Connect::MspMessage m, uint8_t origin) 25 | { 26 | Connect::MspResponse r; 27 | 28 | // not valid msp message, stop processing 29 | if(!m.isReady() || !m.isCmd()) return 0; 30 | 31 | _msp.processCommand(m, r, s); 32 | 33 | switch(protocol) 34 | { 35 | case TELEMETRY_PROTOCOL_CRSF: 36 | _crsf.sendMsp(s, r, origin); 37 | break; 38 | default: 39 | break; 40 | } 41 | 42 | _msp.postCommand(); 43 | 44 | return 1; 45 | } 46 | 47 | } 48 | -------------------------------------------------------------------------------- /lib/Espfc/src/TelemetryManager.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Model.h" 4 | #include "Device/SerialDevice.h" 5 | #include "Telemetry/TelemetryText.h" 6 | #include "Telemetry/TelemetryCRSF.h" 7 | #include "Connect/Msp.hpp" 8 | #include "Connect/MspProcessor.hpp" 9 | 10 | namespace Espfc { 11 | 12 | enum TelemetryProtocol { 13 | TELEMETRY_PROTOCOL_TEXT, 14 | TELEMETRY_PROTOCOL_CRSF, 15 | }; 16 | 17 | class TelemetryManager 18 | { 19 | public: 20 | TelemetryManager(Model& model); 21 | int process(Device::SerialDevice& s, TelemetryProtocol protocol) const; 22 | int processMsp(Device::SerialDevice& s, TelemetryProtocol protocol, Connect::MspMessage m, uint8_t origin); 23 | 24 | private: 25 | Model& _model; 26 | Connect::MspProcessor _msp; 27 | Telemetry::TelemetryText _text; 28 | Telemetry::TelemetryCRSF _crsf; 29 | }; 30 | 31 | } 32 | -------------------------------------------------------------------------------- /lib/Espfc/src/Utils/Bits.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace Espfc { 6 | 7 | namespace Utils { 8 | 9 | inline bool getBit(uint8_t data, uint8_t bitNum) 10 | { 11 | return data & (1 << bitNum); 12 | } 13 | 14 | inline uint8_t setBit(uint8_t data, uint8_t bitNum, bool bitVal) 15 | { 16 | data = (bitVal != 0) ? (data | (1 << bitNum)) : (data & ~(1 << bitNum)); 17 | return data; 18 | } 19 | 20 | inline uint8_t getMaskMsb(uint8_t bitStart, uint8_t bitLen) 21 | { 22 | uint8_t mask = ((1 << bitLen) - 1) << (bitStart - bitLen + 1); 23 | return mask; 24 | } 25 | 26 | inline uint8_t setMasked(uint8_t data, uint8_t mask, uint8_t newData) 27 | { 28 | newData &= mask; // zero all non-important bits in data 29 | data &= ~(mask); // zero all important bits in existing byte 30 | data |= newData; // combine data with existing byte 31 | return data; 32 | } 33 | 34 | /** 35 | * I2Cdev::readBits() compatible 36 | * Read multiple bits from an 8-bit data 37 | * @param bitStart First bit position to read (0-7), Most Significant Bit 38 | * @param bitLen Number of bits to read (no more than 8, no more than start + 1) 39 | */ 40 | inline uint8_t getBitsMsb(uint8_t data, uint8_t bitStart, uint8_t bitLen) 41 | { 42 | // 01101001 read byte 43 | // 76543210 bit numbers 44 | // xxx args: bitStart=4, length=3 45 | // 010 masked 46 | // -> 010 shifted 47 | uint8_t mask = ((1 << bitLen) - 1) << (bitStart - bitLen + 1); 48 | data &= mask; 49 | data >>= (bitStart - bitLen + 1); 50 | return data; 51 | } 52 | 53 | inline uint8_t setBitsMsb(uint8_t data, uint8_t bitStart, uint8_t bitLen, uint8_t bitVal) 54 | { 55 | uint8_t mask = ((1 << bitLen) - 1) << (bitStart - bitLen + 1); 56 | bitVal <<= (bitStart - bitLen + 1); // shift data into correct position 57 | bitVal &= mask; // zero all non-important bits in data 58 | data &= ~(mask); // zero all important bits in existing byte 59 | data |= bitVal; // combine data with existing byte 60 | return data; 61 | } 62 | 63 | inline uint8_t getMaskLsb(uint8_t bitStart, uint8_t bitLen) 64 | { 65 | uint8_t mask = ((1 << bitLen) - 1) << bitStart; 66 | return mask; 67 | } 68 | 69 | /** 70 | * Read multiple bits from an 8-bit data 71 | * @param bitStart Last bit position to read (0-7), Least Significant Bit 72 | * @param bitLen Number of bits to read (not more than 8 - bitStart) 73 | */ 74 | inline uint8_t getBitsLsb(uint8_t data, uint8_t bitStart, uint8_t bitLen) 75 | { 76 | // 01101001 read byte 77 | // 76543210 bit numbers 78 | // xxx args: bitStart=4, length=3 79 | // 101 masked 80 | // -> 101 shifted 81 | uint8_t mask = ((1 << bitLen) - 1); 82 | data >>= bitStart; 83 | data &= mask; 84 | return data; 85 | } 86 | 87 | inline uint8_t setBitsLsb(uint8_t data, uint8_t bitStart, uint8_t bitLen, uint8_t bitVal) 88 | { 89 | uint8_t mask = ((1 << bitLen) - 1) << bitStart; 90 | bitVal <<= bitStart; // shift data into correct position 91 | bitVal &= mask; // zero all non-important bits in data 92 | data &= ~(mask); // zero all important bits in existing byte 93 | data |= bitVal; // combine data with existing byte 94 | return data; 95 | } 96 | } 97 | 98 | } 99 | -------------------------------------------------------------------------------- /lib/Espfc/src/Utils/Crc.cpp: -------------------------------------------------------------------------------- 1 | #include "Utils/Crc.hpp" 2 | #include "Utils/MemoryHelper.h" 3 | 4 | namespace Espfc { 5 | 6 | namespace Utils { 7 | 8 | uint8_t FAST_CODE_ATTR crc8_dvb_s2(uint8_t crc, const uint8_t a) 9 | { 10 | crc ^= a; 11 | for (size_t i = 0; i < 8; ++i) 12 | { 13 | if (crc & 0x80) 14 | { 15 | crc = (crc << 1) ^ 0xD5; 16 | } 17 | else 18 | { 19 | crc = crc << 1; 20 | } 21 | } 22 | return crc; 23 | } 24 | 25 | uint8_t FAST_CODE_ATTR crc8_dvb_s2(uint8_t crc, const uint8_t *data, size_t len) 26 | { 27 | while (len-- > 0) 28 | { 29 | crc = crc8_dvb_s2(crc, *data++); 30 | } 31 | return crc; 32 | } 33 | 34 | uint8_t FAST_CODE_ATTR crc8_xor(uint8_t checksum, const uint8_t a) 35 | { 36 | return checksum ^ a; 37 | } 38 | 39 | uint8_t FAST_CODE_ATTR crc8_xor(uint8_t checksum, const uint8_t *data, size_t len) 40 | { 41 | while (len-- > 0) 42 | { 43 | checksum = crc8_xor(checksum, *data++); 44 | } 45 | return checksum; 46 | } 47 | 48 | } 49 | 50 | } -------------------------------------------------------------------------------- /lib/Espfc/src/Utils/Crc.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace Espfc { 7 | 8 | namespace Utils { 9 | 10 | uint8_t crc8_dvb_s2(uint8_t crc, const uint8_t a); 11 | uint8_t crc8_dvb_s2(uint8_t crc, const uint8_t *data, size_t len); 12 | uint8_t crc8_xor(uint8_t checksum, const uint8_t a); 13 | uint8_t crc8_xor(uint8_t checksum, const uint8_t *data, size_t len); 14 | 15 | } 16 | 17 | } 18 | -------------------------------------------------------------------------------- /lib/Espfc/src/Utils/FFTAnalyzer.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // https://github.com/espressif/esp-dsp/blob/5f2bfe1f3ee7c9b024350557445b32baf6407a08/examples/fft4real/main/dsps_fft4real_main.c 4 | 5 | #include 6 | #include 7 | #include "Utils/Filter.h" 8 | #include "Utils/Math.hpp" 9 | 10 | namespace Espfc { 11 | 12 | namespace Utils { 13 | 14 | enum FFTPhase { 15 | PHASE_COLLECT, 16 | PHASE_FFT, 17 | PHASE_PEAKS 18 | }; 19 | 20 | template 21 | class FFTAnalyzer 22 | { 23 | public: 24 | FFTAnalyzer(); 25 | ~FFTAnalyzer(); 26 | 27 | int begin(int16_t rate, const DynamicFilterConfig& config, size_t axis); 28 | int update(float v); 29 | 30 | static constexpr size_t PEAKS_MAX = 8; 31 | Utils::Peak peaks[PEAKS_MAX]; 32 | 33 | private: 34 | void clearPeaks(); 35 | 36 | static constexpr size_t BINS = SAMPLES >> 1; 37 | 38 | int16_t _rate; 39 | int16_t _freq_min; 40 | int16_t _freq_max; 41 | int16_t _peak_count; 42 | 43 | size_t _idx; 44 | FFTPhase _phase; 45 | size_t _begin; 46 | size_t _end; 47 | float _bin_width; 48 | 49 | // fft input 50 | //__attribute__((aligned(16))) float _in[SAMPLES]; 51 | 52 | // fft output 53 | //__attribute__((aligned(16))) float _out[SAMPLES]; 54 | 55 | // Window coefficients 56 | //__attribute__((aligned(16))) float _win[SAMPLES]; 57 | 58 | float* _in; 59 | float* _out; 60 | float* _win; 61 | }; 62 | 63 | } 64 | 65 | } 66 | -------------------------------------------------------------------------------- /lib/Espfc/src/Utils/FilterHelper.cpp: -------------------------------------------------------------------------------- 1 | #include "FilterHelper.h" 2 | #include "MemoryHelper.h" 3 | 4 | namespace Espfc { 5 | 6 | namespace Utils { 7 | 8 | float FAST_CODE_ATTR applyFilter(Filter& filter, float sample) 9 | { 10 | return filter.update(sample); 11 | } 12 | 13 | VectorFloat FAST_CODE_ATTR applyFilter(Filter filters[3], const VectorFloat& samples) 14 | { 15 | VectorFloat result; 16 | result.x = filters[0].update(samples.x); 17 | result.y = filters[1].update(samples.y); 18 | result.z = filters[2].update(samples.z); 19 | return result; 20 | } 21 | 22 | } 23 | 24 | } 25 | -------------------------------------------------------------------------------- /lib/Espfc/src/Utils/FilterHelper.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Utils/Filter.h" 4 | #include 5 | 6 | namespace Espfc { 7 | 8 | namespace Utils { 9 | 10 | float applyFilter(Filter& filter, float sample); 11 | 12 | VectorFloat applyFilter(Filter filters[3], const VectorFloat& samples); 13 | 14 | } 15 | 16 | } 17 | -------------------------------------------------------------------------------- /lib/Espfc/src/Utils/FreqAnalyzer.cpp: -------------------------------------------------------------------------------- 1 | #include "Utils/FreqAnalyzer.hpp" 2 | #include "Utils/Math.hpp" 3 | 4 | namespace Espfc { 5 | 6 | namespace Utils { 7 | 8 | FreqAnalyzer::FreqAnalyzer() {} 9 | 10 | int FreqAnalyzer::begin(int rate, DynamicFilterConfig config) 11 | { 12 | _rate = rate; 13 | _freq_min = config.min_freq; 14 | _freq_max = config.max_freq; 15 | _pitch_freq_raise = _pitch_freq_fall = (_freq_min + _freq_max) * 0.5f; 16 | _pitch_count_raise = _pitch_count_fall = 0; 17 | _bpf.begin(FilterConfig(FILTER_BPF, 180, 100), _rate); // 100 - 180 - 260 [Hz] 18 | return 1; 19 | } 20 | 21 | // calculate noise pitch freq 22 | void FreqAnalyzer::update(float v) 23 | { 24 | // pitch detection 25 | noise = _bpf.update(v); 26 | bool sign = noise > 0.f; 27 | float k = 0.33f; 28 | 29 | // detect rising zero crossing 30 | if(sign && !_sign_prev) { 31 | float f = Utils::clamp(_rate / std::max(_pitch_count_raise, 1), _freq_min, _freq_max); 32 | _pitch_freq_raise += k * (f - _pitch_freq_raise); 33 | _pitch_count_raise = 0; 34 | } 35 | 36 | // detect falling zero crossing 37 | if(!sign && _sign_prev) { 38 | float f = Utils::clamp(_rate / std::max(_pitch_count_fall, 1), _freq_min, _freq_max); 39 | _pitch_freq_fall += k * (f - _pitch_freq_fall); 40 | _pitch_count_fall = 0; 41 | } 42 | 43 | _sign_prev = sign; 44 | _pitch_count_raise++; 45 | _pitch_count_fall++; 46 | 47 | freq = (_pitch_freq_raise + _pitch_freq_fall) * 0.5f; 48 | } 49 | 50 | } 51 | 52 | } 53 | -------------------------------------------------------------------------------- /lib/Espfc/src/Utils/FreqAnalyzer.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Utils/Filter.h" 4 | 5 | namespace Espfc { 6 | 7 | namespace Utils { 8 | 9 | class FreqAnalyzer 10 | { 11 | public: 12 | FreqAnalyzer(); 13 | 14 | int begin(int rate, DynamicFilterConfig config); 15 | void update(float v); 16 | 17 | float freq; 18 | float noise; 19 | 20 | private: 21 | Utils::Filter _bpf; 22 | float _rate; 23 | 24 | float _freq_min; 25 | float _freq_max; 26 | 27 | int _pitch_count_raise; 28 | int _pitch_count_fall; 29 | 30 | float _pitch_freq_raise; 31 | float _pitch_freq_fall; 32 | 33 | bool _sign_prev; 34 | }; 35 | 36 | } 37 | 38 | } 39 | -------------------------------------------------------------------------------- /lib/Espfc/src/Utils/Logger.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Debug_Espfc.h" 4 | 5 | namespace Espfc { 6 | 7 | namespace Utils { 8 | 9 | class Logger 10 | { 11 | public: 12 | Logger(): _buff(nullptr), _size(0), _tail(0) { } 13 | 14 | ~Logger() 15 | { 16 | if(!_buff) return; 17 | 18 | delete[] _buff; 19 | _buff = nullptr; 20 | _size = 0; 21 | _tail = 0; 22 | } 23 | 24 | int begin(size_t size = 2048) 25 | { 26 | _size = size; 27 | _tail = 0; 28 | _buff = new char[size]; 29 | _buff[0] = '\0'; 30 | return 1; 31 | } 32 | 33 | Logger& info() 34 | { 35 | LOG_SERIAL_DEBUG("I") 36 | append('I'); 37 | log(String{(float)millis() * 0.001f, 2}); 38 | return *this; 39 | } 40 | 41 | Logger& err() 42 | { 43 | LOG_SERIAL_DEBUG("E") 44 | append('E'); 45 | log(String{(float)millis() * 0.001f, 2}); 46 | return *this; 47 | } 48 | 49 | template 50 | Logger& log(const T& v) 51 | { 52 | LOG_SERIAL_DEBUG(' ') 53 | LOG_SERIAL_DEBUG(v) 54 | append(' '); 55 | append(String(v)); 56 | return *this; 57 | } 58 | 59 | template 60 | Logger& loghex(const T& v) 61 | { 62 | LOG_SERIAL_DEBUG(' ') 63 | LOG_SERIAL_DEBUG_HEX(v) 64 | append(' '); 65 | append(String(v, HEX)); 66 | return *this; 67 | } 68 | 69 | template 70 | Logger& logln(const T& v) 71 | { 72 | LOG_SERIAL_DEBUG(' ') 73 | LOG_SERIAL_DEBUG(v) 74 | append(' '); 75 | append(String(v)); 76 | return endl(); 77 | } 78 | 79 | Logger& endl() 80 | { 81 | LOG_SERIAL_DEBUG('\r') 82 | LOG_SERIAL_DEBUG('\n') 83 | append('\r'); 84 | append('\n'); 85 | return *this; 86 | } 87 | 88 | void append(const String& s) 89 | { 90 | append(s.c_str(), s.length()); 91 | } 92 | 93 | void append(const char * s, size_t len) 94 | { 95 | for(size_t i = 0; i < len; i++) 96 | { 97 | append(s[i]); 98 | } 99 | } 100 | 101 | void append(char c) 102 | { 103 | if(_size > 0 && _tail < _size - 1) 104 | { 105 | _buff[_tail] = c; 106 | _tail++; 107 | _buff[_tail] = '\0'; 108 | } 109 | } 110 | 111 | const char * c_str() const 112 | { 113 | return _buff; 114 | } 115 | 116 | const size_t length() const 117 | { 118 | return _tail; 119 | } 120 | 121 | private: 122 | char * _buff; 123 | size_t _size; 124 | size_t _tail; 125 | }; 126 | 127 | } 128 | 129 | } 130 | -------------------------------------------------------------------------------- /lib/Espfc/src/Utils/MemoryHelper.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #ifdef ESP32 4 | #include 5 | #define FAST_CODE_ATTR IRAM_ATTR 6 | #else 7 | #define FAST_CODE_ATTR 8 | #endif 9 | -------------------------------------------------------------------------------- /lib/Espfc/src/Utils/RingBuf.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | // https://gist.github.com/edwintcloud/d547a4f9ccaf7245b06f0e8782acefaa 6 | 7 | namespace Espfc { 8 | 9 | namespace Utils { 10 | 11 | template 12 | class RingBuf 13 | { 14 | public: 15 | enum { Capacity = Size + 1 }; 16 | 17 | RingBuf(): _tail(0), _head(0) {} 18 | ~RingBuf() {} 19 | 20 | bool push(const Element& item) 21 | { 22 | if(isFull()) return false; 23 | 24 | _array[_tail] = item; 25 | _tail = _increment(_tail); 26 | 27 | return true; 28 | } 29 | 30 | size_t push(const Element *items, size_t len) 31 | { 32 | len = std::min(available(), len); 33 | for(size_t i = 0; i < len; i++) 34 | { 35 | _array[_tail] = items[i]; 36 | _tail = _increment(_tail); 37 | } 38 | return len; 39 | } 40 | 41 | bool pop(Element& item) 42 | { 43 | if(isEmpty()) return false; 44 | 45 | item = _array[_head]; 46 | _head = _increment(_head); 47 | 48 | return true; 49 | } 50 | 51 | size_t pop(Element * items, size_t len) 52 | { 53 | len = std::min(size(), len); 54 | for(size_t i = 0; i < len; i++) 55 | { 56 | items[i] = _array[_head]; 57 | _head = _increment(_head); 58 | } 59 | return len; 60 | } 61 | 62 | bool isEmpty() const 63 | { 64 | return _head == _tail; 65 | } 66 | 67 | bool isFull() const 68 | { 69 | return _head == _increment(_tail); 70 | } 71 | 72 | size_t size() const 73 | { 74 | if (_tail >= _head) return _tail - _head; 75 | return Capacity - (_head - _tail); 76 | } 77 | 78 | size_t available() const 79 | { 80 | return Size - size(); 81 | } 82 | 83 | private: 84 | size_t _increment(size_t idx) const { return (idx + 1) % Capacity; } 85 | size_t _tail; 86 | size_t _head; 87 | Element _array[Capacity]; 88 | }; 89 | 90 | } 91 | 92 | } 93 | -------------------------------------------------------------------------------- /lib/Espfc/src/Utils/Sma.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace Espfc { 6 | 7 | namespace Utils { 8 | 9 | template 10 | class Sma 11 | { 12 | public: 13 | Sma(); 14 | void begin(size_t count); 15 | SampleType update(const SampleType& input); 16 | 17 | private: 18 | SampleType _samples[MaxSize]; 19 | SampleType _sum; 20 | size_t _idx; 21 | size_t _count; 22 | float _inv_count; 23 | }; 24 | 25 | } 26 | 27 | } 28 | -------------------------------------------------------------------------------- /lib/Espfc/src/Utils/Sma.ipp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Utils/Sma.hpp" 4 | 5 | namespace Espfc { 6 | 7 | namespace Utils { 8 | 9 | template 10 | Sma::Sma(): _idx(0), _count(MaxSize) 11 | { 12 | begin(MaxSize); 13 | } 14 | 15 | template 16 | void Sma::begin(size_t count) 17 | { 18 | _count = std::min(count, MaxSize); 19 | _inv_count = 1.f / _count; 20 | } 21 | 22 | template 23 | SampleType Sma::update(const SampleType& input) 24 | { 25 | if(_count > 1) 26 | { 27 | _sum -= _samples[_idx]; 28 | _sum += input; 29 | _samples[_idx] = input; 30 | if (++_idx >= _count) _idx = 0; 31 | return _sum * _inv_count; 32 | } 33 | return input; 34 | } 35 | 36 | } 37 | 38 | } 39 | -------------------------------------------------------------------------------- /lib/Espfc/src/Utils/Stats.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Utils/Timer.h" 4 | #include 5 | 6 | namespace Espfc { 7 | 8 | enum StatCounter { 9 | COUNTER_GYRO_READ, 10 | COUNTER_GYRO_FILTER, 11 | COUNTER_GYRO_FFT, 12 | COUNTER_RPM_UPDATE, 13 | COUNTER_ACCEL_READ, 14 | COUNTER_ACCEL_FILTER, 15 | COUNTER_MAG_READ, 16 | COUNTER_MAG_FILTER, 17 | COUNTER_BARO, 18 | COUNTER_IMU_FUSION, 19 | COUNTER_IMU_FUSION2, 20 | COUNTER_OUTER_PID, 21 | COUNTER_INNER_PID, 22 | COUNTER_MIXER, 23 | COUNTER_MIXER_WRITE, 24 | COUNTER_MIXER_READ, 25 | COUNTER_BLACKBOX, 26 | COUNTER_INPUT_READ, 27 | COUNTER_INPUT_FILTER, 28 | COUNTER_FAILSAFE, 29 | COUNTER_ACTUATOR, 30 | COUNTER_TELEMETRY, 31 | COUNTER_SERIAL, 32 | COUNTER_WIFI, 33 | COUNTER_BATTERY, 34 | COUNTER_GPS_READ, 35 | COUNTER_CPU_0, 36 | COUNTER_CPU_1, 37 | COUNTER_COUNT 38 | }; 39 | 40 | namespace Utils { 41 | 42 | class Stats 43 | { 44 | public: 45 | class Measure 46 | { 47 | public: 48 | inline Measure(Stats& stats, StatCounter counter): _stats(stats), _counter(counter) 49 | { 50 | _stats.start(_counter); 51 | } 52 | inline ~Measure() 53 | { 54 | _stats.end(_counter); 55 | } 56 | private: 57 | Stats& _stats; 58 | StatCounter _counter; 59 | }; 60 | 61 | Stats(); 62 | 63 | void start(StatCounter c); 64 | void end(StatCounter c); 65 | 66 | void update(); 67 | 68 | void loopTick(); 69 | uint32_t loopTime() const; 70 | float getLoad(StatCounter c) const; 71 | 72 | /** 73 | * @brief Get the Time of counter normalized to 1 ms 74 | */ 75 | float getTime(StatCounter c) const; 76 | float getReal(StatCounter c) const; 77 | float getFreq(StatCounter c) const; 78 | float getTotalLoad() const; 79 | float getTotalTime() const; 80 | float getCpuLoad() const; 81 | float getCpuTime() const; 82 | const char * getName(StatCounter c) const; 83 | 84 | Utils::Timer timer; 85 | 86 | private: 87 | uint32_t _start[COUNTER_COUNT]; 88 | uint32_t _sum[COUNTER_COUNT]; 89 | uint32_t _count[COUNTER_COUNT]; 90 | float _avg[COUNTER_COUNT]; 91 | float _freq[COUNTER_COUNT]; 92 | float _real[COUNTER_COUNT]; 93 | uint32_t _loop_last; 94 | int32_t _loop_time; 95 | }; 96 | 97 | } 98 | 99 | } 100 | -------------------------------------------------------------------------------- /lib/Espfc/src/Utils/Storage.cpp: -------------------------------------------------------------------------------- 1 | #ifndef UNIT_TEST 2 | 3 | #include "Utils/Storage.h" 4 | #include "ModelConfig.h" 5 | #include 6 | #include 7 | 8 | #if defined(NO_GLOBAL_INSTANCES) || defined(NO_GLOBAL_EEPROM) 9 | static EEPROMClass EEPROM; 10 | #endif 11 | 12 | namespace Espfc { 13 | 14 | namespace Utils { 15 | 16 | int Storage::begin() 17 | { 18 | EEPROM.begin(EEPROM_SIZE); 19 | static_assert(sizeof(ModelConfig) <= EEPROM_SIZE, "ModelConfig Size too big"); 20 | return 1; 21 | } 22 | 23 | StorageResult Storage::load(ModelConfig& config) const 24 | { 25 | //return STORAGE_ERR_BAD_MAGIC; 26 | 27 | int addr = 0; 28 | uint8_t magic = EEPROM.read(addr++); 29 | if(EEPROM_MAGIC != magic) 30 | { 31 | return STORAGE_ERR_BAD_MAGIC; 32 | } 33 | 34 | uint8_t version = EEPROM.read(addr++); 35 | if(EEPROM_VERSION != version) 36 | { 37 | return STORAGE_ERR_BAD_VERSION; 38 | } 39 | 40 | uint16_t size = 0; 41 | size = EEPROM.read(addr++); 42 | size |= EEPROM.read(addr++) << 8; 43 | if(size != sizeof(ModelConfig)) 44 | { 45 | return STORAGE_ERR_BAD_SIZE; 46 | } 47 | 48 | EEPROM.get(addr, config); 49 | return STORAGE_LOAD_SUCCESS; 50 | } 51 | 52 | StorageResult Storage::save(const ModelConfig& config) 53 | { 54 | int addr = 0; 55 | uint16_t size = sizeof(ModelConfig); 56 | EEPROM.write(addr++, EEPROM_MAGIC); 57 | EEPROM.write(addr++, EEPROM_VERSION); 58 | EEPROM.write(addr++, size & 0xFF); 59 | EEPROM.write(addr++, (size >> 8) & 0xFF); 60 | EEPROM.put(addr, config); 61 | bool ok = EEPROM.commit(); 62 | if(!ok) return STORAGE_SAVE_ERROR; 63 | return STORAGE_SAVE_SUCCESS; 64 | } 65 | 66 | } 67 | 68 | } 69 | 70 | #endif 71 | -------------------------------------------------------------------------------- /lib/Espfc/src/Utils/Storage.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace Espfc { 4 | 5 | enum StorageResult 6 | { 7 | STORAGE_NONE, 8 | STORAGE_LOAD_SUCCESS, 9 | STORAGE_SAVE_SUCCESS, 10 | STORAGE_SAVE_ERROR, 11 | STORAGE_ERR_BAD_MAGIC, 12 | STORAGE_ERR_BAD_VERSION, 13 | STORAGE_ERR_BAD_SIZE, 14 | }; 15 | 16 | } 17 | 18 | #ifndef UNIT_TEST 19 | 20 | #include "ModelConfig.h" 21 | 22 | namespace Espfc { 23 | 24 | namespace Utils { 25 | 26 | class Storage 27 | { 28 | public: 29 | int begin(); 30 | StorageResult load(ModelConfig& config) const; 31 | StorageResult save(const ModelConfig& config); 32 | 33 | private: 34 | static constexpr uint8_t EEPROM_MAGIC = 0xA5; 35 | static constexpr uint8_t EEPROM_VERSION = 0x01; 36 | static constexpr size_t EEPROM_SIZE = 2048; 37 | }; 38 | 39 | } 40 | 41 | } 42 | 43 | #endif 44 | -------------------------------------------------------------------------------- /lib/Espfc/src/Utils/Timer.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "Utils/Timer.h" 3 | #include "Utils/MemoryHelper.h" 4 | 5 | namespace Espfc { 6 | 7 | namespace Utils { 8 | 9 | Timer::Timer(): interval(0), last(0), next(0), iteration(0), delta(0) 10 | { 11 | } 12 | 13 | int Timer::setInterval(uint32_t interval) 14 | { 15 | this->interval = interval; 16 | this->rate = (1000000UL + interval / 2) / interval; 17 | this->denom = 1; 18 | this->delta = this->interval; 19 | this->intervalf = this->interval * 0.000001f; 20 | iteration = 0; 21 | return 1; 22 | } 23 | 24 | int Timer::setRate(uint32_t rate, uint32_t denom) 25 | { 26 | this->rate = (rate + denom / 2) / denom; 27 | this->interval = (1000000UL + this->rate / 2) / this->rate; 28 | this->denom = denom; 29 | this->delta = this->interval; 30 | this->intervalf = this->interval * 0.000001f; 31 | iteration = 0; 32 | return 1; 33 | } 34 | 35 | bool FAST_CODE_ATTR Timer::check() 36 | { 37 | return check(micros()); 38 | } 39 | 40 | int FAST_CODE_ATTR Timer::update() 41 | { 42 | return update(micros()); 43 | } 44 | 45 | bool FAST_CODE_ATTR Timer::check(uint32_t now) 46 | { 47 | if(interval == 0) return false; 48 | if(now < next) return false; 49 | return update(now); 50 | } 51 | 52 | int FAST_CODE_ATTR Timer::update(uint32_t now) 53 | { 54 | next = now + interval; 55 | delta = now - last; 56 | last = now; 57 | iteration++; 58 | return 1; 59 | } 60 | 61 | bool FAST_CODE_ATTR Timer::syncTo(const Timer& t, uint32_t slot) 62 | { 63 | if(denom > 0) 64 | { 65 | if(slot > denom - 1) slot = denom - 1; 66 | if(t.iteration % denom != slot) return false; 67 | return update(micros()); 68 | } 69 | return check(micros()); 70 | } 71 | 72 | } 73 | 74 | } 75 | -------------------------------------------------------------------------------- /lib/Espfc/src/Utils/Timer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace Espfc { 6 | 7 | namespace Utils { 8 | 9 | class Timer 10 | { 11 | public: 12 | Timer(); 13 | int setInterval(uint32_t interval); 14 | int setRate(uint32_t rate, uint32_t denom = 1u); 15 | 16 | bool check(); 17 | int update(); 18 | bool check(uint32_t now); 19 | int update(uint32_t now); 20 | bool syncTo(const Timer& t, uint32_t slot = 0u); 21 | 22 | uint32_t interval; 23 | uint32_t rate; 24 | uint32_t denom; 25 | 26 | uint32_t last; 27 | uint32_t next; 28 | volatile uint32_t iteration; 29 | uint32_t delta; 30 | float intervalf; 31 | }; 32 | 33 | } 34 | 35 | } 36 | -------------------------------------------------------------------------------- /lib/Espfc/src/Wireless.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Model.h" 4 | #include "Device/SerialDeviceAdapter.h" 5 | 6 | #ifdef ESPFC_SERIAL_SOFT_0_WIFI 7 | #if defined(ESPFC_WIFI_ALT) 8 | #include 9 | #elif defined(ESPFC_WIFI) 10 | #include 11 | #endif 12 | 13 | namespace Espfc { 14 | 15 | class Wireless 16 | { 17 | enum Status { 18 | STOPPED, 19 | STARTED, 20 | }; 21 | public: 22 | Wireless(Model& model); 23 | 24 | int begin(); 25 | int update(); 26 | 27 | void startAp(); 28 | int connect(); 29 | void wifiEventConnected(const String& ssid, int channel); 30 | void wifiEventApConnected(const uint8_t* mac); 31 | void wifiEventGotIp(const IPAddress& ip); 32 | void wifiEventDisconnected(); 33 | 34 | private: 35 | Model& _model; 36 | Status _status; 37 | WiFiServer _server; 38 | WiFiClient _client; 39 | Device::SerialDeviceAdapter _adapter; 40 | #ifdef ESPFC_WIFI_ALT 41 | WiFiEventHandler _events[4]; 42 | #endif 43 | }; 44 | 45 | } 46 | 47 | #endif 48 | -------------------------------------------------------------------------------- /lib/Gps/library.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "Gps", 3 | "version": "1.0.0" 4 | } 5 | -------------------------------------------------------------------------------- /lib/Gps/src/Gps.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "GpsProtocol.hpp" 4 | #include "GpsParser.hpp" 5 | -------------------------------------------------------------------------------- /lib/Gps/src/GpsParser.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "GpsProtocol.hpp" 4 | #include 5 | 6 | namespace Gps { 7 | 8 | class UbxParser 9 | { 10 | public: 11 | void parse(uint8_t c, UbxMessage& m) 12 | { 13 | switch (m.status) 14 | { 15 | case UBX_STATE_READY: 16 | case UBX_STATE_IDLE: 17 | if (c == UBX_SYNC0) m.status = UBX_STATE_SYNC; 18 | else m.status = UBX_STATE_IDLE; 19 | break; 20 | 21 | case UBX_STATE_SYNC: 22 | if (c == UBX_SYNC1) m.status = UBX_STATE_CLASS; 23 | else m.status = UBX_STATE_IDLE; 24 | break; 25 | 26 | case UBX_STATE_CLASS: 27 | m.msgId = c; 28 | m.status = UBX_STATE_ID; 29 | break; 30 | 31 | case UBX_STATE_ID: 32 | m.msgId |= c << 8; 33 | m.status = UBX_STATE_LEN0; 34 | break; 35 | 36 | case UBX_STATE_LEN0: 37 | m.length = c; 38 | m.status = UBX_STATE_LEN1; 39 | break; 40 | 41 | case UBX_STATE_LEN1: 42 | m.length |= ((uint16_t)c << 8); 43 | if(m.length == 0) m.status = UBX_STATE_CRC0; 44 | else if(m.length >= 511) m.status = UBX_STATE_IDLE; 45 | else m.status = UBX_STATE_PAYLOAD; 46 | break; 47 | 48 | case UBX_STATE_PAYLOAD: 49 | if(m.written < 511) { 50 | m.payload[m.written] = c; 51 | m.written++; 52 | } 53 | if(m.written == m.length) m.status = UBX_STATE_CRC0; 54 | break; 55 | 56 | case UBX_STATE_CRC0: 57 | m.crc = c; 58 | m.status = UBX_STATE_CRC1; 59 | break; 60 | 61 | case UBX_STATE_CRC1: 62 | m.crc |= c << 8; 63 | if(m.crc == m.checksum()) m.status = UBX_STATE_READY; 64 | else m.status = UBX_STATE_IDLE; 65 | break; 66 | } 67 | } 68 | }; 69 | 70 | class NmeaParser 71 | { 72 | public: 73 | void parse(uint8_t c, NmeaMessage& m) 74 | { 75 | switch(m.status) 76 | { 77 | case NMEA_STATE_READY: 78 | case NMEA_STATE_IDLE: 79 | if(c == '$') m.status = NMEA_STATE_PAYLOAD; 80 | break; 81 | 82 | case NMEA_STATE_PAYLOAD: 83 | if (c == '\r') m.status = NMEA_STATE_END; 84 | else if (c == '\n') m.status = NMEA_STATE_IDLE; 85 | else { 86 | if (m.length < 511) { 87 | m.payload[m.length++] = c; 88 | m.payload[m.length] = '\0'; 89 | } 90 | } 91 | break; 92 | 93 | case NMEA_STATE_END: 94 | if (c == '\n') { 95 | m.status = NMEA_STATE_READY; 96 | } 97 | break; 98 | } 99 | } 100 | }; 101 | 102 | } 103 | -------------------------------------------------------------------------------- /lib/MultiButton/library.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "MultiButton", 3 | "version": "1.3.0" 4 | } 5 | -------------------------------------------------------------------------------- /lib/betaflight/library.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "betaflight", 3 | "version": "4.5.0" 4 | } 5 | -------------------------------------------------------------------------------- /lib/betaflight/src/blackbox/blackbox_encoding.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of Cleanflight and Betaflight. 3 | * 4 | * Cleanflight and Betaflight are free software. You can redistribute 5 | * this software and/or modify this software under the terms of the 6 | * GNU General Public License as published by the Free Software 7 | * Foundation, either version 3 of the License, or (at your option) 8 | * any later version. 9 | * 10 | * Cleanflight and Betaflight are distributed in the hope that they 11 | * will be useful, but WITHOUT ANY WARRANTY; without even the implied 12 | * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 13 | * See the GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this software. 17 | * 18 | * If not, see . 19 | */ 20 | 21 | #pragma once 22 | 23 | int blackboxPrintf(const char *fmt, ...); 24 | void blackboxPrintfHeaderLine(const char *name, const char *fmt, ...); 25 | 26 | void blackboxWriteUnsignedVB(uint32_t value); 27 | void blackboxWriteSignedVB(int32_t value); 28 | void blackboxWriteSignedVBArray(int32_t *array, int count); 29 | void blackboxWriteSigned16VBArray(int16_t *array, int count); 30 | void blackboxWriteS16(int16_t value); 31 | void blackboxWriteTag2_3S32(int32_t *values); 32 | int blackboxWriteTag2_3SVariable(int32_t *values); 33 | void blackboxWriteTag8_4S16(int32_t *values); 34 | void blackboxWriteTag8_8SVB(int32_t *values, int valueCount); 35 | void blackboxWriteU32(int32_t value); 36 | void blackboxWriteFloat(float value); 37 | -------------------------------------------------------------------------------- /lib/betaflight/src/blackbox/blackbox_io.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of Cleanflight and Betaflight. 3 | * 4 | * Cleanflight and Betaflight are free software. You can redistribute 5 | * this software and/or modify this software under the terms of the 6 | * GNU General Public License as published by the Free Software 7 | * Foundation, either version 3 of the License, or (at your option) 8 | * any later version. 9 | * 10 | * Cleanflight and Betaflight are distributed in the hope that they 11 | * will be useful, but WITHOUT ANY WARRANTY; without even the implied 12 | * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 13 | * See the GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this software. 17 | * 18 | * If not, see . 19 | */ 20 | 21 | #pragma once 22 | 23 | typedef enum { 24 | BLACKBOX_RESERVE_SUCCESS, 25 | BLACKBOX_RESERVE_TEMPORARY_FAILURE, 26 | BLACKBOX_RESERVE_PERMANENT_FAILURE 27 | } blackboxBufferReserveStatus_e; 28 | 29 | /* 30 | * We want to limit how bursty our writes to the device are. Note that this will also restrict the maximum size of a 31 | * header write we can make: 32 | */ 33 | #define BLACKBOX_MAX_ACCUMULATED_HEADER_BUDGET 256 34 | 35 | /* 36 | * Ideally, each iteration in which we are logging headers would write a similar amount of data to the device as a 37 | * regular logging iteration. This way we won't hog the CPU by making a gigantic write: 38 | */ 39 | #define BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION 64 40 | 41 | extern int32_t blackboxHeaderBudget; 42 | 43 | void blackboxOpen(void); 44 | void blackboxWrite(uint8_t value); 45 | int blackboxWriteString(const char *s); 46 | 47 | void blackboxDeviceFlush(void); 48 | bool blackboxDeviceFlushForce(void); 49 | bool blackboxDeviceFlushForceComplete(void); 50 | 51 | bool blackboxDeviceOpen(void); 52 | void blackboxDeviceClose(void); 53 | 54 | void blackboxEraseAll(void); 55 | bool isBlackboxErased(void); 56 | 57 | bool blackboxDeviceBeginLog(void); 58 | bool blackboxDeviceEndLog(bool retainLog); 59 | 60 | bool isBlackboxDeviceFull(void); 61 | bool isBlackboxDeviceWorking(void); 62 | int32_t blackboxGetLogNumber(void); 63 | 64 | void blackboxReplenishHeaderBudget(void); 65 | blackboxBufferReserveStatus_e blackboxDeviceReserveBufferSpace(int32_t bytes); 66 | int8_t blackboxGetLogFileNo(void); 67 | -------------------------------------------------------------------------------- /lib/betaflight/src/build/build_config.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/build/build_config.h -------------------------------------------------------------------------------- /lib/betaflight/src/build/debug.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/build/debug.h -------------------------------------------------------------------------------- /lib/betaflight/src/build/version.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/build/version.h -------------------------------------------------------------------------------- /lib/betaflight/src/cli/settings.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/cli/settings.h -------------------------------------------------------------------------------- /lib/betaflight/src/common/axis.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/common/axis.h -------------------------------------------------------------------------------- /lib/betaflight/src/common/encoding.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/common/encoding.h -------------------------------------------------------------------------------- /lib/betaflight/src/common/maths.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/common/maths.h -------------------------------------------------------------------------------- /lib/betaflight/src/common/printf.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/common/printf.h -------------------------------------------------------------------------------- /lib/betaflight/src/common/time.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/common/time.h -------------------------------------------------------------------------------- /lib/betaflight/src/common/utils.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/common/utils.h -------------------------------------------------------------------------------- /lib/betaflight/src/config/config.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/config/config.h -------------------------------------------------------------------------------- /lib/betaflight/src/config/feature.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/config/feature.h -------------------------------------------------------------------------------- /lib/betaflight/src/config/parameter_group.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/config/parameter_group.h -------------------------------------------------------------------------------- /lib/betaflight/src/config/parameter_group_ids.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/config/parameter_group_ids.h -------------------------------------------------------------------------------- /lib/betaflight/src/drivers/buf_writer.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/drivers/buf_writer.h -------------------------------------------------------------------------------- /lib/betaflight/src/drivers/compass/compass.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/drivers/compass/compass.h -------------------------------------------------------------------------------- /lib/betaflight/src/drivers/dshot.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/drivers/dshot.h -------------------------------------------------------------------------------- /lib/betaflight/src/drivers/io.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/drivers/io.h -------------------------------------------------------------------------------- /lib/betaflight/src/drivers/io_types.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/drivers/io_types.h -------------------------------------------------------------------------------- /lib/betaflight/src/drivers/light_led.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/drivers/light_led.h -------------------------------------------------------------------------------- /lib/betaflight/src/drivers/sdcard.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/drivers/sdcard.h -------------------------------------------------------------------------------- /lib/betaflight/src/drivers/sensor.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/drivers/sensor.h -------------------------------------------------------------------------------- /lib/betaflight/src/drivers/serial.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/drivers/serial.h -------------------------------------------------------------------------------- /lib/betaflight/src/drivers/time.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/drivers/time.h -------------------------------------------------------------------------------- /lib/betaflight/src/drivers/timer.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/drivers/timer.h -------------------------------------------------------------------------------- /lib/betaflight/src/extract_headers.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | FILES=$(grep -R '#include "' . | sed -e 's/.*"\(.*\)"/\1/' | uniq) 4 | 5 | for f in $FILES 6 | do 7 | DIR="$(dirname $f)" 8 | if [ $DIR != "." ] 9 | then 10 | mkdir -p $DIR 11 | touch $f 12 | echo "create $f" 13 | fi 14 | done 15 | 16 | -------------------------------------------------------------------------------- /lib/betaflight/src/fc/board_info.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/fc/board_info.h -------------------------------------------------------------------------------- /lib/betaflight/src/fc/config.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/fc/config.h -------------------------------------------------------------------------------- /lib/betaflight/src/fc/controlrate_profile.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/fc/controlrate_profile.h -------------------------------------------------------------------------------- /lib/betaflight/src/fc/parameter_names.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/fc/parameter_names.h -------------------------------------------------------------------------------- /lib/betaflight/src/fc/rc.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/fc/rc.h -------------------------------------------------------------------------------- /lib/betaflight/src/fc/rc_controls.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/fc/rc_controls.h -------------------------------------------------------------------------------- /lib/betaflight/src/fc/rc_modes.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/fc/rc_modes.h -------------------------------------------------------------------------------- /lib/betaflight/src/fc/runtime_config.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/fc/runtime_config.h -------------------------------------------------------------------------------- /lib/betaflight/src/flight/failsafe.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/flight/failsafe.h -------------------------------------------------------------------------------- /lib/betaflight/src/flight/gps_rescue.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/flight/gps_rescue.h -------------------------------------------------------------------------------- /lib/betaflight/src/flight/mixer.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/flight/mixer.h -------------------------------------------------------------------------------- /lib/betaflight/src/flight/navigation.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/flight/navigation.h -------------------------------------------------------------------------------- /lib/betaflight/src/flight/pid.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/flight/pid.h -------------------------------------------------------------------------------- /lib/betaflight/src/flight/position.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/flight/position.h -------------------------------------------------------------------------------- /lib/betaflight/src/flight/rpm_filter.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/flight/rpm_filter.h -------------------------------------------------------------------------------- /lib/betaflight/src/flight/servos.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/flight/servos.h -------------------------------------------------------------------------------- /lib/betaflight/src/io/asyncfatfs/asyncfatfs.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/io/asyncfatfs/asyncfatfs.h -------------------------------------------------------------------------------- /lib/betaflight/src/io/beeper.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/io/beeper.h -------------------------------------------------------------------------------- /lib/betaflight/src/io/flashfs.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/io/flashfs.h -------------------------------------------------------------------------------- /lib/betaflight/src/io/gps.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/io/gps.h -------------------------------------------------------------------------------- /lib/betaflight/src/io/serial.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/io/serial.h -------------------------------------------------------------------------------- /lib/betaflight/src/io/serial_4way.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of Cleanflight and Betaflight. 3 | * 4 | * Cleanflight and Betaflight are free software. You can redistribute 5 | * this software and/or modify this software under the terms of the 6 | * GNU General Public License as published by the Free Software 7 | * Foundation, either version 3 of the License, or (at your option) 8 | * any later version. 9 | * 10 | * Cleanflight and Betaflight are distributed in the hope that they 11 | * will be useful, but WITHOUT ANY WARRANTY; without even the implied 12 | * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 13 | * See the GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this software. 17 | * 18 | * If not, see . 19 | */ 20 | 21 | /* 22 | * Author: 4712 23 | */ 24 | #pragma once 25 | 26 | #include "drivers/io_types.h" 27 | #include "io/serial_4way_impl.h" 28 | 29 | #define imC2 0 30 | #define imSIL_BLB 1 31 | #define imATM_BLB 2 32 | #define imSK 3 33 | #define imARM_BLB 4 34 | 35 | extern uint8_t selected_esc; 36 | 37 | extern ioMem_t ioMem; 38 | 39 | typedef union __attribute__ ((packed)) { 40 | uint8_t bytes[2]; 41 | uint16_t word; 42 | } uint8_16_u; 43 | 44 | typedef union __attribute__ ((packed)) { 45 | uint8_t bytes[4]; 46 | uint16_t words[2]; 47 | uint32_t dword; 48 | } uint8_32_u; 49 | 50 | //extern uint8_32_u DeviceInfo; 51 | 52 | bool isMcuConnected(void); 53 | uint8_t esc4wayInit(void); 54 | struct serialPort_s; 55 | void esc4wayProcess(struct serialPort_s *mspPort); 56 | void esc4wayRelease(void); 57 | -------------------------------------------------------------------------------- /lib/betaflight/src/io/serial_4way_avrootloader.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of Cleanflight and Betaflight. 3 | * 4 | * Cleanflight and Betaflight are free software. You can redistribute 5 | * this software and/or modify this software under the terms of the 6 | * GNU General Public License as published by the Free Software 7 | * Foundation, either version 3 of the License, or (at your option) 8 | * any later version. 9 | * 10 | * Cleanflight and Betaflight are distributed in the hope that they 11 | * will be useful, but WITHOUT ANY WARRANTY; without even the implied 12 | * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 13 | * See the GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this software. 17 | * 18 | * If not, see . 19 | */ 20 | 21 | /* 22 | * Author: 4712 23 | * for info about Hagens AVRootloader: 24 | * http://www.mikrocontroller.net/topic/avr-bootloader-mit-verschluesselung 25 | */ 26 | 27 | #pragma once 28 | // Bootloader result codes 29 | #define brSUCCESS 0x30 30 | #define brERRORVERIFY 0xC0 31 | #define brERRORCOMMAND 0xC1 32 | #define brERRORCRC 0xC2 33 | #define brNONE 0xFF 34 | 35 | void BL_SendBootInit(void); 36 | uint8_t BL_ConnectEx(uint8_32_u *pDeviceInfo); 37 | uint8_t BL_SendCMDKeepAlive(void); 38 | uint8_t BL_PageErase(ioMem_t *pMem); 39 | uint8_t BL_ReadEEprom(ioMem_t *pMem); 40 | uint8_t BL_WriteEEprom(ioMem_t *pMem); 41 | uint8_t BL_WriteFlash(ioMem_t *pMem); 42 | uint8_t BL_ReadFlash(uint8_t interface_mode, ioMem_t *pMem); 43 | uint8_t BL_VerifyFlash(ioMem_t *pMem); 44 | void BL_SendCMDRunRestartBootloader(uint8_32_u *pDeviceInfo); 45 | -------------------------------------------------------------------------------- /lib/betaflight/src/io/serial_4way_impl.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of Cleanflight and Betaflight. 3 | * 4 | * Cleanflight and Betaflight are free software. You can redistribute 5 | * this software and/or modify this software under the terms of the 6 | * GNU General Public License as published by the Free Software 7 | * Foundation, either version 3 of the License, or (at your option) 8 | * any later version. 9 | * 10 | * Cleanflight and Betaflight are distributed in the hope that they 11 | * will be useful, but WITHOUT ANY WARRANTY; without even the implied 12 | * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 13 | * See the GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this software. 17 | * 18 | * If not, see . 19 | */ 20 | 21 | /* 22 | * Author: 4712 23 | */ 24 | #pragma once 25 | 26 | #include "drivers/io_types.h" 27 | 28 | typedef struct { 29 | IO_t io; 30 | } escHardware_t; 31 | 32 | extern uint8_t selected_esc; 33 | 34 | bool isEscHi(uint8_t selEsc); 35 | bool isEscLo(uint8_t selEsc); 36 | void setEscHi(uint8_t selEsc); 37 | void setEscLo(uint8_t selEsc); 38 | void setEscInput(uint8_t selEsc); 39 | void setEscOutput(uint8_t selEsc); 40 | 41 | #define ESC_IS_HI isEscHi(selected_esc) 42 | #define ESC_IS_LO isEscLo(selected_esc) 43 | #define ESC_SET_HI setEscHi(selected_esc) 44 | #define ESC_SET_LO setEscLo(selected_esc) 45 | #define ESC_INPUT setEscInput(selected_esc) 46 | #define ESC_OUTPUT setEscOutput(selected_esc) 47 | 48 | typedef struct ioMem_s { 49 | uint8_t D_NUM_BYTES; 50 | uint8_t D_FLASH_ADDR_H; 51 | uint8_t D_FLASH_ADDR_L; 52 | uint8_t *D_PTR_I; 53 | } ioMem_t; 54 | -------------------------------------------------------------------------------- /lib/betaflight/src/io/serial_4way_stk500v2.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of Cleanflight and Betaflight. 3 | * 4 | * Cleanflight and Betaflight are free software. You can redistribute 5 | * this software and/or modify this software under the terms of the 6 | * GNU General Public License as published by the Free Software 7 | * Foundation, either version 3 of the License, or (at your option) 8 | * any later version. 9 | * 10 | * Cleanflight and Betaflight are distributed in the hope that they 11 | * will be useful, but WITHOUT ANY WARRANTY; without even the implied 12 | * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 13 | * See the GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this software. 17 | * 18 | * If not, see . 19 | */ 20 | 21 | /* 22 | * Author: 4712 23 | */ 24 | #pragma once 25 | 26 | uint8_t Stk_SignOn(void); 27 | uint8_t Stk_ConnectEx(uint8_32_u *pDeviceInfo); 28 | uint8_t Stk_ReadEEprom(ioMem_t *pMem); 29 | uint8_t Stk_WriteEEprom(ioMem_t *pMem); 30 | uint8_t Stk_ReadFlash(ioMem_t *pMem); 31 | uint8_t Stk_WriteFlash(ioMem_t *pMem); 32 | uint8_t Stk_Chip_Erase(void); 33 | -------------------------------------------------------------------------------- /lib/betaflight/src/msp/msp_protocol_v2_betaflight.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of Cleanflight, Betaflight and INAV. 3 | * 4 | * Cleanflight and Betaflight are free software. You can redistribute 5 | * this software and/or modify this software under the terms of the 6 | * GNU General Public License as published by the Free Software 7 | * Foundation, either version 3 of the License, or (at your option) 8 | * any later version. 9 | * 10 | * Cleanflight, Betaflight and INAV are distributed in the hope that they 11 | * will be useful, but WITHOUT ANY WARRANTY; without even the implied 12 | * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 13 | * See the GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this software. 17 | * 18 | * If not, see . 19 | */ 20 | 21 | #define MSP2_BETAFLIGHT_BIND 0x3000 22 | #define MSP2_MOTOR_OUTPUT_REORDERING 0x3001 23 | #define MSP2_SET_MOTOR_OUTPUT_REORDERING 0x3002 24 | -------------------------------------------------------------------------------- /lib/betaflight/src/msp/msp_protocol_v2_common.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of Cleanflight, Betaflight and INAV. 3 | * 4 | * Cleanflight and Betaflight are free software. You can redistribute 5 | * this software and/or modify this software under the terms of the 6 | * GNU General Public License as published by the Free Software 7 | * Foundation, either version 3 of the License, or (at your option) 8 | * any later version. 9 | * 10 | * Cleanflight, Betaflight and INAV are distributed in the hope that they 11 | * will be useful, but WITHOUT ANY WARRANTY; without even the implied 12 | * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 13 | * See the GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this software. 17 | * 18 | * If not, see . 19 | */ 20 | 21 | #define MSP2_COMMON_SERIAL_CONFIG 0x1009 22 | #define MSP2_COMMON_SET_SERIAL_CONFIG 0x100A 23 | -------------------------------------------------------------------------------- /lib/betaflight/src/msp/msp_serial.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/msp/msp_serial.h -------------------------------------------------------------------------------- /lib/betaflight/src/pg/motor.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/pg/motor.h -------------------------------------------------------------------------------- /lib/betaflight/src/pg/pg.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/pg/pg.h -------------------------------------------------------------------------------- /lib/betaflight/src/pg/pg_ids.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/pg/pg_ids.h -------------------------------------------------------------------------------- /lib/betaflight/src/pg/rx.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/pg/rx.h -------------------------------------------------------------------------------- /lib/betaflight/src/rx/rx.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/rx/rx.h -------------------------------------------------------------------------------- /lib/betaflight/src/sensors/acceleration.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/sensors/acceleration.h -------------------------------------------------------------------------------- /lib/betaflight/src/sensors/barometer.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/sensors/barometer.h -------------------------------------------------------------------------------- /lib/betaflight/src/sensors/battery.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/sensors/battery.h -------------------------------------------------------------------------------- /lib/betaflight/src/sensors/compass.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/sensors/compass.h -------------------------------------------------------------------------------- /lib/betaflight/src/sensors/gyro.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/sensors/gyro.h -------------------------------------------------------------------------------- /lib/betaflight/src/sensors/rangefinder.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/sensors/rangefinder.h -------------------------------------------------------------------------------- /lib/betaflight/src/sensors/sonar.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rtlopez/esp-fc/65e9a1d7c25eca0cb39deab851c6b014d74b0ba6/lib/betaflight/src/sensors/sonar.h -------------------------------------------------------------------------------- /lib/printf/library.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "printf", 3 | "version": "1.0.0" 4 | } 5 | -------------------------------------------------------------------------------- /lib/readme.txt: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for the project specific (private) libraries. 3 | PlatformIO will compile them to static libraries and link to executable file. 4 | 5 | The source code of each library should be placed in separate directory, like 6 | "lib/private_lib/[here are source files]". 7 | 8 | For example, see how can be organized `Foo` and `Bar` libraries: 9 | 10 | |--lib 11 | | |--Bar 12 | | | |--docs 13 | | | |--examples 14 | | | |--src 15 | | | |- Bar.c 16 | | | |- Bar.h 17 | | |--Foo 18 | | | |- Foo.c 19 | | | |- Foo.h 20 | | |- readme.txt --> THIS FILE 21 | |- platformio.ini 22 | |--src 23 | |- main.c 24 | 25 | Then in `src/main.c` you should use: 26 | 27 | #include 28 | #include 29 | 30 | // rest H/C/CPP code 31 | 32 | PlatformIO will find your libraries automatically, configure preprocessor's 33 | include paths and build them. 34 | 35 | More information about PlatformIO Library Dependency Finder 36 | - http://docs.platformio.org/page/librarymanager/ldf.html 37 | -------------------------------------------------------------------------------- /merge_firmware.py: -------------------------------------------------------------------------------- 1 | Import("env") 2 | 3 | APP_BIN = "$BUILD_DIR/${PROGNAME}.bin" 4 | MERGED_BIN = "$BUILD_DIR/${PROGNAME}_0x00.bin" 5 | BOARD_CONFIG = env.BoardConfig() 6 | 7 | 8 | def merge_bin(source, target, env): 9 | # The list contains all extra images (bootloader, partitions, eboot) and 10 | # the final application binary 11 | flash_images = env.Flatten(env.get("FLASH_EXTRA_IMAGES", [])) + ["$ESP32_APP_OFFSET", APP_BIN] 12 | 13 | # Run esptool to merge images into a single binary 14 | env.Execute( 15 | " ".join( 16 | [ 17 | "$PYTHONEXE", 18 | "$OBJCOPY", 19 | "--chip", 20 | BOARD_CONFIG.get("build.mcu", "esp32"), 21 | "merge_bin", 22 | "--flash_size", 23 | BOARD_CONFIG.get("upload.flash_size", "4MB"), 24 | "-o", 25 | MERGED_BIN, 26 | ] 27 | + flash_images 28 | ) 29 | ) 30 | 31 | # Add a post action that runs esptoolpy to merge available flash images 32 | env.AddPostAction(APP_BIN , merge_bin) 33 | 34 | # Patch the upload command to flash the merged binary at address 0x0 35 | #env.Replace( 36 | # UPLOADERFLAGS=[ 37 | # f 38 | # for f in env.get("UPLOADERFLAGS") 39 | # if f not in env.Flatten(env.get("FLASH_EXTRA_IMAGES")) 40 | # ] 41 | # + ["0x0", MERGED_BIN], 42 | # UPLOADCMD='"$PYTHONEXE" "$UPLOADER" $UPLOADERFLAGS', 43 | #) -------------------------------------------------------------------------------- /partitions_4M_nota.csv: -------------------------------------------------------------------------------- 1 | # Name, Type, SubType, Offset, Size, Flags 2 | nvs, data, nvs, 0x009000, 0x005000, 3 | otadata, data, ota, 0x00e000, 0x002000, 4 | app0, app, ota_0, 0x010000, 0x140000, 5 | spiffs, data, spiffs, 0x150000, 0x2A0000, 6 | coredump, data, coredump,0x3F0000, 0x010000, 7 | --------------------------------------------------------------------------------