Repository: dgatf/msrc Branch: master Commit: 95b865993dd2 Files: 189 Total size: 1.4 MB Directory structure: gitextract_7zabuz1p/ ├── .clang-format ├── .gitattributes ├── .github/ │ ├── copilot-instructions.md │ └── workflows/ │ ├── build_release.yaml │ ├── ci_firmware.yaml │ ├── ci_msrc_link.yaml │ ├── dev_firmware.yaml │ └── dev_msrc_link.yaml ├── .gitignore ├── .gitmodules ├── .vscode/ │ ├── c_cpp_properties.json │ ├── launch.json │ └── settings.json ├── LICENSE ├── README.md ├── board/ │ ├── CMakeLists.txt │ ├── freertos/ │ │ ├── CMakeLists.txt │ │ └── FreeRTOSConfig.h │ ├── pico_sdk_import.cmake │ └── project/ │ ├── CMakeLists.txt │ ├── common.c │ ├── common.h │ ├── config.c │ ├── config.h │ ├── constants.h │ ├── dbg_rb.c │ ├── dbg_rb.h │ ├── dbg_task.c │ ├── dbg_task.h │ ├── led.c │ ├── led.h │ ├── main.c │ ├── pio/ │ │ ├── CMakeLists.txt │ │ ├── capture_edge.c │ │ ├── capture_edge.h │ │ ├── capture_edge.pio │ │ ├── castle_link.c │ │ ├── castle_link.h │ │ ├── castle_link.pio │ │ ├── i2c_multi.c │ │ ├── i2c_multi.h │ │ ├── i2c_multi.pio │ │ ├── uart_rx.c │ │ ├── uart_rx.h │ │ ├── uart_rx.pio │ │ ├── uart_tx.c │ │ ├── uart_tx.h │ │ ├── uart_tx.pio │ │ ├── ws2812.c │ │ ├── ws2812.h │ │ └── ws2812.pio │ ├── protocol/ │ │ ├── CMakeLists.txt │ │ ├── crsf.c │ │ ├── crsf.h │ │ ├── fbus.c │ │ ├── fbus.h │ │ ├── fport.c │ │ ├── fport.h │ │ ├── frsky_d.c │ │ ├── frsky_d.h │ │ ├── ghst.c │ │ ├── ghst.h │ │ ├── hitec.c │ │ ├── hitec.h │ │ ├── hott.c │ │ ├── hott.h │ │ ├── ibus.c │ │ ├── ibus.h │ │ ├── jetiex.c │ │ ├── jetiex.h │ │ ├── jetiex_sensor.c │ │ ├── jetiex_sensor.h │ │ ├── jr_dmss.c │ │ ├── jr_dmss.h │ │ ├── multiplex.c │ │ ├── multiplex.h │ │ ├── sanwa.c │ │ ├── sanwa.h │ │ ├── sbus.c │ │ ├── sbus.h │ │ ├── smartport.c │ │ ├── smartport.h │ │ ├── srxl.c │ │ ├── srxl.h │ │ ├── srxl2.c │ │ ├── srxl2.h │ │ ├── xbus.c │ │ └── xbus.h │ ├── sensor/ │ │ ├── CMakeLists.txt │ │ ├── airspeed.c │ │ ├── airspeed.h │ │ ├── auto_offset.c │ │ ├── auto_offset.h │ │ ├── bmp180.c │ │ ├── bmp180.h │ │ ├── bmp280.c │ │ ├── bmp280.h │ │ ├── cell_count.c │ │ ├── cell_count.h │ │ ├── current.c │ │ ├── current.h │ │ ├── distance.c │ │ ├── distance.h │ │ ├── esc_apd_f.c │ │ ├── esc_apd_f.h │ │ ├── esc_apd_hv.c │ │ ├── esc_apd_hv.h │ │ ├── esc_castle.c │ │ ├── esc_castle.h │ │ ├── esc_hw3.c │ │ ├── esc_hw3.h │ │ ├── esc_hw4.c │ │ ├── esc_hw4.h │ │ ├── esc_hw5.c │ │ ├── esc_hw5.h │ │ ├── esc_kontronik.c │ │ ├── esc_kontronik.h │ │ ├── esc_omp_m4.c │ │ ├── esc_omp_m4.h │ │ ├── esc_openyge.c │ │ ├── esc_openyge.h │ │ ├── esc_pwm.c │ │ ├── esc_pwm.h │ │ ├── esc_ztw.c │ │ ├── esc_ztw.h │ │ ├── fuel_meter.c │ │ ├── fuel_meter.h │ │ ├── gpio.c │ │ ├── gpio.h │ │ ├── gps.c │ │ ├── gps.h │ │ ├── ina3221.c │ │ ├── ina3221.h │ │ ├── mpu6050.c │ │ ├── mpu6050.h │ │ ├── ms5611.c │ │ ├── ms5611.h │ │ ├── ntc.c │ │ ├── ntc.h │ │ ├── pwm_out.c │ │ ├── pwm_out.h │ │ ├── smart_esc.c │ │ ├── smart_esc.h │ │ ├── voltage.c │ │ ├── voltage.h │ │ ├── vspeed.c │ │ ├── vspeed.h │ │ ├── xgzp68xxd.c │ │ └── xgzp68xxd.h │ ├── serial_monitor.c │ ├── serial_monitor.h │ ├── sim_rx.c │ ├── sim_rx.h │ ├── uart.c │ ├── uart.h │ ├── uart_pio.c │ ├── uart_pio.h │ ├── usb.c │ └── usb.h ├── case/ │ ├── MSRC_case_zero-Lower case.stl │ ├── MSRC_case_zero-Upper case.stl │ ├── MSRC_case_zero.3mf │ ├── MSRC_case_zero.FCStd │ └── MSRC_case_zero_PLA_1h27m.gcode ├── include/ │ └── shared.h ├── lua/ │ └── MSRC.lua ├── msrc_gui/ │ ├── appdir/ │ │ └── msrc.desktop │ ├── circuitdialog.cpp │ ├── circuitdialog.h │ ├── circuitdialog.ui │ ├── main.cpp │ ├── mainwindow.cpp │ ├── mainwindow.h │ ├── mainwindow.ui │ ├── msrc_gui.pro │ └── resources.qrc └── msrc_gui_web/ ├── README.md ├── css/ │ └── style.css ├── index.html ├── js/ │ ├── app.js │ ├── circuit.js │ ├── config_struct.js │ ├── serial.js │ ├── ui.js │ └── version.js ├── manifest.json ├── sw.js └── test/ ├── layout_test.html └── verify_layout.js ================================================ FILE CONTENTS ================================================ ================================================ FILE: .clang-format ================================================ BasedOnStyle: Google IndentWidth: 4 ColumnLimit: 120 ================================================ FILE: .gitattributes ================================================ case/** filter=lfs diff=lfs merge=lfs -text case/** linguist-detectable=false ================================================ FILE: .github/copilot-instructions.md ================================================ # MSRC – Copilot Instructions ## Project Overview MSRC (Multi Sensor for RC) is a telemetry system for RC models running on the **RP2040** microcontroller. It reads data from sensors (ESC, GPS, barometer, analog) and transmits it to an RC receiver using one of ~19 telemetry protocols. A companion **Qt5 desktop app** (`msrc_gui/`) configures the board over USB CDC. A Lua script (`lua/MSRC.lua`) enables wireless config from OpenTX/EdgeTX transmitters via SmartPort. ## Architecture ``` main.c → protocol_task (priority 3-4) → set_config() → sensor tasks (priority 2) ├── usb_task (priority 1) GUI ↔ board binary protocol ├── led_task (priority 1) WS2812 on GPIO 16 └── dbg_task (priority 1) Ring-buffer debug output ``` **Data flow:** Sensor tasks write to shared `float *` pointers (atomic on Cortex-M0+). Protocol tasks read these floats, format them per-protocol, and transmit to the receiver via UART/PIO. No mutexes are used for sensor data—relies on aligned 32-bit atomic writes. **UART notification model:** ISRs push bytes into FreeRTOS queues, then after a packet-gap timeout call `vTaskNotifyGiveIndexedFromISR(context.uart0_notify_task_handle, 1, ...)`. Tasks block on `ulTaskNotifyTakeIndexed(1, pdTRUE, portMAX_DELAY)`. ## Key Directories | Path | Purpose | |------|---------| | `board/project/protocol/` | 18 receiver protocol implementations (smartport, crsf, ibus, sbus, etc.) | | `board/project/sensor/` | ~30 sensor drivers (ESC variants, GPS, baro, analog, fuel, GPIO) | | `board/project/pio/` | PIO programs (.pio + .c/.h): soft UART, Castle Link, I2C, WS2812 | | `board/project/` | Core firmware: main.c, config, UART, USB, LED, common utilities | | `include/shared.h` | **Shared between firmware AND GUI** — all enums (`rx_protocol_t`, `esc_protocol_t`) and `config_t` struct | | `msrc_gui/` | Qt5 desktop configuration app (MSRC Link) | | `lua/MSRC.lua` | OpenTX/EdgeTX Lua script for wireless config via SmartPort | ## Protocol Communication Models Protocols follow two models: ### Request/Response (SmartPort, IBUS, XBUS, Hitec, SBUS, SRXL, Multiplex, JetiEx, Sanwa, HOTT, JR DMSS) The protocol task blocks on `ulTaskNotifyTakeIndexed(1, pdTRUE, portMAX_DELAY)` until the receiver sends a request. The task then parses the request, looks up the relevant sensor `float *` values, formats them per protocol, and sends a response. ### Periodic Push (CRSF, GHST, FBUS, FPort) The protocol task sends telemetry frames at a fixed interval using `vTaskDelay()`, reading current sensor floats each cycle. **SmartPort is special** — besides telemetry, it supports a maintenance/config mode for the Lua configuration script. Commands `0x30`/`0x31`/`0x32` get/set individual `config_t` fields by their SmartPort `data_id`. ## Adding a New Sensor Follow the established pattern (see `board/project/sensor/esc_hw3.c` as reference): 1. **Header** — Define a parameter struct with config fields and `float *` output pointers: ```c typedef struct my_sensor_parameters_t { float alpha; float *value; // allocated by protocol layer } my_sensor_parameters_t; ``` 2. **Task function** — `void my_sensor_task(void *parameters)`: - Copy parameter struct locally: `my_sensor_parameters_t parameter = *(my_sensor_parameters_t *)parameters;` - Signal readiness: `xTaskNotifyGive(context.receiver_task_handle);` - Initialize output: `*parameter.value = 0;` - Loop: read hardware, apply `get_average()`, write to `*parameter.value` 3. **Wire in protocol** — In each protocol's `set_config()`, allocate `malloc(sizeof(float))` for the output pointer and create the sensor task with `xTaskCreate`. Wait for task init: `ulTaskNotifyTake(pdTRUE, portMAX_DELAY)`. 4. **CMake** — Add the `.c` file to `board/project/sensor/CMakeLists.txt`. 5. **Stack size** — Define `STACK_MY_SENSOR` in `constants.h` as `(measured_min + STACK_EXTRA)`. ## Adding a New Protocol Follow the pattern in `board/project/protocol/ibus.c`: 1. **`set_config()`** — Read `config_t`, create sensor tasks based on enabled sensors, build protocol-specific sensor list. 2. **Main task** — Init UART via `uart0_begin()`, call `set_config()`, enter loop (request/response or periodic push). 3. **Format function** — Convert `float *` sensor values to protocol wire format (handle byte order, scaling, units). 4. **Register in `main.c`** — Add `case RX_MY_PROTO:` to the switch, calling `xTaskCreate(my_proto_task, ...)`. 5. **Update enums** — Add to `rx_protocol_t` in `include/shared.h` (**both** `#ifdef __cplusplus` and C blocks must be updated in sync). 6. **Update GUI** — Add the protocol to `msrc_gui/mainwindow.cpp` combo box (`ui->cbReceiver->addItem(...)`) and handle protocol-specific widget visibility in `on_cbReceiver_currentTextChanged()`. ## Spektrum Smart ESC/BAT Sensor The Smart ESC driver (`board/project/sensor/smart_esc.c`) is the most complex sensor. It **emulates a Spektrum SRXL2 receiver** to communicate with a Spektrum Smart ESC/Battery over UART1 at 115200 baud. ### How it works 1. **PWM capture** — Uses PIO (`capture_edge`, pio0) to read throttle (GPIO 12) and reverse (GPIO 13) PWM signals from the actual receiver. 2. **SRXL2 handshake** — On startup, performs SRXL2 handshake with the ESC (ID `0x40`), presenting itself as receiver ID `0x21`. 3. **Control packets** — Every 10ms (`SRXL2_INTERVAL_MS`), sends `SRXL2_PACKET_TYPE_CONTROL` packets forwarding captured throttle/reverse channels. Requests telemetry every 10th packet (`reply_id = esc_id`). 4. **Telemetry parsing** — Receives telemetry in XBUS format, parsing two device types: - **`XBUS_ESC_ID`**: RPM, voltage, current, FET temp, BEC voltage/current, BEC temp - **`XBUS_SMART_BAT` (`0x42`)**: Sub-types `0x00` (realtime: temp, current, consumption), `0x10`/`0x20`/`0x30` (individual cell voltages, up to 18), `0x80` (battery ID: cell count, cycles) ### Parameter struct (extensive) ```c typedef struct smart_esc_parameters_t { bool calc_consumption; float rpm_multiplier; float alpha_rpm, alpha_voltage, alpha_current, alpha_temperature; float *rpm, *voltage, *current, *temperature_fet, *temperature_bec, *voltage_bec, *current_bec; float *temperature_bat, *current_bat, *consumption; float *cell[18]; uint8_t *cells; uint16_t *cycles; } smart_esc_parameters_t; ``` ### Protocol wiring In `set_config()`, Smart ESC is created at **priority 4** (higher than normal sensors) because it needs low-latency SRXL2 timing. All 18 cell pointers are individually `malloc`'d. All protocols support Smart ESC — the wiring pattern in `set_config()` allocates ~25 float pointers: ```c if (config->esc_protocol == ESC_SMART) { smart_esc_parameters_t parameter; parameter.rpm = malloc(sizeof(float)); // ... allocate all pointers ... for (uint i = 0; i < 18; i++) parameter.cell[i] = malloc(sizeof(float)); parameter.cells = malloc(sizeof(uint8_t)); parameter.cycles = malloc(sizeof(uint16_t)); xTaskCreate(smart_esc_task, "smart_esc_task", STACK_SMART_ESC, (void *)¶meter, 4, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } ``` Config field: `config->smart_esc_calc_consumption` — if `true`, consumption is calculated from motor current via `get_consumption()` instead of using the Smart Battery's reported value. ## Config System - `config_t` is defined in `include/shared.h` — shared across firmware, GUI, and Lua script. - Stored in **RP2040 flash** at offset 768KB (`CONFIG_FLASH_TARGET_OFFSET`). Read via XIP pointer, written with interrupts disabled (`save_and_disable_interrupts()`). - Supports migration from old flash offset (512KB → 768KB) in `config_read()`. - Each field has a SmartPort `data_id` comment (0x5101–0x5152) used by the Lua config script. - **When adding fields:** append before `spare` fields and decrement a spare. Bump `CONFIG_VERSION` in `config.h`. Update `config_forze_write()` defaults in `config.c`. - Default values via compile-time `#define`s at top of `config.c` (e.g. `#define RX_PROTOCOL RX_SMARTPORT`). - Defaults can be force-written holding GPIO 15 low at boot, or by `CONFIG_FORZE_WRITE`. ## USB Protocol (Board ↔ GUI) Binary protocol over CDC USB serial. All messages start with header byte `0x30`: | Bytes | Direction | Description | |-------|-----------|-------------| | `0x30 0x30` + `config_t` bytes | GUI→Board | Write config to flash (board blinks LED, writes, requires reset) | | `0x30 0x31` | GUI→Board | Request current config | | `0x30 0x32` + `config_t` bytes | Board→GUI | Config response | | `0x30 0x33` | GUI→Board | Enable debug output (text stream over USB) | | `0x30 0x34` | GUI→Board | Disable debug output | | `0x30 0x35` | GUI→Board | Force write default config to flash | The USB task (`board/project/usb.c`) polls every 1 second with `getchar_timeout_us(1000)`. Config is sent/received as raw `config_t` struct bytes — **both sides must agree on struct layout and `CONFIG_VERSION`**. ## GUI Application (MSRC Link) Qt5/C++ desktop app in `msrc_gui/`. Key files: | File | Purpose | |------|---------| | `mainwindow.cpp/.h` | Main application logic: serial comm, config UI, circuit diagram | | `mainwindow.ui` | Qt Designer UI layout | | `circuitdialog.cpp/.h` | Zoomable circuit diagram overlay dialog | | `circuitdialog.ui` | Circuit dialog layout | | `main.cpp` | App entry point | | `msrc_gui.pro` | qmake project file | | `res/` | PNG images for circuit diagram overlays (one per sensor/receiver type) | ### Architecture - Uses `QSerialPort` at 115200/8N1 for USB CDC communication. - Port list is auto-refreshed every 1 second via `QTimer`. - On connect: disables debug, waits 2 seconds, requests config. Config arrives as `sizeof(config_t) + 2` bytes, validated by header `[0x30, 0x32]` and `config.version`. - Two main flows: `setUiFromConfig()` (config struct → UI widgets) and `getConfigFromUi()` (UI widgets → config struct). These must be updated whenever `config_t` changes. - **Circuit diagram**: `generateCircuit()` composites PNG overlays on a base RP2040 Zero pinout image depending on which sensors/receiver are enabled. Each sensor/receiver type has a dedicated PNG in `res/`. - **Debug mode**: when enabled, all serial data is displayed as text in the debug panel (`ptDebug`). - Config can be saved/loaded as `.cfg` binary files (raw `config_t` dump). - Protocol-specific widgets are shown/hidden dynamically in `on_cbReceiver_currentTextChanged()` — e.g. fuel meter only for SmartPort/JetiEx/XBUS/HOTT/FPort/FBUS; GPIO only for SmartPort/FPort/FBUS. - ESC combo maps directly to `esc_protocol_t` enum values (index + 1, since ESC_NONE = 0 means unchecked). ### When modifying the GUI 1. Update both `setUiFromConfig()` and `getConfigFromUi()` — they are the bidirectional mapping between `config_t` and UI. 2. For new receiver protocols: add to `cbReceiver` combo with the enum value as `userData`, update visibility logic in `on_cbReceiver_currentTextChanged()`. 3. For new sensors: add UI widgets in `mainwindow.ui`, update circuit diagram with a new PNG overlay. ## GPIO Pinout | GPIO | Function | |------|----------| | 0 | UART0 TX → receiver RX | | 1 | UART0 RX ← receiver TX | | 2 | I2C1 SDA (receiver, with pull-up) | | 3 | I2C1 SCL (receiver, with pull-up) | | 4 | PWM capture / Castle PWM / UART1 TX (unused) | | 5 | UART1 RX ← ESC TX / Castle ESC / Serial monitor | | 6 | PIO UART RX ← GPS TX | | 7 | Clock stretch NPN switch (XBUS) | | 8 | I2C0 SDA (vario/sensors) | | 9 | I2C0 SCL (vario/sensors) | | 10 | PWM out | | 11 | Fuel meter PWM capture | | 12 | Smart ESC throttle PWM capture | | 13 | Smart ESC reverse PWM capture | | 14 | PIO UART TX → GPS RX | | 15 | Restore default config (pull low at boot) | | 16 | WS2812 LED | | 17–22 | Digital GPIO switches | | 26 | ADC0 — Voltage | | 27 | ADC1 — Current | | 28 | ADC2 — NTC temperature | | 29 | ADC3 — Airspeed | Assignments are in `board/project/constants.h` — **do not reuse pins** without checking this table. ## Lua Configuration Script `lua/MSRC.lua` runs on OpenTX/EdgeTX transmitters for **wireless in-field configuration** via SmartPort telemetry. It reads/writes individual `config_t` fields by their `data_id` (0x5101–0x5152) using SmartPort maintenance mode (`0x30`/`0x31` frame commands). Pages: sensor ID, refresh rates, averaging, ESC config, GPS, vario, fuel meter, GPIO, analog sensors, gyro. When adding a `config_t` field addressable via Lua: assign a `data_id` comment in `shared.h`, handle it in SmartPort maintenance mode (`smartport.c`), and add the corresponding entry in `MSRC.lua`. ## Build ```sh # Firmware (requires Pico SDK, ARM toolchain) cd board && mkdir build && cd build cmake .. && make -j$(nproc) # Output: MSRC-RP2040.uf2 # GUI (requires Qt5, including QtSerialPort) cd msrc_gui && qmake && make ``` Version is derived from `git describe --tags`. The version string is injected as `PROJECT_VERSION` define in both firmware and GUI. ### Debugging with SWD probe In `constants.h`, there are commented-out lines to remap UART from uart0 to uart1 for probe debugging. In `board/project/CMakeLists.txt`, toggle `pico_enable_stdio_usb` / `pico_enable_stdio_uart` (1/0 ↔ 0/1). ### Flashing Hold BOOTSEL on the RP2040, connect USB, drag `MSRC-RP2040.uf2` to the mass storage device. Or use `picotool`. ## Code Conventions - **C99** for firmware, **C++/Qt5** for GUI. No C++ in firmware code. - Global `context_t context` in `main.c` holds all FreeRTOS task/queue handles—never create additional globals for IPC. - Sensor averaging uses `get_average(alpha, prev, new)` (exponential moving average) from `common.c`. Alpha is stored in config as `float`, converted from user-facing "averaging elements" via `ALPHA(n) = 2.0 / (n + 1)`. - Debug output via `debug(...)` macro—guarded by runtime `context.debug` flag, not compile-time. Can be toggled via USB commands. - Timeout callbacks use `add_alarm_in_ms()` to zero sensor values on data loss. - `#ifdef SIM_SENSORS` / `#ifdef SIM_RX` inject fake data for testing without hardware. - PIO `.pio` files are compiled to headers via `pico_generate_pio_header()` in CMake. - Byte order: use `swap_16()` / `swap_32()` macros from `common.h` for protocol wire formats. - All structs used for wire protocols use `__attribute__((packed))`. ## FreeRTOS Priorities | Priority | Tasks | |----------|-------| | 4 | SmartPort, FPort, FBUS, Smart ESC (latency-sensitive) | | 3 | All other protocol tasks, timer service | | 2 | All sensor tasks | | 1 | USB, LED, debug | Time slicing is **disabled** (`configUSE_TIME_SLICING 0`). Equal-priority tasks do not round-robin — each must yield explicitly. FreeRTOS tick rate is 500Hz (2ms). Task notification index 1 is used for UART data arrival (`configTASK_NOTIFICATION_ARRAY_ENTRIES = 3`). ================================================ FILE: .github/workflows/build_release.yaml ================================================ name: Build Release on: release: types: [published] env: FIRMWARE_FILENAME: MSRC-RP2040_${{ github.ref_name }}.uf2 LINK_FILENAME: msrc_link_${{ github.ref_name }} permissions: contents: write pages: write id-token: write jobs: firmware: runs-on: ubuntu-latest steps: - name: Get actions code uses: actions/checkout@v4 with: submodules: 'true' - name: Git tags run: git fetch --prune --unshallow - name: Build firmware uses: samyarsadat/Pico-Build-Action@v1 with: source_dir: "board" output_dir: "../build" cmake_args: "-DCMAKE_BUILD_TYPE=Release" - name: Set version run: sudo mv build/project/MSRC-RP2040.uf2 build/project/MSRC-RP2040_${{ github.ref_name }}.uf2 - name: Release uses: softprops/action-gh-release@v2 with: files: build/project/MSRC-RP2040_${{ github.ref_name }}.uf2 - name: Generate attestation uses: actions/attest-build-provenance@v3 with: subject-path: build/project/MSRC-RP2040_${{ github.ref_name }}.uf2 github-token: ${{ secrets.TOKEN }} linux: runs-on: ubuntu-22.04 steps: - name: Get actions code uses: actions/checkout@v4 - name: Install QT uses: jurplel/install-qt-action@v4 with: version: '6.8.*' modules: qtserialport - name: Git tags run: git fetch --prune --unshallow - name: Build run: | mkdir build cd build qmake ../msrc_gui make - name: Install LinuxDeploy run: | cd msrc_gui wget https://github.com/probonopd/linuxdeployqt/releases/download/continuous/linuxdeployqt-continuous-x86_64.AppImage sudo chmod +x linuxdeployqt-continuous-x86_64.AppImage - name: Install fuse run: sudo apt install libfuse2 - name: Create AppImage run: | cp build/msrc_gui msrc_gui/appdir cd msrc_gui/appdir ../linuxdeployqt-continuous-x86_64.AppImage msrc.desktop -appimage -extra-plugins=iconengines,platformthemes/libqgtk3.so mv *.AppImage msrc_link_${{ github.ref_name }}.AppImage - name: Release uses: softprops/action-gh-release@v2 with: files: msrc_gui/appdir/msrc_link_${{ github.ref_name }}.AppImage - name: Generate attestation uses: actions/attest-build-provenance@v3 with: subject-path: msrc_gui/appdir/msrc_link_${{ github.ref_name }}.AppImage github-token: ${{ secrets.TOKEN }} macos: runs-on: macos-14 steps: - name: Get actions code uses: actions/checkout@v4 - name: Install QT uses: jurplel/install-qt-action@v4 with: version: '6.8.*' modules: qtserialport - name: Git tags run: git fetch --prune --unshallow - name: Build run: | mkdir build cd build qmake ../msrc_gui make mv msrc_gui.app msrc_link_${{ github.ref_name }}.app macdeployqt msrc_link_${{ github.ref_name }}.app -dmg - name: Release uses: softprops/action-gh-release@v2 with: files: build/msrc_link_${{ github.ref_name }}.dmg - name: Generate attestation uses: actions/attest-build-provenance@v3 with: subject-path: build/msrc_link_${{ github.ref_name }}.dmg github-token: ${{ secrets.TOKEN }} windows: runs-on: windows-2022 steps: - name: Get actions code uses: actions/checkout@v4 - name: Install qt static uses: orestonce/install-qt-static@v0.4.3 with: version: Qt6.5.3-Windows-x86_64-MinGW13.2.0-ucrt-staticFull-20240527 - name: Git tags run: git fetch --prune --unshallow - name: Build run: | mkdir build cd build qmake ../msrc_gui make mingw32-make release mv release/msrc_gui.exe release/msrc_link_${{ github.ref_name }}.exe - name: Release uses: softprops/action-gh-release@v2 with: files: build/release/msrc_link_${{ github.ref_name }}.exe - name: Generate attestation uses: actions/attest-build-provenance@v3 with: subject-path: build/release/msrc_link_${{ github.ref_name }}.exe github-token: ${{ secrets.TOKEN }} ================================================ FILE: .github/workflows/ci_firmware.yaml ================================================ # gh search commits --hash 272effd --repo dgatf/msrc name: CI - MSRC firmware on: workflow_dispatch: {} jobs: build: runs-on: ubuntu-latest permissions: contents: read id-token: write attestations: write steps: - name: Get actions code uses: actions/checkout@v4 with: submodules: 'true' - name: Git tags run: git fetch --prune --unshallow - name: Build firmware uses: samyarsadat/Pico-Build-Action@v1 with: source_dir: "board" output_dir: "../build" cmake_args: "-DCMAKE_BUILD_TYPE=Release" - name: Generate attestation uses: actions/attest-build-provenance@v3 with: subject-path: build/project/MSRC-RP2040.uf2 github-token: ${{ secrets.TOKEN }} #- name: Upload artifact # uses: actions/upload-artifact@v4 # with: # overwrite: 'true' # name: MSRC-RP2040.uf2 # path: build/project/MSRC-RP2040.uf2]] - name: Upload to Google Drive uses: logickoder/g-drive-upload@main with: credentials: ${{ secrets.GOOGLE_ID }} filename: 'build/project/MSRC-RP2040.uf2' folderId: ${{ secrets.FOLDER_ID }} overwrite: 'true' name: MSRC-RP2040.uf2 ================================================ FILE: .github/workflows/ci_msrc_link.yaml ================================================ name: CI - MSRC Link on: workflow_dispatch: {} jobs: linux: runs-on: ubuntu-22.04 permissions: contents: read id-token: write attestations: write steps: - name: Get actions code uses: actions/checkout@v4 - name: Install QT uses: jurplel/install-qt-action@v4 with: version: '6.8.*' modules: qtserialport - name: Git tags run: git fetch --prune --unshallow - name: Build run: | mkdir build cd build qmake ../msrc_gui make - name: Install LinuxDeploy run: | cd msrc_gui wget https://github.com/probonopd/linuxdeployqt/releases/download/continuous/linuxdeployqt-continuous-x86_64.AppImage sudo chmod +x linuxdeployqt-continuous-x86_64.AppImage - name: Install fuse run: sudo apt install libfuse2 - name: Create AppImage run: | cp build/msrc_gui msrc_gui/appdir cd msrc_gui/appdir ../linuxdeployqt-continuous-x86_64.AppImage msrc.desktop -appimage -extra-plugins=iconengines,platformthemes/libqgtk3.so mv *.AppImage msrc_gui.AppImage - name: Generate attestation uses: actions/attest-build-provenance@v3 with: subject-path: msrc_gui/appdir/msrc_gui.AppImage github-token: ${{ secrets.TOKEN }} - name: Upload to Google Drive uses: logickoder/g-drive-upload@main with: credentials: ${{ secrets.GOOGLE_ID }} filename: 'msrc_gui/appdir/msrc_gui.AppImage' folderId: ${{ secrets.FOLDER_ID }} overwrite: 'true' name: msrc_link.AppImage macos: runs-on: macos-14 permissions: contents: read id-token: write attestations: write steps: - name: Get actions code uses: actions/checkout@v4 - name: Install QT uses: jurplel/install-qt-action@v4 with: version: '6.8.*' modules: qtserialport - name: Git tags run: git fetch --prune --unshallow - name: Build run: | mkdir build cd build qmake ../msrc_gui make macdeployqt msrc_gui.app -dmg - name: Generate attestation uses: actions/attest-build-provenance@v3 with: subject-path: build/msrc_gui.dmg github-token: ${{ secrets.TOKEN }} - name: Upload to Google Drive uses: logickoder/g-drive-upload@main with: credentials: ${{ secrets.GOOGLE_ID }} filename: 'build/msrc_gui.dmg' folderId: ${{ secrets.FOLDER_ID }} overwrite: 'true' name: msrc_link.dmg windows: runs-on: windows-2022 permissions: contents: read id-token: write attestations: write steps: - name: Get actions code uses: actions/checkout@v4 - name: install qt static uses: orestonce/install-qt-static@v0.4.3 with: version: Qt6.5.3-Windows-x86_64-MinGW13.2.0-ucrt-staticFull-20240527 - name: Git tags run: git fetch --prune --unshallow - name: Build run: | mkdir build cd build qmake ../msrc_gui make mingw32-make release - name: Generate attestation uses: actions/attest-build-provenance@v3 with: subject-path: build/release/msrc_gui.exe github-token: ${{ secrets.TOKEN }} - name: Upload to Google Drive uses: logickoder/g-drive-upload@main with: credentials: ${{ secrets.GOOGLE_ID }} filename: 'build/release/msrc_gui.exe' folderId: ${{ secrets.FOLDER_ID }} overwrite: 'true' name: msrc_link.exe ================================================ FILE: .github/workflows/dev_firmware.yaml ================================================ name: Dev Release (Firmware) on: workflow_dispatch: {} push: branches: - master paths: - board/** permissions: contents: write id-token: write attestations: write concurrency: group: dev-release-firmware cancel-in-progress: true jobs: publish: runs-on: ubuntu-latest steps: - name: Checkout uses: actions/checkout@v4 with: submodules: true fetch-depth: 0 - name: Build firmware uses: samyarsadat/Pico-Build-Action@v1 with: source_dir: "board" output_dir: "../build" cmake_args: "-DCMAKE_BUILD_TYPE=Release" - name: Prepare artifact run: | mkdir -p dist cp build/project/MSRC-RP2040.uf2 dist/MSRC_firmware.uf2 - name: Generate attestation uses: actions/attest-build-provenance@v3 with: subject-path: dist/MSRC_firmware.uf2 github-token: ${{ secrets.GITHUB_TOKEN }} - name: Write release body run: | cat > release_body.md << EOF Development (auto) Firmware: - commit: "${{ github.event.head_commit.message }}" - commit-url: "https://github.com/${{ github.repository }}/commit/${{ github.sha }}" EOF - name: Publish firmware to Development release uses: ncipollo/release-action@v1 with: tag: dev-latest name: Development (auto) prerelease: true allowUpdates: true commit: ${{ github.sha }} artifacts: dist/MSRC_firmware.uf2 artifactContentType: application/octet-stream replacesArtifacts: true generateReleaseNotes: false bodyFile: release_body.md ================================================ FILE: .github/workflows/dev_msrc_link.yaml ================================================ name: Dev Release - MSRC Link on: workflow_dispatch: {} push: branches: - master paths: - msrc_gui/** permissions: contents: write id-token: write attestations: write concurrency: group: dev-release-link cancel-in-progress: true jobs: linux: runs-on: ubuntu-22.04 steps: - name: Checkout uses: actions/checkout@v4 with: fetch-depth: 0 - name: Install Qt uses: jurplel/install-qt-action@v4 with: version: '6.8.*' modules: qtserialport - name: Fetch tags run: git fetch --force --tags - name: Build run: | mkdir -p build cd build qmake ../msrc_gui make - name: Install linuxdeployqt run: | cd msrc_gui wget -q https://github.com/probonopd/linuxdeployqt/releases/download/continuous/linuxdeployqt-continuous-x86_64.AppImage chmod +x linuxdeployqt-continuous-x86_64.AppImage - name: Install fuse run: sudo apt-get update && sudo apt-get install -y libfuse2 - name: Create AppImage run: | cp build/msrc_gui msrc_gui/appdir cd msrc_gui/appdir ../linuxdeployqt-continuous-x86_64.AppImage msrc.desktop -appimage -extra-plugins=iconengines,platformthemes/libqgtk3.so mv ./*.AppImage ./MSRC_Link_Linux.AppImage - name: Collect artifact run: | mkdir -p dist cp msrc_gui/appdir/MSRC_Link_Linux.AppImage dist/MSRC_Link_Linux.AppImage - name: Upload artifact (Linux) uses: actions/upload-artifact@v4 with: name: msrc-link-linux path: dist/MSRC_Link_Linux.AppImage if-no-files-found: error macos: runs-on: macos-14 steps: - name: Checkout uses: actions/checkout@v4 with: fetch-depth: 0 - name: Install Qt uses: jurplel/install-qt-action@v4 with: version: '6.8.*' modules: qtserialport - name: Fetch tags run: git fetch --force --tags - name: Build run: | mkdir -p build cd build qmake ../msrc_gui make macdeployqt msrc_gui.app -dmg - name: Collect artifact run: | mkdir -p dist cp build/msrc_gui.dmg dist/MSRC_Link_macOS.dmg - name: Upload artifact (macOS) uses: actions/upload-artifact@v4 with: name: msrc-link-macos path: dist/MSRC_Link_macOS.dmg if-no-files-found: error windows: runs-on: windows-2022 steps: - name: Checkout uses: actions/checkout@v4 with: fetch-depth: 0 - name: Install Qt (static) uses: orestonce/install-qt-static@v0.4.3 with: version: Qt6.5.3-Windows-x86_64-MinGW13.2.0-ucrt-staticFull-20240527 - name: Fetch tags run: git fetch --force --tags - name: Build shell: bash run: | mkdir -p build cd build qmake ../msrc_gui make mingw32-make release - name: Collect Windows executable shell: bash run: | mkdir -p dist cp build/release/msrc_gui.exe dist/MSRC_Link_Windows.exe - name: Upload artifact (Windows) uses: actions/upload-artifact@v4 with: name: msrc-link-windows path: dist/MSRC_Link_Windows.exe if-no-files-found: error publish: runs-on: ubuntu-latest needs: [linux, macos, windows] steps: - name: Download artifacts uses: actions/download-artifact@v4 with: path: dist - name: Normalize dist layout run: | set -euo pipefail find dist -type f -name "MSRC_Link_Linux.AppImage" -exec cp -f {} dist/ \; find dist -type f -name "MSRC_Link_macOS.dmg" -exec cp -f {} dist/ \; find dist -type f -name "MSRC_Link_Windows.exe" -exec cp -f {} dist/ \; test -f dist/MSRC_Link_Linux.AppImage test -f dist/MSRC_Link_macOS.dmg test -f dist/MSRC_Link_Windows.exe - name: Generate attestation (Linux) uses: actions/attest-build-provenance@v3 with: subject-path: dist/MSRC_Link_Linux.AppImage github-token: ${{ secrets.GITHUB_TOKEN }} - name: Generate attestation (macOS) uses: actions/attest-build-provenance@v3 with: subject-path: dist/MSRC_Link_macOS.dmg github-token: ${{ secrets.GITHUB_TOKEN }} - name: Generate attestation (Windows) uses: actions/attest-build-provenance@v3 with: subject-path: dist/MSRC_Link_Windows.exe github-token: ${{ secrets.GITHUB_TOKEN }} - name: Write release body run: | cat > release_body.md << EOF Development (auto) MSRC Link: - commit: "${{ github.event.head_commit.message }}" - commit-url: "https://github.com/${{ github.repository }}/commit/${{ github.sha }}" EOF - name: Publish MSRC Link to Development release uses: ncipollo/release-action@v1 with: tag: dev-latest name: Development (auto) prerelease: true allowUpdates: true commit: ${{ github.sha }} artifacts: dist/MSRC_Link_Linux.AppImage,dist/MSRC_Link_macOS.dmg,dist/MSRC_Link_Windows.exe replacesArtifacts: true generateReleaseNotes: false ================================================ FILE: .gitignore ================================================ misc build/ build_*/ msrc_gui/msrc_gui.pro.user* ### macOS ### .DS_Store ================================================ FILE: .gitmodules ================================================ [submodule "board/freertos/FreeRTOS-Kernel"] path = board/freertos/FreeRTOS-Kernel url = https://github.com/FreeRTOS/FreeRTOS-Kernel.git ================================================ FILE: .vscode/c_cpp_properties.json ================================================ { "configurations": [ { "name": "Default", "includePath": [ "${workspaceFolder}/board/project/**", "${workspaceFolder}/build/**", "${env:PICO_SDK_PATH}/src/**", "${workspaceFolder}/board/freertos/FreeRTOS-Kernel/include/", "${workspaceFolder}/board/freertos/FreeRTOS-Kernel/portable/ThirdParty/GCC/RP2040/include/**", "${workspaceFolder}/board/freertos/" ], "intelliSenseMode": "gcc-arm", "compilerPath": "/usr/bin/arm-none-eabi-gcc", "cStandard": "c17", "cppStandard": "c++14" } ], "version": 4 } ================================================ FILE: .vscode/launch.json ================================================ { "version": "0.2.0", "configurations": [ { "name": "Pico Debug", "cwd": "${workspaceRoot}", "executable": "${command:cmake.launchTargetPath}", "request": "launch", "type": "cortex-debug", "servertype": "openocd", //"gdbPath" : "${env:HOME}/.platformio/packages/toolchain-gccarmnoneeabi/bin/arm-none-eabi-gdb", "gdbPath" : "${env:HOME}/src/gcc-arm-none-eabi-10/bin/arm-none-eabi-gdb", "device": "RP2040", "configFiles": [ "interface/picoprobe.cfg", "target/rp2040.cfg" ], "svdFile": "${env:PICO_SDK_PATH}/src/rp2040/hardware_regs/rp2040.svd", "runToMain": true, // Work around for stopping at main on restart "postRestartCommands": [ "break main", "continue" ], "searchDir": ["${env:HOME}/src/pico/openocd/tcl"], //"showDevDebugOutput": true, } ] } ================================================ FILE: .vscode/settings.json ================================================ { // These settings tweaks to the cmake plugin will ensure // that you debug using cortex-debug instead of trying to launch // a Pico binary on the host "C_Cpp.autoAddFileAssociations" : false, "C_Cpp.default.configurationProvider": "ms-vscode.cmake-tools", "cmake.options.statusBarVisibility" : "visible", "cmake.buildBeforeRun": true, "cmake.sourceDirectory": "${workspaceFolder}/board", "cortex-debug.openocdPath": "${env:HOME}/src/pico/openocd/src/openocd", "cortex-debug.armToolchainPath": "${env:HOME}/.platformio/packages/toolchain-gccarmnoneeabi/bin", "cortex-debug.variableUseNaturalFormat": true, } ================================================ FILE: LICENSE ================================================ GNU GENERAL PUBLIC LICENSE Version 3, 29 June 2007 Copyright (C) 2007 Free Software Foundation, Inc. Everyone is permitted to copy and distribute verbatim copies of this license document, but changing it is not allowed. Preamble The GNU General Public License is a free, copyleft license for software and other kinds of works. The licenses for most software and other practical works are designed to take away your freedom to share and change the works. By contrast, the GNU General Public License is intended to guarantee your freedom to share and change all versions of a program--to make sure it remains free software for all its users. We, the Free Software Foundation, use the GNU General Public License for most of our software; it applies also to any other work released this way by its authors. You can apply it to your programs, too. When we speak of free software, we are referring to freedom, not price. Our General Public Licenses are designed to make sure that you have the freedom to distribute copies of free software (and charge for them if you wish), that you receive source code or can get it if you want it, that you can change the software or use pieces of it in new free programs, and that you know you can do these things. To protect your rights, we need to prevent others from denying you these rights or asking you to surrender the rights. Therefore, you have certain responsibilities if you distribute copies of the software, or if you modify it: responsibilities to respect the freedom of others. For example, if you distribute copies of such a program, whether gratis or for a fee, you must pass on to the recipients the same freedoms that you received. You must make sure that they, too, receive or can get the source code. And you must show them these terms so they know their rights. Developers that use the GNU GPL protect your rights with two steps: (1) assert copyright on the software, and (2) offer you this License giving you legal permission to copy, distribute and/or modify it. For the developers' and authors' protection, the GPL clearly explains that there is no warranty for this free software. For both users' and authors' sake, the GPL requires that modified versions be marked as changed, so that their problems will not be attributed erroneously to authors of previous versions. Some devices are designed to deny users access to install or run modified versions of the software inside them, although the manufacturer can do so. This is fundamentally incompatible with the aim of protecting users' freedom to change the software. The systematic pattern of such abuse occurs in the area of products for individuals to use, which is precisely where it is most unacceptable. Therefore, we have designed this version of the GPL to prohibit the practice for those products. If such problems arise substantially in other domains, we stand ready to extend this provision to those domains in future versions of the GPL, as needed to protect the freedom of users. Finally, every program is threatened constantly by software patents. States should not allow patents to restrict development and use of software on general-purpose computers, but in those that do, we wish to avoid the special danger that patents applied to a free program could make it effectively proprietary. To prevent this, the GPL assures that patents cannot be used to render the program non-free. The precise terms and conditions for copying, distribution and modification follow. TERMS AND CONDITIONS 0. Definitions. "This License" refers to version 3 of the GNU General Public License. "Copyright" also means copyright-like laws that apply to other kinds of works, such as semiconductor masks. "The Program" refers to any copyrightable work licensed under this License. Each licensee is addressed as "you". "Licensees" and "recipients" may be individuals or organizations. To "modify" a work means to copy from or adapt all or part of the work in a fashion requiring copyright permission, other than the making of an exact copy. The resulting work is called a "modified version" of the earlier work or a work "based on" the earlier work. A "covered work" means either the unmodified Program or a work based on the Program. To "propagate" a work means to do anything with it that, without permission, would make you directly or secondarily liable for infringement under applicable copyright law, except executing it on a computer or modifying a private copy. Propagation includes copying, distribution (with or without modification), making available to the public, and in some countries other activities as well. To "convey" a work means any kind of propagation that enables other parties to make or receive copies. Mere interaction with a user through a computer network, with no transfer of a copy, is not conveying. An interactive user interface displays "Appropriate Legal Notices" to the extent that it includes a convenient and prominently visible feature that (1) displays an appropriate copyright notice, and (2) tells the user that there is no warranty for the work (except to the extent that warranties are provided), that licensees may convey the work under this License, and how to view a copy of this License. If the interface presents a list of user commands or options, such as a menu, a prominent item in the list meets this criterion. 1. Source Code. The "source code" for a work means the preferred form of the work for making modifications to it. "Object code" means any non-source form of a work. A "Standard Interface" means an interface that either is an official standard defined by a recognized standards body, or, in the case of interfaces specified for a particular programming language, one that is widely used among developers working in that language. The "System Libraries" of an executable work include anything, other than the work as a whole, that (a) is included in the normal form of packaging a Major Component, but which is not part of that Major Component, and (b) serves only to enable use of the work with that Major Component, or to implement a Standard Interface for which an implementation is available to the public in source code form. A "Major Component", in this context, means a major essential component (kernel, window system, and so on) of the specific operating system (if any) on which the executable work runs, or a compiler used to produce the work, or an object code interpreter used to run it. The "Corresponding Source" for a work in object code form means all the source code needed to generate, install, and (for an executable work) run the object code and to modify the work, including scripts to control those activities. However, it does not include the work's System Libraries, or general-purpose tools or generally available free programs which are used unmodified in performing those activities but which are not part of the work. For example, Corresponding Source includes interface definition files associated with source files for the work, and the source code for shared libraries and dynamically linked subprograms that the work is specifically designed to require, such as by intimate data communication or control flow between those subprograms and other parts of the work. The Corresponding Source need not include anything that users can regenerate automatically from other parts of the Corresponding Source. The Corresponding Source for a work in source code form is that same work. 2. Basic Permissions. All rights granted under this License are granted for the term of copyright on the Program, and are irrevocable provided the stated conditions are met. This License explicitly affirms your unlimited permission to run the unmodified Program. The output from running a covered work is covered by this License only if the output, given its content, constitutes a covered work. This License acknowledges your rights of fair use or other equivalent, as provided by copyright law. You may make, run and propagate covered works that you do not convey, without conditions so long as your license otherwise remains in force. You may convey covered works to others for the sole purpose of having them make modifications exclusively for you, or provide you with facilities for running those works, provided that you comply with the terms of this License in conveying all material for which you do not control copyright. Those thus making or running the covered works for you must do so exclusively on your behalf, under your direction and control, on terms that prohibit them from making any copies of your copyrighted material outside their relationship with you. Conveying under any other circumstances is permitted solely under the conditions stated below. Sublicensing is not allowed; section 10 makes it unnecessary. 3. Protecting Users' Legal Rights From Anti-Circumvention Law. No covered work shall be deemed part of an effective technological measure under any applicable law fulfilling obligations under article 11 of the WIPO copyright treaty adopted on 20 December 1996, or similar laws prohibiting or restricting circumvention of such measures. When you convey a covered work, you waive any legal power to forbid circumvention of technological measures to the extent such circumvention is effected by exercising rights under this License with respect to the covered work, and you disclaim any intention to limit operation or modification of the work as a means of enforcing, against the work's users, your or third parties' legal rights to forbid circumvention of technological measures. 4. Conveying Verbatim Copies. You may convey verbatim copies of the Program's source code as you receive it, in any medium, provided that you conspicuously and appropriately publish on each copy an appropriate copyright notice; keep intact all notices stating that this License and any non-permissive terms added in accord with section 7 apply to the code; keep intact all notices of the absence of any warranty; and give all recipients a copy of this License along with the Program. You may charge any price or no price for each copy that you convey, and you may offer support or warranty protection for a fee. 5. Conveying Modified Source Versions. You may convey a work based on the Program, or the modifications to produce it from the Program, in the form of source code under the terms of section 4, provided that you also meet all of these conditions: a) The work must carry prominent notices stating that you modified it, and giving a relevant date. b) The work must carry prominent notices stating that it is released under this License and any conditions added under section 7. This requirement modifies the requirement in section 4 to "keep intact all notices". c) You must license the entire work, as a whole, under this License to anyone who comes into possession of a copy. This License will therefore apply, along with any applicable section 7 additional terms, to the whole of the work, and all its parts, regardless of how they are packaged. This License gives no permission to license the work in any other way, but it does not invalidate such permission if you have separately received it. d) If the work has interactive user interfaces, each must display Appropriate Legal Notices; however, if the Program has interactive interfaces that do not display Appropriate Legal Notices, your work need not make them do so. A compilation of a covered work with other separate and independent works, which are not by their nature extensions of the covered work, and which are not combined with it such as to form a larger program, in or on a volume of a storage or distribution medium, is called an "aggregate" if the compilation and its resulting copyright are not used to limit the access or legal rights of the compilation's users beyond what the individual works permit. Inclusion of a covered work in an aggregate does not cause this License to apply to the other parts of the aggregate. 6. Conveying Non-Source Forms. You may convey a covered work in object code form under the terms of sections 4 and 5, provided that you also convey the machine-readable Corresponding Source under the terms of this License, in one of these ways: a) Convey the object code in, or embodied in, a physical product (including a physical distribution medium), accompanied by the Corresponding Source fixed on a durable physical medium customarily used for software interchange. b) Convey the object code in, or embodied in, a physical product (including a physical distribution medium), accompanied by a written offer, valid for at least three years and valid for as long as you offer spare parts or customer support for that product model, to give anyone who possesses the object code either (1) a copy of the Corresponding Source for all the software in the product that is covered by this License, on a durable physical medium customarily used for software interchange, for a price no more than your reasonable cost of physically performing this conveying of source, or (2) access to copy the Corresponding Source from a network server at no charge. c) Convey individual copies of the object code with a copy of the written offer to provide the Corresponding Source. This alternative is allowed only occasionally and noncommercially, and only if you received the object code with such an offer, in accord with subsection 6b. d) Convey the object code by offering access from a designated place (gratis or for a charge), and offer equivalent access to the Corresponding Source in the same way through the same place at no further charge. You need not require recipients to copy the Corresponding Source along with the object code. If the place to copy the object code is a network server, the Corresponding Source may be on a different server (operated by you or a third party) that supports equivalent copying facilities, provided you maintain clear directions next to the object code saying where to find the Corresponding Source. Regardless of what server hosts the Corresponding Source, you remain obligated to ensure that it is available for as long as needed to satisfy these requirements. e) Convey the object code using peer-to-peer transmission, provided you inform other peers where the object code and Corresponding Source of the work are being offered to the general public at no charge under subsection 6d. A separable portion of the object code, whose source code is excluded from the Corresponding Source as a System Library, need not be included in conveying the object code work. A "User Product" is either (1) a "consumer product", which means any tangible personal property which is normally used for personal, family, or household purposes, or (2) anything designed or sold for incorporation into a dwelling. In determining whether a product is a consumer product, doubtful cases shall be resolved in favor of coverage. For a particular product received by a particular user, "normally used" refers to a typical or common use of that class of product, regardless of the status of the particular user or of the way in which the particular user actually uses, or expects or is expected to use, the product. A product is a consumer product regardless of whether the product has substantial commercial, industrial or non-consumer uses, unless such uses represent the only significant mode of use of the product. "Installation Information" for a User Product means any methods, procedures, authorization keys, or other information required to install and execute modified versions of a covered work in that User Product from a modified version of its Corresponding Source. The information must suffice to ensure that the continued functioning of the modified object code is in no case prevented or interfered with solely because modification has been made. If you convey an object code work under this section in, or with, or specifically for use in, a User Product, and the conveying occurs as part of a transaction in which the right of possession and use of the User Product is transferred to the recipient in perpetuity or for a fixed term (regardless of how the transaction is characterized), the Corresponding Source conveyed under this section must be accompanied by the Installation Information. But this requirement does not apply if neither you nor any third party retains the ability to install modified object code on the User Product (for example, the work has been installed in ROM). The requirement to provide Installation Information does not include a requirement to continue to provide support service, warranty, or updates for a work that has been modified or installed by the recipient, or for the User Product in which it has been modified or installed. Access to a network may be denied when the modification itself materially and adversely affects the operation of the network or violates the rules and protocols for communication across the network. Corresponding Source conveyed, and Installation Information provided, in accord with this section must be in a format that is publicly documented (and with an implementation available to the public in source code form), and must require no special password or key for unpacking, reading or copying. 7. Additional Terms. "Additional permissions" are terms that supplement the terms of this License by making exceptions from one or more of its conditions. Additional permissions that are applicable to the entire Program shall be treated as though they were included in this License, to the extent that they are valid under applicable law. If additional permissions apply only to part of the Program, that part may be used separately under those permissions, but the entire Program remains governed by this License without regard to the additional permissions. When you convey a copy of a covered work, you may at your option remove any additional permissions from that copy, or from any part of it. (Additional permissions may be written to require their own removal in certain cases when you modify the work.) You may place additional permissions on material, added by you to a covered work, for which you have or can give appropriate copyright permission. Notwithstanding any other provision of this License, for material you add to a covered work, you may (if authorized by the copyright holders of that material) supplement the terms of this License with terms: a) Disclaiming warranty or limiting liability differently from the terms of sections 15 and 16 of this License; or b) Requiring preservation of specified reasonable legal notices or author attributions in that material or in the Appropriate Legal Notices displayed by works containing it; or c) Prohibiting misrepresentation of the origin of that material, or requiring that modified versions of such material be marked in reasonable ways as different from the original version; or d) Limiting the use for publicity purposes of names of licensors or authors of the material; or e) Declining to grant rights under trademark law for use of some trade names, trademarks, or service marks; or f) Requiring indemnification of licensors and authors of that material by anyone who conveys the material (or modified versions of it) with contractual assumptions of liability to the recipient, for any liability that these contractual assumptions directly impose on those licensors and authors. All other non-permissive additional terms are considered "further restrictions" within the meaning of section 10. If the Program as you received it, or any part of it, contains a notice stating that it is governed by this License along with a term that is a further restriction, you may remove that term. If a license document contains a further restriction but permits relicensing or conveying under this License, you may add to a covered work material governed by the terms of that license document, provided that the further restriction does not survive such relicensing or conveying. If you add terms to a covered work in accord with this section, you must place, in the relevant source files, a statement of the additional terms that apply to those files, or a notice indicating where to find the applicable terms. Additional terms, permissive or non-permissive, may be stated in the form of a separately written license, or stated as exceptions; the above requirements apply either way. 8. Termination. You may not propagate or modify a covered work except as expressly provided under this License. Any attempt otherwise to propagate or modify it is void, and will automatically terminate your rights under this License (including any patent licenses granted under the third paragraph of section 11). However, if you cease all violation of this License, then your license from a particular copyright holder is reinstated (a) provisionally, unless and until the copyright holder explicitly and finally terminates your license, and (b) permanently, if the copyright holder fails to notify you of the violation by some reasonable means prior to 60 days after the cessation. Moreover, your license from a particular copyright holder is reinstated permanently if the copyright holder notifies you of the violation by some reasonable means, this is the first time you have received notice of violation of this License (for any work) from that copyright holder, and you cure the violation prior to 30 days after your receipt of the notice. Termination of your rights under this section does not terminate the licenses of parties who have received copies or rights from you under this License. If your rights have been terminated and not permanently reinstated, you do not qualify to receive new licenses for the same material under section 10. 9. Acceptance Not Required for Having Copies. You are not required to accept this License in order to receive or run a copy of the Program. Ancillary propagation of a covered work occurring solely as a consequence of using peer-to-peer transmission to receive a copy likewise does not require acceptance. However, nothing other than this License grants you permission to propagate or modify any covered work. These actions infringe copyright if you do not accept this License. Therefore, by modifying or propagating a covered work, you indicate your acceptance of this License to do so. 10. Automatic Licensing of Downstream Recipients. Each time you convey a covered work, the recipient automatically receives a license from the original licensors, to run, modify and propagate that work, subject to this License. You are not responsible for enforcing compliance by third parties with this License. An "entity transaction" is a transaction transferring control of an organization, or substantially all assets of one, or subdividing an organization, or merging organizations. If propagation of a covered work results from an entity transaction, each party to that transaction who receives a copy of the work also receives whatever licenses to the work the party's predecessor in interest had or could give under the previous paragraph, plus a right to possession of the Corresponding Source of the work from the predecessor in interest, if the predecessor has it or can get it with reasonable efforts. You may not impose any further restrictions on the exercise of the rights granted or affirmed under this License. For example, you may not impose a license fee, royalty, or other charge for exercise of rights granted under this License, and you may not initiate litigation (including a cross-claim or counterclaim in a lawsuit) alleging that any patent claim is infringed by making, using, selling, offering for sale, or importing the Program or any portion of it. 11. Patents. A "contributor" is a copyright holder who authorizes use under this License of the Program or a work on which the Program is based. The work thus licensed is called the contributor's "contributor version". A contributor's "essential patent claims" are all patent claims owned or controlled by the contributor, whether already acquired or hereafter acquired, that would be infringed by some manner, permitted by this License, of making, using, or selling its contributor version, but do not include claims that would be infringed only as a consequence of further modification of the contributor version. For purposes of this definition, "control" includes the right to grant patent sublicenses in a manner consistent with the requirements of this License. Each contributor grants you a non-exclusive, worldwide, royalty-free patent license under the contributor's essential patent claims, to make, use, sell, offer for sale, import and otherwise run, modify and propagate the contents of its contributor version. In the following three paragraphs, a "patent license" is any express agreement or commitment, however denominated, not to enforce a patent (such as an express permission to practice a patent or covenant not to sue for patent infringement). To "grant" such a patent license to a party means to make such an agreement or commitment not to enforce a patent against the party. If you convey a covered work, knowingly relying on a patent license, and the Corresponding Source of the work is not available for anyone to copy, free of charge and under the terms of this License, through a publicly available network server or other readily accessible means, then you must either (1) cause the Corresponding Source to be so available, or (2) arrange to deprive yourself of the benefit of the patent license for this particular work, or (3) arrange, in a manner consistent with the requirements of this License, to extend the patent license to downstream recipients. "Knowingly relying" means you have actual knowledge that, but for the patent license, your conveying the covered work in a country, or your recipient's use of the covered work in a country, would infringe one or more identifiable patents in that country that you have reason to believe are valid. If, pursuant to or in connection with a single transaction or arrangement, you convey, or propagate by procuring conveyance of, a covered work, and grant a patent license to some of the parties receiving the covered work authorizing them to use, propagate, modify or convey a specific copy of the covered work, then the patent license you grant is automatically extended to all recipients of the covered work and works based on it. A patent license is "discriminatory" if it does not include within the scope of its coverage, prohibits the exercise of, or is conditioned on the non-exercise of one or more of the rights that are specifically granted under this License. You may not convey a covered work if you are a party to an arrangement with a third party that is in the business of distributing software, under which you make payment to the third party based on the extent of your activity of conveying the work, and under which the third party grants, to any of the parties who would receive the covered work from you, a discriminatory patent license (a) in connection with copies of the covered work conveyed by you (or copies made from those copies), or (b) primarily for and in connection with specific products or compilations that contain the covered work, unless you entered into that arrangement, or that patent license was granted, prior to 28 March 2007. Nothing in this License shall be construed as excluding or limiting any implied license or other defenses to infringement that may otherwise be available to you under applicable patent law. 12. No Surrender of Others' Freedom. If conditions are imposed on you (whether by court order, agreement or otherwise) that contradict the conditions of this License, they do not excuse you from the conditions of this License. If you cannot convey a covered work so as to satisfy simultaneously your obligations under this License and any other pertinent obligations, then as a consequence you may not convey it at all. For example, if you agree to terms that obligate you to collect a royalty for further conveying from those to whom you convey the Program, the only way you could satisfy both those terms and this License would be to refrain entirely from conveying the Program. 13. Use with the GNU Affero General Public License. Notwithstanding any other provision of this License, you have permission to link or combine any covered work with a work licensed under version 3 of the GNU Affero General Public License into a single combined work, and to convey the resulting work. The terms of this License will continue to apply to the part which is the covered work, but the special requirements of the GNU Affero General Public License, section 13, concerning interaction through a network will apply to the combination as such. 14. Revised Versions of this License. The Free Software Foundation may publish revised and/or new versions of the GNU General Public License from time to time. Such new versions will be similar in spirit to the present version, but may differ in detail to address new problems or concerns. Each version is given a distinguishing version number. If the Program specifies that a certain numbered version of the GNU General Public License "or any later version" applies to it, you have the option of following the terms and conditions either of that numbered version or of any later version published by the Free Software Foundation. If the Program does not specify a version number of the GNU General Public License, you may choose any version ever published by the Free Software Foundation. If the Program specifies that a proxy can decide which future versions of the GNU General Public License can be used, that proxy's public statement of acceptance of a version permanently authorizes you to choose that version for the Program. Later license versions may give you additional or different permissions. However, no additional obligations are imposed on any author or copyright holder as a result of your choosing to follow a later version. 15. Disclaimer of Warranty. THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 16. Limitation of Liability. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGES. 17. Interpretation of Sections 15 and 16. If the disclaimer of warranty and limitation of liability provided above cannot be given local legal effect according to their terms, reviewing courts shall apply local law that most closely approximates an absolute waiver of all civil liability in connection with the Program, unless a warranty or assumption of liability accompanies a copy of the Program in return for a fee. END OF TERMS AND CONDITIONS How to Apply These Terms to Your New Programs If you develop a new program, and you want it to be of the greatest possible use to the public, the best way to achieve this is to make it free software which everyone can redistribute and change under these terms. To do so, attach the following notices to the program. It is safest to attach them to the start of each source file to most effectively state the exclusion of warranty; and each file should have at least the "copyright" line and a pointer to where the full notice is found. Copyright (C) This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see . Also add information on how to contact you by electronic and paper mail. If the program does terminal interaction, make it output a short notice like this when it starts in an interactive mode: Copyright (C) This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. This is free software, and you are welcome to redistribute it under certain conditions; type `show c' for details. The hypothetical commands `show w' and `show c' should show the appropriate parts of the General Public License. Of course, your program's commands might be different; for a GUI interface, you would use an "about box". You should also get your employer (if you work as a programmer) or school, if any, to sign a "copyright disclaimer" for the program, if necessary. For more information on this, and how to apply and follow the GNU GPL, see . The GNU General Public License does not permit incorporating your program into proprietary programs. If your program is a subroutine library, you may consider it more useful to permit linking proprietary applications with the library. If this is what you want to do, use the GNU Lesser General Public License instead of this License. But first, please read . ================================================ FILE: README.md ================================================ # MSRC – Multi Sensor Telemetry for RC (RP2040) [![Donate](https://img.shields.io/badge/Donate-PayPal-green.svg)](https://www.paypal.com/donate/?business=9GKNBVGHKXFGW&no_recurring=0¤cy_code=USD) **MSRC** is a lightweight, low-cost, and highly flexible telemetry system for RC models, based on the RP2040 microcontroller. It allows you to collect data from multiple sensors and transmit it to your receiver using a wide range of telemetry protocols. MSRC is a DIY alternative to commercial solutions, offering significantly reduced weight and cost while maintaining high flexibility and expandability. --- ## Features - Multi-sensor telemetry system based on RP2040 - Support for multiple receiver telemetry protocols - Support for various ESC telemetry protocols - Modular design (only use the sensors you need) - Fully configurable via PC using MSRC Link - Lightweight and cost-effective --- ## Supported Receiver Protocols - FrSky SmartPort - FrSky D - FrSky FPort - FrSky FBUS - Spektrum XBUS - Spektrum SRXL / SRXL2 - FlySky IBUS - Futaba SBUS2 - Multiplex Sensor Bus (MSB) - Jeti EX Bus - Jeti EX Sensor - Hitec - CRSF - Sanwa - HoTT - JR DMSS - GHST --- ## Supported Sensors ### ESC Telemetry - Hobbywing V3 / V4 / V5 / FlyFun - Kontronik - KISS (APD F, BLHeli32, Summit X) - APD HV / UHV - OMP M4 - ZTW - Castle Link - PWM / phase sensor ESCs - Spektrum Smart ESC & Battery ### GPS - Serial GPS (NMEA) ### Vario (I2C) - BMP180 - BMP280 - MS5611 ### Analog Sensors - Voltage - Temperature - Current - Airspeed (MPXV7002) ### Fuel - Fuel flow meter (PWM pulses) - Fuel tank pressure (XGZP68XXD) ### Other - Up to 6 digital switches (GPIO) --- ## Getting Started 1. Download the latest firmware from the **Releases** section 2. Flash the firmware to your RP2040 board 3. Connect your sensors and receiver 4. Configure the device using **MSRC Link** 5. Check telemetry on your transmitter 👉 For detailed instructions, see the [Wiki](https://github.com/dgatf/msrc/wiki) --- ## Documentation Full documentation is available in the wiki: 👉 https://github.com/dgatf/msrc/wiki --- ## Support - Report issues: https://github.com/dgatf/msrc/issues - Discussion forums: - https://www.rcgroups.com/forums/showthread.php?4088405-DIY-MSRC-Multi-sensor-telemetry-for-several-Rx-protocols#post48830585 - https://www.helifreak.com/showthread.php?t=908371 - https://www.openrcforums.com/forum/viewtopic.php?f=84&t=11911 If you would like to add support for a new receiver protocol or sensor, feel free to open an issue. --- ## Donate If you find this project useful, consider supporting its development: 👉 https://www.paypal.com/donate/?business=9GKNBVGHKXFGW&no_recurring=0¤cy_code=USD ================================================ FILE: board/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 3.17.0) include(pico_sdk_import.cmake) project(MSRC-RP2040) include_directories(../include) execute_process( COMMAND git config --global --add safe.directory /github/workspace COMMAND git describe --tags OUTPUT_VARIABLE PROJECT_VERSION OUTPUT_STRIP_TRAILING_WHITESPACE ) message(STATUS "MSRC ${PROJECT_VERSION}") add_compile_definitions(PROJECT_VERSION="${PROJECT_VERSION}") pico_sdk_init() add_subdirectory(freertos) add_subdirectory(project) ================================================ FILE: board/freertos/CMakeLists.txt ================================================ set(PICO_SDK_FREERTOS_SOURCE FreeRTOS-Kernel) add_library(freertos ${PICO_SDK_FREERTOS_SOURCE}/event_groups.c ${PICO_SDK_FREERTOS_SOURCE}/list.c ${PICO_SDK_FREERTOS_SOURCE}/queue.c ${PICO_SDK_FREERTOS_SOURCE}/stream_buffer.c ${PICO_SDK_FREERTOS_SOURCE}/tasks.c ${PICO_SDK_FREERTOS_SOURCE}/timers.c ${PICO_SDK_FREERTOS_SOURCE}/portable/MemMang/heap_3.c ${PICO_SDK_FREERTOS_SOURCE}/portable/GCC/ARM_CM0/port.c ) target_include_directories(freertos PUBLIC . ${PICO_SDK_FREERTOS_SOURCE}/include ${PICO_SDK_FREERTOS_SOURCE}/portable/GCC/ARM_CM0 ) ================================================ FILE: board/freertos/FreeRTOSConfig.h ================================================ #ifndef FREERTOS_CONFIG_H #define FREERTOS_CONFIG_H /* Use Pico SDK ISR handlers */ #define vPortSVCHandler isr_svcall #define xPortPendSVHandler isr_pendsv #define xPortSysTickHandler isr_systick #define configUSE_PREEMPTION 1 #define configUSE_PORT_OPTIMISED_TASK_SELECTION 0 #define configUSE_TICKLESS_IDLE 0 #define configCPU_CLOCK_HZ 133000000 #define configTICK_RATE_HZ 500 #define configMAX_PRIORITIES 5 #define configMINIMAL_STACK_SIZE 128 #define configMAX_TASK_NAME_LEN 16 #define configUSE_16_BIT_TICKS 0 #define configIDLE_SHOULD_YIELD 1 #define configUSE_TASK_NOTIFICATIONS 1 #define configTASK_NOTIFICATION_ARRAY_ENTRIES 3 #define configUSE_MUTEXES 1 #define configUSE_RECURSIVE_MUTEXES 0 #define configUSE_COUNTING_SEMAPHORES 0 #define configQUEUE_REGISTRY_SIZE 10 #define configUSE_QUEUE_SETS 0 #define configUSE_TIME_SLICING 0 #define configUSE_NEWLIB_REENTRANT 0 #define configENABLE_BACKWARD_COMPATIBILITY 0 #define configNUM_THREAD_LOCAL_STORAGE_POINTERS 5 #define configSTACK_DEPTH_TYPE uint16_t #define configMESSAGE_BUFFER_LENGTH_TYPE size_t /* Memory allocation related definitions. */ #define configSUPPORT_STATIC_ALLOCATION 0 #define configSUPPORT_DYNAMIC_ALLOCATION 1 #define configAPPLICATION_ALLOCATED_HEAP 1 /* Hook function related definitions. */ #define configUSE_IDLE_HOOK 0 #define configUSE_TICK_HOOK 0 #define configCHECK_FOR_STACK_OVERFLOW 0 #define configUSE_MALLOC_FAILED_HOOK 0 #define configUSE_DAEMON_TASK_STARTUP_HOOK 0 /* Run time and task stats gathering related definitions. */ #define configGENERATE_RUN_TIME_STATS 0 #define configUSE_TRACE_FACILITY 0 #define configUSE_STATS_FORMATTING_FUNCTIONS 0 /* Co-routine related definitions. */ #define configUSE_CO_ROUTINES 0 #define configMAX_CO_ROUTINE_PRIORITIES 1 /* Software timer related definitions. */ #define configUSE_TIMERS 1 #define configTIMER_TASK_PRIORITY 3 #define configTIMER_QUEUE_LENGTH 10 #define configTIMER_TASK_STACK_DEPTH configMINIMAL_STACK_SIZE /* Define to trap errors during development. */ #define configASSERT( x ) /* Optional functions - most linkers will remove unused functions anyway. */ #define INCLUDE_vTaskPrioritySet 1 #define INCLUDE_uxTaskPriorityGet 1 #define INCLUDE_vTaskDelete 1 #define INCLUDE_vTaskSuspend 1 #define INCLUDE_xResumeFromISR 1 #define INCLUDE_vTaskDelayUntil 1 #define INCLUDE_vTaskDelay 1 #define INCLUDE_xTaskGetSchedulerState 1 #define INCLUDE_xTaskGetCurrentTaskHandle 1 #define INCLUDE_uxTaskGetStackHighWaterMark 1 #define INCLUDE_xTaskGetIdleTaskHandle 0 #define INCLUDE_eTaskGetState 0 #define INCLUDE_xEventGroupSetBitFromISR 1 #define INCLUDE_xTimerPendFunctionCall 0 #define INCLUDE_xTaskAbortDelay 0 #define INCLUDE_xTaskGetHandle 0 #define INCLUDE_xTaskResumeFromISR 1 /* A header file that defines trace macro can be included here. */ #endif /* FREERTOS_CONFIG_H */ ================================================ FILE: board/pico_sdk_import.cmake ================================================ # This is a copy of /external/pico_sdk_import.cmake # This can be dropped into an external project to help locate this SDK # It should be include()ed prior to project() # todo document if (DEFINED ENV{PICO_SDK_PATH} AND (NOT PICO_SDK_PATH)) set(PICO_SDK_PATH $ENV{PICO_SDK_PATH}) message("Using PICO_SDK_PATH from environment ('${PICO_SDK_PATH}')") endif () if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT} AND (NOT PICO_SDK_FETCH_FROM_GIT)) set(PICO_SDK_FETCH_FROM_GIT $ENV{PICO_SDK_FETCH_FROM_GIT}) message("Using PICO_SDK_FETCH_FROM_GIT from environment ('${PICO_SDK_FETCH_FROM_GIT}')") endif () if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT_PATH} AND (NOT PICO_SDK_FETCH_FROM_GIT_PATH)) set(PICO_SDK_FETCH_FROM_GIT_PATH $ENV{PICO_SDK_FETCH_FROM_GIT_PATH}) message("Using PICO_SDK_FETCH_FROM_GIT_PATH from environment ('${PICO_SDK_FETCH_FROM_GIT_PATH}')") endif () set(PICO_SDK_PATH "${PICO_SDK_PATH}" CACHE PATH "Path to the PICO SDK") set(PICO_SDK_FETCH_FROM_GIT "${PICO_SDK_FETCH_FROM_GIT}" CACHE BOOL "Set to ON to fetch copy of PICO SDK from git if not otherwise locatable") set(PICO_SDK_FETCH_FROM_GIT_PATH "${PICO_SDK_FETCH_FROM_GIT_PATH}" CACHE FILEPATH "location to download SDK") if (NOT PICO_SDK_PATH) if (PICO_SDK_FETCH_FROM_GIT) include(FetchContent) set(FETCHCONTENT_BASE_DIR_SAVE ${FETCHCONTENT_BASE_DIR}) if (PICO_SDK_FETCH_FROM_GIT_PATH) get_filename_component(FETCHCONTENT_BASE_DIR "${PICO_SDK_FETCH_FROM_GIT_PATH}" REALPATH BASE_DIR "${CMAKE_SOURCE_DIR}") endif () FetchContent_Declare( pico_sdk GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk GIT_TAG master ) if (NOT pico_sdk) message("Downloading PICO SDK") FetchContent_Populate(pico_sdk) set(PICO_SDK_PATH ${pico_sdk_SOURCE_DIR}) endif () set(FETCHCONTENT_BASE_DIR ${FETCHCONTENT_BASE_DIR_SAVE}) else () message(FATAL_ERROR "PICO SDK location was not specified. Please set PICO_SDK_PATH or set PICO_SDK_FETCH_FROM_GIT to on to fetch from git." ) endif () endif () get_filename_component(PICO_SDK_PATH "${PICO_SDK_PATH}" REALPATH BASE_DIR "${CMAKE_BINARY_DIR}") if (NOT EXISTS ${PICO_SDK_PATH}) message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' not found") endif () set(PICO_SDK_INIT_CMAKE_FILE ${PICO_SDK_PATH}/pico_sdk_init.cmake) if (NOT EXISTS ${PICO_SDK_INIT_CMAKE_FILE}) message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' does not appear to contain the PICO SDK") endif () set(PICO_SDK_PATH ${PICO_SDK_PATH} CACHE PATH "Path to the PICO SDK" FORCE) include(${PICO_SDK_INIT_CMAKE_FILE}) ================================================ FILE: board/project/CMakeLists.txt ================================================ include_directories(. pio protocol sensor) add_executable(${PROJECT_NAME}) add_subdirectory(protocol) add_subdirectory(sensor) add_subdirectory(pio) target_sources(${PROJECT_NAME} PRIVATE main.c uart.c common.c led.c config.c sim_rx.c uart_pio.c usb.c serial_monitor.c dbg_rb.c dbg_task.c ) target_link_libraries(${PROJECT_NAME} pico_stdlib freertos hardware_uart hardware_irq hardware_pwm hardware_clocks hardware_adc hardware_pio hardware_i2c hardware_flash pico_i2c_slave ) pico_add_extra_outputs(${PROJECT_NAME}) pico_enable_stdio_usb(${PROJECT_NAME} 1) # 1 normal, 0 probe debug pico_enable_stdio_uart(${PROJECT_NAME} 0) # 0 normal, 1 probe debug ================================================ FILE: board/project/common.c ================================================ #include "common.h" #include #include #include "hardware/adc.h" #include "hardware/i2c.h" #include "pico/stdlib.h" float get_average(float alpha, float prev_value, float new_value) { if (isnan(new_value)) return prev_value; if (alpha > 1) return new_value; else return (1 - alpha) * prev_value + alpha * new_value; } float get_consumption(float current, uint16_t current_max, uint32_t *timestamp) { if (!*timestamp) { *timestamp = time_us_32(); return 0; } uint32_t now = time_us_32(); // us uint32_t interval = (now - *timestamp) / 1000; // ms float mah = current * interval / 3600.0; *timestamp = now; if (interval > 2000 || (current_max && (mah > current_max * interval / 3600.0))) return 0; return mah; } float voltage_read(uint8_t adc_num) { adc_select_input(adc_num); return adc_read() * BOARD_VCC / ADC_RESOLUTION; } float get_altitude(float pressure, float temperature, float pressure_initial) { if (pressure_initial == 0) return 0; return (temperature + 273.15) * (1 - pow(pressure / pressure_initial, 1 / 5.256)) / 0.0065; } void get_vspeed(float *vspeed, float altitude, uint interval) { static float altitude_prev = 0; static uint ts = 0; uint now = time_us_32(); if (now - ts >= interval * 1000) { *vspeed = (altitude - altitude_prev) / ((now - ts) / 1000000.0); altitude_prev = altitude; ts = time_us_32(); } } void get_vspeed_gps(float *vspeed, float altitude, uint interval) { static float altitude_prev = 0; static uint ts = 0; uint now = time_us_32(); if (now - ts >= interval * 1000) { *vspeed = (altitude - altitude_prev) / ((now - ts) / 1000000.0); altitude_prev = altitude; ts = time_us_32(); } } /* void circular_buffer_add(buffer_node_t *node, void *item) { buffer_node_t *new_node = malloc(sizeof(buffer_node_t)); new_node->item = item; if (node == NULL) { node = new_node; new_node->next = new_node; } else { new_node->next = node->next; node->next = new_node; node = new_node; } } void *circular_buffer_current() { if (node) return node->item; return NULL; } void circular_buffer_next(buffer_node_t *node) { if (node) node = node->next; } void circular_buffer_empty(buffer_node_t *node) { if (node) { buffer_node_t *first_node, *next_node; first_node = node; do { next_node = node->next; free(node->item); free(node); node = next_node; } while (next_node != first_node); node = NULL; } } */ ================================================ FILE: board/project/common.h ================================================ #ifndef COMMON_H #define COMMON_H #include #include #include #include "constants.h" #include "pico/stdlib.h" #include "pico/types.h" #include "shared.h" #include "dbg_task.h" /* Debug Disconnect Vcc from the RC model to the board before connecting USB Telemetry may not work properly in debug mode */ #define MSRC_DEBUG 0 // 0 = no debug, 1 = debug level 1, 2 = debug level 2 // #define SIM_RX // #define SIM_SENSORS // #define SIM_SMARTPORT_SEND_CONFIG_LUA // #define SIM_SMARTPORT_RECEIVE_CONFIG_LUA // #define SIM_SMARTPORT_SEND_SENSOR_ID // #define SIM_SMARTPORT_RECEIVE_SENSOR_ID #define PI 3.14159265358979323846 #define swap_16(value) (((value & 0xFF) << 8) | (value & 0xFF00) >> 8) #define swap_24(value) (((value & 0xFF) << 16) | (value & 0xFF00) | (value & 0xFF0000) >> 16) #define swap_32(value) \ (((value & 0xFF) << 24) | ((value & 0xFF00) << 8) | ((value & 0xFF0000) >> 8) | ((value & 0xFF000000) >> 24)) #define debug(...) \ do { \ if (context.debug) dbg_write(__VA_ARGS__); \ } while (0) #define debug_buffer(buffer, length, format) \ do { \ if (context.debug) \ dbg_write_buffer((const uint8_t *)(buffer), (length), (format)); \ } while (0) #define debug2(...) \ if (context.debug == 2) printf(__VA_ARGS__) #define debug_buffer2(buffer, length, format) \ if (context.debug == 2) \ for (int i = 0; i < (length); i++) printf((format), (buffer)[i]) typedef struct context_t { TaskHandle_t pwm_out_task_handle, uart0_notify_task_handle, uart1_notify_task_handle, uart_pio_notify_task_handle, receiver_task_handle, led_task_handle, usb_task_handle; QueueHandle_t uart0_queue_handle, uart1_queue_handle, uart_rx_pio_queue_handle, uart_tx_pio_queue_handle, tasks_queue_handle, sensors_queue_handle; alarm_pool_t *uart_alarm_pool; uint8_t debug, led_cycles; uint16_t led_cycle_duration; } context_t; float get_average(float alpha, float prev_value, float new_value); float get_consumption(float current, uint16_t current_max, uint32_t *timestamp); float voltage_read(uint8_t adc_num); float get_altitude(float pressure, float temperature, float P0); void get_vspeed(float *vspeed, float altitude, uint interval); void get_vspeed_gps(float *vspeed, float altitude, uint interval); #endif ================================================ FILE: board/project/config.c ================================================ #include "config.h" #include #include #include #include #include "pico/stdlib.h" #define CONFIG_OLD_FLASH_TARGET_OFFSET (512 * 1024) // 512kB after start of flash for program (uf2) + 512kB for config /* Receiver protocol */ #define RX_PROTOCOL \ RX_SMARTPORT // RX_SMARTPORT, RX_XBUS, RX_SRXL, RX_FRSKY_D, RX_IBUS, RX_SBUS, RX_MULTIPLEX, RX_JETIEX, RX_HITEC /* Sensors */ #define ESC_PROTOCOL ESC_NONE // ESC_NONE, ESC_HW3, ESC_HW4, ESC_PWM, ESC_CASTLE, ESC_KONTRONIK, ESC_APD_F, ESC_APD_HV #define ENABLE_GPS false #define GPS_BAUD_RATE 9600 #define ENABLE_ANALOG_VOLTAGE false #define ENABLE_ANALOG_CURRENT false #define ENABLE_ANALOG_NTC false #define ENABLE_ANALOG_AIRSPEED false #define I2C1_TYPE I2C_NONE // I2C_NONE, I2C_BMP280, I2C_MS5611, I2C_BMP180 #define I2C1_ADDRESS 0x77 // 0x76 (BMP280), 0x77 (BMP180, MS5611) /* Pwm out */ #define ENABLE_PWM_OUT false /* Refresh rate in ms (only Frsky) */ #define REFRESH_RATE_RPM 1000 #define REFRESH_RATE_VOLTAGE 1000 #define REFRESH_RATE_CURRENT 1000 #define REFRESH_RATE_TEMPERATURE 1000 #define REFRESH_RATE_GPS 1000 #define REFRESH_RATE_CONSUMPTION 1000 #define REFRESH_RATE_VARIO 1000 #define REFRESH_RATE_AIRSPEED 1000 #define REFRESH_RATE_DEFAULT 1000 /* Averaging elements (1 = no averaging) */ #define AVERAGING_ELEMENTS_RPM 1 #define AVERAGING_ELEMENTS_VOLTAGE 1 #define AVERAGING_ELEMENTS_CURRENT 1 #define AVERAGING_ELEMENTS_TEMPERATURE 1 #define AVERAGING_ELEMENTS_VARIO 1 #define AVERAGING_ELEMENTS_AIRSPEED 1 /* Analog rate*/ #define ANALOG_RATE 10 /* Analog voltage sensors */ #define ANALOG_VOLTAGE_MULTIPLIER 7.8 // 7.8 if using 68k and 10k as proposed in the docs /* Analog current sensor */ #define ANALOG_CURRENT_MULTIPLIER 1 // current_multiplier = 1000 / sensitivity(mV/A) /* Zero current output voltage offset or quiescent voltage (voltage offset when I = 0, Viout) - All hall effect core sensors (Amploc) are bidirectional. Viout=Vs/2 - Hall effect coreless sensors (IC) (ACS758) can be bidirectional or directional. Recommended to use directional for higher sensitivity. Viout defined in datasheet - If CURRENT_AUTO_OFFSET is true, then after 5 seconds, voltage read is set as offset. It is recommended to use auto offset */ #define ANALOG_CURRENT_OFFSET 0 #define ANALOG_CURRENT_AUTO_OFFSET true /* Analog airspeed */ #define AIRSPEED_OFFSET 0 // mV #define AIRSPEED_VCC 5 // V /* RPM multipliers (optional, this may be done in transmitter*/ #define RPM_PAIR_OF_POLES 1 #define RPM_PINION_TEETH 1 // For helis #define RPM_MAIN_TEETH 1 // For helis /* Vario */ #define VARIO_AUTO_OFFSET false #define BMP280_FILTER 3 // BMP Filter. Higher filter = lower noise: 1 - low, 2 - medium, 3 - high /* Only smartport and opentx */ #define SMARTPORT_SENSOR_ID 15 // Sensor Id #define SMARTPORT_DATA_ID 0x5100 // DataId (sensor type) /* Ibus */ #define IBUS_GPS_ALTERNATIVE_COORDINATES false /* XBus */ #define XBUS_CLOCK_STRECH_SWITCH false #define XBUS_ALTERNATIVE_VOLT_TEMP false /* Jeti Ex */ #define JETI_GPS_SPEED_UNITS_KMH true /* Serial monitor */ #define SERIAL_MONITOR_BAUDRATE 19200 #define SERIAL_MONITOR_STOPBITS 1 #define SERIAL_MONITOR_PARITY 0 #define SERIAL_MONITOR_TIMEOUT_MS 1 #define SERIAL_MONITOR_INVERTED false #define SERIAL_MONITOR_GPIO 5 // GPIO 1 or 5 #define SERIAL_MONITOR_FORMAT 0 // 0 = hex, 1 = text /* Add init delay for FlyFun ESC. Enable if the ESC doesn't arm */ #define ENABLE_ESC_INIT_DELAY false #define ESC_INIT_DELAY_DURATION 10000 /* HW V4/V5 parameters */ #define ESC_HW4_MANUAL_OFFSET 1 #define ESC_HW4_CURRENT_OFFSET 0 #define ESC_HW4_CURRENT_THRESHOLD 10 #define ESC_HW4_VOLTAGE_MULTIPLIER 0.0088 #define ESC_HW4_CURRENT_MULTIPLIER 0.3 #define ESC_HW4_CURRENT_MAX 250 /* Fuel flow sensor */ #define ENABLE_FUEL_FLOW false #define FUEL_FLOW_ML_PER_MINUTE 0.01 /* Fuel pressure */ #define XGZP68XXD_K 64 /* GPIO */ #define GPIO_INTERVAL_MS 1000 #define GPIO_MASK 0 /* GPS */ #define GPS_RATE 1 /* Gyro */ #define GYRO_ADDRESS 0x68 // 0x68 or 0x69 #define ACCEL_SENSITIVITY 0 // 0 = 2g, 1 = 4g, 2 = 8g, 3 = 16g #define GYRO_SENSITIVITY 0 // 0 = 250 dps, 1 = 500 dps, 2 = 1000 dps, 3 = 2000 dps #define GYRO_WEIGHTING 96 // 0 - 100% #define GYRO_FILTER 0 // 0 = no filter, 1 = 184Hz, 2 = 92Hz, 3 = 41Hz, 4 = 20Hz, 5 = 10Hz, 6 = 5Hz /* INA3221 */ #define INA3221_FILTER 1 // 1 - 1024 #define INA3221_CELLS 3 // 1 to 12 #define SENSOR_ID_SRXL2 1 // Sensor Id for SRXL2 protocol (if used). 1-15 #define NTC_OFFSET 0 config_t *config_read() { uint16_t *version = (uint16_t *)(XIP_BASE + CONFIG_FLASH_TARGET_OFFSET); if (*version != CONFIG_VERSION) { uint16_t *version_old = (uint16_t *)(XIP_BASE + CONFIG_OLD_FLASH_TARGET_OFFSET); if (*version_old == CONFIG_VERSION) { // migrate old config uint8_t flash[FLASH_PAGE_SIZE] = {0}; memcpy(flash, (uint8_t *)version_old, sizeof(config_t)); uint32_t ints = save_and_disable_interrupts(); flash_range_erase(CONFIG_FLASH_TARGET_OFFSET, FLASH_SECTOR_SIZE); flash_range_program(CONFIG_FLASH_TARGET_OFFSET, flash, FLASH_PAGE_SIZE); restore_interrupts(ints); } else { // write default config config_forze_write(); } } return (config_t *)(XIP_BASE + CONFIG_FLASH_TARGET_OFFSET); } void config_write(config_t *config) { uint8_t flash[FLASH_PAGE_SIZE] = {0}; memcpy(flash, (uint8_t *)config, sizeof(config_t)); uint32_t ints = save_and_disable_interrupts(); flash_range_erase(CONFIG_FLASH_TARGET_OFFSET, FLASH_SECTOR_SIZE); flash_range_program(CONFIG_FLASH_TARGET_OFFSET, flash, FLASH_PAGE_SIZE); restore_interrupts(ints); } void config_get(config_t *config) { memcpy(config, (config_t *)(XIP_BASE + CONFIG_FLASH_TARGET_OFFSET), sizeof(config_t)); config->debug = MSRC_DEBUG; } void config_forze_write() { config_t config = {0}; config.version = CONFIG_VERSION; config.rx_protocol = RX_PROTOCOL; config.esc_protocol = ESC_PROTOCOL; config.enable_gps = ENABLE_GPS; config.gps_baudrate = GPS_BAUD_RATE; config.enable_analog_voltage = ENABLE_ANALOG_VOLTAGE; config.enable_analog_current = ENABLE_ANALOG_CURRENT; config.enable_analog_ntc = ENABLE_ANALOG_NTC; config.enable_analog_airspeed = ENABLE_ANALOG_AIRSPEED; config.i2c_module = I2C1_TYPE; config.alpha_rpm = ALPHA(AVERAGING_ELEMENTS_RPM); config.alpha_voltage = ALPHA(AVERAGING_ELEMENTS_VOLTAGE); config.alpha_current = ALPHA(AVERAGING_ELEMENTS_CURRENT); config.alpha_temperature = ALPHA(AVERAGING_ELEMENTS_TEMPERATURE); config.alpha_vario = ALPHA(AVERAGING_ELEMENTS_VARIO); config.alpha_airspeed = ALPHA(AVERAGING_ELEMENTS_AIRSPEED); config.refresh_rate_rpm = REFRESH_RATE_RPM; config.refresh_rate_voltage = REFRESH_RATE_VOLTAGE; config.refresh_rate_current = REFRESH_RATE_CURRENT; config.refresh_rate_temperature = REFRESH_RATE_TEMPERATURE; config.refresh_rate_gps = REFRESH_RATE_GPS; config.refresh_rate_consumption = REFRESH_RATE_CONSUMPTION; config.refresh_rate_vario = REFRESH_RATE_VARIO; config.refresh_rate_airspeed = REFRESH_RATE_AIRSPEED; config.refresh_rate_default = REFRESH_RATE_DEFAULT; config.analog_voltage_multiplier = ANALOG_VOLTAGE_MULTIPLIER; config.analog_current_multiplier = ANALOG_CURRENT_MULTIPLIER; config.analog_current_offset = ANALOG_CURRENT_OFFSET; config.analog_current_autoffset = ANALOG_CURRENT_AUTO_OFFSET; config.pairOfPoles = RPM_PAIR_OF_POLES; config.mainTeeth = RPM_MAIN_TEETH; config.pinionTeeth = RPM_PINION_TEETH; config.rpm_multiplier = RPM_PINION_TEETH / (1.0 * RPM_MAIN_TEETH * RPM_PAIR_OF_POLES); config.bmp280_filter = BMP280_FILTER; config.enable_pwm_out = ENABLE_PWM_OUT; config.smartport_sensor_id = SMARTPORT_SENSOR_ID; config.smartport_data_id = SMARTPORT_DATA_ID; config.vario_auto_offset = VARIO_AUTO_OFFSET; config.xbus_clock_stretch = XBUS_CLOCK_STRECH_SWITCH; config.jeti_gps_speed_units_kmh = JETI_GPS_SPEED_UNITS_KMH; config.enable_esc_hw4_init_delay = ENABLE_ESC_INIT_DELAY; config.esc_hw4_init_delay_duration = ESC_INIT_DELAY_DURATION; config.esc_hw4_current_thresold = ESC_HW4_CURRENT_THRESHOLD; config.esc_hw4_current_max = ESC_HW4_CURRENT_MAX; config.esc_hw4_voltage_multiplier = ESC_HW4_VOLTAGE_MULTIPLIER; config.esc_hw4_current_multiplier = ESC_HW4_CURRENT_MULTIPLIER; config.ibus_alternative_coordinates = IBUS_GPS_ALTERNATIVE_COORDINATES; config.debug = MSRC_DEBUG; config.esc_hw4_is_manual_offset = ESC_HW4_MANUAL_OFFSET; config.esc_hw4_offset = ESC_HW4_CURRENT_OFFSET; config.xbus_use_alternative_volt_temp = XBUS_ALTERNATIVE_VOLT_TEMP; config.serial_monitor_baudrate = SERIAL_MONITOR_BAUDRATE; config.serial_monitor_stop_bits = SERIAL_MONITOR_STOPBITS; config.serial_monitor_parity = SERIAL_MONITOR_PARITY; config.serial_monitor_timeout_ms = SERIAL_MONITOR_TIMEOUT_MS; config.serial_monitor_inverted = SERIAL_MONITOR_INVERTED; config.airspeed_offset = AIRSPEED_OFFSET; config.airspeed_vcc = AIRSPEED_VCC * 100; config.fuel_flow_ml_per_pulse = FUEL_FLOW_ML_PER_MINUTE; config.enable_fuel_flow = ENABLE_FUEL_FLOW; config.enable_fuel_pressure = false; config.xgzp68xxd_k = XGZP68XXD_K; config.serial_monitor_format = SERIAL_MONITOR_FORMAT; config.serial_monitor_gpio = SERIAL_MONITOR_GPIO; config.gps_rate = GPS_RATE; config.gpio_interval = GPIO_INTERVAL_MS; config.gpio_mask = GPIO_MASK; config.sbus_battery_slot = true; config.fport_inverted = false; config.esc_hw4_auto_detect = true; config.enable_gyro = false; config.mpu6050_acc_scale = ACCEL_SENSITIVITY; config.mpu6050_gyro_scale = GYRO_SENSITIVITY; config.mpu6050_gyro_weighting = GYRO_WEIGHTING; //config.gyro_rate = GYRO_RATE; config.ina3221_filter = INA3221_FILTER; config.lipo_cells = INA3221_CELLS; config.enable_lipo = false; config.sensor_id_srxl2 = SENSOR_ID_SRXL2; config.ntc_offset = NTC_OFFSET; config_write(&config); } ================================================ FILE: board/project/config.h ================================================ #ifndef CONFIG_H #define CONFIG_H #include "common.h" #define CONFIG_FORZE_WRITE false #define CONFIG_VERSION 2 #define CONFIG_FLASH_TARGET_OFFSET (768 * 1024) // 768kB after start of flash. Program (uf2) max size 768kB extern context_t context; config_t *config_read(); void config_write(config_t *config); void config_forze_write(); void config_get(config_t *config); #endif ================================================ FILE: board/project/constants.h ================================================ #ifndef CONSTANTS_H #define CONSTANTS_H /* Pins */ #define UART0_TX_GPIO 0 // receiver rx #define UART0_RX_GPIO 1 // receiver tx #define I2C1_SDA_GPIO 2 // receiver sda (with pull up) #define I2C1_SCL_GPIO 3 // receiver scl (with pull up) #define UART1_TX_GPIO 4 // not used #define UART1_RX_GPIO 5 // esc tx #define PWM_CAPTURE_GPIO 4 // pwm (rpm) #define CASTLE_PWM_GPIO 4 // receiver throttle signal #define CASTLE_ESC_GPIO 5 // esc throttle signal #define UART_TX_PIO_GPIO 14 // gps rx #define UART_RX_PIO_GPIO 6 // gps tx #define CLOCK_STRETCH_GPIO 7 // npn switch #define I2C0_SDA_GPIO 8 // i2c module (vario) #define I2C0_SCL_GPIO 9 // i2c module (vario) #define PWM_OUT_GPIO 10 // pwm out #define FUELMETER_CAPTURE_GPIO 11 // fuel meter (pwm in) #define ADC0_GPIO 26 // Voltage #define ADC1_GPIO 27 // Current #define ADC2_GPIO 28 // NTC #define ADC3_GPIO 29 // Airspeed #define SMART_ESC_PWM_GPIO 12 // receiver throttle signal #define RESTORE_GPIO 15 /* UARTS */ #define UART_RECEIVER uart0 #define UART_RECEIVER_RX UART0_RX_GPIO #define UART_RECEIVER_TX UART0_TX_GPIO #define UART_ESC uart1 #define UART_ESC_RX UART1_RX_GPIO #define UART_ESC_TX UART1_TX_GPIO // set receiver to uart1 when debugging with probe // #define UART_RECEIVER uart1 // #define UART_RECEIVER_RX UART1_RX_GPIO // #define UART_RECEIVER_TX UART1_TX_GPIO /* ADC */ #define ANALOG_SENSOR_INTERVAL_MS 500 #define BOARD_VCC 3.3 #define ADC_RESOLUTION 4096 /* Stack */ #define STACK_EXTRA 100 #define STACK_RX_IBUS (200 + STACK_EXTRA) #define STACK_RX_FRSKY_D (654 + STACK_EXTRA) #define STACK_RX_MULTIPLEX (182 + STACK_EXTRA) #define STACK_RX_SMARTPORT (190 + STACK_EXTRA) #define STACK_RX_JETIEX (240 + STACK_EXTRA) #define STACK_RX_SBUS (220 + STACK_EXTRA) #define STACK_RX_HITEC (200 + STACK_EXTRA) #define STACK_RX_XBUS (200 + STACK_EXTRA) #define STACK_RX_SRXL (200 + STACK_EXTRA) #define STACK_RX_SRXL2 (300 + STACK_EXTRA) #define STACK_SERIAL_MONITOR (250 + STACK_EXTRA) #define STACK_RX_CRSF (292 + STACK_EXTRA) #define STACK_RX_HOTT (450 + STACK_EXTRA) #define STACK_RX_SANWA (200 + STACK_EXTRA) #define STACK_RX_JR_PROPO (300 + STACK_EXTRA) #define STACK_RX_FPORT (190 + STACK_EXTRA) #define STACK_RX_FBUS (190 + STACK_EXTRA) #define STACK_RX_GHST (300 + STACK_EXTRA) #define STACK_RX_JETIEX_SENSOR (240 + STACK_EXTRA) #define STACK_SIM_RX (160 + STACK_EXTRA) #define STACK_SEND_SBUS_PACKET (700 + STACK_EXTRA) #define STACK_SENSOR_FRSKY_D (164 + STACK_EXTRA) #define STACK_SENSOR_FRSKY_D_CELL (158 + STACK_EXTRA) #define STACK_SENSOR_SMARTPORT (180 + STACK_EXTRA) #define STACK_SENSOR_SMARTPORT_DOUBLE (166 + STACK_EXTRA) #define STACK_SENSOR_SMARTPORT_COORDINATE (164 + STACK_EXTRA) #define STACK_SENSOR_SMARTPORT_DATETIME (164 + STACK_EXTRA) #define STACK_SENSOR_SMARTPORT_CELL (166 + STACK_EXTRA) #define STACK_SMARTPORT_PACKET_TASK (160 + STACK_EXTRA) #define STACK_SMARTPORT_SENSOR_VOID_TASK (166 + STACK_EXTRA) #define STACK_ESC_HW3 (168 + STACK_EXTRA) #define STACK_ESC_HW4 (250 + STACK_EXTRA) #define STACK_ESC_HW5 (300 + STACK_EXTRA) #define STACK_ESC_PWM (160 + STACK_EXTRA) #define STACK_ESC_CASTLE (500 + STACK_EXTRA) #define STACK_ESC_KONTRONIK (212 + STACK_EXTRA) #define STACK_ESC_APD_F (194 + STACK_EXTRA) #define STACK_ESC_APD_HV (194 + STACK_EXTRA) #define STACK_SMART_ESC (232 + STACK_EXTRA) #define STACK_ESC_OMP_M4 (192 + STACK_EXTRA) #define STACK_ESC_ZTW (202 + STACK_EXTRA) #define STACK_ESC_OPENYGE (250 + STACK_EXTRA) #define STACK_GPS (322 + STACK_EXTRA) #define STACK_VOLTAGE (156 + STACK_EXTRA) #define STACK_CURRENT (166 + STACK_EXTRA) #define STACK_NTC (160 + STACK_EXTRA) #define STACK_AIRSPEED (184 + STACK_EXTRA) #define STACK_FUEL_METER (200 + STACK_EXTRA) #define STACK_FUEL_PRESSURE (200 + STACK_EXTRA) #define STACK_GPIO (150 + STACK_EXTRA) #define STACK_MPU6050 (252 + STACK_EXTRA) #define STACK_INA3221 (200 + STACK_EXTRA) #define STACK_BMP280 (228 + STACK_EXTRA) #define STACK_MS5611 (172 + STACK_EXTRA) #define STACK_BMP180 (174 + STACK_EXTRA) #define STACK_VSPEED (152 + STACK_EXTRA) #define STACK_DISTANCE (152 + STACK_EXTRA) #define STACK_CELL_COUNT (180 + STACK_EXTRA) #define STACK_AUTO_OFFSET (140 + STACK_EXTRA) #define STACK_PWM_OUT (200 + STACK_EXTRA) #define STACK_USB (196 + STACK_EXTRA) #define STACK_LED (186 + STACK_EXTRA) /* RPM multiplier */ #define RPM_MULTIPLIER (RPM_PINION_TEETH / (1.0 * RPM_MAIN_TEETH * RPM_PAIR_OF_POLES)) /* Averaging elements to alpha */ #define ALPHA(ELEMENTS) (2.0 / ((ELEMENTS) + 1)) /* Averaging alpha to elements */ #define ELEMENTS(ALPHA) (uint) round((2.0 / (ALPHA) - 1)) #endif ================================================ FILE: board/project/dbg_rb.c ================================================ #include "dbg_rb.h" static uint8_t *s_buf = NULL; static size_t s_cap = 0; static volatile size_t s_head = 0; // write index static volatile size_t s_tail = 0; // read index static inline size_t rb_next(size_t idx) { return (idx + 1) % s_cap; } bool dbg_rb_init(uint8_t *storage, size_t capacity) { if (!storage || capacity < 64) return false; s_buf = storage; s_cap = capacity; s_head = 0; s_tail = 0; return true; } size_t dbg_rb_available(void) { size_t head = s_head; size_t tail = s_tail; if (head >= tail) return head - tail; return (s_cap - tail) + head; } size_t dbg_rb_free(void) { // One byte is left unused to distinguish full vs empty. return (s_cap - 1) - dbg_rb_available(); } size_t dbg_rb_push(const uint8_t *data, size_t len) { if (!s_buf || !data || len == 0) return 0; size_t written = 0; while (written < len) { // If next head equals tail, buffer is full. size_t next = rb_next(s_head); if (next == s_tail) break; s_buf[s_head] = data[written++]; s_head = next; } return written; } size_t dbg_rb_pop(uint8_t *out, size_t len) { if (!s_buf || !out || len == 0) return 0; size_t read = 0; while (read < len) { if (s_tail == s_head) break; // empty out[read++] = s_buf[s_tail]; s_tail = rb_next(s_tail); } return read; } ================================================ FILE: board/project/dbg_rb.h ================================================ #pragma once #include #include #include #ifdef __cplusplus extern "C" { #endif // Ring buffer init. capacity must be >= 64 and power-of-two recommended. bool dbg_rb_init(uint8_t *storage, size_t capacity); // Write raw bytes into RB. Returns number of bytes written (may be < len if full). size_t dbg_rb_push(const uint8_t *data, size_t len); // Pop up to len bytes. Returns number of bytes popped. size_t dbg_rb_pop(uint8_t *out, size_t len); // How many bytes currently stored. size_t dbg_rb_available(void); // Free space remaining. size_t dbg_rb_free(void); #ifdef __cplusplus } #endif ================================================ FILE: board/project/dbg_task.c ================================================ // dbg_task.c #include "dbg_task.h" #include "dbg_rb.h" #include #include #include #include "FreeRTOS.h" #include "task.h" // -------- config -------- #ifndef DBG_RB_CAPACITY #define DBG_RB_CAPACITY 2048u #endif #ifndef DBG_PRINTF_TMP #define DBG_PRINTF_TMP 256u #endif #ifndef DBG_FLUSH_CHUNK #define DBG_FLUSH_CHUNK 64u #endif // ------------------------ static TaskHandle_t s_dbg_task = NULL; // Static RB storage static uint8_t s_rb_storage[DBG_RB_CAPACITY]; static void dbg_notify(void) { if (s_dbg_task) { (void)xTaskNotifyGive(s_dbg_task); } } void dbg_task_init(void) { // Safe to call multiple times static bool inited = false; if (inited) return; inited = true; (void)dbg_rb_init(s_rb_storage, sizeof(s_rb_storage)); } static void dbg_task_fn(void *arg) { (void)arg; uint8_t chunk[DBG_FLUSH_CHUNK]; for (;;) { // Wait until something is written, but also wake periodically. (void)ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(50)); // Drain RB while (dbg_rb_available() > 0) { size_t n = dbg_rb_pop(chunk, sizeof(chunk)); for (size_t i = 0; i < n; i++) { // Use putchar to avoid heavy printf parsing. putchar((int)chunk[i]); } // Optional: flush stdio if your backend needs it // fflush(stdout); taskYIELD(); } } } void dbg_task_start(unsigned stack_words, unsigned priority) { dbg_task_init(); if (s_dbg_task) return; // already started (void)xTaskCreate( dbg_task_fn, "dbg", (configSTACK_DEPTH_TYPE)stack_words, NULL, (UBaseType_t)priority, &s_dbg_task ); } int dbg_write(const char *fmt, ...) { if (!fmt) return 0; char tmp[DBG_PRINTF_TMP]; va_list ap; va_start(ap, fmt); int n = vsnprintf(tmp, sizeof(tmp), fmt, ap); va_end(ap); if (n <= 0) return n; // If truncated, n is the number that *would* have been written. size_t to_write = (size_t)n; if (to_write >= sizeof(tmp)) { to_write = sizeof(tmp) - 1; // write truncated content } (void)dbg_rb_push((const uint8_t *)tmp, to_write); dbg_notify(); return n; } void dbg_write_buffer(const uint8_t *buffer, size_t length, const char *format) { if (!buffer || length == 0 || !format) return; // Format each byte into a small temp then push. // Keep this small to avoid big stack usage. char tmp[32]; for (size_t i = 0; i < length; i++) { int n = snprintf(tmp, sizeof(tmp), format, buffer[i]); if (n > 0) { size_t to_write = (size_t)n; if (to_write >= sizeof(tmp)) to_write = sizeof(tmp) - 1; (void)dbg_rb_push((const uint8_t *)tmp, to_write); } } dbg_notify(); } ================================================ FILE: board/project/dbg_task.h ================================================ #pragma once #include #include #ifdef __cplusplus extern "C" { #endif // Must be called once at boot, after scheduler init but before using debug macros is ok too. void dbg_task_init(void); // Start the debug task. Call once after scheduler start context is ready. // stack_words: FreeRTOS words, not bytes. priority: low (e.g. 1). void dbg_task_start(unsigned stack_words, unsigned priority); // printf-like write into ring buffer + notify debug task. int dbg_write(const char *fmt, ...); // Write an array into ring buffer with per-byte formatting (no prefix). // Example format: "%02X " or "%u," etc. You control separators. void dbg_write_buffer(const uint8_t *buffer, size_t length, const char *format); #ifdef __cplusplus } #endif ================================================ FILE: board/project/led.c ================================================ #include "led.h" #include "pico/stdlib.h" #include "ws2812.h" void led_task() { const uint WS2812_PIN = 16; gpio_init(PICO_DEFAULT_LED_PIN); gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT); ws2812_init(pio0, WS2812_PIN, 800000); while (1) { for (uint8_t i = context.led_cycles; i > 0; i--) { gpio_put(PICO_DEFAULT_LED_PIN, 1); put_pixel_rgb(0, 0, 100); vTaskDelay(context.led_cycle_duration / 2 / portTICK_PERIOD_MS); gpio_put(PICO_DEFAULT_LED_PIN, 0); put_pixel_rgb(0, 0, 0); vTaskDelay(context.led_cycle_duration / 2 / portTICK_PERIOD_MS); } vTaskSuspend(NULL); } } ================================================ FILE: board/project/led.h ================================================ #ifndef LED_H #define LED_H #include "common.h" extern context_t context; void led_task(); #endif ================================================ FILE: board/project/main.c ================================================ #include #include "config.h" #include "frsky_d.h" #include "hitec.h" #include "ibus.h" #include "jetiex.h" #include "led.h" #include "multiplex.h" #include "sbus.h" #include "serial_monitor.h" #include "sim_rx.h" #include "smartport.h" #include "srxl.h" #include "srxl2.h" #include "usb.h" #include "xbus.h" #include "crsf.h" #include "hott.h" #include "sanwa.h" #include "jr_dmss.h" #include "fport.h" #include "fbus.h" #include "ghst.h" #include "jetiex_sensor.h" #include "dbg_task.h" context_t context; void vApplicationStackOverflowHook(TaskHandle_t xTask, char *pcTaskName) { printf("stack overflow %p %s\r\n", xTask, pcTaskName); while (1) ; } int main() { stdio_init_all(); gpio_init(RESTORE_GPIO); gpio_pull_up(RESTORE_GPIO); if (CONFIG_FORZE_WRITE || !gpio_get(RESTORE_GPIO)) config_forze_write(); config_t *config = config_read(); context.debug = config->debug; if (context.debug) sleep_ms(1000); xTaskCreate(usb_task, "usb_task", STACK_USB, NULL, 1, &context.usb_task_handle); dbg_task_init(); dbg_task_start(512, 1); debug("\n\nMSRC init"); context.led_cycle_duration = 200; context.led_cycles = 3; xTaskCreate(led_task, "led_task", STACK_LED, NULL, 1, &context.led_task_handle); switch (config->rx_protocol) { case RX_XBUS: xTaskCreate(xbus_task, "xbus_task", STACK_RX_XBUS, NULL, 3, &context.receiver_task_handle); break; case RX_IBUS: xTaskCreate(ibus_task, "ibus_task", STACK_RX_IBUS, NULL, 3, &context.receiver_task_handle); context.uart0_notify_task_handle = context.receiver_task_handle; break; case RX_FRSKY_D: xTaskCreate(frsky_d_task, "frsky_d_task", STACK_RX_FRSKY_D, NULL, 3, &context.receiver_task_handle); context.uart0_notify_task_handle = context.receiver_task_handle; break; case RX_MULTIPLEX: xTaskCreate(multiplex_task, "multiplex_task", STACK_RX_MULTIPLEX, NULL, 3, &context.receiver_task_handle); context.uart0_notify_task_handle = context.receiver_task_handle; break; case RX_SMARTPORT: xTaskCreate(smartport_task, "smartport_task", STACK_RX_SMARTPORT, NULL, 4, &context.receiver_task_handle); context.uart0_notify_task_handle = context.receiver_task_handle; break; case RX_JETIEX: xTaskCreate(jetiex_task, "jetiex_task", STACK_RX_JETIEX, NULL, 3, &context.receiver_task_handle); context.uart0_notify_task_handle = context.receiver_task_handle; break; case RX_SBUS: xTaskCreate(sbus_task, "sbus_task", STACK_RX_SBUS, NULL, 3, &context.receiver_task_handle); context.uart0_notify_task_handle = context.receiver_task_handle; break; case RX_HITEC: xTaskCreate(hitec_task, "hitec_task", STACK_RX_HITEC, NULL, 3, &context.receiver_task_handle); break; case RX_SRXL: xTaskCreate(srxl_task, "srxl_task", STACK_RX_SRXL, NULL, 3, &context.receiver_task_handle); context.uart0_notify_task_handle = context.receiver_task_handle; break; case RX_SRXL2: xTaskCreate(srxl2_task, "srxl2_task", STACK_RX_SRXL2, NULL, 3, &context.receiver_task_handle); context.uart0_notify_task_handle = context.receiver_task_handle; break; case SERIAL_MONITOR: xTaskCreate(serial_monitor_task, "serial_monitor", STACK_SERIAL_MONITOR, NULL, 3, &context.receiver_task_handle); context.uart0_notify_task_handle = context.receiver_task_handle; context.uart1_notify_task_handle = context.receiver_task_handle; context.uart_pio_notify_task_handle = context.receiver_task_handle; break; case RX_CRSF: xTaskCreate(crsf_task, "crfs_task", STACK_RX_CRSF, NULL, 3, &context.receiver_task_handle); context.uart0_notify_task_handle = context.receiver_task_handle; break; case RX_HOTT: xTaskCreate(hott_task, "hott_task", STACK_RX_HOTT, NULL, 3, &context.receiver_task_handle); context.uart0_notify_task_handle = context.receiver_task_handle; break; case RX_SANWA: xTaskCreate(sanwa_task, "sanwa_task", STACK_RX_SANWA, NULL, 3, &context.receiver_task_handle); context.uart0_notify_task_handle = context.receiver_task_handle; break; case RX_JR_PROPO: xTaskCreate(jr_dmss_task, "jr_dmss_task", STACK_RX_JR_PROPO, NULL, 3, &context.receiver_task_handle); context.uart0_notify_task_handle = context.receiver_task_handle; break; case RX_FPORT: xTaskCreate(fport_task, "fport_task", STACK_RX_FPORT, NULL, 4, &context.receiver_task_handle); context.uart0_notify_task_handle = context.receiver_task_handle; break; case RX_FBUS: xTaskCreate(fbus_task, "fbus_task", STACK_RX_FBUS, NULL, 4, &context.receiver_task_handle); context.uart0_notify_task_handle = context.receiver_task_handle; break; case RX_GHST: xTaskCreate(ghst_task, "ghst_task", STACK_RX_GHST, NULL, 3, &context.receiver_task_handle); context.uart0_notify_task_handle = context.receiver_task_handle; break; case RX_JETIEX_SENSOR: xTaskCreate(jetiex_sensor_task, "jetiex_sensor_task", STACK_RX_JETIEX_SENSOR, NULL, 3, &context.receiver_task_handle); context.uart_pio_notify_task_handle = context.receiver_task_handle; break; } #ifdef SIM_RX sim_rx_parameters_t parameter = {config->rx_protocol}; xTaskCreate(sim_rx_task, "sim_rx_task", STACK_SIM_RX, ¶meter, 3, NULL); #endif vTaskStartScheduler(); while (1) ; } ================================================ FILE: board/project/pio/CMakeLists.txt ================================================ target_sources(${PROJECT_NAME} PRIVATE capture_edge.c castle_link.c i2c_multi.c uart_rx.c ws2812.c uart_tx.c ) target_link_libraries(${PROJECT_NAME} pico_stdlib hardware_irq hardware_pio hardware_clocks hardware_dma ) pico_generate_pio_header(${PROJECT_NAME} ${CMAKE_CURRENT_LIST_DIR}/capture_edge.pio) pico_generate_pio_header(${PROJECT_NAME} ${CMAKE_CURRENT_LIST_DIR}/i2c_multi.pio) pico_generate_pio_header(${PROJECT_NAME} ${CMAKE_CURRENT_LIST_DIR}/uart_rx.pio) pico_generate_pio_header(${PROJECT_NAME} ${CMAKE_CURRENT_LIST_DIR}/uart_tx.pio) pico_generate_pio_header(${PROJECT_NAME} ${CMAKE_CURRENT_LIST_DIR}/castle_link.pio) pico_generate_pio_header(${PROJECT_NAME} ${CMAKE_CURRENT_LIST_DIR}/ws2812.pio) ================================================ FILE: board/project/pio/capture_edge.c ================================================ /* * Copyright (c) 2022, Daniel Gorbea * All rights reserved. * * This source code is licensed under the MIT-style license found in the * LICENSE file in the root directory of this source tree. * * Library for pin capture timer for RP2040 */ #include "capture_edge.h" #include #include "hardware/dma.h" #include "hardware/irq.h" #define MAX_PIN_COUNT 2 static uint pins_, counter_; static const uint reload_counter_ = 0xffffffff, reload_pins_ = 1; static uint sm_, offset_, dma_channel_write_pins_, dma_channel_write_counter_, dma_channel_counter_, dma_channel_reload_counter_, dma_channel_reload_pins_, dma_channel_reload_write_counter_, pin_count_; static PIO pio_; static void (*handler_[MAX_PIN_COUNT])(uint counter, edge_type_t edge) = {NULL}; static inline void handler_pio(void); static inline edge_type_t get_captured_edge(uint pin, uint pins, uint prev); static inline uint bit_value(uint pos); void capture_edge_init(PIO pio, uint pin_base, uint pin_count, float clk_div, uint irq) { pio_ = pio; pin_count_ = pin_count; // pio capture sm_ = pio_claim_unused_sm(pio_, true); offset_ = pio_add_program(pio_, &capture_edge_program); pio_sm_set_consecutive_pindirs(pio_, sm_, pin_base, pin_count, false); pio_sm_config c = capture_edge_program_get_default_config(offset_); sm_config_set_clkdiv(&c, clk_div); sm_config_set_in_pins(&c, pin_base); pio->instr_mem[offset_ + 3] = pio_encode_in(pio_pins, pin_count); pio->instr_mem[offset_ + 4] = pio_encode_in(pio_null, 32 - pin_count); sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_RX); if (irq == PIO0_IRQ_0 || irq == PIO1_IRQ_0) pio_set_irq0_source_enabled(pio_, (enum pio_interrupt_source)(pis_interrupt0 + CAPTURE_EDGE_IRQ_NUM), true); else pio_set_irq1_source_enabled(pio_, (enum pio_interrupt_source)(pis_interrupt0 + CAPTURE_EDGE_IRQ_NUM), true); pio_interrupt_clear(pio_, CAPTURE_EDGE_IRQ_NUM); pio_sm_init(pio_, sm_, offset_ + capture_edge_offset_start, &c); irq_set_exclusive_handler(irq, handler_pio); irq_set_enabled(irq, true); // get dma channels dma_channel_write_pins_ = dma_claim_unused_channel(true); dma_channel_write_counter_ = dma_claim_unused_channel(true); dma_channel_counter_ = dma_claim_unused_channel(true); dma_channel_reload_counter_ = dma_claim_unused_channel(true); dma_channel_reload_pins_ = dma_claim_unused_channel(true); dma_channel_reload_write_counter_ = dma_claim_unused_channel(true); // dma channel write pins dma_channel_config config_dma_channel_write_pins = dma_channel_get_default_config(dma_channel_write_pins_); channel_config_set_transfer_data_size(&config_dma_channel_write_pins, DMA_SIZE_32); channel_config_set_write_increment(&config_dma_channel_write_pins, false); channel_config_set_read_increment(&config_dma_channel_write_pins, false); channel_config_set_dreq(&config_dma_channel_write_pins, pio_get_dreq(pio_, sm_, false)); channel_config_set_chain_to(&config_dma_channel_write_pins, dma_channel_reload_pins_); dma_channel_configure(dma_channel_write_pins_, &config_dma_channel_write_pins, &pins_, // write address &pio_->rxf[sm_], // read address 1, false); // dma channel write counter dma_channel_config config_dma_channel_write_counter = dma_channel_get_default_config(dma_channel_write_counter_); channel_config_set_transfer_data_size(&config_dma_channel_write_counter, DMA_SIZE_32); channel_config_set_write_increment(&config_dma_channel_write_counter, false); channel_config_set_read_increment(&config_dma_channel_write_counter, false); channel_config_set_dreq(&config_dma_channel_write_pins, pio_get_dreq(pio_, sm_, false)); channel_config_set_chain_to(&config_dma_channel_write_counter, dma_channel_reload_write_counter_); dma_channel_configure(dma_channel_write_counter_, &config_dma_channel_write_counter, &counter_, // write address &dma_hw->ch[dma_channel_counter_].transfer_count, // read address 1, false); // dma channel counter dma_channel_config config_dma_channel_counter = dma_channel_get_default_config(dma_channel_counter_); channel_config_set_write_increment(&config_dma_channel_counter, false); channel_config_set_read_increment(&config_dma_channel_counter, false); uint dma_timer = dma_claim_unused_timer(true); dma_timer_set_fraction(dma_timer, 1, COUNTER_CYCLES); channel_config_set_dreq(&config_dma_channel_counter, dma_get_timer_dreq(dma_timer)); channel_config_set_chain_to(&config_dma_channel_counter, dma_channel_reload_counter_); dma_channel_configure(dma_channel_counter_, &config_dma_channel_counter, NULL, // write address NULL, // read address 0xffffffff, false); // dma channel reload counter dma_channel_config config_dma_channel_reload_counter = dma_channel_get_default_config(dma_channel_reload_counter_); channel_config_set_transfer_data_size(&config_dma_channel_reload_counter, DMA_SIZE_32); channel_config_set_write_increment(&config_dma_channel_reload_counter, false); channel_config_set_read_increment(&config_dma_channel_reload_counter, false); dma_channel_configure(dma_channel_reload_counter_, &config_dma_channel_reload_counter, &dma_hw->ch[dma_channel_counter_].al1_transfer_count_trig, // write address &reload_counter_, // read address 1, false); // dma channel reload pins dma_channel_config config_dma_channel_reload_pins = dma_channel_get_default_config(dma_channel_reload_pins_); channel_config_set_transfer_data_size(&config_dma_channel_reload_pins, DMA_SIZE_32); channel_config_set_write_increment(&config_dma_channel_reload_pins, false); channel_config_set_read_increment(&config_dma_channel_reload_pins, false); dma_channel_configure(dma_channel_reload_pins_, &config_dma_channel_reload_pins, &dma_hw->ch[dma_channel_write_pins_].al1_transfer_count_trig, // write address &reload_pins_, // read address 1, false); // dma channel reload write counter dma_channel_config config_dma_channel_reload_write_counter = dma_channel_get_default_config(dma_channel_reload_write_counter_); channel_config_set_transfer_data_size(&config_dma_channel_reload_write_counter, DMA_SIZE_32); channel_config_set_write_increment(&config_dma_channel_reload_write_counter, false); channel_config_set_read_increment(&config_dma_channel_reload_write_counter, false); dma_channel_configure(dma_channel_reload_write_counter_, &config_dma_channel_reload_write_counter, &dma_hw->ch[dma_channel_write_counter_].al1_transfer_count_trig, // write address &reload_pins_, // read address 1, false); dma_start_channel_mask((1 << dma_channel_write_pins_) | (1 << dma_channel_write_counter_) | (1 << dma_channel_counter_)); pio_sm_set_enabled(pio_, sm_, true); } void capture_edge_set_handler(uint pin, capture_handler_t handler) { if (pin < pin_count_) { handler_[pin] = handler; } } void capture_edge_remove(void) { for (uint pin = 0; pin < pin_count_; pin++) capture_edge_set_handler(pin, NULL); pio_remove_program(pio_, &capture_edge_program, offset_); pio_sm_unclaim(pio_, sm_); dma_channel_unclaim(dma_channel_write_pins_); dma_channel_unclaim(dma_channel_write_counter_); dma_channel_unclaim(dma_channel_counter_); dma_channel_unclaim(dma_channel_reload_counter_); dma_channel_unclaim(dma_channel_reload_pins_); dma_channel_unclaim(dma_channel_reload_write_counter_); } static inline void handler_pio(void) { static uint prev_pins = 0; uint counter = ~counter_; uint pins = pins_; for (uint pin = 0; pin < pin_count_; pin++) { edge_type_t edge = get_captured_edge(pin, pins, prev_pins); if (edge && *handler_[pin]) handler_[pin](counter, edge); } prev_pins = pins; pio_interrupt_clear(pio_, CAPTURE_EDGE_IRQ_NUM); } static inline edge_type_t get_captured_edge(uint pin, uint pins, uint prev) { if ((bit_value(pin) & pins) ^ (bit_value(pin) & prev) && (bit_value(pin) & pins)) return EDGE_RISE; if ((bit_value(pin) & pins) ^ (bit_value(pin) & prev) && !(bit_value(pin) & pins)) return EDGE_FALL; return EDGE_NONE; } static inline uint bit_value(uint pos) { return 1 << pos; } ================================================ FILE: board/project/pio/capture_edge.h ================================================ /* * Copyright (c) 2022, Daniel Gorbea * All rights reserved. * * This source code is licensed under the MIT-style license found in the * LICENSE file in the root directory of this source tree. * * Library for pin capture timer for RP2040 */ #ifndef CAPTURE_EDGE #define CAPTURE_EDGE #ifdef __cplusplus extern "C" { #endif #include "capture_edge.pio.h" #include "hardware/pio.h" typedef enum edge_type_t { EDGE_NONE, EDGE_FALL, EDGE_RISE } edge_type_t; typedef void (*capture_handler_t)(uint counter, edge_type_t edge); void capture_edge_init(PIO pio, uint pin_base, uint pin_count, float clk_div, uint irq); void capture_edge_set_handler(uint pin, capture_handler_t handler); void capture_edge_remove(void); #ifdef __cplusplus } #endif #endif ================================================ FILE: board/project/pio/capture_edge.pio ================================================ /* * Copyright (c) 2022, Daniel Gorbea * All rights reserved. * * This source code is licensed under the MIT-style license found in the * LICENSE file in the root directory of this source tree. * * Library for pin capture timer for RP2040 */ .define PUBLIC CAPTURE_EDGE_IRQ_NUM 3 // use 0 to 3 .define PUBLIC COUNTER_CYCLES 5 .program capture_edge capture: irq CAPTURE_EDGE_IRQ_NUM push .wrap_target public start: mov y x // pins to prev nop // in pins CAPTURE_EDGE_PIN_COUNT // read pins nop // in null ZERO_COUNT mov x isr // pins to x jmp x!=y capture // capture .wrap ================================================ FILE: board/project/pio/castle_link.c ================================================ #include "castle_link.h" #include #include "hardware/irq.h" #include "hardware/pio.h" #include "pico/stdlib.h" static uint sm_pulse_, sm_counter_, offset_pulse_, offset_counter_; static PIO pio_; static void (*handler_)(castle_link_telemetry_t packet) = {NULL}; static inline void handler_pio(); void castle_link_init(PIO pio, uint pin, uint irq) { pio_ = pio; sm_pulse_ = pio_claim_unused_sm(pio_, true); offset_pulse_ = pio_add_program(pio_, &pulse_program); pio_gpio_init(pio_, pin + 1); pio_sm_set_consecutive_pindirs(pio_, sm_pulse_, pin, 2, false); pio_sm_set_pins(pio, sm_pulse_, 0); pio_sm_config c_pulse = pulse_program_get_default_config(offset_pulse_); sm_config_set_clkdiv(&c_pulse, 10); sm_config_set_in_pins(&c_pulse, pin); sm_config_set_set_pins(&c_pulse, pin + 1, 1); pio_sm_init(pio_, sm_pulse_, offset_pulse_, &c_pulse); pio_sm_set_enabled(pio_, sm_pulse_, true); sm_counter_ = pio_claim_unused_sm(pio_, true); offset_counter_ = pio_add_program(pio_, &counter_program); pio_sm_config c_counter = counter_program_get_default_config(offset_counter_); sm_config_set_clkdiv(&c_counter, 5); sm_config_set_in_pins(&c_counter, pin); if (irq == PIO0_IRQ_0 || irq == PIO1_IRQ_0) pio_set_irq0_source_enabled(pio_, (enum pio_interrupt_source)(pis_interrupt0 + CASTLE_LINK_IRQ_NUM), true); else pio_set_irq1_source_enabled(pio_, (enum pio_interrupt_source)(pis_interrupt0 + CASTLE_LINK_IRQ_NUM), true); pio_interrupt_clear(pio_, CASTLE_LINK_IRQ_NUM); pio_sm_init(pio_, sm_counter_, offset_counter_ + counter_offset_start, &c_counter); pio_sm_set_enabled(pio_, sm_counter_, true); irq_set_exclusive_handler(irq, handler_pio); irq_set_enabled(irq, true); } void castle_link_set_handler(castle_link_handler_t handler) { handler_ = handler; } void castle_link_remove() { castle_link_set_handler(NULL); pio_remove_program(pio_, &pulse_program, offset_pulse_); pio_remove_program(pio_, &counter_program, offset_counter_); pio_sm_unclaim(pio_, sm_pulse_); pio_sm_unclaim(pio_, sm_counter_); } static inline void handler_pio() { static uint index = 0; static const float scaler[11] = {0, 20, 4, 50, 1, 0.2502, 20416.7, 4, 4, 30, 63.8125}; static uint value[12]; pio_interrupt_clear(pio_, CASTLE_LINK_IRQ_NUM); if (pio_sm_is_rx_fifo_full(pio_, sm_counter_)) { pio_sm_clear_fifos(pio_, sm_counter_); return; } uint data = pio_sm_get_blocking(pio_, sm_counter_); if (data > 50000) { index = 0; // printf("%i \n", data); return; } if (index > 10) return; value[index] = data; // printf("(%u)%u ", index, value[index]); if (index == 10) { uint calibration; castle_link_telemetry_t packet; if (value[9] < value[10]) { calibration = value[0] / 2 + value[9]; packet.is_temp_ntc = true; float temp_raw = ((float)value[10] - calibration / 2) * scaler[10] / calibration; packet.temperature = 1 / (log(temp_raw * 10200.0 / (255.0 - temp_raw) / 10000.0) / 3455.0 + 1.0 / 298.0) - 273.0; } else { calibration = value[0] / 2 + value[10]; packet.is_temp_ntc = false; packet.temperature = ((float)value[9] - calibration / 2) * scaler[9] / calibration; } packet.voltage = ((float)value[1] - calibration / 2) * scaler[1] / calibration; packet.ripple_voltage = ((float)value[2] - calibration / 2) * scaler[2] / calibration; packet.current = ((float)value[3] - calibration / 2) * scaler[3] / calibration; packet.thr = ((float)value[4] - calibration / 2) * scaler[4] / calibration; packet.output = ((float)value[5] - calibration / 2) * scaler[5] / calibration; packet.rpm = ((float)value[6] - calibration / 2) * scaler[6] / calibration; packet.voltage_bec = ((float)value[7] - calibration / 2) * scaler[7] / calibration; packet.current_bec = ((float)value[8] - calibration / 2) * scaler[8] / calibration; if (packet.voltage < 0) packet.voltage = 0; if (packet.ripple_voltage < 0) packet.ripple_voltage = 0; if (packet.current < 0) packet.current = 0; if (packet.thr < 0) packet.thr = 0; if (packet.output < 0) packet.output = 0; if (packet.rpm < 0) packet.rpm = 0; if (packet.voltage_bec < 0) packet.voltage_bec = 0; if (packet.current_bec < 0) packet.current_bec = 0; if (packet.temperature < 0) packet.temperature = 0; handler_(packet); } index++; } ================================================ FILE: board/project/pio/castle_link.h ================================================ #ifndef PIO_CASTLE #define PIO_CASTLE #include "castle_link.pio.h" /* castle telemetry index element scaler 0 sync 1 calib 1 (1000us) 2 Volt (V) 20 3 rippleVolt (V) 4 4 Curr (A) 50 5 Thr (0.1-0.2 ms esc pulse) 1 6 Output power (%) 0.2502 7 rpm 20416.7 8 becVolt (V) 4 9 becCurr (A) 4 10 temp (C) or calib 2 (500us) 30 11 temp ntc (C) or calib 2 (500us) 63.8125 */ typedef struct castle_link_telemetry_t { float voltage; float ripple_voltage; float current; float thr; float output; float rpm; float voltage_bec; float current_bec; float temperature; bool is_temp_ntc; } castle_link_telemetry_t; typedef void (*castle_link_handler_t)(castle_link_telemetry_t packet); void castle_link_init(PIO pio, uint pin_base, uint irq); void castle_link_set_handler(castle_link_handler_t handler); void castle_link_remove(); #endif ================================================ FILE: board/project/pio/castle_link.pio ================================================ /** * Copyright (c) 2022, Daniel Gorbea * All rights reserved. * * This source code is licensed under the MIT-style license found in the * LICENSE file in the root directory of this source tree. */ .define PUBLIC CASTLE_LINK_IRQ_NUM 0 // use 0 to 3 // 0 RX input // 1 PWM input/output // in base = 0 // set base = 1 .program pulse .wrap_target wait 1 pin 0 // rx pulse start -> esc is output set pindirs 1 [31] // set pin 0 output (1) wait 0 pin 0 // end of pwm rx -> esc is input. start counter (irq 4) set pindirs 0 [31] // set pin 0 input (0) irq 4 .wrap // 0 RX input // 1 PWM input // in base = 0 .program counter // counter increment every 5 cycles capture: mov isr ~y // copy counter to isr push noblock // send counter to fifo irq CASTLE_LINK_IRQ_NUM // raise irq .wrap_target public start: mov y ~null // init counter wait irq 4 loop: in pins 2 // read pins in null 31 // fill with zeros and shift to esc pin mov x isr jmp !x capture // jmp if x=0 jmp y-- loop // decrement counter .wrap ================================================ FILE: board/project/pio/i2c_multi.c ================================================ #include "i2c_multi.h" #include #include #include "hardware/irq.h" #include "hardware/pio.h" #include "i2c_multi.pio.h" #include "pico/stdlib.h" #define CLK_DIV 16 static i2c_multi_t *i2c_multi; static void (*receive_handler)(uint8_t data, bool is_address) = NULL; static void (*request_handler)(uint8_t address) = NULL; static void (*stop_handler)(uint8_t length) = NULL; static inline void start_condition_program_init(PIO pio, uint sm, uint offset, uint pin); static inline void stop_condition_program_init(PIO pio, uint sm, uint offset, uint pin); static inline void read_byte_program_init(PIO pio, uint sm, uint offset, uint pin); static inline void write_byte_program_init(PIO pio, uint sm, uint offset, uint pin); static inline void byte_handler_pio(); static inline void stop_handler_pio(); static inline uint8_t transpond_byte(uint8_t byte); void i2c_multi_init(PIO pio, uint pin) { i2c_multi = (i2c_multi_t *)malloc(sizeof(i2c_multi_t)); i2c_multi->pio = pio; i2c_multi->status = I2C_IDLE; i2c_multi->pin = pin; i2c_multi->bytes_count = 0; i2c_multi_disable_all_addresses(); i2c_multi->buffer = NULL; i2c_multi->buffer_start = NULL; uint pio_irq0 = (pio == pio0 ? PIO0_IRQ_0 : PIO1_IRQ_0); uint pio_irq1 = (pio == pio0 ? PIO0_IRQ_1 : PIO1_IRQ_1); i2c_multi->length = -1; pio_gpio_init(pio, pin); pio_gpio_init(pio, pin + 1); i2c_multi->offset_start = pio_add_program(pio, &start_condition_program); i2c_multi->sm_start = pio_claim_unused_sm(pio, true); start_condition_program_init(pio, i2c_multi->sm_start, i2c_multi->offset_start, pin); i2c_multi->offset_stop = pio_add_program(pio, &stop_condition_program); i2c_multi->sm_stop = pio_claim_unused_sm(pio, true); stop_condition_program_init(pio, i2c_multi->sm_stop, i2c_multi->offset_stop, pin); i2c_multi->offset_read = pio_add_program(pio, &read_byte_program); i2c_multi->sm_read = pio_claim_unused_sm(pio, true); read_byte_program_init(pio, i2c_multi->sm_read, i2c_multi->offset_read, pin); i2c_multi->offset_write = pio_add_program(pio, &write_byte_program); i2c_multi->sm_write = pio_claim_unused_sm(pio, true); write_byte_program_init(pio, i2c_multi->sm_write, i2c_multi->offset_write, pin); pio_sm_put(pio, i2c_multi->sm_read, (((uint32_t)do_ack_program_instructions[1]) << 16) | do_ack_program_instructions[0]); pio_sm_put(pio, i2c_multi->sm_read, (((uint32_t)do_ack_program_instructions[3]) << 16) | do_ack_program_instructions[2]); irq_set_exclusive_handler(pio_irq0, byte_handler_pio); irq_set_enabled(pio_irq0, true); irq_set_exclusive_handler(pio_irq1, stop_handler_pio); irq_set_enabled(pio_irq1, true); } void i2c_multi_set_write_buffer(uint8_t *buffer) { i2c_multi->buffer = buffer; i2c_multi->buffer_start = buffer; } void i2c_multi_set_receive_handler(i2c_multi_receive_handler_t handler) { receive_handler = handler; } void i2c_multi_set_request_handler(i2c_multi_request_handler_t handler) { request_handler = handler; } void i2c_multi_set_stop_handler(i2c_multi_stop_handler_t handler) { stop_handler = handler; } void i2c_multi_enable_address(uint8_t address) { i2c_multi->address[address / 32] |= 1 << (address % 32); } void i2c_multi_disable_address(uint8_t address) { i2c_multi->address[address / 32] &= ~(1 << (address % 32)); } void i2c_multi_enable_all_addresses() { i2c_multi->address[0] = 0xFFFFFFFF; i2c_multi->address[1] = 0xFFFFFFFF; i2c_multi->address[2] = 0xFFFFFFFF; i2c_multi->address[3] = 0xFFFFFFFF; } void i2c_multi_disable_all_addresses() { i2c_multi->address[0] = 0; i2c_multi->address[1] = 0; i2c_multi->address[2] = 0; i2c_multi->address[3] = 0; } bool i2c_multi_is_address_enabled(uint8_t address) { return i2c_multi->address[address / 32] & (1 << (address % 32)); } void i2c_multi_disable() { pio_sm_set_enabled(i2c_multi->pio, i2c_multi->sm_read, false); pio_sm_set_enabled(i2c_multi->pio, i2c_multi->sm_write, false); pio_sm_set_enabled(i2c_multi->pio, i2c_multi->sm_start, false); pio_sm_set_enabled(i2c_multi->pio, i2c_multi->sm_stop, false); pio_sm_clear_fifos(i2c_multi->pio, i2c_multi->sm_read); pio_sm_clear_fifos(i2c_multi->pio, i2c_multi->sm_write); gpio_set_input_enabled(i2c_multi->pin, true); gpio_set_input_enabled(i2c_multi->pin + 1, true); i2c_multi->bytes_count = 0; i2c_multi->status = I2C_IDLE; i2c_multi->buffer = i2c_multi->buffer_start; } void i2c_multi_restart() { i2c_multi_disable(); pio_sm_restart(i2c_multi->pio, i2c_multi->sm_start); pio_sm_restart(i2c_multi->pio, i2c_multi->sm_stop); pio_sm_restart(i2c_multi->pio, i2c_multi->sm_read); pio_sm_restart(i2c_multi->pio, i2c_multi->sm_write); pio_sm_put(i2c_multi->pio, i2c_multi->sm_read, (((uint32_t)do_ack_program_instructions[1]) << 16) | do_ack_program_instructions[0]); pio_sm_put(i2c_multi->pio, i2c_multi->sm_read, (((uint32_t)do_ack_program_instructions[3]) << 16) | do_ack_program_instructions[2]); pio_sm_set_enabled(i2c_multi->pio, i2c_multi->sm_read, true); pio_sm_set_enabled(i2c_multi->pio, i2c_multi->sm_write, true); pio_sm_set_enabled(i2c_multi->pio, i2c_multi->sm_start, true); pio_sm_set_enabled(i2c_multi->pio, i2c_multi->sm_stop, true); } void i2c_multi_remove() { receive_handler = NULL; request_handler = NULL; stop_handler = NULL; pio_set_irq0_source_enabled(i2c_multi->pio, pis_interrupt0, false); pio_set_irq1_source_enabled(i2c_multi->pio, pis_interrupt1, false); pio_clear_instruction_memory(i2c_multi->pio); pio_sm_unclaim(i2c_multi->pio, i2c_multi->sm_start); pio_sm_unclaim(i2c_multi->pio, i2c_multi->sm_stop); pio_sm_unclaim(i2c_multi->pio, i2c_multi->sm_read); pio_sm_unclaim(i2c_multi->pio, i2c_multi->sm_write); i2c_multi->buffer = NULL; i2c_multi->buffer_start = NULL; i2c_multi->bytes_count = 0; i2c_multi->status = I2C_IDLE; gpio_set_input_enabled(i2c_multi->pin, true); gpio_set_input_enabled(i2c_multi->pin + 1, true); free(i2c_multi); } void i2c_multi_fixed_length(int16_t length) { i2c_multi->length = length; } static inline void start_condition_program_init(PIO pio, uint sm, uint offset, uint pin) { pio_sm_config c = start_condition_program_get_default_config(offset); sm_config_set_in_pins(&c, pin); sm_config_set_clkdiv(&c, CLK_DIV); sm_config_set_jmp_pin(&c, pin + 1); pio_sm_init(pio, sm, offset + start_condition_offset_start, &c); pio_sm_set_enabled(pio, sm, true); } static inline void stop_condition_program_init(PIO pio, uint sm, uint offset, uint pin) { pio_sm_config c = stop_condition_program_get_default_config(offset); sm_config_set_in_pins(&c, pin); sm_config_set_clkdiv(&c, CLK_DIV); sm_config_set_jmp_pin(&c, pin + 1); pio_sm_init(pio, sm, offset + stop_condition_offset_start, &c); pio_sm_set_enabled(pio, sm, true); pio_set_irq1_source_enabled(pio, pis_interrupt1, true); pio_interrupt_clear(pio, 1); } static inline void read_byte_program_init(PIO pio, uint sm, uint offset, uint pin) { pio_sm_config c = read_byte_program_get_default_config(offset); sm_config_set_in_pins(&c, pin); sm_config_set_clkdiv(&c, CLK_DIV); sm_config_set_out_shift(&c, true, true, 32); pio_set_irq0_source_enabled(pio, pis_interrupt0, true); pio_interrupt_clear(pio, 0); sm_config_set_set_pins(&c, pin, 2); sm_config_set_sideset_pins(&c, pin); pio_sm_init(pio, sm, offset, &c); pio_sm_set_enabled(pio, sm, true); } static inline void write_byte_program_init(PIO pio, uint sm, uint offset, uint pin) { pio_sm_config c = write_byte_program_get_default_config(offset); sm_config_set_in_pins(&c, pin); sm_config_set_out_pins(&c, pin, 1); sm_config_set_set_pins(&c, pin, 2); sm_config_set_sideset_pins(&c, pin); sm_config_set_clkdiv(&c, CLK_DIV); sm_config_set_out_shift(&c, true, true, 32); sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_TX); sm_config_set_jmp_pin(&c, pin); pio_sm_init(pio, sm, offset, &c); pio_sm_set_enabled(pio, sm, true); } static inline void byte_handler_pio() { uint8_t received = 0; bool is_address = false; i2c_multi->bytes_count++; if (i2c_multi->status != I2C_WRITE) { received = transpond_byte(pio_sm_get_blocking(i2c_multi->pio, i2c_multi->sm_read) >> 24); // Do the bit-reverse here as PIO instructions are scarce } if (i2c_multi->status == I2C_IDLE) { if (!i2c_multi_is_address_enabled(received >> 1)) { i2c_multi->status = I2C_IDLE; i2c_multi->bytes_count = 0; pio_sm_put(i2c_multi->pio, i2c_multi->sm_read, (((uint32_t)do_ack_program_instructions[10] + i2c_multi->offset_read) << 16) | do_ack_program_instructions[9]); pio_sm_put(i2c_multi->pio, i2c_multi->sm_read, (((uint32_t)do_ack_program_instructions[1]) << 16) | do_ack_program_instructions[0]); pio_sm_put(i2c_multi->pio, i2c_multi->sm_read, (((uint32_t)do_ack_program_instructions[3]) << 16) | do_ack_program_instructions[2]); pio_interrupt_clear(i2c_multi->pio, 0); return; } if (received & 1) { i2c_multi->status = I2C_WRITE; } else { i2c_multi->status = I2C_READ; } is_address = true; } if (i2c_multi->status == I2C_READ) { pio_sm_put(i2c_multi->pio, i2c_multi->sm_read, (((uint32_t)do_ack_program_instructions[5]) << 16) | do_ack_program_instructions[4]); pio_sm_put(i2c_multi->pio, i2c_multi->sm_read, (((uint32_t)do_ack_program_instructions[7] + i2c_multi->offset_read) << 16) | do_ack_program_instructions[6]); pio_sm_put(i2c_multi->pio, i2c_multi->sm_read, (((uint32_t)do_ack_program_instructions[1]) << 16) | do_ack_program_instructions[0]); pio_sm_put(i2c_multi->pio, i2c_multi->sm_read, (((uint32_t)do_ack_program_instructions[3]) << 16) | do_ack_program_instructions[2]); if (receive_handler) { if (is_address) { receive_handler(received >> 1, true); } else receive_handler(received, false); } } if (i2c_multi->status == I2C_WRITE && is_address) { if (request_handler) { request_handler(received >> 1); } uint8_t value = 0; if (i2c_multi->buffer) { value = transpond_byte(*i2c_multi->buffer); i2c_multi->buffer++; } pio_sm_put(i2c_multi->pio, i2c_multi->sm_read, (((uint32_t)do_ack_program_instructions[5]) << 16) | do_ack_program_instructions[4]); pio_sm_put(i2c_multi->pio, i2c_multi->sm_read, (((uint32_t)do_ack_program_instructions[8] + i2c_multi->offset_read) << 16) | do_ack_program_instructions[6]); pio_sm_put(i2c_multi->pio, i2c_multi->sm_read, (((uint32_t)do_ack_program_instructions[1]) << 16) | do_ack_program_instructions[0]); pio_sm_put_blocking(i2c_multi->pio, i2c_multi->sm_read, (((uint32_t)do_ack_program_instructions[3]) << 16) | do_ack_program_instructions[2]); pio_sm_put(i2c_multi->pio, i2c_multi->sm_write, value); pio_sm_put(i2c_multi->pio, i2c_multi->sm_write, (((uint32_t)wait_ack_program_instructions[1]) << 16) | wait_ack_program_instructions[0]); pio_sm_put(i2c_multi->pio, i2c_multi->sm_write, (((uint32_t)wait_ack_program_instructions[3]) << 16) | wait_ack_program_instructions[2]); } if (i2c_multi->status == I2C_WRITE && !is_address) { if (i2c_multi->length == -1 || (i2c_multi->length > 0 && i2c_multi->bytes_count < i2c_multi->length + 1)) { uint8_t value = 0; if (i2c_multi->buffer) { value = transpond_byte(*i2c_multi->buffer); i2c_multi->buffer++; } pio_sm_put(i2c_multi->pio, i2c_multi->sm_write, (((uint32_t)wait_ack_program_instructions[5]) << 16) | wait_ack_program_instructions[4]); pio_sm_put(i2c_multi->pio, i2c_multi->sm_write, (((uint32_t)wait_ack_program_instructions[7] + i2c_multi->offset_write) << 16) | (wait_ack_program_instructions[6]) + i2c_multi->offset_write); pio_sm_put(i2c_multi->pio, i2c_multi->sm_write, value); pio_sm_put(i2c_multi->pio, i2c_multi->sm_write, (((uint32_t)wait_ack_program_instructions[1]) << 16) | wait_ack_program_instructions[0]); pio_sm_put(i2c_multi->pio, i2c_multi->sm_write, (((uint32_t)wait_ack_program_instructions[3]) << 16) | wait_ack_program_instructions[2]); } else { pio_sm_exec(i2c_multi->pio, i2c_multi->sm_read, i2c_multi->offset_read); pio_sm_clear_fifos(i2c_multi->pio, i2c_multi->sm_write); pio_sm_exec(i2c_multi->pio, i2c_multi->sm_write, wait_ack_program_instructions[8]); pio_sm_exec(i2c_multi->pio, i2c_multi->sm_write, wait_ack_program_instructions[9] + i2c_multi->offset_write); if (stop_handler) { stop_handler(i2c_multi->bytes_count - 1); } i2c_multi->bytes_count = 0; i2c_multi->status = I2C_IDLE; } } pio_interrupt_clear(i2c_multi->pio, 0); } static inline void stop_handler_pio() { pio_interrupt_clear(i2c_multi->pio, 1); if (i2c_multi->status == I2C_IDLE) return; pio_sm_exec(i2c_multi->pio, i2c_multi->sm_read, i2c_multi->offset_read); pio_sm_clear_fifos(i2c_multi->pio, i2c_multi->sm_write); pio_sm_exec(i2c_multi->pio, i2c_multi->sm_write, wait_ack_program_instructions[8]); pio_sm_exec(i2c_multi->pio, i2c_multi->sm_write, wait_ack_program_instructions[9] + i2c_multi->offset_write); i2c_multi->buffer = i2c_multi->buffer_start; if (stop_handler) { stop_handler(i2c_multi->bytes_count - 1); } i2c_multi->bytes_count = 0; i2c_multi->status = I2C_IDLE; } static inline uint8_t transpond_byte(uint8_t byte) { uint8_t transponded = ((byte & 0x1) << 7) | (((byte & 0x2) >> 1) << 6) | (((byte & 0x4) >> 2) << 5) | (((byte & 0x8) >> 3) << 4) | (((byte & 0x10) >> 4) << 3) | (((byte & 0x20) >> 5) << 2) | (((byte & 0x40) >> 6) << 1) | (((byte & 0x80) >> 7)); return transponded; } ================================================ FILE: board/project/pio/i2c_multi.h ================================================ #ifndef I2C_MULTI #define I2C_MULTI #include "hardware/pio.h" #include "common.h" typedef enum i2c_multi_status_t { I2C_IDLE, I2C_READ, I2C_WRITE } i2c_multi_status_t; typedef void (*i2c_multi_receive_handler_t)(uint8_t data, bool is_address); typedef void (*i2c_multi_request_handler_t)(uint8_t address); typedef void (*i2c_multi_stop_handler_t)(uint8_t length); typedef struct i2c_multi_t { PIO pio; uint offset_read, offset_write, sm_read, sm_write, offset_start, offset_stop, sm_start, sm_stop, pin; i2c_multi_status_t status; uint8_t *buffer, *buffer_start; uint8_t bytes_count; int16_t length; uint address[4]; } i2c_multi_t; extern context_t context; void i2c_multi_init(PIO pio, uint pin); void i2c_multi_set_write_buffer(uint8_t *buffer); void i2c_multi_set_receive_handler(i2c_multi_receive_handler_t handler); void i2c_multi_set_request_handler(i2c_multi_request_handler_t handler); void i2c_multi_set_stop_handler(i2c_multi_stop_handler_t handler); void i2c_multi_enable_address(uint8_t address); void i2c_multi_disable_address(uint8_t address); void i2c_multi_enable_all_addresses(); void i2c_multi_disable_all_addresses(); bool i2c_multi_is_address_enabled(uint8_t address); void i2c_multi_disable(); void i2c_multi_restart(); void i2c_multi_remove(); void i2c_multi_fixed_length(int16_t length); #endif ================================================ FILE: board/project/pio/i2c_multi.pio ================================================ /** * ------------------------------------------------------------------------------- * * Copyright (c) 2022, Daniel Gorbea * All rights reserved. * * This source code is licensed under the MIT-style license found in the * LICENSE file in the root directory of this source tree. * * ------------------------------------------------------------------------------- * * I2C slave multi - answer to multiple addresses * * SDA = pin * SCL = pin + 1 * * Add external pull ups, 1k - 3.3k * * Define handlers and write buffer * * ------------------------------------------------------------------------------- */ // sda 0, scl 1 .program start_condition // 5 do_irq: irq 1 // Ensure a stop has been handled, in case this is a repeat start irq 4 public start: .wrap_target wait 1 pin 0 wait 0 pin 0 jmp pin do_irq .wrap .program stop_condition // 4 do_irq: irq 1 public start: .wrap_target wait 0 pin 0 wait 1 pin 0 jmp pin do_irq .wrap .program read_byte // 10 .side_set 2 opt pindirs wait irq 4 read: set x 7 side 0 bit_loop: wait 0 pin 1 wait 1 pin 1 in pins 1 jmp x-- bit_loop push noblock do_ack: out exec 16 jmp do_ack do_irq: irq 5 .program do_ack .side_set 2 opt pindirs wait 0 pin 1 set pins 0 side 3 nop irq wait 0 set pins 0 side 1 wait 1 pin 1 wait 0 pin 1 // read (receive request) jmp 1 // write (write request) jmp 9 // address not enabled set pindirs 0 jmp 0 .program write_byte // 9 .side_set 2 opt pindirs wait irq 5 write: set x 7 side 1 bit_loop: wait 0 pin 1 out pins 1 wait 1 pin 1 jmp x-- bit_loop out null 32 wait_ack: out exec 16 jmp wait_ack .program wait_ack .side_set 2 opt pindirs // clk stretch wait 0 pin 1 nop //set pindirs 2 set pins 0 side 2 irq wait 0 // check ack nop wait 1 pin 1 side 0 jmp pin 0 jmp 1 // stop out null 32 side 0 jmp 0 ================================================ FILE: board/project/pio/uart_rx.c ================================================ #include "uart_rx.h" #include "hardware/clocks.h" #include "hardware/irq.h" #include "hardware/pio.h" #include "pico/stdlib.h" static uint sm_, offset_; static PIO pio_; static void (*handler_)(uint8_t data) = NULL; static inline void handler_pio(void); uint uart_rx_init(PIO pio, uint pin, uint baudrate, uint irq) { pio_ = pio; sm_ = pio_claim_unused_sm(pio_, true); pio_sm_set_consecutive_pindirs(pio_, sm_, pin, 1, false); pio_gpio_init(pio_, pin); gpio_pull_up(pin); offset_ = pio_add_program(pio_, &uart_rx_program); pio_sm_config c = uart_rx_program_get_default_config(offset_); sm_config_set_in_pins(&c, pin); sm_config_set_jmp_pin(&c, pin); sm_config_set_in_shift(&c, true, false, 32); sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_RX); float div = (float)clock_get_hz(clk_sys) / (UART_RX_CYCLES_PER_BIT * baudrate); sm_config_set_clkdiv(&c, div); if (irq == PIO0_IRQ_0 || irq == PIO1_IRQ_0) pio_set_irq0_source_enabled(pio_, (enum pio_interrupt_source)(pis_interrupt0 + UART_RX_IRQ_NUM), true); else pio_set_irq1_source_enabled(pio_, (enum pio_interrupt_source)(pis_interrupt0 + UART_RX_IRQ_NUM), true); pio_interrupt_clear(pio_, UART_RX_IRQ_NUM); pio_sm_init(pio_, sm_, offset_, &c); pio_sm_set_enabled(pio_, sm_, true); irq_set_exclusive_handler(irq, handler_pio); irq_set_enabled(irq, true); return sm_; } void uart_rx_set_handler(uart_rx_handler_t handler) { handler_ = handler; } void uart_rx_remove(void) { uart_rx_set_handler(NULL); pio_remove_program(pio_, &uart_rx_program, offset_); pio_sm_unclaim(pio_, sm_); } static inline void handler_pio(void) { pio_interrupt_clear(pio_, UART_RX_IRQ_NUM); if (handler_) { while (pio_sm_get_rx_fifo_level(pio_, sm_)) { uint data = pio_sm_get_blocking(pio_, sm_); handler_(data >> 24); } } } ================================================ FILE: board/project/pio/uart_rx.h ================================================ #ifndef UART_RX #define UART_RX #include "uart_rx.pio.h" typedef void (*uart_rx_handler_t)(uint8_t data); uint uart_rx_init(PIO pio, uint pin, uint baudrate, uint irq); void uart_rx_set_handler(uart_rx_handler_t handler); void uart_rx_remove(); #endif ================================================ FILE: board/project/pio/uart_rx.pio ================================================ /** * Copyright (c) 2022, Daniel Gorbea * All rights reserved. * * This source code is licensed under the MIT-style license found in the * LICENSE file in the root directory of this source tree. */ .define PUBLIC UART_RX_IRQ_NUM 2 // use 0 to 3 .define PUBLIC UART_RX_CYCLES_PER_BIT 16 // we want here resolution to reduce the error when sampling. more cycles less error .program uart_rx // 1 bit every 16 cycles start: wait 0 pin 0 set x 7 [16+16/2-2] bit_loop: in pins 1 jmp x-- bit_loop [16-2] jmp pin good_stop wait 1 pin 0 [4] jmp start good_stop: push irq UART_RX_IRQ_NUM ================================================ FILE: board/project/pio/uart_tx.c ================================================ #include "uart_tx.h" #include "hardware/clocks.h" #include "hardware/pio.h" #include "pico/stdlib.h" static uint sm_, offset_; static PIO pio_; static uint8_t data_bits_; static uint8_t parity_; uint uart_tx_init(PIO pio, uint pin, uint baudrate, uint data_bits, uint stop_bits, uint parity) { pio_ = pio; data_bits_ = data_bits; parity_ = parity; gpio_pull_up(pin); sm_ = pio_claim_unused_sm(pio_, true); pio_gpio_init(pio_, pin); pio_sm_set_consecutive_pindirs(pio_, sm_, pin, 1, false); pio_sm_set_pins(pio, sm_, 0xFFFF); offset_ = pio_add_program(pio_, &uart_tx_program); pio_->instr_mem[offset_] = pio_encode_set(pio_x, parity == UART_PARITY_NONE ? data_bits - 1 : data_bits); pio_->instr_mem[offset_ + 6] = pio_encode_set(pio_pins, 1) | pio_encode_delay(16 * stop_bits - 1); pio_sm_config c = uart_tx_program_get_default_config(offset_); sm_config_set_out_pins(&c, pin, 1); sm_config_set_set_pins(&c, pin, 1); sm_config_set_in_shift(&c, true, false, 32); sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_TX); float div = (float)clock_get_hz(clk_sys) / (UART_TX_CYCLES_PER_BIT * baudrate); sm_config_set_clkdiv(&c, div); pio_sm_init(pio_, sm_, offset_, &c); pio_sm_set_enabled(pio_, sm_, true); return sm_; } void uart_tx_write(uint32_t c) { if (parity_ != UART_PARITY_NONE) { bool parity_bit = 0; for (uint i = 0; i < data_bits_; i++) { parity_bit ^= (c >> i) & 1; } c |= (parity_bit << (data_bits_)); if (parity_ == UART_PARITY_ODD) { c ^= (1 << (data_bits_)); } } pio_sm_put_blocking(pio_, sm_, c); } void uart_tx_write_bytes(void *data, uint8_t length) { for (uint8_t i = 0; i < length; i++) { switch (data_bits_) { case 1 ... 8: uart_tx_write(((uint8_t *)data)[i]); break; case 9 ... 16: uart_tx_write(((uint16_t *)data)[i]); break; case 17 ... 32: uart_tx_write(((uint32_t *)data)[i]); break; } } } void uart_tx_remove(void) { pio_remove_program(pio_, &uart_tx_program, offset_); pio_sm_unclaim(pio_, sm_); } ================================================ FILE: board/project/pio/uart_tx.h ================================================ #ifndef UART_TX #define UART_TX #include "uart_tx.pio.h" uint uart_tx_init(PIO pio, uint pin, uint baudrate, uint data_bits, uint stop_bits, uint parity); void uart_tx_write(uint32_t c); void uart_tx_write_bytes(void *data, uint8_t length); void uart_tx_remove(void); #endif ================================================ FILE: board/project/pio/uart_tx.pio ================================================ /** * Copyright (c) 2025, Daniel Gorbea * All rights reserved. * * This source code is licensed under the MIT-style license found in the * LICENSE file in the root directory of this source tree. */ .define PUBLIC UART_TX_CYCLES_PER_BIT 16 // we want here resolution to reduce division error when writing. more cycles less error .program uart_tx // 1 bit every 16 cycles start: nop // set x n (0 to 31). Number of data bits pull block set pindirs 1 // take control of pin set pins 0 [16-2] // start bit bit_loop: out pins 1 jmp x-- bit_loop [16-2] nop // set pins 1 [16*stop-1]. Stop bits (1-2) nop [16] set pindirs 0 // release pin ================================================ FILE: board/project/pio/ws2812.c ================================================ #include "ws2812.h" #include "hardware/clocks.h" #include "hardware/pio.h" #include "pico/stdlib.h" static uint sm_; static PIO pio_; static inline uint32_t urgb_u32(uint8_t r, uint8_t g, uint8_t b); void ws2812_init(PIO pio, uint pin, float freq) { uint offset = pio_add_program(pio, &ws2812_program); pio_ = pio; sm_ = pio_claim_unused_sm(pio, true); pio_gpio_init(pio, pin); pio_sm_set_consecutive_pindirs(pio, sm_, pin, 1, true); pio_sm_config c = ws2812_program_get_default_config(offset); sm_config_set_sideset_pins(&c, pin); sm_config_set_out_shift(&c, false, true, 24); sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_TX); int cycles_per_bit = ws2812_T1 + ws2812_T2 + ws2812_T3; float div = clock_get_hz(clk_sys) / (freq * cycles_per_bit); sm_config_set_clkdiv(&c, div); pio_sm_init(pio, sm_, offset, &c); pio_sm_set_enabled(pio, sm_, true); } void put_pixel_rgb(uint8_t r, uint8_t g, uint8_t b) { pio_sm_put_blocking(pio_, sm_, urgb_u32(r, g, b) << 8u); } static inline uint32_t urgb_u32(uint8_t r, uint8_t g, uint8_t b) { return ((uint32_t)(r) << 8) | ((uint32_t)(g) << 16) | (uint32_t)(b); } ================================================ FILE: board/project/pio/ws2812.h ================================================ #ifndef WS2812_H #define WS2812_H #include "ws2812.pio.h" void ws2812_init(PIO pio, uint pin, float freq); void put_pixel_rgb(uint8_t r, uint8_t g, uint8_t b); #endif ================================================ FILE: board/project/pio/ws2812.pio ================================================ ; ; Copyright (c) 2020 Raspberry Pi (Trading) Ltd. ; ; SPDX-License-Identifier: BSD-3-Clause ; .program ws2812 .side_set 1 .define public T1 2 .define public T2 5 .define public T3 3 .wrap_target bitloop: out x, 1 side 0 [T3 - 1] ; Side-set still takes place when instruction stalls jmp !x do_zero side 1 [T1 - 1] ; Branch on the bit we shifted out. Positive pulse do_one: jmp bitloop side 1 [T2 - 1] ; Continue driving high, for a long pulse do_zero: nop side 0 [T2 - 1] ; Or drive low, for a short pulse .wrap ================================================ FILE: board/project/protocol/CMakeLists.txt ================================================ target_sources(${PROJECT_NAME} PRIVATE frsky_d.c hitec.c ibus.c jetiex.c multiplex.c sbus.c smartport.c srxl.c xbus.c srxl2.c crsf.c hott.c sanwa.c jr_dmss.c fport.c fbus.c ghst.c jetiex_sensor.c ) ================================================ FILE: board/project/protocol/crsf.c ================================================ #include "crsf.h" #include #include #include #include #include "airspeed.h" #include "bmp180.h" #include "bmp280.h" #include "config.h" #include "current.h" #include "esc_apd_f.h" #include "esc_apd_hv.h" #include "esc_castle.h" #include "esc_hw3.h" #include "esc_hw4.h" #include "esc_hw5.h" #include "esc_kontronik.h" #include "esc_omp_m4.h" #include "esc_pwm.h" #include "esc_ztw.h" #include "gps.h" #include "ibus.h" #include "ina3221.h" #include "mpu6050.h" #include "ms5611.h" #include "ntc.h" #include "pwm_out.h" #include "smart_esc.h" #include "uart.h" #include "uart_pio.h" #include "voltage.h" #define CRSF_FRAMETYPE_GPS 0x02 #define CRSF_FRAMETYPE_VARIO 0x07 #define CRSF_FRAMETYPE_BATTERY_SENSOR 0x08 #define CRSF_FRAMETYPE_BARO_ALTITUDE 0x09 #define CRSF_FRAMETYPE_HEARTBEAT 0x0B #define CRSF_FRAMETYPE_LINK_STATISTICS 0x14 #define CRSF_FRAMETYPE_RC_CHANNELS_PACKED 0x16 #define CRSF_FRAMETYPE_SUBSET_RC_CHANNELS_PACKED 0x17 #define CRSF_FRAMETYPE_LINK_STATISTICS_RX 0x1C #define CRSF_FRAMETYPE_LINK_STATISTICS_TX 0x1D #define CRSF_FRAMETYPE_ATTITUDE 0x1E #define CRSF_FRAMETYPE_FLIGHT_MODE 0x21 #define CRSF_FRAMETYPE_AIRSPEED 0x0A #define CRSF_FRAMETYPE_RPM 0x0C #define CRSF_FRAMETYPE_TEMP 0x0D #define CRSF_FRAMETYPE_VOLTAGES 0x0E #define CRSF_FRAMETYPE_GPS_TIME 0x03 #define CRSF_FRAMETYPE_GPS_EXTENDED 0x06 #define CRSF_TIMEOUT_US 1000 typedef enum crsf_sensor_type_t { TYPE_GPS, TYPE_VARIO, TYPE_BATERY, TYPE_BARO, TYPE_AIRSPEED, TYPE_RPM, TYPE_TEMP, TYPE_VOLTAGES, TYPE_GPS_TIME, TYPE_GPS_EXTENDED, TYPE_ATTITUDE, MAX_SENSORS } crsf_sensor_type_t; /* CRSF frame has the structure: Device address: (uint8_t) Frame length: length in bytes including Type (uint8_t) Type: (uint8_t) CRC: (uint8_t), crc of and */ typedef struct crsf_sensor_gps_formatted_t { int32_t latitude; // degree / 10,000,000 big endian int32_t longitude; // degree / 10,000,000 big endian uint16_t groundspeed; // km/h / 10 big endian uint16_t heading; // GPS heading, degree/100 big endian uint16_t altitude; // meters, +1000m big endian uint8_t satellites; // satellites } __attribute__((packed)) crsf_sensor_gps_formatted_t; typedef struct crsf_sensor_vario_formatted_t { int16_t vspeed; // cm/s } __attribute__((packed)) crsf_sensor_vario_formatted_t; typedef struct crsf_sensor_baro_formatted_t { uint16_t altitude; int16_t vspeed; // cm/s } __attribute__((packed)) crsf_sensor_baro_formatted_t; typedef struct crsf_sensor_battery_formatted_t { uint16_t voltage; // V * 10 big endian uint16_t current; // A * 10 big endian uint32_t capacity : 24; // used capacity mah big endian uint8_t remaining; // % } __attribute__((packed)) crsf_sensor_battery_formatted_t; typedef struct crsf_sensor_airspeed_formatted_t { uint16_t speed; // V * 10 big endian } __attribute__((packed)) crsf_sensor_airspeed_formatted_t; typedef struct crsf_sensor_rpm_formatted_t { uint8_t source; uint32_t rpm : 24; } __attribute__((packed)) crsf_sensor_rpm_formatted_t; typedef struct crsf_sensor_temp_formatted_t { uint8_t source; int16_t temp; } __attribute__((packed)) crsf_sensor_temp_formatted_t; typedef struct crsf_sensor_voltages_formatted_t { uint8_t source; uint16_t voltage; } __attribute__((packed)) crsf_sensor_voltages_formatted_t; typedef struct crsf_sensor_gps_time_formatted_t { uint16_t year; uint8_t month; uint8_t day; uint8_t hour; uint8_t minute; uint8_t second; uint16_t millisecond; } __attribute__((packed)) crsf_sensor_gps_time_formatted_t; typedef struct crsf_sensor_gps_extended_formatted_t { uint8_t fix_type; // Current GPS fix quality int16_t n_speed; // Northward (north = positive) Speed [cm/sec] int16_t e_speed; // Eastward (east = positive) Speed [cm/sec] int16_t v_speed; // Vertical (up = positive) Speed [cm/sec] int16_t h_speed_acc; // Horizontal Speed accuracy cm/sec int16_t track_acc; // Heading accuracy in degrees scaled with 1e-1 degrees times 10) int16_t alt_ellipsoid; // Meters Height above GPS Ellipsoid (not MSL) int16_t h_acc; // horizontal accuracy in cm int16_t v_acc; // vertical accuracy in cm uint8_t reserved; uint8_t hDOP; // Horizontal dilution of precision,Dimensionless in nits of.1. uint8_t vDOP; // vertical dilution of precision, Dimensionless in nits of .1. } __attribute__((packed)) crsf_sensor_gps_extended_formatted_t; typedef struct crsf_sensor_gps_t { float *latitude; // degree / 10,000,000 big endian float *longitude; // degree / 10,000,000 big endian float *groundspeed; // km/h / 10 big endian float *heading; // GPS heading, degree/100 big endian float *altitude; // meters, +1000m big endian float *satellites; // satellites } crsf_sensor_gps_t; typedef struct crsf_sensor_attitude_formatted_t { int16_t pitch; // angle ( rad / 10000 ) int16_t roll; // angle ( rad / 10000 ) int16_t yaw; // angle ( rad / 10000 ) } __attribute__((packed)) crsf_sensor_attitude_formatted_t; typedef struct crsf_sensor_vario_t { float *vspeed; // cm/s } crsf_sensor_vario_t; typedef struct crsf_sensor_baro_t { float *altitude; float *vspeed; // cm/s } crsf_sensor_baro_t; typedef struct crsf_sensor_battery_t { float *voltage; // V * 10 big endian float *current; // A * 10 big endian float *capacity; // used capacity mah big endian float *remaining; // % } crsf_sensor_battery_t; typedef struct crsf_sensor_airspeed_t { float *speed; // km/h * 10 big endian } crsf_sensor_airspeed_t; typedef struct crsf_sensor_rpm_t { float *rpm; } crsf_sensor_rpm_t; typedef struct crsf_sensor_temp_t { uint8_t temperature_count; float *temperature[5]; // V * 10 big endian } crsf_sensor_temp_t; typedef struct crsf_sensor_voltages_t { bool is_cell_average; uint8_t *cell_count; float *cell[18]; uint8_t voltage_count; float *voltage[10]; } crsf_sensor_voltages_t; typedef struct crsf_sensor_gps_time_t { float *date; float *time; } crsf_sensor_gps_time_t; typedef struct crsf_sensor_gps_extended_t { float *fix; float *n_speed; float *e_speed; float *v_speed; float *h_speed_acc; float *track_acc; float *alt_ellipsoid; float *h_acc; float *v_acc; float *hdop; float *vdop; } crsf_sensor_gps_extended_t; typedef struct crsf_sensor_attitude_t { float *pitch; float *roll; float *yaw; } crsf_sensor_attitude_t; typedef struct crsf_sensors_t { bool enabled_sensors[MAX_SENSORS]; crsf_sensor_gps_t gps; crsf_sensor_vario_t vario; crsf_sensor_battery_t battery; crsf_sensor_baro_t baro; crsf_sensor_airspeed_t airspeed; crsf_sensor_rpm_t rpm; crsf_sensor_temp_t temperature; crsf_sensor_voltages_t voltages; crsf_sensor_gps_time_t gps_time; crsf_sensor_gps_extended_t gps_extended; crsf_sensor_attitude_t attitude; } crsf_sensors_t; static uint8_t format_sensor(crsf_sensors_t *sensors, uint8_t type, uint8_t *buffer); static void send_packet(crsf_sensors_t *sensors); static uint8_t get_crc(const uint8_t *ptr, uint32_t len); static uint8_t crc8(uint8_t crc, unsigned char a); static void set_config(crsf_sensors_t *sensors); static inline uint sensor_count(crsf_sensors_t *sensors); void crsf_task(void *parameters) { crsf_sensors_t sensors = {0}; set_config(&sensors); context.led_cycle_duration = 6; context.led_cycles = 1; uart0_begin(416666L, UART_RECEIVER_TX, UART_RECEIVER_RX, CRSF_TIMEOUT_US, 8, 1, UART_PARITY_NONE, false, false); debug("\nCRSF init"); while (1) { vTaskDelay(20 / portTICK_PERIOD_MS); send_packet(&sensors); } } static uint8_t format_sensor(crsf_sensors_t *sensors, uint8_t type, uint8_t *buffer) { // Packet format: [sync] [len (from type)] [type] [payload] [crc8 from type] uint len = 0; buffer[0] = 0xC8; switch (type) { case TYPE_GPS: { buffer[1] = sizeof(crsf_sensor_gps_formatted_t) + 2; buffer[2] = CRSF_FRAMETYPE_GPS; crsf_sensor_gps_formatted_t sensor = {0}; if (sensors->gps.latitude) sensor.latitude = swap_32((int32_t)(*sensors->gps.latitude * 10000000L)); if (sensors->gps.longitude) sensor.longitude = swap_32((int32_t)(*sensors->gps.longitude * 10000000L)); if (sensors->gps.groundspeed) sensor.groundspeed = swap_16((uint16_t)(fabs(*sensors->gps.groundspeed * 10))); if (sensors->gps.satellites) sensor.satellites = *sensors->gps.satellites; if (sensors->gps.heading) sensor.heading = swap_16((uint16_t)(*sensors->gps.heading * 100)); if (sensors->gps.altitude) { float altitude = *sensors->gps.altitude + 1000; if (altitude < 0) altitude = 0; if (altitude > 65535) altitude = 65535; sensor.altitude = swap_16((uint16_t)altitude); } memcpy(&buffer[3], &sensor, sizeof(crsf_sensor_gps_formatted_t)); buffer[3 + sizeof(crsf_sensor_gps_formatted_t)] = get_crc(&buffer[2], sizeof(crsf_sensor_gps_formatted_t) + 1); len = sizeof(crsf_sensor_gps_formatted_t) + 4; break; } case TYPE_VARIO: { buffer[1] = sizeof(crsf_sensor_vario_formatted_t) + 2; buffer[2] = CRSF_FRAMETYPE_VARIO; crsf_sensor_vario_formatted_t sensor = {0}; if (sensors->vario.vspeed) sensor.vspeed = swap_16((int16_t)(*sensors->vario.vspeed * 100)); memcpy(&buffer[3], &sensor, sizeof(crsf_sensor_vario_formatted_t)); buffer[3 + sizeof(crsf_sensor_vario_formatted_t)] = get_crc(&buffer[2], sizeof(crsf_sensor_vario_formatted_t) + 1); len = sizeof(crsf_sensor_vario_formatted_t) + 4; break; } case TYPE_BATERY: { buffer[1] = sizeof(crsf_sensor_battery_formatted_t) + 2; buffer[2] = CRSF_FRAMETYPE_BATTERY_SENSOR; crsf_sensor_battery_formatted_t sensor = {0}; if (sensors->battery.voltage) sensor.voltage = swap_16((uint16_t)(*sensors->battery.voltage * 10)); if (sensors->battery.current) sensor.current = swap_16((uint16_t)(*sensors->battery.current * 10)); if (sensors->battery.capacity) sensor.capacity = swap_24((uint32_t)*sensors->battery.capacity); memcpy(&buffer[3], &sensor, sizeof(crsf_sensor_battery_formatted_t)); buffer[3 + sizeof(crsf_sensor_battery_formatted_t)] = get_crc(&buffer[2], sizeof(crsf_sensor_battery_formatted_t) + 1); len = sizeof(crsf_sensor_battery_formatted_t) + 4; break; } case TYPE_BARO: { buffer[1] = sizeof(crsf_sensor_baro_formatted_t) + 2; buffer[2] = CRSF_FRAMETYPE_BARO_ALTITUDE; crsf_sensor_baro_formatted_t sensor = {0}; if (sensors->baro.vspeed) sensor.vspeed = swap_16((int16_t)(*sensors->baro.vspeed * 100)); if (sensors->baro.altitude) { float altitude = *sensors->baro.altitude + 1000; if (altitude < 0) altitude = 0; if (altitude > 3276) altitude = 3276; sensor.altitude = swap_16((uint16_t)(altitude * 10)); } memcpy(&buffer[3], &sensor, sizeof(crsf_sensor_baro_formatted_t)); buffer[3 + sizeof(crsf_sensor_baro_formatted_t)] = get_crc(&buffer[2], sizeof(crsf_sensor_baro_formatted_t) + 1); len = sizeof(crsf_sensor_baro_formatted_t) + 4; break; } case TYPE_AIRSPEED: { buffer[1] = sizeof(crsf_sensor_airspeed_formatted_t) + 2; buffer[2] = CRSF_FRAMETYPE_AIRSPEED; crsf_sensor_airspeed_formatted_t sensor = {0}; if (sensors->airspeed.speed) sensor.speed = swap_16((int16_t)(*sensors->airspeed.speed * 10)); memcpy(&buffer[3], &sensor, sizeof(crsf_sensor_airspeed_formatted_t)); buffer[3 + sizeof(crsf_sensor_airspeed_formatted_t)] = get_crc(&buffer[2], sizeof(crsf_sensor_airspeed_formatted_t) + 1); len = sizeof(crsf_sensor_airspeed_formatted_t) + 4; break; } case TYPE_RPM: { buffer[1] = sizeof(crsf_sensor_rpm_formatted_t) + 2; buffer[2] = CRSF_FRAMETYPE_RPM; crsf_sensor_rpm_formatted_t sensor = {0}; if (sensors->rpm.rpm) sensor.rpm = swap_24((int32_t)(*sensors->rpm.rpm)); memcpy(&buffer[3], &sensor, sizeof(crsf_sensor_rpm_formatted_t)); buffer[3 + sizeof(crsf_sensor_rpm_formatted_t)] = get_crc(&buffer[2], sizeof(crsf_sensor_rpm_formatted_t) + 1); len = sizeof(crsf_sensor_rpm_formatted_t) + 4; break; } case TYPE_TEMP: { static uint8_t count = 0; buffer[1] = sizeof(crsf_sensor_temp_formatted_t) + 2; buffer[2] = CRSF_FRAMETYPE_TEMP; crsf_sensor_temp_formatted_t sensor = {0}; sensor.source = count; if (sensors->temperature.temperature[count]) sensor.temp = swap_16((int16_t)(*sensors->temperature.temperature[count] * 10)); memcpy(&buffer[3], &sensor, sizeof(crsf_sensor_temp_formatted_t)); buffer[3 + sizeof(crsf_sensor_temp_formatted_t)] = get_crc(&buffer[2], sizeof(crsf_sensor_temp_formatted_t) + 1); len = sizeof(crsf_sensor_temp_formatted_t) + 4; count++; if (count >= sensors->temperature.temperature_count) count = 0; break; } case TYPE_VOLTAGES: { static uint8_t cell_index = 0; static uint8_t voltage_index = 0; static bool send_cell = true; if (*sensors->voltages.cell_count == 0) send_cell = false; buffer[2] = CRSF_FRAMETYPE_VOLTAGES; crsf_sensor_voltages_formatted_t sensor = {0}; buffer[1] = sizeof(crsf_sensor_voltages_formatted_t) + 2; if (send_cell && *sensors->voltages.cell_count > 0) { sensor.source = cell_index; if (sensors->voltages.is_cell_average && sensors->voltages.cell[0]) { sensor.voltage = swap_16((uint16_t)(*sensors->voltages.cell[0] * 1000)); } else if (sensors->voltages.cell[cell_index]) { sensor.voltage = swap_16((uint16_t)(*sensors->voltages.cell[cell_index] * 1000)); } memcpy(&buffer[3], &sensor, sizeof(crsf_sensor_voltages_formatted_t)); buffer[3 + sizeof(crsf_sensor_voltages_formatted_t)] = get_crc(&buffer[2], sizeof(crsf_sensor_voltages_formatted_t) + 1); len = sizeof(crsf_sensor_voltages_formatted_t) + 4; buffer[1] = sizeof(crsf_sensor_voltages_formatted_t) + 2; cell_index++; if (cell_index >= *sensors->voltages.cell_count) { cell_index = 0; if (sensors->voltages.voltage_count) send_cell = false; } } else if (sensors->voltages.voltage_count > 0) { sensor.source = voltage_index + 128; sensor.voltage = swap_16((uint16_t)(*sensors->voltages.voltage[voltage_index] * 1000)); memcpy(&buffer[3], &sensor, sizeof(crsf_sensor_voltages_formatted_t)); buffer[3 + sizeof(crsf_sensor_voltages_formatted_t)] = get_crc(&buffer[2], sizeof(crsf_sensor_voltages_formatted_t) + 1); len = sizeof(crsf_sensor_voltages_formatted_t) + 4; buffer[1] = sizeof(crsf_sensor_voltages_formatted_t) + 2; voltage_index++; if (voltage_index >= sensors->voltages.voltage_count) { voltage_index = 0; send_cell = true; if (*sensors->voltages.cell_count) send_cell = true; } } break; } case TYPE_GPS_TIME: { buffer[1] = sizeof(crsf_sensor_gps_time_formatted_t) + 2; buffer[2] = CRSF_FRAMETYPE_GPS_TIME; crsf_sensor_gps_time_formatted_t sensor = {0}; uint16_t year = *sensors->gps_time.date / 10000 + 2000; sensor.year = swap_16(year); sensor.month = ((uint)*sensors->gps_time.date - (year - 2000) * 10000) / 100; sensor.day = (uint)*sensors->gps_time.date - (year - 2000) * 10000 - sensor.month * 100; sensor.hour = (uint)*sensors->gps_time.time / 10000; sensor.minute = ((uint)*sensors->gps_time.time - sensor.hour * 10000) / 100; sensor.second = ((uint)*sensors->gps_time.time - sensor.hour * 10000 - sensor.minute * 100); memcpy(&buffer[3], &sensor, sizeof(crsf_sensor_gps_time_formatted_t)); buffer[3 + sizeof(crsf_sensor_gps_time_formatted_t)] = get_crc(&buffer[2], sizeof(crsf_sensor_gps_time_formatted_t) + 1); len = sizeof(crsf_sensor_gps_time_formatted_t) + 4; break; } case TYPE_GPS_EXTENDED: { buffer[1] = sizeof(crsf_sensor_gps_extended_formatted_t) + 2; buffer[2] = CRSF_FRAMETYPE_GPS_EXTENDED; crsf_sensor_gps_extended_formatted_t sensor = {0}; sensor.fix_type = *sensors->gps_extended.fix; sensor.n_speed = *sensors->gps_extended.n_speed / 10; sensor.e_speed = *sensors->gps_extended.e_speed / 10; sensor.v_speed = *sensors->gps_extended.v_speed / 10; sensor.h_speed_acc = *sensors->gps_extended.h_speed_acc / 10; sensor.track_acc = *sensors->gps_extended.track_acc / 10; sensor.alt_ellipsoid = *sensors->gps_extended.alt_ellipsoid / 10; sensor.hDOP = *sensors->gps_extended.hdop * 10; sensor.vDOP = *sensors->gps_extended.vdop * 10; memcpy(&buffer[3], &sensor, sizeof(crsf_sensor_gps_extended_formatted_t)); buffer[3 + sizeof(crsf_sensor_gps_extended_formatted_t)] = get_crc(&buffer[2], sizeof(crsf_sensor_gps_extended_formatted_t) + 1); len = sizeof(crsf_sensor_gps_extended_formatted_t) + 4; break; } case TYPE_ATTITUDE: { buffer[1] = sizeof(crsf_sensor_attitude_formatted_t) + 2; buffer[2] = CRSF_FRAMETYPE_ATTITUDE; crsf_sensor_attitude_formatted_t sensor = {0}; if (sensors->attitude.pitch) sensor.pitch = swap_16((int16_t)(*sensors->attitude.pitch * 10000)); if (sensors->attitude.roll) sensor.roll = swap_16((int16_t)(*sensors->attitude.roll * 10000)); if (sensors->attitude.yaw) sensor.yaw = swap_16((int16_t)(*sensors->attitude.yaw * 10000)); memcpy(&buffer[3], &sensor, sizeof(crsf_sensor_attitude_formatted_t)); buffer[3 + sizeof(crsf_sensor_attitude_formatted_t)] = get_crc(&buffer[2], sizeof(crsf_sensor_attitude_formatted_t) + 1); len = sizeof(crsf_sensor_attitude_formatted_t) + 4; break; } } return len; } static inline uint sensor_count(crsf_sensors_t *sensors) { uint count = 0; for (uint i = 0; i < MAX_SENSORS; i++) if (sensors->enabled_sensors[i]) count++; return count; } static void send_packet(crsf_sensors_t *sensors) { if (!sensor_count(sensors)) return; static uint type = 0; uint8_t buffer[64] = {0}; while (!sensors->enabled_sensors[type % MAX_SENSORS]) type++; uint len = format_sensor(sensors, type % MAX_SENSORS, buffer); uart0_write_bytes(buffer, len); type++; debug("\nCRSF (%u) > ", uxTaskGetStackHighWaterMark(NULL)); debug_buffer(buffer, len, "0x%X "); // blink led vTaskResume(context.led_task_handle); } static uint8_t get_crc(const uint8_t *ptr, uint32_t len) { uint8_t crc = 0; for (uint32_t i = 0; i < len; i++) { crc = crc8(crc, *ptr++); } return crc; } static uint8_t crc8(uint8_t crc, unsigned char a) { crc ^= a; for (int ii = 0; ii < 8; ++ii) { if (crc & 0x80) { crc = (crc << 1) ^ 0xD5; } else { crc = crc << 1; } } return crc; } static void set_config(crsf_sensors_t *sensors) { config_t *config = config_read(); TaskHandle_t task_handle; float *baro_temp = NULL, *baro_pressure = NULL; if (config->esc_protocol == ESC_PWM) { esc_pwm_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, malloc(sizeof(float))}; xTaskCreate(esc_pwm_task, "esc_pwm_task", STACK_ESC_PWM, (void *)¶meter, 2, &task_handle); sensors->enabled_sensors[TYPE_RPM] = true; sensors->rpm.rpm = parameter.rpm; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_HW3) { esc_hw3_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, malloc(sizeof(float))}; xTaskCreate(esc_hw3_task, "esc_hw3_task", STACK_ESC_HW3, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; sensors->enabled_sensors[TYPE_RPM] = true; sensors->rpm.rpm = parameter.rpm; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_HW4) { esc_hw4_parameters_t parameter = {config->rpm_multiplier, config->enable_pwm_out, config->enable_esc_hw4_init_delay, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, config->esc_hw4_voltage_multiplier, config->esc_hw4_current_multiplier, config->esc_hw4_current_thresold, config->esc_hw4_current_max, config->esc_hw4_is_manual_offset, config->esc_hw4_auto_detect, config->esc_hw4_offset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_hw4_task, "esc_hw4_task", STACK_ESC_HW4, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); if (config->enable_pwm_out) { xTaskCreate(pwm_out_task, "pwm_out", STACK_PWM_OUT, (void *)parameter.rpm, 2, &task_handle); context.pwm_out_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } sensors->enabled_sensors[TYPE_BATERY] = true; sensors->battery.voltage = parameter.voltage; sensors->battery.current = parameter.current; sensors->battery.capacity = parameter.consumption; sensors->enabled_sensors[TYPE_RPM] = true; sensors->rpm.rpm = parameter.rpm; sensors->enabled_sensors[TYPE_TEMP] = true; sensors->temperature.temperature[sensors->temperature.temperature_count] = parameter.temperature_fet; sensors->temperature.temperature_count++; sensors->temperature.temperature[sensors->temperature.temperature_count] = parameter.temperature_bec; sensors->temperature.temperature_count++; sensors->enabled_sensors[TYPE_VOLTAGES] = true; sensors->voltages.is_cell_average = true; sensors->voltages.cell_count = parameter.cell_count; sensors->voltages.cell[0] = parameter.cell_voltage; } if (config->esc_protocol == ESC_HW5) { esc_hw5_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_hw5_task, "esc_hw5_task", STACK_ESC_HW5, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); sensors->enabled_sensors[TYPE_BATERY] = true; sensors->battery.voltage = parameter.voltage; sensors->battery.current = parameter.current; sensors->battery.capacity = parameter.consumption; sensors->enabled_sensors[TYPE_RPM] = true; sensors->rpm.rpm = parameter.rpm; sensors->enabled_sensors[TYPE_TEMP] = true; sensors->temperature.temperature[sensors->temperature.temperature_count] = parameter.temperature_fet; sensors->temperature.temperature_count++; sensors->temperature.temperature[sensors->temperature.temperature_count] = parameter.temperature_bec; sensors->temperature.temperature_count++; sensors->enabled_sensors[TYPE_VOLTAGES] = true; sensors->voltages.is_cell_average = true; sensors->voltages.cell_count = parameter.cell_count; sensors->voltages.cell[0] = parameter.cell_voltage; sensors->voltages.voltage[sensors->voltages.voltage_count] = parameter.voltage_bec; sensors->voltages.voltage_count++; } if (config->esc_protocol == ESC_CASTLE) { esc_castle_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_castle_task, "esc_castle_task", STACK_ESC_CASTLE, (void *)¶meter, 2, &task_handle); sensors->enabled_sensors[TYPE_BATERY] = true; sensors->battery.voltage = parameter.voltage; sensors->battery.current = parameter.current; sensors->battery.capacity = parameter.consumption; sensors->enabled_sensors[TYPE_RPM] = true; sensors->rpm.rpm = parameter.rpm; sensors->enabled_sensors[TYPE_TEMP] = true; sensors->temperature.temperature[sensors->temperature.temperature_count] = parameter.temperature; sensors->temperature.temperature_count++; sensors->enabled_sensors[TYPE_VOLTAGES] = true; sensors->voltages.is_cell_average = true; sensors->voltages.cell_count = parameter.cell_count; sensors->voltages.cell[0] = parameter.cell_voltage; sensors->voltages.voltage[sensors->voltages.voltage_count] = parameter.voltage_bec; sensors->voltages.voltage_count++; sensors->voltages.voltage[sensors->voltages.voltage_count] = parameter.ripple_voltage; sensors->voltages.voltage_count++; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_KONTRONIK) { esc_kontronik_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_kontronik_task, "esc_kontronik_task", STACK_ESC_KONTRONIK, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; sensors->enabled_sensors[TYPE_BATERY] = true; sensors->battery.voltage = parameter.voltage; sensors->battery.current = parameter.current; sensors->battery.capacity = parameter.consumption; sensors->enabled_sensors[TYPE_RPM] = true; sensors->rpm.rpm = parameter.rpm; sensors->enabled_sensors[TYPE_TEMP] = true; sensors->temperature.temperature[sensors->temperature.temperature_count] = parameter.temperature_fet; sensors->temperature.temperature_count++; sensors->temperature.temperature[sensors->temperature.temperature_count] = parameter.temperature_bec; sensors->temperature.temperature_count++; sensors->enabled_sensors[TYPE_VOLTAGES] = true; sensors->voltages.is_cell_average = true; sensors->voltages.cell_count = parameter.cell_count; sensors->voltages.cell[0] = parameter.cell_voltage; sensors->voltages.voltage[sensors->voltages.voltage_count] = parameter.voltage_bec; sensors->voltages.voltage_count++; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_APD_F) { esc_apd_f_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_apd_f_task, "esc_apd_f_task", STACK_ESC_APD_F, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; sensors->enabled_sensors[TYPE_BATERY] = true; sensors->battery.voltage = parameter.voltage; sensors->battery.current = parameter.current; sensors->battery.capacity = parameter.consumption; sensors->enabled_sensors[TYPE_RPM] = true; sensors->rpm.rpm = parameter.rpm; sensors->enabled_sensors[TYPE_TEMP] = true; sensors->temperature.temperature[sensors->temperature.temperature_count] = parameter.temperature; sensors->temperature.temperature_count++; sensors->enabled_sensors[TYPE_VOLTAGES] = true; sensors->voltages.is_cell_average = true; sensors->voltages.cell_count = parameter.cell_count; sensors->voltages.cell[0] = parameter.cell_voltage; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_APD_HV) { esc_apd_hv_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_apd_hv_task, "esc_apd_hv_task", STACK_ESC_APD_HV, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; sensors->enabled_sensors[TYPE_BATERY] = true; sensors->battery.voltage = parameter.voltage; sensors->battery.current = parameter.current; sensors->battery.capacity = parameter.consumption; sensors->enabled_sensors[TYPE_RPM] = true; sensors->rpm.rpm = parameter.rpm; sensors->enabled_sensors[TYPE_TEMP] = true; sensors->temperature.temperature[sensors->temperature.temperature_count] = parameter.temperature; sensors->temperature.temperature_count++; sensors->enabled_sensors[TYPE_VOLTAGES] = true; sensors->voltages.is_cell_average = true; sensors->voltages.cell_count = parameter.cell_count; sensors->voltages.cell[0] = parameter.cell_voltage; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_SMART) { smart_esc_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temperature_fet = malloc(sizeof(float)); parameter.temperature_bec = malloc(sizeof(float)); parameter.voltage_bec = malloc(sizeof(float)); parameter.current_bec = malloc(sizeof(float)); parameter.temperature_bat = malloc(sizeof(float)); parameter.current_bat = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); for (uint i = 0; i < 18; i++) parameter.cell[i] = malloc(sizeof(float)); parameter.cells = malloc(sizeof(uint8_t)); parameter.cycles = malloc(sizeof(uint16_t)); xTaskCreate(smart_esc_task, "smart_esc_task", STACK_SMART_ESC, (void *)¶meter, 4, &task_handle); context.uart1_notify_task_handle = task_handle; sensors->enabled_sensors[TYPE_BATERY] = true; sensors->battery.voltage = parameter.voltage; sensors->battery.current = parameter.current; sensors->battery.capacity = parameter.consumption; sensors->enabled_sensors[TYPE_RPM] = true; sensors->rpm.rpm = parameter.rpm; sensors->enabled_sensors[TYPE_TEMP] = true; sensors->temperature.temperature[sensors->temperature.temperature_count] = parameter.temperature_fet; sensors->temperature.temperature_count++; sensors->temperature.temperature[sensors->temperature.temperature_count] = parameter.temperature_bec; sensors->temperature.temperature_count++; sensors->temperature.temperature[sensors->temperature.temperature_count] = parameter.temperature_bat; sensors->temperature.temperature_count++; sensors->enabled_sensors[TYPE_VOLTAGES] = true; sensors->voltages.is_cell_average = false; sensors->voltages.cell_count = parameter.cells; for (uint i = 0; i < 18; i++) sensors->voltages.cell[i] = parameter.cell[i]; sensors->voltages.voltage[sensors->voltages.voltage_count] = parameter.voltage_bec; sensors->voltages.voltage_count++; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_OMP_M4) { esc_omp_m4_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temp_esc = malloc(sizeof(float)); parameter.temp_motor = malloc(sizeof(float)); parameter.cell_voltage = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); parameter.cell_count = malloc(sizeof(uint8_t)); xTaskCreate(esc_omp_m4_task, "esc_omp_m4_task", STACK_ESC_OMP_M4, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; sensors->enabled_sensors[TYPE_BATERY] = true; sensors->battery.voltage = parameter.voltage; sensors->battery.current = parameter.current; sensors->battery.capacity = parameter.consumption; sensors->enabled_sensors[TYPE_RPM] = true; sensors->rpm.rpm = parameter.rpm; sensors->enabled_sensors[TYPE_TEMP] = true; sensors->temperature.temperature[sensors->temperature.temperature_count] = parameter.temp_esc; sensors->temperature.temperature_count++; sensors->temperature.temperature[sensors->temperature.temperature_count] = parameter.temp_motor; sensors->temperature.temperature_count++; sensors->enabled_sensors[TYPE_VOLTAGES] = true; sensors->voltages.is_cell_average = true; sensors->voltages.cell_count = parameter.cell_count; sensors->voltages.cell[0] = parameter.cell_voltage; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_ZTW) { esc_ztw_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temp_esc = malloc(sizeof(float)); parameter.temp_motor = malloc(sizeof(float)); parameter.bec_voltage = malloc(sizeof(float)); parameter.cell_voltage = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); parameter.cell_count = malloc(sizeof(uint8_t)); xTaskCreate(esc_ztw_task, "esc_ztw_task", STACK_ESC_ZTW, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; sensors->enabled_sensors[TYPE_BATERY] = true; sensors->battery.voltage = parameter.voltage; sensors->battery.current = parameter.current; sensors->battery.capacity = parameter.consumption; sensors->enabled_sensors[TYPE_RPM] = true; sensors->rpm.rpm = parameter.rpm; sensors->enabled_sensors[TYPE_TEMP] = true; sensors->temperature.temperature[sensors->temperature.temperature_count] = parameter.temp_esc; sensors->temperature.temperature_count++; sensors->temperature.temperature[sensors->temperature.temperature_count] = parameter.temp_motor; sensors->temperature.temperature_count++; sensors->enabled_sensors[TYPE_VOLTAGES] = true; sensors->voltages.is_cell_average = true; sensors->voltages.cell_count = parameter.cell_count; sensors->voltages.cell[0] = parameter.cell_voltage; sensors->voltages.voltage[sensors->voltages.voltage_count] = parameter.bec_voltage; sensors->voltages.voltage_count++; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_gps) { gps_parameters_t parameter; parameter.protocol = config->gps_protocol; parameter.baudrate = config->gps_baudrate; parameter.rate = config->gps_rate; parameter.lat = malloc(sizeof(double)); parameter.lon = malloc(sizeof(double)); parameter.alt = malloc(sizeof(float)); parameter.spd = malloc(sizeof(float)); parameter.cog = malloc(sizeof(float)); parameter.hdop = malloc(sizeof(float)); parameter.sat = malloc(sizeof(float)); parameter.time = malloc(sizeof(float)); parameter.date = malloc(sizeof(float)); parameter.vspeed = malloc(sizeof(float)); parameter.dist = malloc(sizeof(float)); parameter.spd_kmh = malloc(sizeof(float)); parameter.fix = malloc(sizeof(float)); parameter.vdop = malloc(sizeof(float)); parameter.speed_acc = malloc(sizeof(float)); parameter.h_acc = malloc(sizeof(float)); parameter.v_acc = malloc(sizeof(float)); parameter.track_acc = malloc(sizeof(float)); parameter.n_vel = malloc(sizeof(float)); parameter.e_vel = malloc(sizeof(float)); parameter.v_vel = malloc(sizeof(float)); parameter.alt_elipsiod = malloc(sizeof(float)); parameter.pdop = malloc(sizeof(float)); parameter.fix_type = malloc(sizeof(uint8_t)); parameter.home_set = malloc(sizeof(uint8_t)); parameter.alt_home = malloc(sizeof(float)); xTaskCreate(gps_task, "gps_task", STACK_GPS, (void *)¶meter, 2, &task_handle); context.uart_pio_notify_task_handle = task_handle; sensors->enabled_sensors[TYPE_GPS] = true; sensors->gps.latitude = parameter.lat; sensors->gps.longitude = parameter.lon; sensors->gps.groundspeed = parameter.spd_kmh; sensors->gps.heading = parameter.cog; sensors->gps.satellites = parameter.sat; sensors->gps.altitude = parameter.alt; /*sensors->enabled_sensors[TYPE_GPS_TIME] = true; sensors->gps_time.date = parameter.date; sensors->gps_time.time = parameter.time; sensors->enabled_sensors[TYPE_GPS_EXTENDED] = true; sensors->gps_extended.hdop = parameter.hdop; sensors->gps_extended.fix = parameter.fix; sensors->gps_extended.vdop = parameter.vdop; sensors->gps_extended.n_speed = parameter.n_vel; sensors->gps_extended.e_speed = parameter.e_vel; sensors->gps_extended.v_speed = parameter.v_vel; sensors->gps_extended.h_speed_acc = parameter.h_acc; sensors->gps_extended.track_acc = parameter.track_acc; sensors->gps_extended.alt_ellipsoid = parameter.alt_elipsiod; sensors->gps_extended.h_acc = parameter.h_acc; sensors->gps_extended.v_acc = parameter.v_acc;*/ ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_voltage) { voltage_parameters_t parameter = {0, config->analog_rate, config->alpha_voltage, config->analog_voltage_multiplier, malloc(sizeof(float))}; xTaskCreate(voltage_task, "voltage_task", STACK_VOLTAGE, (void *)¶meter, 2, &task_handle); sensors->enabled_sensors[TYPE_VOLTAGES] = true; sensors->voltages.voltage[sensors->voltages.voltage_count] = parameter.voltage; sensors->voltages.voltage_count++; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_current) { current_parameters_t parameter = {1, config->analog_rate, config->alpha_current, config->analog_current_multiplier, config->analog_current_offset, config->analog_current_autoffset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(current_task, "current_task", STACK_CURRENT, (void *)¶meter, 2, &task_handle); sensors->enabled_sensors[TYPE_BATERY] = true; sensors->battery.current = parameter.current; sensors->battery.capacity = parameter.consumption; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_ntc) { ntc_parameters_t parameter = {2, config->analog_rate, config->ntc_offset, config->alpha_temperature, malloc(sizeof(float))}; xTaskCreate(ntc_task, "ntc_task", STACK_NTC, (void *)¶meter, 2, &task_handle); sensors->enabled_sensors[TYPE_TEMP] = true; sensors->temperature.temperature[sensors->temperature.temperature_count] = parameter.ntc; sensors->temperature.temperature_count++; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_BMP280) { bmp280_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, 0, config->bmp280_filter, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(bmp280_task, "bmp280_task", STACK_BMP280, (void *)¶meter, 2, &task_handle); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } sensors->enabled_sensors[TYPE_BARO] = true; sensors->baro.altitude = parameter.altitude; sensors->baro.vspeed = parameter.vspeed; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_MS5611) { ms5611_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, 0, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(ms5611_task, "ms5611_task", STACK_MS5611, (void *)¶meter, 2, &task_handle); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } sensors->enabled_sensors[TYPE_BARO] = true; sensors->baro.altitude = parameter.altitude; sensors->baro.vspeed = parameter.vspeed; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_BMP180) { bmp180_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(bmp180_task, "bmp180_task", STACK_BMP180, (void *)¶meter, 2, &task_handle); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } sensors->enabled_sensors[TYPE_BARO] = true; sensors->baro.altitude = parameter.altitude; sensors->baro.vspeed = parameter.vspeed; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_airspeed) { airspeed_parameters_t parameter = {3, config->analog_rate, config->alpha_airspeed, (float)config->airspeed_offset / 1000, (float)config->airspeed_vcc / 100, baro_temp, baro_pressure, malloc(sizeof(float))}; xTaskCreate(airspeed_task, "airspeed_task", STACK_AIRSPEED, (void *)¶meter, 2, &task_handle); sensors->enabled_sensors[TYPE_AIRSPEED] = true; sensors->airspeed.speed = parameter.airspeed; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_gyro) { mpu6050_parameters_t parameter = {1, 0, config->mpu6050_acc_scale, config->mpu6050_gyro_scale, config->mpu6050_gyro_weighting, config->mpu6050_filter, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(mpu6050_task, "mpu6050_task", STACK_MPU6050, (void *)¶meter, 2, &task_handle); sensors->enabled_sensors[TYPE_ATTITUDE] = true; sensors->attitude.pitch = parameter.pitch; sensors->attitude.roll = parameter.roll; sensors->attitude.yaw = parameter.yaw; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_lipo) { float *cell_prev = 0; sensors->voltages.cell_count = malloc(sizeof(uint8_t)); uint8_t c1 = 0, c2 = 0; if (config->lipo_cells > 0) { ina3221_parameters_t parameter = { .i2c_address = 0x40, .filter = config->ina3221_filter, .cell_count = MIN(config->lipo_cells, 3), .cell[0] = malloc(sizeof(float)), .cell[1] = malloc(sizeof(float)), .cell[2] = malloc(sizeof(float)), .cell_prev = malloc(sizeof(float)), }; c1 = parameter.cell_count; *parameter.cell_prev = 0; cell_prev = parameter.cell[2]; xTaskCreate(ina3221_task, "ina3221_task", STACK_INA3221, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); sensors->enabled_sensors[TYPE_VOLTAGES] = true; for (uint i = 0; i < 3; i++) { sensors->voltages.cell[i] = parameter.cell[i]; } } if (config->lipo_cells > 3) { ina3221_parameters_t parameter = { .i2c_address = 0x41, .filter = config->ina3221_filter, .cell_count = MIN(config->lipo_cells - 3, 3), .cell[0] = malloc(sizeof(float)), .cell[1] = malloc(sizeof(float)), .cell[2] = malloc(sizeof(float)), .cell_prev = malloc(sizeof(float)), }; c2 = parameter.cell_count; parameter.cell_prev = cell_prev; cell_prev = parameter.cell[2]; xTaskCreate(ina3221_task, "ina3221_task", STACK_INA3221, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); for (uint i = 0; i < 3; i++) { sensors->voltages.cell[i + 3] = parameter.cell[i]; } } *sensors->voltages.cell_count = c1 + c2; } } ================================================ FILE: board/project/protocol/crsf.h ================================================ #ifndef CRSF_H #define CRSF_H #include "common.h" extern context_t context; void crsf_task(void *parameters); #endif ================================================ FILE: board/project/protocol/fbus.c ================================================ #include "fbus.h" #include #include #include #include #include "airspeed.h" #include "bmp180.h" #include "bmp280.h" #include "config.h" #include "current.h" #include "esc_apd_f.h" #include "esc_apd_hv.h" #include "esc_castle.h" #include "esc_hw3.h" #include "esc_hw4.h" #include "esc_hw5.h" #include "esc_kontronik.h" #include "esc_omp_m4.h" #include "esc_openyge.h" #include "esc_pwm.h" #include "esc_ztw.h" #include "fuel_meter.h" #include "gpio.h" #include "gps.h" #include "ina3221.h" #include "mpu6050.h" #include "ms5611.h" #include "ntc.h" #include "pwm_out.h" #include "smart_esc.h" #include "smartport.h" #include "stdlib.h" #include "uart.h" #include "uart_pio.h" #include "voltage.h" typedef struct fbus_packet_t { uint8_t len; uint8_t sensor_id; uint8_t frame_id; uint16_t data_id; uint32_t value; uint8_t crc; } __attribute__((packed)) fbus_packet_t; static SemaphoreHandle_t semaphore_sensor = NULL; static uint8_t sensor_id; static void process(smartport_parameters_t *parameter); static void sensor_task(void *parameters); static void sensor_void_task(void *parameters); static void sensor_double_task(void *parameters); static void sensor_coordinates_task(void *parameters); static void sensor_datetime_task(void *parameters); static void sensor_cell_task(void *parameters); static void sensor_cell_individual_task(void *parameters); static void sensor_gpio_task(void *parameters); static void send_packet(uint8_t frame_id, uint16_t data_id, uint32_t value); static void set_config(smartport_parameters_t *parameter); void fbus_task(void *parameters) { smartport_parameters_t parameter; context.led_cycle_duration = 6; context.led_cycles = 1; semaphore_sensor = xSemaphoreCreateBinary(); xSemaphoreTake(semaphore_sensor, 0); set_config(¶meter); debug("\nFbus init"); while (1) { ulTaskNotifyTakeIndexed(1, pdTRUE, portMAX_DELAY); process(¶meter); } } static void sensor_task(void *parameters) { smartport_sensor_parameters_t parameter = *(smartport_sensor_parameters_t *)parameters; xTaskNotifyGive(context.receiver_task_handle); while (1) { vTaskDelay(parameter.rate / portTICK_PERIOD_MS); xSemaphoreTake(semaphore_sensor, portMAX_DELAY); int32_t data_formatted = smartport_format(parameter.data_id, *parameter.value); debug("\nFBUS. Sensor (%u) > ", uxTaskGetStackHighWaterMark(NULL)); send_packet(0x10, parameter.data_id, data_formatted); } } static void sensor_gpio_task(void *parameters) { smartport_sensor_gpio_parameters_t parameter = *(smartport_sensor_gpio_parameters_t *)parameters; xTaskNotifyGive(context.receiver_task_handle); uint cont = 0; while (1) { vTaskDelay(parameter.rate / portTICK_PERIOD_MS); xSemaphoreTake(semaphore_sensor, portMAX_DELAY); if (parameter.gpio_mask) { while (!(parameter.gpio_mask & (1 << cont))) { cont++; if (cont == 6) cont = 0; } float value = *parameter.value & (1 << cont) ? 1 : 0; uint16_t data_id = parameter.data_id + 17 + cont; int32_t data_formatted = smartport_format(data_id, value); debug("\nFBUS. Sensor GPIO (%u) > GPIO: %u STATE: %u > ", uxTaskGetStackHighWaterMark(NULL), 17 + cont, (uint)value); send_packet(0x10, data_id, data_formatted); cont++; if (cont == 6) cont = 0; } } } static void sensor_void_task(void *parameters) { while (1) { xSemaphoreTake(semaphore_sensor, portMAX_DELAY); if (context.debug == 2) printf("\nFBUS. Sensor void (%u) > ", uxTaskGetStackHighWaterMark(NULL)); send_packet(0, 0, 0); } } static void sensor_double_task(void *parameters) { smartport_sensor_double_parameters_t parameter = *(smartport_sensor_double_parameters_t *)parameters; xTaskNotifyGive(context.receiver_task_handle); while (1) { vTaskDelay(parameter.rate / portTICK_PERIOD_MS); xSemaphoreTake(semaphore_sensor, portMAX_DELAY); float v_l = parameter.value_l ? *parameter.value_l : 0.0f; float v_h = parameter.value_h ? *parameter.value_h : 0.0f; uint32_t data_formatted = smartport_format_double(parameter.data_id, v_l, v_h); debug("\nFBUS. Sensor double (%u) > ", uxTaskGetStackHighWaterMark(NULL)); send_packet(0x10, parameter.data_id, data_formatted); } } static void sensor_coordinates_task(void *parameters) { smartport_sensor_coordinate_parameters_t parameter = *(smartport_sensor_coordinate_parameters_t *)parameters; xTaskNotifyGive(context.receiver_task_handle); while (1) { vTaskDelay(parameter.rate / portTICK_PERIOD_MS); xSemaphoreTake(semaphore_sensor, portMAX_DELAY); uint32_t data_formatted; if (parameter.type == SMARTPORT_LATITUDE) data_formatted = smartport_format_coordinate(parameter.type, *parameter.latitude); else data_formatted = smartport_format_coordinate(parameter.type, *parameter.longitude); parameter.type = !parameter.type; debug("\nFBUS. Sensor coordinates (%u) > ", uxTaskGetStackHighWaterMark(NULL)); send_packet(0x10, GPS_LONG_LATI_FIRST_ID, data_formatted); } } static void sensor_datetime_task(void *parameters) { smartport_sensor_datetime_parameters_t parameter = *(smartport_sensor_datetime_parameters_t *)parameters; xTaskNotifyGive(context.receiver_task_handle); while (1) { vTaskDelay(parameter.rate / portTICK_PERIOD_MS); xSemaphoreTake(semaphore_sensor, portMAX_DELAY); uint32_t data_formatted; if (parameter.type == SMARTPORT_DATE) data_formatted = smartport_format_datetime(parameter.type, *parameter.date); else data_formatted = smartport_format_datetime(parameter.type, *parameter.time); parameter.type = !parameter.type; debug("\nFBUS. Sensor datetime (%u) > ", uxTaskGetStackHighWaterMark(NULL)); send_packet(0x10, GPS_TIME_DATE_FIRST_ID, data_formatted); } } static void sensor_cell_task(void *parameters) { smartport_sensor_cell_parameters_t parameter = *(smartport_sensor_cell_parameters_t *)parameters; xTaskNotifyGive(context.receiver_task_handle); uint8_t cell_index = 0; while (1) { vTaskDelay(parameter.rate / portTICK_PERIOD_MS); xSemaphoreTake(semaphore_sensor, portMAX_DELAY); if (!*parameter.cell_count) return; uint32_t data_formatted = smartport_format_cell(cell_index, *parameter.cell_voltage); cell_index++; if (cell_index > *parameter.cell_count - 1) cell_index = 0; debug("\nFBUS. Sensor cell (%u) > ", uxTaskGetStackHighWaterMark(NULL)); send_packet(0x10, CELLS_FIRST_ID, data_formatted); } } static void sensor_cell_individual_task(void *parameters) { smartport_sensor_cell_individual_parameters_t parameter = *(smartport_sensor_cell_individual_parameters_t *)parameters; xTaskNotifyGive(context.receiver_task_handle); uint8_t cell_index = 0; while (1) { vTaskDelay(parameter.rate / portTICK_PERIOD_MS); xSemaphoreTake(semaphore_sensor, portMAX_DELAY); // No cells configured → skip if (!parameter.cell_count || *parameter.cell_count == 0) { continue; } float value = 0.0f; // Safety: check index and pointer before dereferencing if (cell_index < *parameter.cell_count && parameter.cell_voltage[cell_index] != NULL) { value = *parameter.cell_voltage[cell_index]; } uint32_t data_formatted = smartport_format_cell(cell_index, value); debug("\nFBUS. Sensor cell (%u) > ", uxTaskGetStackHighWaterMark(NULL)); send_packet(0x10, CELLS_FIRST_ID, data_formatted); // Next cell cell_index++; if (cell_index >= *parameter.cell_count) { cell_index = 0; } } } static void process(smartport_parameters_t *parameter) { uint length = uart0_available(); if (length < 3 || length > 128) return; uint8_t data[128]; uart0_read_bytes(data, MIN(length, 128)); debug("\nFBUS (%u) < ", uxTaskGetStackHighWaterMark(NULL)); debug_buffer(data, length, "0x%X "); uint idx = 0; if (data[0] != 0x08) { // Skip to next frame based on LEN idx = data[0] + 2; // Control frame (0xFF ...), RSSI byte present but not counted in LEN if (data[1] == 0xFF) idx++; } if (idx + 10 != length) return; // Expecting exactly one frame with LEN=8 (plus LEN and CRC, ignoring RSSI if present) if (data[idx] == 0x08 && data[idx + 1] == smartport_sensor_id_to_crc(sensor_id) && data[idx + 2] == 0x10) { xSemaphoreGive(semaphore_sensor); // FBUS requires a small turnaround delay before replying (RTOS tick = 2ms). vTaskDelay(pdMS_TO_TICKS(2)); xSemaphoreTake(semaphore_sensor, 0); } } static void send_packet(uint8_t frame_id, uint16_t data_id, uint32_t value) { fbus_packet_t packet = {0}; packet.len = 0x08; packet.sensor_id = smartport_sensor_id_to_crc(sensor_id); packet.frame_id = 0x10; packet.data_id = data_id; packet.value = value; packet.crc = smartport_get_crc(&packet.len, sizeof(packet) - 1); // CRC over LEN to before CRC uart0_write_bytes((uint8_t *)&packet, sizeof(packet)); debug_buffer((uint8_t *)&packet, sizeof(packet), "0x%X "); vTaskResume(context.led_task_handle); } static void set_config(smartport_parameters_t *parameter) { config_t *config = config_read(); uart0_begin(460800, UART_RECEIVER_TX, UART_RECEIVER_RX, TIMEOUT_US, 8, 1, UART_PARITY_NONE, config->fbus_inverted, true); TaskHandle_t task_handle; float *baro_temp = NULL, *baro_pressure = NULL; parameter->sensor_id = config->smartport_sensor_id; sensor_id = config->smartport_sensor_id; parameter->data_id = 0x5100; if (config->esc_protocol == ESC_PWM) { esc_pwm_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, malloc(sizeof(float))}; xTaskCreate(esc_pwm_task, "esc_pwm_task", STACK_ESC_PWM, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_double_parameters_t parameter_sensor_double; parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = parameter.rpm; parameter_sensor_double.value_h = NULL; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_HW3) { esc_hw3_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, malloc(sizeof(float))}; xTaskCreate(esc_hw3_task, "esc_hw3_task", STACK_ESC_HW3, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_double_parameters_t parameter_sensor_double; parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = parameter.rpm; parameter_sensor_double.value_h = NULL; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_HW4) { esc_hw4_parameters_t parameter = {config->rpm_multiplier, config->enable_pwm_out, config->enable_esc_hw4_init_delay, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, config->esc_hw4_voltage_multiplier, config->esc_hw4_current_multiplier, config->esc_hw4_current_thresold, config->esc_hw4_current_max, config->esc_hw4_is_manual_offset, config->esc_hw4_auto_detect, config->esc_hw4_offset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_hw4_task, "esc_hw4_task", STACK_ESC_HW4, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); if (config->enable_pwm_out) { xTaskCreate(pwm_out_task, "pwm_out", STACK_PWM_OUT, (void *)parameter.rpm, 2, &task_handle); context.pwm_out_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } smartport_sensor_parameters_t parameter_sensor; smartport_sensor_double_parameters_t parameter_sensor_double; smartport_sensor_cell_parameters_t parameter_sensor_cell; parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = parameter.rpm; parameter_sensor_double.value_h = parameter.consumption; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_double.data_id = ESC_POWER_FIRST_ID; parameter_sensor_double.value_l = parameter.voltage; parameter_sensor_double.value_h = parameter.current; parameter_sensor_double.rate = config->refresh_rate_voltage; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID; parameter_sensor.value = parameter.temperature_fet; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID + 1; parameter_sensor.value = parameter.temperature_bec; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_cell.cell_count = parameter.cell_count; parameter_sensor_cell.cell_voltage = parameter.cell_voltage; parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_task, "sensor_cell_task", STACK_SENSOR_SMARTPORT_CELL, (void *)¶meter_sensor_cell, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_HW5) { esc_hw5_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_hw5_task, "esc_hw5_task", STACK_ESC_HW5, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; smartport_sensor_double_parameters_t parameter_sensor_double; smartport_sensor_cell_parameters_t parameter_sensor_cell; parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = parameter.rpm; parameter_sensor_double.value_h = parameter.consumption; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_double.data_id = ESC_POWER_FIRST_ID; parameter_sensor_double.value_l = parameter.voltage; parameter_sensor_double.value_h = parameter.current; parameter_sensor_double.rate = config->refresh_rate_voltage; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID; parameter_sensor.value = parameter.temperature_fet; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID + 1; parameter_sensor.value = parameter.temperature_bec; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID + 2; parameter_sensor.value = parameter.temperature_motor; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_double.data_id = ESC_POWER_FIRST_ID + 1; parameter_sensor_double.value_l = parameter.voltage_bec; parameter_sensor_double.value_h = parameter.current_bec; parameter_sensor_double.rate = config->refresh_rate_voltage; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_cell.cell_count = parameter.cell_count; parameter_sensor_cell.cell_voltage = parameter.cell_voltage; parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_task, "sensor_cell_task", STACK_SENSOR_SMARTPORT_CELL, (void *)¶meter_sensor_cell, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_CASTLE) { esc_castle_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_castle_task, "esc_castle_task", STACK_ESC_CASTLE, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; smartport_sensor_double_parameters_t parameter_sensor_double; smartport_sensor_cell_parameters_t parameter_sensor_cell; parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = parameter.rpm; parameter_sensor_double.value_h = parameter.consumption; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_double.data_id = ESC_POWER_FIRST_ID; parameter_sensor_double.value_l = parameter.voltage; parameter_sensor_double.value_h = parameter.current; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_double.data_id = ESC_POWER_FIRST_ID + 1; parameter_sensor_double.value_l = parameter.voltage_bec; parameter_sensor_double.value_h = parameter.current_bec; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID; parameter_sensor.value = parameter.temperature; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_cell.cell_count = parameter.cell_count; parameter_sensor_cell.cell_voltage = parameter.cell_voltage; parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_task, "sensor_cell_task", STACK_SENSOR_SMARTPORT_CELL, (void *)¶meter_sensor_cell, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_KONTRONIK) { esc_kontronik_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_kontronik_task, "esc_kontronik_task", STACK_ESC_KONTRONIK, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; smartport_sensor_double_parameters_t parameter_sensor_double; smartport_sensor_cell_parameters_t parameter_sensor_cell; parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = parameter.rpm; parameter_sensor_double.value_h = parameter.consumption; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_double.data_id = ESC_POWER_FIRST_ID; parameter_sensor_double.value_l = parameter.voltage; parameter_sensor_double.value_h = parameter.current; parameter_sensor_double.rate = config->refresh_rate_voltage; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID; parameter_sensor.value = parameter.temperature_fet; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID + 1; parameter_sensor.value = parameter.temperature_bec; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_cell.cell_count = parameter.cell_count; parameter_sensor_cell.cell_voltage = parameter.cell_voltage; parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_task, "sensor_cell_task", STACK_SENSOR_SMARTPORT_CELL, (void *)¶meter_sensor_cell, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_APD_F) { esc_apd_f_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_apd_f_task, "esc_apd_f_task", STACK_ESC_APD_F, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; smartport_sensor_double_parameters_t parameter_sensor_double; smartport_sensor_cell_parameters_t parameter_sensor_cell; parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = parameter.rpm; parameter_sensor_double.value_h = parameter.consumption; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_double.data_id = ESC_POWER_FIRST_ID; parameter_sensor_double.value_l = parameter.voltage; parameter_sensor_double.value_h = parameter.current; parameter_sensor_double.rate = config->refresh_rate_voltage; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID; parameter_sensor.value = parameter.temperature; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_cell.cell_count = parameter.cell_count; parameter_sensor_cell.cell_voltage = parameter.cell_voltage; parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_task, "sensor_cell_task", STACK_SENSOR_SMARTPORT_CELL, (void *)¶meter_sensor_cell, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_APD_HV) { esc_apd_hv_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_apd_hv_task, "esc_apd_hv_task", STACK_ESC_APD_HV, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; smartport_sensor_double_parameters_t parameter_sensor_double; smartport_sensor_cell_parameters_t parameter_sensor_cell; parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = parameter.rpm; parameter_sensor_double.value_h = parameter.consumption; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_double.data_id = ESC_POWER_FIRST_ID; parameter_sensor_double.value_l = parameter.voltage; parameter_sensor_double.value_h = parameter.current; parameter_sensor_double.rate = config->refresh_rate_voltage; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID; parameter_sensor.value = parameter.temperature; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_cell.cell_count = parameter.cell_count; parameter_sensor_cell.cell_voltage = parameter.cell_voltage; parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_task, "sensor_cell_task", STACK_SENSOR_SMARTPORT_CELL, (void *)¶meter_sensor_cell, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_SMART) { smart_esc_parameters_t parameter; parameter.calc_consumption = config->smart_esc_calc_consumption; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temperature_fet = malloc(sizeof(float)); parameter.temperature_bec = malloc(sizeof(float)); parameter.voltage_bec = malloc(sizeof(float)); parameter.current_bec = malloc(sizeof(float)); parameter.temperature_bat = malloc(sizeof(float)); parameter.current_bat = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); for (uint i = 0; i < 18; i++) parameter.cell[i] = malloc(sizeof(float)); parameter.cells = malloc(sizeof(uint8_t)); parameter.cycles = malloc(sizeof(uint16_t)); xTaskCreate(smart_esc_task, "smart_esc_task", STACK_SMART_ESC, (void *)¶meter, 4, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; smartport_sensor_double_parameters_t parameter_sensor_double; smartport_sensor_cell_individual_parameters_t parameter_sensor_cell; // rpm & consumption parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = parameter.rpm; parameter_sensor_double.value_h = parameter.consumption; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // voltage & current parameter_sensor_double.data_id = ESC_POWER_FIRST_ID; parameter_sensor_double.value_l = parameter.voltage; parameter_sensor_double.value_h = parameter.current; parameter_sensor_double.rate = config->refresh_rate_voltage; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // bec. voltage & current parameter_sensor_double.data_id = SBEC_POWER_FIRST_ID; parameter_sensor_double.value_l = parameter.voltage_bec; parameter_sensor_double.value_h = parameter.current_bec; parameter_sensor_double.rate = config->refresh_rate_voltage; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // temp_fet parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID; parameter_sensor.value = parameter.temperature_fet; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // temp_bec parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID + 1; parameter_sensor.value = parameter.temperature_bec; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // temp_bat parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID + 2; parameter_sensor.value = parameter.temperature_bat; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // current_bat parameter_sensor.data_id = CURR_FIRST_ID + 1; parameter_sensor.value = parameter.current_bat; parameter_sensor.rate = config->refresh_rate_current; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // cells parameter_sensor_cell.cell_count = parameter.cells; // Pointer provided by ESC_SMART task for (uint i = 0; i < 18; i++) { parameter_sensor_cell.cell_voltage[i] = parameter.cell[i]; // One pointer per cell } parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_individual_task, "sensor_cell_task", STACK_SENSOR_SMARTPORT_CELL, (void *)¶meter_sensor_cell, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // cycles /*parameter_sensor.data_id = DIY_FIRST_ID + 100; parameter_sensor.value = parameter.cycles; parameter_sensor.rate = config->refresh_rate_default; xTaskCreate(sensor_cell_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY);*/ } if (config->esc_protocol == ESC_OMP_M4) { esc_omp_m4_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temp_esc = malloc(sizeof(float)); parameter.temp_motor = malloc(sizeof(float)); parameter.cell_voltage = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); parameter.cell_count = malloc(sizeof(uint8_t)); xTaskCreate(esc_omp_m4_task, "esc_omp_m4_task", STACK_ESC_OMP_M4, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; smartport_sensor_double_parameters_t parameter_sensor_double; smartport_sensor_cell_parameters_t parameter_sensor_cell; parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = parameter.rpm; parameter_sensor_double.value_h = parameter.consumption; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_double.data_id = ESC_POWER_FIRST_ID; parameter_sensor_double.value_l = parameter.voltage; parameter_sensor_double.value_h = parameter.current; parameter_sensor_double.rate = config->refresh_rate_voltage; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID; parameter_sensor.value = parameter.temp_esc; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID + 2; parameter_sensor.value = parameter.temp_motor; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_cell.cell_count = parameter.cell_count; parameter_sensor_cell.cell_voltage = parameter.cell_voltage; parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_task, "sensor_cell_task", STACK_SENSOR_SMARTPORT_CELL, (void *)¶meter_sensor_cell, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_ZTW) { esc_ztw_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temp_esc = malloc(sizeof(float)); parameter.temp_motor = malloc(sizeof(float)); parameter.bec_voltage = malloc(sizeof(float)); parameter.cell_voltage = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); parameter.cell_count = malloc(sizeof(uint8_t)); xTaskCreate(esc_ztw_task, "esc_ztw_task", STACK_ESC_ZTW, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; smartport_sensor_double_parameters_t parameter_sensor_double; smartport_sensor_cell_parameters_t parameter_sensor_cell; parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = parameter.rpm; parameter_sensor_double.value_h = parameter.consumption; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_double.data_id = ESC_POWER_FIRST_ID; parameter_sensor_double.value_l = parameter.voltage; parameter_sensor_double.value_h = parameter.current; parameter_sensor_double.rate = config->refresh_rate_voltage; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID; parameter_sensor.value = parameter.temp_esc; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID + 2; parameter_sensor.value = parameter.temp_motor; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_cell.cell_count = parameter.cell_count; parameter_sensor_cell.cell_voltage = parameter.cell_voltage; parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_task, "sensor_cell_task", STACK_SENSOR_SMARTPORT_CELL, (void *)¶meter_sensor_cell, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_OPENYGE) { esc_openyge_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.pwm_out = config->enable_pwm_out; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temperature_fet = malloc(sizeof(float)); parameter.temperature_bec = malloc(sizeof(float)); parameter.cell_voltage = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); parameter.voltage_bec = malloc(sizeof(float)); parameter.current_bec = malloc(sizeof(float)); parameter.throttle = malloc(sizeof(float)); parameter.pwm_percent = malloc(sizeof(float)); parameter.cell_count = malloc(sizeof(uint8_t)); xTaskCreate(esc_openyge_task, "esc_openyge_task", STACK_ESC_OPENYGE, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; smartport_sensor_double_parameters_t parameter_sensor_double; smartport_sensor_cell_parameters_t parameter_sensor_cell; // RPM and Consumption sensor parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = parameter.rpm; parameter_sensor_double.value_h = parameter.consumption; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // Main voltage and current sensor parameter_sensor_double.data_id = ESC_POWER_FIRST_ID; parameter_sensor_double.value_l = parameter.voltage; parameter_sensor_double.value_h = parameter.current; parameter_sensor_double.rate = config->refresh_rate_voltage; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // FET Temperature sensor parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID; parameter_sensor.value = parameter.temperature_fet; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // BEC Temperature sensor parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID + 2; parameter_sensor.value = parameter.temperature_bec; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // BEC voltage and current sensor parameter_sensor_double.data_id = ESC_POWER_FIRST_ID + 2; parameter_sensor_double.value_l = parameter.voltage_bec; parameter_sensor_double.value_h = parameter.current_bec; parameter_sensor_double.rate = config->refresh_rate_voltage; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // Cell voltage sensor parameter_sensor_cell.cell_count = parameter.cell_count; parameter_sensor_cell.cell_voltage = parameter.cell_voltage; parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_task, "sensor_cell_task", STACK_SENSOR_SMARTPORT_CELL, (void *)¶meter_sensor_cell, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_gps) { gps_parameters_t parameter; parameter.protocol = config->gps_protocol; parameter.baudrate = config->gps_baudrate; parameter.rate = config->gps_rate; parameter.lat = malloc(sizeof(double)); parameter.lon = malloc(sizeof(double)); parameter.alt = malloc(sizeof(float)); parameter.spd = malloc(sizeof(float)); parameter.cog = malloc(sizeof(float)); parameter.hdop = malloc(sizeof(float)); parameter.sat = malloc(sizeof(float)); parameter.time = malloc(sizeof(float)); parameter.date = malloc(sizeof(float)); parameter.vspeed = malloc(sizeof(float)); parameter.dist = malloc(sizeof(float)); parameter.spd_kmh = malloc(sizeof(float)); parameter.fix = malloc(sizeof(float)); parameter.vdop = malloc(sizeof(float)); parameter.speed_acc = malloc(sizeof(float)); parameter.h_acc = malloc(sizeof(float)); parameter.v_acc = malloc(sizeof(float)); parameter.track_acc = malloc(sizeof(float)); parameter.n_vel = malloc(sizeof(float)); parameter.e_vel = malloc(sizeof(float)); parameter.v_vel = malloc(sizeof(float)); parameter.alt_elipsiod = malloc(sizeof(float)); parameter.pdop = malloc(sizeof(float)); xTaskCreate(gps_task, "gps_task", STACK_GPS, (void *)¶meter, 2, &task_handle); context.uart_pio_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_coordinate_parameters_t parameter_sensor_coordinate; parameter_sensor_coordinate.type = SMARTPORT_LATITUDE; parameter_sensor_coordinate.latitude = parameter.lat; parameter_sensor_coordinate.longitude = parameter.lon; parameter_sensor_coordinate.rate = config->refresh_rate_gps; xTaskCreate(sensor_coordinates_task, "sensor_coordinates_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor_coordinate, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_datetime_parameters_t parameter_sensor_datetime; parameter_sensor_datetime.type = SMARTPORT_DATE; parameter_sensor_datetime.date = parameter.date; parameter_sensor_datetime.time = parameter.time; parameter_sensor_datetime.rate = 1000; xTaskCreate(sensor_datetime_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor_datetime, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; parameter_sensor.data_id = GPS_ALT_FIRST_ID; parameter_sensor.value = parameter.alt; parameter_sensor.rate = config->refresh_rate_gps; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = GPS_SPEED_FIRST_ID; parameter_sensor.value = parameter.spd; parameter_sensor.rate = config->refresh_rate_gps; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = GPS_COURS_FIRST_ID; parameter_sensor.value = parameter.cog; parameter_sensor.rate = config->refresh_rate_gps; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = VARIO_FIRST_ID + 1; parameter_sensor.value = parameter.vspeed; parameter_sensor.rate = config->refresh_rate_gps; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = DIY_FIRST_ID + 3; parameter_sensor.value = parameter.sat; parameter_sensor.rate = config->refresh_rate_gps; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = DIST_FIRST_ID; parameter_sensor.value = parameter.dist; parameter_sensor.rate = config->refresh_rate_gps; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = DIY_FIRST_ID + 5; parameter_sensor.value = parameter.pdop; parameter_sensor.rate = config->refresh_rate_gps; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_voltage) { voltage_parameters_t parameter = {0, config->analog_rate, config->alpha_voltage, config->analog_voltage_multiplier, malloc(sizeof(float))}; xTaskCreate(voltage_task, "voltage_task", STACK_VOLTAGE, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; parameter_sensor.data_id = A3_FIRST_ID; parameter_sensor.value = parameter.voltage; parameter_sensor.rate = config->refresh_rate_voltage; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_current) { current_parameters_t parameter = {1, config->analog_rate, config->alpha_current, config->analog_current_multiplier, config->analog_current_offset, config->analog_current_autoffset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(current_task, "current_task", STACK_CURRENT, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; parameter_sensor.data_id = CURR_FIRST_ID; parameter_sensor.value = parameter.current; parameter_sensor.rate = config->refresh_rate_current; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_double_parameters_t parameter_sensor_double; parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = NULL; parameter_sensor_double.value_h = parameter.consumption; parameter_sensor_double.rate = config->refresh_rate_current; xTaskCreate(sensor_double_task, "sensor_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_BMP280) { bmp280_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, 0, config->bmp280_filter, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(bmp280_task, "bmp280_task", STACK_BMP280, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } smartport_sensor_parameters_t parameter_sensor; parameter_sensor.data_id = ALT_FIRST_ID; parameter_sensor.value = parameter.altitude; parameter_sensor.rate = config->refresh_rate_vario; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = VARIO_FIRST_ID; parameter_sensor.value = parameter.vspeed; parameter_sensor.rate = config->refresh_rate_vario; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_MS5611) { ms5611_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, 0, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(ms5611_task, "ms5611_task", STACK_MS5611, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } smartport_sensor_parameters_t parameter_sensor; parameter_sensor.data_id = ALT_FIRST_ID; parameter_sensor.value = parameter.altitude; parameter_sensor.rate = config->refresh_rate_vario; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = VARIO_FIRST_ID; parameter_sensor.value = parameter.vspeed; parameter_sensor.rate = config->refresh_rate_vario; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_BMP180) { bmp180_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(bmp180_task, "bmp180_task", STACK_BMP180, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } smartport_sensor_parameters_t parameter_sensor; parameter_sensor.data_id = ALT_FIRST_ID; parameter_sensor.value = parameter.altitude; parameter_sensor.rate = config->refresh_rate_vario; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = VARIO_FIRST_ID; parameter_sensor.value = parameter.vspeed; parameter_sensor.rate = config->refresh_rate_vario; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_ntc) { ntc_parameters_t parameter = {2, config->analog_rate, config->ntc_offset, config->alpha_temperature, malloc(sizeof(float))}; xTaskCreate(ntc_task, "ntc_task", STACK_NTC, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID; parameter_sensor.value = parameter.ntc; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_airspeed) { airspeed_parameters_t parameter = {3, config->analog_rate, config->alpha_airspeed, (float)config->airspeed_offset / 1000, (float)config->airspeed_vcc / 100, baro_temp, baro_pressure, malloc(sizeof(float))}; xTaskCreate(airspeed_task, "airspeed_task", STACK_AIRSPEED, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; parameter_sensor.data_id = AIR_SPEED_FIRST_ID; parameter_sensor.value = parameter.airspeed; parameter_sensor.rate = config->refresh_rate_airspeed; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_fuel_flow) { fuel_meter_parameters_t parameter = {config->fuel_flow_ml_per_pulse, malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(fuel_meter_task, "fuel_meter_task", STACK_FUEL_METER, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; parameter_sensor.data_id = GASSUIT_FLOW_FIRST_ID; parameter_sensor.value = parameter.consumption_instant; parameter_sensor.rate = config->refresh_rate_default; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = GASSUIT_RES_VOL_FIRST_ID; parameter_sensor.value = parameter.consumption_total; parameter_sensor.rate = config->refresh_rate_default; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->gpio_mask) { gpio_parameters_t parameter = {config->gpio_mask, config->gpio_interval, malloc(sizeof(float))}; xTaskCreate(gpio_task, "gpio_task", STACK_GPIO, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_gpio_parameters_t parameter_sensor; parameter_sensor.data_id = DIY_FIRST_ID; parameter_sensor.value = parameter.value; parameter_sensor.rate = config->gpio_interval; parameter_sensor.gpio_mask = config->gpio_mask; xTaskCreate(sensor_gpio_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_gyro) { mpu6050_parameters_t parameter = {1, 0, config->mpu6050_acc_scale, config->mpu6050_gyro_scale, config->mpu6050_gyro_weighting, config->mpu6050_filter, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(mpu6050_task, "mpu6050_task", STACK_MPU6050, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; parameter_sensor.data_id = DIY_FIRST_ID + 6; parameter_sensor.value = parameter.pitch; parameter_sensor.rate = config->refresh_rate_default; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = DIY_FIRST_ID + 7; parameter_sensor.value = parameter.roll; parameter_sensor.rate = config->refresh_rate_default; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = DIY_FIRST_ID + 8; parameter_sensor.value = parameter.yaw; parameter_sensor.rate = config->refresh_rate_default; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ACCX_FIRST_ID; parameter_sensor.value = parameter.acc_x; parameter_sensor.rate = config->refresh_rate_default; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ACCY_FIRST_ID; parameter_sensor.value = parameter.acc_y; parameter_sensor.rate = config->refresh_rate_default; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ACCZ_FIRST_ID; parameter_sensor.value = parameter.acc_z; parameter_sensor.rate = config->refresh_rate_default; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = DIY_FIRST_ID + 9; parameter_sensor.value = parameter.acc; parameter_sensor.rate = config->refresh_rate_default; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_lipo && config->lipo_cells > 0) { smartport_sensor_cell_individual_parameters_t parameter_sensor_cell; float *cell_prev = NULL; // Maximum supported cells: 6 (two INA3221 devices) uint8_t lipo_cells = MIN(config->lipo_cells, 6); // Initialize all cell pointers to NULL (safety) for (uint i = 0; i < 18; i++) { parameter_sensor_cell.cell_voltage[i] = NULL; } // Configure task parameters parameter_sensor_cell.rate = config->refresh_rate_voltage; parameter_sensor_cell.cell_count = malloc(sizeof(uint8_t)); *parameter_sensor_cell.cell_count = lipo_cells; // --- First INA3221: cells 0–2 ----------------------------------------- uint8_t cells_first = MIN(lipo_cells, 3); if (cells_first > 0) { ina3221_parameters_t parameter = { .i2c_address = 0x40, .filter = config->ina3221_filter, .cell_count = cells_first, .cell[0] = malloc(sizeof(float)), .cell[1] = malloc(sizeof(float)), .cell[2] = malloc(sizeof(float)), .cell_prev = malloc(sizeof(float)), }; // First INA has no previous cell reference *parameter.cell_prev = 0; cell_prev = parameter.cell[2]; xTaskCreate(ina3221_task, "ina3221_task", STACK_INA3221, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // Store cell pointers for SmartPort for (uint i = 0; i < cells_first; i++) { parameter_sensor_cell.cell_voltage[i] = parameter.cell[i]; } } // --- Second INA3221: cells 3–5 ---------------------------------------- if (lipo_cells > 3) { uint8_t cells_second = MIN((uint8_t)(lipo_cells - 3), (uint8_t)3); ina3221_parameters_t parameter = { .i2c_address = 0x41, .filter = config->ina3221_filter, .cell_count = cells_second, .cell[0] = malloc(sizeof(float)), .cell[1] = malloc(sizeof(float)), .cell[2] = malloc(sizeof(float)), .cell_prev = cell_prev, // Link to the last cell of the previous INA }; xTaskCreate(ina3221_task, "ina3221_task", STACK_INA3221, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // Store cell pointers for SmartPort for (uint i = 0; i < cells_second; i++) { parameter_sensor_cell.cell_voltage[i + 3] = parameter.cell[i]; } } // --- Start SmartPort task: cycle through individual cells ------------- xTaskCreate(sensor_cell_individual_task, "sensor_cell_task", STACK_SENSOR_SMARTPORT_CELL, (void *)¶meter_sensor_cell, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } } ================================================ FILE: board/project/protocol/fbus.h ================================================ #ifndef FBUS_H #define FBUS_H #include "common.h" extern context_t context; void fbus_task(void *parameters); #endif ================================================ FILE: board/project/protocol/fport.c ================================================ #include "fport.h" #include #include #include #include #include "airspeed.h" #include "bmp180.h" #include "bmp280.h" #include "config.h" #include "current.h" #include "esc_apd_f.h" #include "esc_apd_hv.h" #include "esc_castle.h" #include "esc_hw3.h" #include "esc_hw4.h" #include "esc_hw5.h" #include "esc_kontronik.h" #include "esc_omp_m4.h" #include "esc_openyge.h" #include "esc_pwm.h" #include "esc_ztw.h" #include "fuel_meter.h" #include "gpio.h" #include "gps.h" #include "ina3221.h" #include "mpu6050.h" #include "ms5611.h" #include "ntc.h" #include "pwm_out.h" #include "smart_esc.h" #include "smartport.h" #include "stdlib.h" #include "uart.h" #include "uart_pio.h" #include "voltage.h" #define FPORT_DELIM 0x7E #define FPORT_ESC 0x7D #define FPORT_XOR 0x20 #define FPORT_FRAME_MAX 96 static SemaphoreHandle_t semaphore_sensor = NULL; static void process(smartport_parameters_t *parameter); static void sensor_task(void *parameters); static void sensor_void_task(void *parameters); static void sensor_double_task(void *parameters); static void sensor_coordinates_task(void *parameters); static void sensor_datetime_task(void *parameters); static void sensor_cell_task(void *parameters); static void sensor_cell_individual_task(void *parameters); static void sensor_gpio_task(void *parameters); static void send_packet(uint8_t frame_id, uint16_t data_id, uint32_t value); static void set_config(smartport_parameters_t *parameter); void fport_task(void *parameters) { smartport_parameters_t parameter; context.led_cycle_duration = 6; context.led_cycles = 1; semaphore_sensor = xSemaphoreCreateBinary(); xSemaphoreTake(semaphore_sensor, 0); set_config(¶meter); debug("\nFport init"); while (1) { ulTaskNotifyTakeIndexed(1, pdTRUE, portMAX_DELAY); process(¶meter); } } static void sensor_task(void *parameters) { smartport_sensor_parameters_t parameter = *(smartport_sensor_parameters_t *)parameters; xTaskNotifyGive(context.receiver_task_handle); while (1) { vTaskDelay(parameter.rate / portTICK_PERIOD_MS); xSemaphoreTake(semaphore_sensor, portMAX_DELAY); int32_t data_formatted = smartport_format(parameter.data_id, *parameter.value); debug("\nFPort. Sensor (%u) > ", uxTaskGetStackHighWaterMark(NULL)); send_packet(0x10, parameter.data_id, data_formatted); } } static void sensor_gpio_task(void *parameters) { smartport_sensor_gpio_parameters_t parameter = *(smartport_sensor_gpio_parameters_t *)parameters; xTaskNotifyGive(context.receiver_task_handle); uint cont = 0; while (1) { vTaskDelay(parameter.rate / portTICK_PERIOD_MS); xSemaphoreTake(semaphore_sensor, portMAX_DELAY); if (parameter.gpio_mask) { while (!(parameter.gpio_mask & (1 << cont))) { cont++; if (cont == 6) cont = 0; } float value = *parameter.value & (1 << cont) ? 1 : 0; uint16_t data_id = parameter.data_id + 17 + cont; int32_t data_formatted = smartport_format(data_id, value); debug("\nFPort. Sensor GPIO (%u) > GPIO: %u STATE: %u > ", uxTaskGetStackHighWaterMark(NULL), 17 + cont, (uint)value); send_packet(0x10, data_id, data_formatted); cont++; if (cont == 6) cont = 0; } } } static void sensor_void_task(void *parameters) { while (1) { xSemaphoreTake(semaphore_sensor, portMAX_DELAY); if (context.debug == 2) printf("\nFPort. Sensor void (%u) > ", uxTaskGetStackHighWaterMark(NULL)); send_packet(0, 0, 0); } } static void sensor_double_task(void *parameters) { smartport_sensor_double_parameters_t parameter = *(smartport_sensor_double_parameters_t *)parameters; xTaskNotifyGive(context.receiver_task_handle); while (1) { vTaskDelay(parameter.rate / portTICK_PERIOD_MS); xSemaphoreTake(semaphore_sensor, portMAX_DELAY); float v_l = parameter.value_l ? *parameter.value_l : 0.0f; float v_h = parameter.value_h ? *parameter.value_h : 0.0f; uint32_t data_formatted = smartport_format_double(parameter.data_id, v_l, v_h); debug("\nFPort. Sensor double (%u) > ", uxTaskGetStackHighWaterMark(NULL)); send_packet(0x10, parameter.data_id, data_formatted); } } static void sensor_coordinates_task(void *parameters) { smartport_sensor_coordinate_parameters_t parameter = *(smartport_sensor_coordinate_parameters_t *)parameters; xTaskNotifyGive(context.receiver_task_handle); while (1) { vTaskDelay(parameter.rate / portTICK_PERIOD_MS); xSemaphoreTake(semaphore_sensor, portMAX_DELAY); uint32_t data_formatted; if (parameter.type == SMARTPORT_LATITUDE) data_formatted = smartport_format_coordinate(parameter.type, *parameter.latitude); else data_formatted = smartport_format_coordinate(parameter.type, *parameter.longitude); parameter.type = !parameter.type; debug("\nFPort. Sensor coordinates (%u) > ", uxTaskGetStackHighWaterMark(NULL)); send_packet(0x10, GPS_LONG_LATI_FIRST_ID, data_formatted); } } static void sensor_datetime_task(void *parameters) { smartport_sensor_datetime_parameters_t parameter = *(smartport_sensor_datetime_parameters_t *)parameters; xTaskNotifyGive(context.receiver_task_handle); while (1) { vTaskDelay(parameter.rate / portTICK_PERIOD_MS); xSemaphoreTake(semaphore_sensor, portMAX_DELAY); uint32_t data_formatted; if (parameter.type == SMARTPORT_DATE) data_formatted = smartport_format_datetime(parameter.type, *parameter.date); else data_formatted = smartport_format_datetime(parameter.type, *parameter.time); parameter.type = !parameter.type; debug("\nFPort. Sensor datetime (%u) > ", uxTaskGetStackHighWaterMark(NULL)); send_packet(0x10, GPS_TIME_DATE_FIRST_ID, data_formatted); } } static void sensor_cell_task(void *parameters) { smartport_sensor_cell_parameters_t parameter = *(smartport_sensor_cell_parameters_t *)parameters; xTaskNotifyGive(context.receiver_task_handle); uint8_t cell_index = 0; while (1) { vTaskDelay(parameter.rate / portTICK_PERIOD_MS); xSemaphoreTake(semaphore_sensor, portMAX_DELAY); if (!*parameter.cell_count) return; uint32_t data_formatted = smartport_format_cell(cell_index, *parameter.cell_voltage); cell_index++; if (cell_index > *parameter.cell_count - 1) cell_index = 0; debug("\nFPort. Sensor cell (%u) > ", uxTaskGetStackHighWaterMark(NULL)); send_packet(0x10, CELLS_FIRST_ID, data_formatted); } } static void sensor_cell_individual_task(void *parameters) { smartport_sensor_cell_individual_parameters_t parameter = *(smartport_sensor_cell_individual_parameters_t *)parameters; xTaskNotifyGive(context.receiver_task_handle); uint8_t cell_index = 0; while (1) { vTaskDelay(parameter.rate / portTICK_PERIOD_MS); xSemaphoreTake(semaphore_sensor, portMAX_DELAY); // No cells configured → skip if (!parameter.cell_count || *parameter.cell_count == 0) { continue; } float value = 0.0f; // Safety: check index and pointer before dereferencing if (cell_index < *parameter.cell_count && parameter.cell_voltage[cell_index] != NULL) { value = *parameter.cell_voltage[cell_index]; } uint32_t data_formatted = smartport_format_cell(cell_index, value); debug("\nFPort. Sensor cell (%u) > ", uxTaskGetStackHighWaterMark(NULL)); send_packet(0x10, CELLS_FIRST_ID, data_formatted); // Next cell cell_index++; if (cell_index >= *parameter.cell_count) { cell_index = 0; } } } static void process(smartport_parameters_t *parameter) { uint length = uart0_available(); if (length < 3 || length > 128) return; uint8_t data[128]; uart0_read_bytes(data, MIN(length, 128)); // Byte stuffing in-place uint8_t delta = 0; uint i; for (i = 0; i < length; i++) { data[i] = data[i + delta]; if (data[i] == 0x7D) { delta++; data[i] = data[i + delta] ^ 0x20; } } debug("\nFPORT (%u) < ", uxTaskGetStackHighWaterMark(NULL)); debug_buffer(data, length - delta, "0x%X "); uint idx = 0; if (data[1] != 0x08) { // Skip to next frame based on LEN idx = data[1] + 5; } if (idx + 11 != length - delta) return; // Expecting exactly one frame with LEN=8 (plus LEN and CRC, ignoring RSSI if present) if (data[idx] == 0x08 && data[idx + 2] == 0x10) { xSemaphoreGive(semaphore_sensor); // FBUS requires a small turnaround delay before replying (RTOS tick = 2ms). vTaskDelay(pdMS_TO_TICKS(2)); xSemaphoreTake(semaphore_sensor, 0); } } static void send_packet(uint8_t frame_id, uint16_t data_id, uint32_t value) { uint16_t crc = 0; uint8_t *u8p; // len smartport_send_byte(0x08, &crc); // type smartport_send_byte(0x01, &crc); // frame_id smartport_send_byte(frame_id, &crc); // data_id u8p = (uint8_t *)&data_id; smartport_send_byte(u8p[0], &crc); smartport_send_byte(u8p[1], &crc); // value u8p = (uint8_t *)&value; smartport_send_byte(u8p[0], &crc); smartport_send_byte(u8p[1], &crc); smartport_send_byte(u8p[2], &crc); smartport_send_byte(u8p[3], &crc); // crc smartport_send_byte(0xFF - (uint8_t)crc, NULL); // blink vTaskResume(context.led_task_handle); } static void set_config(smartport_parameters_t *parameter) { config_t *config = config_read(); uart0_begin(115200, UART_RECEIVER_TX, UART_RECEIVER_RX, TIMEOUT_US, 8, 1, UART_PARITY_NONE, config->fport_inverted, true); TaskHandle_t task_handle; float *baro_temp = NULL, *baro_pressure = NULL; parameter->data_id = 0x5100; if (config->esc_protocol == ESC_PWM) { esc_pwm_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, malloc(sizeof(float))}; xTaskCreate(esc_pwm_task, "esc_pwm_task", STACK_ESC_PWM, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_double_parameters_t parameter_sensor_double; parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = parameter.rpm; parameter_sensor_double.value_h = NULL; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_HW3) { esc_hw3_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, malloc(sizeof(float))}; xTaskCreate(esc_hw3_task, "esc_hw3_task", STACK_ESC_HW3, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_double_parameters_t parameter_sensor_double; parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = parameter.rpm; parameter_sensor_double.value_h = NULL; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_HW4) { esc_hw4_parameters_t parameter = {config->rpm_multiplier, config->enable_pwm_out, config->enable_esc_hw4_init_delay, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, config->esc_hw4_voltage_multiplier, config->esc_hw4_current_multiplier, config->esc_hw4_current_thresold, config->esc_hw4_current_max, config->esc_hw4_is_manual_offset, config->esc_hw4_auto_detect, config->esc_hw4_offset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_hw4_task, "esc_hw4_task", STACK_ESC_HW4, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); if (config->enable_pwm_out) { xTaskCreate(pwm_out_task, "pwm_out", STACK_PWM_OUT, (void *)parameter.rpm, 2, &task_handle); context.pwm_out_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } smartport_sensor_parameters_t parameter_sensor; smartport_sensor_double_parameters_t parameter_sensor_double; smartport_sensor_cell_parameters_t parameter_sensor_cell; parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = parameter.rpm; parameter_sensor_double.value_h = parameter.consumption; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_double.data_id = ESC_POWER_FIRST_ID; parameter_sensor_double.value_l = parameter.voltage; parameter_sensor_double.value_h = parameter.current; parameter_sensor_double.rate = config->refresh_rate_voltage; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID; parameter_sensor.value = parameter.temperature_fet; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID + 1; parameter_sensor.value = parameter.temperature_bec; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_cell.cell_count = parameter.cell_count; parameter_sensor_cell.cell_voltage = parameter.cell_voltage; parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_task, "sensor_cell_task", STACK_SENSOR_SMARTPORT_CELL, (void *)¶meter_sensor_cell, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_HW5) { esc_hw5_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_hw5_task, "esc_hw5_task", STACK_ESC_HW5, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; smartport_sensor_double_parameters_t parameter_sensor_double; smartport_sensor_cell_parameters_t parameter_sensor_cell; parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = parameter.rpm; parameter_sensor_double.value_h = parameter.consumption; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_double.data_id = ESC_POWER_FIRST_ID; parameter_sensor_double.value_l = parameter.voltage; parameter_sensor_double.value_h = parameter.current; parameter_sensor_double.rate = config->refresh_rate_voltage; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID; parameter_sensor.value = parameter.temperature_fet; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID + 1; parameter_sensor.value = parameter.temperature_bec; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID + 2; parameter_sensor.value = parameter.temperature_motor; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_double.data_id = ESC_POWER_FIRST_ID + 1; parameter_sensor_double.value_l = parameter.voltage_bec; parameter_sensor_double.value_h = parameter.current_bec; parameter_sensor_double.rate = config->refresh_rate_voltage; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_cell.cell_count = parameter.cell_count; parameter_sensor_cell.cell_voltage = parameter.cell_voltage; parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_task, "sensor_cell_task", STACK_SENSOR_SMARTPORT_CELL, (void *)¶meter_sensor_cell, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_CASTLE) { esc_castle_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_castle_task, "esc_castle_task", STACK_ESC_CASTLE, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; smartport_sensor_double_parameters_t parameter_sensor_double; smartport_sensor_cell_parameters_t parameter_sensor_cell; parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = parameter.rpm; parameter_sensor_double.value_h = parameter.consumption; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_double.data_id = ESC_POWER_FIRST_ID; parameter_sensor_double.value_l = parameter.voltage; parameter_sensor_double.value_h = parameter.current; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_double.data_id = ESC_POWER_FIRST_ID + 1; parameter_sensor_double.value_l = parameter.voltage_bec; parameter_sensor_double.value_h = parameter.current_bec; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID; parameter_sensor.value = parameter.temperature; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_cell.cell_count = parameter.cell_count; parameter_sensor_cell.cell_voltage = parameter.cell_voltage; parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_task, "sensor_cell_task", STACK_SENSOR_SMARTPORT_CELL, (void *)¶meter_sensor_cell, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_KONTRONIK) { esc_kontronik_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_kontronik_task, "esc_kontronik_task", STACK_ESC_KONTRONIK, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; smartport_sensor_double_parameters_t parameter_sensor_double; smartport_sensor_cell_parameters_t parameter_sensor_cell; parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = parameter.rpm; parameter_sensor_double.value_h = parameter.consumption; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_double.data_id = ESC_POWER_FIRST_ID; parameter_sensor_double.value_l = parameter.voltage; parameter_sensor_double.value_h = parameter.current; parameter_sensor_double.rate = config->refresh_rate_voltage; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID; parameter_sensor.value = parameter.temperature_fet; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID + 1; parameter_sensor.value = parameter.temperature_bec; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_cell.cell_count = parameter.cell_count; parameter_sensor_cell.cell_voltage = parameter.cell_voltage; parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_task, "sensor_cell_task", STACK_SENSOR_SMARTPORT_CELL, (void *)¶meter_sensor_cell, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_APD_F) { esc_apd_f_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_apd_f_task, "esc_apd_f_task", STACK_ESC_APD_F, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; smartport_sensor_double_parameters_t parameter_sensor_double; smartport_sensor_cell_parameters_t parameter_sensor_cell; parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = parameter.rpm; parameter_sensor_double.value_h = parameter.consumption; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_double.data_id = ESC_POWER_FIRST_ID; parameter_sensor_double.value_l = parameter.voltage; parameter_sensor_double.value_h = parameter.current; parameter_sensor_double.rate = config->refresh_rate_voltage; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID; parameter_sensor.value = parameter.temperature; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_cell.cell_count = parameter.cell_count; parameter_sensor_cell.cell_voltage = parameter.cell_voltage; parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_task, "sensor_cell_task", STACK_SENSOR_SMARTPORT_CELL, (void *)¶meter_sensor_cell, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_APD_HV) { esc_apd_hv_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_apd_hv_task, "esc_apd_hv_task", STACK_ESC_APD_HV, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; smartport_sensor_double_parameters_t parameter_sensor_double; smartport_sensor_cell_parameters_t parameter_sensor_cell; parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = parameter.rpm; parameter_sensor_double.value_h = parameter.consumption; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_double.data_id = ESC_POWER_FIRST_ID; parameter_sensor_double.value_l = parameter.voltage; parameter_sensor_double.value_h = parameter.current; parameter_sensor_double.rate = config->refresh_rate_voltage; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID; parameter_sensor.value = parameter.temperature; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_cell.cell_count = parameter.cell_count; parameter_sensor_cell.cell_voltage = parameter.cell_voltage; parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_task, "sensor_cell_task", STACK_SENSOR_SMARTPORT_CELL, (void *)¶meter_sensor_cell, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_SMART) { smart_esc_parameters_t parameter; parameter.calc_consumption = config->smart_esc_calc_consumption; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temperature_fet = malloc(sizeof(float)); parameter.temperature_bec = malloc(sizeof(float)); parameter.voltage_bec = malloc(sizeof(float)); parameter.current_bec = malloc(sizeof(float)); parameter.temperature_bat = malloc(sizeof(float)); parameter.current_bat = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); for (uint i = 0; i < 18; i++) parameter.cell[i] = malloc(sizeof(float)); parameter.cells = malloc(sizeof(uint8_t)); parameter.cycles = malloc(sizeof(uint16_t)); xTaskCreate(smart_esc_task, "smart_esc_task", STACK_SMART_ESC, (void *)¶meter, 4, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; smartport_sensor_double_parameters_t parameter_sensor_double; smartport_sensor_cell_individual_parameters_t parameter_sensor_cell; // rpm & consumption parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = parameter.rpm; parameter_sensor_double.value_h = parameter.consumption; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // voltage & current parameter_sensor_double.data_id = ESC_POWER_FIRST_ID; parameter_sensor_double.value_l = parameter.voltage; parameter_sensor_double.value_h = parameter.current; parameter_sensor_double.rate = config->refresh_rate_voltage; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // bec. voltage & current parameter_sensor_double.data_id = SBEC_POWER_FIRST_ID; parameter_sensor_double.value_l = parameter.voltage_bec; parameter_sensor_double.value_h = parameter.current_bec; parameter_sensor_double.rate = config->refresh_rate_voltage; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // temp_fet parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID; parameter_sensor.value = parameter.temperature_fet; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // temp_bec parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID + 1; parameter_sensor.value = parameter.temperature_bec; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // temp_bat parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID + 2; parameter_sensor.value = parameter.temperature_bat; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // current_bat parameter_sensor.data_id = CURR_FIRST_ID + 1; parameter_sensor.value = parameter.current_bat; parameter_sensor.rate = config->refresh_rate_current; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // cells parameter_sensor_cell.cell_count = parameter.cells; // Pointer provided by ESC_SMART task for (uint i = 0; i < 18; i++) { parameter_sensor_cell.cell_voltage[i] = parameter.cell[i]; // One pointer per cell } parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_individual_task, "sensor_cell_task", STACK_SENSOR_SMARTPORT_CELL, (void *)¶meter_sensor_cell, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // cycles /*parameter_sensor.data_id = DIY_FIRST_ID + 100; parameter_sensor.value = parameter.cycles; parameter_sensor.rate = config->refresh_rate_default; xTaskCreate(sensor_cell_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY);*/ } if (config->esc_protocol == ESC_OMP_M4) { esc_omp_m4_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temp_esc = malloc(sizeof(float)); parameter.temp_motor = malloc(sizeof(float)); parameter.cell_voltage = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); parameter.cell_count = malloc(sizeof(uint8_t)); xTaskCreate(esc_omp_m4_task, "esc_omp_m4_task", STACK_ESC_OMP_M4, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; smartport_sensor_double_parameters_t parameter_sensor_double; smartport_sensor_cell_parameters_t parameter_sensor_cell; parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = parameter.rpm; parameter_sensor_double.value_h = parameter.consumption; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_double.data_id = ESC_POWER_FIRST_ID; parameter_sensor_double.value_l = parameter.voltage; parameter_sensor_double.value_h = parameter.current; parameter_sensor_double.rate = config->refresh_rate_voltage; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID; parameter_sensor.value = parameter.temp_esc; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID + 2; parameter_sensor.value = parameter.temp_motor; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_cell.cell_count = parameter.cell_count; parameter_sensor_cell.cell_voltage = parameter.cell_voltage; parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_task, "sensor_cell_task", STACK_SENSOR_SMARTPORT_CELL, (void *)¶meter_sensor_cell, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_ZTW) { esc_ztw_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temp_esc = malloc(sizeof(float)); parameter.temp_motor = malloc(sizeof(float)); parameter.bec_voltage = malloc(sizeof(float)); parameter.cell_voltage = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); parameter.cell_count = malloc(sizeof(uint8_t)); xTaskCreate(esc_ztw_task, "esc_ztw_task", STACK_ESC_ZTW, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; smartport_sensor_double_parameters_t parameter_sensor_double; smartport_sensor_cell_parameters_t parameter_sensor_cell; parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = parameter.rpm; parameter_sensor_double.value_h = parameter.consumption; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_double.data_id = ESC_POWER_FIRST_ID; parameter_sensor_double.value_l = parameter.voltage; parameter_sensor_double.value_h = parameter.current; parameter_sensor_double.rate = config->refresh_rate_voltage; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID; parameter_sensor.value = parameter.temp_esc; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID + 2; parameter_sensor.value = parameter.temp_motor; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_cell.cell_count = parameter.cell_count; parameter_sensor_cell.cell_voltage = parameter.cell_voltage; parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_task, "sensor_cell_task", STACK_SENSOR_SMARTPORT_CELL, (void *)¶meter_sensor_cell, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_OPENYGE) { esc_openyge_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.pwm_out = config->enable_pwm_out; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temperature_fet = malloc(sizeof(float)); parameter.temperature_bec = malloc(sizeof(float)); parameter.cell_voltage = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); parameter.voltage_bec = malloc(sizeof(float)); parameter.current_bec = malloc(sizeof(float)); parameter.throttle = malloc(sizeof(float)); parameter.pwm_percent = malloc(sizeof(float)); parameter.cell_count = malloc(sizeof(uint8_t)); xTaskCreate(esc_openyge_task, "esc_openyge_task", STACK_ESC_OPENYGE, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; smartport_sensor_double_parameters_t parameter_sensor_double; smartport_sensor_cell_parameters_t parameter_sensor_cell; // RPM and Consumption sensor parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = parameter.rpm; parameter_sensor_double.value_h = parameter.consumption; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // Main voltage and current sensor parameter_sensor_double.data_id = ESC_POWER_FIRST_ID; parameter_sensor_double.value_l = parameter.voltage; parameter_sensor_double.value_h = parameter.current; parameter_sensor_double.rate = config->refresh_rate_voltage; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // FET Temperature sensor parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID; parameter_sensor.value = parameter.temperature_fet; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // BEC Temperature sensor parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID + 2; parameter_sensor.value = parameter.temperature_bec; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // BEC voltage and current sensor parameter_sensor_double.data_id = ESC_POWER_FIRST_ID + 2; parameter_sensor_double.value_l = parameter.voltage_bec; parameter_sensor_double.value_h = parameter.current_bec; parameter_sensor_double.rate = config->refresh_rate_voltage; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // Cell voltage sensor parameter_sensor_cell.cell_count = parameter.cell_count; parameter_sensor_cell.cell_voltage = parameter.cell_voltage; parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_task, "sensor_cell_task", STACK_SENSOR_SMARTPORT_CELL, (void *)¶meter_sensor_cell, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_gps) { gps_parameters_t parameter; parameter.protocol = config->gps_protocol; parameter.baudrate = config->gps_baudrate; parameter.rate = config->gps_rate; parameter.lat = malloc(sizeof(double)); parameter.lon = malloc(sizeof(double)); parameter.alt = malloc(sizeof(float)); parameter.spd = malloc(sizeof(float)); parameter.cog = malloc(sizeof(float)); parameter.hdop = malloc(sizeof(float)); parameter.sat = malloc(sizeof(float)); parameter.time = malloc(sizeof(float)); parameter.date = malloc(sizeof(float)); parameter.vspeed = malloc(sizeof(float)); parameter.dist = malloc(sizeof(float)); parameter.spd_kmh = malloc(sizeof(float)); parameter.fix = malloc(sizeof(float)); parameter.vdop = malloc(sizeof(float)); parameter.speed_acc = malloc(sizeof(float)); parameter.h_acc = malloc(sizeof(float)); parameter.v_acc = malloc(sizeof(float)); parameter.track_acc = malloc(sizeof(float)); parameter.n_vel = malloc(sizeof(float)); parameter.e_vel = malloc(sizeof(float)); parameter.v_vel = malloc(sizeof(float)); parameter.alt_elipsiod = malloc(sizeof(float)); parameter.pdop = malloc(sizeof(float)); parameter.fix_type = malloc(sizeof(uint8_t)); parameter.home_set = malloc(sizeof(uint8_t)); parameter.alt_home = malloc(sizeof(float)); xTaskCreate(gps_task, "gps_task", STACK_GPS, (void *)¶meter, 2, &task_handle); context.uart_pio_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_coordinate_parameters_t parameter_sensor_coordinate; parameter_sensor_coordinate.type = SMARTPORT_LATITUDE; parameter_sensor_coordinate.latitude = parameter.lat; parameter_sensor_coordinate.longitude = parameter.lon; parameter_sensor_coordinate.rate = config->refresh_rate_gps; xTaskCreate(sensor_coordinates_task, "sensor_coordinates_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor_coordinate, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_datetime_parameters_t parameter_sensor_datetime; parameter_sensor_datetime.type = SMARTPORT_DATE; parameter_sensor_datetime.date = parameter.date; parameter_sensor_datetime.time = parameter.time; parameter_sensor_datetime.rate = 1000; xTaskCreate(sensor_datetime_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor_datetime, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; parameter_sensor.data_id = GPS_ALT_FIRST_ID; parameter_sensor.value = parameter.alt; parameter_sensor.rate = config->refresh_rate_gps; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = GPS_SPEED_FIRST_ID; parameter_sensor.value = parameter.spd; parameter_sensor.rate = config->refresh_rate_gps; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = GPS_COURS_FIRST_ID; parameter_sensor.value = parameter.cog; parameter_sensor.rate = config->refresh_rate_gps; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = VARIO_FIRST_ID + 1; parameter_sensor.value = parameter.vspeed; parameter_sensor.rate = config->refresh_rate_gps; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = DIY_FIRST_ID + 3; parameter_sensor.value = parameter.sat; parameter_sensor.rate = config->refresh_rate_gps; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = DIST_FIRST_ID; parameter_sensor.value = parameter.dist; parameter_sensor.rate = config->refresh_rate_gps; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = DIY_FIRST_ID + 5; parameter_sensor.value = parameter.pdop; parameter_sensor.rate = config->refresh_rate_gps; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_voltage) { voltage_parameters_t parameter = {0, config->analog_rate, config->alpha_voltage, config->analog_voltage_multiplier, malloc(sizeof(float))}; xTaskCreate(voltage_task, "voltage_task", STACK_VOLTAGE, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; parameter_sensor.data_id = A3_FIRST_ID; parameter_sensor.value = parameter.voltage; parameter_sensor.rate = config->refresh_rate_voltage; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_current) { current_parameters_t parameter = {1, config->analog_rate, config->alpha_current, config->analog_current_multiplier, config->analog_current_offset, config->analog_current_autoffset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(current_task, "current_task", STACK_CURRENT, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; parameter_sensor.data_id = CURR_FIRST_ID; parameter_sensor.value = parameter.current; parameter_sensor.rate = config->refresh_rate_current; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_double_parameters_t parameter_sensor_double; parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = NULL; parameter_sensor_double.value_h = parameter.consumption; parameter_sensor_double.rate = config->refresh_rate_current; xTaskCreate(sensor_double_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_BMP280) { bmp280_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, 0, config->bmp280_filter, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(bmp280_task, "bmp280_task", STACK_BMP280, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } smartport_sensor_parameters_t parameter_sensor; parameter_sensor.data_id = ALT_FIRST_ID; parameter_sensor.value = parameter.altitude; parameter_sensor.rate = config->refresh_rate_vario; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = VARIO_FIRST_ID; parameter_sensor.value = parameter.vspeed; parameter_sensor.rate = config->refresh_rate_vario; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_MS5611) { ms5611_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, 0, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(ms5611_task, "ms5611_task", STACK_MS5611, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } smartport_sensor_parameters_t parameter_sensor; parameter_sensor.data_id = ALT_FIRST_ID; parameter_sensor.value = parameter.altitude; parameter_sensor.rate = config->refresh_rate_vario; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = VARIO_FIRST_ID; parameter_sensor.value = parameter.vspeed; parameter_sensor.rate = config->refresh_rate_vario; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_BMP180) { bmp180_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(bmp180_task, "bmp180_task", STACK_BMP180, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } smartport_sensor_parameters_t parameter_sensor; parameter_sensor.data_id = ALT_FIRST_ID; parameter_sensor.value = parameter.altitude; parameter_sensor.rate = config->refresh_rate_vario; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = VARIO_FIRST_ID; parameter_sensor.value = parameter.vspeed; parameter_sensor.rate = config->refresh_rate_vario; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_ntc) { ntc_parameters_t parameter = {2, config->analog_rate, config->ntc_offset, config->alpha_temperature, malloc(sizeof(float))}; xTaskCreate(ntc_task, "ntc_task", STACK_NTC, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID; parameter_sensor.value = parameter.ntc; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_airspeed) { airspeed_parameters_t parameter = {3, config->analog_rate, config->alpha_airspeed, (float)config->airspeed_offset / 1000, (float)config->airspeed_vcc / 100, baro_temp, baro_pressure, malloc(sizeof(float))}; xTaskCreate(airspeed_task, "airspeed_task", STACK_AIRSPEED, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; parameter_sensor.data_id = AIR_SPEED_FIRST_ID; parameter_sensor.value = parameter.airspeed; parameter_sensor.rate = config->refresh_rate_airspeed; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_fuel_flow) { fuel_meter_parameters_t parameter = {config->fuel_flow_ml_per_pulse, malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(fuel_meter_task, "fuel_meter_task", STACK_FUEL_METER, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; parameter_sensor.data_id = GASSUIT_FLOW_FIRST_ID; parameter_sensor.value = parameter.consumption_instant; parameter_sensor.rate = config->refresh_rate_default; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = GASSUIT_RES_VOL_FIRST_ID; parameter_sensor.value = parameter.consumption_total; parameter_sensor.rate = config->refresh_rate_default; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->gpio_mask) { gpio_parameters_t parameter = {config->gpio_mask, config->gpio_interval, malloc(sizeof(float))}; xTaskCreate(gpio_task, "gpio_task", STACK_GPIO, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_gpio_parameters_t parameter_sensor; parameter_sensor.data_id = DIY_FIRST_ID; parameter_sensor.value = parameter.value; parameter_sensor.rate = config->gpio_interval; parameter_sensor.gpio_mask = config->gpio_mask; xTaskCreate(sensor_gpio_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_gyro) { mpu6050_parameters_t parameter = {1, 0, config->mpu6050_acc_scale, config->mpu6050_gyro_scale, config->mpu6050_gyro_weighting, config->mpu6050_filter, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(mpu6050_task, "mpu6050_task", STACK_MPU6050, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; parameter_sensor.data_id = DIY_FIRST_ID + 6; parameter_sensor.value = parameter.pitch; parameter_sensor.rate = config->refresh_rate_default; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = DIY_FIRST_ID + 7; parameter_sensor.value = parameter.roll; parameter_sensor.rate = config->refresh_rate_default; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = DIY_FIRST_ID + 8; parameter_sensor.value = parameter.yaw; parameter_sensor.rate = config->refresh_rate_default; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ACCX_FIRST_ID; parameter_sensor.value = parameter.acc_x; parameter_sensor.rate = config->refresh_rate_default; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ACCY_FIRST_ID; parameter_sensor.value = parameter.acc_y; parameter_sensor.rate = config->refresh_rate_default; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ACCZ_FIRST_ID; parameter_sensor.value = parameter.acc_z; parameter_sensor.rate = config->refresh_rate_default; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = DIY_FIRST_ID + 9; parameter_sensor.value = parameter.acc; parameter_sensor.rate = config->refresh_rate_default; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_lipo && config->lipo_cells > 0) { smartport_sensor_cell_individual_parameters_t parameter_sensor_cell; float *cell_prev = NULL; // Maximum supported cells: 6 (two INA3221 devices) uint8_t lipo_cells = MIN(config->lipo_cells, 6); // Initialize all cell pointers to NULL (safety) for (uint i = 0; i < 18; i++) { parameter_sensor_cell.cell_voltage[i] = NULL; } // Configure task parameters parameter_sensor_cell.rate = config->refresh_rate_voltage; parameter_sensor_cell.cell_count = malloc(sizeof(uint8_t)); *parameter_sensor_cell.cell_count = lipo_cells; // --- First INA3221: cells 0–2 ----------------------------------------- uint8_t cells_first = MIN(lipo_cells, 3); if (cells_first > 0) { ina3221_parameters_t parameter = { .i2c_address = 0x40, .filter = config->ina3221_filter, .cell_count = cells_first, .cell[0] = malloc(sizeof(float)), .cell[1] = malloc(sizeof(float)), .cell[2] = malloc(sizeof(float)), .cell_prev = malloc(sizeof(float)), }; // First INA has no previous cell reference *parameter.cell_prev = 0; cell_prev = parameter.cell[2]; xTaskCreate(ina3221_task, "ina3221_task", STACK_INA3221, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // Store cell pointers for SmartPort for (uint i = 0; i < cells_first; i++) { parameter_sensor_cell.cell_voltage[i] = parameter.cell[i]; } } // --- Second INA3221: cells 3–5 ---------------------------------------- if (lipo_cells > 3) { uint8_t cells_second = MIN((uint8_t)(lipo_cells - 3), (uint8_t)3); ina3221_parameters_t parameter = { .i2c_address = 0x41, .filter = config->ina3221_filter, .cell_count = cells_second, .cell[0] = malloc(sizeof(float)), .cell[1] = malloc(sizeof(float)), .cell[2] = malloc(sizeof(float)), .cell_prev = cell_prev, // Link to the last cell of the previous INA }; xTaskCreate(ina3221_task, "ina3221_task", STACK_INA3221, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // Store cell pointers for SmartPort for (uint i = 0; i < cells_second; i++) { parameter_sensor_cell.cell_voltage[i + 3] = parameter.cell[i]; } } // --- Start SmartPort task: cycle through individual cells ------------- xTaskCreate(sensor_cell_individual_task, "sensor_cell_task", STACK_SENSOR_SMARTPORT_CELL, (void *)¶meter_sensor_cell, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } } ================================================ FILE: board/project/protocol/fport.h ================================================ #ifndef FPORT_H #define FPORT_H #include "common.h" extern context_t context; void fport_task(void *parameters); #endif ================================================ FILE: board/project/protocol/frsky_d.c ================================================ #include "frsky_d.h" #include #include #include #include #include "airspeed.h" #include "bmp180.h" #include "bmp280.h" #include "config.h" #include "current.h" #include "esc_apd_f.h" #include "esc_apd_hv.h" #include "esc_castle.h" #include "esc_hw3.h" #include "esc_hw4.h" #include "esc_hw5.h" #include "esc_kontronik.h" #include "esc_omp_m4.h" #include "esc_openyge.h" #include "esc_pwm.h" #include "esc_ztw.h" #include "gps.h" #include "mpu6050.h" #include "ms5611.h" #include "ntc.h" #include "pwm_out.h" #include "smart_esc.h" #include "uart.h" #include "uart_pio.h" #include "voltage.h" /* FrSky D Data Id */ #define FRSKY_D_GPS_ALT_BP_ID 0x01 #define FRSKY_D_TEMP1_ID 0x02 #define FRSKY_D_RPM_ID 0x03 #define FRSKY_D_FUEL_ID 0x04 #define FRSKY_D_TEMP2_ID 0x05 #define FRSKY_D_CELL_VOLT_ID 0x06 #define FRSKY_D_GPS_ALT_AP_ID 0x09 #define FRSKY_D_BARO_ALT_BP_ID 0x10 #define FRSKY_D_GPS_SPEED_BP_ID 0x11 #define FRSKY_D_GPS_LONG_BP_ID 0x12 #define FRSKY_D_GPS_LAT_BP_ID 0x13 #define FRSKY_D_GPS_COURS_BP_ID 0x14 #define FRSKY_D_GPS_DAY_MONTH_ID 0x15 #define FRSKY_D_GPS_YEAR_ID 0x16 #define FRSKY_D_GPS_HOUR_MIN_ID 0x17 #define FRSKY_D_GPS_SEC_ID 0x18 #define FRSKY_D_GPS_SPEED_AP_ID 0x19 #define FRSKY_D_GPS_LONG_AP_ID 0x1A #define FRSKY_D_GPS_LAT_AP_ID 0x1B #define FRSKY_D_GPS_COURS_AP_ID 0x1C #define FRSKY_D_BARO_ALT_AP_ID 0x21 #define FRSKY_D_GPS_LONG_EW_ID 0x22 #define FRSKY_D_GPS_LAT_NS_ID 0x23 #define FRSKY_D_ACCEL_X_ID 0x24 #define FRSKY_D_ACCEL_Y_ID 0x25 #define FRSKY_D_ACCEL_Z_ID 0x26 #define FRSKY_D_CURRENT_ID 0x28 #define FRSKY_D_VARIO_ID 0x30 #define FRSKY_D_VFAS_ID 0x39 #define FRSKY_D_VOLTS_BP_ID 0x3A #define FRSKY_D_VOLTS_AP_ID 0x3B #define FRSKY_D_FRSKY_LAST_ID 0x3F #define FRSKY_D_D_RSSI_ID 0xF0 #define FRSKY_D_D_A1_ID 0xF1 #define FRSKY_D_D_A2_ID 0xF2 #define FRSKY_D_INTERVAL 10 typedef struct frsky_d_sensor_parameters_t { uint8_t data_id; float *value; uint16_t rate; } frsky_d_sensor_parameters_t; typedef struct frsky_d_sensor_cell_parameters_t { float *voltage; uint8_t *count; uint16_t rate; } frsky_d_sensor_cell_parameters_t; static SemaphoreHandle_t semaphore = NULL; static void sensor_task(void *parameters); static void send_packet(uint8_t dataId, uint16_t value); static void send_byte(uint8_t c, bool header); static uint16_t format(uint8_t data_id, float value); static void set_config(); void frsky_d_task(void *parameters) { context.led_cycle_duration = 6; context.led_cycles = 1; uart0_begin(9600, UART_RECEIVER_TX, UART_RECEIVER_RX, 0, 8, 1, UART_PARITY_NONE, true, false); semaphore = xSemaphoreCreateMutex(); set_config(); debug("\nFrsky D init"); vTaskSuspend(NULL); vTaskDelete(NULL); } static void sensor_task(void *parameters) { frsky_d_sensor_parameters_t parameter = *(frsky_d_sensor_parameters_t *)parameters; xTaskNotifyGive(context.receiver_task_handle); while (1) { vTaskDelay(parameter.rate / portTICK_PERIOD_MS); xSemaphoreTake(semaphore, portMAX_DELAY); uint16_t data_formatted = format(parameter.data_id, *parameter.value); debug("\nFrSky D. Sensor (%u) > ", uxTaskGetStackHighWaterMark(NULL)); send_packet(parameter.data_id, data_formatted); xSemaphoreGive(semaphore); } } static void sensor_cell_task(void *parameters) { frsky_d_sensor_cell_parameters_t parameter = *(frsky_d_sensor_cell_parameters_t *)parameters; xTaskNotifyGive(context.receiver_task_handle); uint8_t cell_index = 0; while (1) { vTaskDelay(parameter.rate / portTICK_PERIOD_MS); xSemaphoreTake(semaphore, portMAX_DELAY); uint value = *parameter.voltage * 50; uint16_t data_formatted = (cell_index << 4) | ((value & 0xF00) >> 8) | ((value & 0x0FF) << 8); cell_index++; cell_index %= *parameter.count; debug("\nFrSky D. Sensor cell (%u) > ", uxTaskGetStackHighWaterMark(NULL)); send_packet(FRSKY_D_CELL_VOLT_ID, data_formatted); xSemaphoreGive(semaphore); } } static uint16_t format(uint8_t data_id, float value) { if (data_id == FRSKY_D_GPS_ALT_BP_ID || data_id == FRSKY_D_BARO_ALT_BP_ID || data_id == FRSKY_D_GPS_SPEED_BP_ID || data_id == FRSKY_D_GPS_COURS_BP_ID) return (int16_t)value; if (data_id == FRSKY_D_GPS_ALT_AP_ID || data_id == FRSKY_D_BARO_ALT_AP_ID || data_id == FRSKY_D_GPS_SPEED_AP_ID || data_id == FRSKY_D_GPS_COURS_AP_ID) return (abs(value) - (int16_t)abs(value)) * 10000; if (data_id == FRSKY_D_GPS_LONG_AP_ID || data_id == FRSKY_D_GPS_LAT_AP_ID) { float min = fabs(value) * 60; return (min - (uint)min) * 10000; } if (data_id == FRSKY_D_VOLTS_BP_ID) return value * 2; if (data_id == FRSKY_D_VOLTS_AP_ID) return ((value * 2) - (int16_t)(value * 2)) * 10000; if (data_id == FRSKY_D_GPS_LONG_BP_ID || data_id == FRSKY_D_GPS_LAT_BP_ID) { float coord = fabs(value); uint8_t deg = coord; uint8_t min = (coord - deg) * 60; char buf[7]; sprintf(buf, "%u%02u", deg, min); // (ddd)mm return atoi(buf); } if (data_id == FRSKY_D_GPS_LONG_EW_ID) { if (value >= 0) return 'E'; return 'O'; } if (data_id == FRSKY_D_GPS_LAT_NS_ID) { if (value >= 0) return 'N'; return 'S'; } if (data_id == FRSKY_D_GPS_YEAR_ID) { return ((uint)value % 100) + 2000; // ddmmyy -> yyyy } if (data_id == FRSKY_D_GPS_DAY_MONTH_ID) { // ddmmyy -> ddmm return value / 100; } if (data_id == FRSKY_D_GPS_HOUR_MIN_ID) { // hhmmss -> hhmm return value / 100; } if (data_id == FRSKY_D_GPS_SEC_ID) { // hhmmss -> ss return (uint)value % 100; } if (data_id == FRSKY_D_CURRENT_ID || data_id == FRSKY_D_VFAS_ID) return round(value * 10); if (data_id == FRSKY_D_RPM_ID) return value / 60; return round(value); } static void send_byte(uint8_t c, bool header) { if ((c == 0x5D || c == 0x5E) && !header) { uart0_write(0x5D); c ^= 0x60; } uart0_write(c); debug("%X ", c); } static void send_packet(uint8_t data_id, uint16_t value) { uint8_t *u8p; // header send_byte(0x5E, true); // data_id send_byte(data_id, false); // value u8p = (uint8_t *)&value; send_byte(u8p[0], false); send_byte(u8p[1], false); // footer send_byte(0x5E, true); // blink vTaskResume(context.led_task_handle); } static void set_config() { config_t *config = config_read(); TaskHandle_t task_handle; frsky_d_sensor_parameters_t parameter_sensor; frsky_d_sensor_cell_parameters_t parameter_sensor_cell; float *baro_temp = NULL, *baro_pressure = NULL; if (config->esc_protocol == ESC_PWM) { esc_pwm_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, malloc(sizeof(float))}; xTaskCreate(esc_pwm_task, "esc_pwm_task", STACK_ESC_PWM, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_RPM_ID; parameter_sensor.value = parameter.rpm; parameter_sensor.rate = config->refresh_rate_rpm; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_HW3) { esc_hw3_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, malloc(sizeof(float))}; xTaskCreate(esc_hw3_task, "esc_hw3_task", STACK_ESC_HW3, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_RPM_ID; parameter_sensor.value = parameter.rpm; parameter_sensor.rate = config->refresh_rate_rpm; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_HW4) { esc_hw4_parameters_t parameter = {config->rpm_multiplier, config->enable_pwm_out, config->enable_esc_hw4_init_delay, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, config->esc_hw4_voltage_multiplier, config->esc_hw4_current_multiplier, config->esc_hw4_current_thresold, config->esc_hw4_current_max, config->esc_hw4_is_manual_offset, config->esc_hw4_auto_detect, config->esc_hw4_offset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_hw4_task, "esc_hw4_task", STACK_ESC_HW4, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); if (config->enable_pwm_out) { xTaskCreate(pwm_out_task, "pwm_out", STACK_PWM_OUT, (void *)parameter.rpm, 2, &task_handle); context.pwm_out_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } parameter_sensor.data_id = FRSKY_D_RPM_ID; parameter_sensor.value = parameter.rpm; parameter_sensor.rate = config->refresh_rate_rpm; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_VOLTS_BP_ID; parameter_sensor.value = parameter.voltage; parameter_sensor.rate = config->refresh_rate_voltage; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_VOLTS_AP_ID; parameter_sensor.value = parameter.voltage; parameter_sensor.rate = config->refresh_rate_voltage; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_CURRENT_ID; parameter_sensor.value = parameter.current; parameter_sensor.rate = config->refresh_rate_current; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_TEMP1_ID; parameter_sensor.value = parameter.temperature_fet; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_TEMP2_ID; parameter_sensor.value = parameter.temperature_bec; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_cell.voltage = parameter.cell_voltage; parameter_sensor_cell.count = parameter.cell_count; parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_task, "sensor_cell_task", STACK_SENSOR_FRSKY_D_CELL, (void *)¶meter_sensor_cell, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_FUEL_ID; parameter_sensor.value = parameter.consumption; parameter_sensor.rate = config->refresh_rate_consumption; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_HW5) { esc_hw5_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_hw5_task, "esc_hw5_task", STACK_ESC_HW5, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_RPM_ID; parameter_sensor.value = parameter.rpm; parameter_sensor.rate = config->refresh_rate_rpm; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_VOLTS_BP_ID; parameter_sensor.value = parameter.voltage; parameter_sensor.rate = config->refresh_rate_voltage; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_VOLTS_AP_ID; parameter_sensor.value = parameter.voltage; parameter_sensor.rate = config->refresh_rate_voltage; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_CURRENT_ID; parameter_sensor.value = parameter.current; parameter_sensor.rate = config->refresh_rate_current; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_TEMP1_ID; parameter_sensor.value = parameter.temperature_fet; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_TEMP2_ID; parameter_sensor.value = parameter.temperature_bec; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_cell.voltage = parameter.cell_voltage; parameter_sensor_cell.count = parameter.cell_count; parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_task, "sensor_cell_task", STACK_SENSOR_FRSKY_D_CELL, (void *)¶meter_sensor_cell, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_FUEL_ID; parameter_sensor.value = parameter.consumption; parameter_sensor.rate = config->refresh_rate_consumption; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_CASTLE) { esc_castle_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_castle_task, "esc_castle_task", STACK_ESC_CASTLE, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_RPM_ID; parameter_sensor.value = parameter.rpm; parameter_sensor.rate = config->refresh_rate_rpm; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_VOLTS_BP_ID; parameter_sensor.value = parameter.voltage; parameter_sensor.rate = config->refresh_rate_voltage; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_VOLTS_AP_ID; parameter_sensor.value = parameter.voltage; parameter_sensor.rate = config->refresh_rate_voltage; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_CURRENT_ID; parameter_sensor.value = parameter.current; parameter_sensor.rate = config->refresh_rate_current; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_TEMP1_ID; parameter_sensor.value = parameter.temperature; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_cell.voltage = parameter.cell_voltage; parameter_sensor_cell.count = parameter.cell_count; parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_task, "sensor_cell_task", STACK_SENSOR_FRSKY_D_CELL, (void *)¶meter_sensor_cell, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_FUEL_ID; parameter_sensor.value = parameter.consumption; parameter_sensor.rate = config->refresh_rate_consumption; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_KONTRONIK) { esc_kontronik_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_kontronik_task, "esc_kontronik_task", STACK_ESC_KONTRONIK, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_RPM_ID; parameter_sensor.value = parameter.rpm; parameter_sensor.rate = config->refresh_rate_rpm; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_VOLTS_BP_ID; parameter_sensor.value = parameter.voltage; parameter_sensor.rate = config->refresh_rate_voltage; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_VOLTS_AP_ID; parameter_sensor.value = parameter.voltage; parameter_sensor.rate = config->refresh_rate_voltage; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_CURRENT_ID; parameter_sensor.value = parameter.current; parameter_sensor.rate = config->refresh_rate_current; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_TEMP1_ID; parameter_sensor.value = parameter.temperature_fet; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_TEMP2_ID; parameter_sensor.value = parameter.temperature_bec; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_cell.voltage = parameter.cell_voltage; parameter_sensor_cell.count = parameter.cell_count; parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_task, "sensor_cell_task", STACK_SENSOR_FRSKY_D_CELL, (void *)¶meter_sensor_cell, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_FUEL_ID; parameter_sensor.value = parameter.consumption; parameter_sensor.rate = config->refresh_rate_consumption; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_APD_F) { esc_apd_f_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_apd_f_task, "esc_apd_f_task", STACK_ESC_APD_F, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_RPM_ID; parameter_sensor.value = parameter.rpm; parameter_sensor.rate = config->refresh_rate_rpm; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_VOLTS_BP_ID; parameter_sensor.value = parameter.voltage; parameter_sensor.rate = config->refresh_rate_voltage; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_VOLTS_AP_ID; parameter_sensor.value = parameter.voltage; parameter_sensor.rate = config->refresh_rate_voltage; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_CURRENT_ID; parameter_sensor.value = parameter.current; parameter_sensor.rate = config->refresh_rate_current; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_TEMP1_ID; parameter_sensor.value = parameter.temperature; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_cell.voltage = parameter.cell_voltage; parameter_sensor_cell.count = parameter.cell_count; parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_task, "sensor_cell_task", STACK_SENSOR_FRSKY_D_CELL, (void *)¶meter_sensor_cell, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_FUEL_ID; parameter_sensor.value = parameter.consumption; parameter_sensor.rate = config->refresh_rate_consumption; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_APD_HV) { esc_apd_hv_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_apd_hv_task, "esc_apd_hv_task", STACK_ESC_APD_HV, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_RPM_ID; parameter_sensor.value = parameter.rpm; parameter_sensor.rate = config->refresh_rate_rpm; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_VOLTS_BP_ID; parameter_sensor.value = parameter.voltage; parameter_sensor.rate = config->refresh_rate_voltage; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_VOLTS_AP_ID; parameter_sensor.value = parameter.voltage; parameter_sensor.rate = config->refresh_rate_voltage; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_CURRENT_ID; parameter_sensor.value = parameter.current; parameter_sensor.rate = config->refresh_rate_current; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_TEMP1_ID; parameter_sensor.value = parameter.temperature; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_cell.voltage = parameter.cell_voltage; parameter_sensor_cell.count = parameter.cell_count; parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_task, "sensor_cell_task", STACK_SENSOR_FRSKY_D_CELL, (void *)¶meter_sensor_cell, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_FUEL_ID; parameter_sensor.value = parameter.consumption; parameter_sensor.rate = config->refresh_rate_consumption; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_SMART) { smart_esc_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temperature_fet = malloc(sizeof(float)); parameter.temperature_bec = malloc(sizeof(float)); parameter.voltage_bec = malloc(sizeof(float)); parameter.current_bec = malloc(sizeof(float)); parameter.temperature_bat = malloc(sizeof(float)); parameter.current_bat = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); for (uint i = 0; i < 18; i++) parameter.cell[i] = malloc(sizeof(float)); parameter.cells = malloc(sizeof(uint8_t)); parameter.cycles = malloc(sizeof(uint16_t)); xTaskCreate(smart_esc_task, "smart_esc_task", STACK_SMART_ESC, (void *)¶meter, 4, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_RPM_ID; parameter_sensor.value = parameter.rpm; parameter_sensor.rate = config->refresh_rate_rpm; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_VOLTS_BP_ID; parameter_sensor.value = parameter.voltage; parameter_sensor.rate = config->refresh_rate_voltage; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_VOLTS_AP_ID; parameter_sensor.value = parameter.voltage; parameter_sensor.rate = config->refresh_rate_voltage; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_CURRENT_ID; parameter_sensor.value = parameter.current; parameter_sensor.rate = config->refresh_rate_current; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_TEMP1_ID; parameter_sensor.value = parameter.temperature_fet; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); /*parameter_sensor_cell.voltage = parameter.cell_voltage; parameter_sensor_cell.count = parameter.cell_count; parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_task, "sensor_cell_task", STACK_SENSOR_FRSKY_D_CELL, (void *)¶meter_sensor_cell, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY);*/ parameter_sensor.data_id = FRSKY_D_FUEL_ID; parameter_sensor.value = parameter.consumption; parameter_sensor.rate = config->refresh_rate_consumption; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_OMP_M4) { esc_omp_m4_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temp_esc = malloc(sizeof(float)); parameter.temp_motor = malloc(sizeof(float)); parameter.cell_voltage = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); parameter.cell_count = malloc(sizeof(uint8_t)); xTaskCreate(esc_omp_m4_task, "esc_omp_m4_task", STACK_ESC_OMP_M4, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_RPM_ID; parameter_sensor.value = parameter.rpm; parameter_sensor.rate = config->refresh_rate_rpm; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_VOLTS_BP_ID; parameter_sensor.value = parameter.voltage; parameter_sensor.rate = config->refresh_rate_voltage; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_VOLTS_AP_ID; parameter_sensor.value = parameter.voltage; parameter_sensor.rate = config->refresh_rate_voltage; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_CURRENT_ID; parameter_sensor.value = parameter.current; parameter_sensor.rate = config->refresh_rate_current; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_TEMP1_ID; parameter_sensor.value = parameter.temp_esc; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_TEMP2_ID; parameter_sensor.value = parameter.temp_motor; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_cell.voltage = parameter.cell_voltage; parameter_sensor_cell.count = parameter.cell_count; parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_task, "sensor_cell_task", STACK_SENSOR_FRSKY_D_CELL, (void *)¶meter_sensor_cell, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_FUEL_ID; parameter_sensor.value = parameter.consumption; parameter_sensor.rate = config->refresh_rate_consumption; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_ZTW) { esc_ztw_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.bec_voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temp_esc = malloc(sizeof(float)); parameter.temp_motor = malloc(sizeof(float)); parameter.cell_voltage = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); parameter.cell_count = malloc(sizeof(uint8_t)); xTaskCreate(esc_omp_m4_task, "esc_omp_m4_task", STACK_ESC_ZTW, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_RPM_ID; parameter_sensor.value = parameter.rpm; parameter_sensor.rate = config->refresh_rate_rpm; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_VOLTS_BP_ID; parameter_sensor.value = parameter.voltage; parameter_sensor.rate = config->refresh_rate_voltage; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_VOLTS_AP_ID; parameter_sensor.value = parameter.voltage; parameter_sensor.rate = config->refresh_rate_voltage; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_CURRENT_ID; parameter_sensor.value = parameter.current; parameter_sensor.rate = config->refresh_rate_current; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_TEMP1_ID; parameter_sensor.value = parameter.temp_esc; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_TEMP2_ID; parameter_sensor.value = parameter.temp_motor; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_cell.voltage = parameter.cell_voltage; parameter_sensor_cell.count = parameter.cell_count; parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_task, "sensor_cell_task", STACK_SENSOR_FRSKY_D_CELL, (void *)¶meter_sensor_cell, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_FUEL_ID; parameter_sensor.value = parameter.consumption; parameter_sensor.rate = config->refresh_rate_consumption; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_OPENYGE) { esc_openyge_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.pwm_out = config->enable_pwm_out; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temperature_fet = malloc(sizeof(float)); parameter.temperature_bec = malloc(sizeof(float)); parameter.cell_voltage = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); parameter.voltage_bec = malloc(sizeof(float)); parameter.current_bec = malloc(sizeof(float)); parameter.throttle = malloc(sizeof(float)); parameter.pwm_percent = malloc(sizeof(float)); parameter.cell_count = malloc(sizeof(uint8_t)); xTaskCreate(esc_openyge_task, "esc_openyge_task", STACK_ESC_OPENYGE, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); frsky_d_sensor_parameters_t parameter_sensor; frsky_d_sensor_cell_parameters_t parameter_sensor_cell; parameter_sensor.data_id = FRSKY_D_RPM_ID; parameter_sensor.value = parameter.rpm; parameter_sensor.rate = config->refresh_rate_rpm; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_VOLTS_BP_ID; parameter_sensor.value = parameter.voltage; parameter_sensor.rate = config->refresh_rate_voltage; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_CURRENT_ID; parameter_sensor.value = parameter.current; parameter_sensor.rate = config->refresh_rate_current; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_TEMP1_ID; parameter_sensor.value = parameter.temperature_fet; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_TEMP2_ID; parameter_sensor.value = parameter.temperature_bec; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_cell.voltage = parameter.cell_voltage; parameter_sensor_cell.count = parameter.cell_count; parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_task, "sensor_cell_task", STACK_SENSOR_FRSKY_D_CELL, (void *)¶meter_sensor_cell, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_FUEL_ID; parameter_sensor.value = parameter.consumption; parameter_sensor.rate = config->refresh_rate_consumption; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_gps) { gps_parameters_t parameter; parameter.protocol = config->gps_protocol; parameter.baudrate = config->gps_baudrate; parameter.rate = config->gps_rate; parameter.lat = malloc(sizeof(double)); parameter.lon = malloc(sizeof(double)); parameter.alt = malloc(sizeof(float)); parameter.spd = malloc(sizeof(float)); parameter.cog = malloc(sizeof(float)); parameter.hdop = malloc(sizeof(float)); parameter.sat = malloc(sizeof(float)); parameter.time = malloc(sizeof(float)); parameter.date = malloc(sizeof(float)); parameter.vspeed = malloc(sizeof(float)); parameter.dist = malloc(sizeof(float)); parameter.spd_kmh = malloc(sizeof(float)); parameter.fix = malloc(sizeof(float)); parameter.vdop = malloc(sizeof(float)); parameter.speed_acc = malloc(sizeof(float)); parameter.h_acc = malloc(sizeof(float)); parameter.v_acc = malloc(sizeof(float)); parameter.track_acc = malloc(sizeof(float)); parameter.n_vel = malloc(sizeof(float)); parameter.e_vel = malloc(sizeof(float)); parameter.v_vel = malloc(sizeof(float)); parameter.alt_elipsiod = malloc(sizeof(float)); parameter.pdop = malloc(sizeof(float)); parameter.fix_type = malloc(sizeof(uint8_t)); parameter.home_set = malloc(sizeof(uint8_t)); parameter.alt_home = malloc(sizeof(float)); xTaskCreate(gps_task, "gps_task", STACK_GPS, (void *)¶meter, 2, &task_handle); context.uart_pio_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_GPS_LONG_BP_ID; parameter_sensor.value = parameter.lon; parameter_sensor.rate = config->refresh_rate_gps; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_GPS_LONG_AP_ID; parameter_sensor.value = parameter.lon; parameter_sensor.rate = config->refresh_rate_gps; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_GPS_LONG_EW_ID; parameter_sensor.value = parameter.lon; parameter_sensor.rate = config->refresh_rate_gps; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_GPS_LAT_BP_ID; parameter_sensor.value = parameter.lat; parameter_sensor.rate = config->refresh_rate_gps; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_GPS_LAT_AP_ID; parameter_sensor.value = parameter.lat; parameter_sensor.rate = config->refresh_rate_gps; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_GPS_LAT_NS_ID; parameter_sensor.value = parameter.lat; parameter_sensor.rate = config->refresh_rate_gps; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_GPS_ALT_BP_ID; parameter_sensor.value = parameter.alt; parameter_sensor.rate = config->refresh_rate_gps; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_GPS_ALT_AP_ID; parameter_sensor.value = parameter.alt; parameter_sensor.rate = config->refresh_rate_gps; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_GPS_SPEED_BP_ID; parameter_sensor.value = parameter.spd; parameter_sensor.rate = config->refresh_rate_gps; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_GPS_SPEED_AP_ID; parameter_sensor.value = parameter.spd; parameter_sensor.rate = config->refresh_rate_gps; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_GPS_COURS_BP_ID; parameter_sensor.value = parameter.cog; parameter_sensor.rate = config->refresh_rate_gps; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_GPS_COURS_AP_ID; parameter_sensor.value = parameter.cog; parameter_sensor.rate = config->refresh_rate_gps; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_GPS_YEAR_ID; parameter_sensor.value = parameter.date; parameter_sensor.rate = 1000; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_GPS_DAY_MONTH_ID; parameter_sensor.value = parameter.date; parameter_sensor.rate = 1000; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_GPS_HOUR_MIN_ID; parameter_sensor.value = parameter.time; parameter_sensor.rate = 1000; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_GPS_SEC_ID; parameter_sensor.value = parameter.time; parameter_sensor.rate = config->refresh_rate_gps; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_VARIO_ID; parameter_sensor.value = parameter.vspeed; parameter_sensor.rate = config->refresh_rate_gps; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_voltage) { voltage_parameters_t parameter = {0, config->analog_rate, config->alpha_voltage, config->analog_voltage_multiplier, malloc(sizeof(float))}; xTaskCreate(voltage_task, "voltage_task", STACK_VOLTAGE, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_VOLTS_BP_ID; parameter_sensor.value = parameter.voltage; parameter_sensor.rate = config->refresh_rate_voltage; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_VOLTS_AP_ID; parameter_sensor.value = parameter.voltage; parameter_sensor.rate = config->refresh_rate_voltage; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_current) { current_parameters_t parameter = {1, config->analog_rate, config->alpha_current, config->analog_current_multiplier, config->analog_current_offset, config->analog_current_autoffset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(current_task, "current_task", STACK_CURRENT, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_CURRENT_ID; parameter_sensor.value = parameter.current; parameter_sensor.rate = config->refresh_rate_current; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); /**new_sensor = (sensor_frsky_d_t){FRSKY_D_FUEL_ID, parameter.consumption, config->refresh_rate_consumption}; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY);*/ } if (config->enable_analog_ntc) { ntc_parameters_t parameter = {2, config->analog_rate, config->ntc_offset, config->alpha_temperature, malloc(sizeof(float))}; xTaskCreate(ntc_task, "ntc_task", STACK_NTC, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_TEMP1_ID; parameter_sensor.value = parameter.ntc; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_BMP280) { bmp280_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, 0, config->bmp280_filter, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(bmp280_task, "bmp280_task", STACK_BMP280, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } parameter_sensor.data_id = FRSKY_D_BARO_ALT_BP_ID; parameter_sensor.value = parameter.altitude; parameter_sensor.rate = config->refresh_rate_vario; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_BARO_ALT_AP_ID; parameter_sensor.value = parameter.altitude; parameter_sensor.rate = config->refresh_rate_vario; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_VARIO_ID; parameter_sensor.value = parameter.vspeed; parameter_sensor.rate = config->refresh_rate_vario; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_MS5611) { ms5611_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, 0, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(ms5611_task, "ms5611_task", STACK_MS5611, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_BARO_ALT_BP_ID; parameter_sensor.value = parameter.altitude; parameter_sensor.rate = config->refresh_rate_vario; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_BARO_ALT_AP_ID; parameter_sensor.value = parameter.altitude; parameter_sensor.rate = config->refresh_rate_vario; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } parameter_sensor.data_id = FRSKY_D_VARIO_ID; parameter_sensor.value = parameter.vspeed; parameter_sensor.rate = config->refresh_rate_vario; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_BMP180) { bmp180_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(bmp180_task, "bmp180_task", STACK_BMP180, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } parameter_sensor.data_id = FRSKY_D_BARO_ALT_BP_ID; parameter_sensor.value = parameter.altitude; parameter_sensor.rate = config->refresh_rate_vario; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_BARO_ALT_AP_ID; parameter_sensor.value = parameter.altitude; parameter_sensor.rate = config->refresh_rate_vario; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_VARIO_ID; parameter_sensor.value = parameter.vspeed; parameter_sensor.rate = config->refresh_rate_vario; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_airspeed) { airspeed_parameters_t parameter = {3, config->analog_rate, config->alpha_airspeed, (float)config->airspeed_offset / 1000, (float)config->airspeed_vcc / 100, baro_temp, baro_pressure, malloc(sizeof(float))}; xTaskCreate(airspeed_task, "airspeed_task", STACK_AIRSPEED, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_GPS_SPEED_BP_ID; parameter_sensor.value = parameter.airspeed; parameter_sensor.rate = config->refresh_rate_airspeed; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_GPS_SPEED_AP_ID; parameter_sensor.value = parameter.airspeed; parameter_sensor.rate = config->refresh_rate_airspeed; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_FRSKY_D, (void *)¶meter_sensor, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_gyro) { mpu6050_parameters_t parameter = {1, 0, config->mpu6050_acc_scale, config->mpu6050_gyro_scale, config->mpu6050_gyro_weighting, config->mpu6050_filter, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(mpu6050_task, "mpu6050_task", STACK_MPU6050, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_ACCEL_X_ID; parameter_sensor.value = parameter.acc_x; parameter_sensor.rate = config->refresh_rate_default; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_ACCEL_Y_ID; parameter_sensor.value = parameter.acc_y; parameter_sensor.rate = config->refresh_rate_default; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = FRSKY_D_ACCEL_Z_ID; parameter_sensor.value = parameter.acc_z; parameter_sensor.rate = config->refresh_rate_default; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } } ================================================ FILE: board/project/protocol/frsky_d.h ================================================ #ifndef FRSKY_D_H #define FRSKY_D_H #include "common.h" extern context_t context; void frsky_d_task(void *parameters); #endif ================================================ FILE: board/project/protocol/ghst.c ================================================ #include "ghst.h" #include #include #include #include #include "airspeed.h" #include "bmp180.h" #include "bmp280.h" #include "config.h" #include "current.h" #include "esc_apd_f.h" #include "esc_apd_hv.h" #include "esc_castle.h" #include "esc_hw3.h" #include "esc_hw4.h" #include "esc_hw5.h" #include "esc_kontronik.h" #include "esc_omp_m4.h" #include "esc_pwm.h" #include "esc_ztw.h" #include "gps.h" #include "ibus.h" #include "ms5611.h" #include "ntc.h" #include "pwm_out.h" #include "smart_esc.h" #include "uart.h" #include "uart_pio.h" #include "voltage.h" #define GHST_DL_PACK_STAT 0x23 // Battery Status #define GHST_DL_GPS_PRIMARY 0x25 // Primary GPS Data #define GHST_DL_GPS_SECONDARY 0x26 // Secondary GPS Data #define GHST_DL_MAGBARO 0x27 // Magnetometer, Barometer (and Vario) Data #define GHST_DL_MSP_RESP 0x28 // MSP Response #define GHST_ADDR_RX 0x89 #define GHST_PACKET_LEN 0x0C #define GHST_TIMEOUT_US 500 #define TYPE_PACK_STAT 0 #define TYPE_GPS_PRIMARY 1 #define TYPE_GPS_SECONDARY 2 #define TYPE_MAGBARO 3 #define MAX_SENSORS 4 typedef struct ghst_sensor_pack_stat_formatted_t { uint16_t voltage; // 10mV uint16_t current; // 10mA uint16_t consumed; // 10mAh uint8_t rx_volt; // 100mV } __attribute__((packed)) ghst_sensor_pack_stat_formatted_t; typedef struct ghst_sensor_gps_primary_formatted_t { uint32_t latitude; // degree / 10,000,000 uint32_t longitude; // degree / 10,000,000 int16_t altitude; // meters } __attribute__((packed)) ghst_sensor_gps_primary_formatted_t; typedef struct ghst_sensor_gps_secondary_formatted_t { uint16_t groundspeed; // cm/s uint16_t heading; // GPS heading, degree/10 uint8_t satellites; // satellites uint16_t distance; // 10m uint16_t homedir; // degree/10 uint8_t flags; // fix 0x01, fix home 0x02 } __attribute__((packed)) ghst_sensor_gps_secondary_formatted_t; typedef struct ghst_sensor_magbaro_formatted_t { uint16_t heading; // degree/10 int16_t altitude; // m int16_t vario; // cm/s uint8_t flags; // maghead 0x01, baroalt 0x02, vario 0x04 } __attribute__((packed)) ghst_sensor_magbaro_formatted_t; typedef struct ghst_sensor_pack_stat_t { float *voltage; float *current; float *consumed; float *rx_volt; } ghst_sensor_pack_stat_t; typedef struct ghst_sensor_gps_primary_t { float *latitude; float *longitude; float *altitude; } ghst_sensor_gps_primary_t; typedef struct ghst_sensor_gps_secondary_t { float *groundspeed; float *heading; float *satellites; float *distance; float *homedir; float *flags; uint8_t *home_set; uint8_t *fix_type; } ghst_sensor_gps_secondary_t; typedef struct ghst_sensor_magbaro_t { float *heading; float *altitude; float *vario; float *flags; } ghst_sensor_magbaro_t; typedef struct ghst_sensors_t { bool enabled_sensors[MAX_SENSORS]; ghst_sensor_pack_stat_t pack_stat; ghst_sensor_gps_primary_t gps_primary; ghst_sensor_gps_secondary_t gps_secondary; ghst_sensor_magbaro_t magbaro; } ghst_sensors_t; static void process(ghst_sensors_t *sensors); static uint8_t format_sensor(ghst_sensors_t *sensors, uint8_t type, uint8_t *buffer); static void send_packet(ghst_sensors_t *sensors); static uint8_t get_crc(const uint8_t *ptr, uint32_t len); static uint8_t crc8(uint8_t crc, unsigned char a); static void set_config(ghst_sensors_t *sensors); static inline uint sensor_count(ghst_sensors_t *sensors); void ghst_task(void *parameters) { ghst_sensors_t sensors = {0}; set_config(&sensors); context.led_cycle_duration = 6; context.led_cycles = 1; uart0_begin(420000L, UART_RECEIVER_TX, UART_RECEIVER_RX, GHST_TIMEOUT_US, 8, 1, UART_PARITY_NONE, false, true); debug("\nGHST init"); while (1) { ulTaskNotifyTakeIndexed(1, pdTRUE, portMAX_DELAY); process(&sensors); } } static void process(ghst_sensors_t *sensors) { if (uart0_available() == GHST_PACKET_LEN + 2) { uint8_t data[GHST_PACKET_LEN + 2]; uart0_read_bytes(data, GHST_PACKET_LEN + 2); debug("\nGHST (%u) < ", uxTaskGetStackHighWaterMark(NULL)); debug_buffer(data, GHST_PACKET_LEN + 2, "0x%X "); uint8_t crc = get_crc(data + 2, GHST_PACKET_LEN - 1); // crc from type, size 11 // send telemetry if (data[0] == GHST_ADDR_RX) { if (data[13] == crc) send_packet(sensors); else debug("\nGHST. Bad CRC 0x%X - 0x%X", data[13], crc); } } } static uint8_t format_sensor(ghst_sensors_t *sensors, uint8_t type, uint8_t *buffer) { // Packet format: [rx_addr] [len] [type] [payload] [crc8 from type] buffer[0] = GHST_ADDR_RX; buffer[1] = GHST_PACKET_LEN; switch (type) { case TYPE_PACK_STAT: { buffer[2] = GHST_DL_PACK_STAT; ghst_sensor_pack_stat_formatted_t sensor = {0}; if (sensors->pack_stat.voltage) sensor.voltage = *sensors->pack_stat.voltage * 100; if (sensors->pack_stat.current) sensor.current = *sensors->pack_stat.current * 100; if (sensors->pack_stat.consumed) sensor.consumed = *sensors->pack_stat.consumed / 10; memcpy(&buffer[3], &sensor, sizeof(ghst_sensor_pack_stat_formatted_t)); buffer[GHST_PACKET_LEN + 1] = get_crc(&buffer[2], GHST_PACKET_LEN + 1); break; } case TYPE_GPS_PRIMARY: { buffer[2] = GHST_DL_GPS_PRIMARY; ghst_sensor_gps_primary_formatted_t sensor = {0}; if (sensors->gps_primary.latitude) sensor.latitude = *sensors->gps_primary.latitude * 10000000L; if (sensors->gps_primary.longitude) sensor.longitude = *sensors->gps_primary.longitude * 10000000L; if (sensors->gps_primary.altitude) sensor.altitude = *sensors->gps_primary.altitude; memcpy(&buffer[3], &sensor, sizeof(ghst_sensor_gps_primary_formatted_t)); buffer[GHST_PACKET_LEN + 1] = get_crc(&buffer[2], GHST_PACKET_LEN + 1); break; } case TYPE_GPS_SECONDARY: { buffer[2] = GHST_DL_GPS_SECONDARY; ghst_sensor_gps_secondary_formatted_t sensor = {0}; if (sensors->gps_secondary.groundspeed) sensor.groundspeed = fabs(*sensors->gps_secondary.groundspeed * 1000.0F / 36); if (sensors->gps_secondary.heading) sensor.heading = *sensors->gps_secondary.heading * 10; if (sensors->gps_secondary.satellites) sensor.satellites = *sensors->gps_secondary.satellites; if (sensors->gps_secondary.distance) sensor.distance = *sensors->gps_secondary.distance / 10; if (sensors->gps_secondary.homedir) sensor.homedir = *sensors->gps_secondary.homedir * 10; sensor.flags = 0; if (sensors->gps_secondary.fix_type && *sensors->gps_secondary.fix_type >= 1) { sensor.flags |= 0x01; } if (sensors->gps_secondary.home_set && *sensors->gps_secondary.home_set) { sensor.flags |= 0x02; } memcpy(&buffer[3], &sensor, sizeof(ghst_sensor_gps_secondary_formatted_t)); buffer[GHST_PACKET_LEN + 1] = get_crc(&buffer[2], GHST_PACKET_LEN + 1); break; } case TYPE_MAGBARO: { buffer[2] = GHST_DL_MAGBARO; ghst_sensor_magbaro_formatted_t sensor = {0}; if (sensors->magbaro.vario) sensor.vario = *sensors->magbaro.vario * 100; memcpy(&buffer[3], &sensor, sizeof(ghst_sensor_magbaro_formatted_t)); buffer[GHST_PACKET_LEN + 1] = get_crc(&buffer[2], GHST_PACKET_LEN + 1); break; } } } static inline uint sensor_count(ghst_sensors_t *sensors) { uint count = 0; for (uint i = 0; i < MAX_SENSORS; i++) if (sensors->enabled_sensors[i]) count++; return count; } static void send_packet(ghst_sensors_t *sensors) { if (!sensor_count(sensors)) return; static uint type = 0; uint8_t buffer[GHST_PACKET_LEN + 2] = {0}; while (!sensors->enabled_sensors[type % MAX_SENSORS]) type++; format_sensor(sensors, type % MAX_SENSORS, buffer); uart0_write_bytes(buffer, GHST_PACKET_LEN + 2); type++; debug("\nGHST (%u) > ", uxTaskGetStackHighWaterMark(NULL)); debug_buffer(buffer, GHST_PACKET_LEN + 2, "0x%X "); // blink led vTaskResume(context.led_task_handle); } static uint8_t get_crc(const uint8_t *ptr, uint32_t len) { uint8_t crc = 0; for (uint32_t i = 0; i < len; i++) { crc = crc8(crc, *ptr++); } return crc; } static uint8_t crc8(uint8_t crc, unsigned char a) { crc ^= a; for (int ii = 0; ii < 8; ++ii) { if (crc & 0x80) { crc = (crc << 1) ^ 0xD5; } else { crc = crc << 1; } } return crc; } static void set_config(ghst_sensors_t *sensors) { config_t *config = config_read(); TaskHandle_t task_handle; if (config->esc_protocol == ESC_APD_F) { esc_apd_f_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_apd_f_task, "esc_apd_f_task", STACK_ESC_APD_F, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; sensors->enabled_sensors[TYPE_PACK_STAT] = true; sensors->pack_stat.voltage = parameter.voltage; sensors->pack_stat.current = parameter.current; sensors->pack_stat.consumed = parameter.consumption; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_gps) { gps_parameters_t parameter; parameter.protocol = config->gps_protocol; parameter.baudrate = config->gps_baudrate; parameter.rate = config->gps_rate; parameter.lat = malloc(sizeof(double)); parameter.lon = malloc(sizeof(double)); parameter.alt = malloc(sizeof(float)); parameter.spd = malloc(sizeof(float)); parameter.cog = malloc(sizeof(float)); parameter.hdop = malloc(sizeof(float)); parameter.sat = malloc(sizeof(float)); parameter.time = malloc(sizeof(float)); parameter.date = malloc(sizeof(float)); parameter.vspeed = malloc(sizeof(float)); parameter.dist = malloc(sizeof(float)); parameter.spd_kmh = malloc(sizeof(float)); parameter.fix = malloc(sizeof(float)); parameter.vdop = malloc(sizeof(float)); parameter.speed_acc = malloc(sizeof(float)); parameter.h_acc = malloc(sizeof(float)); parameter.v_acc = malloc(sizeof(float)); parameter.track_acc = malloc(sizeof(float)); parameter.n_vel = malloc(sizeof(float)); parameter.e_vel = malloc(sizeof(float)); parameter.v_vel = malloc(sizeof(float)); parameter.alt_elipsiod = malloc(sizeof(float)); parameter.pdop = malloc(sizeof(float)); parameter.fix_type = malloc(sizeof(uint8_t)); parameter.home_set = malloc(sizeof(uint8_t)); parameter.alt_home = malloc(sizeof(float)); xTaskCreate(gps_task, "gps_task", STACK_GPS, (void *)¶meter, 2, &task_handle); context.uart_pio_notify_task_handle = task_handle; sensors->enabled_sensors[TYPE_GPS_PRIMARY] = true; sensors->gps_primary.latitude = parameter.lat; sensors->gps_primary.longitude = parameter.lon; sensors->gps_primary.altitude = parameter.alt; sensors->enabled_sensors[TYPE_GPS_SECONDARY] = true; sensors->gps_secondary.groundspeed = parameter.spd_kmh; sensors->gps_secondary.heading = parameter.cog; sensors->gps_secondary.satellites = parameter.sat; sensors->gps_secondary.distance = parameter.dist; sensors->gps_secondary.home_set = parameter.home_set; sensors->gps_secondary.fix_type = parameter.fix_type; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_voltage) { voltage_parameters_t parameter = {0, config->analog_rate, config->alpha_voltage, config->analog_voltage_multiplier, malloc(sizeof(float))}; xTaskCreate(voltage_task, "voltage_task", STACK_VOLTAGE, (void *)¶meter, 2, &task_handle); sensors->enabled_sensors[TYPE_PACK_STAT] = true; sensors->pack_stat.voltage = parameter.voltage; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_current) { current_parameters_t parameter = {1, config->analog_rate, config->alpha_current, config->analog_current_multiplier, config->analog_current_offset, config->analog_current_autoffset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(current_task, "current_task", STACK_CURRENT, (void *)¶meter, 2, &task_handle); sensors->enabled_sensors[TYPE_PACK_STAT] = true; sensors->pack_stat.current = parameter.current; sensors->pack_stat.consumed = parameter.consumption; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_BMP280) { bmp280_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, 0, config->bmp280_filter, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(bmp280_task, "bmp280_task", STACK_BMP280, (void *)¶meter, 2, &task_handle); sensors->enabled_sensors[TYPE_MAGBARO] = true; sensors->magbaro.altitude = parameter.altitude; sensors->magbaro.vario = parameter.vspeed; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_MS5611) { ms5611_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, 0, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(ms5611_task, "ms5611_task", STACK_MS5611, (void *)¶meter, 2, &task_handle); sensors->enabled_sensors[TYPE_MAGBARO] = true; sensors->magbaro.altitude = parameter.altitude; sensors->magbaro.vario = parameter.vspeed; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_BMP180) { bmp180_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(bmp180_task, "bmp180_task", STACK_BMP180, (void *)¶meter, 2, &task_handle); sensors->enabled_sensors[TYPE_MAGBARO] = true; sensors->magbaro.altitude = parameter.altitude; sensors->magbaro.vario = parameter.vspeed; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } } ================================================ FILE: board/project/protocol/ghst.h ================================================ #ifndef GHST_H #define GHST_H #include "common.h" extern context_t context; void ghst_task(void *parameters); #endif ================================================ FILE: board/project/protocol/hitec.c ================================================ #include "hitec.h" #include #include #include #include "airspeed.h" #include "bmp180.h" #include "bmp280.h" #include "config.h" #include "current.h" #include "esc_apd_f.h" #include "esc_apd_hv.h" #include "esc_castle.h" #include "esc_hw3.h" #include "esc_hw4.h" #include "esc_hw5.h" #include "esc_kontronik.h" #include "esc_omp_m4.h" #include "esc_pwm.h" #include "esc_ztw.h" #include "gps.h" #include "hardware/i2c.h" #include "hardware/irq.h" #include "ms5611.h" #include "ntc.h" #include "pico/stdlib.h" #include "pwm_out.h" #include "smart_esc.h" #include "stdlib.h" #include "uart.h" #include "uart_pio.h" #include "voltage.h" #define I2C_INTR_MASK_RD_REQ 0x00000020 #define I2C_ADDRESS 0x08 #define TIMEOUT 1000 #define FRAME_LENGTH 7 #define FRAME_0X11 0 #define FRAME_0X12 1 #define FRAME_0X13 2 #define FRAME_0X14 3 #define FRAME_0X15 4 #define FRAME_0X16 5 #define FRAME_0X17 6 #define FRAME_0X18 7 #define FRAME_0X19 8 #define FRAME_0X1A 9 #define FRAME_0X1B 10 #define FRAME_0X11_RX_BATT 0 #define FRAME_0X12_GPS_LAT 0 #define FRAME_0X12_TIME 1 #define FRAME_0X13_GPS_LON 0 #define FRAME_0X13_TEMP2 1 #define FRAME_0X14_GPS_SPD 0 #define FRAME_0X14_GPS_ALT 1 #define FRAME_0X14_TEMP1 2 #define FRAME_0X15_FUEL 0 #define FRAME_0X15_RPM1 1 #define FRAME_0X15_RPM2 2 #define FRAME_0X16_DATE 0 #define FRAME_0X16_TIME 1 #define FRAME_0X17_COG 0 #define FRAME_0X17_SATS 1 #define FRAME_0X17_TEMP3 2 #define FRAME_0X17_TEMP4 3 #define FRAME_0X18_VOLT 0 #define FRAME_0X18_AMP 1 #define FRAME_0X19_AMP1 0 #define FRAME_0X19_AMP2 1 #define FRAME_0X19_AMP3 2 #define FRAME_0X19_AMP4 3 #define FRAME_0X1A_ASPD 0 #define FRAME_0X1B_ALTU 0 #define FRAME_0X1B_ALTF 1 typedef struct sensor_hitec_t { bool is_enabled_frame[11]; float *frame_0x11[1]; float *frame_0x12[2]; float *frame_0x13[2]; float *frame_0x14[3]; float *frame_0x15[3]; float *frame_0x16[2]; float *frame_0x17[4]; float *frame_0x18[2]; float *frame_0x19[4]; float *frame_0x1A[1]; float *frame_0x1B[2]; } sensor_hitec_t; static sensor_hitec_t *sensor; static uint8_t packet[FRAME_LENGTH] = {0}; static volatile uint8_t cont = 0; static volatile alarm_id_t alarm_id_recovery = 0; // For bus recovery timeout static volatile alarm_id_t alarm_id_reinit = 0; // For mandatory deinit/init after each frame static void i2c_handler(i2c_inst_t *i2c, i2c_slave_event_t event); static void set_config(); static int64_t alarm_recovery(alarm_id_t id, void *user_data); // I2C bus recovery alarm static int64_t alarm_reinit(alarm_id_t id, void *user_data); // Periodic deinit/init after each frame static int next_frame(void); static void format_packet(uint8_t frame, uint8_t *buffer); static void i2c_bus_recovery(void); void hitec_task(void *parameters) { sensor = malloc(sizeof(sensor_hitec_t)); *sensor = (sensor_hitec_t){{0}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}}; context.led_cycle_duration = 6; context.led_cycles = 1; set_config(); i2c_bus_recovery(); // Arm initial I2C bus watchdog: if no requests arrive soon after startup, // perform another bus recovery to handle a receiver that powers up later. alarm_id_recovery = add_alarm_in_us(1000 * 1000, alarm_recovery, NULL, true); debug("\nHitec init"); vTaskSuspend(NULL); } void hitec_i2c_handler(void) { for (uint i = 0; i < FRAME_LENGTH + 1; i++) { i2c_handler(i2c1, I2C_SLAVE_REQUEST); } } static void i2c_handler(i2c_inst_t *i2c, i2c_slave_event_t event) { switch (event) { case I2C_SLAVE_REQUEST: if (cont == 0) { int frame = next_frame(); if (frame < 0) { return; } format_packet((uint8_t)frame, packet); } if (cont < FRAME_LENGTH) { // Send next byte i2c_write_byte_raw(i2c1, packet[cont++]); // Refresh I2C bus watchdog timeout (1 s). // As long as requests keep coming, the watchdog will never fire. if (alarm_id_recovery != 0) { cancel_alarm(alarm_id_recovery); } alarm_id_recovery = add_alarm_in_us(1000 * 1000, alarm_recovery, NULL, true); // If we have just sent the last valid byte of the frame, // schedule a deinit/init after a short delay. if (cont == FRAME_LENGTH) { // This handles the extra dummy read from Hitec master. if (alarm_id_reinit != 0) { cancel_alarm(alarm_id_reinit); } // 5 ms after frame end should be enough for master to finish its extra read alarm_id_reinit = add_alarm_in_us(5 * 1000, alarm_reinit, NULL, true); debug("\nHitec (%u) > ", uxTaskGetStackHighWaterMark(context.receiver_task_handle)); debug_buffer(packet, FRAME_LENGTH, "0x%X "); } } else { // Extra read from Hitec master: we just NACK and reset frame counter. cont = 0; } break; } vTaskResume(context.led_task_handle); } static int64_t alarm_reinit(alarm_id_t id, void *user_data) { // Mark current reinit alarm as consumed alarm_id_reinit = 0; // Short deinit/init sequence after each telemetry frame. // This is required because Hitec master always performs one extra read, // leaving the RP2040 I2C slave state machine in a bad state. i2c_slave_deinit(i2c1); sleep_us(5); // Small delay to ensure peripheral is fully stopped i2c_slave_init(i2c1, I2C_ADDRESS, i2c_handler); // Reset frame counter just in case cont = 0; return 0; } static int64_t alarm_recovery(alarm_id_t id, void *user_data) { // I2C bus watchdog callback. // This is only reached if no I2C requests have been seen for the whole timeout // interval (1 s), because each request cancels and re-arms the watchdog. // While the bus is inactive/stuck, this function will keep running once per // second until a new request arrives and the handler cancels the alarm. i2c_bus_recovery(); return 1000 * 1000; // Keep watchdog running: retry bus recovery every 1 s } static void i2c_bus_recovery(void) { // Reset frame cont = 0; // Deinitialize I2C peripheral to release control of the bus i2c_slave_deinit(i2c1); // SCL as output, SDA as input with pull-up gpio_init(I2C1_SCL_GPIO); gpio_set_dir(I2C1_SCL_GPIO, GPIO_OUT); gpio_put(I2C1_SCL_GPIO, 1); gpio_init(I2C1_SDA_GPIO); gpio_set_dir(I2C1_SDA_GPIO, GPIO_IN); gpio_pull_up(I2C1_SDA_GPIO); sleep_us(5); // If SDA is low, someone is holding it: give up to 9 pulses on SCL for (int i = 0; i < 9 && !gpio_get(I2C1_SDA_GPIO); i++) { gpio_put(I2C1_SCL_GPIO, 0); sleep_us(5); gpio_put(I2C1_SCL_GPIO, 1); sleep_us(5); } // Generate STOP: SDA goes high while SCL is high // First ensure SDA is output and low gpio_set_dir(I2C1_SDA_GPIO, GPIO_OUT); gpio_put(I2C1_SDA_GPIO, 0); gpio_put(I2C1_SCL_GPIO, 1); sleep_us(5); gpio_put(I2C1_SDA_GPIO, 1); sleep_us(5); // Reinitialize I2C peripheral after bus recovery gpio_set_function(I2C1_SDA_GPIO, GPIO_FUNC_I2C); gpio_set_function(I2C1_SCL_GPIO, GPIO_FUNC_I2C); gpio_pull_up(I2C1_SDA_GPIO); gpio_pull_up(I2C1_SCL_GPIO); // Reinitialize the slave i2c_slave_init(i2c1, I2C_ADDRESS, i2c_handler); } static int next_frame(void) { static uint8_t frame = 0; uint cont = 0; frame++; frame %= 11; while (!sensor->is_enabled_frame[frame] && cont < 12) { frame++; frame %= 11; cont++; } if (cont == 12) return -1; return frame; } static void format_packet(uint8_t frame, uint8_t *buffer) { int32_t valueS32; uint16_t valueU16; uint16_t valueS16; uint8_t valueU8; buffer[0] = frame + 0x11; buffer[1] = 0; buffer[2] = 0; buffer[3] = 0; buffer[4] = 0; buffer[5] = 0; buffer[6] = frame + 0x11; switch (frame) { case FRAME_0X11: buffer[1] = 0xAF; buffer[3] = 0x2D; if (sensor->frame_0x11[FRAME_0X11_RX_BATT]) { valueU16 = *sensor->frame_0x11[FRAME_0X11_RX_BATT] * 28; buffer[4] = valueU16 >> 8; buffer[5] = valueU16; } break; case FRAME_0X12: if (sensor->frame_0x12[FRAME_0X12_GPS_LAT]) { double degrees = *sensor->frame_0x12[FRAME_0X12_GPS_LAT]; int8_t deg = degrees; int8_t min = (degrees - deg) * 60; double sec = ((degrees - deg) * 60 - min) * 60; int16_t sec_x_100 = sec * 100; int16_t deg_min = deg * 100 + min; buffer[1] = sec_x_100 >> 8; buffer[2] = sec_x_100; buffer[3] = deg_min >> 8; buffer[4] = deg_min; } if (sensor->frame_0x12[FRAME_0X12_TIME]) { valueU8 = *sensor->frame_0x12[FRAME_0X12_TIME]; } break; case FRAME_0X13: if (sensor->frame_0x13[FRAME_0X13_GPS_LON]) { float degrees = *sensor->frame_0x13[FRAME_0X13_GPS_LON]; int8_t deg = degrees; int8_t min = (degrees - deg) * 60; float sec = ((degrees - deg) * 60 - min) * 60; int16_t sec_x_100 = sec * 100; int16_t deg_min = deg * 100 + min; buffer[1] = sec_x_100 >> 8; buffer[2] = sec_x_100; buffer[3] = deg_min >> 8; buffer[4] = deg_min; } if (sensor->frame_0x13[FRAME_0X13_TEMP2]) { valueU8 = round(*sensor->frame_0x13[FRAME_0X13_TEMP2] + 40); buffer[5] = valueU8; } break; case FRAME_0X14: if (sensor->frame_0x14[FRAME_0X14_GPS_SPD]) { valueU16 = round(*sensor->frame_0x14[FRAME_0X14_GPS_SPD] * 1.852); buffer[1] = valueU16 >> 8; buffer[2] = valueU16; } if (sensor->frame_0x14[FRAME_0X14_GPS_ALT]) { valueS16 = round(*sensor->frame_0x14[FRAME_0X14_GPS_ALT]); buffer[3] = valueS16 >> 8; buffer[4] = valueS16; } if (sensor->frame_0x14[FRAME_0X14_TEMP1]) { valueU8 = round(*sensor->frame_0x14[FRAME_0X14_TEMP1] + 40); buffer[5] = valueU8; } break; case FRAME_0X15: if (sensor->frame_0x15[FRAME_0X15_RPM1]) { valueU16 = round(*sensor->frame_0x15[FRAME_0X15_RPM1]); buffer[2] = valueU16; buffer[3] = valueU16 >> 8; } if (sensor->frame_0x15[FRAME_0X15_RPM2]) { valueU16 = round(*sensor->frame_0x15[FRAME_0X15_RPM2]); buffer[4] = valueU16; buffer[5] = valueU16 >> 8; } break; case FRAME_0X16: if (sensor->frame_0x16[FRAME_0X16_DATE]) { valueS32 = *sensor->frame_0x16[FRAME_0X16_DATE]; buffer[3] = valueS32 / 10000; // year buffer[2] = (valueS32 - buffer[3] * 10000UL) / 100; // month buffer[1] = valueS32 - buffer[3] * 10000UL - buffer[2] * 100; // day } if (sensor->frame_0x16[FRAME_0X16_TIME]) { valueS32 = *sensor->frame_0x16[FRAME_0X16_TIME]; buffer[4] = valueS32 / 10000; // hour buffer[5] = (valueS32 - buffer[4] * 10000UL) / 100; // minute } break; case FRAME_0X17: if (sensor->frame_0x17[FRAME_0X17_COG]) { valueU16 = round(*sensor->frame_0x17[FRAME_0X17_COG]); buffer[1] = valueU16 >> 8; buffer[2] = valueU16; } if (sensor->frame_0x17[FRAME_0X17_SATS]) { valueU8 = *sensor->frame_0x17[FRAME_0X17_SATS]; buffer[3] = valueU8; } if (sensor->frame_0x17[FRAME_0X17_TEMP3]) { valueU8 = round(*sensor->frame_0x17[FRAME_0X17_TEMP3] + 40); buffer[4] = valueU8; } if (sensor->frame_0x17[FRAME_0X17_TEMP4]) { valueU8 = round(*sensor->frame_0x17[FRAME_0X17_TEMP4] + 40); buffer[5] = valueU8; } break; case FRAME_0X18: if (sensor->frame_0x18[FRAME_0X18_VOLT]) { valueU16 = round((*sensor->frame_0x18[FRAME_0X18_VOLT] - 0.2) * 10); buffer[1] = valueU16; buffer[2] = valueU16 >> 8; } if (sensor->frame_0x18[FRAME_0X18_AMP]) { /* value for stock transmitter (tbc) */ // valueU16 = (*sensor->frame_0x18[FRAME_0X18_AMP] + 114.875) * 1.441; /* value for opentx transmitter */ valueU16 = round(*sensor->frame_0x18[FRAME_0X18_AMP]); buffer[3] = valueU16; buffer[4] = valueU16 >> 8; } break; case FRAME_0X19: if (sensor->frame_0x19[FRAME_0X19_AMP1]) { valueU8 = round(*sensor->frame_0x19[FRAME_0X19_AMP1] * 10); buffer[5] = valueU8; } if (sensor->frame_0x19[FRAME_0X19_AMP2]) { valueU8 = round(*sensor->frame_0x19[FRAME_0X19_AMP2] * 10); buffer[5] = valueU8; } if (sensor->frame_0x19[FRAME_0X19_AMP3]) { valueU8 = round(*sensor->frame_0x19[FRAME_0X19_AMP3] * 10); buffer[5] = valueU8; } if (sensor->frame_0x19[FRAME_0X19_AMP4]) { valueU8 = round(*sensor->frame_0x19[FRAME_0X19_AMP4] * 10); buffer[5] = valueU8; } break; case FRAME_0X1A: if (sensor->frame_0x1A[FRAME_0X1A_ASPD]) { valueU16 = round(*sensor->frame_0x1A[FRAME_0X1A_ASPD]); buffer[3] = valueU16 >> 8; buffer[4] = valueU16; } break; case FRAME_0X1B: if (sensor->frame_0x1B[FRAME_0X1B_ALTU]) { valueU16 = round(*sensor->frame_0x1B[FRAME_0X1B_ALTU]); buffer[1] = valueU16 >> 8; buffer[2] = valueU16; } if (sensor->frame_0x1B[FRAME_0X1B_ALTF]) { valueU16 = round(*sensor->frame_0x1B[FRAME_0X1B_ALTF]); buffer[3] = valueU16 >> 8; buffer[4] = valueU16; } break; } } static void set_config(void) { config_t *config = config_read(); TaskHandle_t task_handle; float *baro_temp = NULL, *baro_pressure = NULL; if (config->esc_protocol == ESC_PWM) { esc_pwm_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, malloc(sizeof(float))}; xTaskCreate(esc_pwm_task, "esc_pwm_task", STACK_ESC_PWM, (void *)¶meter, 2, &task_handle); sensor->frame_0x15[FRAME_0X15_RPM1] = parameter.rpm; sensor->is_enabled_frame[FRAME_0X15] = true; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_HW3) { esc_hw3_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, malloc(sizeof(float))}; xTaskCreate(esc_hw3_task, "esc_hw3_task", STACK_ESC_HW3, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; sensor->frame_0x15[FRAME_0X15_RPM1] = parameter.rpm; sensor->is_enabled_frame[FRAME_0X15] = true; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_HW4) { esc_hw4_parameters_t parameter = {config->rpm_multiplier, config->enable_pwm_out, config->enable_esc_hw4_init_delay, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, config->esc_hw4_voltage_multiplier, config->esc_hw4_current_multiplier, config->esc_hw4_current_thresold, config->esc_hw4_current_max, config->esc_hw4_is_manual_offset, config->esc_hw4_auto_detect, config->esc_hw4_offset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_hw4_task, "esc_hw4_task", STACK_ESC_HW4, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); if (config->enable_pwm_out) { xTaskCreate(pwm_out_task, "pwm_out", STACK_PWM_OUT, (void *)parameter.rpm, 2, &task_handle); context.pwm_out_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } sensor->frame_0x15[FRAME_0X15_RPM1] = parameter.rpm; sensor->frame_0x18[FRAME_0X18_VOLT] = parameter.voltage; sensor->frame_0x18[FRAME_0X18_AMP] = parameter.current; sensor->frame_0x14[FRAME_0X14_TEMP1] = parameter.temperature_fet; sensor->frame_0x13[FRAME_0X13_TEMP2] = parameter.temperature_bec; sensor->is_enabled_frame[FRAME_0X15] = true; sensor->is_enabled_frame[FRAME_0X18] = true; sensor->is_enabled_frame[FRAME_0X14] = true; sensor->is_enabled_frame[FRAME_0X13] = true; } if (config->esc_protocol == ESC_HW5) { esc_hw5_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_hw5_task, "esc_hw5_task", STACK_ESC_HW5, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); sensor->frame_0x15[FRAME_0X15_RPM1] = parameter.rpm; sensor->frame_0x18[FRAME_0X18_VOLT] = parameter.voltage; sensor->frame_0x18[FRAME_0X18_AMP] = parameter.current; sensor->frame_0x14[FRAME_0X14_TEMP1] = parameter.temperature_fet; sensor->frame_0x13[FRAME_0X13_TEMP2] = parameter.temperature_bec; sensor->is_enabled_frame[FRAME_0X15] = true; sensor->is_enabled_frame[FRAME_0X18] = true; sensor->is_enabled_frame[FRAME_0X14] = true; sensor->is_enabled_frame[FRAME_0X13] = true; } if (config->esc_protocol == ESC_CASTLE) { esc_castle_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_castle_task, "esc_castle_task", STACK_ESC_CASTLE, (void *)¶meter, 2, &task_handle); sensor->frame_0x15[FRAME_0X15_RPM1] = parameter.rpm; sensor->frame_0x18[FRAME_0X18_VOLT] = parameter.voltage; sensor->frame_0x18[FRAME_0X18_AMP] = parameter.current; sensor->frame_0x14[FRAME_0X14_TEMP1] = parameter.temperature; sensor->is_enabled_frame[FRAME_0X15] = true; sensor->is_enabled_frame[FRAME_0X18] = true; sensor->is_enabled_frame[FRAME_0X14] = true; sensor->is_enabled_frame[FRAME_0X13] = true; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_KONTRONIK) { esc_kontronik_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_kontronik_task, "esc_kontronik_task", STACK_ESC_KONTRONIK, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; sensor->frame_0x15[FRAME_0X15_RPM1] = parameter.rpm; sensor->frame_0x18[FRAME_0X18_VOLT] = parameter.voltage; sensor->frame_0x18[FRAME_0X18_AMP] = parameter.current; sensor->frame_0x14[FRAME_0X14_TEMP1] = parameter.temperature_fet; sensor->frame_0x13[FRAME_0X13_TEMP2] = parameter.temperature_bec; sensor->is_enabled_frame[FRAME_0X15] = true; sensor->is_enabled_frame[FRAME_0X18] = true; sensor->is_enabled_frame[FRAME_0X14] = true; sensor->is_enabled_frame[FRAME_0X13] = true; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_APD_F) { esc_apd_f_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_apd_f_task, "esc_apd_f_task", STACK_ESC_APD_F, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; sensor->frame_0x15[FRAME_0X15_RPM1] = parameter.rpm; sensor->frame_0x18[FRAME_0X18_VOLT] = parameter.voltage; sensor->frame_0x18[FRAME_0X18_AMP] = parameter.current; sensor->frame_0x14[FRAME_0X14_TEMP1] = parameter.temperature; sensor->is_enabled_frame[FRAME_0X15] = true; sensor->is_enabled_frame[FRAME_0X18] = true; sensor->is_enabled_frame[FRAME_0X14] = true; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_APD_HV) { esc_apd_hv_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_apd_hv_task, "esc_apd_hv_task", STACK_ESC_APD_HV, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; sensor->frame_0x15[FRAME_0X15_RPM1] = parameter.rpm; sensor->frame_0x18[FRAME_0X18_VOLT] = parameter.voltage; sensor->frame_0x18[FRAME_0X18_AMP] = parameter.current; sensor->frame_0x14[FRAME_0X14_TEMP1] = parameter.temperature; sensor->is_enabled_frame[FRAME_0X15] = true; sensor->is_enabled_frame[FRAME_0X18] = true; sensor->is_enabled_frame[FRAME_0X14] = true; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_SMART) { smart_esc_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temperature_fet = malloc(sizeof(float)); parameter.temperature_bec = malloc(sizeof(float)); parameter.voltage_bec = malloc(sizeof(float)); parameter.current_bec = malloc(sizeof(float)); parameter.temperature_bat = malloc(sizeof(float)); parameter.current_bat = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); for (uint i = 0; i < 18; i++) parameter.cell[i] = malloc(sizeof(float)); parameter.cells = malloc(sizeof(uint8_t)); parameter.cycles = malloc(sizeof(uint16_t)); xTaskCreate(smart_esc_task, "smart_esc_task", STACK_SMART_ESC, (void *)¶meter, 4, &task_handle); context.uart1_notify_task_handle = task_handle; sensor->frame_0x15[FRAME_0X15_RPM1] = parameter.rpm; sensor->frame_0x18[FRAME_0X18_VOLT] = parameter.voltage; sensor->frame_0x18[FRAME_0X18_AMP] = parameter.current; sensor->frame_0x14[FRAME_0X14_TEMP1] = parameter.temperature_fet; sensor->is_enabled_frame[FRAME_0X15] = true; sensor->is_enabled_frame[FRAME_0X18] = true; sensor->is_enabled_frame[FRAME_0X14] = true; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_OMP_M4) { esc_omp_m4_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temp_esc = malloc(sizeof(float)); parameter.temp_motor = malloc(sizeof(float)); parameter.cell_voltage = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); parameter.cell_count = malloc(sizeof(uint8_t)); xTaskCreate(esc_omp_m4_task, "esc_omp_m4_task", STACK_ESC_OMP_M4, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; sensor->frame_0x15[FRAME_0X15_RPM1] = parameter.rpm; sensor->frame_0x18[FRAME_0X18_VOLT] = parameter.voltage; sensor->frame_0x18[FRAME_0X18_AMP] = parameter.current; sensor->frame_0x14[FRAME_0X14_TEMP1] = parameter.temp_esc; sensor->is_enabled_frame[FRAME_0X15] = true; sensor->is_enabled_frame[FRAME_0X18] = true; sensor->is_enabled_frame[FRAME_0X14] = true; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_ZTW) { esc_ztw_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temp_esc = malloc(sizeof(float)); parameter.temp_motor = malloc(sizeof(float)); parameter.bec_voltage = malloc(sizeof(float)); parameter.cell_voltage = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); parameter.cell_count = malloc(sizeof(uint8_t)); xTaskCreate(esc_omp_m4_task, "esc_omp_m4_task", STACK_ESC_OMP_M4, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; sensor->frame_0x15[FRAME_0X15_RPM1] = parameter.rpm; sensor->frame_0x18[FRAME_0X18_VOLT] = parameter.voltage; sensor->frame_0x18[FRAME_0X18_AMP] = parameter.current; sensor->frame_0x14[FRAME_0X14_TEMP1] = parameter.temp_esc; sensor->is_enabled_frame[FRAME_0X15] = true; sensor->is_enabled_frame[FRAME_0X18] = true; sensor->is_enabled_frame[FRAME_0X14] = true; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_gps) { gps_parameters_t parameter; parameter.protocol = config->gps_protocol; parameter.baudrate = config->gps_baudrate; parameter.rate = config->gps_rate; parameter.lat = malloc(sizeof(double)); parameter.lon = malloc(sizeof(double)); parameter.alt = malloc(sizeof(float)); parameter.spd = malloc(sizeof(float)); parameter.cog = malloc(sizeof(float)); parameter.hdop = malloc(sizeof(float)); parameter.sat = malloc(sizeof(float)); parameter.time = malloc(sizeof(float)); parameter.date = malloc(sizeof(float)); parameter.vspeed = malloc(sizeof(float)); parameter.dist = malloc(sizeof(float)); parameter.spd_kmh = malloc(sizeof(float)); parameter.fix = malloc(sizeof(float)); parameter.vdop = malloc(sizeof(float)); parameter.speed_acc = malloc(sizeof(float)); parameter.h_acc = malloc(sizeof(float)); parameter.v_acc = malloc(sizeof(float)); parameter.track_acc = malloc(sizeof(float)); parameter.n_vel = malloc(sizeof(float)); parameter.e_vel = malloc(sizeof(float)); parameter.v_vel = malloc(sizeof(float)); parameter.alt_elipsiod = malloc(sizeof(float)); parameter.pdop = malloc(sizeof(float)); parameter.fix_type = malloc(sizeof(uint8_t)); parameter.home_set = malloc(sizeof(uint8_t)); parameter.alt_home = malloc(sizeof(float)); xTaskCreate(gps_task, "gps_task", STACK_GPS, (void *)¶meter, 2, &task_handle); context.uart_pio_notify_task_handle = task_handle; sensor->frame_0x17[FRAME_0X17_SATS] = parameter.sat; sensor->frame_0x12[FRAME_0X12_GPS_LAT] = parameter.lat; sensor->frame_0x13[FRAME_0X13_GPS_LON] = parameter.lon; sensor->frame_0x14[FRAME_0X14_GPS_ALT] = parameter.alt; sensor->frame_0x14[FRAME_0X14_GPS_SPD] = parameter.spd; sensor->frame_0x17[FRAME_0X17_COG] = parameter.cog; sensor->frame_0x16[FRAME_0X16_DATE] = parameter.date; sensor->frame_0x16[FRAME_0X16_TIME] = parameter.time; sensor->is_enabled_frame[FRAME_0X17] = true; sensor->is_enabled_frame[FRAME_0X12] = true; sensor->is_enabled_frame[FRAME_0X13] = true; sensor->is_enabled_frame[FRAME_0X14] = true; sensor->is_enabled_frame[FRAME_0X16] = true; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_voltage) { voltage_parameters_t parameter = {0, config->analog_rate, config->alpha_voltage, config->analog_voltage_multiplier, malloc(sizeof(float))}; xTaskCreate(voltage_task, "voltage_task", STACK_VOLTAGE, (void *)¶meter, 2, &task_handle); sensor->frame_0x18[FRAME_0X18_VOLT] = parameter.voltage; sensor->is_enabled_frame[FRAME_0X18] = true; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_current) { current_parameters_t parameter = {1, config->analog_rate, config->alpha_current, config->analog_current_multiplier, config->analog_current_offset, config->analog_current_autoffset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(current_task, "current_task", STACK_CURRENT, (void *)¶meter, 2, &task_handle); sensor->frame_0x18[FRAME_0X18_AMP] = parameter.current; sensor->is_enabled_frame[FRAME_0X18] = true; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_ntc) { ntc_parameters_t parameter = {2, config->analog_rate, config->ntc_offset, config->alpha_temperature, malloc(sizeof(float))}; xTaskCreate(ntc_task, "ntc_task", STACK_NTC, (void *)¶meter, 2, &task_handle); sensor->frame_0x14[FRAME_0X14_TEMP1] = parameter.ntc; sensor->is_enabled_frame[FRAME_0X14] = true; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_BMP280) { bmp280_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, 0, config->bmp280_filter, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(bmp280_task, "bmp280_task", STACK_BMP280, (void *)¶meter, 2, &task_handle); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } sensor->frame_0x1B[FRAME_0X1B_ALTU] = parameter.altitude; sensor->is_enabled_frame[FRAME_0X1B] = true; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_MS5611) { ms5611_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, 0, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(ms5611_task, "ms5611_task", STACK_MS5611, (void *)¶meter, 2, &task_handle); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } sensor->frame_0x1B[FRAME_0X1B_ALTU] = parameter.altitude; sensor->is_enabled_frame[FRAME_0X1B] = true; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_BMP180) { bmp180_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(bmp180_task, "bmp180_task", STACK_BMP180, (void *)¶meter, 2, &task_handle); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } sensor->frame_0x1B[FRAME_0X1B_ALTU] = parameter.altitude; sensor->is_enabled_frame[FRAME_0X1B] = true; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_airspeed) { airspeed_parameters_t parameter = {3, config->analog_rate, config->alpha_airspeed, (float)config->airspeed_offset / 1000, (float)config->airspeed_vcc / 100, baro_temp, baro_pressure, malloc(sizeof(float))}; xTaskCreate(airspeed_task, "airspeed_task", STACK_AIRSPEED, (void *)¶meter, 2, &task_handle); sensor->frame_0x1A[FRAME_0X1A_ASPD] = parameter.airspeed; sensor->is_enabled_frame[FRAME_0X1A] = true; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } } ================================================ FILE: board/project/protocol/hitec.h ================================================ #ifndef HITEC_H #define HITEC_H #include "common.h" extern context_t context; void hitec_task(void *parameters); void hitec_i2c_handler(void); #endif ================================================ FILE: board/project/protocol/hott.c ================================================ #include "hott.h" #include #include #include #include #include #include #include "airspeed.h" #include "bmp180.h" #include "bmp280.h" #include "common.h" #include "config.h" #include "current.h" #include "esc_apd_f.h" #include "esc_apd_hv.h" #include "esc_castle.h" #include "esc_hw3.h" #include "esc_hw4.h" #include "esc_hw5.h" #include "esc_kontronik.h" #include "esc_omp_m4.h" #include "esc_pwm.h" #include "esc_ztw.h" #include "fuel_meter.h" #include "gps.h" #include "hardware/i2c.h" #include "hardware/irq.h" #include "ibus.h" #include "ina3221.h" #include "ms5611.h" #include "ntc.h" #include "pico/stdlib.h" #include "pwm_out.h" #include "smart_esc.h" #include "stdlib.h" #include "string.h" #include "uart.h" #include "uart_pio.h" #include "voltage.h" #include "xgzp68xxd.h" #define HOTT_VARIO_MODULE_ID 0x89 #define HOTT_GPS_MODULE_ID 0x8A #define HOTT_ESC_MODULE_ID 0x8C #define HOTT_GENERAL_AIR_MODULE_ID 0x8D #define HOTT_ELECTRIC_AIR_MODULE_ID 0x8E // not used, for internal Hott telemetry #define HOTT_VARIO_TEXT_ID ((HOTT_VARIO_MODULE_ID << 4) & 0xFF) #define HOTT_GPS_TEXT_ID ((HOTT_GPS_MODULE_ID << 4) & 0xFF) #define HOTT_ESC_TEXT_ID ((HOTT_ESC_MODULE_ID << 4) & 0xFF) #define HOTT_GENERAL_AIR_TEXT_ID ((HOTT_GENERAL_AIR_MODULE_ID << 4) & 0xFF) #define HOTT_ELECTRIC_AIR_TEXT_ID ((HOTT_ELECTRIC_AIR_MODULE_ID << 4) & 0xFF) #define HOTT_BINARY_MODE_REQUEST_ID 0x80 #define HOTT_TEXT_MODE_REQUEST_ID 0x7F #define HOTT_TIMEOUT_US 5000 #define HOTT_INTERBYTE_DELAY_US 500 #define HOTT_PACKET_LENGHT 2 #define HOTT_START_BYTE 0x7C #define HOTT_END_BYTE 0x7D // TYPE #define HOTT_TYPE_VARIO 0 #define HOTT_TYPE_ESC 1 #define HOTT_TYPE_GENERAL 2 #define HOTT_TYPE_GPS 3 #define HOTT_TYPE_ELECTRIC 4 // VARIO #define HOTT_VARIO_ALTITUDE 0 #define HOTT_VARIO_M1S 1 #define HOTT_VARIO_M3S 2 #define HOTT_VARIO_M10S 3 #define HOTT_VARIO_COUNT 4 // ESC #define HOTT_ESC_VOLTAGE 0 #define HOTT_ESC_CONSUMPTION 1 #define HOTT_ESC_TEMPERATURE 2 #define HOTT_ESC_CURRENT 3 #define HOTT_ESC_RPM 4 #define HOTT_ESC_THROTTLE 5 // 0-100% #define HOTT_ESC_SPEED 6 #define HOTT_ESC_BEC_VOLTAGE 7 #define HOTT_ESC_BEC_CURRENT 8 #define HOTT_ESC_BEC_TEMPERATURE 9 #define HOTT_ESC_EXT_TEMPERATURE 10 #define HOTT_ESC_COUNT 11 // ELECTRIC - used by internal receiver vario (shouldnt be used) #define HOTT_ELECTRIC_EXT_TEMPERATURE 0 #define HOTT_ELECTRIC_CELL_BAT_1_VOLTAGE 1 #define HOTT_ELECTRIC_CELL_BAT_2_VOLTAGE 2 #define HOTT_ELECTRIC_BAT_1_VOLTAGE 3 #define HOTT_ELECTRIC_BAT_2_VOLTAGE 4 #define HOTT_ELECTRIC_TEMPERATURE_1 5 #define HOTT_ELECTRIC_TEMPERATURE_2 6 #define HOTT_ELECTRIC_HEIGHT 7 #define HOTT_ELECTRIC_CURRENT 8 #define HOTT_ELECTRIC_CAPACITY 9 #define HOTT_ELECTRIC_M2S 10 #define HOTT_ELECTRIC_M3S 11 #define HOTT_ELECTRIC_RPM 12 #define HOTT_ELECTRIC_MINUTES 13 #define HOTT_ELECTRIC_SECONDS 14 #define HOTT_ELECTRIC_SPEED 15 #define HOTT_ELECTRIC_COUNT 16 // GPS #define HOTT_GPS_DIRECTION 0 #define HOTT_GPS_SPEED 1 #define HOTT_GPS_LATITUDE 2 #define HOTT_GPS_LONGITUDE 3 #define HOTT_GPS_DISTANCE 4 #define HOTT_GPS_ALTITUDE 5 #define HOTT_GPS_CLIMBRATE 6 #define HOTT_GPS_CLIMBRATE3S 7 #define HOTT_GPS_SATS 8 #define HOTT_GPS_FIX 9 #define HOTT_GPS_TIME 10 #define HOTT_GPS_HOME_ALTITUDE 11 #define HOTT_GPS_COUNT 12 // GENERAL #define HOTT_GENERAL_CELL_1 0 #define HOTT_GENERAL_CELL_2 1 #define HOTT_GENERAL_CELL_3 2 #define HOTT_GENERAL_CELL_4 3 #define HOTT_GENERAL_CELL_5 4 #define HOTT_GENERAL_CELL_6 5 #define HOTT_GENERAL_BATTERY_1 6 #define HOTT_GENERAL_BATTERY_2 7 #define HOTT_GENERAL_TEMP_1 8 #define HOTT_GENERAL_TEMP_2 9 #define HOTT_GENERAL_FUEL 10 // ml #define HOTT_GENERAL_RPM_1 11 #define HOTT_GENERAL_ALTITUDE 12 #define HOTT_GENERAL_CLIMBRATE 13 #define HOTT_GENERAL_CLIMBRATE3S 14 #define HOTT_GENERAL_CURRENT 15 #define HOTT_GENERAL_VOLTAGE 16 #define HOTT_GENERAL_CAPACITY 17 #define HOTT_GENERAL_SPEED 18 #define HOTT_GENERAL_RPM_2 19 #define HOTT_GENERAL_PRESSURE 20 #define HOTT_GENERAL_COUNT 21 #define HOTT_KEY_RIGHT 0xE #define HOTT_KEY_DOWN 0xB #define HOTT_KEY_UP 0xD #define HOTT_KEY_SET 0x9 #define HOTT_KEY_LEFT 0x7 #define ALARMS_FLASH_TARGET_OFFSET (CONFIG_FLASH_TARGET_OFFSET + 4096) // after program + config // Missing: 0x01, 0x04, 0x05, 0x0C, 0x15 #define ALARM_VOICE_NO_ALARM 0x00 #define ALARM_VOICE_UNKNOWN1 0x01 #define ALARM_VOICE_NEGATIVE_DIFFERENCE_2 0x02 #define ALARM_VOICE_NEGATIVE_DIFFERENCE_1 0x03 #define ALARM_VOICE_UNKNOWN2 0x04 #define ALARM_VOICE_UNKNOWN3 0x05 #define ALARM_VOICE_MIN_SENSOR_1_TEMP 0x06 #define ALARM_VOICE_MIN_SENSOR_2_TEMP 0x07 #define ALARM_VOICE_MAX_SENSOR_1_TEMP 0x08 #define ALARM_VOICE_MAX_SENSOR_2_TEMP 0x09 #define ALARM_VOICE_MAX_SENSOR_1_VOLTAGE 0x0A #define ALARM_VOICE_MAX_SENSOR_2_VOLTAGE 0x0B #define ALARM_VOICE_UNKNOWN4 0x0C #define ALARM_VOICE_POSITIVE_DIFFERENCE_2 0x0D #define ALARM_VOICE_POSITIVE_DIFFERENCE_1 0x0E #define ALARM_VOICE_MIN_ALTITUDE 0x0F #define ALARM_VOICE_MIN_POWER_VOLTAGE 0x10 #define ALARM_VOICE_MIN_CELL_VOLTAGE 0x11 #define ALARM_VOICE_MIN_SENSOR_1_VOLTAGE 0x12 #define ALARM_VOICE_MIN_SENSOR_2_VOLTAGE 0x13 #define ALARM_VOICE_MIN_RPM 0x14 #define ALARM_VOICE_UNKNOWN5 0x15 #define ALARM_VOICE_MAX_CAPACITY 0x16 #define ALARM_VOICE_MAX_CURRENT 0x17 #define ALARM_VOICE_MAX_POWER_VOLTAGE 0x18 #define ALARM_VOICE_MAX_RPM 0x19 #define ALARM_VOICE_MAX_ALTITUDE 0x1A typedef enum alarm_vario_t { ALARM_BITMASK_VARIO_ALTITUDE = 0, ALARM_BITMASK_VARIO_MAX_ALTITUDE, ALARM_BITMASK_VARIO_MIN_ALTITUDE, ALARM_BITMASK_VARIO_M1S, ALARM_BITMASK_VARIO_M3S, ALARM_BITMASK_VARIO_M10S, } alarm_vario_t; typedef enum alarm_airesc_t { ALARM_BITMASK_AIRESC_VOLTAGE = 0, ALARM_BITMASK_AIRESC_MIN_VOLTAGE, ALARM_BITMASK_AIRESC_CAPACITY, ALARM_BITMASK_AIRESC_TEMPERATURE, ALARM_BITMASK_AIRESC_MAX_TEMPERATURE, ALARM_BITMASK_AIRESC_CURRENT, ALARM_BITMASK_AIRESC_MAX_CURRENT, ALARM_BITMASK_AIRESC_RPM, ALARM_BITMASK_AIRESC_MAX_RPM, ALARM_BITMASK_AIRESC_THROTTLE, ALARM_BITMASK_AIRESC_SPEED, ALARM_BITMASK_AIRESC_MAX_SPEED, ALARM_BITMASK_AIRESC_BEC_VOLTAGE, ALARM_BITMASK_AIRESC_MIN_BEC_VOLTAGE, ALARM_BITMASK_AIRESC_BEC_CURRENT, ALARM_BITMASK_AIRESC_MIN_BEC_CURRENT, ALARM_BITMASK_AIRESC_MAX_BEC_CURRENT, ALARM_BITMASK_AIRESC_BEC_TEMPERATURE, ALARM_BITMASK_AIRESC_MAX_BEC_TEMPERATURE, ALARM_BITMASK_AIRESC_EXT_TEMPERATURE, ALARM_BITMASK_AIRESC_MAX_EXT_TEMPERATURE, ALARM_BITMASK_AIRESC_RPM_WITHOUT_GEAR, } alarm_airesc_t; typedef enum alarm_general_air_t { ALARM_BITMASK_GENERAL_AIR_CELL_VOLTAGE = 0, ALARM_BITMASK_GENERAL_AIR_BATTERY_1, ALARM_BITMASK_GENERAL_AIR_BATTERY_2, ALARM_BITMASK_GENERAL_AIR_TEMPERATURE_1, ALARM_BITMASK_GENERAL_AIR_TEMPERATURE_2, ALARM_BITMASK_GENERAL_AIR_FUEL_PERCENT, ALARM_BITMASK_GENERAL_AIR_FUEL_ML, ALARM_BITMASK_GENERAL_AIR_RPM, ALARM_BITMASK_GENERAL_AIR_ALTITUDE, ALARM_BITMASK_GENERAL_AIR_CLIMBRATE, ALARM_BITMASK_GENERAL_AIR_CLIMBRATE3S, ALARM_BITMASK_GENERAL_AIR_CURRENT, ALARM_BITMASK_GENERAL_AIR_VOLTAGE, ALARM_BITMASK_GENERAL_AIR_CAPACITY, ALARM_BITMASK_GENERAL_AIR_SPEED, ALARM_BITMASK_GENERAL_AIR_MIN_CELL_VOLTAGE, ALARM_BITMASK_GENERAL_AIR_MIN_CELL_VOLTAGE_NUM, ALARM_BITMASK_GENERAL_AIR_RPM_2, } alarm_general_air_t; typedef enum alarm_gps_t { ALARM_BITMASK_GPS_FLIGHT_DIRECTION = 0, ALARM_BITMASK_GPS_SPEED, ALARM_BITMASK_GPS_LATITUDE, ALARM_BITMASK_GPS_LONGITUDE, ALARM_BITMASK_GPS_DISTANCE, ALARM_BITMASK_GPS_ALTITUDE, ALARM_BITMASK_GPS_CLIMBRATE, ALARM_BITMASK_GPS_CLIMBRATE3S, ALARM_BITMASK_GPS_SATS, } alarm_general_gps_t; typedef struct hott_sensor_vario_t { uint8_t startByte; // 1 uint8_t sensorID; uint8_t warningID; uint8_t sensorTextID; uint8_t alarmInverse; uint16_t altitude; // 6-7 value + 500 (e.g. 0m = 500) uint16_t maxAltitude; // 8-9 uint16_t minAltitude; uint16_t m1s; // 12-13 (value * 100) + 30000 (e.g. 10m = 31000) uint16_t m3s; // ?? idem uint16_t m10s; // ?? idem uint8_t text[24]; uint8_t version; uint8_t empty; uint8_t endByte; uint8_t checksum; } __attribute__((packed)) hott_sensor_vario_t; typedef struct hott_sensor_airesc_t { uint8_t startByte; // 1 uint8_t sensorID; // 2 uint8_t warningID; // 3 uint8_t sensorTextID; // Byte 4 uint16_t alarmInverse; // Byte 5, 6 uint16_t inputVolt; // Byte 7,8 uint16_t minInputVolt; // Byte 9,10 uint16_t capacity; // Byte 11,12 uint8_t escTemperature; // Byte 13 uint8_t maxEscTemperature; // Byte 14 uint16_t current; // Byte 15,16 uint16_t maxCurrent; // Byte 17,18 uint16_t RPM; // Byte 19,20 uint16_t maxRPM; // Byte 21,22 uint8_t throttlePercent; // Byte 23 uint16_t speed; // Byte 24,25 uint16_t maxSpeed; // Byte 26,27 uint8_t BECVoltage; // Byte 28 uint8_t minBECVoltage; // Byte 29 uint8_t BECCurrent; // Byte 30 uint8_t minBECCurrent; // Byte 31 uint8_t maxBECCurrent; // Byte 32 uint8_t PWM; // Byte 33 uint8_t BECTemperature; // Byte 34 uint8_t maxBECTemperature; // Byte 35 uint8_t motorOrExtTemperature; // Byte 36 uint8_t maxMotorOrExtTemperature; // Byte 37 uint16_t RPMWithoutGearOrExt; // Byte 38,39 uint8_t timing; // Byte 40 uint8_t advancedTiming; // Byte 41 uint8_t highestCurrentMotorNumber; // Byte 42 uint8_t version; /* Byte 43: 00 version number */ uint8_t endByte; /* Byte 44: 0x7D Ende byte */ uint8_t checksum; /* Byte 45: Parity Byte */ } __attribute__((packed)) hott_sensor_airesc_t; // used by internal receiver vario (shouldnt be used) typedef struct hott_sensor_electric_air_t { uint8_t startByte; // 1 uint8_t sensorID; // 2 uint8_t warningID; // 3: Alarm uint8_t sensorTextID; // 4: uint16_t alarmInverse; // 5 6: alarm bitmask. Value is displayed inverted uint8_t cell1L; // 7: Low Voltage Cell 1 in 0,02 V steps uint8_t cell2L; // 8: Low Voltage Cell 2 in 0,02 V steps uint8_t cell3L; // 9: Low Voltage Cell 3 in 0,02 V steps uint8_t cell4L; // 10: Low Voltage Cell 4 in 0,02 V steps uint8_t cell5L; // 11: Low Voltage Cell 5 in 0,02 V steps uint8_t cell6L; // 12: Low Voltage Cell 6 in 0,02 V steps uint8_t cell7L; // 13: Low Voltage Cell 7 in 0,02 V steps uint8_t cell1H; // 14: High Voltage Cell 1 in 0.02 V steps uint8_t cell2H; // 15 uint8_t cell3H; // 16 uint8_t cell4H; // 17 uint8_t cell5H; // 18 uint8_t cell6H; // 19 uint8_t cell7H; // 20 uint16_t battery1; // 21 Battery 1 in 100mv steps; 50 == 5V uint16_t battery2; // 23 Battery 2 in 100mv steps; 50 == 5V uint8_t temp1; // 25 Temp 1; Offset of 20. 20 == 0C uint8_t temp2; // 26 Temp 2; Offset of 20. 20 == 0C uint16_t height; // 27 28 Height. Offset -500. 500 == 0 uint16_t current; // 29 30 1 = 0.1A uint16_t driveVoltage; // 31 uint16_t capacity; // 33 34 mAh uint16_t m2s; // 35 36 /* Steigrate m2s; 0x48 == 0 uint8_t m3s; // 37 /* Steigrate m3s; 0x78 == 0 uint16_t rpm; // 38 39 /* RPM. 10er steps; 300 == 3000rpm uint8_t minutes; // 40 uint8_t seconds; // 41 uint8_t speed; // 42 uint8_t version; // 43 uint8_t endByte; // 44 uint8_t checksum; // 45 } __attribute__((packed)) hott_sensor_electric_air_t; typedef struct hott_sensor_general_air_t { uint8_t startByte; //#01 start byte constant value 0x7c uint8_t sensorID; //#02 EAM sensort id. constat value 0x8d=GENRAL AIR MODULE uint8_t warningID; //#03 1=A 2=B ... 0x1a=Z 0 = no alarm /* VOICE OR BIP WARNINGS Alarme sonore A.. Z, octet correspondant 1 à 26 0x00 00 0 No alarm 0x01 01 A 0x02 02 B Negative Difference 2 B 0x03 03 C Negative Difference 1 C 0x04 04 D 0x05 05 E 0x06 06 F Min. Sensor 1 temp. F 0x07 07 G Min. Sensor 2 temp. G 0x08 08 H Max. Sensor 1 temp. H 0x09 09 I Max. Sensor 2 temp. I 0xA 10 J Max. Sens. 1 voltage J 0xB 11 K Max. Sens. 2 voltage K 0xC 12 L 0xD 13 M Positive Difference 2 M 0xE 14 N Positive Difference 1 N 0xF 15 O Min. Altitude O 0x10 16 P Min. Power Voltage P // We use this one for Battery Warning 0x11 17 Q Min. Cell voltage Q 0x12 18 R Min. Sens. 1 voltage R 0x13 19 S Min. Sens. 2 voltage S 0x14 20 T Minimum RPM T 0x15 21 U 0x16 22 V Max. used capacity V 0x17 23 W Max. Current W 0x18 24 X Max. Power Voltage X 0x19 25 Y Maximum RPM Y 0x1A 26 Z Max. Altitude Z */ uint8_t sensorTextID; //#04 constant value 0xd0 uint16_t alarmInverse; //#05-06 alarm bitmask. Value is displayed inverted // 0 all cell voltage // 1 Battery 1 // 2 Battery 2 // 3 Temperature 1 // 4 Temperature 2 // 5 Fuel // 6 mAh // 7 Altitude // 8 main power current // 9 main power voltage // 10 Altitude // 11 m/s // 12 m/3s // 13 unknown // 14 unknown // 15 "ON" sign/text msg active uint8_t cell[6]; //#7 Volt Cell 1 (in 2 mV increments, 210 == 4.20 V) //#8 Volt Cell 2 (in 2 mV increments, 210 == 4.20 V) //#9 Volt Cell 3 (in 2 mV increments, 210 == 4.20 V) //#10 Volt Cell 4 (in 2 mV increments, 210 == 4.20 V) //#11 Volt Cell 5 (in 2 mV increments, 210 == 4.20 V) //#12 Volt Cell 6 (in 2 mV increments, 210 == 4.20 V) uint16_t battery1; //#13 LSB battery 1 voltage LSB value. 0.1V steps. 50 = 5.5V only pos. voltages //#14 MSB uint16_t battery2; //#15 LSB battery 2 voltage LSB value. 0.1V steps. 50 = 5.5V only pos. voltages //#16 MSB uint8_t temperature1; //#17 Temperature 1. Offset of 20. a value of 20 = 0°C uint8_t temperature2; //#18 Temperature 2. Offset of 20. a value of 20 = 0°C uint8_t fuel_procent; //#19 Fuel capacity in %. Values 0--100 // graphical display ranges: 0-100% with new firmwares of the radios MX12/MX20/... uint16_t fuel_ml; //#20 LSB Fuel in ml scale. Full = 65535! //#21 MSB uint16_t rpm; //#22 RPM in 10 RPM steps. 300 = 3000rpm //#23 MSB uint16_t altitude; //#24 altitude in meters. offset of 500, 500 = 0m //#25 MSB uint16_t climbrate; //#26 climb rate in 0.01m/s. Value of 30000 = 0.00 m/s //#27 MSB uint8_t climbrate3s; //#28 climb rate in m/3sec. Value of 120 = 0m/3sec uint16_t current; //#29 current in 0.1A steps 100 == 10,0A //#30 MSB current display only goes up to 99.9 A (continuous) uint16_t main_voltage; //#31 LSB Main power voltage using 0.1V steps 100 == 10,0V //#32 MSB (Appears in GAM display right as alternate display.) uint16_t batt_cap; //#33 LSB used battery capacity in 10mAh steps //#34 MSB uint16_t speed; //#35 LSB (air?) speed in km/h(?) we are using ground speed here per default //#36 MSB speed uint8_t min_cell_volt; //#37 minimum cell voltage in 2mV steps. 124 = 2,48V uint8_t min_cell_volt_num; //#38 number of the cell with the lowest voltage uint16_t rpm2; //#39 LSB 2nd RPM in 10 RPM steps. 100 == 1000rpm //#40 MSB uint8_t general_error_number; //#41 General Error Number (Voice Error == 12) TODO: more documentation uint8_t pressure; //#42 High pressure up to 16bar. 0,1bar scale. 20 == 2.0 bar // 1 bar = 10 hoch 5 Pa uint8_t version; //#43 version number (Bytes 35 .43 new but not yet in the record in the display!) uint8_t endByte; //#44 stop byte 0x7D uint8_t checksum; //#45 CHECKSUM CRC/Parity (calculated dynamicaly) } __attribute__((packed)) hott_sensor_general_air_t; typedef struct hott_sensor_gps_t { uint8_t startByte; /* Byte 1: 0x7C = Start byte data */ uint8_t sensorID; /* Byte 2: 0x8A = GPS Sensor */ uint8_t warningID; /* Byte 3: 0…= warning beeps */ uint8_t sensorTextID; /* Byte 4: 160 0xA0 Sensor ID Neu! */ uint16_t alarmInverse; /* Byte 5-6: 01 inverse status */ uint8_t flightDirection; /* Byte 7: 119 = Flugricht./dir. 1 = 2°; 0° (North), 9 0° (East), 180° (South), 270° (West) */ uint16_t GPSSpeed; /* Byte 8: 8 = Geschwindigkeit/GPS speed low byte 8km/h */ uint8_t LatitudeNS; /* Byte 10: 000 = N = 48°39’988 */ uint16_t LatitudeDegMin; /* Byte 11: 231 0xE7 = 0x12E7 = 4839 */ uint16_t LatitudeSec; /* Byte 13: 171 220 = 0xDC = 0x03DC =0988 */ uint8_t longitudeEW; /* Byte 15: 000 = E= 9° 25’9360 */ uint16_t longitudeDegMin; /* Byte 16: 150 157 = 0x9D = 0x039D = 0925 */ uint16_t longitudeSec; /* Byte 18: 056 144 = 0x90 0x2490 = 9360*/ uint16_t distance; /* Byte 20: 027 123 = Entfernung/distance low byte 6 = 6 m */ uint16_t altitude; /* Byte 22: 243 244 = Höhe/Altitude low byte 500 = 0m */ uint16_t climbrate; /* Byte 24: 48 = Low Byte m/s resolution 0.01m 48 = 30000 = 0.00m/s (1=0.01m/s) */ uint8_t climbrate3s; /* Byte 26: climbrate in m/3s resolution, value of 120 = 0 m/3s*/ uint8_t GPSNumSat; /* Byte 27: GPS.Satelites (number of satelites) (1 byte) */ uint8_t GPSFixChar; /* Byte 28: GPS.FixChar. (GPS fix character. display, if DGPS, 2D oder 3D) (1 byte) */ uint8_t homeDirection; /* Byte 29: HomeDirection (direction from starting point to Model position) (1 byte) */ uint8_t angleXdirection; /* Byte 30: angle x-direction (1 byte) */ uint8_t angleYdirection; /* Byte 31: angle y-direction (1 byte) */ uint8_t angleZdirection; /* Byte 32: angle z-direction (1 byte) */ uint8_t gps_time_h; //#33 UTC time hours uint8_t gps_time_m; //#34 UTC time minutes uint8_t gps_time_s; //#35 UTC time seconds uint8_t gps_time_sss; //#36 UTC time milliseconds uint16_t msl_altitude; //#37 mean sea level altitude uint8_t vibration; /* Byte 39: vibration (1 bytes) */ uint8_t Ascii4; /* Byte 40: 00 ASCII Free Character [4] appears right to home distance */ uint8_t Ascii5; /* Byte 41: 00 ASCII Free Character [5] appears right to home direction*/ uint8_t GPS_fix; /* Byte 42: 00 ASCII Free Character [6], we use it for GPS FIX */ uint8_t version; /* Byte 43: 00 version number */ uint8_t endByte; /* Byte 44: 0x7D Ende byte */ uint8_t checksum; /* Byte 45: Parity Byte */ } __attribute__((packed)) hott_sensor_gps_t; typedef struct hott_text_msg_t { uint8_t start_byte; //#01 Starting constant value == 0x7b uint8_t esc; //#02 Escape (higher-ranking menu in text mode or Text mode leave) // 0x00 to stay normal // 0x01 to exit // I will send 2 times, so the ESCAPE works really well, so two data frames with 0x01 in byte 2 uint8_t warning_beeps; //#03 1=A 2=B ... char text[8][21]; //#04...#171 168 ASCII text to display to // Bit 7 = 1 -> Inverse character display // Display 21x8 uint8_t stop_byte; //#172 constant value 0x7d uint8_t checksum; //#173 Checksum / parity } __attribute__((packed)) hott_text_msg_t; typedef struct hott_sensors_t { bool is_enabled[4]; float *gps[HOTT_GPS_COUNT]; float *vario[HOTT_VARIO_COUNT]; float *esc[HOTT_ESC_COUNT]; float *general_air[HOTT_GENERAL_COUNT]; } hott_sensors_t; typedef struct trigger_t { float value; float max; float incr; char str[21]; } trigger_t; typedef enum gps_triggers_t { TRIGGER_GPS_MIN_SPEED, TRIGGER_GPS_MAX_SPEED, TRIGGER_GPS_MIN_ALTITUDE, TRIGGER_GPS_MAX_ALTITUDE, TRIGGER_GPS_MAX_CLIMB, TRIGGER_GPS_MIN_SATS, TRIGGER_GPS_MAX_DISTANCE, TRIGGERS_GPS } gps_triggers_t; typedef enum vario_triggers_t { TRIGGER_VARIO_MIN_ALTITUDE, TRIGGER_VARIO_MAX_ALTITUDE, TRIGGER_VARIO_VSPD, TRIGGERS_VARIO } vario_triggers_t; typedef enum esc_triggers_t { TRIGGER_ESC_CONSUMPTION, TRIGGER_ESC_TEMPERATURE, TRIGGER_ESC_MIN_RPM, TRIGGER_ESC_MAX_RPM, TRIGGER_ESC_VOLTAGE, TRIGGER_ESC_CURRENT, TRIGGERS_ESC } esc_triggers_t; typedef enum general_triggers_t { TRIGGER_GENERAL_BATTERY, TRIGGER_GENERAL_CAPACITY, TRIGGER_GENERAL_TEMPERATURE, TRIGGER_GENERAL_MIN_ALTITUDE, TRIGGER_GENERAL_MAX_ALTITUDE, TRIGGER_GENERAL_CURRENT, TRIGGERS_GENERAL } general_triggers_t; typedef struct triggers_value_t { float gps[TRIGGERS_GPS]; float vario[TRIGGERS_VARIO]; float esc[TRIGGERS_ESC]; float general[TRIGGERS_GENERAL]; } triggers_value_t; typedef struct triggers_menu_t { trigger_t gps[TRIGGERS_GPS]; trigger_t vario[TRIGGERS_VARIO]; trigger_t esc[TRIGGERS_ESC]; trigger_t general[TRIGGERS_GENERAL]; } triggers_menu_t; typedef struct triggers_t { triggers_value_t *triggers; triggers_menu_t *pages; } triggers_t; typedef struct vario_alarm_parameters_t { float *altitude; float *vspd; float m1s; float m3s; float m10s; } vario_alarm_parameters_t; vario_alarm_parameters_t vario_alarm_parameters; float *baro_temp = NULL, *baro_pressure = NULL; static void process(hott_sensors_t *sensors, triggers_t *alarms); static void format_binary_packet(triggers_t *alarms, hott_sensors_t *sensors, uint8_t address); static void format_text_packet(triggers_t *alarms, hott_sensors_t *sensors, uint8_t sensor_id, uint8_t key); static void send_packet(uint8_t *buffer, uint len); static void module_alarms_save(triggers_value_t *module_alarms); static triggers_value_t *alarms_read(void); static uint8_t get_crc(const uint8_t *buffer, uint len); static void set_config(hott_sensors_t *sensors); static int64_t interval_1000_callback(alarm_id_t id, void *parameters); static int64_t interval_3000_callback(alarm_id_t id, void *parameters); static int64_t interval_10000_callback(alarm_id_t id, void *parameters); void hott_task(void *parameters) { hott_sensors_t sensors = {0}; triggers_menu_t pages = {.gps = {{.max = 200, .incr = 1, .str = "Speed Min"}, {.max = 200, .incr = 1, .str = "Speed Max"}, {.max = 5000, .incr = 1, .str = "Alt Min"}, {.max = 5000, .incr = 1, .str = "Alt Max"}, {.max = 200, .incr = 1, .str = "Vspd Max"}, {.max = 20, .incr = 1, .str = "Sats Min"}, {.max = 20000, .incr = 10, .str = "Dist Max"}}, .vario = {{.max = 5000, .incr = 1, .str = "Alt Min"}, {.max = 5000, .incr = 1, .str = "Alt Max"}, {.max = 200, .incr = 1, .str = "Vspd Max"}}, .esc = {{.max = 30000, .incr = 10, .str = "Cons Max"}, {.max = 100, .incr = 1, .str = "Temp Max"}, {.max = 20000, .incr = 10, .str = "RPM Min"}, {.max = 20000, .incr = 10, .str = "RPM Max"}, {.max = 100, .incr = 0.1, .str = "Volt Min"}, {.max = 300, .incr = 1, .str = "Curr Max"}}, .general = {{.max = 100, .incr = 0.1, .str = "Volt Min"}, {.max = 100, .incr = 10, .str = "Cons Max"}, {.max = 100, .incr = 1, .str = "Temp Max"}, {.max = 5000, .incr = 1, .str = "Alt Min"}, {.max = 5000, .incr = 1, .str = "Alt Max"}, {.max = 300, .incr = 0.1, .str = "Curr Max"}}}; triggers_value_t triggers_value; memcpy(&triggers_value, (uint8_t *)alarms_read(), sizeof(triggers_value_t)); triggers_t hott_alarms = {.triggers = &triggers_value, .pages = &pages}; set_config(&sensors); context.led_cycle_duration = 6; context.led_cycles = 1; uart0_begin(19200, UART_RECEIVER_TX, UART_RECEIVER_RX, HOTT_TIMEOUT_US, 8, 1, UART_PARITY_NONE, false, true); debug("\nHOTT init"); while (1) { ulTaskNotifyTakeIndexed(1, pdTRUE, portMAX_DELAY); process(&sensors, &hott_alarms); } } static void process(hott_sensors_t *sensors, triggers_t *alarms) { uint8_t len = uart0_available(); if (len == HOTT_PACKET_LENGHT || len == HOTT_PACKET_LENGHT + 1) { uint8_t buffer[len]; uart0_read_bytes(buffer, len); debug("\nHOTT (%u) < ", uxTaskGetStackHighWaterMark(NULL)); debug_buffer(buffer, len, "0x%X "); if (buffer[0] == HOTT_BINARY_MODE_REQUEST_ID) format_binary_packet(alarms, sensors, buffer[1]); else if (buffer[0] == HOTT_TEXT_MODE_REQUEST_ID) format_text_packet(alarms, sensors, buffer[1] >> 4, buffer[1] & 0x0F); } } static void format_text_packet(triggers_t *alarms, hott_sensors_t *sensors, uint8_t sensor_id, uint8_t key) { static uint8_t item = 1; static uint8_t last_sensor = 0; static bool is_selected = false; hott_text_msg_t packet = {0}; packet.start_byte = 0x7B; packet.esc = 0x00; packet.warning_beeps = 0x00; uint size = 0; trigger_t *module_alarms_pages = NULL; float *module_alarms_triggers = NULL; if (sensor_id != last_sensor) { item = 1; is_selected = false; } last_sensor = sensor_id; // Select module alarms switch (0x80 | sensor_id) { case HOTT_GPS_MODULE_ID: if (!sensors->is_enabled[HOTT_TYPE_GPS]) return; size = TRIGGERS_GPS; module_alarms_pages = alarms->pages->gps; module_alarms_triggers = alarms->triggers->gps; strcpy(packet.text[0], "GPS sensor"); break; case HOTT_VARIO_MODULE_ID: if (!sensors->is_enabled[HOTT_TYPE_VARIO]) return; size = TRIGGERS_VARIO; module_alarms_pages = alarms->pages->vario; module_alarms_triggers = alarms->triggers->vario; strcpy(packet.text[0], "Vario sensor"); break; case HOTT_ESC_MODULE_ID: if (!sensors->is_enabled[HOTT_TYPE_ESC]) return; size = TRIGGERS_ESC; module_alarms_pages = alarms->pages->esc; module_alarms_triggers = alarms->triggers->esc; strcpy(packet.text[0], "ESC sensor"); break; case HOTT_GENERAL_AIR_MODULE_ID: if (!sensors->is_enabled[HOTT_TYPE_GENERAL]) return; size = TRIGGERS_GENERAL; module_alarms_pages = alarms->pages->general; module_alarms_triggers = alarms->triggers->general; strcpy(packet.text[0], "General sensor"); break; default: break; } if (!module_alarms_triggers) return; // Handle events if (key == HOTT_KEY_LEFT) { if (is_selected) { module_alarms_triggers[item] -= 10 * module_alarms_pages[item].incr; if (module_alarms_triggers[item] < 0) module_alarms_triggers[item] = 0; } else { packet.esc = 0x01; // exit strcat(packet.text[0], " (Exit)"); is_selected = false; item = 1; } } else if (key == HOTT_KEY_RIGHT && is_selected) { module_alarms_triggers[item] += 10 * module_alarms_pages[item].incr; if (module_alarms_triggers[item] > module_alarms_pages[item].max) module_alarms_triggers[item] = module_alarms_pages[item].max; } else if (key == HOTT_KEY_UP) { if (!is_selected) { item++; if (item > size - 1) item = size - 1; } else { module_alarms_triggers[item] += module_alarms_pages[item].incr; if (module_alarms_triggers[item] > module_alarms_pages[item].max) module_alarms_triggers[item] = module_alarms_pages[item].max; } } else if (key == HOTT_KEY_DOWN) { if (!is_selected) { item--; if (item < 0) item = 0; } else { module_alarms_triggers[item] -= module_alarms_pages[item].incr; if (module_alarms_triggers[item] < 0) module_alarms_triggers[item] = 0; } } else if (key == HOTT_KEY_SET) { if (is_selected) { module_alarms_save(alarms->triggers); strcat(packet.text[0], " (Saved)"); debug("\nHOTT (%u). Saved sensor config.", uxTaskGetStackHighWaterMark(NULL)); is_selected = false; } else { is_selected = true; } } else { debug("\nHOTT (%u). Unknown key 0x%X.", uxTaskGetStackHighWaterMark(NULL), key); } // Draw page // 123456789012345678901 // 12345 if (size > 7) size = 7; for (int i = 0; i < size; i++) { if (module_alarms_triggers[i] < 0) module_alarms_triggers[i] = 0; if (module_alarms_triggers[i] > module_alarms_pages[i].max) module_alarms_triggers[i] = module_alarms_pages[i].max; if (isinf(module_alarms_triggers[i])) module_alarms_triggers[i] = 0; if (isnan(module_alarms_triggers[i])) module_alarms_triggers[i] = 0; if (module_alarms_pages[i].incr == 1) snprintf(packet.text[i + 1], 21, " %-13s %5.0f", module_alarms_pages[i].str, module_alarms_triggers[i]); else snprintf(packet.text[i + 1], 21, " %-13s %5.1f", module_alarms_pages[i].str, module_alarms_triggers[i]); } packet.text[item + 1][0] |= '>'; if (is_selected) { for (int i = 16; i < 21; i++) packet.text[item + 1][i] |= 0x80; } // Send packet packet.stop_byte = 0x7D; packet.checksum = get_crc((uint8_t *)&packet, sizeof(packet) - 1); debug("\nHOTT (%u). Send sensor menu. Item %d Selected %s Key 0x%X Exit %d Value %.1f", uxTaskGetStackHighWaterMark(NULL), item, is_selected ? "Yes" : "No", key, packet.esc, module_alarms_triggers[item]); for (uint i = 0; i < size + 1; i++) { debug("\n%.21s", packet.text[i]); } send_packet((uint8_t *)&packet, sizeof(packet)); } static void format_binary_packet(triggers_t *alarms, hott_sensors_t *sensors, uint8_t address) { // packet in little endian switch (address) { case HOTT_VARIO_MODULE_ID: { static uint16_t max_altitude = 0, min_altitude = 0xFFFF; if (!sensors->is_enabled[HOTT_TYPE_VARIO]) return; hott_sensor_vario_t packet = {0}; packet.startByte = HOTT_START_BYTE; packet.sensorID = HOTT_VARIO_MODULE_ID; packet.sensorTextID = HOTT_VARIO_TEXT_ID; if (*sensors->vario[HOTT_VARIO_ALTITUDE] < alarms->triggers->vario[TRIGGER_VARIO_MIN_ALTITUDE]) { packet.warningID = ALARM_VOICE_MIN_ALTITUDE; packet.alarmInverse |= 1 << ALARM_BITMASK_VARIO_ALTITUDE; packet.alarmInverse |= 1 << ALARM_BITMASK_VARIO_MIN_ALTITUDE; } if (*sensors->vario[HOTT_VARIO_ALTITUDE] > alarms->triggers->vario[TRIGGER_VARIO_MAX_ALTITUDE]) { packet.warningID = ALARM_VOICE_MAX_ALTITUDE; packet.alarmInverse |= 1 << ALARM_BITMASK_VARIO_ALTITUDE; packet.alarmInverse |= 1 << ALARM_BITMASK_VARIO_MAX_ALTITUDE; } if (*sensors->vario[HOTT_VARIO_M1S] < alarms->triggers->vario[TRIGGER_VARIO_VSPD]) { packet.alarmInverse |= 1 << ALARM_BITMASK_VARIO_M1S; } packet.altitude = *sensors->vario[HOTT_VARIO_ALTITUDE] + 500; if (max_altitude < packet.altitude) max_altitude = packet.altitude; if (min_altitude > packet.altitude) min_altitude = packet.altitude; packet.maxAltitude = max_altitude; packet.minAltitude = min_altitude; packet.m1s = *sensors->vario[HOTT_VARIO_M1S] * 100 + 30000; packet.m3s = vario_alarm_parameters.m3s; packet.m10s = vario_alarm_parameters.m10s; packet.endByte = HOTT_END_BYTE; packet.checksum = get_crc((uint8_t *)&packet, sizeof(packet) - 1); send_packet((uint8_t *)&packet, sizeof(packet)); break; } case HOTT_ESC_MODULE_ID: { if (!sensors->is_enabled[HOTT_TYPE_ESC]) return; hott_sensor_airesc_t packet = {0}; static uint16_t minInputVolt = 0xFFFF; static uint8_t maxEscTemperature = 0; static uint16_t maxCurrent = 0; static uint16_t maxRPM = 0; static uint16_t maxSpeed = 0; static uint16_t minBECCurrent = 0xFFFF; static uint16_t maxBECCurrent = 0; static uint16_t minBECVoltage = 0xFFFF; static uint16_t maxBECTemperature = 0; static uint16_t maxMotorOrExtTemperature = 0; packet.startByte = HOTT_START_BYTE; packet.sensorID = HOTT_ESC_MODULE_ID; packet.sensorTextID = HOTT_ESC_TEXT_ID; if (sensors->esc[HOTT_ESC_VOLTAGE]) { packet.inputVolt = *sensors->esc[HOTT_ESC_VOLTAGE] * 10; if (packet.inputVolt < minInputVolt) packet.minInputVolt = packet.inputVolt; if (*sensors->esc[HOTT_ESC_VOLTAGE] < alarms->triggers->esc[TRIGGER_ESC_VOLTAGE]) { packet.warningID = ALARM_VOICE_MIN_POWER_VOLTAGE; packet.alarmInverse |= 1 << ALARM_BITMASK_AIRESC_MIN_VOLTAGE; packet.alarmInverse |= 1 << ALARM_BITMASK_AIRESC_VOLTAGE; } } if (sensors->esc[HOTT_ESC_CONSUMPTION]) { packet.capacity = *sensors->esc[HOTT_ESC_CONSUMPTION] / 10; if (*sensors->esc[HOTT_ESC_CONSUMPTION] > alarms->triggers->esc[TRIGGER_ESC_CONSUMPTION]) { packet.warningID = ALARM_VOICE_MAX_CAPACITY; packet.alarmInverse |= 1 << ALARM_BITMASK_AIRESC_CAPACITY; } } if (sensors->esc[HOTT_ESC_TEMPERATURE]) { packet.escTemperature = *sensors->esc[HOTT_ESC_TEMPERATURE] + 20; if (packet.escTemperature > maxEscTemperature) packet.maxEscTemperature = packet.escTemperature; if (*sensors->esc[HOTT_ESC_TEMPERATURE] > alarms->triggers->esc[TRIGGER_ESC_TEMPERATURE]) { packet.warningID = ALARM_VOICE_MAX_SENSOR_1_TEMP; packet.alarmInverse |= 1 << ALARM_BITMASK_AIRESC_TEMPERATURE; packet.alarmInverse |= 1 << ALARM_BITMASK_AIRESC_MAX_TEMPERATURE; } } else { packet.escTemperature = 20; } if (sensors->esc[HOTT_ESC_CURRENT]) { packet.current = *sensors->esc[HOTT_ESC_CURRENT] * 10; if (packet.current > maxCurrent) packet.maxCurrent = packet.current; if (*sensors->esc[HOTT_ESC_CURRENT] > alarms->triggers->esc[TRIGGER_ESC_CURRENT]) { packet.warningID = ALARM_VOICE_MAX_CURRENT; packet.alarmInverse |= 1 << ALARM_BITMASK_AIRESC_MAX_CURRENT; packet.alarmInverse |= 1 << ALARM_BITMASK_AIRESC_CURRENT; } } if (sensors->esc[HOTT_ESC_RPM]) { packet.RPM = *sensors->esc[HOTT_ESC_RPM] / 10; if (packet.RPM > maxRPM) packet.maxRPM = packet.RPM; if (*sensors->esc[HOTT_ESC_RPM] < alarms->triggers->esc[TRIGGER_ESC_MIN_RPM]) { packet.warningID = ALARM_VOICE_MIN_RPM; packet.alarmInverse |= 1 << ALARM_BITMASK_AIRESC_RPM; } if (*sensors->esc[HOTT_ESC_RPM] > alarms->triggers->esc[TRIGGER_ESC_MAX_RPM]) { packet.warningID = ALARM_VOICE_MAX_RPM; packet.alarmInverse |= 1 << ALARM_BITMASK_AIRESC_RPM; packet.alarmInverse |= 1 << ALARM_BITMASK_AIRESC_MAX_RPM; } } // uint8_t throttlePercent; // Byte 22 if (sensors->esc[HOTT_ESC_SPEED]) { packet.speed = *sensors->esc[HOTT_ESC_SPEED]; if (packet.speed > maxSpeed) packet.maxSpeed = packet.speed; } if (sensors->esc[HOTT_ESC_BEC_VOLTAGE]) { packet.BECVoltage = *sensors->esc[HOTT_ESC_BEC_VOLTAGE] * 10; if (packet.minBECVoltage < minBECVoltage) packet.minBECVoltage = packet.BECVoltage; } if (sensors->esc[HOTT_ESC_BEC_CURRENT]) { packet.BECCurrent = *sensors->esc[HOTT_ESC_BEC_CURRENT] * 10; if (packet.BECCurrent < minBECCurrent) packet.minBECCurrent = packet.BECCurrent; if (packet.BECCurrent > maxBECCurrent) packet.maxBECCurrent = packet.BECCurrent; } // uint8_t PWM; // Byte 32 if (sensors->esc[HOTT_ESC_BEC_TEMPERATURE]) { packet.BECTemperature = *sensors->esc[HOTT_ESC_BEC_TEMPERATURE] + 20; if (packet.BECTemperature > maxBECTemperature) packet.maxBECTemperature = packet.BECTemperature; } if (sensors->esc[HOTT_ESC_EXT_TEMPERATURE]) { packet.motorOrExtTemperature = *sensors->esc[HOTT_ESC_EXT_TEMPERATURE] + 20; if (packet.motorOrExtTemperature > maxMotorOrExtTemperature) packet.maxMotorOrExtTemperature = packet.motorOrExtTemperature; } // uint16_t RPMWithoutGearOrExt; // Byte 37 // uint8_t timing; // Byte 39 // uint8_t advancedTiming; // Byte 40 // uint8_t highestCurrentMotorNumber; // Byte 41 packet.endByte = HOTT_END_BYTE; packet.checksum = get_crc((uint8_t *)&packet, sizeof(packet) - 1); send_packet((uint8_t *)&packet, sizeof(packet)); break; } case HOTT_GENERAL_AIR_MODULE_ID: { if (!sensors->is_enabled[HOTT_TYPE_GENERAL]) return; hott_sensor_general_air_t packet = {0}; packet.startByte = HOTT_START_BYTE; packet.sensorID = HOTT_GENERAL_AIR_MODULE_ID; packet.sensorTextID = HOTT_GENERAL_AIR_TEXT_ID; if (sensors->general_air[HOTT_GENERAL_BATTERY_1]) { packet.battery1 = *sensors->general_air[HOTT_GENERAL_BATTERY_1] * 10; if (*sensors->general_air[HOTT_GENERAL_BATTERY_1] < alarms->triggers->general[TRIGGER_GENERAL_BATTERY]) { packet.warningID = ALARM_VOICE_MIN_POWER_VOLTAGE; packet.alarmInverse |= 1 << ALARM_BITMASK_GENERAL_AIR_BATTERY_1; } } if (sensors->general_air[HOTT_GENERAL_CURRENT]) { packet.current = *sensors->general_air[HOTT_GENERAL_CURRENT] * 10; if (*sensors->general_air[HOTT_GENERAL_CURRENT] < alarms->triggers->general[TRIGGER_GENERAL_CURRENT]) { packet.warningID = ALARM_VOICE_MAX_CURRENT; packet.alarmInverse |= 1 << ALARM_BITMASK_GENERAL_AIR_CURRENT; } } if (sensors->general_air[HOTT_GENERAL_CAPACITY]) { packet.batt_cap = *sensors->general_air[HOTT_GENERAL_CAPACITY] / 10; if (*sensors->general_air[HOTT_GENERAL_CAPACITY] > alarms->triggers->general[TRIGGER_GENERAL_CAPACITY]) { packet.warningID = ALARM_VOICE_MAX_CAPACITY; packet.alarmInverse |= 1 << ALARM_BITMASK_GENERAL_AIR_CAPACITY; } } if (sensors->general_air[HOTT_GENERAL_PRESSURE]) { packet.pressure = *sensors->general_air[HOTT_GENERAL_PRESSURE] * 1e-5 * 10; // Pa -> bar (in steps of 0.1 bar) } if (sensors->general_air[HOTT_GENERAL_ALTITUDE]) { if (*sensors->vario[HOTT_GENERAL_ALTITUDE] < alarms->triggers->vario[TRIGGER_GENERAL_MIN_ALTITUDE]) { packet.warningID = ALARM_VOICE_MIN_ALTITUDE; packet.alarmInverse |= 1 << ALARM_BITMASK_GENERAL_AIR_ALTITUDE; } if (*sensors->vario[HOTT_GENERAL_ALTITUDE] > alarms->triggers->vario[TRIGGER_GENERAL_MAX_ALTITUDE]) { packet.warningID = ALARM_VOICE_MAX_ALTITUDE; packet.alarmInverse |= 1 << ALARM_BITMASK_GENERAL_AIR_ALTITUDE; } packet.altitude = *sensors->general_air[HOTT_GENERAL_ALTITUDE] + 500; } if (sensors->general_air[HOTT_GENERAL_CLIMBRATE]) { packet.climbrate = *sensors->general_air[HOTT_GENERAL_CLIMBRATE] * 100 + 30000; } if (sensors->general_air[HOTT_GENERAL_CELL_1]) { packet.cell[0] = *sensors->general_air[HOTT_GENERAL_CELL_1] * 50; } if (sensors->general_air[HOTT_GENERAL_CELL_2]) { packet.cell[1] = *sensors->general_air[HOTT_GENERAL_CELL_2] * 50; } if (sensors->general_air[HOTT_GENERAL_CELL_3]) { packet.cell[2] = *sensors->general_air[HOTT_GENERAL_CELL_3] * 50; } if (sensors->general_air[HOTT_GENERAL_CELL_4]) { packet.cell[3] = *sensors->general_air[HOTT_GENERAL_CELL_4] * 50; } if (sensors->general_air[HOTT_GENERAL_CELL_5]) { packet.cell[4] = *sensors->general_air[HOTT_GENERAL_CELL_5] * 50; } if (sensors->general_air[HOTT_GENERAL_CELL_6]) { packet.cell[5] = *sensors->general_air[HOTT_GENERAL_CELL_6] * 50; } if (sensors->general_air[HOTT_GENERAL_TEMP_1]) { packet.temperature1 = *sensors->general_air[HOTT_GENERAL_TEMP_1] + 20; if (*sensors->general_air[HOTT_GENERAL_TEMP_1] > alarms->triggers->general[TRIGGER_GENERAL_TEMPERATURE]) { packet.warningID = ALARM_VOICE_MAX_SENSOR_1_TEMP; packet.alarmInverse |= 1 << ALARM_BITMASK_GENERAL_AIR_TEMPERATURE_1; } } else { packet.temperature1 = 20; } if (sensors->general_air[HOTT_GENERAL_TEMP_2]) { packet.temperature2 = *sensors->general_air[HOTT_GENERAL_TEMP_2] + 20; } else { packet.temperature2 = 20; } packet.endByte = HOTT_END_BYTE; packet.checksum = get_crc((uint8_t *)&packet, sizeof(packet) - 1); send_packet((uint8_t *)&packet, sizeof(packet)); break; } case HOTT_GPS_MODULE_ID: { if (!sensors->is_enabled[HOTT_TYPE_GPS]) return; hott_sensor_gps_t packet = {0}; if (*sensors->vario[HOTT_GPS_SPEED] < alarms->triggers->vario[TRIGGER_GPS_MIN_SPEED]) { packet.alarmInverse |= 1 << ALARM_BITMASK_GPS_SPEED; } if (*sensors->vario[HOTT_GPS_SPEED] > alarms->triggers->vario[TRIGGER_GPS_MAX_SPEED]) { packet.alarmInverse |= 1 << ALARM_BITMASK_GPS_SPEED; } if (*sensors->vario[HOTT_GPS_ALTITUDE] < alarms->triggers->vario[TRIGGER_GPS_MIN_ALTITUDE]) { packet.warningID = ALARM_VOICE_MIN_ALTITUDE; packet.alarmInverse |= 1 << ALARM_BITMASK_GPS_ALTITUDE; } if (*sensors->vario[HOTT_GPS_ALTITUDE] > alarms->triggers->vario[TRIGGER_GPS_MAX_ALTITUDE]) { packet.warningID = ALARM_VOICE_MAX_ALTITUDE; packet.alarmInverse |= 1 << ALARM_BITMASK_GPS_ALTITUDE; } if (*sensors->vario[HOTT_GPS_CLIMBRATE] < alarms->triggers->vario[TRIGGER_GPS_MAX_CLIMB]) { packet.alarmInverse |= 1 << ALARM_BITMASK_GPS_CLIMBRATE; } if (*sensors->vario[HOTT_GPS_SATS] < alarms->triggers->vario[TRIGGER_GPS_MIN_SATS]) { packet.alarmInverse |= 1 << ALARM_BITMASK_GPS_SATS; } packet.startByte = HOTT_START_BYTE; packet.sensorID = HOTT_GPS_MODULE_ID; packet.sensorTextID = HOTT_GPS_TEXT_ID; packet.flightDirection = *sensors->gps[HOTT_GPS_DIRECTION] / 2; // 2° packet.GPSSpeed = *sensors->gps[HOTT_GPS_SPEED]; // km/h packet.LatitudeNS = (*sensors->gps[HOTT_GPS_LATITUDE] < 0) ? 1 : 0; { float lat = fabsf(*sensors->gps[HOTT_GPS_LATITUDE]); uint16_t deg = (uint16_t)lat; float min_f = (lat - (float)deg) * 60.0f; uint16_t min_i = (uint16_t)min_f; /* HoTT: DDMM */ packet.LatitudeDegMin = (uint16_t)(deg * 100u + min_i); /* HoTT: minutes fraction * 10000 */ packet.LatitudeSec = (uint16_t)lroundf((min_f - (float)min_i) * 10000.0f); } packet.longitudeEW = (*sensors->gps[HOTT_GPS_LONGITUDE] < 0) ? 1 : 0; { float lon = fabsf(*sensors->gps[HOTT_GPS_LONGITUDE]); uint16_t deg = (uint16_t)lon; float min_f = (lon - (float)deg) * 60.0f; uint16_t min_i = (uint16_t)min_f; /* HoTT: DDMM */ packet.longitudeDegMin = (uint16_t)(deg * 100u + min_i); /* HoTT: minutes fraction * 10000 */ packet.longitudeSec = (uint16_t)lroundf((min_f - (float)min_i) * 10000.0f); } packet.distance = *sensors->gps[HOTT_GPS_DISTANCE]; float climbrate = *sensors->gps[HOTT_GPS_CLIMBRATE] * 100 + 30000; if (climbrate < 0) climbrate = 0; packet.climbrate = climbrate; // 30000, 0.00 // packet.climbrate3s = *sensors->gps[HOTT_GPS_ALTITUDE]; // 120, 0 packet.GPSNumSat = *sensors->gps[HOTT_GPS_SATS] == 0 ? 0xFF : *sensors->gps[HOTT_GPS_SATS]; uint8_t fix_type = *sensors->gps[HOTT_GPS_FIX]; if (fix_type == 0) packet.GPSFixChar = ' '; else if (fix_type == 1) packet.GPSFixChar = '2'; else packet.GPSFixChar = '3'; // (1 byte) uint8_t homeDirection; // Byte 29: HomeDirection (direction from starting point to Model // position) (1 byte) uint hour = (uint)(*sensors->gps[HOTT_GPS_TIME]) / 10000; uint min = (uint)(*sensors->gps[HOTT_GPS_TIME]) / 100 - hour * 100; uint sec = (uint)(*sensors->gps[HOTT_GPS_TIME]) - hour * 10000 - min * 100; packet.gps_time_h = hour; packet.gps_time_m = min; packet.gps_time_s = sec; // uint8_t gps_time_sss;//#36 UTC time milliseconds if (*sensors->gps[HOTT_GPS_ALTITUDE] < 0) packet.msl_altitude = 0; else packet.msl_altitude = *sensors->gps[HOTT_GPS_ALTITUDE]; if (*sensors->gps[HOTT_GPS_ALTITUDE] - *sensors->gps[HOTT_GPS_HOME_ALTITUDE] + 500 < 0) packet.altitude = 0; else packet.altitude = *sensors->gps[HOTT_GPS_ALTITUDE] - *sensors->gps[HOTT_GPS_HOME_ALTITUDE] + 500; // uint8_t vibration; // Byte 39 vibrations level in % // uint8_t Ascii4; // Byte 40: 00 ASCII Free Character [4] appears right to home distance // uint8_t Ascii5; // Byte 41: 00 ASCII Free Character [5] appears right to home direction packet.GPS_fix = (fix_type == 0) ? ' ' : (fix_type == 1) ? '2' : '3'; // uint8_t version; // Byte 43: 00 version number packet.endByte = HOTT_END_BYTE; packet.checksum = get_crc((uint8_t *)&packet, sizeof(packet) - 1); send_packet((uint8_t *)&packet, sizeof(packet)); break; } } } static void send_packet(uint8_t *buffer, uint len) { for (uint i = 0; i < len; i++) { uart0_write(*(buffer + i)); sleep_us(HOTT_INTERBYTE_DELAY_US); } vTaskResume(context.led_task_handle); if (len < 100) { debug("\nHOTT (%u) %u > ", uxTaskGetStackHighWaterMark(NULL), len); debug_buffer(buffer, len, "0x%X "); } } static triggers_value_t *alarms_read(void) { triggers_value_t *alarms = (triggers_value_t *)(XIP_BASE + ALARMS_FLASH_TARGET_OFFSET); return (triggers_value_t *)(XIP_BASE + ALARMS_FLASH_TARGET_OFFSET); } static void module_alarms_save(triggers_value_t *triggers_value) { uint8_t flash[FLASH_PAGE_SIZE] = {0}; memcpy(flash, (uint8_t *)triggers_value, sizeof(triggers_value_t)); uint32_t ints = save_and_disable_interrupts(); flash_range_erase(ALARMS_FLASH_TARGET_OFFSET, FLASH_SECTOR_SIZE); flash_range_program(ALARMS_FLASH_TARGET_OFFSET, flash, sizeof(triggers_value_t)); restore_interrupts(ints); debug("\nHOTT Alarms saved"); } static uint8_t get_crc(const uint8_t *buffer, uint len) { uint16_t crc = 0; for (uint i = 0; i < len; i++) { crc += buffer[i]; } // debug("\n>CRC: 0x%X", crc & 0xFF); return crc; } static void set_config(hott_sensors_t *sensors) { config_t *config = config_read(); TaskHandle_t task_handle; if (config->esc_protocol == ESC_PWM) { esc_pwm_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, malloc(sizeof(float))}; xTaskCreate(esc_pwm_task, "esc_pwm_task", STACK_ESC_PWM, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); sensors->is_enabled[HOTT_TYPE_ESC] = true; sensors->esc[HOTT_ESC_RPM] = parameter.rpm; } if (config->esc_protocol == ESC_HW3) { esc_hw3_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, malloc(sizeof(float))}; xTaskCreate(esc_hw3_task, "esc_hw3_task", STACK_ESC_HW3, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); sensors->is_enabled[HOTT_TYPE_ESC] = true; sensors->esc[HOTT_ESC_RPM] = parameter.rpm; } if (config->esc_protocol == ESC_HW4) { esc_hw4_parameters_t parameter = {config->rpm_multiplier, config->enable_pwm_out, config->enable_esc_hw4_init_delay, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, config->esc_hw4_voltage_multiplier, config->esc_hw4_current_multiplier, config->esc_hw4_current_thresold, config->esc_hw4_current_max, config->esc_hw4_is_manual_offset, config->esc_hw4_auto_detect, config->esc_hw4_offset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_hw4_task, "esc_hw4_task", STACK_ESC_HW4, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); if (config->enable_pwm_out) { xTaskCreate(pwm_out_task, "pwm_out", STACK_PWM_OUT, (void *)parameter.rpm, 2, &task_handle); context.pwm_out_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } sensors->is_enabled[HOTT_TYPE_ESC] = true; sensors->esc[HOTT_ESC_RPM] = parameter.rpm; sensors->esc[HOTT_ESC_TEMPERATURE] = parameter.temperature_fet; sensors->esc[HOTT_ESC_BEC_TEMPERATURE] = parameter.temperature_bec; sensors->esc[HOTT_ESC_VOLTAGE] = parameter.voltage; sensors->esc[HOTT_ESC_CURRENT] = parameter.current; sensors->esc[HOTT_ESC_CONSUMPTION] = parameter.consumption; } if (config->esc_protocol == ESC_HW5) { esc_hw5_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_hw5_task, "esc_hw5_task", STACK_ESC_HW5, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); sensors->is_enabled[HOTT_TYPE_ESC] = true; sensors->esc[HOTT_ESC_RPM] = parameter.rpm; sensors->esc[HOTT_ESC_TEMPERATURE] = parameter.temperature_fet; sensors->esc[HOTT_ESC_BEC_TEMPERATURE] = parameter.temperature_bec; sensors->esc[HOTT_ESC_VOLTAGE] = parameter.voltage; sensors->esc[HOTT_ESC_BEC_VOLTAGE] = parameter.voltage_bec; sensors->esc[HOTT_ESC_CURRENT] = parameter.current; sensors->esc[HOTT_ESC_BEC_CURRENT] = parameter.current_bec; sensors->esc[HOTT_ESC_CONSUMPTION] = parameter.consumption; sensors->esc[HOTT_ESC_EXT_TEMPERATURE] = parameter.temperature_motor; } if (config->esc_protocol == ESC_CASTLE) { esc_castle_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_castle_task, "esc_castle_task", STACK_ESC_CASTLE, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); sensors->is_enabled[HOTT_TYPE_ESC] = true; sensors->esc[HOTT_ESC_RPM] = parameter.rpm; sensors->esc[HOTT_ESC_TEMPERATURE] = parameter.temperature; sensors->esc[HOTT_ESC_VOLTAGE] = parameter.voltage; sensors->esc[HOTT_ESC_BEC_VOLTAGE] = parameter.voltage_bec; sensors->esc[HOTT_ESC_CURRENT] = parameter.current; sensors->esc[HOTT_ESC_BEC_CURRENT] = parameter.current_bec; sensors->esc[HOTT_ESC_CONSUMPTION] = parameter.consumption; sensors->esc[HOTT_ESC_EXT_TEMPERATURE] = parameter.consumption; } if (config->esc_protocol == ESC_KONTRONIK) { esc_kontronik_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_kontronik_task, "esc_kontronik_task", STACK_ESC_KONTRONIK, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); sensors->is_enabled[HOTT_TYPE_ESC] = true; sensors->esc[HOTT_ESC_RPM] = parameter.rpm; sensors->esc[HOTT_ESC_TEMPERATURE] = parameter.temperature_fet; sensors->esc[HOTT_ESC_BEC_TEMPERATURE] = parameter.temperature_bec; sensors->esc[HOTT_ESC_VOLTAGE] = parameter.voltage; sensors->esc[HOTT_ESC_BEC_VOLTAGE] = parameter.voltage_bec; sensors->esc[HOTT_ESC_CURRENT] = parameter.current; sensors->esc[HOTT_ESC_BEC_CURRENT] = parameter.current_bec; sensors->esc[HOTT_ESC_CONSUMPTION] = parameter.consumption; } if (config->esc_protocol == ESC_APD_F) { esc_apd_f_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_apd_f_task, "esc_apd_f_task", STACK_ESC_APD_F, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); sensors->is_enabled[HOTT_TYPE_ESC] = true; sensors->esc[HOTT_ESC_RPM] = parameter.rpm; sensors->esc[HOTT_ESC_TEMPERATURE] = parameter.temperature; sensors->esc[HOTT_ESC_VOLTAGE] = parameter.voltage; sensors->esc[HOTT_ESC_CURRENT] = parameter.current; sensors->esc[HOTT_ESC_CONSUMPTION] = parameter.consumption; } if (config->esc_protocol == ESC_APD_HV) { esc_apd_hv_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_apd_hv_task, "esc_apd_hv_task", STACK_ESC_APD_HV, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); sensors->is_enabled[HOTT_TYPE_ESC] = true; sensors->esc[HOTT_ESC_RPM] = parameter.rpm; sensors->esc[HOTT_ESC_TEMPERATURE] = parameter.temperature; sensors->esc[HOTT_ESC_VOLTAGE] = parameter.voltage; sensors->esc[HOTT_ESC_CURRENT] = parameter.current; sensors->esc[HOTT_ESC_CONSUMPTION] = parameter.consumption; } if (config->esc_protocol == ESC_SMART) { smart_esc_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temperature_fet = malloc(sizeof(float)); parameter.temperature_bec = malloc(sizeof(float)); parameter.voltage_bec = malloc(sizeof(float)); parameter.current_bec = malloc(sizeof(float)); parameter.temperature_bat = malloc(sizeof(float)); parameter.current_bat = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); for (uint i = 0; i < 18; i++) parameter.cell[i] = malloc(sizeof(float)); parameter.cells = malloc(sizeof(uint8_t)); parameter.cycles = malloc(sizeof(uint16_t)); xTaskCreate(smart_esc_task, "smart_esc_task", STACK_SMART_ESC, (void *)¶meter, 4, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); sensors->is_enabled[HOTT_TYPE_ESC] = true; sensors->esc[HOTT_ESC_RPM] = parameter.rpm; sensors->esc[HOTT_ESC_TEMPERATURE] = parameter.temperature_fet; sensors->esc[HOTT_ESC_BEC_TEMPERATURE] = parameter.temperature_bec; sensors->esc[HOTT_ESC_VOLTAGE] = parameter.voltage; sensors->esc[HOTT_ESC_CURRENT] = parameter.current; sensors->esc[HOTT_ESC_BEC_VOLTAGE] = parameter.voltage_bec; sensors->esc[HOTT_ESC_BEC_CURRENT] = parameter.current_bec; sensors->is_enabled[HOTT_TYPE_GENERAL] = true; sensors->general_air[HOTT_GENERAL_TEMP_1] = parameter.temperature_bat; sensors->general_air[HOTT_GENERAL_CURRENT] = parameter.current_bat; sensors->general_air[HOTT_GENERAL_CAPACITY] = parameter.consumption; sensors->general_air[HOTT_GENERAL_CELL_1] = parameter.cell[0]; sensors->general_air[HOTT_GENERAL_CELL_2] = parameter.cell[1]; sensors->general_air[HOTT_GENERAL_CELL_3] = parameter.cell[2]; sensors->general_air[HOTT_GENERAL_CELL_4] = parameter.cell[3]; sensors->general_air[HOTT_GENERAL_CELL_5] = parameter.cell[4]; sensors->general_air[HOTT_GENERAL_CELL_6] = parameter.cell[5]; } if (config->esc_protocol == ESC_OMP_M4) { esc_omp_m4_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temp_esc = malloc(sizeof(float)); parameter.temp_motor = malloc(sizeof(float)); parameter.cell_voltage = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); parameter.cell_count = malloc(sizeof(uint8_t)); xTaskCreate(esc_omp_m4_task, "esc_omp_m4_task", STACK_SMART_ESC, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); sensors->is_enabled[HOTT_TYPE_ESC] = true; sensors->esc[HOTT_ESC_RPM] = parameter.rpm; sensors->esc[HOTT_ESC_TEMPERATURE] = parameter.temp_esc; sensors->esc[HOTT_ESC_VOLTAGE] = parameter.voltage; sensors->esc[HOTT_ESC_CURRENT] = parameter.current; sensors->esc[HOTT_ESC_CONSUMPTION] = parameter.consumption; sensors->esc[HOTT_ESC_EXT_TEMPERATURE] = parameter.temp_motor; } if (config->esc_protocol == ESC_ZTW) { esc_ztw_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temp_esc = malloc(sizeof(float)); parameter.temp_motor = malloc(sizeof(float)); parameter.bec_voltage = malloc(sizeof(float)); parameter.cell_voltage = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); parameter.cell_count = malloc(sizeof(uint8_t)); xTaskCreate(esc_omp_m4_task, "esc_ztw_task", STACK_ESC_ZTW, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); sensors->is_enabled[HOTT_TYPE_ESC] = true; sensors->esc[HOTT_ESC_RPM] = parameter.rpm; sensors->esc[HOTT_ESC_TEMPERATURE] = parameter.temp_esc; sensors->esc[HOTT_ESC_VOLTAGE] = parameter.voltage; sensors->esc[HOTT_ESC_CURRENT] = parameter.current; sensors->esc[HOTT_ESC_CONSUMPTION] = parameter.consumption; sensors->esc[HOTT_ESC_EXT_TEMPERATURE] = parameter.temp_motor; } if (config->enable_gps) { gps_parameters_t parameter; parameter.protocol = config->gps_protocol; parameter.baudrate = config->gps_baudrate; parameter.rate = config->gps_rate; parameter.lat = malloc(sizeof(double)); parameter.lon = malloc(sizeof(double)); parameter.alt = malloc(sizeof(float)); parameter.spd = malloc(sizeof(float)); parameter.cog = malloc(sizeof(float)); parameter.hdop = malloc(sizeof(float)); parameter.sat = malloc(sizeof(float)); parameter.time = malloc(sizeof(float)); parameter.date = malloc(sizeof(float)); parameter.vspeed = malloc(sizeof(float)); parameter.dist = malloc(sizeof(float)); parameter.spd_kmh = malloc(sizeof(float)); parameter.fix = malloc(sizeof(float)); parameter.vdop = malloc(sizeof(float)); parameter.speed_acc = malloc(sizeof(float)); parameter.h_acc = malloc(sizeof(float)); parameter.v_acc = malloc(sizeof(float)); parameter.track_acc = malloc(sizeof(float)); parameter.n_vel = malloc(sizeof(float)); parameter.e_vel = malloc(sizeof(float)); parameter.v_vel = malloc(sizeof(float)); parameter.alt_elipsiod = malloc(sizeof(float)); parameter.pdop = malloc(sizeof(float)); parameter.fix_type = malloc(sizeof(uint8_t)); parameter.home_set = malloc(sizeof(uint8_t)); parameter.alt_home = malloc(sizeof(float)); xTaskCreate(gps_task, "gps_task", STACK_GPS, (void *)¶meter, 2, &task_handle); context.uart_pio_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); sensors->is_enabled[HOTT_TYPE_GPS] = true; sensors->gps[HOTT_GPS_LATITUDE] = parameter.lat; sensors->gps[HOTT_GPS_LONGITUDE] = parameter.lon; sensors->gps[HOTT_GPS_SATS] = parameter.sat; sensors->gps[HOTT_GPS_FIX] = parameter.fix; sensors->gps[HOTT_GPS_ALTITUDE] = parameter.alt; sensors->gps[HOTT_GPS_SPEED] = parameter.spd_kmh; sensors->gps[HOTT_GPS_DIRECTION] = parameter.cog; sensors->gps[HOTT_GPS_DISTANCE] = parameter.dist; sensors->gps[HOTT_GPS_CLIMBRATE] = parameter.vspeed; sensors->gps[HOTT_GPS_TIME] = parameter.time; sensors->gps[HOTT_GPS_HOME_ALTITUDE] = parameter.alt_home; } if (config->enable_analog_voltage) { voltage_parameters_t parameter = {0, config->analog_rate, config->alpha_voltage, config->analog_voltage_multiplier, malloc(sizeof(float))}; xTaskCreate(voltage_task, "voltage_task", STACK_VOLTAGE, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); sensors->is_enabled[HOTT_TYPE_GENERAL] = true; sensors->general_air[HOTT_GENERAL_BATTERY_1] = parameter.voltage; } if (config->enable_analog_current) { current_parameters_t parameter = {1, config->analog_rate, config->alpha_current, config->analog_current_multiplier, config->analog_current_offset, config->analog_current_autoffset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(current_task, "current_task", STACK_CURRENT, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); sensors->is_enabled[HOTT_TYPE_GENERAL] = true; sensors->general_air[HOTT_GENERAL_CURRENT] = parameter.current; sensors->general_air[HOTT_GENERAL_CAPACITY] = parameter.consumption; } if (config->enable_analog_ntc) { ntc_parameters_t parameter = {2, config->analog_rate, config->ntc_offset, config->alpha_temperature, malloc(sizeof(float))}; xTaskCreate(ntc_task, "ntc_task", STACK_NTC, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); sensors->is_enabled[HOTT_TYPE_GENERAL] = true; sensors->general_air[HOTT_GENERAL_TEMP_1] = parameter.ntc; } if (config->enable_analog_airspeed) { airspeed_parameters_t parameter = {3, config->analog_rate, config->alpha_airspeed, (float)config->airspeed_offset / 1000, (float)config->airspeed_vcc / 100, baro_temp, baro_pressure, malloc(sizeof(float))}; xTaskCreate(airspeed_task, "airspeed_task", STACK_AIRSPEED, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); sensors->is_enabled[HOTT_TYPE_ESC] = true; sensors->esc[HOTT_ESC_SPEED] = parameter.airspeed; } if (config->i2c_module == I2C_BMP280) { bmp280_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, 0, config->bmp280_filter, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(bmp280_task, "bmp280_task", STACK_BMP280, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } sensors->is_enabled[HOTT_TYPE_VARIO] = true; sensors->vario[HOTT_VARIO_ALTITUDE] = parameter.altitude; sensors->vario[HOTT_VARIO_M1S] = parameter.vspeed; sensors->is_enabled[HOTT_TYPE_GENERAL] = true; sensors->general_air[HOTT_GENERAL_ALTITUDE] = parameter.altitude; sensors->general_air[HOTT_GENERAL_CLIMBRATE] = parameter.vspeed; vario_alarm_parameters.altitude = parameter.altitude; add_alarm_in_ms(1000, interval_1000_callback, &vario_alarm_parameters, false); add_alarm_in_ms(3000, interval_3000_callback, &vario_alarm_parameters, false); add_alarm_in_ms(10000, interval_10000_callback, &vario_alarm_parameters, false); } if (config->i2c_module == I2C_MS5611) { ms5611_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, 0, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(ms5611_task, "ms5611_task", STACK_MS5611, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } sensors->is_enabled[HOTT_TYPE_VARIO] = true; sensors->vario[HOTT_VARIO_ALTITUDE] = parameter.altitude; sensors->vario[HOTT_VARIO_M1S] = parameter.vspeed; sensors->is_enabled[HOTT_TYPE_GENERAL] = true; sensors->general_air[HOTT_GENERAL_ALTITUDE] = parameter.altitude; sensors->general_air[HOTT_GENERAL_CLIMBRATE] = parameter.vspeed; vario_alarm_parameters.altitude = parameter.altitude; add_alarm_in_ms(1000, interval_1000_callback, &vario_alarm_parameters, false); add_alarm_in_ms(3000, interval_3000_callback, &vario_alarm_parameters, false); add_alarm_in_ms(10000, interval_10000_callback, &vario_alarm_parameters, false); } if (config->i2c_module == I2C_BMP180) { bmp180_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(bmp180_task, "bmp180_task", STACK_BMP180, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } sensors->is_enabled[HOTT_TYPE_VARIO] = true; sensors->vario[HOTT_VARIO_ALTITUDE] = parameter.altitude; sensors->vario[HOTT_VARIO_M1S] = parameter.vspeed; sensors->is_enabled[HOTT_TYPE_GENERAL] = true; sensors->general_air[HOTT_GENERAL_ALTITUDE] = parameter.altitude; sensors->general_air[HOTT_GENERAL_CLIMBRATE] = parameter.vspeed; vario_alarm_parameters.altitude = parameter.altitude; add_alarm_in_ms(1000, interval_1000_callback, &vario_alarm_parameters, false); add_alarm_in_ms(3000, interval_3000_callback, &vario_alarm_parameters, false); add_alarm_in_ms(10000, interval_10000_callback, &vario_alarm_parameters, false); } if (config->enable_fuel_flow) { fuel_meter_parameters_t parameter = {config->fuel_flow_ml_per_pulse, malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(fuel_meter_task, "fuel_meter_task", STACK_FUEL_METER, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); sensors->is_enabled[HOTT_TYPE_GENERAL] = true; sensors->general_air[HOTT_GENERAL_FUEL] = parameter.consumption_total; } if (config->enable_fuel_pressure) { xgzp68xxd_parameters_t parameter = {config->xgzp68xxd_k, malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(xgzp68xxd_task, "fuel_pressure_task", STACK_FUEL_PRESSURE, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); sensors->is_enabled[HOTT_TYPE_GENERAL] = true; sensors->general_air[HOTT_GENERAL_PRESSURE] = parameter.pressure; } if (config->enable_lipo) { float *cell_prev = 0; if (config->lipo_cells > 0) { ina3221_parameters_t parameter = { .i2c_address = 0x40, .filter = config->ina3221_filter, .cell_count = MIN(config->lipo_cells, 3), .cell[0] = malloc(sizeof(float)), .cell[1] = malloc(sizeof(float)), .cell[2] = malloc(sizeof(float)), .cell_prev = malloc(sizeof(float)), }; *parameter.cell_prev = 0; cell_prev = parameter.cell[2]; xTaskCreate(ina3221_task, "ina3221_task", STACK_INA3221, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); sensors->is_enabled[HOTT_TYPE_GENERAL] = true; sensors->general_air[HOTT_GENERAL_CELL_1] = parameter.cell[0]; sensors->general_air[HOTT_GENERAL_CELL_2] = parameter.cell[1]; sensors->general_air[HOTT_GENERAL_CELL_3] = parameter.cell[2]; } if (config->lipo_cells > 3) { ina3221_parameters_t parameter = { .i2c_address = 0x41, .filter = config->ina3221_filter, .cell_count = MIN(config->lipo_cells - 3, 3), .cell[0] = malloc(sizeof(float)), .cell[1] = malloc(sizeof(float)), .cell[2] = malloc(sizeof(float)), .cell_prev = malloc(sizeof(float)), }; parameter.cell_prev = cell_prev; cell_prev = parameter.cell[2]; xTaskCreate(ina3221_task, "ina3221_task", STACK_INA3221, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); sensors->general_air[HOTT_GENERAL_CELL_4] = parameter.cell[0]; sensors->general_air[HOTT_GENERAL_CELL_5] = parameter.cell[1]; sensors->general_air[HOTT_GENERAL_CELL_6] = parameter.cell[2]; } } } static int64_t interval_1000_callback(alarm_id_t id, void *parameters) { vario_alarm_parameters_t *parameter = (vario_alarm_parameters_t *)parameters; static float prev = 0; parameter->m1s = (*parameter->altitude - prev) * 100 + 30000; #ifdef SIM_SENSORS vario_alarm_parameters.m1s = 12 * 100 + 30000; #endif prev = *parameter->altitude; return 1000000L; } static int64_t interval_3000_callback(alarm_id_t id, void *parameters) { vario_alarm_parameters_t *parameter = (vario_alarm_parameters_t *)parameters; static float prev = 0; parameter->m3s = (*parameter->altitude - prev) * 100 + 30000; *parameter->vspd = parameter->m3s / 3.0F; #ifdef SIM_SENSORS vario_alarm_parameters.m3s = 34 * 100 + 30000; #endif prev = *parameter->altitude; return 3000000L; } static int64_t interval_10000_callback(alarm_id_t id, void *parameters) { vario_alarm_parameters_t *parameter = (vario_alarm_parameters_t *)parameters; static float prev = 0; parameter->m10s = (*parameter->altitude - prev) * 100 + 30000; #ifdef SIM_SENSORS vario_alarm_parameters.m10s = 56 * 100 + 30000; #endif prev = *parameter->altitude; return 10000000L; } ================================================ FILE: board/project/protocol/hott.h ================================================ #ifndef HOTT_H #define HOTT_H #include "common.h" extern context_t context; void hott_task(void *parameters); #endif ================================================ FILE: board/project/protocol/ibus.c ================================================ #include "ibus.h" #include #include #include #include "airspeed.h" #include "bmp180.h" #include "bmp280.h" #include "config.h" #include "current.h" #include "esc_apd_f.h" #include "esc_apd_hv.h" #include "esc_castle.h" #include "esc_hw3.h" #include "esc_hw4.h" #include "esc_hw5.h" #include "esc_kontronik.h" #include "esc_omp_m4.h" #include "esc_openyge.h" #include "esc_pwm.h" #include "esc_ztw.h" #include "gps.h" #include "mpu6050.h" #include "ms5611.h" #include "ntc.h" #include "pwm_out.h" #include "smart_esc.h" #include "uart.h" #include "uart_pio.h" #include "voltage.h" /* Flysky IBUS Data Id */ #define IBUS_ID_VOLTAGE 0x00 // Internal Voltage #define IBUS_ID_TEMPERATURE 0x01 // Temperature #define IBUS_ID_MOT 0x02 // RPM #define IBUS_ID_EXTV 0x03 // External Voltage #define IBUS_ID_CELL_VOLTAGE 0x04 // Avg Cell voltage #define IBUS_ID_BAT_CURR 0x05 // battery current A * 100 #define IBUS_ID_FUEL 0x06 // remaining battery percentage / mah drawn otherwise or fuel level no unit! #define IBUS_ID_RPM 0x07 // throttle value / battery capacity #define IBUS_ID_CMP_HEAD 0x08 // Heading 0..360 deg 0north 2bytes #define IBUS_ID_CLIMB_RATE 0x09 // 2 bytes m/s *100 signed #define IBUS_ID_COG \ 0x0A // 2 bytes Course over ground(NOT heading but direction of movement) in degrees * 100 0.0..359.99 degrees. // unknown max uint #define IBUS_ID_GPS_STATUS 0x0B // 2 bytes #define IBUS_ID_ACC_X 0x0C // 2 bytes m/s *100 signed #define IBUS_ID_ACC_Y 0x0D // 2 bytes m/s *100 signed #define IBUS_ID_ACC_Z 0x0E // 2 bytes m/s *100 signed #define IBUS_ID_ROLL 0x0F // 2 bytes deg *100 signed #define IBUS_ID_PITCH 0x10 // 2 bytes deg *100 signed #define IBUS_ID_YAW 0x11 // 2 bytes deg *100 signed #define IBUS_ID_VERTICAL_SPEED 0x12 // 2 bytes m/s *100 signed #define IBUS_ID_GROUND_SPEED 0x13 // 2 bytes m/s *100 different unit than build-in sensor #define IBUS_ID_GPS_DIST 0x14 // 2 bytes distance from home m unsigned #define IBUS_ID_ARMED 0x15 // 2 bytes #define IBUS_ID_FLIGHT_MODE 0x16 // 2 bytes #define IBUS_ID_PRES 0x41 // Pressure #define IBUS_ID_ODO1 0x7C // Odometer1 #define IBUS_ID_ODO2 0x7D // Odometer2 #define IBUS_ID_SPE 0x7E // Speed 2 bytes km/h * 100 #define IBUS_ID_TX_V 0x7F // TX Voltage #define IBUS_ID_GPS_LAT 0x80 // 4bytes signed WGS84 in degrees * 1E7 #define IBUS_ID_GPS_LON 0x81 // 4bytes signed WGS84 in degrees * 1E7 #define IBUS_ID_GPS_ALT 0x82 // 4bytes signed!!! GPS alt m*100 #define IBUS_ID_ALT 0x83 // 4bytes signed!!! Alt m*100 #define IBUS_ID_S84 0x84 #define IBUS_ID_S85 0x85 #define IBUS_ID_S86 0x86 #define IBUS_ID_S87 0x87 #define IBUS_ID_S88 0x88 #define IBUS_ID_S89 0x89 #define IBUS_ID_S8a 0x8A #define IBUS_ID_RX_SIG_AFHDS3 0xF7 // SIG #define IBUS_ID_RX_SNR_AFHDS3 0xF8 // SNR #define IBUS_ID_ALT_FLYSKY 0xF9 // Altitude 2 bytes signed in m - used in FlySky native TX #define IBUS_ID_RX_SNR 0xFA // SNR #define IBUS_ID_RX_NOISE 0xFB // Noise #define IBUS_ID_RX_RSSI 0xFC // RSSI #define IBUS_ID_RX_ERR_RATE 0xFE // Error rate #define IBUS_ID_END 0xFF // AC type telemetry with multiple values in one packet #define IBUS_ID_GPS_FULL 0xFD #define IBUS_ID_VOLT_FULL 0xF0 #define IBUS_ID_ACC_FULL 0xEF #define IBUS_ID_TX_RSSI 0x200 // Pseudo id outside 1 byte range of FlySky sensors #define IBUS_TYPE_U16 0 #define IBUS_TYPE_S16 1 #define IBUS_TYPE_U32 2 #define IBUS_TYPE_S32 3 #define IBUS_TYPE_GPS 4 #define IBUS_RECEIVED_NONE 0 #define IBUS_RECEIVED_POLL 1 #define IBUS_COMMAND_DISCOVER 0x8 #define IBUS_COMMAND_TYPE 0x9 #define IBUS_COMMAND_MEASURE 0xA #define IBUS_TIMEOUT_US 1000 #define IBUS_PACKET_LENGHT 4 typedef struct sensor_ibus_t { uint8_t data_id; uint8_t type; float *value; } sensor_ibus_t; static void process(sensor_ibus_t **sensor); static void send_packet(uint8_t command, uint8_t address, sensor_ibus_t *sensor_ibus); static void send_byte(uint8_t c, uint16_t *crc_p); static int32_t format(uint8_t data_id, float value); static bool check_crc(uint8_t *data); static void add_sensor(sensor_ibus_t *new_sensor, sensor_ibus_t **sensor, uint16_t sensormask); static void set_config(sensor_ibus_t **sensor, uint16_t sensormask); void ibus_task(void *parameters) { sensor_ibus_t *sensor[16] = {NULL}; uint16_t sensor_mask = 0B1111111111111110; context.led_cycle_duration = 6; context.led_cycles = 1; uart0_begin(115200, UART_RECEIVER_TX, UART_RECEIVER_RX, IBUS_TIMEOUT_US, 8, 1, UART_PARITY_NONE, false, true); set_config(sensor, sensor_mask); debug("\nIbus init"); while (1) { ulTaskNotifyTakeIndexed(1, pdTRUE, portMAX_DELAY); process(sensor); } } static void process(sensor_ibus_t **sensor) { if (uart0_available() == IBUS_PACKET_LENGHT) { uint8_t command = 0; uint8_t address = 0; uint8_t data[IBUS_PACKET_LENGHT]; uart0_read_bytes(data, IBUS_PACKET_LENGHT); if (data[0] == IBUS_PACKET_LENGHT) { debug("\nIbus (%u) < ", uxTaskGetStackHighWaterMark(NULL)); debug_buffer(data, data[0], "0x%X "); if (check_crc(data)) { command = data[1] >> 4; address = data[1] & 0x0F; if (!sensor[address]) return; if (command == IBUS_COMMAND_DISCOVER || command == IBUS_COMMAND_TYPE || IBUS_COMMAND_MEASURE) send_packet(command, address, sensor[address]); } else debug(" - Bad CRC"); } } } static void send_packet(uint8_t command, uint8_t address, sensor_ibus_t *sensor) { uint8_t *u8_p = NULL; uint16_t crc = 0; uint16_t type; uint8_t lenght = 0; int32_t value_formatted = 0; switch (sensor->type) { case IBUS_TYPE_S16: case IBUS_TYPE_U16: lenght = 2; break; case IBUS_TYPE_S32: lenght = 4; break; case IBUS_TYPE_GPS: lenght = 14; break; } switch (command) { case IBUS_COMMAND_DISCOVER: lenght = 0; break; case IBUS_COMMAND_TYPE: type = lenght << 8 | sensor->data_id; u8_p = (uint8_t *)&type; lenght = 2; break; case IBUS_COMMAND_MEASURE: if (sensor->value) { value_formatted = format(sensor->data_id, *sensor->value); } u8_p = (uint8_t *)&value_formatted; break; } debug("\nIbus (%u) > ", uxTaskGetStackHighWaterMark(NULL)); // lenght send_byte(4 + lenght, &crc); // command & address send_byte(command << 4 | address, &crc); // value for (uint8_t i = 0; i < lenght; i++) { send_byte(u8_p[i], &crc); } // crc crc = 0xFFFF - crc; u8_p = (uint8_t *)&crc; send_byte(u8_p[0], NULL); send_byte(u8_p[1], NULL); // blink led vTaskResume(context.led_task_handle); } static void send_byte(uint8_t c, uint16_t *crc_p) { if (crc_p != NULL) { uint16_t crc = *crc_p; crc += c; *crc_p = crc; } uart0_write(c); debug("%X ", c); } static bool check_crc(uint8_t *data) { uint16_t crc = 0xFFFF; uint8_t lenght = data[0]; for (uint8_t i = 0; i < lenght - 2; i++) crc -= data[i]; if (crc == (uint16_t)data[lenght - 2] << 8 || data[lenght - 1]) return true; return false; } static void add_sensor(sensor_ibus_t *new_sensor, sensor_ibus_t **sensor, uint16_t sensor_mask) { for (uint8_t i = 0; i < 16; i++) { if (sensor[i] == NULL && sensor_mask & 1 << i) { sensor[i] = new_sensor; return; } } } static int32_t format(uint8_t data_id, float value) { if (data_id == IBUS_ID_TEMPERATURE) return round((value + 40) * 10); if (data_id == IBUS_ID_EXTV || data_id == IBUS_ID_CELL_VOLTAGE || data_id == IBUS_ID_BAT_CURR || data_id == IBUS_ID_CLIMB_RATE || data_id == IBUS_ID_COG || data_id == IBUS_ID_VERTICAL_SPEED || data_id == IBUS_ID_GROUND_SPEED || data_id == IBUS_ID_GPS_ALT || data_id == IBUS_ID_PRES || data_id == IBUS_ID_ALT || data_id == IBUS_ID_ACC_X || data_id == IBUS_ID_ACC_Y || data_id == IBUS_ID_ACC_Z || data_id == IBUS_ID_ROLL || data_id == IBUS_ID_PITCH || data_id == IBUS_ID_YAW) return round(value * 100); if (data_id == IBUS_ID_GPS_LAT || data_id == IBUS_ID_GPS_LON) return round(value * 1e7); if (data_id == IBUS_ID_SPE) return round(value * 100 * 1.852); if (data_id == IBUS_ID_GPS_STATUS) return value * 256; if (data_id == IBUS_ID_S84 || data_id == IBUS_ID_S85) return value / 60 * 1e5; return round(value); } static void set_config(sensor_ibus_t **sensor, uint16_t sensormask) { /* - Sensor at address 0x00 is reserved - Sensor at address 0x01 is recerved in some receivers types. But the poll at address 0x01, if present, has to be answered (so there is a dummy sensor at address 0x01), otherwise the sensor poll scan is stopped from receiver - TODO: dinamically set the sensor address to allocate an additional sensor in receivers with only one sensor masked */ config_t *config = config_read(); TaskHandle_t task_handle; sensor_ibus_t *new_sensor; float *baro_temp = NULL, *baro_pressure = NULL; new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_END, 0, NULL}; add_sensor(new_sensor, sensor, sensormask); if (config->esc_protocol == ESC_PWM) { esc_pwm_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, malloc(sizeof(float))}; xTaskCreate(esc_pwm_task, "esc_pwm_task", STACK_ESC_PWM, (void *)¶meter, 2, &task_handle); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_MOT, IBUS_TYPE_U16, parameter.rpm}; add_sensor(new_sensor, sensor, sensormask); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_HW3) { esc_hw3_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, malloc(sizeof(float))}; xTaskCreate(esc_hw3_task, "esc_hw3_task", STACK_ESC_HW3, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_MOT, IBUS_TYPE_U16, parameter.rpm}; add_sensor(new_sensor, sensor, sensormask); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_HW4) { esc_hw4_parameters_t parameter = {config->rpm_multiplier, config->enable_pwm_out, config->enable_esc_hw4_init_delay, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, config->esc_hw4_voltage_multiplier, config->esc_hw4_current_multiplier, config->esc_hw4_current_thresold, config->esc_hw4_current_max, config->esc_hw4_is_manual_offset, config->esc_hw4_auto_detect, config->esc_hw4_offset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_hw4_task, "esc_hw4_task", STACK_ESC_HW4, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); if (config->enable_pwm_out) { xTaskCreate(pwm_out_task, "pwm_out", STACK_PWM_OUT, (void *)parameter.rpm, 2, &task_handle); context.pwm_out_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_MOT, IBUS_TYPE_U16, parameter.rpm}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_EXTV, IBUS_TYPE_U16, parameter.voltage}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_BAT_CURR, IBUS_TYPE_U16, parameter.current}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_TEMPERATURE, IBUS_TYPE_U16, parameter.temperature_fet}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_TEMPERATURE, IBUS_TYPE_U16, parameter.temperature_bec}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_CELL_VOLTAGE, IBUS_TYPE_U16, parameter.cell_voltage}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_FUEL, IBUS_TYPE_U16, parameter.consumption}; add_sensor(new_sensor, sensor, sensormask); } if (config->esc_protocol == ESC_HW5) { esc_hw5_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_hw5_task, "esc_hw5_task", STACK_ESC_HW5, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_MOT, IBUS_TYPE_U16, parameter.rpm}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_EXTV, IBUS_TYPE_U16, parameter.voltage}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_BAT_CURR, IBUS_TYPE_U16, parameter.current}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_TEMPERATURE, IBUS_TYPE_U16, parameter.temperature_fet}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_TEMPERATURE, IBUS_TYPE_U16, parameter.temperature_bec}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_CELL_VOLTAGE, IBUS_TYPE_U16, parameter.cell_voltage}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_FUEL, IBUS_TYPE_U16, parameter.consumption}; add_sensor(new_sensor, sensor, sensormask); } if (config->esc_protocol == ESC_CASTLE) { esc_castle_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_castle_task, "esc_castle_task", STACK_ESC_CASTLE, (void *)¶meter, 2, &task_handle); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_MOT, IBUS_TYPE_U16, parameter.rpm}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_EXTV, IBUS_TYPE_U16, parameter.voltage}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_BAT_CURR, IBUS_TYPE_U16, parameter.current}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_TEMPERATURE, IBUS_TYPE_U16, parameter.temperature}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_CELL_VOLTAGE, IBUS_TYPE_U16, parameter.cell_voltage}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_FUEL, IBUS_TYPE_U16, parameter.consumption}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_EXTV, IBUS_TYPE_U16, parameter.ripple_voltage}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_EXTV, IBUS_TYPE_U16, parameter.voltage_bec}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_BAT_CURR, IBUS_TYPE_U16, parameter.current_bec}; add_sensor(new_sensor, sensor, sensormask); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_KONTRONIK) { esc_kontronik_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_kontronik_task, "esc_kontronik_task", STACK_ESC_KONTRONIK, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_MOT, IBUS_TYPE_U16, parameter.rpm}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_EXTV, IBUS_TYPE_U16, parameter.voltage}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_BAT_CURR, IBUS_TYPE_U16, parameter.current}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_EXTV, IBUS_TYPE_U16, parameter.voltage_bec}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_BAT_CURR, IBUS_TYPE_U16, parameter.current_bec}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_TEMPERATURE, IBUS_TYPE_U16, parameter.temperature_fet}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_TEMPERATURE, IBUS_TYPE_U16, parameter.temperature_bec}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_CELL_VOLTAGE, IBUS_TYPE_U16, parameter.cell_voltage}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_FUEL, IBUS_TYPE_U16, parameter.consumption}; add_sensor(new_sensor, sensor, sensormask); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_APD_F) { esc_apd_f_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_apd_f_task, "esc_apd_f_task", STACK_ESC_APD_F, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_MOT, IBUS_TYPE_U16, parameter.rpm}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_EXTV, IBUS_TYPE_U16, parameter.voltage}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_BAT_CURR, IBUS_TYPE_U16, parameter.current}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_TEMPERATURE, IBUS_TYPE_U16, parameter.temperature}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_CELL_VOLTAGE, IBUS_TYPE_U16, parameter.cell_voltage}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_FUEL, IBUS_TYPE_U16, parameter.consumption}; add_sensor(new_sensor, sensor, sensormask); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_APD_HV) { esc_apd_hv_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_apd_hv_task, "esc_apd_hv_task", STACK_ESC_APD_HV, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_MOT, IBUS_TYPE_U16, parameter.rpm}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_EXTV, IBUS_TYPE_U16, parameter.voltage}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_BAT_CURR, IBUS_TYPE_U16, parameter.current}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_TEMPERATURE, IBUS_TYPE_U16, parameter.temperature}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_CELL_VOLTAGE, IBUS_TYPE_U16, parameter.cell_voltage}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_FUEL, IBUS_TYPE_U16, parameter.consumption}; add_sensor(new_sensor, sensor, sensormask); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_SMART) { smart_esc_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temperature_fet = malloc(sizeof(float)); parameter.temperature_bec = malloc(sizeof(float)); parameter.voltage_bec = malloc(sizeof(float)); parameter.current_bec = malloc(sizeof(float)); parameter.temperature_bat = malloc(sizeof(float)); parameter.current_bat = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); for (uint i = 0; i < 18; i++) parameter.cell[i] = malloc(sizeof(float)); parameter.cells = malloc(sizeof(uint8_t)); parameter.cycles = malloc(sizeof(uint16_t)); xTaskCreate(smart_esc_task, "smart_esc_task", STACK_SMART_ESC, (void *)¶meter, 4, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_MOT, IBUS_TYPE_U16, parameter.rpm}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_EXTV, IBUS_TYPE_U16, parameter.voltage}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_BAT_CURR, IBUS_TYPE_U16, parameter.current}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_EXTV, IBUS_TYPE_U16, parameter.voltage_bec}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_BAT_CURR, IBUS_TYPE_U16, parameter.current_bec}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_TEMPERATURE, IBUS_TYPE_U16, parameter.temperature_fet}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_TEMPERATURE, IBUS_TYPE_U16, parameter.temperature_bec}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_FUEL, IBUS_TYPE_U16, parameter.consumption}; add_sensor(new_sensor, sensor, sensormask); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_OMP_M4) { esc_omp_m4_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temp_esc = malloc(sizeof(float)); parameter.temp_motor = malloc(sizeof(float)); parameter.cell_voltage = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); parameter.cell_count = malloc(sizeof(uint8_t)); xTaskCreate(esc_omp_m4_task, "esc_omp_m4_task", STACK_ESC_OMP_M4, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_MOT, IBUS_TYPE_U16, parameter.rpm}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_EXTV, IBUS_TYPE_U16, parameter.voltage}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_BAT_CURR, IBUS_TYPE_U16, parameter.current}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_TEMPERATURE, IBUS_TYPE_U16, parameter.temp_esc}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_TEMPERATURE, IBUS_TYPE_U16, parameter.temp_motor}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_CELL_VOLTAGE, IBUS_TYPE_U16, parameter.cell_voltage}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_FUEL, IBUS_TYPE_U16, parameter.consumption}; add_sensor(new_sensor, sensor, sensormask); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_ZTW) { esc_ztw_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temp_esc = malloc(sizeof(float)); parameter.temp_motor = malloc(sizeof(float)); parameter.bec_voltage = malloc(sizeof(float)); parameter.cell_voltage = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); parameter.cell_count = malloc(sizeof(uint8_t)); xTaskCreate(esc_ztw_task, "esc_ztw_task", STACK_ESC_ZTW, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_MOT, IBUS_TYPE_U16, parameter.rpm}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_EXTV, IBUS_TYPE_U16, parameter.voltage}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_BAT_CURR, IBUS_TYPE_U16, parameter.current}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_TEMPERATURE, IBUS_TYPE_U16, parameter.temp_esc}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_TEMPERATURE, IBUS_TYPE_U16, parameter.temp_motor}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_CELL_VOLTAGE, IBUS_TYPE_U16, parameter.cell_voltage}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_FUEL, IBUS_TYPE_U16, parameter.consumption}; add_sensor(new_sensor, sensor, sensormask); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_OPENYGE) { esc_openyge_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.pwm_out = config->enable_pwm_out; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temperature_fet = malloc(sizeof(float)); parameter.temperature_bec = malloc(sizeof(float)); parameter.cell_voltage = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); parameter.voltage_bec = malloc(sizeof(float)); parameter.current_bec = malloc(sizeof(float)); parameter.throttle = malloc(sizeof(float)); parameter.pwm_percent = malloc(sizeof(float)); parameter.cell_count = malloc(sizeof(uint8_t)); xTaskCreate(esc_openyge_task, "esc_openyge_task", STACK_ESC_OPENYGE, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_MOT, IBUS_TYPE_U16, parameter.rpm}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_EXTV, IBUS_TYPE_U16, parameter.voltage}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_BAT_CURR, IBUS_TYPE_U16, parameter.current}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_EXTV, IBUS_TYPE_U16, parameter.voltage_bec}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_BAT_CURR, IBUS_TYPE_U16, parameter.current_bec}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_TEMPERATURE, IBUS_TYPE_U16, parameter.temperature_fet}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_TEMPERATURE, IBUS_TYPE_U16, parameter.temperature_bec}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_CELL_VOLTAGE, IBUS_TYPE_U16, parameter.cell_voltage}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_FUEL, IBUS_TYPE_U16, parameter.consumption}; add_sensor(new_sensor, sensor, sensormask); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_gps) { gps_parameters_t parameter; parameter.protocol = config->gps_protocol; parameter.baudrate = config->gps_baudrate; parameter.rate = config->gps_rate; parameter.lat = malloc(sizeof(double)); parameter.lon = malloc(sizeof(double)); parameter.alt = malloc(sizeof(float)); parameter.spd = malloc(sizeof(float)); parameter.cog = malloc(sizeof(float)); parameter.hdop = malloc(sizeof(float)); parameter.sat = malloc(sizeof(float)); parameter.time = malloc(sizeof(float)); parameter.date = malloc(sizeof(float)); parameter.vspeed = malloc(sizeof(float)); parameter.dist = malloc(sizeof(float)); parameter.spd_kmh = malloc(sizeof(float)); parameter.fix = malloc(sizeof(float)); parameter.vdop = malloc(sizeof(float)); parameter.speed_acc = malloc(sizeof(float)); parameter.h_acc = malloc(sizeof(float)); parameter.v_acc = malloc(sizeof(float)); parameter.track_acc = malloc(sizeof(float)); parameter.n_vel = malloc(sizeof(float)); parameter.e_vel = malloc(sizeof(float)); parameter.v_vel = malloc(sizeof(float)); parameter.alt_elipsiod = malloc(sizeof(float)); parameter.pdop = malloc(sizeof(float)); parameter.fix_type = malloc(sizeof(uint8_t)); parameter.home_set = malloc(sizeof(uint8_t)); parameter.alt_home = malloc(sizeof(float)); xTaskCreate(gps_task, "gps_task", STACK_GPS, (void *)¶meter, 2, &task_handle); context.uart_pio_notify_task_handle = task_handle; new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_GPS_STATUS, IBUS_TYPE_U16, parameter.sat}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_GPS_LAT, IBUS_TYPE_S32, parameter.lat}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_GPS_LON, IBUS_TYPE_S32, parameter.lon}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_GPS_ALT, IBUS_TYPE_S32, parameter.alt}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_SPE, IBUS_TYPE_U16, parameter.spd}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_COG, IBUS_TYPE_U16, parameter.cog}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_CLIMB_RATE, IBUS_TYPE_S16, parameter.vspeed}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_GPS_DIST, IBUS_TYPE_U16, parameter.dist}; add_sensor(new_sensor, sensor, sensormask); if (config->ibus_alternative_coordinates) { new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_S84, IBUS_TYPE_S32, parameter.lat}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_S85, IBUS_TYPE_S32, parameter.lon}; add_sensor(new_sensor, sensor, sensormask); } ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_voltage) { voltage_parameters_t parameter = {0, config->analog_rate, config->alpha_voltage, config->analog_voltage_multiplier, malloc(sizeof(float))}; xTaskCreate(voltage_task, "voltage_task", STACK_VOLTAGE, (void *)¶meter, 2, &task_handle); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_EXTV, IBUS_TYPE_U16, parameter.voltage}; add_sensor(new_sensor, sensor, sensormask); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_current) { current_parameters_t parameter = {1, config->analog_rate, config->alpha_current, config->analog_current_multiplier, config->analog_current_offset, config->analog_current_autoffset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(current_task, "current_task", STACK_CURRENT, (void *)¶meter, 2, &task_handle); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_BAT_CURR, IBUS_TYPE_U16, parameter.current}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_FUEL, IBUS_TYPE_U16, parameter.consumption}; add_sensor(new_sensor, sensor, sensormask); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_ntc) { ntc_parameters_t parameter = {2, config->analog_rate, config->ntc_offset, config->alpha_temperature, malloc(sizeof(float))}; xTaskCreate(ntc_task, "ntc_task", STACK_NTC, (void *)¶meter, 2, &task_handle); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_TEMPERATURE, IBUS_TYPE_U16, parameter.ntc}; add_sensor(new_sensor, sensor, sensormask); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_BMP280) { bmp280_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, 0, config->bmp280_filter, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(bmp280_task, "bmp280_task", STACK_BMP280, (void *)¶meter, 2, &task_handle); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_TEMPERATURE, IBUS_TYPE_U16, parameter.temperature}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_ALT, IBUS_TYPE_S32, parameter.altitude}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_CLIMB_RATE, IBUS_TYPE_S16, parameter.vspeed}; add_sensor(new_sensor, sensor, sensormask); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_MS5611) { ms5611_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, 0, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(ms5611_task, "ms5611_task", STACK_MS5611, (void *)¶meter, 2, &task_handle); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_TEMPERATURE, IBUS_TYPE_U16, parameter.temperature}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_ALT, IBUS_TYPE_S32, parameter.altitude}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_CLIMB_RATE, IBUS_TYPE_S16, parameter.vspeed}; add_sensor(new_sensor, sensor, sensormask); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_BMP180) { bmp180_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(bmp180_task, "bmp180_task", STACK_BMP180, (void *)¶meter, 2, &task_handle); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_TEMPERATURE, IBUS_TYPE_U16, parameter.temperature}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_ALT, IBUS_TYPE_S32, parameter.altitude}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_CLIMB_RATE, IBUS_TYPE_S16, parameter.vspeed}; add_sensor(new_sensor, sensor, sensormask); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_airspeed) { airspeed_parameters_t parameter = {3, config->analog_rate, config->alpha_airspeed, (float)config->airspeed_offset / 1000, (float)config->airspeed_vcc / 100, baro_temp, baro_pressure, malloc(sizeof(float))}; xTaskCreate(airspeed_task, "airspeed_task", STACK_AIRSPEED, (void *)¶meter, 2, &task_handle); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_SPE, IBUS_TYPE_U16, parameter.airspeed}; add_sensor(new_sensor, sensor, sensormask); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_gyro) { mpu6050_parameters_t parameter = {1, 0, config->mpu6050_acc_scale, config->mpu6050_gyro_scale, config->mpu6050_gyro_weighting, config->mpu6050_filter, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(mpu6050_task, "mpu6050_task", STACK_MPU6050, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_ACC_X, IBUS_TYPE_S16, parameter.acc_x}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_ACC_Y, IBUS_TYPE_S16, parameter.acc_y}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_ACC_Z, IBUS_TYPE_S16, parameter.acc_z}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_PITCH, IBUS_TYPE_S16, parameter.pitch}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_ROLL, IBUS_TYPE_S16, parameter.roll}; add_sensor(new_sensor, sensor, sensormask); new_sensor = malloc(sizeof(sensor_ibus_t)); *new_sensor = (sensor_ibus_t){IBUS_ID_YAW, IBUS_TYPE_S16, parameter.yaw}; add_sensor(new_sensor, sensor, sensormask); } } ================================================ FILE: board/project/protocol/ibus.h ================================================ #ifndef IBUS_H #define IBUS_H #include "common.h" extern context_t context; void ibus_task(void *parameters); #endif ================================================ FILE: board/project/protocol/jetiex.c ================================================ #include "jetiex.h" #include #include #include "airspeed.h" #include "bmp180.h" #include "bmp280.h" #include "config.h" #include "current.h" #include "esc_apd_f.h" #include "esc_apd_hv.h" #include "esc_castle.h" #include "esc_hw3.h" #include "esc_hw4.h" #include "esc_hw5.h" #include "esc_kontronik.h" #include "esc_omp_m4.h" #include "esc_pwm.h" #include "esc_ztw.h" #include "fuel_meter.h" #include "gps.h" #include "ina3221.h" #include "mpu6050.h" #include "ms5611.h" #include "ntc.h" #include "pwm_out.h" #include "smart_esc.h" #include "stdlib.h" #include "string.h" #include "uart.h" #include "uart_pio.h" #include "voltage.h" #include "xgzp68xxd.h" #define JETIEX_WAIT 0 #define JETIEX_SEND 1 #define JETIEX_PACKET_LENGHT 8 #define JETIEX_TIMEOUT_US 500 #define JETIEX_BAUDRATE_TIMEOUT_MS 5000 static void process(uint *baudrate, sensor_jetiex_t **sensor); static void send_packet(uint8_t packet_id, sensor_jetiex_t **sensor); static void add_sensor_text(uint8_t *buffer, uint8_t *buffer_index, uint8_t sensor_index, sensor_jetiex_t *sensor); static void add_sensor_value(uint8_t *buffer, uint8_t *buffer_index, uint8_t sensor_index, sensor_jetiex_t *sensor); static int64_t timeout_callback(alarm_id_t id, void *parameters); static uint8_t crc8(uint8_t *crc, uint8_t crc_length); static uint8_t update_crc8(uint8_t crc, uint8_t crc_seed); static uint16_t crc16(uint8_t *p, uint16_t len); static uint16_t update_crc16(uint16_t crc, uint8_t data); void jetiex_task(void *parameters) { uint baudrate = 125000L; sensor_jetiex_t *sensor[16] = {NULL}; context.led_cycle_duration = 6; context.led_cycles = 1; uart0_begin(baudrate, UART_RECEIVER_TX, UART_RECEIVER_RX, JETIEX_TIMEOUT_US, 8, 1, UART_PARITY_NONE, false, true); jeti_set_config(sensor); debug("\nJeti Ex init"); while (1) { ulTaskNotifyTakeIndexed(1, pdTRUE, portMAX_DELAY); process(&baudrate, sensor); } } uint8_t jeti_create_telemetry_buffer(uint8_t *buffer, bool packet_type, sensor_jetiex_t **sensor) { static uint8_t sensor_index_value = 0; static int8_t sensor_index_text = -1; uint8_t buffer_index = 7; if (sensor[0] == NULL) return 0; if (packet_type) { buffer[1] = 0x40; add_sensor_value(buffer, &buffer_index, sensor_index_value + 1, sensor[sensor_index_value]); sensor_index_value++; if (sensor_index_value >= 16 || sensor[sensor_index_value] == NULL) sensor_index_value = 0; else { add_sensor_value(buffer, &buffer_index, sensor_index_value + 1, sensor[sensor_index_value]); sensor_index_value++; if (sensor_index_value >= 16 || sensor[sensor_index_value] == NULL) sensor_index_value = 0; } } else { add_sensor_text(buffer, &buffer_index, sensor_index_text + 1, sensor_index_text == -1 ? NULL : sensor[sensor_index_text]); sensor_index_text++; if (sensor_index_text >= 16 || sensor[sensor_index_text] == NULL) sensor_index_text = -1; } buffer[0] = 0x0F; buffer[1] |= buffer_index - 1; buffer[2] = JETIEX_MFG_ID_LOW; buffer[3] = JETIEX_MFG_ID_HIGH; buffer[4] = JETIEX_DEV_ID_LOW; buffer[5] = JETIEX_DEV_ID_HIGH; buffer[6] = 0x00; buffer[buffer_index] = crc8(buffer + 1, buffer_index - 1); return buffer_index + 1; } void jeti_add_sensor(sensor_jetiex_t *new_sensor, sensor_jetiex_t **sensors) { static uint8_t sensor_count = 0; if (sensor_count < 15) { sensors[sensor_count] = new_sensor; new_sensor->data_id = sensor_count; sensor_count++; } } void jeti_set_config(sensor_jetiex_t **sensor) { config_t *config = config_read(); TaskHandle_t task_handle; sensor_jetiex_t *new_sensor; float *baro_temp = NULL, *baro_pressure = NULL; if (config->esc_protocol == ESC_PWM) { esc_pwm_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, malloc(sizeof(float))}; xTaskCreate(esc_pwm_task, "esc_pwm_task", STACK_ESC_PWM, (void *)¶meter, 2, &task_handle); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT22, JETIEX_FORMAT_0_DECIMAL, "RPM", "RPM", parameter.rpm}; jeti_add_sensor(new_sensor, sensor); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_HW3) { esc_hw3_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, malloc(sizeof(float))}; xTaskCreate(esc_hw3_task, "esc_hw3_task", STACK_ESC_HW3, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT22, JETIEX_FORMAT_0_DECIMAL, "RPM", "RPM", parameter.rpm}; jeti_add_sensor(new_sensor, sensor); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_HW4) { esc_hw4_parameters_t parameter = {config->rpm_multiplier, config->enable_pwm_out, config->enable_esc_hw4_init_delay, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, config->esc_hw4_voltage_multiplier, config->esc_hw4_current_multiplier, config->esc_hw4_current_thresold, config->esc_hw4_current_max, config->esc_hw4_is_manual_offset, config->esc_hw4_auto_detect, config->esc_hw4_offset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_hw4_task, "esc_hw4_task", STACK_ESC_HW4, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); if (config->enable_pwm_out) { xTaskCreate(pwm_out_task, "pwm_out", STACK_PWM_OUT, (void *)parameter.rpm, 2, &task_handle); context.pwm_out_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT22, JETIEX_FORMAT_0_DECIMAL, "RPM", "RPM", parameter.rpm}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_1_DECIMAL, "Current", "A", parameter.current}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_2_DECIMAL, "Voltage", "V", parameter.voltage}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_0_DECIMAL, "Temp FET", "C", parameter.temperature_fet}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_0_DECIMAL, "Temp BEC", "C", parameter.temperature_bec}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_2_DECIMAL, "Cell Voltage", "V", parameter.cell_voltage}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT22, JETIEX_FORMAT_0_DECIMAL, "Consumption", "mAh", parameter.consumption}; jeti_add_sensor(new_sensor, sensor); } if (config->esc_protocol == ESC_HW5) { esc_hw5_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_hw5_task, "esc_hw5_task", STACK_ESC_HW5, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT22, JETIEX_FORMAT_0_DECIMAL, "RPM", "RPM", parameter.rpm}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_1_DECIMAL, "Current", "A", parameter.current}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_1_DECIMAL, "Voltage", "V", parameter.voltage}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_0_DECIMAL, "Temp FET", "C", parameter.temperature_fet}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_0_DECIMAL, "Temp BEC", "C", parameter.temperature_bec}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_0_DECIMAL, "Temp Motor", "C", parameter.temperature_motor}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_1_DECIMAL, "Voltage BEC", "C", parameter.voltage_bec}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_1_DECIMAL, "Current BEC", "C", parameter.current_bec}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_2_DECIMAL, "Cell Voltage", "V", parameter.cell_voltage}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT22, JETIEX_FORMAT_0_DECIMAL, "Consumption", "mAh", parameter.consumption}; jeti_add_sensor(new_sensor, sensor); } if (config->esc_protocol == ESC_CASTLE) { esc_castle_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_hw4_task, "esc_castle_task", STACK_ESC_CASTLE, (void *)¶meter, 2, &task_handle); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT22, JETIEX_FORMAT_0_DECIMAL, "RPM", "RPM", parameter.rpm}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_1_DECIMAL, "Current", "A", parameter.current}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_2_DECIMAL, "Voltage", "V", parameter.voltage}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_0_DECIMAL, "Temperature", "C", parameter.temperature}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_2_DECIMAL, "Ripple Voltage BEC", "V", parameter.ripple_voltage}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_2_DECIMAL, "BEC Voltage", "V", parameter.voltage_bec}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_2_DECIMAL, "BEC Current", "A", parameter.current_bec}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_2_DECIMAL, "Cell Voltage", "V", parameter.cell_voltage}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT22, JETIEX_FORMAT_0_DECIMAL, "Consumption", "mAh", parameter.consumption}; jeti_add_sensor(new_sensor, sensor); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_KONTRONIK) { esc_kontronik_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_kontronik_task, "esc_kontronik_task", STACK_ESC_KONTRONIK, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT22, JETIEX_FORMAT_0_DECIMAL, "RPM", "RPM", parameter.rpm}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_1_DECIMAL, "Current", "A", parameter.current}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_2_DECIMAL, "Voltage", "V", parameter.voltage}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_2_DECIMAL, "Current BEC", "A", parameter.current_bec}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_2_DECIMAL, "Voltage BEC", "V", parameter.voltage_bec}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_0_DECIMAL, "Temp FET", "C", parameter.temperature_fet}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_0_DECIMAL, "Temp BEC", "C", parameter.temperature_bec}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_2_DECIMAL, "Cell Voltage", "V", parameter.cell_voltage}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT22, JETIEX_FORMAT_0_DECIMAL, "Consumption", "mAh", parameter.consumption}; jeti_add_sensor(new_sensor, sensor); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_APD_F) { esc_apd_f_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_apd_f_task, "esc_apd_f_task", STACK_ESC_APD_F, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT22, JETIEX_FORMAT_0_DECIMAL, "RPM", "RPM", parameter.rpm}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_1_DECIMAL, "Current", "A", parameter.current}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_2_DECIMAL, "Voltage", "V", parameter.voltage}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_0_DECIMAL, "Temp", "C", parameter.temperature}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_2_DECIMAL, "Cell Voltage", "V", parameter.cell_voltage}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT22, JETIEX_FORMAT_0_DECIMAL, "Consumption", "mAh", parameter.consumption}; jeti_add_sensor(new_sensor, sensor); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_APD_HV) { esc_apd_hv_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_apd_hv_task, "esc_apd_hv_task", STACK_ESC_APD_HV, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT22, JETIEX_FORMAT_0_DECIMAL, "RPM", "RPM", parameter.rpm}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_1_DECIMAL, "Current", "A", parameter.current}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_2_DECIMAL, "Voltage", "V", parameter.voltage}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_0_DECIMAL, "Temp", "C", parameter.temperature}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_2_DECIMAL, "Cell Voltage", "V", parameter.cell_voltage}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT22, JETIEX_FORMAT_0_DECIMAL, "Consumption", "mAh", parameter.consumption}; jeti_add_sensor(new_sensor, sensor); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_OMP_M4) { esc_omp_m4_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temp_esc = malloc(sizeof(float)); parameter.temp_motor = malloc(sizeof(float)); parameter.cell_voltage = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); parameter.cell_count = malloc(sizeof(uint8_t)); xTaskCreate(esc_omp_m4_task, "esc_omp_m4_task", STACK_ESC_OMP_M4, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT22, JETIEX_FORMAT_0_DECIMAL, "RPM", "RPM", parameter.rpm}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_1_DECIMAL, "Current", "A", parameter.current}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_2_DECIMAL, "Voltage", "V", parameter.voltage}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_0_DECIMAL, "Temp ESC", "C", parameter.temp_esc}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_0_DECIMAL, "Temp Motor", "C", parameter.temp_motor}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_2_DECIMAL, "Cell Voltage", "V", parameter.cell_voltage}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT22, JETIEX_FORMAT_0_DECIMAL, "Consumption", "mAh", parameter.consumption}; jeti_add_sensor(new_sensor, sensor); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_ZTW) { esc_ztw_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temp_esc = malloc(sizeof(float)); parameter.temp_motor = malloc(sizeof(float)); parameter.bec_voltage = malloc(sizeof(float)); parameter.cell_voltage = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); parameter.cell_count = malloc(sizeof(uint8_t)); xTaskCreate(esc_omp_m4_task, "esc_ztw_task", STACK_ESC_ZTW, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT22, JETIEX_FORMAT_0_DECIMAL, "RPM", "RPM", parameter.rpm}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_1_DECIMAL, "Current", "A", parameter.current}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_2_DECIMAL, "Voltage", "V", parameter.voltage}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_2_DECIMAL, "Voltage BEC", "V", parameter.bec_voltage}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_0_DECIMAL, "Temp ESC", "C", parameter.temp_esc}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_0_DECIMAL, "Temp Motor", "C", parameter.temp_motor}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_2_DECIMAL, "Cell Voltage", "V", parameter.cell_voltage}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT22, JETIEX_FORMAT_0_DECIMAL, "Consumption", "mAh", parameter.consumption}; jeti_add_sensor(new_sensor, sensor); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_SMART) { smart_esc_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temperature_fet = malloc(sizeof(float)); parameter.temperature_bec = malloc(sizeof(float)); parameter.voltage_bec = malloc(sizeof(float)); parameter.current_bec = malloc(sizeof(float)); parameter.temperature_bat = malloc(sizeof(float)); parameter.current_bat = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); for (uint i = 0; i < 18; i++) parameter.cell[i] = malloc(sizeof(float)); parameter.cells = malloc(sizeof(uint8_t)); parameter.cycles = malloc(sizeof(uint16_t)); xTaskCreate(smart_esc_task, "smart_esc_task", STACK_SMART_ESC, (void *)¶meter, 4, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT22, JETIEX_FORMAT_0_DECIMAL, "RPM", "RPM", parameter.rpm}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_1_DECIMAL, "Current", "A", parameter.current}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_2_DECIMAL, "Voltage", "V", parameter.voltage}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_1_DECIMAL, "Current BEC", "A", parameter.current_bec}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_2_DECIMAL, "Voltage BEC", "V", parameter.voltage_bec}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_0_DECIMAL, "Temp FET", "C", parameter.temperature_fet}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_0_DECIMAL, "Temp BEC", "C", parameter.temperature_bec}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT22, JETIEX_FORMAT_0_DECIMAL, "Consumption", "mAh", parameter.consumption}; jeti_add_sensor(new_sensor, sensor); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_gps) { gps_parameters_t parameter; parameter.protocol = config->gps_protocol; parameter.baudrate = config->gps_baudrate; parameter.rate = config->gps_rate; parameter.lat = malloc(sizeof(double)); parameter.lon = malloc(sizeof(double)); parameter.alt = malloc(sizeof(float)); parameter.spd = malloc(sizeof(float)); parameter.cog = malloc(sizeof(float)); parameter.hdop = malloc(sizeof(float)); parameter.sat = malloc(sizeof(float)); parameter.time = malloc(sizeof(float)); parameter.date = malloc(sizeof(float)); parameter.vspeed = malloc(sizeof(float)); parameter.dist = malloc(sizeof(float)); parameter.spd_kmh = malloc(sizeof(float)); parameter.fix = malloc(sizeof(float)); parameter.vdop = malloc(sizeof(float)); parameter.speed_acc = malloc(sizeof(float)); parameter.h_acc = malloc(sizeof(float)); parameter.v_acc = malloc(sizeof(float)); parameter.track_acc = malloc(sizeof(float)); parameter.n_vel = malloc(sizeof(float)); parameter.e_vel = malloc(sizeof(float)); parameter.v_vel = malloc(sizeof(float)); parameter.alt_elipsiod = malloc(sizeof(float)); parameter.pdop = malloc(sizeof(float)); parameter.fix_type = malloc(sizeof(uint8_t)); parameter.home_set = malloc(sizeof(uint8_t)); parameter.alt_home = malloc(sizeof(float)); xTaskCreate(gps_task, "gps_task", STACK_GPS, (void *)¶meter, 2, &task_handle); context.uart_pio_notify_task_handle = task_handle; new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT6, JETIEX_FORMAT_0_DECIMAL, "Sats", "", parameter.sat}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_COORDINATES, JETIEX_FORMAT_LAT, "Latitude", "", parameter.lat}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_COORDINATES, JETIEX_FORMAT_LON, "Longitude", "", parameter.lon}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT22, JETIEX_FORMAT_1_DECIMAL, "Altitude", "m", parameter.alt}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); if (config->jeti_gps_speed_units_kmh) *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_1_DECIMAL, "Speed", "km/h", parameter.spd_kmh}; else *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_1_DECIMAL, "Speed", "kts", parameter.spd}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_1_DECIMAL, "COG", "", parameter.cog}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_1_DECIMAL, "Vspeed", "m/s", parameter.vspeed}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT22, JETIEX_FORMAT_0_DECIMAL, "Dist", "m", parameter.dist}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_TIMEDATE, JETIEX_FORMAT_TIME, "Time", "", parameter.time}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_TIMEDATE, JETIEX_FORMAT_DATE, "Date", "", parameter.date}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_1_DECIMAL, "HDOP", "", parameter.hdop}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_2_DECIMAL, "PDOP", "", parameter.pdop}; jeti_add_sensor(new_sensor, sensor); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_voltage) { voltage_parameters_t parameter = {0, config->analog_rate, config->alpha_voltage, config->analog_voltage_multiplier, malloc(sizeof(float))}; xTaskCreate(voltage_task, "voltage_task", STACK_VOLTAGE, (void *)¶meter, 2, &task_handle); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_2_DECIMAL, "Voltage", "V", parameter.voltage}; jeti_add_sensor(new_sensor, sensor); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_current) { current_parameters_t parameter = {1, config->analog_rate, config->alpha_current, config->analog_current_multiplier, config->analog_current_offset, config->analog_current_autoffset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(current_task, "current_task", STACK_CURRENT, (void *)¶meter, 2, &task_handle); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_1_DECIMAL, "Current", "A", parameter.current}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT22, JETIEX_FORMAT_0_DECIMAL, "Consumption", "mAh", parameter.consumption}; jeti_add_sensor(new_sensor, sensor); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_ntc) { ntc_parameters_t parameter = {2, config->analog_rate, config->ntc_offset, config->alpha_temperature, malloc(sizeof(float))}; xTaskCreate(ntc_task, "ntc_task", STACK_NTC, (void *)¶meter, 2, &task_handle); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_0_DECIMAL, "Temperature", "C", parameter.ntc}; jeti_add_sensor(new_sensor, sensor); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_BMP280) { bmp280_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, 0, config->bmp280_filter, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(bmp280_task, "bmp280_task", STACK_BMP280, (void *)¶meter, 2, &task_handle); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_0_DECIMAL, "Air temperature", "C", parameter.temperature}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_0_DECIMAL, "Altitude", "m", parameter.altitude}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT22, JETIEX_FORMAT_2_DECIMAL, "Vspeed", "m/s", parameter.vspeed}; jeti_add_sensor(new_sensor, sensor); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_MS5611) { ms5611_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, 0, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(ms5611_task, "ms5611_task", STACK_MS5611, (void *)¶meter, 2, &task_handle); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_0_DECIMAL, "Air temperature", "C", parameter.temperature}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_0_DECIMAL, "Altitude", "m", parameter.altitude}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT22, JETIEX_FORMAT_2_DECIMAL, "Vspeed", "m/s", parameter.vspeed}; jeti_add_sensor(new_sensor, sensor); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_BMP180) { bmp180_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(bmp180_task, "bmp180_task", STACK_BMP180, (void *)¶meter, 2, &task_handle); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_0_DECIMAL, "Air temperature", "C", parameter.temperature}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_0_DECIMAL, "Altitude", "m", parameter.altitude}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT22, JETIEX_FORMAT_2_DECIMAL, "Vspeed", "m/s", parameter.vspeed}; jeti_add_sensor(new_sensor, sensor); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_airspeed) { airspeed_parameters_t parameter = {3, config->analog_rate, config->alpha_airspeed, (float)config->airspeed_offset / 1000, (float)config->airspeed_vcc / 100, baro_temp, baro_pressure, malloc(sizeof(float))}; xTaskCreate(airspeed_task, "airspeed_task", STACK_AIRSPEED, (void *)¶meter, 2, &task_handle); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_1_DECIMAL, "Air speed", "km/h", parameter.airspeed}; jeti_add_sensor(new_sensor, sensor); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_fuel_flow) { fuel_meter_parameters_t parameter = {config->fuel_flow_ml_per_pulse, malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(fuel_meter_task, "fuel_meter_task", STACK_FUEL_METER, (void *)¶meter, 2, &task_handle); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_2_DECIMAL, "Instant consumption", "ml/min", parameter.consumption_instant}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_1_DECIMAL, "Total consumption", "ml", parameter.consumption_total}; jeti_add_sensor(new_sensor, sensor); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_fuel_pressure) { xgzp68xxd_parameters_t parameter = {config->xgzp68xxd_k, malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(xgzp68xxd_task, "fuel_pressure_task", STACK_FUEL_PRESSURE, (void *)¶meter, 2, &task_handle); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT22, JETIEX_FORMAT_0_DECIMAL, "Tank pressure", "Pa", parameter.pressure}; jeti_add_sensor(new_sensor, sensor); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_gyro) { mpu6050_parameters_t parameter = {1, 0, config->mpu6050_acc_scale, config->mpu6050_gyro_scale, config->mpu6050_gyro_weighting, config->mpu6050_filter, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(mpu6050_task, "mpu6050_task", STACK_MPU6050, (void *)¶meter, 2, &task_handle); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_0_DECIMAL, "Pitch", "dps", parameter.pitch}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_0_DECIMAL, "Roll", "dps", parameter.roll}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_0_DECIMAL, "Yaw", "dps", parameter.yaw}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_0_DECIMAL, "Acc X", "g", parameter.acc_x}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_0_DECIMAL, "Acc Y", "g", parameter.acc_y}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_0_DECIMAL, "Acc Z", "g", parameter.acc_z}; jeti_add_sensor(new_sensor, sensor); new_sensor = malloc(sizeof(sensor_jetiex_t)); *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_0_DECIMAL, "Acc", "g", parameter.acc}; jeti_add_sensor(new_sensor, sensor); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_lipo) { float *cell_prev = 0; if (config->lipo_cells > 0) { ina3221_parameters_t parameter = { .i2c_address = 0x40, .filter = config->ina3221_filter, .cell_count = MIN(config->lipo_cells, 3), .cell[0] = malloc(sizeof(float)), .cell[1] = malloc(sizeof(float)), .cell[2] = malloc(sizeof(float)), .cell_prev = malloc(sizeof(float)), }; *parameter.cell_prev = 0; cell_prev = parameter.cell[2]; xTaskCreate(ina3221_task, "ina3221_task", STACK_INA3221, (void *)¶meter, 2, &task_handle); for (uint8_t i = 0; i < MIN(parameter.cell_count, 3); i++) { new_sensor = malloc(sizeof(sensor_jetiex_t)); new_sensor->data_id = 0; new_sensor->type = JETIEX_TYPE_INT14; new_sensor->format = JETIEX_FORMAT_2_DECIMAL; new_sensor->value = parameter.cell[i]; jeti_add_sensor(new_sensor, sensor); } ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->lipo_cells > 3) { ina3221_parameters_t parameter = { .i2c_address = 0x41, .filter = config->ina3221_filter, .cell_count = MIN(config->lipo_cells - 3, 3), .cell[0] = malloc(sizeof(float)), .cell[1] = malloc(sizeof(float)), .cell[2] = malloc(sizeof(float)), .cell_prev = malloc(sizeof(float)), }; parameter.cell_prev = cell_prev; cell_prev = parameter.cell[2]; xTaskCreate(ina3221_task, "ina3221_task", STACK_INA3221, (void *)¶meter, 2, &task_handle); for (uint8_t i = 0; i < parameter.cell_count; i++) { new_sensor = malloc(sizeof(sensor_jetiex_t)); new_sensor->data_id = 0; new_sensor->type = JETIEX_TYPE_INT14; new_sensor->format = JETIEX_FORMAT_2_DECIMAL; new_sensor->value = parameter.cell[i]; jeti_add_sensor(new_sensor, sensor); } ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } } } static void process(uint *baudrate, sensor_jetiex_t **sensor) { static alarm_id_t timeout_alarm_id = 0; uint8_t packetId; uint8_t length = uart0_available(); if (length) { uint8_t data[length]; uart0_read_bytes(data, length); if (context.debug == 2) { printf("\nJeti Ex(%u) < ", uxTaskGetStackHighWaterMark(NULL)); for (uint8_t i = 0; i < length; i++) { printf("%X ", data[i]); } } uint8_t packet[JETIEX_PACKET_LENGHT]; if (data[0] == 0x3E && data[1] == 0x3 && length - data[2] == JETIEX_PACKET_LENGHT) { memcpy(packet, data + data[2], JETIEX_PACKET_LENGHT); debug("\nJeti Ex(%u) < ", uxTaskGetStackHighWaterMark(NULL)); debug_buffer(packet, JETIEX_PACKET_LENGHT, "0x%X "); } else if (length == JETIEX_PACKET_LENGHT) { memcpy(packet, data, JETIEX_PACKET_LENGHT); } else { return; } if (crc16(packet, JETIEX_PACKET_LENGHT) == 0) { if (packet[0] == 0x3D && packet[1] == 0x01 && packet[4] == 0x3A) { if (timeout_alarm_id) cancel_alarm(timeout_alarm_id); uint8_t packet_id = packet[3]; send_packet(packet_id, sensor); timeout_alarm_id = add_alarm_in_ms(JETIEX_BAUDRATE_TIMEOUT_MS, timeout_callback, &baudrate, false); } } } } static void send_packet(uint8_t packet_id, sensor_jetiex_t **sensor) { static uint8_t packet_count = 0; uint8_t ex_buffer[36] = {0}; uint8_t length_telemetry_buffer = jeti_create_telemetry_buffer(ex_buffer + 6, packet_count % 16, sensor); ex_buffer[0] = 0x3B; ex_buffer[1] = 0x01; ex_buffer[2] = length_telemetry_buffer + 8; ex_buffer[3] = packet_id; ex_buffer[4] = 0x3A; ex_buffer[5] = length_telemetry_buffer; uint16_t crc = crc16(ex_buffer, length_telemetry_buffer + 6); ex_buffer[length_telemetry_buffer + 6] = crc; ex_buffer[length_telemetry_buffer + 7] = crc >> 8; uart0_write_bytes(ex_buffer, length_telemetry_buffer + 8); debug("\nJeti Ex %s (%u) > ", packet_count % 16 ? "Values " : "Text ", uxTaskGetStackHighWaterMark(NULL)); debug_buffer(ex_buffer, length_telemetry_buffer + 8, "0x%X "); packet_count++; // blink led vTaskResume(context.led_task_handle); } static void add_sensor_value(uint8_t *buffer, uint8_t *buffer_index, uint8_t sensor_index, sensor_jetiex_t *sensor) { if (sensor) { uint8_t format = sensor->format << 5; if (sensor->type == JETIEX_TYPE_INT6) { int8_t value = *sensor->value * pow(10, sensor->format); if (value > 0x1F) value = 0x1F; else if (value < -0x1F) value = -0x1F; value &= ~(3 << 5); value |= format; *(buffer + *buffer_index) = sensor_index << 4 | sensor->type; *(buffer + *buffer_index + 1) = value; *buffer_index += 2; } else if (sensor->type == JETIEX_TYPE_INT14) { int16_t value = *sensor->value * pow(10, sensor->format); if (value > 0x1FFF) value = 0x1FFF; if (value < -0x1FFF) value = -0x1FFF; value &= ~((uint16_t)3 << (5 + 8)); value |= (uint16_t)format << 8; *(buffer + *buffer_index) = sensor_index << 4 | sensor->type; *(buffer + *buffer_index + 1) = value; *(buffer + *buffer_index + 2) = value >> 8; *buffer_index += 3; } else if (sensor->type == JETIEX_TYPE_INT22) { int32_t value = *sensor->value * pow(10, sensor->format); if (value > 0x1FFFFF) value = 0x1FFFFF; else if (value < -0x1FFFFF) value = -0x1FFFFF; value &= ~((uint32_t)3 << (5 + 16)); value |= (uint32_t)format << 16; *(buffer + *buffer_index) = sensor_index << 4 | sensor->type; *(buffer + *buffer_index + 1) = value; *(buffer + *buffer_index + 2) = value >> 8; *(buffer + *buffer_index + 3) = value >> 16; *buffer_index += 4; } else if (sensor->type == JETIEX_TYPE_INT30) { int32_t value = *sensor->value * pow(10, sensor->format); if (value > 0x1FFFFFFF) value = 0x1FFFFFFF; else if (value < -0x1FFFFFFF) value = -0x1FFFFFFF; value &= ~((uint32_t)3 << (5 + 24)); value |= (uint32_t)format << 24; *(buffer + *buffer_index) = sensor_index << 4 | sensor->type; *(buffer + *buffer_index + 1) = value; *(buffer + *buffer_index + 2) = value >> 8; *(buffer + *buffer_index + 3) = value >> 16; *(buffer + *buffer_index + 4) = value >> 24; *buffer_index += 5; } else if (sensor->type == JETIEX_TYPE_TIMEDATE) { // rawvalue: yymmdd/hhmmss // byte 1: day/second // byte 2: month/minute // byte 3(bits 1-5): year/hour // byte 3(bit 6): 0=time 1=date uint32_t value = *sensor->value; uint8_t hourYearFormat = format; hourYearFormat |= value / 10000; // hour, year uint8_t minuteMonth = (value / 100 - (value / 10000) * 100); // minute, month uint8_t secondDay = value - (value / 100) * 100; // second, day *(buffer + *buffer_index) = sensor_index << 4 | sensor->type; *(buffer + *buffer_index + 1) = secondDay; *(buffer + *buffer_index + 2) = minuteMonth; *(buffer + *buffer_index + 3) = hourYearFormat; *buffer_index += 4; } else if (sensor->type == JETIEX_TYPE_COORDINATES) { // rawvalue: minutes // byte 1-2: MMmmm // byte 3: DD // byte 4(bit 6): 0=lat 1=lon // byte 4(bit 7): 0=+(N,E), 1=-(S,W) float value = *sensor->value; if (value < 0) { format |= 1 << 6; value *= -1; } *(buffer + *buffer_index) = sensor_index << 4 | sensor->type; uint8_t degrees = value; uint16_t minutes = (value - degrees) * 60 * 1000; *(buffer + *buffer_index + 1) = minutes; *(buffer + *buffer_index + 2) = minutes >> 8; *(buffer + *buffer_index + 3) = degrees; *(buffer + *buffer_index + 4) = format; *buffer_index += 5; } } } static void add_sensor_text(uint8_t *buffer, uint8_t *buffer_index, uint8_t sensor_index, sensor_jetiex_t *sensor) { uint8_t lenText, lenUnit; if (sensor) { lenText = strlen(sensor->text); lenUnit = strlen(sensor->unit); } else { lenText = strlen("MSRC"); lenUnit = strlen(""); } *(buffer + *buffer_index) = sensor_index; *(buffer + *buffer_index + 1) = lenText << 3 | lenUnit; *buffer_index += 2; strcpy((char *)buffer + *buffer_index, sensor ? sensor->text : "MSRC"); *buffer_index += lenText; strcpy((char *)buffer + *buffer_index, sensor ? sensor->unit : ""); *buffer_index += lenUnit; } static int64_t timeout_callback(alarm_id_t id, void *parameters) { float *baudrate = (float *)parameters; if (*baudrate == 125000L) *baudrate = 250000L; else *baudrate = 125000L; uart0_begin(*baudrate, UART_RECEIVER_TX, UART_RECEIVER_RX, JETIEX_TIMEOUT_US, 8, 1, UART_PARITY_NONE, false, true); debug("\nJeti Ex timeout. Baudrate"); return 0; } static uint8_t crc8(uint8_t *crc, uint8_t crc_length) { uint8_t crc_up = 0; uint8_t c; for (c = 0; c < crc_length; c++) { crc_up = update_crc8(crc[c], crc_up); } return crc_up; } static uint8_t update_crc8(uint8_t crc, uint8_t crc_seed) { uint8_t crc_u; uint8_t i; crc_u = crc; crc_u ^= crc_seed; for (i = 0; i < 8; i++) { crc_u = (crc_u & 0x80) ? 0x07 ^ (crc_u << 1) : (crc_u << 1); } return crc_u; } static uint16_t crc16(uint8_t *p, uint16_t len) { uint16_t crc16_data = 0; while (len--) { crc16_data = update_crc16(crc16_data, p[0]); p++; } return (crc16_data); } static uint16_t update_crc16(uint16_t crc, uint8_t data) { uint16_t ret_val; data ^= (uint8_t)(crc) & (uint8_t)(0xFF); data ^= data << 4; ret_val = ((((uint16_t)data << 8) | ((crc & 0xFF00) >> 8)) ^ (uint8_t)(data >> 4) ^ ((uint16_t)data << 3)); return ret_val; } ================================================ FILE: board/project/protocol/jetiex.h ================================================ #ifndef JETIEX_H #define JETIEX_H #include "common.h" #define JETIEX_TYPE_INT6 0 #define JETIEX_TYPE_INT14 1 #define JETIEX_TYPE_INT22 4 #define JETIEX_TYPE_TIMEDATE 5 #define JETIEX_TYPE_INT30 8 #define JETIEX_TYPE_COORDINATES 9 #define JETIEX_FORMAT_0_DECIMAL 0 #define JETIEX_FORMAT_1_DECIMAL 1 #define JETIEX_FORMAT_2_DECIMAL 2 #define JETIEX_FORMAT_3_DECIMAL 3 #define JETIEX_FORMAT_DATE 1 #define JETIEX_FORMAT_LON 1 #define JETIEX_FORMAT_TIME 0 #define JETIEX_FORMAT_LAT 0 #define JETIEX_MFG_ID_LOW 0x00 #define JETIEX_MFG_ID_HIGH 0xA4 #define JETIEX_DEV_ID_LOW 0x00 #define JETIEX_DEV_ID_HIGH 0xA4 typedef struct sensor_jetiex_t { uint8_t data_id; uint8_t type; uint8_t format; char text[32]; char unit[8]; float *value; } sensor_jetiex_t; extern context_t context; void jetiex_task(void *parameters); void jeti_add_sensor(sensor_jetiex_t *new_sensor, sensor_jetiex_t **sensors); uint8_t jeti_create_telemetry_buffer(uint8_t *buffer, bool packet_type, sensor_jetiex_t **sensor); void jeti_set_config(sensor_jetiex_t **sensor); #endif ================================================ FILE: board/project/protocol/jetiex_sensor.c ================================================ #include "jetiex_sensor.h" #include #include #include "airspeed.h" #include "bmp180.h" #include "bmp280.h" #include "config.h" #include "current.h" #include "esc_apd_f.h" #include "esc_apd_hv.h" #include "esc_castle.h" #include "esc_hw3.h" #include "esc_hw4.h" #include "esc_hw5.h" #include "esc_kontronik.h" #include "esc_omp_m4.h" #include "esc_pwm.h" #include "esc_ztw.h" #include "fuel_meter.h" #include "gps.h" #include "mpu6050.h" #include "ms5611.h" #include "ntc.h" #include "pwm_out.h" #include "smart_esc.h" #include "stdlib.h" #include "string.h" #include "uart.h" #include "uart_pio.h" #include "voltage.h" #include "xgzp68xxd.h" #define JETIEX_SENSOR_INTERVAL_MS 150 static int64_t alarm_packet(alarm_id_t id, void *user_data); static void process(uint *baudrate, sensor_jetiex_t **sensor); static void send_packet(sensor_jetiex_t **sensor); static int64_t timeout_callback(alarm_id_t id, void *parameters); void jetiex_sensor_task(void *parameters) { uint baudrate = 9600; sensor_jetiex_t *sensor[16] = {NULL}; context.led_cycle_duration = 6; context.led_cycles = 1; uart_pio_begin(baudrate, UART_RECEIVER_TX, UART_GPIO_NONE, 0, pio1, PIO1_IRQ_0, 9, 2, UART_PARITY_ODD); jeti_set_config(sensor); add_alarm_in_us(0, alarm_packet, NULL, true); debug("\nJeti Ex Sensor init"); while (1) { ulTaskNotifyTakeIndexed(1, pdTRUE, portMAX_DELAY); send_packet(sensor); } } static void send_packet(sensor_jetiex_t **sensor) { static uint8_t packet_count = 0; // send telemetry frame uint8_t buffer[36] = {0}; uint length_telemetry_buffer = jeti_create_telemetry_buffer(buffer, packet_count % 16, sensor); uart_pio_write(0x7E); for (uint8_t i = 0; i < length_telemetry_buffer; i++) uart_pio_write((uint32_t)buffer[i] | 0x100); // send simple text frame uart_pio_write(0xFE); for (uint8_t i = 0; i < 32; i++) uart_pio_write(0x100); uart_pio_write(0xFF); debug("\nJeti Ex Sensor %s (%u) > ", packet_count % 16 ? "Values " : "Text ", uxTaskGetStackHighWaterMark(NULL)); debug_buffer(buffer, length_telemetry_buffer, "0x%X "); packet_count++; // blink led vTaskResume(context.led_task_handle); } static int64_t alarm_packet(alarm_id_t id, void *user_data) { BaseType_t xHigherPriorityTaskWoken = pdFALSE; vTaskNotifyGiveIndexedFromISR(context.receiver_task_handle, 1, &xHigherPriorityTaskWoken); portYIELD_FROM_ISR(xHigherPriorityTaskWoken); return JETIEX_SENSOR_INTERVAL_MS * 1000; } ================================================ FILE: board/project/protocol/jetiex_sensor.h ================================================ #ifndef JETIEX_SENSOR_H #define JETIEX_SENSOR_H #include "common.h" #include "jetiex.h" extern context_t context; void jetiex_sensor_task(void *parameters); #endif ================================================ FILE: board/project/protocol/jr_dmss.c ================================================ #include "jr_dmss.h" #include #include #include "airspeed.h" #include "bmp180.h" #include "bmp280.h" #include "config.h" #include "current.h" #include "esc_apd_f.h" #include "esc_apd_hv.h" #include "esc_castle.h" #include "esc_hw3.h" #include "esc_hw4.h" #include "esc_hw5.h" #include "esc_kontronik.h" #include "esc_omp_m4.h" #include "esc_openyge.h" #include "esc_pwm.h" #include "esc_ztw.h" #include "ms5611.h" #include "ntc.h" #include "pico/stdlib.h" #include "pwm_out.h" #include "smart_esc.h" #include "stdlib.h" #include "uart.h" #include "voltage.h" #define JR_DMSS_TEMPERATURE_SENSOR_ID 1 #define JR_DMSS_RPM_SENSOR_ID 2 #define JR_DMSS_VARIO_SENSOR_ID 3 #define JR_DMSS_AIRSPEED_SENSOR_ID 5 #define JR_DMSS_BATTERY_SENSOR_ID 8 #define JR_DMSS_TEMPERATURE_TEMPERATURE_INDEX 0x0 // 1ºC #define JR_DMSS_RPM_RPM_INDEX 0x0 // 1 rpm #define JR_DMSS_VARIO_ALTITUDE_INDEX 0x3 // 0.1m #define JR_DMSS_VARIO_VSPEED_INDEX 0x2 // 0.1 m/s #define JR_DMSS_VARIO_PRESSURE_INDEX 0x1 // 0.1 hPa #define JR_DMSS_AIRSPEED_AIRSPEED_INDEX 0x0 // 1 km/h #define JR_DMSS_BATTERY_VOLTAGE_INDEX 0x0 // 0.01 V #define JR_DMSS_BATTERY_CURRENT_INDEX 0x80 // 0.01 A #define JR_DMSS_BATTERY_CAPACITY_INDEX 0x8 // 1 mAh #define JR_DMSS_BATTERY_POWER_INDEX 0x90 // 0.1 W #define JR_DMSS_TIMEOUT_US 300 #define JR_DMSS_PACKET_LENGHT 1 #define PIN_BASE 12 typedef enum sensor_t { TEMPERATURE, RPM, ALTITUDE, VSPEED, PRESSURE, AIRSPEED, VOLTAGE, CURRENT, CAPACITY, POWER } sensor_t; static uint8_t s_crc_array[256] = { 0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83, 0xc2, 0x9c, 0x7e, 0x20, 0xa3, 0xfd, 0x1f, 0x41, 0x9d, 0xc3, 0x21, 0x7f, 0xfc, 0xa2, 0x40, 0x1e, 0x5f, 0x01, 0xe3, 0xbd, 0x3e, 0x60, 0x82, 0xdc, 0x23, 0x7d, 0x9f, 0xc1, 0x42, 0x1c, 0xfe, 0xa0, 0xe1, 0xbf, 0x5d, 0x03, 0x80, 0xde, 0x3c, 0x62, 0xbe, 0xe0, 0x02, 0x5c, 0xdf, 0x81, 0x63, 0x3d, 0x7c, 0x22, 0xc0, 0x9e, 0x1d, 0x43, 0xa1, 0xff, 0x46, 0x18, 0xfa, 0xa4, 0x27, 0x79, 0x9b, 0xc5, 0x84, 0xda, 0x38, 0x66, 0xe5, 0xbb, 0x59, 0x07, 0xdb, 0x85, 0x67, 0x39, 0xba, 0xe4, 0x06, 0x58, 0x19, 0x47, 0xa5, 0xfb, 0x78, 0x26, 0xc4, 0x9a, 0x65, 0x3b, 0xd9, 0x87, 0x04, 0x5a, 0xb8, 0xe6, 0xa7, 0xf9, 0x1b, 0x45, 0xc6, 0x98, 0x7a, 0x24, 0xf8, 0xa6, 0x44, 0x1a, 0x99, 0xc7, 0x25, 0x7b, 0x3a, 0x64, 0x86, 0xd8, 0x5b, 0x05, 0xe7, 0xb9, 0x8c, 0xd2, 0x30, 0x6e, 0xed, 0xb3, 0x51, 0x0f, 0x4e, 0x10, 0xf2, 0xac, 0x2f, 0x71, 0x93, 0xcd, 0x11, 0x4f, 0xad, 0xf3, 0x70, 0x2e, 0xcc, 0x92, 0xd3, 0x8d, 0x6f, 0x31, 0xb2, 0xec, 0x0e, 0x50, 0xaf, 0xf1, 0x13, 0x4d, 0xce, 0x90, 0x72, 0x2c, 0x6d, 0x33, 0xd1, 0x8f, 0x0c, 0x52, 0xb0, 0xee, 0x32, 0x6c, 0x8e, 0xd0, 0x53, 0x0d, 0xef, 0xb1, 0xf0, 0xae, 0x4c, 0x12, 0x91, 0xcf, 0x2d, 0x73, 0xca, 0x94, 0x76, 0x28, 0xab, 0xf5, 0x17, 0x49, 0x08, 0x56, 0xb4, 0xea, 0x69, 0x37, 0xd5, 0x8b, 0x57, 0x09, 0xeb, 0xb5, 0x36, 0x68, 0x8a, 0xd4, 0x95, 0xcb, 0x29, 0x77, 0xf4, 0xaa, 0x48, 0x16, 0xe9, 0xb7, 0x55, 0x0b, 0x88, 0xd6, 0x34, 0x6a, 0x2b, 0x75, 0x97, 0xc9, 0x4a, 0x14, 0xf6, 0xa8, 0x74, 0x2a, 0xc8, 0x96, 0x15, 0x4b, 0xa9, 0xf7, 0xb6, 0xe8, 0x0a, 0x54, 0xd7, 0x89, 0x6b, 0x35}; static void process(float **sensor); static void send_packet(uint8_t address, float **sensor); static uint8_t crc_tab1e(uint8_t data, uint8_t crc); static uint8_t crc8(uint8_t *buffer, uint8_t length); static void set_config(float **sensor); void jr_dmss_task(void *parameters) { float *sensor[10] = {NULL}; context.led_cycle_duration = 6; context.led_cycles = 1; uart0_begin(250000L, UART_RECEIVER_TX, UART_RECEIVER_RX, JR_DMSS_TIMEOUT_US, 8, 2, UART_PARITY_NONE, false, true); set_config(sensor); debug("\nJR Propo init"); while (1) { ulTaskNotifyTakeIndexed(1, pdTRUE, portMAX_DELAY); process(sensor); } } static void process(float **sensor) { uint8_t len = uart0_available(); uint8_t buffer[len]; uart0_read_bytes(buffer, len); if (len == JR_DMSS_PACKET_LENGHT) { // debug("\nJR Propo (%u) < %X", uxTaskGetStackHighWaterMark(NULL), buffer[0]); send_packet(buffer[0], sensor); } } static void send_packet(uint8_t address, float **sensor) { uint8_t buffer[6]; switch (address) { case JR_DMSS_TEMPERATURE_SENSOR_ID: { if (sensor[TEMPERATURE] == NULL) return; buffer[0] = JR_DMSS_TEMPERATURE_SENSOR_ID | 0xE0; buffer[1] = 0x3; buffer[2] = JR_DMSS_TEMPERATURE_TEMPERATURE_INDEX; uint16_t value = *sensor[TEMPERATURE]; buffer[3] = value >> 8; buffer[4] = value; buffer[5] = crc8(buffer, 5); uart0_write_bytes(buffer, sizeof(buffer)); vTaskResume(context.led_task_handle); debug("\nJR Propo (%u) > ", uxTaskGetStackHighWaterMark(NULL)); debug_buffer(buffer, sizeof(buffer), "0x%X "); break; } case JR_DMSS_RPM_SENSOR_ID: { if (sensor[RPM] == NULL) return; buffer[0] = JR_DMSS_RPM_SENSOR_ID | 0xE0; buffer[1] = 0x3; buffer[2] = JR_DMSS_RPM_RPM_INDEX; uint16_t value = *sensor[RPM]; buffer[3] = value >> 8; buffer[4] = value; buffer[5] = crc8(buffer, 5); uart0_write_bytes(buffer, sizeof(buffer)); vTaskResume(context.led_task_handle); debug("\nJR Propo (%u) > ", uxTaskGetStackHighWaterMark(NULL)); debug_buffer(buffer, sizeof(buffer), "0x%X "); break; } case JR_DMSS_VARIO_SENSOR_ID: { { static uint8_t index = 0; buffer[0] = JR_DMSS_VARIO_SENSOR_ID | 0xE0; buffer[1] = 0x3; uint16_t value; if ((index % 3) == 0) { if (sensor[ALTITUDE] == NULL) { index++; return; } buffer[2] = JR_DMSS_VARIO_ALTITUDE_INDEX; value = *sensor[ALTITUDE] * 10; } else if ((index % 3) == 1) { if (sensor[VSPEED] == NULL) { index++; return; } buffer[2] = JR_DMSS_VARIO_VSPEED_INDEX; value = *sensor[VSPEED] * 10; } else if ((index % 3) == 2) { if (sensor[PRESSURE] == NULL) { index++; return; } buffer[2] = JR_DMSS_VARIO_PRESSURE_INDEX; value = *sensor[PRESSURE] / 100 * 10; // Pa -> 0.1 hPa } buffer[3] = value >> 8; buffer[4] = value; buffer[5] = crc8(buffer, 5); uart0_write_bytes(buffer, sizeof(buffer)); vTaskResume(context.led_task_handle); debug("\nJR Propo (%u) > ", uxTaskGetStackHighWaterMark(NULL)); debug_buffer(buffer, sizeof(buffer), "0x%X "); index++; break; } } case JR_DMSS_AIRSPEED_SENSOR_ID: { if (sensor[AIRSPEED] == NULL) return; buffer[0] = JR_DMSS_AIRSPEED_SENSOR_ID | 0xE0; buffer[1] = 0x3; buffer[2] = JR_DMSS_AIRSPEED_AIRSPEED_INDEX; uint16_t value = *sensor[AIRSPEED]; buffer[3] = value >> 8; buffer[4] = value; buffer[5] = crc8(buffer, 5); uart0_write_bytes(buffer, sizeof(buffer)); vTaskResume(context.led_task_handle); debug("\nJR Propo (%u) > ", uxTaskGetStackHighWaterMark(NULL)); debug_buffer(buffer, sizeof(buffer), "0x%X "); break; } case JR_DMSS_BATTERY_SENSOR_ID: { static uint8_t index = 0; buffer[0] = JR_DMSS_BATTERY_SENSOR_ID | 0xE0; buffer[1] = 0x3; uint16_t value; if ((index % 3) == 0) { if (sensor[VOLTAGE] == NULL) { index++; return; } buffer[2] = JR_DMSS_BATTERY_VOLTAGE_INDEX; value = *sensor[VOLTAGE] * 100; } else if ((index % 3) == 1) { if (sensor[CURRENT] == NULL) { index++; return; } buffer[2] = JR_DMSS_BATTERY_CURRENT_INDEX; value = *sensor[CURRENT] * 100; } else if ((index % 3) == 2) { if (sensor[CAPACITY] == NULL) { index++; return; } buffer[2] = JR_DMSS_BATTERY_CAPACITY_INDEX; value = *sensor[CAPACITY]; } buffer[3] = value >> 8; buffer[4] = value; buffer[5] = crc8(buffer, 5); uart0_write_bytes(buffer, sizeof(buffer)); vTaskResume(context.led_task_handle); debug("\nJR Propo (%u) > ", uxTaskGetStackHighWaterMark(NULL)); debug_buffer(buffer, sizeof(buffer), "0x%X "); index++; break; } } } static uint8_t crc_tab1e(uint8_t data, uint8_t crc) { uint16_t index = (data ^ crc) & 0xff; crc = s_crc_array[index]; return crc; } static uint8_t crc8(uint8_t *buffer, uint8_t length) { uint8_t crc = 0; while (length-- > 0) { crc = crc_tab1e(*buffer++, crc); } return crc; } static void set_config(float **sensor) { config_t *config = config_read(); TaskHandle_t task_handle; float *new_sensor; float *baro_temp = NULL, *baro_pressure = NULL; if (config->esc_protocol == ESC_PWM) { esc_pwm_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, malloc(sizeof(float))}; xTaskCreate(esc_pwm_task, "esc_pwm_task", STACK_ESC_PWM, (void *)¶meter, 2, &task_handle); new_sensor = malloc(sizeof(float)); new_sensor = parameter.rpm; sensor[RPM] = new_sensor; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_HW3) { esc_hw3_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, malloc(sizeof(float))}; xTaskCreate(esc_hw3_task, "esc_hw3_task", STACK_ESC_HW3, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(float)); new_sensor = parameter.rpm; sensor[VOLTAGE] = new_sensor; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_HW4) { esc_hw4_parameters_t parameter = {config->rpm_multiplier, config->enable_pwm_out, config->enable_esc_hw4_init_delay, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, config->esc_hw4_voltage_multiplier, config->esc_hw4_current_multiplier, config->esc_hw4_current_thresold, config->esc_hw4_current_max, config->esc_hw4_is_manual_offset, config->esc_hw4_auto_detect, config->esc_hw4_offset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_hw4_task, "esc_hw4_task", STACK_ESC_HW4, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); if (config->enable_pwm_out) { xTaskCreate(pwm_out_task, "pwm_out", STACK_PWM_OUT, (void *)parameter.rpm, 2, &task_handle); context.pwm_out_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } new_sensor = malloc(sizeof(float)); new_sensor = parameter.rpm; sensor[RPM] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.voltage; sensor[VOLTAGE] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.current; sensor[CURRENT] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.temperature_fet; sensor[TEMPERATURE] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.consumption; sensor[CAPACITY] = new_sensor; } if (config->esc_protocol == ESC_HW5) { esc_hw5_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_hw5_task, "esc_hw5_task", STACK_ESC_HW5, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); new_sensor = malloc(sizeof(float)); new_sensor = parameter.rpm; sensor[RPM] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.voltage; sensor[VOLTAGE] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.current; sensor[CURRENT] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.temperature_fet; sensor[TEMPERATURE] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.consumption; sensor[CAPACITY] = new_sensor; } if (config->esc_protocol == ESC_CASTLE) { esc_castle_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_castle_task, "esc_castle_task", STACK_ESC_CASTLE, (void *)¶meter, 2, &task_handle); new_sensor = malloc(sizeof(float)); new_sensor = parameter.rpm; sensor[RPM] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.voltage; sensor[VOLTAGE] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.current; sensor[CURRENT] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.temperature; sensor[TEMPERATURE] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.consumption; sensor[CAPACITY] = new_sensor; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_KONTRONIK) { esc_kontronik_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_kontronik_task, "esc_kontronik_task", STACK_ESC_KONTRONIK, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(float)); new_sensor = parameter.rpm; sensor[RPM] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.voltage; sensor[VOLTAGE] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.current; sensor[CURRENT] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.temperature_fet; sensor[TEMPERATURE] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.consumption; sensor[CAPACITY] = new_sensor; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_APD_F) { esc_apd_f_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_apd_f_task, "esc_apd_f_task", STACK_ESC_APD_F, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(float)); new_sensor = parameter.rpm; sensor[RPM] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.voltage; sensor[VOLTAGE] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.current; sensor[CURRENT] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.temperature; sensor[TEMPERATURE] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.consumption; sensor[CAPACITY] = new_sensor; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_APD_HV) { esc_apd_hv_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_apd_hv_task, "esc_apd_hv_task", STACK_ESC_APD_HV, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(float)); new_sensor = parameter.rpm; sensor[RPM] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.voltage; sensor[VOLTAGE] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.current; sensor[CURRENT] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.temperature; sensor[TEMPERATURE] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.consumption; sensor[CAPACITY] = new_sensor; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_SMART) { smart_esc_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temperature_fet = malloc(sizeof(float)); parameter.temperature_bec = malloc(sizeof(float)); parameter.voltage_bec = malloc(sizeof(float)); parameter.current_bec = malloc(sizeof(float)); parameter.temperature_bat = malloc(sizeof(float)); parameter.current_bat = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); for (uint i = 0; i < 18; i++) parameter.cell[i] = malloc(sizeof(float)); parameter.cells = malloc(sizeof(uint8_t)); parameter.cycles = malloc(sizeof(uint16_t)); xTaskCreate(smart_esc_task, "smart_esc_task", STACK_SMART_ESC, (void *)¶meter, 4, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(float)); new_sensor = parameter.rpm; sensor[RPM] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.voltage; sensor[VOLTAGE] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.current; sensor[CURRENT] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.temperature_fet; sensor[TEMPERATURE] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.consumption; sensor[CAPACITY] = new_sensor; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_OMP_M4) { esc_omp_m4_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temp_esc = malloc(sizeof(float)); parameter.temp_motor = malloc(sizeof(float)); parameter.cell_voltage = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); parameter.cell_count = malloc(sizeof(uint8_t)); xTaskCreate(esc_omp_m4_task, "esc_omp_m4_task", STACK_ESC_OMP_M4, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(float)); new_sensor = parameter.rpm; sensor[RPM] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.voltage; sensor[VOLTAGE] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.current; sensor[CURRENT] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.temp_esc; sensor[TEMPERATURE] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.consumption; sensor[CAPACITY] = new_sensor; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_ZTW) { esc_ztw_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temp_esc = malloc(sizeof(float)); parameter.temp_motor = malloc(sizeof(float)); parameter.bec_voltage = malloc(sizeof(float)); parameter.cell_voltage = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); parameter.cell_count = malloc(sizeof(uint8_t)); xTaskCreate(esc_ztw_task, "esc_ztw_task", STACK_ESC_ZTW, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(float)); new_sensor = parameter.rpm; sensor[RPM] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.voltage; sensor[VOLTAGE] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.current; sensor[CURRENT] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.temp_esc; sensor[TEMPERATURE] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.consumption; sensor[CAPACITY] = new_sensor; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_OPENYGE) { esc_openyge_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.pwm_out = config->enable_pwm_out; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temperature_fet = malloc(sizeof(float)); parameter.temperature_bec = malloc(sizeof(float)); parameter.cell_voltage = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); parameter.voltage_bec = malloc(sizeof(float)); parameter.current_bec = malloc(sizeof(float)); parameter.throttle = malloc(sizeof(float)); parameter.pwm_percent = malloc(sizeof(float)); parameter.cell_count = malloc(sizeof(uint8_t)); xTaskCreate(esc_openyge_task, "esc_openyge_task", STACK_ESC_OPENYGE, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(float)); new_sensor = parameter.rpm; sensor[RPM] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.voltage; sensor[VOLTAGE] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.current; sensor[CURRENT] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.temperature_fet; sensor[TEMPERATURE] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.consumption; sensor[CAPACITY] = new_sensor; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_voltage) { voltage_parameters_t parameter = {0, config->analog_rate, config->alpha_voltage, config->analog_voltage_multiplier, malloc(sizeof(float))}; xTaskCreate(voltage_task, "voltage_task", STACK_VOLTAGE, (void *)¶meter, 2, &task_handle); new_sensor = malloc(sizeof(float)); new_sensor = parameter.voltage; sensor[VOLTAGE] = new_sensor; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_current) { current_parameters_t parameter = {1, config->analog_rate, config->alpha_current, config->analog_current_multiplier, config->analog_current_offset, config->analog_current_autoffset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(current_task, "current_task", STACK_CURRENT, (void *)¶meter, 2, &task_handle); new_sensor = malloc(sizeof(float)); new_sensor = parameter.current; sensor[CURRENT] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.consumption; sensor[CAPACITY] = new_sensor; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_ntc) { ntc_parameters_t parameter = {2, config->analog_rate, config->ntc_offset, config->alpha_temperature, malloc(sizeof(float))}; xTaskCreate(ntc_task, "ntc_task", STACK_NTC, (void *)¶meter, 2, &task_handle); new_sensor = malloc(sizeof(float)); new_sensor = parameter.ntc; sensor[TEMPERATURE] = new_sensor; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_BMP280) { bmp280_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, 0, config->bmp280_filter, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(bmp280_task, "bmp280_task", STACK_BMP280, (void *)¶meter, 2, &task_handle); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } new_sensor = malloc(sizeof(float)); new_sensor = parameter.altitude; sensor[ALTITUDE] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.vspeed; sensor[VSPEED] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.pressure; sensor[PRESSURE] = new_sensor; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_MS5611) { ms5611_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, 0, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(ms5611_task, "ms5611_task", STACK_MS5611, (void *)¶meter, 2, &task_handle); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } new_sensor = malloc(sizeof(float)); new_sensor = parameter.altitude; sensor[ALTITUDE] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.vspeed; sensor[VSPEED] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.pressure; sensor[PRESSURE] = new_sensor; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_BMP180) { bmp180_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(bmp180_task, "bmp180_task", STACK_BMP180, (void *)¶meter, 2, &task_handle); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } new_sensor = malloc(sizeof(float)); new_sensor = parameter.altitude; sensor[ALTITUDE] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.vspeed; sensor[VSPEED] = new_sensor; new_sensor = malloc(sizeof(float)); new_sensor = parameter.pressure; sensor[PRESSURE] = new_sensor; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_airspeed) { airspeed_parameters_t parameter = {3, config->analog_rate, config->alpha_airspeed, (float)config->airspeed_offset / 1000, (float)config->airspeed_vcc / 100, baro_temp, baro_pressure, malloc(sizeof(float))}; xTaskCreate(airspeed_task, "airspeed_task", STACK_AIRSPEED, (void *)¶meter, 2, &task_handle); new_sensor = malloc(sizeof(float)); new_sensor = parameter.airspeed; sensor[AIRSPEED] = new_sensor; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } } ================================================ FILE: board/project/protocol/jr_dmss.h ================================================ #ifndef JR_DMSS_H #define JR_DMSS_H #include "common.h" extern context_t context; void jr_dmss_task(void *parameters); #endif ================================================ FILE: board/project/protocol/multiplex.c ================================================ #include "multiplex.h" #include #include #include "airspeed.h" #include "bmp180.h" #include "bmp280.h" #include "config.h" #include "current.h" #include "esc_apd_f.h" #include "esc_apd_hv.h" #include "esc_castle.h" #include "esc_hw3.h" #include "esc_hw4.h" #include "esc_hw5.h" #include "esc_kontronik.h" #include "esc_pwm.h" #include "ms5611.h" #include "gps.h" #include "ntc.h" #include "pico/stdlib.h" #include "pwm_out.h" #include "stdlib.h" #include "uart.h" #include "uart_pio.h" #include "voltage.h" #include "smart_esc.h" #include "esc_omp_m4.h" #include "esc_ztw.h" /* Flysky MULTIPLEX FHSS Data Id */ #define MULTIPLEX_VOLTAGE 1 #define MULTIPLEX_CURRENT 2 #define MULTIPLEX_VARIO 3 #define MULTIPLEX_SPEED 4 #define MULTIPLEX_RPM 5 #define MULTIPLEX_TEMP 6 #define MULTIPLEX_COURSE 7 #define MULTIPLEX_ALTITUDE 8 #define MULTIPLEX_LEVEL 9 #define MULTIPLEX_RSSI 10 #define MULTIPLEX_CONSUMPTION 11 #define MULTIPLEX_FLUID 12 #define MULTIPLEX_DISTANCE 13 #define MULTIPLEX_RECEIVED_NONE 0 #define MULTIPLEX_RECEIVED_POLL 1 #define MULTIPLEX_COMMAND_DISCOVER 0x8 #define MULTIPLEX_COMMAND_TYPE 0x9 #define MULTIPLEX_COMMAND_MEASURE 0xA #define MULTIPLEX_TIMEOUT_US 1000 #define MULTIPLEX_PACKET_LENGHT 1 typedef struct sensor_multiplex_t { uint8_t data_id; float *value; } sensor_multiplex_t; static void process(sensor_multiplex_t **sensor); static void send_packet(uint8_t address, sensor_multiplex_t *sensor); static int16_t format(uint8_t data_id, float value); static void add_sensor(sensor_multiplex_t *new_sensor, sensor_multiplex_t **sensors); static void set_config(sensor_multiplex_t **sensors); void multiplex_task(void *parameters) { sensor_multiplex_t *sensor[16] = {NULL}; context.led_cycle_duration = 6; context.led_cycles = 1; uart0_begin(38400, UART_RECEIVER_TX, UART_RECEIVER_RX, MULTIPLEX_TIMEOUT_US, 8, 1, UART_PARITY_NONE, false, true); set_config(sensor); debug("\nMultiplex init"); while (1) { ulTaskNotifyTakeIndexed(1, pdTRUE, portMAX_DELAY); process(sensor); } } static void process(sensor_multiplex_t **sensor) { uint8_t address = 0; if (uart0_available() == MULTIPLEX_PACKET_LENGHT) { uart0_read_bytes(&address, MULTIPLEX_PACKET_LENGHT); debug("\nMultiplex (%u) < %X", uxTaskGetStackHighWaterMark(NULL), address); if (address < 16) { send_packet(address, sensor[address]); } } } static void send_packet(uint8_t address, sensor_multiplex_t *sensor) { if (!sensor) return; uint8_t sensor_id = address << 4 | sensor->data_id; uart0_write(sensor_id); int16_t value = format(sensor->data_id, *sensor->value); uart0_write_bytes((uint8_t *)&value, 2); vTaskResume(context.led_task_handle); debug("\nMultiplex (%u) > %X %X", uxTaskGetStackHighWaterMark(NULL), sensor_id, value); } static void add_sensor(sensor_multiplex_t *new_sensor, sensor_multiplex_t **sensors) { static uint8_t sensor_count = 0; if (sensor_count < 16) { sensors[sensor_count] = new_sensor; sensor_count++; } } static int16_t format(uint8_t data_id, float value) { int16_t formatted; if (data_id == MULTIPLEX_VOLTAGE || data_id == MULTIPLEX_CURRENT || data_id == MULTIPLEX_VARIO || data_id == MULTIPLEX_SPEED || data_id == MULTIPLEX_TEMP || data_id == MULTIPLEX_COURSE || data_id == MULTIPLEX_DISTANCE) formatted = round(value * 10); else if (data_id == MULTIPLEX_RPM) formatted = round(value / 10); else formatted = round(value); if (formatted > 16383) formatted = 16383; if (formatted < -16383) formatted = -16383; bool isNegative = false; if (formatted < 0) isNegative = true; formatted <<= 1; if (isNegative) formatted |= 1 << 15; return formatted; } static void set_config(sensor_multiplex_t **sensors) { config_t *config = config_read(); TaskHandle_t task_handle; sensor_multiplex_t *new_sensor; float *baro_temp = NULL, *baro_pressure = NULL; if (config->esc_protocol == ESC_PWM) { esc_pwm_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, malloc(sizeof(float))}; xTaskCreate(esc_pwm_task, "esc_pwm_task", STACK_ESC_PWM, (void *)¶meter, 2, &task_handle); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_RPM, parameter.rpm}; add_sensor(new_sensor, sensors); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_HW3) { esc_hw3_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, malloc(sizeof(float))}; xTaskCreate(esc_hw3_task, "esc_hw3_task", STACK_ESC_HW3, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_RPM, parameter.rpm}; add_sensor(new_sensor, sensors); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_HW4) { esc_hw4_parameters_t parameter = {config->rpm_multiplier, config->enable_pwm_out, config->enable_esc_hw4_init_delay, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, config->esc_hw4_voltage_multiplier, config->esc_hw4_current_multiplier, config->esc_hw4_current_thresold, config->esc_hw4_current_max, config->esc_hw4_is_manual_offset, config->esc_hw4_auto_detect, config->esc_hw4_offset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_hw4_task, "esc_hw4_task", STACK_ESC_HW4, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); if (config->enable_pwm_out) { xTaskCreate(pwm_out_task, "pwm_out", STACK_PWM_OUT, (void *)parameter.rpm, 2, &task_handle); context.pwm_out_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_RPM, parameter.rpm}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_VOLTAGE, parameter.voltage}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_CURRENT, parameter.current}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_TEMP, parameter.temperature_fet}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_TEMP, parameter.temperature_bec}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_VOLTAGE, parameter.cell_voltage}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_CONSUMPTION, parameter.consumption}; add_sensor(new_sensor, sensors); } if (config->esc_protocol == ESC_HW5) { esc_hw5_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_hw5_task, "esc_hw5_task", STACK_ESC_HW5, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_RPM, parameter.rpm}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_VOLTAGE, parameter.voltage}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_CURRENT, parameter.current}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_TEMP, parameter.temperature_fet}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_TEMP, parameter.temperature_bec}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_TEMP, parameter.temperature_motor}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_VOLTAGE, parameter.voltage_bec}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_CURRENT, parameter.current_bec}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_VOLTAGE, parameter.cell_voltage}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_CONSUMPTION, parameter.consumption}; add_sensor(new_sensor, sensors); } if (config->esc_protocol == ESC_CASTLE) { esc_castle_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_castle_task, "esc_castle_task", STACK_ESC_CASTLE, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_RPM, parameter.rpm}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_VOLTAGE, parameter.voltage}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_CURRENT, parameter.current}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_TEMP, parameter.temperature}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_VOLTAGE, parameter.cell_voltage}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_CONSUMPTION, parameter.consumption}; add_sensor(new_sensor, sensors); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_KONTRONIK) { esc_kontronik_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_kontronik_task, "esc_kontronik_task", STACK_ESC_KONTRONIK, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_RPM, parameter.rpm}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_VOLTAGE, parameter.voltage}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_CURRENT, parameter.current}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_VOLTAGE, parameter.voltage_bec}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_CURRENT, parameter.current_bec}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_TEMP, parameter.temperature_fet}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_TEMP, parameter.temperature_bec}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_VOLTAGE, parameter.cell_voltage}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_CONSUMPTION, parameter.consumption}; add_sensor(new_sensor, sensors); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_APD_F) { esc_apd_f_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_apd_f_task, "esc_apd_f_task", STACK_ESC_APD_F, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_RPM, parameter.rpm}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_VOLTAGE, parameter.voltage}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_CURRENT, parameter.current}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_TEMP, parameter.temperature}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_VOLTAGE, parameter.cell_voltage}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_CONSUMPTION, parameter.consumption}; add_sensor(new_sensor, sensors); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_APD_HV) { esc_apd_hv_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_apd_hv_task, "esc_apd_hv_task", STACK_ESC_APD_HV, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_RPM, parameter.rpm}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_VOLTAGE, parameter.voltage}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_CURRENT, parameter.current}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_TEMP, parameter.temperature}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_VOLTAGE, parameter.cell_voltage}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_CONSUMPTION, parameter.consumption}; add_sensor(new_sensor, sensors); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_SMART) { smart_esc_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temperature_fet = malloc(sizeof(float)); parameter.temperature_bec = malloc(sizeof(float)); parameter.voltage_bec = malloc(sizeof(float)); parameter.current_bec = malloc(sizeof(float)); parameter.temperature_bat = malloc(sizeof(float)); parameter.current_bat = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); for (uint i = 0; i < 18; i++) parameter.cell[i] = malloc(sizeof(float)); parameter.cells = malloc(sizeof(uint8_t)); parameter.cycles = malloc(sizeof(uint16_t)); xTaskCreate(smart_esc_task, "smart_esc_task", STACK_SMART_ESC, (void *)¶meter, 4, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_RPM, parameter.rpm}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_VOLTAGE, parameter.voltage}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_CURRENT, parameter.current}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_TEMP, parameter.temperature_fet}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_CONSUMPTION, parameter.consumption}; add_sensor(new_sensor, sensors); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_OMP_M4) { esc_omp_m4_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temp_esc = malloc(sizeof(float)); parameter.temp_motor = malloc(sizeof(float)); parameter.cell_voltage = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); parameter.cell_count = malloc(sizeof(uint8_t)); xTaskCreate(esc_omp_m4_task, "esc_omp_m4_task", STACK_ESC_OMP_M4, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_RPM, parameter.rpm}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_VOLTAGE, parameter.voltage}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_CURRENT, parameter.current}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_TEMP, parameter.temp_esc}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_VOLTAGE, parameter.cell_voltage}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_CONSUMPTION, parameter.consumption}; add_sensor(new_sensor, sensors); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_ZTW) { esc_ztw_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temp_esc = malloc(sizeof(float)); parameter.temp_motor = malloc(sizeof(float)); parameter.bec_voltage = malloc(sizeof(float)); parameter.cell_voltage = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); parameter.cell_count = malloc(sizeof(uint8_t)); xTaskCreate(esc_ztw_task, "esc_ztw_task", STACK_ESC_ZTW, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_RPM, parameter.rpm}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_VOLTAGE, parameter.voltage}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_CURRENT, parameter.current}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_TEMP, parameter.temp_esc}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_VOLTAGE, parameter.cell_voltage}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_CONSUMPTION, parameter.consumption}; add_sensor(new_sensor, sensors); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_gps) { gps_parameters_t parameter; parameter.protocol = config->gps_protocol; parameter.baudrate = config->gps_baudrate; parameter.rate = config->gps_rate; parameter.lat = malloc(sizeof(double)); parameter.lon = malloc(sizeof(double)); parameter.alt = malloc(sizeof(float)); parameter.spd = malloc(sizeof(float)); parameter.cog = malloc(sizeof(float)); parameter.hdop = malloc(sizeof(float)); parameter.sat = malloc(sizeof(float)); parameter.time = malloc(sizeof(float)); parameter.date = malloc(sizeof(float)); parameter.vspeed = malloc(sizeof(float)); parameter.dist = malloc(sizeof(float)); parameter.spd_kmh = malloc(sizeof(float)); parameter.fix = malloc(sizeof(float)); parameter.vdop = malloc(sizeof(float)); parameter.speed_acc = malloc(sizeof(float)); parameter.h_acc = malloc(sizeof(float)); parameter.v_acc = malloc(sizeof(float)); parameter.track_acc = malloc(sizeof(float)); parameter.n_vel = malloc(sizeof(float)); parameter.e_vel = malloc(sizeof(float)); parameter.v_vel = malloc(sizeof(float)); parameter.alt_elipsiod = malloc(sizeof(float)); parameter.pdop = malloc(sizeof(float)); parameter.fix_type = malloc(sizeof(uint8_t)); parameter.home_set = malloc(sizeof(uint8_t)); parameter.alt_home = malloc(sizeof(float)); xTaskCreate(gps_task, "gps_task", STACK_GPS, (void *)¶meter, 2, &task_handle); context.uart_pio_notify_task_handle = task_handle; new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_ALTITUDE, parameter.alt}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_SPEED, parameter.spd}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_VARIO, parameter.vspeed}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_DISTANCE, parameter.dist}; add_sensor(new_sensor, sensors); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_voltage) { voltage_parameters_t parameter = {0, config->analog_rate, config->alpha_voltage, config->analog_voltage_multiplier, malloc(sizeof(float))}; xTaskCreate(voltage_task, "voltage_task", STACK_VOLTAGE, (void *)¶meter, 2, &task_handle); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_VOLTAGE, parameter.voltage}; add_sensor(new_sensor, sensors); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_current) { current_parameters_t parameter = {1, config->analog_rate, config->alpha_current, config->analog_current_multiplier, config->analog_current_offset, config->analog_current_autoffset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(current_task, "current_task", STACK_CURRENT, (void *)¶meter, 2, &task_handle); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_CURRENT, parameter.current}; add_sensor(new_sensor, sensors); *new_sensor = (sensor_multiplex_t){MULTIPLEX_CONSUMPTION, parameter.consumption}; add_sensor(new_sensor, sensors); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_ntc) { ntc_parameters_t parameter = {2, config->analog_rate, config->ntc_offset, config->alpha_temperature, malloc(sizeof(float))}; xTaskCreate(ntc_task, "ntc_task", STACK_NTC, (void *)¶meter, 2, &task_handle); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_TEMP, parameter.ntc}; add_sensor(new_sensor, sensors); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_BMP280) { bmp280_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, 0, config->bmp280_filter, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(bmp280_task, "bmp280_task", STACK_BMP280, (void *)¶meter, 2, &task_handle); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_TEMP, parameter.temperature}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_ALTITUDE, parameter.altitude}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_VARIO, parameter.vspeed}; add_sensor(new_sensor, sensors); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_MS5611) { ms5611_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, 0, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(ms5611_task, "ms5611_task", STACK_MS5611, (void *)¶meter, 2, &task_handle); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_TEMP, parameter.temperature}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_ALTITUDE, parameter.altitude}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_VARIO, parameter.vspeed}; add_sensor(new_sensor, sensors); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_BMP180) { bmp180_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(bmp180_task, "bmp180_task", STACK_BMP180, (void *)¶meter, 2, &task_handle); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_TEMP, parameter.temperature}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_ALTITUDE, parameter.altitude}; add_sensor(new_sensor, sensors); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_VARIO, parameter.vspeed}; add_sensor(new_sensor, sensors); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_airspeed) { airspeed_parameters_t parameter = {3, config->analog_rate, config->alpha_airspeed, (float)config->airspeed_offset / 1000, (float)config->airspeed_vcc / 100, baro_temp, baro_pressure, malloc(sizeof(float))}; xTaskCreate(airspeed_task, "airspeed_task", STACK_AIRSPEED, (void *)¶meter, 2, &task_handle); new_sensor = malloc(sizeof(sensor_multiplex_t)); *new_sensor = (sensor_multiplex_t){MULTIPLEX_SPEED, parameter.airspeed}; add_sensor(new_sensor, sensors); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } } ================================================ FILE: board/project/protocol/multiplex.h ================================================ #ifndef MULTIPLEX_H #define MULTIPLEX_H #include "common.h" extern context_t context; void multiplex_task(void *parameters); #endif ================================================ FILE: board/project/protocol/sanwa.c ================================================ #include #include #include #include #include "airspeed.h" #include "bmp180.h" #include "bmp280.h" #include "config.h" #include "crsf.h" #include "current.h" #include "esc_apd_f.h" #include "esc_apd_hv.h" #include "esc_castle.h" #include "esc_hw3.h" #include "esc_hw4.h" #include "esc_hw5.h" #include "esc_kontronik.h" #include "esc_pwm.h" #include "hardware/clocks.h" #include "hardware/pwm.h" #include "ibus.h" #include "ms5611.h" #include "ntc.h" #include "pwm_out.h" #include "uart.h" #include "uart_pio.h" #include "voltage.h" #define SANWA_TIMEOUT_US 500 #define TYPE_TEMP_MOTOR 0 #define TYPE_TEMP_ESC 1 #define TYPE_UNKNOWN 2 #define TYPE_RPM1 3 #define TYPE_RPM2 4 #define TYPE_VOLT 5 #define MAX_SENSORS 6 #define TELEMETRY_PACKET_LENGHT 5 #define CHANNEL_PACKET_LENGHT 10 #define GPIO_PWM_CH1 10 #define GPIO_PWM_CH2 11 #define PWM_FREQ 20 // ms typedef struct sanwa_sensor_formatted_t { uint8_t header; uint8_t type; uint8_t msb; uint8_t lsb; uint8_t crc; } __attribute__((packed)) sanwa_sensor_formatted_t; static void process(float **sensors); static void format_sensor(float *sensor, uint8_t type, sanwa_sensor_formatted_t *sensor_formatted); static void send_packet(float **sensors); static uint8_t get_crc(const uint8_t *buffer, uint len); static void set_config(float **sensors); void sanwa_task(void *parameters) { /*gpio_set_function(GPIO_PWM_CH1, GPIO_FUNC_PWM); gpio_set_function(GPIO_PWM_CH2, GPIO_FUNC_PWM); uint slice_num_ch1 = pwm_gpio_to_slice_num(GPIO_PWM_CH1); uint slice_num_ch2 = pwm_gpio_to_slice_num(GPIO_PWM_CH2); pwm_config config_ch1 = pwm_get_default_config(); pwm_config config_ch2 = pwm_get_default_config(); uint clk_div = PWM_FREQ * clock_get_hz(clk_sys) / 1000 / 65536.0; // clk_div = pulse_freq * clk_sys_hz / (1000 * 65536) pwm_config_set_wrap(&config_ch1, 0xFFFF); pwm_set_gpio_level(GPIO_PWM_CH1, 65536 / 20); // set ch1 to lowest position pwm_set_gpio_level(GPIO_PWM_CH2, 65536 / 20); // set ch2 to lowest position*/ float *sensors[6] = {0}; set_config(sensors); context.led_cycle_duration = 6; context.led_cycles = 1; uart0_begin(115200L, UART_RECEIVER_TX, UART_RECEIVER_RX, SANWA_TIMEOUT_US, 8, 1, UART_PARITY_NONE, false, true); debug("\nSanwa init"); while (1) { ulTaskNotifyTakeIndexed(1, pdTRUE, portMAX_DELAY); process(sensors); } } static void process(float **sensors) { if (uart0_available() == CHANNEL_PACKET_LENGHT) { uint8_t buffer[CHANNEL_PACKET_LENGHT]; uart0_read_bytes(buffer, CHANNEL_PACKET_LENGHT); debug("\nSanwa (%u) < ", uxTaskGetStackHighWaterMark(NULL)); debug_buffer(buffer, CHANNEL_PACKET_LENGHT, "0x%X "); if (buffer[0] == 0x01 && get_crc(buffer, CHANNEL_PACKET_LENGHT - 1) == buffer[CHANNEL_PACKET_LENGHT - 1]) { // pwm_set_gpio_level(GPIO_PWM_CH1, (buffer[1] << 8) | buffer[2]); // pwm_set_gpio_level(GPIO_PWM_CH2, (buffer[3] << 8) | buffer[4]); send_packet(sensors); } else { debug("\nSanwa. Bad header"); } } } static void send_packet(float **sensors) { if (!sensors[TYPE_TEMP_MOTOR] && !sensors[TYPE_TEMP_ESC] && !sensors[TYPE_RPM1] && !sensors[TYPE_RPM2] && !sensors[TYPE_VOLT]) return; static uint type = TYPE_TEMP_MOTOR; sanwa_sensor_formatted_t sensor_formatted = {0}; while (!sensors[type % MAX_SENSORS]) type++; format_sensor(sensors[type % MAX_SENSORS], type % MAX_SENSORS, &sensor_formatted); uart0_write_bytes((uint8_t *)&sensor_formatted, TELEMETRY_PACKET_LENGHT); type++; debug("\nSanwa (%u) > ", uxTaskGetStackHighWaterMark(NULL)); debug_buffer((uint8_t *)&sensor_formatted, TELEMETRY_PACKET_LENGHT, "0x%X "); // blink led vTaskResume(context.led_task_handle); } static void format_sensor(float *sensor, uint8_t type, sanwa_sensor_formatted_t *sensor_formatted) { // Packet format: [sync] [type] [msb] [lsb] [crc8] static float rpm = 0; sensor_formatted->header = 0x01; sensor_formatted->type = type; switch (type) { case TYPE_TEMP_MOTOR: { static uint8_t prev = 0; sensor_formatted->msb = prev; sensor_formatted->lsb = *sensor; prev = sensor_formatted->lsb; sensor_formatted->crc = get_crc((uint8_t *)sensor_formatted, TELEMETRY_PACKET_LENGHT - 1); break; } case TYPE_TEMP_ESC: { static uint8_t prev = 0; sensor_formatted->msb = prev; sensor_formatted->lsb = *sensor; prev = sensor_formatted->lsb; sensor_formatted->crc = get_crc((uint8_t *)sensor_formatted, TELEMETRY_PACKET_LENGHT - 1); break; } case TYPE_RPM1: { rpm = *sensor; sensor_formatted->msb = (uint)(rpm / 2) >> 8; sensor_formatted->lsb = rpm / 2; sensor_formatted->crc = get_crc((uint8_t *)sensor_formatted, TELEMETRY_PACKET_LENGHT - 1); break; } case TYPE_RPM2: { sensor_formatted->msb = (uint)(rpm / 2) >> 8; sensor_formatted->lsb = rpm / 2; sensor_formatted->crc = get_crc((uint8_t *)sensor_formatted, TELEMETRY_PACKET_LENGHT - 1); break; } case TYPE_VOLT: { uint16_t volt = *sensor * 100; sensor_formatted->msb = volt >> 8; sensor_formatted->lsb = volt; sensor_formatted->crc = get_crc((uint8_t *)sensor_formatted, TELEMETRY_PACKET_LENGHT - 1); break; } } } static uint8_t get_crc(const uint8_t *buffer, uint len) { uint8_t crc = 0; for (uint i = 0; i < len; i++) crc += buffer[i]; return crc; } static void set_config(float **sensors) { config_t *config = config_read(); TaskHandle_t task_handle; if (config->esc_protocol == ESC_PWM) { esc_pwm_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, malloc(sizeof(float))}; xTaskCreate(esc_pwm_task, "esc_pwm_task", STACK_ESC_PWM, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); sensors[TYPE_RPM1] = parameter.rpm; sensors[TYPE_RPM2] = parameter.rpm; } if (config->esc_protocol == ESC_HW3) { esc_hw3_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, malloc(sizeof(float))}; xTaskCreate(esc_hw3_task, "esc_hw3_task", STACK_ESC_HW3, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); sensors[TYPE_RPM1] = parameter.rpm; } if (config->esc_protocol == ESC_HW4) { esc_hw4_parameters_t parameter = {config->rpm_multiplier, config->enable_pwm_out, config->enable_esc_hw4_init_delay, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, config->esc_hw4_voltage_multiplier, config->esc_hw4_current_multiplier, config->esc_hw4_current_thresold, config->esc_hw4_current_max, config->esc_hw4_is_manual_offset, config->esc_hw4_auto_detect, config->esc_hw4_offset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_hw4_task, "esc_hw4_task", STACK_ESC_HW4, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); /*if (config->enable_pwm_out) { xTaskCreate(pwm_out_task, "pwm_out", STACK_PWM_OUT, (void *)parameter.rpm, 2, &task_handle); context.pwm_out_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); }*/ sensors[TYPE_TEMP_ESC] = parameter.temperature_fet; sensors[TYPE_TEMP_MOTOR] = parameter.temperature_bec; // note this is bec temp, not motor temp sensors[TYPE_VOLT] = parameter.voltage; sensors[TYPE_RPM1] = parameter.rpm; sensors[TYPE_RPM2] = parameter.rpm; } if (config->esc_protocol == ESC_HW5) { esc_hw5_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_hw5_task, "esc_hw5_task", STACK_ESC_HW5, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); sensors[TYPE_TEMP_ESC] = parameter.temperature_fet; sensors[TYPE_TEMP_MOTOR] = parameter.temperature_bec; // note this is bec temp, not motor temp sensors[TYPE_VOLT] = parameter.voltage; sensors[TYPE_RPM1] = parameter.rpm; sensors[TYPE_RPM2] = parameter.rpm; } if (config->esc_protocol == ESC_CASTLE) { esc_castle_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_castle_task, "esc_castle_task", STACK_ESC_CASTLE, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); sensors[TYPE_TEMP_ESC] = parameter.temperature; sensors[TYPE_VOLT] = parameter.voltage; sensors[TYPE_RPM1] = parameter.rpm; sensors[TYPE_RPM2] = parameter.rpm; } if (config->esc_protocol == ESC_KONTRONIK) { esc_kontronik_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_kontronik_task, "esc_kontronik_task", STACK_ESC_KONTRONIK, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); sensors[TYPE_TEMP_ESC] = parameter.temperature_fet; sensors[TYPE_TEMP_MOTOR] = parameter.temperature_bec; // note this is bec temp, not motor temp sensors[TYPE_VOLT] = parameter.voltage; sensors[TYPE_RPM1] = parameter.rpm; sensors[TYPE_RPM2] = parameter.rpm; } if (config->esc_protocol == ESC_APD_F) { esc_apd_f_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_apd_f_task, "esc_apd_f_task", STACK_ESC_APD_F, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); sensors[TYPE_TEMP_ESC] = parameter.temperature; sensors[TYPE_VOLT] = parameter.voltage; sensors[TYPE_RPM1] = parameter.rpm; sensors[TYPE_RPM2] = parameter.rpm; } if (config->esc_protocol == ESC_APD_HV) { esc_apd_hv_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_apd_hv_task, "esc_apd_hv_task", STACK_ESC_APD_HV, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); sensors[TYPE_TEMP_ESC] = parameter.temperature; sensors[TYPE_VOLT] = parameter.voltage; sensors[TYPE_RPM1] = parameter.rpm; sensors[TYPE_RPM2] = parameter.rpm; } if (config->enable_analog_voltage) { voltage_parameters_t parameter = {0, config->analog_rate, config->alpha_voltage, config->analog_voltage_multiplier, malloc(sizeof(float))}; xTaskCreate(voltage_task, "voltage_task", STACK_VOLTAGE, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); sensors[TYPE_VOLT] = parameter.voltage; } if (config->enable_analog_ntc) { ntc_parameters_t parameter = {2, config->analog_rate, config->ntc_offset, config->alpha_temperature, malloc(sizeof(float))}; xTaskCreate(ntc_task, "ntc_task", STACK_NTC, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); sensors[TYPE_TEMP_MOTOR] = parameter.ntc; } } ================================================ FILE: board/project/protocol/sanwa.h ================================================ #ifndef SANWA_H #define SANWA_H #include "common.h" extern context_t context; void sanwa_task(void *parameters); #endif ================================================ FILE: board/project/protocol/sbus.c ================================================ #include "sbus.h" #include #include #include #include "airspeed.h" #include "bmp180.h" #include "bmp280.h" #include "config.h" #include "current.h" #include "esc_apd_f.h" #include "esc_apd_hv.h" #include "esc_castle.h" #include "esc_hw3.h" #include "esc_hw4.h" #include "esc_hw5.h" #include "esc_kontronik.h" #include "esc_omp_m4.h" #include "esc_openyge.h" #include "esc_pwm.h" #include "esc_ztw.h" #include "gps.h" #include "ms5611.h" #include "ntc.h" #include "pwm_out.h" #include "smart_esc.h" #include "uart.h" #include "uart_pio.h" #include "voltage.h" #define SLOT_TEMP1 1 #define SLOT_RPM 2 #define SLOT_VARIO_SPEED 3 #define SLOT_VARIO_ALT 4 #define SLOT_VARIO_RESSURE 5 #define SLOT_VOLT_V1 6 #define SLOT_VOLT_V2 7 #define SLOT_GPS_SPD 8 #define SLOT_GPS_ALT 9 #define SLOT_GPS_TIME 10 #define SLOT_GPS_VARIO 11 #define SLOT_GPS_LAT1 12 #define SLOT_GPS_LAT2 13 #define SLOT_GPS_LON1 14 #define SLOT_GPS_LON2 15 #define SLOT_AIR_SPEED 16 #define SLOT_POWER_CURR3 21 #define SLOT_POWER_VOLT3 22 #define SLOT_POWER_CONS3 23 #define SLOT_POWER_CURR1 24 #define SLOT_POWER_VOLT1 25 #define SLOT_POWER_CONS1 26 #define SLOT_POWER_CURR2 27 #define SLOT_POWER_VOLT2 28 #define SLOT_POWER_CONS2 29 #define SLOT_TEMP2 30 #define TIMEOUT_US 500 #define SLOT_0_DELAY 2000 #define INTER_SLOT_DELAY 660 #define PACKET_LENGHT 25 #define SBUS_NEGATIVE_BIT 15 #define SBUS_SOUTH_WEST_BIT 4 #define SBUS_WAIT 0 #define SBUS_SEND 1 /* FASST Sbus Data Id */ #define SBUS_NULL 0 #define SBUS_TEMP 1 #define SBUS_VOLT_V1 2 #define SBUS_VOLT_V2 3 #define SBUS_RPM 4 #define SBUS_POWER_CURR 5 #define SBUS_POWER_VOLT 6 #define SBUS_POWER_CONS 7 #define SBUS_VARIO_ALT 8 // F1672 #define SBUS_VARIO_SPEED 9 #define SBUS_GPS_SPEED 10 // F1675 #define SBUS_GPS_ALTITUDE 11 #define SBUS_GPS_TIME 12 #define SBUS_GPS_VARIO_SPEED 13 #define SBUS_GPS_LATITUDE1 14 #define SBUS_GPS_LATITUDE2 15 #define SBUS_GPS_LONGITUDE1 16 #define SBUS_GPS_LONGITUDE2 17 #define SBUS_AIR_SPEED 18 /** * Slots sensor mapping for Futaba transmitters: * * | Slot | Sensor | * | :----: | :----------------------------------: | * |0 | RX voltage (reserved) | * |1 | Temperature 1 (SBS-01T/TE) | * |2 | RPM (type magnet)(SBS-01RB/RM/RO) | * |3-4 | Vario-F1672 | * |6-7 | Voltage (SBS-01V) | * |8-15 | GPS-F1675 | * |16 | Air speed (SBS-01TAS) | * |17-21 | Unused | * |21-23 | Current 3 (SBS-01C) | * |24-26 | Current 1 (SBS-01C) | * |27-29(+)| Current 2 (SBS-01C) | * |30(+) | Temperature 2 (SBS-01T/TE) | * |31 | Unused | * * (+) Non default slots */ typedef struct sensor_sbus_t { uint8_t data_id; float *value; } sensor_sbus_t; static sensor_sbus_t *sbus_sensor[32] = {NULL}; static uint8_t *gps_fix; static uint packet_id; static volatile uint slot = 0; static volatile bool slot_pending = false; static void process(); static int64_t send_slot_callback(alarm_id_t id, void *parameters); static inline void send_slot(uint8_t slot); static uint16_t format(uint8_t data_id, float value); static void add_sensor(uint8_t slot, sensor_sbus_t *new_sensor); static void set_config(void); static uint8_t get_slot_id(uint8_t slot); void sbus_task(void *parameters) { context.led_cycle_duration = 6; context.led_cycles = 1; set_config(); uart0_begin(100000, UART_RECEIVER_TX, UART_RECEIVER_RX, TIMEOUT_US, 8, 2, UART_PARITY_EVEN, true, true); debug("\nSbus init"); while (1) { ulTaskNotifyTakeIndexed(1, pdTRUE, portMAX_DELAY); process(); if (slot_pending) { send_slot(slot); slot_pending = false; } } } static void process() { if (uart0_available() == PACKET_LENGHT) { uint8_t data[PACKET_LENGHT]; uart0_read_bytes(data, PACKET_LENGHT); debug("\nSbus (%u) < ", uxTaskGetStackHighWaterMark(NULL)); debug_buffer(data, PACKET_LENGHT, "0x%X "); if (data[0] == 0x0F) { if (data[24] == 0x04 || data[24] == 0x14 || data[24] == 0x24 || data[24] == 0x34) { packet_id = data[24] >> 4; add_alarm_in_us(SLOT_0_DELAY - uart0_get_time_elapsed(), send_slot_callback, NULL, true); debug("\nSbus (%u) > ", uxTaskGetStackHighWaterMark(NULL)); } } } } static int64_t send_slot_callback(alarm_id_t id, void *parameters) { BaseType_t xHigherPriorityTaskWoken = pdFALSE; static uint timestamp; static uint8_t index = 0; uint next_alarm; slot = index + packet_id * 8; if (context.debug == 2) { if (slot == 0 || slot == 8 || slot == 16 || slot == 24) printf("\nT:%u\n", uart0_get_time_elapsed()); else printf("\nT:%u\n", time_us_32() - timestamp); } timestamp = time_us_32(); if (index < 7) { index++; next_alarm = INTER_SLOT_DELAY - (time_us_32() - timestamp); } else { index = 0; next_alarm = 0; } vTaskResume(context.led_task_handle); slot_pending = true; vTaskNotifyGiveIndexedFromISR(context.uart0_notify_task_handle, 1, &xHigherPriorityTaskWoken); portYIELD_FROM_ISR(xHigherPriorityTaskWoken); return next_alarm; } static inline void send_slot(uint8_t slot) { debug(" (%u)", slot); uint16_t value = 0; if (sbus_sensor[slot]) { if (sbus_sensor[slot]->value) { value = format(sbus_sensor[slot]->data_id, *sbus_sensor[slot]->value); } else { value = format(sbus_sensor[slot]->data_id, 0); } uint8_t data[3]; data[0] = get_slot_id(slot); data[1] = value; data[2] = value >> 8; uart0_write_bytes(data, 3); debug("%X:%X:%X ", data[0], data[1], data[2]); } } static uint16_t format(uint8_t data_id, float value) { static float coord = 0; if (data_id == SBUS_RPM) { return (uint16_t)round(value / 6); } if (data_id == SBUS_TEMP) { return (uint16_t)round(value + 100) | 0X8000; } if (data_id == SBUS_VOLT_V1) { return __builtin_bswap16((uint16_t)round(value * 10) | 0x8000); } if (data_id == SBUS_VOLT_V2) { return __builtin_bswap16((uint16_t)round(value * 10)); } if (data_id == SBUS_VARIO_SPEED) { return __builtin_bswap16((int16_t)round(value * 100)); } if (data_id == SBUS_VARIO_ALT) { return __builtin_bswap16((int16_t)round(value) | 0x4000); } if (data_id == SBUS_POWER_CURR) { return __builtin_bswap16((uint16_t)round(value * 100) | 0x4000); } if (data_id == SBUS_POWER_VOLT) { return __builtin_bswap16((uint16_t)round((value)*100)); } if (data_id == SBUS_AIR_SPEED) { return __builtin_bswap16((uint16_t)round(value) | 0x4000); } if (data_id == SBUS_GPS_SPEED) { return __builtin_bswap16((uint16_t)round(value * 1.852) | (*gps_fix > 0 ? 0x4000 : 0x0000)); } if (data_id == SBUS_GPS_VARIO_SPEED) { return __builtin_bswap16((int16_t)round(value * 10)); } if (data_id == SBUS_GPS_ALTITUDE) { return __builtin_bswap16((int16_t)round(value) | (*gps_fix > 1 ? 0x4000 : 0x0000)); } if (data_id == SBUS_GPS_LATITUDE1 || data_id == SBUS_GPS_LONGITUDE1) { // bits 1-4: bits 17-20 from minutes precision 4 (minutes*10000 = 20 bits) // bit 5: S/W bit // bits 9-16: degrees coord = value; uint16_t lat_lon = 0; if (coord < 0) { lat_lon = 1 << SBUS_SOUTH_WEST_BIT; coord *= -1; } uint8_t degrees = coord; lat_lon |= degrees << 8; uint32_t minutes = (coord - degrees) * 60 * 10000; // minutes precision 4 lat_lon |= (minutes >> 16) & 0xf; return __builtin_bswap16(lat_lon); } if (data_id == SBUS_GPS_LATITUDE2 || data_id == SBUS_GPS_LONGITUDE2) { // bits 1-16 from minutes precision 4 (minutes*10000 = 20 bits) uint32_t minutes = (coord - (int)coord) * 60 * 10000; // minutes precision 4 return __builtin_bswap16(minutes); } if (data_id == SBUS_GPS_TIME) { if (value > 120000) value -= 120000; uint8_t hours = value / 10000; uint8_t minutes = (uint8_t)(value / 100) - hours * 100; uint8_t seconds = value - hours * 10000 - minutes * 100; return __builtin_bswap16(hours * 3600 + minutes * 60 + seconds); } return __builtin_bswap16(round(value)); } static uint8_t get_slot_id(uint8_t slot) { uint8_t slot_id[32] = {0x03, 0x83, 0x43, 0xC3, 0x23, 0xA3, 0x63, 0xE3, 0x13, 0x93, 0x53, 0xD3, 0x33, 0xB3, 0x73, 0xF3, 0x0B, 0x8B, 0x4B, 0xCB, 0x2B, 0xAB, 0x6B, 0xEB, 0x1B, 0x9B, 0x5B, 0xDB, 0x3B, 0xBB, 0x7B, 0xFB}; return slot_id[slot]; } static void add_sensor(uint8_t slot, sensor_sbus_t *new_sensor) { sbus_sensor[slot] = new_sensor; } static void set_config(void) { config_t *config = config_read(); TaskHandle_t task_handle; sensor_sbus_t *new_sensor; float *baro_temp = NULL, *baro_pressure = NULL; if (config->esc_protocol == ESC_PWM) { esc_pwm_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, malloc(sizeof(float))}; xTaskCreate(esc_pwm_task, "esc_pwm_task", STACK_ESC_PWM, (void *)¶meter, 2, &task_handle); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_RPM, parameter.rpm}; add_sensor(SLOT_RPM, new_sensor); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_HW3) { esc_hw3_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, malloc(sizeof(float))}; xTaskCreate(esc_hw3_task, "esc_hw3_task", STACK_ESC_HW3, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_RPM, parameter.rpm}; add_sensor(SLOT_RPM, new_sensor); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_HW4) { esc_hw4_parameters_t parameter = {config->rpm_multiplier, config->enable_pwm_out, config->enable_esc_hw4_init_delay, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, config->esc_hw4_voltage_multiplier, config->esc_hw4_current_multiplier, config->esc_hw4_current_thresold, config->esc_hw4_current_max, config->esc_hw4_is_manual_offset, config->esc_hw4_auto_detect, config->esc_hw4_offset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_hw4_task, "esc_hw4_task", STACK_ESC_HW4, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); if (config->enable_pwm_out) { xTaskCreate(pwm_out_task, "pwm_out", STACK_PWM_OUT, (void *)parameter.rpm, 2, &task_handle); context.pwm_out_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_RPM, parameter.rpm}; add_sensor(SLOT_RPM, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_VOLT, parameter.voltage}; add_sensor(SLOT_POWER_VOLT1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_CURR, parameter.current}; add_sensor(SLOT_POWER_CURR1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_CONS, parameter.consumption}; add_sensor(SLOT_POWER_CONS1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_TEMP, parameter.temperature_fet}; add_sensor(SLOT_TEMP1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_TEMP, parameter.temperature_bec}; add_sensor(SLOT_TEMP2, new_sensor); // new_sensor = malloc(sizeof(sensor_sbus_t)); //*new_sensor = (sensor_sbus_t){AFHDS2A_ID_CELL_VOLTAGE, parameter.cell_voltage}; // add_sensor(new_sensor); } if (config->esc_protocol == ESC_HW5) { esc_hw5_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_hw5_task, "esc_hw5_task", STACK_ESC_HW5, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_RPM, parameter.rpm}; add_sensor(SLOT_RPM, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_VOLT, parameter.voltage}; add_sensor(SLOT_POWER_VOLT1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_CURR, parameter.current}; add_sensor(SLOT_POWER_CURR1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_CONS, parameter.consumption}; add_sensor(SLOT_POWER_CONS1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_TEMP, parameter.temperature_fet}; add_sensor(SLOT_TEMP1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_TEMP, parameter.temperature_bec}; add_sensor(SLOT_TEMP2, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_VOLT, parameter.voltage_bec}; add_sensor(SLOT_POWER_VOLT3, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_CURR, parameter.current_bec}; add_sensor(SLOT_POWER_CURR3, new_sensor); // new_sensor = malloc(sizeof(sensor_sbus_t)); //*new_sensor = (sensor_sbus_t){AFHDS2A_ID_CELL_VOLTAGE, parameter.cell_voltage}; // add_sensor(new_sensor); } if (config->esc_protocol == ESC_CASTLE) { esc_castle_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_castle_task, "esc_castle_task", STACK_ESC_CASTLE, (void *)¶meter, 2, &task_handle); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_RPM, parameter.rpm}; add_sensor(SLOT_RPM, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_VOLT, parameter.voltage}; add_sensor(SLOT_POWER_VOLT1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_CURR, parameter.current}; add_sensor(SLOT_POWER_CURR1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_VOLT, parameter.voltage_bec}; add_sensor(SLOT_POWER_VOLT3, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_CURR, parameter.current_bec}; add_sensor(SLOT_POWER_CURR3, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_CONS, parameter.consumption}; add_sensor(SLOT_POWER_CONS1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_TEMP, parameter.temperature}; add_sensor(SLOT_TEMP1, new_sensor); // new_sensor = malloc(sizeof(sensor_sbus_t)); //*new_sensor = (sensor_sbus_t){AFHDS2A_ID_CELL_VOLTAGE, parameter.cell_voltage}; // add_sensor(new_sensor); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_KONTRONIK) { esc_kontronik_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_kontronik_task, "esc_kontronik_task", STACK_ESC_KONTRONIK, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_RPM, parameter.rpm}; add_sensor(SLOT_RPM, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_VOLT, parameter.voltage}; add_sensor(SLOT_POWER_VOLT1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_CURR, parameter.current}; add_sensor(SLOT_POWER_CURR1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_CONS, parameter.consumption}; add_sensor(SLOT_POWER_CONS1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_VOLT, parameter.voltage_bec}; add_sensor(SLOT_POWER_VOLT3, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_CURR, parameter.current_bec}; add_sensor(SLOT_POWER_CURR3, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_CONS, parameter.consumption}; add_sensor(SLOT_POWER_CONS3, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_TEMP, parameter.temperature_fet}; add_sensor(SLOT_TEMP1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_TEMP, parameter.temperature_bec}; add_sensor(SLOT_TEMP2, new_sensor); // new_sensor = malloc(sizeof(sensor_sbus_t)); //*new_sensor = (sensor_sbus_t){AFHDS2A_ID_CELL_VOLTAGE, parameter.cell_voltage}; // add_sensor(new_sensor); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_APD_F) { esc_apd_f_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_apd_f_task, "esc_apd_f_task", STACK_ESC_APD_F, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_RPM, parameter.rpm}; add_sensor(SLOT_RPM, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_VOLT, parameter.voltage}; add_sensor(SLOT_POWER_VOLT1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_CURR, parameter.current}; add_sensor(SLOT_POWER_CURR1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_CONS, parameter.consumption}; add_sensor(SLOT_POWER_CONS1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_TEMP, parameter.temperature}; add_sensor(SLOT_TEMP1, new_sensor); // new_sensor = malloc(sizeof(sensor_sbus_t)); //*new_sensor = (sensor_sbus_t){AFHDS2A_ID_CELL_VOLTAGE, parameter.cell_voltage}; // add_sensor(new_sensor); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_APD_HV) { esc_apd_hv_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_apd_hv_task, "esc_apd_hv_task", STACK_ESC_APD_HV, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_RPM, parameter.rpm}; add_sensor(SLOT_RPM, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_VOLT, parameter.voltage}; add_sensor(SLOT_POWER_VOLT1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_CURR, parameter.current}; add_sensor(SLOT_POWER_CURR1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_CONS, parameter.consumption}; add_sensor(SLOT_POWER_CONS1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_TEMP, parameter.temperature}; add_sensor(SLOT_TEMP1, new_sensor); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_SMART) { smart_esc_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temperature_fet = malloc(sizeof(float)); parameter.temperature_bec = malloc(sizeof(float)); parameter.voltage_bec = malloc(sizeof(float)); parameter.current_bec = malloc(sizeof(float)); parameter.temperature_bat = malloc(sizeof(float)); parameter.current_bat = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); for (uint i = 0; i < 18; i++) parameter.cell[i] = malloc(sizeof(float)); parameter.cells = malloc(sizeof(uint8_t)); parameter.cycles = malloc(sizeof(uint16_t)); xTaskCreate(smart_esc_task, "smart_esc_task", STACK_SMART_ESC, (void *)¶meter, 4, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_RPM, parameter.rpm}; add_sensor(SLOT_RPM, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_VOLT, parameter.voltage}; add_sensor(SLOT_POWER_VOLT1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_CURR, parameter.current}; add_sensor(SLOT_POWER_CURR1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_CONS, parameter.consumption}; add_sensor(SLOT_POWER_CONS1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_TEMP, parameter.temperature_fet}; add_sensor(SLOT_TEMP1, new_sensor); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_OMP_M4) { esc_omp_m4_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temp_esc = malloc(sizeof(float)); parameter.temp_motor = malloc(sizeof(float)); parameter.cell_voltage = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); parameter.cell_count = malloc(sizeof(uint8_t)); xTaskCreate(esc_omp_m4_task, "esc_omp_m4_task", STACK_ESC_OMP_M4, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_RPM, parameter.rpm}; add_sensor(SLOT_RPM, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_VOLT, parameter.voltage}; add_sensor(SLOT_POWER_VOLT1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_CURR, parameter.current}; add_sensor(SLOT_POWER_CURR1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_CONS, parameter.consumption}; add_sensor(SLOT_POWER_CONS1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_TEMP, parameter.temp_esc}; add_sensor(SLOT_TEMP1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_TEMP, parameter.temp_motor}; add_sensor(SLOT_TEMP2, new_sensor); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_ZTW) { esc_ztw_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temp_esc = malloc(sizeof(float)); parameter.temp_motor = malloc(sizeof(float)); parameter.bec_voltage = malloc(sizeof(float)); parameter.cell_voltage = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); parameter.cell_count = malloc(sizeof(uint8_t)); xTaskCreate(esc_ztw_task, "esc_ztw_task", STACK_ESC_ZTW, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_RPM, parameter.rpm}; add_sensor(SLOT_RPM, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_VOLT, parameter.voltage}; add_sensor(SLOT_POWER_VOLT1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_CURR, parameter.current}; add_sensor(SLOT_POWER_CURR1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_CONS, parameter.consumption}; add_sensor(SLOT_POWER_CONS1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_TEMP, parameter.temp_esc}; add_sensor(SLOT_TEMP1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_TEMP, parameter.temp_motor}; add_sensor(SLOT_TEMP2, new_sensor); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_OPENYGE) { esc_openyge_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.pwm_out = config->enable_pwm_out; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temperature_fet = malloc(sizeof(float)); parameter.temperature_bec = malloc(sizeof(float)); parameter.cell_voltage = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); parameter.voltage_bec = malloc(sizeof(float)); parameter.current_bec = malloc(sizeof(float)); parameter.throttle = malloc(sizeof(float)); parameter.pwm_percent = malloc(sizeof(float)); parameter.cell_count = malloc(sizeof(uint8_t)); xTaskCreate(esc_openyge_task, "esc_openyge_task", STACK_ESC_OPENYGE, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_RPM, parameter.rpm}; add_sensor(SLOT_RPM, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_VOLT, parameter.voltage}; add_sensor(SLOT_POWER_VOLT1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_CURR, parameter.current}; add_sensor(SLOT_POWER_CURR1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_VOLT, parameter.voltage_bec}; add_sensor(SLOT_POWER_VOLT3, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_CURR, parameter.current_bec}; add_sensor(SLOT_POWER_CURR3, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_CONS, parameter.consumption}; add_sensor(SLOT_POWER_CONS1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_TEMP, parameter.temperature_fet}; add_sensor(SLOT_TEMP1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_TEMP, parameter.temperature_bec}; add_sensor(SLOT_TEMP2, new_sensor); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_gps) { gps_parameters_t parameter; parameter.protocol = config->gps_protocol; parameter.baudrate = config->gps_baudrate; parameter.rate = config->gps_rate; parameter.lat = malloc(sizeof(double)); parameter.lon = malloc(sizeof(double)); parameter.alt = malloc(sizeof(float)); parameter.spd = malloc(sizeof(float)); parameter.cog = malloc(sizeof(float)); parameter.hdop = malloc(sizeof(float)); parameter.sat = malloc(sizeof(float)); parameter.time = malloc(sizeof(float)); parameter.date = malloc(sizeof(float)); parameter.vspeed = malloc(sizeof(float)); parameter.dist = malloc(sizeof(float)); parameter.spd_kmh = malloc(sizeof(float)); parameter.fix = malloc(sizeof(float)); parameter.vdop = malloc(sizeof(float)); parameter.speed_acc = malloc(sizeof(float)); parameter.h_acc = malloc(sizeof(float)); parameter.v_acc = malloc(sizeof(float)); parameter.track_acc = malloc(sizeof(float)); parameter.n_vel = malloc(sizeof(float)); parameter.e_vel = malloc(sizeof(float)); parameter.v_vel = malloc(sizeof(float)); parameter.alt_elipsiod = malloc(sizeof(float)); parameter.pdop = malloc(sizeof(float)); parameter.fix_type = malloc(sizeof(uint8_t)); parameter.home_set = malloc(sizeof(uint8_t)); parameter.alt_home = malloc(sizeof(float)); xTaskCreate(gps_task, "gps_task", STACK_GPS, (void *)¶meter, 2, &task_handle); context.uart_pio_notify_task_handle = task_handle; new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_GPS_LATITUDE1, parameter.lat}; add_sensor(SLOT_GPS_LAT1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_GPS_LATITUDE2, parameter.lat}; add_sensor(SLOT_GPS_LAT2, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_GPS_LONGITUDE1, parameter.lon}; add_sensor(SLOT_GPS_LON1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_GPS_LONGITUDE2, parameter.lon}; add_sensor(SLOT_GPS_LON2, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_GPS_ALTITUDE, parameter.alt}; add_sensor(SLOT_GPS_ALT, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_GPS_SPEED, parameter.spd}; add_sensor(SLOT_GPS_SPD, new_sensor); gps_fix = parameter.fix_type; new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_GPS_VARIO_SPEED, parameter.vspeed}; add_sensor(SLOT_GPS_VARIO, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_GPS_TIME, parameter.time}; add_sensor(SLOT_GPS_TIME, new_sensor); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_voltage) { voltage_parameters_t parameter = {0, config->analog_rate, config->alpha_voltage, config->analog_voltage_multiplier, malloc(sizeof(float))}; xTaskCreate(voltage_task, "voltage_task", STACK_VOLTAGE, (void *)¶meter, 2, &task_handle); if (config->sbus_battery_slot) { new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_VOLT_V1, parameter.voltage}; add_sensor(SLOT_VOLT_V1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_VOLT_V2, NULL}; add_sensor(SLOT_VOLT_V2, new_sensor); } else { new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_VOLT_V1, NULL}; add_sensor(SLOT_VOLT_V1, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_VOLT_V2, parameter.voltage}; add_sensor(SLOT_VOLT_V2, new_sensor); } ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_current) { current_parameters_t parameter = {1, config->analog_rate, config->alpha_current, config->analog_current_multiplier, config->analog_current_offset, config->analog_current_autoffset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(current_task, "current_task", STACK_CURRENT, (void *)¶meter, 2, &task_handle); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_CURR, parameter.current}; add_sensor(SLOT_POWER_CURR2, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_CONS, parameter.consumption}; add_sensor(SLOT_POWER_CONS2, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_POWER_VOLT, NULL}; add_sensor(SLOT_POWER_VOLT2, new_sensor); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_ntc) { ntc_parameters_t parameter = {2, config->analog_rate, config->ntc_offset, config->alpha_temperature, malloc(sizeof(float))}; xTaskCreate(ntc_task, "ntc_task", STACK_NTC, (void *)¶meter, 2, &task_handle); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_TEMP, parameter.ntc}; add_sensor(SLOT_TEMP1, new_sensor); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_BMP280) { bmp280_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, 0, config->bmp280_filter, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(bmp280_task, "bmp280_task", STACK_BMP280, (void *)¶meter, 2, &task_handle); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_VARIO_ALT, parameter.altitude}; add_sensor(SLOT_VARIO_ALT, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_VARIO_SPEED, parameter.vspeed}; add_sensor(SLOT_VARIO_SPEED, new_sensor); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_MS5611) { ms5611_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, 0, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(ms5611_task, "ms5611_task", STACK_MS5611, (void *)¶meter, 2, &task_handle); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_VARIO_ALT, parameter.altitude}; add_sensor(SLOT_VARIO_ALT, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_VARIO_SPEED, parameter.vspeed}; add_sensor(SLOT_VARIO_SPEED, new_sensor); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_BMP180) { bmp180_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(bmp180_task, "bmp180_task", STACK_BMP180, (void *)¶meter, 2, &task_handle); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_VARIO_ALT, parameter.altitude}; add_sensor(SLOT_VARIO_ALT, new_sensor); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_VARIO_SPEED, parameter.vspeed}; add_sensor(SLOT_VARIO_SPEED, new_sensor); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_airspeed) { airspeed_parameters_t parameter = {3, config->analog_rate, config->alpha_airspeed, (float)config->airspeed_offset / 1000, (float)config->airspeed_vcc / 100, baro_temp, baro_pressure, malloc(sizeof(float))}; xTaskCreate(airspeed_task, "airspeed_task", STACK_AIRSPEED, (void *)¶meter, 2, &task_handle); new_sensor = malloc(sizeof(sensor_sbus_t)); *new_sensor = (sensor_sbus_t){SBUS_AIR_SPEED, parameter.airspeed}; add_sensor(SLOT_AIR_SPEED, new_sensor); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } } ================================================ FILE: board/project/protocol/sbus.h ================================================ #ifndef SBUS_H #define SBUS_H #include "common.h" extern context_t context; void sbus_task(void *parameters); #endif ================================================ FILE: board/project/protocol/smartport.c ================================================ #include "smartport.h" #include #include #include #include #include "airspeed.h" #include "bmp180.h" #include "bmp280.h" #include "config.h" #include "current.h" #include "esc_apd_f.h" #include "esc_apd_hv.h" #include "esc_castle.h" #include "esc_hw3.h" #include "esc_hw4.h" #include "esc_hw5.h" #include "esc_kontronik.h" #include "esc_omp_m4.h" #include "esc_openyge.h" #include "esc_pwm.h" #include "esc_ztw.h" #include "fuel_meter.h" #include "gpio.h" #include "gps.h" #include "ina3221.h" #include "mpu6050.h" #include "ms5611.h" #include "ntc.h" #include "pwm_out.h" #include "smart_esc.h" #include "stdlib.h" #include "uart.h" #include "uart_pio.h" #include "voltage.h" #define AIRCR_Register (*((volatile uint32_t *)(PPB_BASE + 0x0ED0C))) #define POLL_LENGHT 2 #define PACKET_LENGHT 10 static SemaphoreHandle_t semaphore_sensor = NULL; static bool is_maintenance_mode = false; static const uint8_t sensor_id_matrix[29] = {0x00, 0xA1, 0x22, 0x83, 0xE4, 0x45, 0xC6, 0x67, 0x48, 0xE9, 0x6A, 0xCB, 0xAC, 0xD, 0x8E, 0x2F, 0xD0, 0x71, 0xF2, 0x53, 0x34, 0x95, 0x16, 0xB7, 0x98, 0x39, 0xBA, 0x1B, 0x0}; static TaskHandle_t packet_task_handle; static QueueHandle_t packet_queue_handle; static config_t *config_lua; static void sensor_task(void *parameters); static void sensor_void_task(void *parameters); static void sensor_double_task(void *parameters); static void sensor_coordinates_task(void *parameters); static void sensor_datetime_task(void *parameters); static void sensor_cell_task(void *parameters); static void sensor_cell_individual_task(void *parameters); static void sensor_gpio_task(void *parameters); static void packet_task(void *parameters); static void process(smartport_parameters_t *parameter); static void send_packet(uint8_t frame_id, uint16_t data_id, uint32_t value); static void set_config(smartport_parameters_t *parameter); static int64_t reboot_callback(alarm_id_t id, void *user_data); void smartport_task(void *parameters) { smartport_parameters_t parameter; context.led_cycle_duration = 6; context.led_cycles = 1; uart0_begin(57600, UART_RECEIVER_TX, UART_RECEIVER_RX, TIMEOUT_US, 8, 1, UART_PARITY_NONE, true, true); semaphore_sensor = xSemaphoreCreateBinary(); xSemaphoreTake(semaphore_sensor, 0); set_config(¶meter); packet_queue_handle = xQueueCreate(32, sizeof(smartport_packet_t)); xTaskCreate(packet_task, "packet_task", STACK_SMARTPORT_PACKET_TASK, (void *)¶meter.data_id, 3, &packet_task_handle); // TaskHandle_t task_handle; // xTaskCreate(sensor_void_task, "sensor_void_task", STACK_SMARTPORT_SENSOR_VOID_TASK, NULL, 2, &task_handle); debug("\nSmartport init"); while (1) { ulTaskNotifyTakeIndexed(1, pdTRUE, portMAX_DELAY); process(¶meter); } } smartport_packet_t smartport_process_packet(smartport_parameters_t *parameter, uint8_t frame_id, uint16_t data_id, uint32_t value) { /* Protocol: cmd | sender | frameId | dataId | value (4B) | sensorId -----------|--------|---------|---------|---------------|--------- get var | radio | 0x30 | 0x51nn | 0 | set var | radio | 0x31 | 0x51nn | value | start save | radio | 0x31 | 0x5201 | 0 | end save | radio | 0x31 | 0x5201 | 1 | */ smartport_packet_t packet = {0}; // set maintenance mode on if (frame_id == 0x21 && (data_id == 0xFFFF || data_id == parameter->data_id) && value == 0x80) { is_maintenance_mode = true; debug("\nSmartport (%u). Maintenance mode ON ", uxTaskGetStackHighWaterMark(NULL)); return packet; } // set maintenance mode off if (frame_id == 0x20 && (data_id == 0xFFFF || data_id == parameter->data_id) && value == 0x80) { is_maintenance_mode = false; debug("\nSmartport (%u). Maintenance mode OFF ", uxTaskGetStackHighWaterMark(NULL)); return packet; } // send sensor id if (frame_id == 0x30 && data_id == parameter->data_id && value == 0x01) { smartport_packet_t packet; packet.value = (parameter->sensor_id - 1) << 8 | 0x01; packet.frame_id = 0x32; packet.data_id = parameter->data_id; debug("\nSmartport (%u). Send sensorId %i (0x%X)", uxTaskGetStackHighWaterMark(NULL), parameter->sensor_id, smartport_sensor_id_to_crc(parameter->sensor_id)); return packet; } // send rate if (frame_id == 0x30 && data_id == parameter->data_id && value == 0x22) { smartport_packet_t packet; packet.value = 0x122; packet.frame_id = 0x32; packet.data_id = parameter->data_id; debug("\nSmartport (%u). Send rate %i (0x%X)", uxTaskGetStackHighWaterMark(NULL), packet.value >> 8, packet.value); return packet; } // send version if (frame_id == 0x30 && data_id == parameter->data_id && value == 0x0C) { packet.value = 0; char version[] = PROJECT_VERSION; memmove(version, version + 1, strlen(version)); char *token; token = strtok(version, " . "); while (token != NULL) { packet.value = packet.value << 4 | atoi(token); token = strtok(NULL, " . "); } packet.value = packet.value >> 4; packet.value = (packet.value << 8) | 0xC; packet.frame_id = 0x32; packet.data_id = parameter->data_id; debug("\nSmartport (%u). Send version %s (0x%X)", uxTaskGetStackHighWaterMark(NULL), PROJECT_VERSION, packet.value); return packet; } // change sensor id if (frame_id == 0x31 && data_id == parameter->data_id && (uint8_t)value == 0x01) { static bool is_changed = false; if (is_changed == true) return packet; uint8_t sensor_id = (value >> 8) + 1; parameter->sensor_id = sensor_id; config_lua = malloc(sizeof(config_t)); memcpy(config_lua, config_read(), sizeof(config_t)); config_lua->smartport_sensor_id = sensor_id; config_write(config_lua); sleep_ms(10); free(config_lua); is_changed = true; debug("\nSmartport (%u). Change sensorId %i (0x%X)", uxTaskGetStackHighWaterMark(NULL), config_lua->smartport_sensor_id, smartport_sensor_id_to_crc(config_lua->smartport_sensor_id)); return packet; } // send config if (frame_id == 0x30 && (uint8_t)value == 0x01) { config_t *config = config_read(); uint8_t frame_id_send = 0x32; bool send = true; switch (data_id) { case 0x5101: { packet.value = 0; char version[] = PROJECT_VERSION; memmove(version, version + 1, strlen(version)); char *token; token = strtok(version, " . "); uint cont = 0; while (token != NULL) { packet.value = packet.value << 8 | atoi(token); token = strtok(NULL, " . "); cont++; } packet.value = packet.value << 8 * (3 - cont); break; } case 0x5103: packet.value = config->esc_protocol; break; case 0x5104: packet.value = config->enable_gps; break; case 0x5105: packet.value = config->gps_baudrate; break; case 0x5106: packet.value = config->enable_analog_voltage; break; case 0x5107: packet.value = config->enable_analog_current; break; case 0x5108: packet.value = config->enable_analog_ntc; break; case 0x5109: packet.value = config->enable_analog_airspeed; break; case 0x510A: packet.value = config->i2c_module; break; case 0x510C: packet.value = ELEMENTS(config->alpha_rpm); break; case 0x510D: packet.value = ELEMENTS(config->alpha_voltage); break; case 0x510E: packet.value = ELEMENTS(config->alpha_current); break; case 0x510F: packet.value = ELEMENTS(config->alpha_temperature); break; case 0x5110: packet.value = ELEMENTS(config->alpha_vario); break; case 0x5111: packet.value = ELEMENTS(config->alpha_airspeed); break; case 0x513A: packet.value = config->refresh_rate_rpm; break; case 0x5113: packet.value = config->refresh_rate_voltage; break; case 0x5114: packet.value = config->refresh_rate_current; break; case 0x5115: packet.value = config->refresh_rate_temperature; break; case 0x5116: packet.value = config->refresh_rate_gps; break; case 0x5117: packet.value = config->refresh_rate_consumption; break; case 0x5118: packet.value = config->refresh_rate_vario; break; case 0x5119: packet.value = config->refresh_rate_airspeed; break; case 0x511A: packet.value = config->refresh_rate_default; break; case 0x511B: packet.value = config->analog_voltage_multiplier * 100; break; case 0x511C: packet.value = config->analog_current_type; break; case 0x511D: packet.value = config->gpio_interval; break; case 0x511E: packet.value = config->analog_current_quiescent_voltage * 100; break; case 0x511F: packet.value = config->analog_current_multiplier * 100; break; case 0x5120: packet.value = config->analog_current_offset * 100; break; case 0x5121: packet.value = config->analog_current_autoffset; break; case 0x5122: packet.value = config->pairOfPoles; break; case 0x5123: packet.value = config->mainTeeth; break; case 0x5124: packet.value = config->pinionTeeth; break; case 0x5125: packet.value = config->rpm_multiplier; break; case 0x5126: packet.value = config->bmp280_filter; break; case 0x5127: packet.value = config->enable_pwm_out; break; case 0x5128: packet.value = config->smartport_sensor_id; break; case 0x512A: packet.value = config->vario_auto_offset; break; case 0x512D: packet.value = config->enable_esc_hw4_init_delay; break; case 0x512E: packet.value = config->esc_hw4_init_delay_duration; break; case 0x512F: packet.value = config->esc_hw4_current_thresold; break; case 0x5130: packet.value = config->esc_hw4_current_max; break; case 0x5131: packet.value = config->esc_hw4_voltage_multiplier * 100000; break; case 0x5132: packet.value = config->esc_hw4_current_multiplier * 100000; break; case 0x5135: packet.value = config->esc_hw4_is_manual_offset; break; case 0x5136: packet.value = config->analog_rate; break; case 0x5138: packet.value = config->gpio_mask; break; case 0x5139: packet.value = config->esc_hw4_offset; break; case 0x513F: packet.value = config->airspeed_offset + 1000; break; case 0x5140: packet.value = config->airspeed_vcc; break; case 0x5141: packet.value = config->fuel_flow_ml_per_pulse * 10000; break; case 0x5142: packet.value = config->enable_fuel_flow; break; case 0x5143: packet.value = config->xgzp68xxd_k; break; case 0x5144: packet.value = config->enable_fuel_pressure; break; case 0x5145: packet.value = config->smart_esc_calc_consumption; break; case 0x5147: packet.value = config->gps_rate; break; case 0x5149: packet.value = config->gps_protocol; break; case 0x514B: packet.value = config->esc_hw4_auto_detect; break; case 0x514C: packet.value = config->mpu6050_acc_scale; break; case 0x514D: packet.value = config->mpu6050_gyro_scale; break; case 0x514E: packet.value = config->mpu6050_gyro_weighting; break; case 0x514F: packet.value = config->enable_gyro; break; case 0x5151: packet.value = config->mpu6050_filter; break; default: send = false; debug("\nSmartport. Unknown request frameId 0x%X dataId 0x%X", frame_id, data_id); break; } if (send) { packet.frame_id = 0x32; packet.data_id = data_id; return packet; } } // receive config if (frame_id == 0x31 && data_id == 0x5201 && value == 0) { config_lua = malloc(sizeof(config_t)); memcpy(config_lua, config_read(), sizeof(config_t)); debug("\nSmartport. Start saving..."); return packet; } if (frame_id == 0x31 && data_id == 0x5201 && value == 1) { config_write(config_lua); sleep_ms(10); free(config_lua); debug("\nSmartport. Complete save config"); return packet; } if (frame_id == 0x31) { uint8_t frame_id_send = 0x32; bool write = true; switch (data_id) { case 0x5103: config_lua->esc_protocol = value; break; case 0x5104: config_lua->enable_gps = value; break; case 0x5105: config_lua->gps_baudrate = value; break; case 0x5106: config_lua->enable_analog_voltage = value; break; case 0x5107: config_lua->enable_analog_current = value; break; case 0x5108: config_lua->enable_analog_ntc = value; break; case 0x5109: config_lua->enable_analog_airspeed = value; break; case 0x510A: config_lua->i2c_module = value; break; case 0x510C: config_lua->alpha_rpm = ALPHA(value); break; case 0x510D: config_lua->alpha_voltage = ALPHA(value); break; case 0x510E: config_lua->alpha_current = ALPHA(value); break; case 0x510F: config_lua->alpha_temperature = ALPHA(value); break; case 0x5110: config_lua->alpha_vario = ALPHA(value); break; case 0x5111: config_lua->alpha_airspeed = ALPHA(value); break; case 0x513A: config_lua->refresh_rate_rpm = value; break; case 0x5113: config_lua->refresh_rate_voltage = value; break; case 0x5114: config_lua->refresh_rate_current = value; break; case 0x5115: config_lua->refresh_rate_temperature = value; break; case 0x5116: config_lua->refresh_rate_gps = value; break; case 0x5117: config_lua->refresh_rate_consumption = value; break; case 0x5118: config_lua->refresh_rate_vario = value; break; case 0x5119: config_lua->refresh_rate_airspeed = value; break; case 0x511A: config_lua->refresh_rate_default = value; break; case 0x511B: config_lua->analog_voltage_multiplier = value / 100.0; break; case 0x511C: config_lua->analog_current_type = value; break; case 0x511D: config_lua->gpio_interval = value; break; case 0x511E: config_lua->analog_current_quiescent_voltage = value / 100.0; break; case 0x511F: config_lua->analog_current_multiplier = value / 100.0; break; case 0x5120: config_lua->analog_current_offset = value / 100.0; break; case 0x5121: config_lua->analog_current_autoffset = value; break; case 0x5122: config_lua->pairOfPoles = value; break; case 0x5123: config_lua->mainTeeth = value; break; case 0x5124: config_lua->pinionTeeth = value; break; case 0x5125: config_lua->rpm_multiplier = value; break; case 0x5126: config_lua->bmp280_filter = value; break; case 0x5127: config_lua->enable_pwm_out = value; break; case 0x5128: config_lua->smartport_sensor_id = value; break; case 0x5129: config_lua->smartport_data_id = value; break; case 0x512A: config_lua->vario_auto_offset = value; break; case 0x512D: config_lua->enable_esc_hw4_init_delay = value; break; case 0x512E: config_lua->esc_hw4_init_delay_duration = value; break; case 0x512F: config_lua->esc_hw4_current_thresold = value; break; case 0x5130: config_lua->esc_hw4_current_max = value; break; case 0x5131: config_lua->esc_hw4_voltage_multiplier = value / 100000.0; break; case 0x5132: config_lua->esc_hw4_current_multiplier = value / 100000.0; break; case 0x5135: config_lua->esc_hw4_is_manual_offset = value; break; case 0x5136: config_lua->analog_rate = value; break; case 0x5138: config_lua->gpio_mask = value; break; case 0x5139: config_lua->esc_hw4_offset = value; break; case 0x513F: config_lua->airspeed_offset = value - 1000; break; case 0x5140: config_lua->airspeed_vcc = value; break; case 0x5141: config_lua->fuel_flow_ml_per_pulse = value / 10000.0; break; case 0x5142: config_lua->enable_fuel_flow = value; break; case 0x5143: config_lua->xgzp68xxd_k = value; break; case 0x5144: config_lua->enable_fuel_pressure = value; break; case 0x5145: config_lua->smart_esc_calc_consumption = value; break; case 0x5147: config_lua->gps_rate = value; break; case 0x5149: config_lua->gps_protocol = value; break; case 0x514B: config_lua->esc_hw4_auto_detect = value; break; case 0x514C: config_lua->mpu6050_acc_scale = value; break; case 0x514D: config_lua->mpu6050_gyro_scale = value; break; case 0x514E: config_lua->mpu6050_gyro_weighting = value; break; case 0x514F: config_lua->enable_gyro = value; break; case 0x5151: config_lua->mpu6050_filter = value; break; default: debug("\nSmartport. Unknown save request. frameId 0x%X dataId 0x%X", frame_id, data_id); break; } debug("\nSmartport. Store config. frameId 0x%X dataId 0x%X value %u", frame_id, data_id, value); } return packet; } int32_t smartport_format(uint16_t data_id, float value) { if ((data_id >= GPS_SPEED_FIRST_ID && data_id <= GPS_SPEED_LAST_ID) || (data_id >= RBOX_BATT1_FIRST_ID && data_id <= RBOX_BATT2_FIRST_ID)) return round(value * 1000); if ((data_id >= ALT_FIRST_ID && data_id <= VARIO_LAST_ID) || (data_id >= VFAS_FIRST_ID && data_id <= VFAS_LAST_ID) || (data_id >= ACCX_FIRST_ID && data_id <= GPS_ALT_LAST_ID) || (data_id >= GPS_COURS_FIRST_ID && data_id <= GPS_COURS_LAST_ID) || (data_id >= A3_FIRST_ID && data_id <= A4_LAST_ID) || data_id == DIY_FIRST_ID + 5) return round(value * 100); if ((data_id >= CURR_FIRST_ID && data_id <= CURR_LAST_ID) || (data_id >= AIR_SPEED_FIRST_ID && data_id <= AIR_SPEED_LAST_ID) || data_id == A1_ID || data_id == A2_ID || data_id == RXBT_ID) return round(value * 10); return round(value); } uint32_t smartport_format_double(uint16_t data_id, float value_l, float value_h) { if ((data_id >= ESC_POWER_FIRST_ID && data_id <= ESC_POWER_LAST_ID) || (data_id >= SBEC_POWER_FIRST_ID && data_id <= SBEC_POWER_LAST_ID)) return (uint32_t)round(value_h * 100) << 16 | (uint16_t)round(value_l * 100); if (data_id >= ESC_RPM_CONS_FIRST_ID && data_id <= ESC_RPM_CONS_LAST_ID) { return (uint32_t)round(value_h) << 16 | (uint16_t)round((value_l) / 100); } return (uint16_t)round(value_h * 500) << 8 | (uint16_t)value_l; } uint32_t smartport_format_coordinate(coordinate_type_t type, float value) { uint32_t data = 0; if (value < 0) { data |= (uint32_t)1 << 30; value *= -1; } if (type == SMARTPORT_LONGITUDE) { data |= (uint32_t)1 << 31; } data |= (uint32_t)(value * 60 * 10000); // deg to min * 10000 return data; } uint32_t smartport_format_datetime(uint8_t type, uint32_t value) { uint8_t dayHour = value / 10000; uint8_t monthMin = value / 100 - dayHour * 100; uint8_t yearSec = value - (value / 100) * 100; if (type == SMARTPORT_DATE) { return (uint32_t)yearSec << 24 | (uint32_t)monthMin << 16 | dayHour << 8 | 0xFF; } return (uint32_t)dayHour << 24 | (uint32_t)monthMin << 16 | yearSec << 8; } uint32_t smartport_format_cell(uint8_t cell_index, float value) { return cell_index | (uint16_t)round(value * 500) << 8; } uint8_t smartport_get_crc(uint8_t *data, uint len) { uint16_t crc = 0; for (uint8_t i = 2; i < len; i++) { crc += data[i]; crc += crc >> 8; crc &= 0x00FF; } return 0xFF - (uint8_t)crc; } void smartport_send_byte(uint8_t c, uint16_t *crcp) { if (crcp != NULL) { uint16_t crc = *crcp; crc += c; crc += crc >> 8; crc &= 0x00FF; *crcp = crc; } if (c == 0x7D || c == 0x7E) { uart0_write(c); c ^= 0x20; } uart0_write(c); debug("0x%X ", c); } uint8_t smartport_sensor_id_to_crc(uint8_t sensor_id) { if (sensor_id < 1 || sensor_id > 28) { return 0; } return sensor_id_matrix[sensor_id - 1]; } uint8_t smartport_sensor_crc_to_id(uint8_t sensor_id_crc) { uint8_t cont = 0; while (sensor_id_crc != sensor_id_matrix[cont] && cont < 28) { cont++; } if (cont == 28) return 0; return cont + 1; } static void process(smartport_parameters_t *parameter) { uint lenght = uart0_available(); if (lenght) { uint8_t data[lenght]; if (context.debug == 2 && lenght != 2) { printf("\n"); debug_buffer2(data, lenght, "0x%X "); } uart0_read_bytes(data, lenght); // send telemetry if (data[0] == 0x7E && data[1] == smartport_sensor_id_to_crc(parameter->sensor_id) && lenght == POLL_LENGHT && !is_maintenance_mode) { debug("\nSmartport (%u) < ", uxTaskGetStackHighWaterMark(NULL)); debug_buffer(data, POLL_LENGHT, "0x%X "); xSemaphoreGive(semaphore_sensor); vTaskDelay(4 / portTICK_PERIOD_MS); xSemaphoreTake(semaphore_sensor, 0); } // send packet if (data[0] == 0x7E && data[1] == smartport_sensor_id_to_crc(parameter->sensor_id) && lenght == POLL_LENGHT && is_maintenance_mode) { debug("\nSmartport (%u) < ", uxTaskGetStackHighWaterMark(NULL)); debug_buffer(data, POLL_LENGHT, "0x%X "); if (uxQueueMessagesWaiting(packet_queue_handle)) xTaskNotifyGive(packet_task_handle); } // receive packet if (lenght >= 10) { uint8_t delta = 0; uint i; for (i = 0; i < lenght; i++) { data[i] = data[i + delta]; if (data[i] == 0x7D) { delta++; data[i] = data[i + delta] ^ 0x20; } } if (i - delta == PACKET_LENGHT) { uint8_t crc = smartport_get_crc(data, PACKET_LENGHT - 1); uint8_t sensor_id = data[1]; uint8_t frame_id = data[2]; uint16_t data_id = (uint16_t)data[4] << 8 | data[3]; // process no telemetry packets if (crc == data[9] && frame_id != 0x10) { uint value = (uint32_t)data[8] << 24 | (uint32_t)data[7] << 16 | (uint16_t)data[6] << 8 | data[5]; debug("\nSmartport. Received packet (%u) sensorId 0x%X FrameId 0x%X DataId 0x%X Value 0x%X < ", uxTaskGetStackHighWaterMark(NULL), sensor_id, frame_id, data_id, value); debug_buffer(data, PACKET_LENGHT, "0x%X "); smartport_packet_t packet = smartport_process_packet(parameter, frame_id, data_id, value); if (packet.data_id != 0) xQueueSendToBack(packet_queue_handle, &packet, 0); } } } } } static int64_t reboot_callback(alarm_id_t id, void *user_data) { AIRCR_Register = 0x5FA0004; } static void sensor_task(void *parameters) { smartport_sensor_parameters_t parameter = *(smartport_sensor_parameters_t *)parameters; xTaskNotifyGive(context.receiver_task_handle); while (1) { vTaskDelay(parameter.rate / portTICK_PERIOD_MS); xSemaphoreTake(semaphore_sensor, portMAX_DELAY); int32_t data_formatted = smartport_format(parameter.data_id, *parameter.value); debug("\nSmartport. Sensor (%u) > ", uxTaskGetStackHighWaterMark(NULL)); send_packet(0x10, parameter.data_id, data_formatted); } } static void sensor_gpio_task(void *parameters) { smartport_sensor_gpio_parameters_t parameter = *(smartport_sensor_gpio_parameters_t *)parameters; xTaskNotifyGive(context.receiver_task_handle); uint cont = 0; while (1) { vTaskDelay(parameter.rate / portTICK_PERIOD_MS); xSemaphoreTake(semaphore_sensor, portMAX_DELAY); if (parameter.gpio_mask) { while (!(parameter.gpio_mask & (1 << cont))) { cont++; if (cont == 6) cont = 0; } float value = *parameter.value & (1 << cont) ? 1 : 0; uint16_t data_id = parameter.data_id + 17 + cont; int32_t data_formatted = smartport_format(data_id, value); debug("\nSmartport. Sensor GPIO (%u) > GPIO: %u STATE: %u > ", uxTaskGetStackHighWaterMark(NULL), 17 + cont, (uint)value); send_packet(0x10, data_id, data_formatted); cont++; if (cont == 6) cont = 0; } } } static void sensor_void_task(void *parameters) { while (1) { xSemaphoreTake(semaphore_sensor, portMAX_DELAY); if (context.debug == 2) printf("\nSmartport. Sensor void (%u) > ", uxTaskGetStackHighWaterMark(NULL)); send_packet(0, 0, 0); } } static void sensor_double_task(void *parameters) { smartport_sensor_double_parameters_t parameter = *(smartport_sensor_double_parameters_t *)parameters; xTaskNotifyGive(context.receiver_task_handle); while (1) { vTaskDelay(parameter.rate / portTICK_PERIOD_MS); xSemaphoreTake(semaphore_sensor, portMAX_DELAY); float v_l = parameter.value_l ? *parameter.value_l : 0.0f; float v_h = parameter.value_h ? *parameter.value_h : 0.0f; uint32_t data_formatted = smartport_format_double(parameter.data_id, v_l, v_h); debug("\nSmartport. Sensor double (%u) > ", uxTaskGetStackHighWaterMark(NULL)); send_packet(0x10, parameter.data_id, data_formatted); } } static void sensor_coordinates_task(void *parameters) { smartport_sensor_coordinate_parameters_t parameter = *(smartport_sensor_coordinate_parameters_t *)parameters; xTaskNotifyGive(context.receiver_task_handle); while (1) { vTaskDelay(parameter.rate / portTICK_PERIOD_MS); xSemaphoreTake(semaphore_sensor, portMAX_DELAY); uint32_t data_formatted; if (parameter.type == SMARTPORT_LATITUDE) data_formatted = smartport_format_coordinate(parameter.type, *parameter.latitude); else data_formatted = smartport_format_coordinate(parameter.type, *parameter.longitude); parameter.type = !parameter.type; debug("\nSmartport. Sensor coordinates (%u) > ", uxTaskGetStackHighWaterMark(NULL)); send_packet(0x10, GPS_LONG_LATI_FIRST_ID, data_formatted); } } static void sensor_datetime_task(void *parameters) { smartport_sensor_datetime_parameters_t parameter = *(smartport_sensor_datetime_parameters_t *)parameters; xTaskNotifyGive(context.receiver_task_handle); while (1) { vTaskDelay(parameter.rate / portTICK_PERIOD_MS); xSemaphoreTake(semaphore_sensor, portMAX_DELAY); uint32_t data_formatted; if (parameter.type == SMARTPORT_DATE) data_formatted = smartport_format_datetime(parameter.type, *parameter.date); else data_formatted = smartport_format_datetime(parameter.type, *parameter.time); parameter.type = !parameter.type; debug("\nSmartport. Sensor datetime (%u) > ", uxTaskGetStackHighWaterMark(NULL)); send_packet(0x10, GPS_TIME_DATE_FIRST_ID, data_formatted); } } static void sensor_cell_task(void *parameters) { smartport_sensor_cell_parameters_t parameter = *(smartport_sensor_cell_parameters_t *)parameters; xTaskNotifyGive(context.receiver_task_handle); uint8_t cell_index = 0; while (1) { vTaskDelay(parameter.rate / portTICK_PERIOD_MS); xSemaphoreTake(semaphore_sensor, portMAX_DELAY); if (!*parameter.cell_count) return; uint32_t data_formatted = smartport_format_cell(cell_index, *parameter.cell_voltage); cell_index++; if (cell_index > *parameter.cell_count - 1) cell_index = 0; debug("\nSmartport. Sensor cell (%u) > ", uxTaskGetStackHighWaterMark(NULL)); send_packet(0x10, CELLS_FIRST_ID, data_formatted); } } static void sensor_cell_individual_task(void *parameters) { smartport_sensor_cell_individual_parameters_t parameter = *(smartport_sensor_cell_individual_parameters_t *)parameters; xTaskNotifyGive(context.receiver_task_handle); uint8_t cell_index = 0; while (1) { vTaskDelay(parameter.rate / portTICK_PERIOD_MS); xSemaphoreTake(semaphore_sensor, portMAX_DELAY); // No cells configured → skip if (!parameter.cell_count || *parameter.cell_count == 0) { continue; } float value = 0.0f; // Safety: check index and pointer before dereferencing if (cell_index < *parameter.cell_count && parameter.cell_voltage[cell_index] != NULL) { value = *parameter.cell_voltage[cell_index]; } uint32_t data_formatted = smartport_format_cell(cell_index, value); debug("\nSmartport. Sensor cell (%u) > ", uxTaskGetStackHighWaterMark(NULL)); send_packet(0x10, CELLS_FIRST_ID, data_formatted); // Next cell cell_index++; if (cell_index >= *parameter.cell_count) { cell_index = 0; } } } static void packet_task(void *parameters) { uint16_t data_id = *(uint16_t *)parameters; smartport_packet_t packet; while (1) { ulTaskNotifyTake(pdTRUE, portMAX_DELAY); xQueueReceive(packet_queue_handle, &packet, 0); debug("\nSmartport. Packet (%u) > ", uxTaskGetStackHighWaterMark(NULL)); send_packet(packet.frame_id, packet.data_id, packet.value); // vTaskDelay(1500 / portTICK_PERIOD_MS); } } static void set_config(smartport_parameters_t *parameter) { config_t *config = config_read(); TaskHandle_t task_handle; float *baro_temp = NULL, *baro_pressure = NULL; parameter->sensor_id = config->smartport_sensor_id; parameter->data_id = 0x5100; if (config->esc_protocol == ESC_PWM) { esc_pwm_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, malloc(sizeof(float))}; xTaskCreate(esc_pwm_task, "esc_pwm_task", STACK_ESC_PWM, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_double_parameters_t parameter_sensor_double; parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = parameter.rpm; parameter_sensor_double.value_h = NULL; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_HW3) { esc_hw3_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, malloc(sizeof(float))}; xTaskCreate(esc_hw3_task, "esc_hw3_task", STACK_ESC_HW3, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_double_parameters_t parameter_sensor_double; parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = parameter.rpm; parameter_sensor_double.value_h = NULL; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_HW4) { esc_hw4_parameters_t parameter = {config->rpm_multiplier, config->enable_pwm_out, config->enable_esc_hw4_init_delay, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, config->esc_hw4_voltage_multiplier, config->esc_hw4_current_multiplier, config->esc_hw4_current_thresold, config->esc_hw4_current_max, config->esc_hw4_is_manual_offset, config->esc_hw4_auto_detect, config->esc_hw4_offset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_hw4_task, "esc_hw4_task", STACK_ESC_HW4, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); if (config->enable_pwm_out) { xTaskCreate(pwm_out_task, "pwm_out", STACK_PWM_OUT, (void *)parameter.rpm, 2, &task_handle); context.pwm_out_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } smartport_sensor_parameters_t parameter_sensor; smartport_sensor_double_parameters_t parameter_sensor_double; smartport_sensor_cell_parameters_t parameter_sensor_cell; parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = parameter.rpm; parameter_sensor_double.value_h = parameter.consumption; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_double.data_id = ESC_POWER_FIRST_ID; parameter_sensor_double.value_l = parameter.voltage; parameter_sensor_double.value_h = parameter.current; parameter_sensor_double.rate = config->refresh_rate_voltage; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID; parameter_sensor.value = parameter.temperature_fet; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID + 1; parameter_sensor.value = parameter.temperature_bec; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_cell.cell_count = parameter.cell_count; parameter_sensor_cell.cell_voltage = parameter.cell_voltage; parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_task, "sensor_cell_task", STACK_SENSOR_SMARTPORT_CELL, (void *)¶meter_sensor_cell, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_HW5) { esc_hw5_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_hw5_task, "esc_hw5_task", STACK_ESC_HW5, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; smartport_sensor_double_parameters_t parameter_sensor_double; smartport_sensor_cell_parameters_t parameter_sensor_cell; parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = parameter.rpm; parameter_sensor_double.value_h = parameter.consumption; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_double.data_id = ESC_POWER_FIRST_ID; parameter_sensor_double.value_l = parameter.voltage; parameter_sensor_double.value_h = parameter.current; parameter_sensor_double.rate = config->refresh_rate_voltage; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID; parameter_sensor.value = parameter.temperature_fet; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID + 1; parameter_sensor.value = parameter.temperature_bec; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID + 2; parameter_sensor.value = parameter.temperature_motor; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_double.data_id = ESC_POWER_FIRST_ID + 1; parameter_sensor_double.value_l = parameter.voltage_bec; parameter_sensor_double.value_h = parameter.current_bec; parameter_sensor_double.rate = config->refresh_rate_voltage; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_cell.cell_count = parameter.cell_count; parameter_sensor_cell.cell_voltage = parameter.cell_voltage; parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_task, "sensor_cell_task", STACK_SENSOR_SMARTPORT_CELL, (void *)¶meter_sensor_cell, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_CASTLE) { esc_castle_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_castle_task, "esc_castle_task", STACK_ESC_CASTLE, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; smartport_sensor_double_parameters_t parameter_sensor_double; smartport_sensor_cell_parameters_t parameter_sensor_cell; parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = parameter.rpm; parameter_sensor_double.value_h = parameter.consumption; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_double.data_id = ESC_POWER_FIRST_ID; parameter_sensor_double.value_l = parameter.voltage; parameter_sensor_double.value_h = parameter.current; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_double.data_id = ESC_POWER_FIRST_ID + 1; parameter_sensor_double.value_l = parameter.voltage_bec; parameter_sensor_double.value_h = parameter.current_bec; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID; parameter_sensor.value = parameter.temperature; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_cell.cell_count = parameter.cell_count; parameter_sensor_cell.cell_voltage = parameter.cell_voltage; parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_task, "sensor_cell_task", STACK_SENSOR_SMARTPORT_CELL, (void *)¶meter_sensor_cell, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_KONTRONIK) { esc_kontronik_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_kontronik_task, "esc_kontronik_task", STACK_ESC_KONTRONIK, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; smartport_sensor_double_parameters_t parameter_sensor_double; smartport_sensor_cell_parameters_t parameter_sensor_cell; parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = parameter.rpm; parameter_sensor_double.value_h = parameter.consumption; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_double.data_id = ESC_POWER_FIRST_ID; parameter_sensor_double.value_l = parameter.voltage; parameter_sensor_double.value_h = parameter.current; parameter_sensor_double.rate = config->refresh_rate_voltage; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID; parameter_sensor.value = parameter.temperature_fet; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID + 1; parameter_sensor.value = parameter.temperature_bec; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_cell.cell_count = parameter.cell_count; parameter_sensor_cell.cell_voltage = parameter.cell_voltage; parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_task, "sensor_cell_task", STACK_SENSOR_SMARTPORT_CELL, (void *)¶meter_sensor_cell, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_APD_F) { esc_apd_f_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_apd_f_task, "esc_apd_f_task", STACK_ESC_APD_F, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; smartport_sensor_double_parameters_t parameter_sensor_double; smartport_sensor_cell_parameters_t parameter_sensor_cell; parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = parameter.rpm; parameter_sensor_double.value_h = parameter.consumption; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_double.data_id = ESC_POWER_FIRST_ID; parameter_sensor_double.value_l = parameter.voltage; parameter_sensor_double.value_h = parameter.current; parameter_sensor_double.rate = config->refresh_rate_voltage; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID; parameter_sensor.value = parameter.temperature; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_cell.cell_count = parameter.cell_count; parameter_sensor_cell.cell_voltage = parameter.cell_voltage; parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_task, "sensor_cell_task", STACK_SENSOR_SMARTPORT_CELL, (void *)¶meter_sensor_cell, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_APD_HV) { esc_apd_hv_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_apd_hv_task, "esc_apd_hv_task", STACK_ESC_APD_HV, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; smartport_sensor_double_parameters_t parameter_sensor_double; smartport_sensor_cell_parameters_t parameter_sensor_cell; parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = parameter.rpm; parameter_sensor_double.value_h = parameter.consumption; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_double.data_id = ESC_POWER_FIRST_ID; parameter_sensor_double.value_l = parameter.voltage; parameter_sensor_double.value_h = parameter.current; parameter_sensor_double.rate = config->refresh_rate_voltage; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID; parameter_sensor.value = parameter.temperature; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_cell.cell_count = parameter.cell_count; parameter_sensor_cell.cell_voltage = parameter.cell_voltage; parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_task, "sensor_cell_task", STACK_SENSOR_SMARTPORT_CELL, (void *)¶meter_sensor_cell, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_SMART) { smart_esc_parameters_t parameter; parameter.calc_consumption = config->smart_esc_calc_consumption; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temperature_fet = malloc(sizeof(float)); parameter.temperature_bec = malloc(sizeof(float)); parameter.voltage_bec = malloc(sizeof(float)); parameter.current_bec = malloc(sizeof(float)); parameter.temperature_bat = malloc(sizeof(float)); parameter.current_bat = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); for (uint i = 0; i < 18; i++) parameter.cell[i] = malloc(sizeof(float)); parameter.cells = malloc(sizeof(uint8_t)); parameter.cycles = malloc(sizeof(uint16_t)); xTaskCreate(smart_esc_task, "smart_esc_task", STACK_SMART_ESC, (void *)¶meter, 4, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; smartport_sensor_double_parameters_t parameter_sensor_double; smartport_sensor_cell_individual_parameters_t parameter_sensor_cell; // rpm & consumption parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = parameter.rpm; parameter_sensor_double.value_h = parameter.consumption; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // voltage & current parameter_sensor_double.data_id = ESC_POWER_FIRST_ID; parameter_sensor_double.value_l = parameter.voltage; parameter_sensor_double.value_h = parameter.current; parameter_sensor_double.rate = config->refresh_rate_voltage; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // bec. voltage & current parameter_sensor_double.data_id = SBEC_POWER_FIRST_ID; parameter_sensor_double.value_l = parameter.voltage_bec; parameter_sensor_double.value_h = parameter.current_bec; parameter_sensor_double.rate = config->refresh_rate_voltage; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // temp_fet parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID; parameter_sensor.value = parameter.temperature_fet; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // temp_bec parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID + 1; parameter_sensor.value = parameter.temperature_bec; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // temp_bat parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID + 2; parameter_sensor.value = parameter.temperature_bat; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // current_bat parameter_sensor.data_id = CURR_FIRST_ID + 1; parameter_sensor.value = parameter.current_bat; parameter_sensor.rate = config->refresh_rate_current; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // cells parameter_sensor_cell.cell_count = parameter.cells; // Pointer provided by ESC_SMART task for (uint i = 0; i < 18; i++) { parameter_sensor_cell.cell_voltage[i] = parameter.cell[i]; // One pointer per cell } parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_individual_task, "sensor_cell_task", STACK_SENSOR_SMARTPORT_CELL, (void *)¶meter_sensor_cell, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // cycles /*parameter_sensor.data_id = DIY_FIRST_ID + 100; parameter_sensor.value = parameter.cycles; parameter_sensor.rate = config->refresh_rate_default; xTaskCreate(sensor_cell_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY);*/ } if (config->esc_protocol == ESC_OMP_M4) { esc_omp_m4_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temp_esc = malloc(sizeof(float)); parameter.temp_motor = malloc(sizeof(float)); parameter.cell_voltage = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); parameter.cell_count = malloc(sizeof(uint8_t)); xTaskCreate(esc_omp_m4_task, "esc_omp_m4_task", STACK_ESC_OMP_M4, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; smartport_sensor_double_parameters_t parameter_sensor_double; smartport_sensor_cell_parameters_t parameter_sensor_cell; parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = parameter.rpm; parameter_sensor_double.value_h = parameter.consumption; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_double.data_id = ESC_POWER_FIRST_ID; parameter_sensor_double.value_l = parameter.voltage; parameter_sensor_double.value_h = parameter.current; parameter_sensor_double.rate = config->refresh_rate_voltage; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID; parameter_sensor.value = parameter.temp_esc; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID + 2; parameter_sensor.value = parameter.temp_motor; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_cell.cell_count = parameter.cell_count; parameter_sensor_cell.cell_voltage = parameter.cell_voltage; parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_task, "sensor_cell_task", STACK_SENSOR_SMARTPORT_CELL, (void *)¶meter_sensor_cell, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_ZTW) { esc_ztw_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temp_esc = malloc(sizeof(float)); parameter.temp_motor = malloc(sizeof(float)); parameter.bec_voltage = malloc(sizeof(float)); parameter.cell_voltage = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); parameter.cell_count = malloc(sizeof(uint8_t)); xTaskCreate(esc_ztw_task, "esc_ztw_task", STACK_ESC_ZTW, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; smartport_sensor_double_parameters_t parameter_sensor_double; smartport_sensor_cell_parameters_t parameter_sensor_cell; parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = parameter.rpm; parameter_sensor_double.value_h = parameter.consumption; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_double.data_id = ESC_POWER_FIRST_ID; parameter_sensor_double.value_l = parameter.voltage; parameter_sensor_double.value_h = parameter.current; parameter_sensor_double.rate = config->refresh_rate_voltage; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID; parameter_sensor.value = parameter.temp_esc; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID + 2; parameter_sensor.value = parameter.temp_motor; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor_cell.cell_count = parameter.cell_count; parameter_sensor_cell.cell_voltage = parameter.cell_voltage; parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_task, "sensor_cell_task", STACK_SENSOR_SMARTPORT_CELL, (void *)¶meter_sensor_cell, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_OPENYGE) { esc_openyge_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.pwm_out = config->enable_pwm_out; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temperature_fet = malloc(sizeof(float)); parameter.temperature_bec = malloc(sizeof(float)); parameter.cell_voltage = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); parameter.voltage_bec = malloc(sizeof(float)); parameter.current_bec = malloc(sizeof(float)); parameter.throttle = malloc(sizeof(float)); parameter.pwm_percent = malloc(sizeof(float)); parameter.cell_count = malloc(sizeof(uint8_t)); xTaskCreate(esc_openyge_task, "esc_openyge_task", STACK_ESC_OPENYGE, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; smartport_sensor_double_parameters_t parameter_sensor_double; smartport_sensor_cell_parameters_t parameter_sensor_cell; // RPM and Consumption sensor parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = parameter.rpm; parameter_sensor_double.value_h = parameter.consumption; parameter_sensor_double.rate = config->refresh_rate_rpm; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // Main voltage and current sensor parameter_sensor_double.data_id = ESC_POWER_FIRST_ID; parameter_sensor_double.value_l = parameter.voltage; parameter_sensor_double.value_h = parameter.current; parameter_sensor_double.rate = config->refresh_rate_voltage; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // FET Temperature sensor parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID; parameter_sensor.value = parameter.temperature_fet; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // BEC Temperature sensor parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID + 2; parameter_sensor.value = parameter.temperature_bec; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // BEC voltage and current sensor parameter_sensor_double.data_id = ESC_POWER_FIRST_ID + 2; parameter_sensor_double.value_l = parameter.voltage_bec; parameter_sensor_double.value_h = parameter.current_bec; parameter_sensor_double.rate = config->refresh_rate_voltage; xTaskCreate(sensor_double_task, "sensor_double_task", STACK_SENSOR_SMARTPORT_DOUBLE, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // Cell voltage sensor parameter_sensor_cell.cell_count = parameter.cell_count; parameter_sensor_cell.cell_voltage = parameter.cell_voltage; parameter_sensor_cell.rate = config->refresh_rate_voltage; xTaskCreate(sensor_cell_task, "sensor_cell_task", STACK_SENSOR_SMARTPORT_CELL, (void *)¶meter_sensor_cell, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_gps) { gps_parameters_t parameter; parameter.protocol = config->gps_protocol; parameter.baudrate = config->gps_baudrate; parameter.rate = config->gps_rate; parameter.lat = malloc(sizeof(double)); parameter.lon = malloc(sizeof(double)); parameter.alt = malloc(sizeof(float)); parameter.spd = malloc(sizeof(float)); parameter.cog = malloc(sizeof(float)); parameter.hdop = malloc(sizeof(float)); parameter.sat = malloc(sizeof(float)); parameter.time = malloc(sizeof(float)); parameter.date = malloc(sizeof(float)); parameter.vspeed = malloc(sizeof(float)); parameter.dist = malloc(sizeof(float)); parameter.spd_kmh = malloc(sizeof(float)); parameter.fix = malloc(sizeof(float)); parameter.vdop = malloc(sizeof(float)); parameter.speed_acc = malloc(sizeof(float)); parameter.h_acc = malloc(sizeof(float)); parameter.v_acc = malloc(sizeof(float)); parameter.track_acc = malloc(sizeof(float)); parameter.n_vel = malloc(sizeof(float)); parameter.e_vel = malloc(sizeof(float)); parameter.v_vel = malloc(sizeof(float)); parameter.alt_elipsiod = malloc(sizeof(float)); parameter.pdop = malloc(sizeof(float)); parameter.fix_type = malloc(sizeof(uint8_t)); parameter.home_set = malloc(sizeof(uint8_t)); parameter.alt_home = malloc(sizeof(float)); xTaskCreate(gps_task, "gps_task", STACK_GPS, (void *)¶meter, 2, &task_handle); context.uart_pio_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_coordinate_parameters_t parameter_sensor_coordinate; parameter_sensor_coordinate.type = SMARTPORT_LATITUDE; parameter_sensor_coordinate.latitude = parameter.lat; parameter_sensor_coordinate.longitude = parameter.lon; parameter_sensor_coordinate.rate = config->refresh_rate_gps; xTaskCreate(sensor_coordinates_task, "sensor_coordinates_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor_coordinate, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_datetime_parameters_t parameter_sensor_datetime; parameter_sensor_datetime.type = SMARTPORT_DATE; parameter_sensor_datetime.date = parameter.date; parameter_sensor_datetime.time = parameter.time; parameter_sensor_datetime.rate = 1000; xTaskCreate(sensor_datetime_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor_datetime, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; parameter_sensor.data_id = GPS_ALT_FIRST_ID; parameter_sensor.value = parameter.alt; parameter_sensor.rate = config->refresh_rate_gps; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = GPS_SPEED_FIRST_ID; parameter_sensor.value = parameter.spd; parameter_sensor.rate = config->refresh_rate_gps; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = GPS_COURS_FIRST_ID; parameter_sensor.value = parameter.cog; parameter_sensor.rate = config->refresh_rate_gps; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = VARIO_FIRST_ID + 1; parameter_sensor.value = parameter.vspeed; parameter_sensor.rate = config->refresh_rate_gps; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = GPS_SATE_FIRST_ID; parameter_sensor.value = parameter.sat; parameter_sensor.rate = 1000; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = DIST_FIRST_ID; parameter_sensor.value = parameter.dist; parameter_sensor.rate = config->refresh_rate_gps; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = DIY_FIRST_ID + 5; parameter_sensor.value = parameter.pdop; parameter_sensor.rate = config->refresh_rate_gps; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_voltage) { voltage_parameters_t parameter = {0, config->analog_rate, config->alpha_voltage, config->analog_voltage_multiplier, malloc(sizeof(float))}; xTaskCreate(voltage_task, "voltage_task", STACK_VOLTAGE, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; parameter_sensor.data_id = A3_FIRST_ID; parameter_sensor.value = parameter.voltage; parameter_sensor.rate = config->refresh_rate_voltage; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_current) { current_parameters_t parameter = {1, config->analog_rate, config->alpha_current, config->analog_current_multiplier, config->analog_current_offset, config->analog_current_autoffset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(current_task, "current_task", STACK_CURRENT, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; parameter_sensor.data_id = CURR_FIRST_ID; parameter_sensor.value = parameter.current; parameter_sensor.rate = config->refresh_rate_current; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_double_parameters_t parameter_sensor_double; parameter_sensor_double.data_id = ESC_RPM_CONS_FIRST_ID; parameter_sensor_double.value_l = NULL; parameter_sensor_double.value_h = parameter.consumption; parameter_sensor_double.rate = config->refresh_rate_current; xTaskCreate(sensor_double_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor_double, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_BMP280) { bmp280_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, 0, config->bmp280_filter, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(bmp280_task, "bmp280_task", STACK_BMP280, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } smartport_sensor_parameters_t parameter_sensor; parameter_sensor.data_id = ALT_FIRST_ID; parameter_sensor.value = parameter.altitude; parameter_sensor.rate = config->refresh_rate_vario; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = VARIO_FIRST_ID; parameter_sensor.value = parameter.vspeed; parameter_sensor.rate = config->refresh_rate_vario; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_MS5611) { ms5611_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, 0, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(ms5611_task, "ms5611_task", STACK_MS5611, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } smartport_sensor_parameters_t parameter_sensor; parameter_sensor.data_id = ALT_FIRST_ID; parameter_sensor.value = parameter.altitude; parameter_sensor.rate = config->refresh_rate_vario; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = VARIO_FIRST_ID; parameter_sensor.value = parameter.vspeed; parameter_sensor.rate = config->refresh_rate_vario; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_BMP180) { bmp180_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(bmp180_task, "bmp180_task", STACK_BMP180, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } smartport_sensor_parameters_t parameter_sensor; parameter_sensor.data_id = ALT_FIRST_ID; parameter_sensor.value = parameter.altitude; parameter_sensor.rate = config->refresh_rate_vario; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = VARIO_FIRST_ID; parameter_sensor.value = parameter.vspeed; parameter_sensor.rate = config->refresh_rate_vario; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_ntc) { ntc_parameters_t parameter = {2, config->analog_rate, config->ntc_offset, config->alpha_temperature, malloc(sizeof(float))}; xTaskCreate(ntc_task, "ntc_task", STACK_NTC, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; parameter_sensor.data_id = ESC_TEMPERATURE_FIRST_ID; parameter_sensor.value = parameter.ntc; parameter_sensor.rate = config->refresh_rate_temperature; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_airspeed) { airspeed_parameters_t parameter = {3, config->analog_rate, config->alpha_airspeed, (float)config->airspeed_offset / 1000, (float)config->airspeed_vcc / 100, baro_temp, baro_pressure, malloc(sizeof(float))}; xTaskCreate(airspeed_task, "airspeed_task", STACK_AIRSPEED, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; parameter_sensor.data_id = AIR_SPEED_FIRST_ID; parameter_sensor.value = parameter.airspeed; parameter_sensor.rate = config->refresh_rate_airspeed; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_fuel_flow) { fuel_meter_parameters_t parameter = {config->fuel_flow_ml_per_pulse, malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(fuel_meter_task, "fuel_meter_task", STACK_FUEL_METER, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; parameter_sensor.data_id = GASSUIT_FLOW_FIRST_ID; parameter_sensor.value = parameter.consumption_instant; parameter_sensor.rate = config->refresh_rate_default; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = GASSUIT_RES_VOL_FIRST_ID; parameter_sensor.value = parameter.consumption_total; parameter_sensor.rate = config->refresh_rate_default; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->gpio_mask) { gpio_parameters_t parameter = {config->gpio_mask, config->gpio_interval, malloc(sizeof(float))}; xTaskCreate(gpio_task, "gpio_task", STACK_GPIO, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_gpio_parameters_t parameter_sensor; parameter_sensor.data_id = DIY_FIRST_ID; parameter_sensor.value = parameter.value; parameter_sensor.rate = config->gpio_interval; parameter_sensor.gpio_mask = config->gpio_mask; xTaskCreate(sensor_gpio_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_gyro) { mpu6050_parameters_t parameter = {1, 0, config->mpu6050_acc_scale, config->mpu6050_gyro_scale, config->mpu6050_gyro_weighting, config->mpu6050_filter, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(mpu6050_task, "mpu6050_task", STACK_MPU6050, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); smartport_sensor_parameters_t parameter_sensor; parameter_sensor.data_id = DIY_FIRST_ID + 6; parameter_sensor.value = parameter.pitch; parameter_sensor.rate = config->refresh_rate_default; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = DIY_FIRST_ID + 7; parameter_sensor.value = parameter.roll; parameter_sensor.rate = config->refresh_rate_default; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = DIY_FIRST_ID + 8; parameter_sensor.value = parameter.yaw; parameter_sensor.rate = config->refresh_rate_default; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ACCX_FIRST_ID; parameter_sensor.value = parameter.acc_x; parameter_sensor.rate = config->refresh_rate_default; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ACCY_FIRST_ID; parameter_sensor.value = parameter.acc_y; parameter_sensor.rate = config->refresh_rate_default; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = ACCZ_FIRST_ID; parameter_sensor.value = parameter.acc_z; parameter_sensor.rate = config->refresh_rate_default; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); parameter_sensor.data_id = DIY_FIRST_ID + 9; parameter_sensor.value = parameter.acc; parameter_sensor.rate = config->refresh_rate_default; xTaskCreate(sensor_task, "sensor_task", STACK_SENSOR_SMARTPORT, (void *)¶meter_sensor, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_lipo && config->lipo_cells > 0) { smartport_sensor_cell_individual_parameters_t parameter_sensor_cell; float *cell_prev = NULL; // Maximum supported cells: 6 (two INA3221 devices) uint8_t lipo_cells = MIN(config->lipo_cells, 6); // Initialize all cell pointers to NULL (safety) for (uint i = 0; i < 18; i++) { parameter_sensor_cell.cell_voltage[i] = NULL; } // Configure task parameters parameter_sensor_cell.rate = config->refresh_rate_voltage; parameter_sensor_cell.cell_count = malloc(sizeof(uint8_t)); *parameter_sensor_cell.cell_count = lipo_cells; // --- First INA3221: cells 0–2 ----------------------------------------- uint8_t cells_first = MIN(lipo_cells, 3); if (cells_first > 0) { ina3221_parameters_t parameter = { .i2c_address = 0x40, .filter = config->ina3221_filter, .cell_count = cells_first, .cell[0] = malloc(sizeof(float)), .cell[1] = malloc(sizeof(float)), .cell[2] = malloc(sizeof(float)), .cell_prev = malloc(sizeof(float)), }; // First INA has no previous cell reference *parameter.cell_prev = 0; cell_prev = parameter.cell[2]; xTaskCreate(ina3221_task, "ina3221_task", STACK_INA3221, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // Store cell pointers for SmartPort for (uint i = 0; i < cells_first; i++) { parameter_sensor_cell.cell_voltage[i] = parameter.cell[i]; } } // --- Second INA3221: cells 3–5 ---------------------------------------- if (lipo_cells > 3) { uint8_t cells_second = MIN((uint8_t)(lipo_cells - 3), (uint8_t)3); ina3221_parameters_t parameter = { .i2c_address = 0x41, .filter = config->ina3221_filter, .cell_count = cells_second, .cell[0] = malloc(sizeof(float)), .cell[1] = malloc(sizeof(float)), .cell[2] = malloc(sizeof(float)), .cell_prev = cell_prev, // Link to the last cell of the previous INA }; xTaskCreate(ina3221_task, "ina3221_task", STACK_INA3221, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // Store cell pointers for SmartPort for (uint i = 0; i < cells_second; i++) { parameter_sensor_cell.cell_voltage[i + 3] = parameter.cell[i]; } } // --- Start SmartPort task: cycle through individual cells ------------- xTaskCreate(sensor_cell_individual_task, "sensor_cell_task", STACK_SENSOR_SMARTPORT_CELL, (void *)¶meter_sensor_cell, 3, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } } static void send_packet(uint8_t frame_id, uint16_t data_id, uint32_t value) { uint16_t crc = 0; uint8_t *u8p; // frame_id smartport_send_byte(frame_id, &crc); // data_id u8p = (uint8_t *)&data_id; smartport_send_byte(u8p[0], &crc); smartport_send_byte(u8p[1], &crc); // value u8p = (uint8_t *)&value; smartport_send_byte(u8p[0], &crc); smartport_send_byte(u8p[1], &crc); smartport_send_byte(u8p[2], &crc); smartport_send_byte(u8p[3], &crc); // crc smartport_send_byte(0xFF - (uint8_t)crc, NULL); // blink vTaskResume(context.led_task_handle); } ================================================ FILE: board/project/protocol/smartport.h ================================================ #ifndef SMARTPORT_H #define SMARTPORT_H #include "common.h" // FrSky Smartport Data Id #define ALT_FIRST_ID 0x0100 // 100 m #define ALT_LAST_ID 0x010f #define VARIO_FIRST_ID 0x0110 // 100 m/s #define VARIO_LAST_ID 0x011f #define CURR_FIRST_ID 0x0200 // 10 A #define CURR_LAST_ID 0x020f #define VFAS_FIRST_ID 0x0210 // 100 v #define VFAS_LAST_ID 0x021f #define CELLS_FIRST_ID 0x0300 // #define CELLS_LAST_ID 0x030f #define T1_FIRST_ID 0x0400 // 1 C #define T1_LAST_ID 0x040f #define T2_FIRST_ID 0x0410 // 1 C #define T2_LAST_ID 0x041f #define DIST_FIRST_ID 0x0420 #define DIST_LAST_ID 0x042f #define RPM_FIRST_ID 0x0500 // 1 rpm #define RPM_LAST_ID 0x050f #define FUEL_FIRST_ID 0x0600 // 1 % #define FUEL_LAST_ID 0x060f #define ACCX_FIRST_ID 0x0700 // 100 g #define ACCX_LAST_ID 0x070f #define ACCY_FIRST_ID 0x0710 // 100 g #define ACCY_LAST_ID 0x071f #define ACCZ_FIRST_ID 0x0720 // 100 g #define ACCZ_LAST_ID 0x072f #define GPS_LONG_LATI_FIRST_ID 0x0800 // bit32(1<<31)=1=LON:=0=LAT, bit31(1<<30)=1=-:=0=+, escaler: 10000 #define GPS_LONG_LATI_LAST_ID 0x080f #define GPS_ALT_FIRST_ID 0x0820 // 100 m #define GPS_ALT_LAST_ID 0x082f #define GPS_SPEED_FIRST_ID 0x0830 // 1000 kts #define GPS_SPEED_LAST_ID 0x083f #define GPS_COURS_FIRST_ID 0x0840 // 100 º #define GPS_COURS_LAST_ID 0x084f #define GPS_TIME_DATE_FIRST_ID 0x0850 // Date: Y M D 0xFF or Time: H M S 0x00 #define GPS_TIME_DATE_LAST_ID 0x085f #define GPS_SATE_FIRST_ID 0x0860 // Satellites, max 32 #define GPS_SATE_LAST_ID 0x086f #define A3_FIRST_ID 0x0900 // 100 v #define A3_LAST_ID 0x090f #define A4_FIRST_ID 0x0910 // 100 v #define A4_LAST_ID 0x091f #define AIR_SPEED_FIRST_ID 0x0a00 // 10 kts #define AIR_SPEED_LAST_ID 0x0a0f #define RBOX_BATT1_FIRST_ID 0x0b00 // 1000 v, 100 A #define RBOX_BATT1_LAST_ID 0x0b0f #define RBOX_BATT2_FIRST_ID 0x0b10 // 1000 v, 100 A #define RBOX_BATT2_LAST_ID 0x0b1f #define RBOX_STATE_FIRST_ID 0x0b20 // 1 #define RBOX_STATE_LAST_ID 0x0b2f #define RBOX_CNSP_FIRST_ID 0x0b30 // 1 mAh (1), 1mAh (2) #define RBOX_CNSP_LAST_ID 0x0b3f #define SD1_FIRST_ID 0x0b40 #define SD1_LAST_ID 0x0b4f #define ESC_POWER_FIRST_ID 0x0b50 // bytes 1,2: 100 V, bytes 3,4: 100 A #define ESC_POWER_LAST_ID 0x0b5f #define ESC_RPM_CONS_FIRST_ID 0x0b60 // bytes 1,2: 0.01 rpm, bytes 3,4: 1 mah #define ESC_RPM_CONS_LAST_ID 0x0b6f #define ESC_TEMPERATURE_FIRST_ID 0x0b70 // 1 C #define ESC_TEMPERATURE_LAST_ID 0x0b7f #define X8R_FIRST_ID 0x0c20 #define X8R_LAST_ID 0x0c2f #define S6R_FIRST_ID 0x0c30 #define S6R_LAST_ID 0x0c3f #define GASSUIT_TEMP1_FIRST_ID 0x0d00 // 1 C #define GASSUIT_TEMP1_LAST_ID 0x0d0f #define GASSUIT_TEMP2_FIRST_ID 0x0d10 // 1 C #define GASSUIT_TEMP2_LAST_ID 0x0d1f #define GASSUIT_SPEED_FIRST_ID 0x0d20 // 1 rpm #define GASSUIT_SPEED_LAST_ID 0x0d2f #define GASSUIT_RES_VOL_FIRST_ID 0x0d30 // 1 ml #define GASSUIT_RES_VOL_LAST_ID 0x0d3f #define GASSUIT_RES_PERC_FIRST_ID 0x0d40 // 1 % #define GASSUIT_RES_PERC_LAST_ID 0x0d4f #define GASSUIT_FLOW_FIRST_ID 0x0d50 // 1 ml/min #define GASSUIT_FLOW_LAST_ID 0x0d5f #define GASSUIT_MAX_FLOW_FIRST_ID 0x0d60 // 1 ml/min #define GASSUIT_MAX_FLOW_LAST_ID 0x0d6f #define GASSUIT_AVG_FLOW_FIRST_ID 0x0d70 // 1 ml/min #define GASSUIT_AVG_FLOW_LAST_ID 0x0d7f #define SBEC_POWER_FIRST_ID 0x0e50 // bytes 1,2: 100 V, bytes 3,4: 100 A #define SBEC_POWER_LAST_ID 0x0e5f #define DIY_FIRST_ID 0x5100 #define DIY_LAST_ID 0x52ff #define DIY_STREAM_FIRST_ID 0x5000 #define DIY_STREAM_LAST_ID 0x50ff #define FACT_TEST_ID 0xf000 #define RSSI_ID 0xf101 #define A1_ID 0xf102 // 10 v #define A2_ID 0xf103 // 10 v #define SP2UART_A_ID 0xfd00 #define SP2UART_B_ID 0xfd01 #define RXBT_ID 0xf104 // 10 v #define RAS_ID 0xf105 #define XJT_VERSION_ID 0xf106 #define FUEL_QTY_FIRST_ID 0x0a10 // 100 ml #define FUEL_QTY_LAST_ID 0x0a1f #define TIMEOUT_US 500 typedef enum coordinate_type_t { SMARTPORT_LATITUDE, SMARTPORT_LONGITUDE, } coordinate_type_t; typedef enum datetime_type_t { SMARTPORT_DATE, SMARTPORT_TIME, } datetime_type_t; typedef struct smartport_parameters_t { uint8_t sensor_id; uint16_t data_id; } smartport_parameters_t; typedef struct smartport_sensor_parameters_t { uint16_t data_id; float *value; uint16_t rate; } smartport_sensor_parameters_t; typedef struct smartport_sensor_gpio_parameters_t { uint16_t data_id; uint8_t gpio_mask; uint8_t *value; uint16_t rate; } smartport_sensor_gpio_parameters_t; typedef struct smartport_sensor_double_parameters_t { uint16_t data_id; float *value_l; float *value_h; uint16_t rate; } smartport_sensor_double_parameters_t; typedef struct smartport_sensor_coordinate_parameters_t { coordinate_type_t type; float *latitude; float *longitude; uint16_t rate; } smartport_sensor_coordinate_parameters_t; typedef struct smartport_sensor_datetime_parameters_t { datetime_type_t type; float *date; float *time; uint16_t rate; } smartport_sensor_datetime_parameters_t; typedef struct smartport_sensor_cell_parameters_t { uint8_t *cell_count; float *cell_voltage; uint16_t rate; } smartport_sensor_cell_parameters_t; typedef struct smartport_sensor_cell_individual_parameters_t { uint8_t *cell_count; float *cell_voltage[18]; uint16_t rate; } smartport_sensor_cell_individual_parameters_t; typedef struct smartport_packet_parameters_t { uint16_t data_id; QueueHandle_t queue_handle; } smartport_packet_parameters_t; typedef struct smartport_packet_t { uint16_t frame_id; uint16_t data_id; uint32_t value; } smartport_packet_t; extern context_t context; void smartport_task(void *parameters); int32_t smartport_format(uint16_t data_id, float value); uint32_t smartport_format_double(uint16_t data_id, float value_l, float value_h); uint32_t smartport_format_coordinate(coordinate_type_t type, float value); uint32_t smartport_format_datetime(uint8_t type, uint32_t value); uint32_t smartport_format_cell(uint8_t cell_index, float value); uint8_t smartport_get_crc(uint8_t *data, uint len); smartport_packet_t smartport_process_packet(smartport_parameters_t *parameter, uint8_t frame_id, uint16_t data_id, uint32_t value); void smartport_send_byte(uint8_t c, uint16_t *crcp); uint8_t smartport_sensor_id_to_crc(uint8_t sensor_id); uint8_t smartport_sensor_crc_to_id(uint8_t sensor_id_crc); #endif ================================================ FILE: board/project/protocol/srxl.c ================================================ #include "srxl.h" #include #include #include "airspeed.h" #include "bmp180.h" #include "bmp280.h" #include "config.h" #include "current.h" #include "esc_apd_f.h" #include "esc_apd_hv.h" #include "esc_castle.h" #include "esc_hw3.h" #include "esc_hw4.h" #include "esc_hw5.h" #include "esc_kontronik.h" #include "esc_omp_m4.h" #include "esc_openyge.h" #include "esc_pwm.h" #include "esc_ztw.h" #include "fuel_meter.h" #include "gps.h" #include "hardware/i2c.h" #include "hardware/irq.h" #include "i2c_multi.h" #include "ina3221.h" #include "mpu6050.h" #include "ms5611.h" #include "ntc.h" #include "pico/stdlib.h" #include "pwm_out.h" #include "string.h" #include "uart.h" #include "uart_pio.h" #include "voltage.h" #include "xgzp68xxd.h" #define SRXL_HEADER 0xA5 #define SRXL_FRAMELEN 18 #define SRXL_TIMEOUT_US 1000 static void process(void); static void send_packet(void); static void set_config(void); void srxl_task(void *parameters) { context.led_cycle_duration = 6; context.led_cycles = 1; uart0_begin(115200, UART_RECEIVER_TX, UART_RECEIVER_RX, SRXL_TIMEOUT_US, 8, 1, UART_PARITY_NONE, false, true); xbus_set_config(); debug("\nSRXL init"); while (1) { // ulTaskNotifyTake(pdTRUE, portMAX_DELAY); ulTaskNotifyTakeIndexed(1, pdTRUE, portMAX_DELAY); process(); } } uint16_t srxl_get_crc(uint8_t *buffer, uint8_t length) { uint16_t crc = 0; for (uint8_t i = 0; i < length; ++i) { crc = srxl_crc16(crc, buffer[i]); } return crc; } uint16_t srxl_crc16(uint16_t crc, uint8_t data) { crc = crc ^ ((uint16_t)data << 8); for (int i = 0; i < 8; ++i) { if (crc & 0x8000) crc = (crc << 1) ^ 0x1021; else crc = crc << 1; } return crc; } uint srxl_sensors_count(void) { uint count = 0; for (uint i = 0; i <= XBUS_STRU_TELE_DIGITAL_AIR; i++) { if (sensor.is_enabled[i]) count++; } return count; } static void process(void) { static bool mute = true; uint8_t length = uart0_available(); if (length) { uint8_t data[length]; uart0_read_bytes(data, length); debug("\nSRXL BUFFER:"); debug_buffer(data, length, " 0x%X"); if (length == SRXL_FRAMELEN) { // uint8_t data[SRXL_FRAMELEN]; if (data[0] == SRXL_HEADER) { debug("\nSRXL (%u) < ", uxTaskGetStackHighWaterMark(NULL)); debug_buffer(data, SRXL_FRAMELEN, "0x%X "); if (!mute) send_packet(); mute = !mute; } } } } static void send_packet(void) { static uint cont = 0; if (!srxl_sensors_count()) return; while (!sensor.is_enabled[cont]) { cont++; if (cont > XBUS_ENERGY) cont = 0; } uint8_t buffer_header[3] = {SRXL_HEADER, 0x80, 0x15}; uart0_write_bytes(buffer_header, 3); debug("\nSRXL (%u) > %X %X %X", uxTaskGetStackHighWaterMark(NULL), buffer_header[0], buffer_header[1], buffer_header[2]); uint8_t buffer[16]; switch (cont) { case XBUS_AIRSPEED: xbus_format_sensor(XBUS_AIRSPEED_ID, buffer); uart0_write_bytes(buffer, sizeof(xbus_airspeed_t)); debug("\nSRXL (%u) < ", uxTaskGetStackHighWaterMark(NULL)); debug_buffer(buffer, sizeof(xbus_airspeed_t), "0x%X "); break; case XBUS_BATTERY: xbus_format_sensor(XBUS_BATTERY_ID, buffer); uart0_write_bytes(buffer, sizeof(xbus_battery_t)); debug("\nSRXL (%u) < ", uxTaskGetStackHighWaterMark(NULL)); debug_buffer(buffer, sizeof(xbus_battery_t), "0x%X "); break; case XBUS_ESC: xbus_format_sensor(XBUS_ESC_ID, buffer); uart0_write_bytes(buffer, sizeof(xbus_esc_t)); debug("\nSRXL (%u) < ", uxTaskGetStackHighWaterMark(NULL)); debug_buffer(buffer, sizeof(xbus_esc_t), "0x%X "); break; case XBUS_GPS_LOC: xbus_format_sensor(XBUS_GPS_LOC_ID, buffer); uart0_write_bytes(buffer, sizeof(xbus_gps_loc_t)); debug("\nSRXL (%u) < ", uxTaskGetStackHighWaterMark(NULL)); debug_buffer(buffer, sizeof(xbus_gps_loc_t), "0x%X "); break; case XBUS_GPS_STAT: xbus_format_sensor(XBUS_GPS_STAT_ID, buffer); uart0_write_bytes(buffer, sizeof(xbus_gps_stat_t)); debug("\nSRXL (%u) < ", uxTaskGetStackHighWaterMark(NULL)); debug_buffer(buffer, sizeof(xbus_gps_stat_t), "0x%X "); break; case XBUS_RPMVOLTTEMP: xbus_format_sensor(XBUS_RPMVOLTTEMP_ID, buffer); uart0_write_bytes(buffer, sizeof(xbus_rpm_volt_temp_t)); debug("\nSRXL (%u) < ", uxTaskGetStackHighWaterMark(NULL)); debug_buffer(buffer, sizeof(xbus_rpm_volt_temp_t), "0x%X "); break; case XBUS_FUEL_FLOW: xbus_format_sensor(XBUS_FUEL_FLOW_ID, buffer); uart0_write_bytes(buffer, sizeof(xbus_fuel_flow_t)); debug("\nSRXL (%u) < ", uxTaskGetStackHighWaterMark(NULL)); debug_buffer(buffer, sizeof(xbus_fuel_flow_t), "0x%X "); break; case XBUS_STRU_TELE_DIGITAL_AIR: xbus_format_sensor(XBUS_STRU_TELE_DIGITAL_AIR_ID, buffer); uart0_write_bytes(buffer, sizeof(xbus_stru_tele_digital_air_t)); debug("\nSRXL (%u) < ", uxTaskGetStackHighWaterMark(NULL)); debug_buffer(buffer, sizeof(xbus_stru_tele_digital_air_t), "0x%X "); break; } uint16_t crc; crc = __builtin_bswap16(srxl_get_crc(buffer, 19)); // all bytes, including header uart0_write_bytes((uint8_t *)&crc, 2); debug("%X ", crc); cont++; vTaskResume(context.led_task_handle); } ================================================ FILE: board/project/protocol/srxl.h ================================================ #ifndef SRXL_H #define SRXL_H #include "common.h" #include "xbus.h" extern context_t context; extern xbus_sensor_t sensor; void srxl_task(void *parameters); uint16_t srxl_get_crc(uint8_t *buffer, uint8_t length); uint16_t srxl_crc16(uint16_t crc, uint8_t data); uint srxl_sensors_count(void); #endif ================================================ FILE: board/project/protocol/srxl2.c ================================================ #include "srxl2.h" #include #include #include "airspeed.h" #include "bmp180.h" #include "bmp280.h" #include "config.h" #include "current.h" #include "esc_apd_f.h" #include "esc_apd_hv.h" #include "esc_castle.h" #include "esc_hw3.h" #include "esc_hw4.h" #include "esc_hw5.h" #include "esc_kontronik.h" #include "esc_omp_m4.h" #include "esc_openyge.h" #include "esc_pwm.h" #include "esc_ztw.h" #include "fuel_meter.h" #include "gps.h" #include "hardware/i2c.h" #include "hardware/irq.h" #include "i2c_multi.h" #include "ina3221.h" #include "mpu6050.h" #include "ms5611.h" #include "ntc.h" #include "pico/stdlib.h" #include "pwm_out.h" #include "srxl.h" #include "string.h" #include "uart.h" #include "uart_pio.h" #include "voltage.h" #include "xgzp68xxd.h" #define SRXL2_DEVICE_PRIORITY 10 #define SRXL2_DEVICE_BAUDRATE 1 // 0 = 115200. 1 = 400000 #define SRXL2_DEVICE_INFO 0 #define SRXL2_DEVICE_UID 0x12345678 #define MAX_BAD_FRAMES 50 static volatile uint8_t dest_id = 0xFF; static volatile uint8_t baudrate = 0; static alarm_id_t alarm_id; static volatile bool send_handshake = false; static uint8_t sensor_id_; static void process(void); static void send_packet(void); static void set_config(void); static int64_t alarm_50ms(alarm_id_t id, void *user_data); void srxl2_task(void *parameters) { alarm_id = add_alarm_in_us(50000, alarm_50ms, NULL, true); context.led_cycle_duration = 6; context.led_cycles = 1; uart0_begin(115200, UART_RECEIVER_TX, UART_RECEIVER_RX, SRXL2_TIMEOUT_US, 8, 1, UART_PARITY_NONE, false, true); config_t *config = config_read(); sensor_id_ = config->sensor_id_srxl2 | 0x30; // Ensure it's between 0x31 and 0x3F if (sensor_id_ < 0x31 || sensor_id_ > 0x3F) sensor_id_ = 0x31; xbus_set_config(); debug("\nSRXL2 init"); while (1) { ulTaskNotifyTakeIndexed(1, pdTRUE, portMAX_DELAY); process(); if (send_handshake) { send_handshake = false; srxl2_send_handshake(uart0, sensor_id_, 0, SRXL2_DEVICE_PRIORITY, SRXL2_DEVICE_BAUDRATE, SRXL2_DEVICE_INFO, SRXL2_DEVICE_UID); } } } void srxl2_send_handshake(uart_inst_t *uart, uint8_t source_id, uint8_t dest_id, uint8_t priority, uint8_t baudrate, uint8_t info, uint uid) { srxl2_handshake_t handshake; handshake.header = SRXL2_HEADER; handshake.type = SRXL2_PACKET_TYPE_HANDSHAKE; handshake.len = SRXL2_HANDSHAKE_LEN; handshake.source_id = source_id; handshake.dest_id = dest_id; handshake.priority = priority; handshake.baudrate = baudrate; handshake.info = info; handshake.uid = uid; uint16_t crc = srxl_get_crc((uint8_t *)&handshake, SRXL2_HANDSHAKE_LEN - 2); handshake.crc = swap_16(crc); if (uart_get_index(uart)) uart1_write_bytes((uint8_t *)&handshake, SRXL2_HANDSHAKE_LEN); else uart0_write_bytes((uint8_t *)&handshake, SRXL2_HANDSHAKE_LEN); debug("\nSRXL2 (%u). Send Handshake >", uxTaskGetStackHighWaterMark(NULL)); debug_buffer((uint8_t *)&handshake, SRXL2_HANDSHAKE_LEN, " 0x%X"); } static void process(void) { uint8_t length = uart0_available(); if (length) { cancel_alarm(alarm_id); alarm_id = add_alarm_in_us(50000, alarm_50ms, NULL, true); // Check for bad frames static uint bad_frames = 0; if (bad_frames > MAX_BAD_FRAMES) { if (baudrate) { uart_set_baudrate(uart0, 115200); baudrate = 0; debug("\nSRXL2. Autobaud 115200"); } else { uart_set_baudrate(uart0, 400000); baudrate = 1; debug("\nSRXL2. Autobaud 400000"); } bad_frames = 0; return; } if (length < 3 || length > 128) { if (length == 1) return; debug("\nSRXL2. Bad Fr %u (len %u)", bad_frames, length); bad_frames++; return; } uint8_t data[length]; uint16_t crc; uart0_read_bytes(data, length); debug("\nSRXL2 (%u) < ", uxTaskGetStackHighWaterMark(NULL)); debug_buffer(data, length, " 0x%X"); crc = srxl_get_crc(data, length - 2); if ((crc >> 8) == data[length - 2] && (crc & 0xFF) == data[length - 1]) { debug(" -> CRC OK"); } else { debug(" -> BAD CRC"); debug(" %X", crc); // Allow packets with wrong crc for handshake if (dest_id != 0xFF || !(data[0] == SRXL2_HEADER && data[1] == SRXL2_PACKET_TYPE_HANDSHAKE && data[4] == sensor_id_)) { bad_frames++; return; } } bad_frames = 0; // Handshake received if (data[0] == SRXL2_HEADER && data[1] == SRXL2_PACKET_TYPE_HANDSHAKE && data[4] == sensor_id_) { dest_id = data[3]; debug("\nSRXL2. Set dest_id 0x%X", dest_id); srxl2_send_handshake(uart0, sensor_id_, dest_id, SRXL2_DEVICE_PRIORITY, SRXL2_DEVICE_BAUDRATE, SRXL2_DEVICE_INFO, SRXL2_DEVICE_UID); } // Send telemetry else if (data[0] == SRXL2_HEADER && data[1] == SRXL2_PACKET_TYPE_CONTROL && data[4] == sensor_id_) { send_packet(); } // Set baudrate else if (data[0] == SRXL2_HEADER && data[1] == SRXL2_PACKET_TYPE_HANDSHAKE && data[4] == 0xFF) { baudrate = data[6]; if (baudrate) uart_set_baudrate(uart0, 400000); else uart_set_baudrate(uart0, 115200); debug("\nSRXL2. Set baudrate %u", baudrate); } } } static void send_packet(void) { static uint cont = 0; if (!srxl_sensors_count()) return; while (!sensor.is_enabled[cont]) { cont++; if (cont > XBUS_STRU_TELE_DIGITAL_AIR) cont = 0; } srxl2_telemetry_t packet = {0}; packet.header = SRXL2_HEADER; packet.type = SRXL2_PACKET_TYPE_TELEMETRY; packet.len = SRXL2_TELEMETRY_LEN; packet.dest_id = dest_id; uint8_t buffer[16]; switch (cont) { case XBUS_AIRSPEED: xbus_format_sensor(XBUS_AIRSPEED_ID, buffer); memcpy((uint8_t *)&packet.xbus_packet, (uint8_t *)buffer, sizeof(xbus_airspeed_t)); break; case XBUS_BATTERY: xbus_format_sensor(XBUS_BATTERY_ID, buffer); memcpy((uint8_t *)&packet.xbus_packet, (uint8_t *)buffer, sizeof(xbus_battery_t)); break; case XBUS_ESC: xbus_format_sensor(XBUS_ESC_ID, buffer); memcpy((uint8_t *)&packet.xbus_packet, (uint8_t *)buffer, sizeof(xbus_esc_t)); break; case XBUS_GPS_LOC: xbus_format_sensor(XBUS_GPS_LOC_ID, buffer); memcpy((uint8_t *)&packet.xbus_packet, (uint8_t *)buffer, sizeof(xbus_gps_loc_t)); break; case XBUS_GPS_STAT: xbus_format_sensor(XBUS_GPS_STAT_ID, buffer); memcpy((uint8_t *)&packet.xbus_packet, (uint8_t *)buffer, sizeof(xbus_gps_stat_t)); break; case XBUS_RPMVOLTTEMP: xbus_format_sensor(XBUS_RPMVOLTTEMP_ID, buffer); memcpy((uint8_t *)&packet.xbus_packet, (uint8_t *)buffer, sizeof(xbus_rpm_volt_temp_t)); break; case XBUS_FUEL_FLOW: xbus_format_sensor(XBUS_FUEL_FLOW_ID, buffer); memcpy((uint8_t *)&packet.xbus_packet, (uint8_t *)buffer, sizeof(xbus_fuel_flow_t)); break; case XBUS_STRU_TELE_DIGITAL_AIR: xbus_format_sensor(XBUS_STRU_TELE_DIGITAL_AIR_ID, buffer); memcpy((uint8_t *)&packet.xbus_packet, buffer, sizeof(xbus_stru_tele_digital_air_t)); break; } uint16_t crc = srxl_get_crc((uint8_t *)&packet, SRXL2_TELEMETRY_LEN - 2); packet.crc = swap_16(crc); uart0_write_bytes((uint8_t *)&packet, SRXL2_TELEMETRY_LEN); cont++; debug("\nSRXL2 (%u) > ", uxTaskGetStackHighWaterMark(NULL)); debug_buffer((uint8_t *)&packet, SRXL2_TELEMETRY_LEN, "0x%X "); vTaskResume(context.led_task_handle); } static int64_t alarm_50ms(alarm_id_t id, void *user_data) { BaseType_t xHigherPriorityTaskWoken = pdFALSE; send_handshake = true; if (baudrate) uart_set_baudrate(uart0, 115200); baudrate = 0; vTaskNotifyGiveIndexedFromISR(context.uart0_notify_task_handle, 1, &xHigherPriorityTaskWoken); portYIELD_FROM_ISR(xHigherPriorityTaskWoken); return 50000; } ================================================ FILE: board/project/protocol/srxl2.h ================================================ #ifndef SRXL2_H #define SRXL2_H #include "common.h" #include "xbus.h" #define SRXL2_TIMEOUT_US 500 #define SRXL2_HEADER 0xA6 #define SRXL2_HANDSHAKE_LEN 14 #define SRXL2_TELEMETRY_LEN 22 #define SRXL2_CONTROL_CMD_CHANNEL 0x00 // channel data #define SRXL2_PACKET_TYPE_HANDSHAKE 0x21 #define SRXL2_PACKET_TYPE_CONTROL 0xCD #define SRXL2_PACKET_TYPE_TELEMETRY 0x80 extern context_t context; extern xbus_sensor_t sensor; typedef struct srxl2_handshake_t { uint8_t header; uint8_t type; uint8_t len; uint8_t source_id; uint8_t dest_id; uint8_t priority; uint8_t baudrate; uint8_t info; uint32_t uid; uint16_t crc; } __attribute__((packed)) srxl2_handshake_t; typedef struct srxl2_telemetry_t { uint8_t header; uint8_t type; uint8_t len; uint8_t dest_id; uint8_t xbus_packet[16]; uint16_t crc; } __attribute__((packed)) srxl2_telemetry_t; void srxl2_task(void *parameters); void srxl2_send_handshake(uart_inst_t *uart, uint8_t source_id, uint8_t dest_id, uint8_t priority, uint8_t baudrate, uint8_t info, uint uid); #endif ================================================ FILE: board/project/protocol/xbus.c ================================================ #include "xbus.h" #include #include #include "airspeed.h" #include "bmp180.h" #include "bmp280.h" #include "config.h" #include "current.h" #include "esc_apd_f.h" #include "esc_apd_hv.h" #include "esc_castle.h" #include "esc_hw3.h" #include "esc_hw4.h" #include "esc_hw5.h" #include "esc_kontronik.h" #include "esc_omp_m4.h" #include "esc_openyge.h" #include "esc_pwm.h" #include "esc_ztw.h" #include "fuel_meter.h" #include "gps.h" #include "hardware/i2c.h" #include "hardware/irq.h" #include "i2c_multi.h" #include "ina3221.h" #include "mpu6050.h" #include "ms5611.h" #include "ntc.h" #include "pico/stdlib.h" #include "pwm_out.h" #include "stdlib.h" #include "string.h" #include "uart.h" #include "uart_pio.h" #include "voltage.h" #include "xgzp68xxd.h" #define XBUS_GPS_FIX (1 << 0) #define XBUS_GPS_3D (1 << 1) #define XBUS_GPS_HOME (1 << 4) static void i2c_request_handler(uint8_t address); // static void set_config(void); static uint8_t bcd8(float value, uint8_t precision); static uint16_t bcd16(float value, uint8_t precision); static uint32_t bcd32(float value, uint8_t precision); static int64_t interval_250_callback(alarm_id_t id, void *parameters); static int64_t interval_500_callback(alarm_id_t id, void *parameters); static int64_t interval_1000_callback(alarm_id_t id, void *parameters); static int64_t interval_1500_callback(alarm_id_t id, void *parameters); static int64_t interval_2000_callback(alarm_id_t id, void *parameters); static int64_t interval_3000_callback(alarm_id_t id, void *parameters); xbus_sensor_t sensor; static volatile int16_t delta_0250ms, delta_0500ms, delta_1000ms, delta_1500ms, delta_2000ms, delta_3000ms; void xbus_i2c_handler(uint8_t address) { i2c_request_handler(address); } void xbus_task(void *parameters) { context.led_cycle_duration = 6; context.led_cycles = 1; PIO pio = pio1; uint pin = I2C1_SDA_GPIO; i2c_multi_init(pio, pin); i2c_multi_set_request_handler(i2c_request_handler); xbus_set_config(); debug("\nXBUS init"); vTaskSuspend(NULL); } void xbus_format_sensor(uint8_t address, uint8_t *buffer) { static float alt = 0; switch (address) { case XBUS_AIRSPEED_ID: { xbus_airspeed_t airspeed; airspeed.identifier = XBUS_AIRSPEED_ID; airspeed.airspeed = swap_16((uint16_t)(*sensor.airspeed[XBUS_AIRSPEED_AIRSPEED])); if (swap_16((uint16_t)(airspeed.airspeed)) > swap_16((uint16_t)(airspeed.max_airspeed))) airspeed.max_airspeed = airspeed.airspeed; memcpy(buffer, &airspeed, sizeof(xbus_airspeed_t)); break; } case XBUS_GPS_LOC_ID: { xbus_gps_loc_t gps_loc; gps_loc.identifier = XBUS_GPS_LOC_ID; uint8_t gps_flags = 0; float lat = *sensor.gps_loc->latitude; if (lat < 0) // N=1,+, S=0,- lat *= -1; else gps_flags |= 1 << XBUS_GPS_INFO_FLAGS_IS_NORTH_BIT; uint deg = lat; float min = (lat - deg) * 60; gps_loc.latitude = ((uint32_t)bcd8(deg, 0) << 24) | bcd32(min, 4); float lon = *sensor.gps_loc->longitude; if (lon < 0) // E=1,+, W=0,- lon *= -1; else gps_flags |= 1 << XBUS_GPS_INFO_FLAGS_IS_EAST_BIT; if (lon >= 100) { gps_flags |= 1 << XBUS_GPS_INFO_FLAGS_LONG_GREATER_99_BIT; lon -= 100; } deg = lon; min = (lon - deg) * 60; gps_loc.longitude = ((uint32_t)bcd8(deg, 0) << 24) | bcd32(min, 4); gps_loc.course = bcd16(*sensor.gps_loc->course, 1); gps_loc.hdop = bcd8(*sensor.gps_loc->hdop, 1); alt = *sensor.gps_loc->altitude_low; if (alt < 0) { gps_flags |= 1 << XBUS_GPS_INFO_FLAGS_NEGATIVE_ALT_BIT; alt *= -1; } gps_loc.gps_flags = gps_flags; gps_loc.altitude_low = bcd16(fmod(alt, 1000), 1); gps_loc.gps_flags |= (*sensor.gps_stat[XBUS_GPS_STAT_FIX_TYPE] > 0 ? 1 : 0) << XBUS_GPS_FIX; gps_loc.gps_flags |= (*sensor.gps_stat[XBUS_GPS_STAT_HOME_SET] > 0 ? 1 : 0) << XBUS_GPS_HOME; gps_loc.gps_flags |= (*sensor.gps_stat[XBUS_GPS_STAT_FIX_TYPE] == 2 ? 1 : 0) << XBUS_GPS_3D; memcpy(buffer, &gps_loc, sizeof(xbus_gps_loc_t)); break; } case XBUS_GPS_STAT_ID: { xbus_gps_stat_t gps_stat; gps_stat.identifier = XBUS_GPS_STAT_ID; gps_stat.speed = bcd16(*sensor.gps_stat[XBUS_GPS_STAT_SPEED], 1); gps_stat.utc = bcd32(*sensor.gps_stat[XBUS_GPS_STAT_TIME], 0) << 8; gps_stat.num_sats = bcd8(*sensor.gps_stat[XBUS_GPS_STAT_SATS], 0); gps_stat.altitude_high = bcd8((uint8_t)(alt / 1000), 0); memcpy(buffer, &gps_stat, sizeof(xbus_gps_stat_t)); break; } case XBUS_ENERGY_ID: { xbus_energy_t energy; energy.identifier = XBUS_ENERGY_ID; if (sensor.energy[XBUS_ENERGY_CURRENT1]) energy.current_a = swap_16((int16_t)(*sensor.energy[XBUS_ENERGY_CURRENT1] * 100)); if (sensor.energy[XBUS_ENERGY_CONSUMPTION1]) energy.charge_used_a = swap_16((int16_t)(*sensor.energy[XBUS_ENERGY_CONSUMPTION1] * 10)); if (sensor.energy[XBUS_ENERGY_VOLTAGE1]) energy.volts_a = swap_16((uint16_t)(*sensor.energy[XBUS_ENERGY_VOLTAGE1] * 100)); if (sensor.energy[XBUS_ENERGY_CURRENT2]) energy.current_b = swap_16((int16_t)(*sensor.energy[XBUS_ENERGY_CURRENT2] * 10)); if (sensor.energy[XBUS_ENERGY_CONSUMPTION2]) energy.charge_used_b = swap_16((int16_t)(*sensor.energy[XBUS_ENERGY_CONSUMPTION2])); if (sensor.energy[XBUS_ENERGY_VOLTAGE2]) energy.volts_b = swap_16((uint16_t)(*sensor.energy[XBUS_ENERGY_VOLTAGE2] * 100)); memcpy(buffer, &energy, sizeof(xbus_energy_t)); break; } case XBUS_ESC_ID: { xbus_esc_t esc; esc.identifier = XBUS_ESC_ID; if (sensor.esc[XBUS_ESC_RPM]) esc.rpm = swap_16((uint16_t)(*sensor.esc[XBUS_ESC_RPM] / 10)); if (sensor.esc[XBUS_ESC_VOLTAGE]) esc.volts_input = swap_16((uint16_t)(*sensor.esc[XBUS_ESC_VOLTAGE] * 100)); if (sensor.esc[XBUS_ESC_TEMPERATURE_FET]) esc.temp_fet = swap_16((uint16_t)(*sensor.esc[XBUS_ESC_TEMPERATURE_FET] * 10)); if (sensor.esc[XBUS_ESC_CURRENT]) esc.current_motor = swap_16((uint16_t)(*sensor.esc[XBUS_ESC_CURRENT] * 100)); if (sensor.esc[XBUS_ESC_TEMPERATURE_BEC]) esc.temp_bec = swap_16((uint16_t)(*sensor.esc[XBUS_ESC_TEMPERATURE_BEC] * 10)); if (sensor.esc[XBUS_ESC_CURRENT_BEC]) esc.current_bec = *sensor.esc[XBUS_ESC_CURRENT_BEC] * 10; if (sensor.esc[XBUS_ESC_VOLTAGE_BEC]) esc.voltage_bec = *sensor.esc[XBUS_ESC_VOLTAGE_BEC] * 20; memcpy(buffer, &esc, sizeof(xbus_esc_t)); break; } case XBUS_BATTERY_ID: { xbus_battery_t battery; battery.identifier = XBUS_BATTERY_ID; if (sensor.battery[XBUS_BATTERY_CURRENT1]) battery.current_a = swap_16((int16_t)(*sensor.battery[XBUS_BATTERY_CURRENT1] * 10)); if (sensor.battery[XBUS_BATTERY_CONSUMPTION1]) battery.charge_used_a = swap_16((int16_t)(*sensor.battery[XBUS_BATTERY_CONSUMPTION1])); if (sensor.battery[XBUS_BATTERY_TEMP1]) { float temp_f = *sensor.battery[XBUS_BATTERY_TEMP1]; temp_f = temp_f * 9.0 / 5.0 + 32; battery.temp_a = swap_16((uint16_t)(temp_f * 10)); } if (sensor.battery[XBUS_BATTERY_CURRENT2]) battery.current_b = swap_16((int16_t)(*sensor.battery[XBUS_BATTERY_CURRENT2] * 10)); if (sensor.battery[XBUS_BATTERY_CONSUMPTION2]) battery.charge_used_b = swap_16((int16_t)(*sensor.battery[XBUS_BATTERY_CONSUMPTION2])); if (sensor.battery[XBUS_BATTERY_TEMP2]) { float temp_f = *sensor.battery[XBUS_BATTERY_TEMP2]; temp_f = temp_f * 9.0 / 5.0 + 32; battery.temp_b = swap_16((uint16_t)(temp_f * 10)); } memcpy(buffer, &battery, sizeof(xbus_battery_t)); break; } case XBUS_VARIO_ID: { xbus_vario_t vario; vario.identifier = XBUS_VARIO_ID; float altitude = *sensor.vario[XBUS_VARIO_ALTITUDE]; vario.altitude = swap_16((int16_t)(altitude * 10)); #ifdef SIM_SENSORS vario.delta_0250ms = swap_16((int16_t)(-10)); vario.delta_0500ms = swap_16((int16_t)(20)); vario.delta_1000ms = swap_16((int16_t)(-12.32)); vario.delta_1500ms = swap_16((int16_t)(15)); vario.delta_2000ms = swap_16((int16_t)(20)); vario.delta_3000ms = swap_16((int16_t)(-300)); #endif memcpy(buffer, &vario, sizeof(xbus_vario_t)); break; } case XBUS_RPMVOLTTEMP_ID: { xbus_rpm_volt_temp_t rpm_volt_temp; rpm_volt_temp.identifier = XBUS_RPMVOLTTEMP_ID; if (sensor.rpm_volt_temp[XBUS_RPMVOLTTEMP_VOLT]) rpm_volt_temp.volts = swap_16((uint16_t)(*sensor.rpm_volt_temp[XBUS_RPMVOLTTEMP_VOLT] * 100)); if (sensor.rpm_volt_temp[XBUS_RPMVOLTTEMP_TEMP]) { float temp_f = *sensor.rpm_volt_temp[XBUS_RPMVOLTTEMP_TEMP]; temp_f = temp_f * 9.0 / 5.0 + 32; rpm_volt_temp.temperature = swap_16((int16_t)(temp_f)); } memcpy(buffer, &rpm_volt_temp, sizeof(xbus_rpm_volt_temp_t)); break; } case XBUS_FUEL_FLOW_ID: { xbus_fuel_flow_t fuel_flow; fuel_flow.identifier = XBUS_FUEL_FLOW_ID; if (sensor.fuel_flow[XBUS_FUEL_FLOW_CONSUMED]) fuel_flow.fuel_consumed_A = swap_16((uint16_t)(*sensor.fuel_flow[XBUS_FUEL_FLOW_CONSUMED] * 10)); if (sensor.fuel_flow[XBUS_FUEL_FLOW_RATE]) fuel_flow.flow_rate_A = swap_16((uint16_t)(*sensor.fuel_flow[XBUS_FUEL_FLOW_RATE] * 10)); memcpy(buffer, &fuel_flow, sizeof(xbus_fuel_flow_t)); break; } case XBUS_STRU_TELE_DIGITAL_AIR_ID: { xbus_stru_tele_digital_air_t stru_tele_digital_air; stru_tele_digital_air.identifier = XBUS_STRU_TELE_DIGITAL_AIR_ID; if (sensor.stru_tele_digital_air[XBUS_DIGITAL_AIR_FUEL_PRESSURE]) stru_tele_digital_air.pressure = swap_16((uint16_t)(*sensor.stru_tele_digital_air[XBUS_DIGITAL_AIR_FUEL_PRESSURE] * 0.000145038 * 10)); // Pa to psi, precision 0.1 psi memcpy(buffer, &stru_tele_digital_air, sizeof(xbus_stru_tele_digital_air_t)); break; } case XBUS_TELE_LIPOMON_ID: { xbus_tele_lipomon_t tele_lipomon; tele_lipomon.identifier = XBUS_TELE_LIPOMON_ID; for (uint i = 0; i < 6; i++) { if (*sensor.tele_lipomon[i] < 1 || !sensor.tele_lipomon[i]) tele_lipomon.cell[i] = 0xFFFF; else tele_lipomon.cell[i] = swap_16((uint16_t)(*sensor.tele_lipomon[i] * 100)); } memcpy(buffer, &tele_lipomon, sizeof(xbus_tele_lipomon_t)); break; } case XBUS_TELE_G_METER_ID: { xbus_tele_g_meter_t tele_g_meter; tele_g_meter.identifier = XBUS_TELE_G_METER_ID; if (sensor.tele_g_meter[XBUS_TELE_G_METER_X]) { tele_g_meter.GForceX = swap_16((int16_t)(*sensor.tele_g_meter[XBUS_TELE_G_METER_X] * 100)); if (swap_16((int16_t)(fabs(tele_g_meter.GForceX))) > swap_16((int16_t)(tele_g_meter.maxGForceX))) { tele_g_meter.maxGForceX = tele_g_meter.GForceX; } } if (sensor.tele_g_meter[XBUS_TELE_G_METER_Y]) { tele_g_meter.GForceY = swap_16((int16_t)(*sensor.tele_g_meter[XBUS_TELE_G_METER_Y] * 100)); if (swap_16((int16_t)(fabs(tele_g_meter.GForceY))) > swap_16((uint16_t)(tele_g_meter.maxGForceY))) { tele_g_meter.maxGForceY = tele_g_meter.GForceY; } } if (sensor.tele_g_meter[XBUS_TELE_G_METER_Z]) { tele_g_meter.GForceZ = swap_16((int16_t)(*sensor.tele_g_meter[XBUS_TELE_G_METER_Z] * 100)); if (swap_16((int16_t)(tele_g_meter.GForceZ)) > swap_16((int16_t)(tele_g_meter.maxGForceZ))) { tele_g_meter.maxGForceZ = tele_g_meter.GForceZ; } if (swap_16((int16_t)(tele_g_meter.GForceZ)) < swap_16((int16_t)(tele_g_meter.minGForceZ))) { tele_g_meter.minGForceZ = tele_g_meter.GForceZ; } } memcpy(buffer, &tele_g_meter, sizeof(xbus_tele_g_meter_t)); break; } case XBUS_TELE_GYRO_ID: { xbus_tele_gyro_t tele_gyro; tele_gyro.identifier = XBUS_TELE_GYRO_ID; if (sensor.tele_gyro[XBUS_TELE_GYRO_ROLL]) tele_gyro.gyroX = swap_16((int16_t)(*sensor.tele_gyro[XBUS_TELE_GYRO_ROLL] * 10)); if (swap_16((int16_t)(fabs(tele_gyro.gyroX))) > swap_16((int16_t)(tele_gyro.maxGyroX))) { tele_gyro.maxGyroX = tele_gyro.gyroX; } if (sensor.tele_gyro[XBUS_TELE_GYRO_PITCH]) tele_gyro.gyroY = swap_16((int16_t)(*sensor.tele_gyro[XBUS_TELE_GYRO_PITCH] * 10)); if (swap_16((int16_t)(fabs(tele_gyro.gyroY))) > swap_16((int16_t)(tele_gyro.maxGyroY))) { tele_gyro.maxGyroY = tele_gyro.gyroY; } if (sensor.tele_gyro[XBUS_TELE_GYRO_YAW]) tele_gyro.gyroZ = swap_16((int16_t)(*sensor.tele_gyro[XBUS_TELE_GYRO_YAW] * 10)); if (swap_16((int16_t)(fabs(tele_gyro.gyroZ))) > swap_16((int16_t)(tele_gyro.maxGyroZ))) { tele_gyro.maxGyroZ = tele_gyro.gyroZ; } memcpy(buffer, &tele_gyro, sizeof(xbus_tele_gyro_t)); break; } case XBUS_MULTICYL_ID: { xbus_multi_cyl_t multicyl = {0}; multicyl.identifier = XBUS_MULTICYL_ID; for (int i = 0; i < 9; i++) { multicyl.temperature[i] = 0xFF; } if (sensor.multicyl[XBUS_MULTICYL_TEMP]) { config_t *config = config_read(); int idx = config->sensor_id_srxl2 - 1; if (idx >= 0 && idx < 9) { float temp_c = *sensor.multicyl[XBUS_MULTICYL_TEMP]; if (temp_c < 30.0f) multicyl.temperature[idx] = 0x00; else if (temp_c > 284.0f) multicyl.temperature[idx] = 0xFE; else multicyl.temperature[idx] = (uint8_t)(temp_c - 30.0f); } } if (sensor.multicyl[XBUS_MULTICYL_VOLTAGE]) { multicyl.batteryV = (uint8_t)((*sensor.multicyl[XBUS_MULTICYL_VOLTAGE] - 3.5f) * 10.0f); } else { multicyl.batteryV = 0xFF; } memcpy(buffer, &multicyl, sizeof(xbus_multi_cyl_t)); break; } } } static void i2c_request_handler(uint8_t address) { debug("\nXBUS (%u) Address: %X Packet: ", uxTaskGetStackHighWaterMark(context.receiver_task_handle), address); uint8_t buffer[16] = {0}; switch (address) { case XBUS_AIRSPEED_ID: if (!sensor.is_enabled[XBUS_AIRSPEED]) break; xbus_format_sensor(address, buffer); i2c_multi_set_write_buffer(buffer); vTaskResume(context.led_task_handle); debug_buffer(buffer, sizeof(xbus_airspeed_t), "0x%X "); break; case XBUS_ALTIMETER_ID: if (!sensor.is_enabled[XBUS_ALTIMETER]) break; break; case XBUS_GPS_LOC_ID: if (!sensor.is_enabled[XBUS_GPS_LOC]) break; xbus_format_sensor(address, buffer); i2c_multi_set_write_buffer(buffer); vTaskResume(context.led_task_handle); debug_buffer(buffer, sizeof(xbus_gps_loc_t), "0x%X "); break; case XBUS_GPS_STAT_ID: if (!sensor.is_enabled[XBUS_GPS_STAT]) break; xbus_format_sensor(address, buffer); i2c_multi_set_write_buffer(buffer); vTaskResume(context.led_task_handle); debug_buffer(buffer, sizeof(xbus_gps_stat_t), "0x%X "); break; case XBUS_ENERGY_ID: if (!sensor.is_enabled[XBUS_ENERGY]) break; xbus_format_sensor(address, buffer); i2c_multi_set_write_buffer(buffer); vTaskResume(context.led_task_handle); debug_buffer(buffer, sizeof(xbus_energy_t), "0x%X "); break; case XBUS_ESC_ID: if (!sensor.is_enabled[XBUS_ESC]) break; xbus_format_sensor(address, buffer); i2c_multi_set_write_buffer(buffer); vTaskResume(context.led_task_handle); debug_buffer(buffer, sizeof(xbus_esc_t), "0x%X "); break; case XBUS_BATTERY_ID: if (!sensor.is_enabled[XBUS_BATTERY]) break; xbus_format_sensor(address, buffer); i2c_multi_set_write_buffer(buffer); vTaskResume(context.led_task_handle); debug_buffer(buffer, sizeof(xbus_battery_t), "0x%X "); break; case XBUS_VARIO_ID: if (!sensor.is_enabled[XBUS_VARIO]) break; xbus_format_sensor(address, buffer); i2c_multi_set_write_buffer(buffer); vTaskResume(context.led_task_handle); debug_buffer(buffer, sizeof(xbus_vario_t), "0x%X "); break; case XBUS_RPMVOLTTEMP_ID: if (!sensor.is_enabled[XBUS_RPMVOLTTEMP]) break; xbus_format_sensor(address, buffer); i2c_multi_set_write_buffer(buffer); vTaskResume(context.led_task_handle); debug_buffer(buffer, sizeof(xbus_rpm_volt_temp_t), "0x%X "); break; case XBUS_FUEL_FLOW_ID: if (!sensor.is_enabled[XBUS_FUEL_FLOW]) break; xbus_format_sensor(address, buffer); i2c_multi_set_write_buffer(buffer); vTaskResume(context.led_task_handle); debug_buffer(buffer, sizeof(xbus_fuel_flow_t), "0x%X "); break; case XBUS_STRU_TELE_DIGITAL_AIR_ID: if (!sensor.is_enabled[XBUS_STRU_TELE_DIGITAL_AIR]) break; xbus_format_sensor(address, buffer); i2c_multi_set_write_buffer(buffer); vTaskResume(context.led_task_handle); debug_buffer(buffer, sizeof(xbus_stru_tele_digital_air_t), "0x%X "); break; case XBUS_TELE_LIPOMON_ID: if (!sensor.is_enabled[XBUS_TELE_LIPOMON]) break; xbus_format_sensor(address, buffer); i2c_multi_set_write_buffer(buffer); vTaskResume(context.led_task_handle); debug_buffer(buffer, sizeof(xbus_tele_lipomon_t), "0x%X "); break; } } void xbus_set_config(void) { config_t *config = config_read(); TaskHandle_t task_handle; float *baro_temp = NULL, *baro_pressure = NULL; if (config->esc_protocol == ESC_PWM) { esc_pwm_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, malloc(sizeof(float))}; xTaskCreate(esc_pwm_task, "esc_pwm_task", STACK_ESC_PWM, (void *)¶meter, 2, &task_handle); sensor.esc[XBUS_ESC_RPM] = parameter.rpm; sensor.is_enabled[XBUS_ESC] = true; i2c_multi_enable_address(XBUS_ESC_ID); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_HW3) { esc_hw3_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, malloc(sizeof(float))}; xTaskCreate(esc_hw3_task, "esc_hw3_task", STACK_ESC_HW3, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; sensor.esc[XBUS_ESC_RPM] = parameter.rpm; sensor.is_enabled[XBUS_ESC] = true; i2c_multi_enable_address(XBUS_ESC_ID); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_HW4) { esc_hw4_parameters_t parameter = {config->rpm_multiplier, config->enable_pwm_out, config->enable_esc_hw4_init_delay, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, config->esc_hw4_voltage_multiplier, config->esc_hw4_current_multiplier, config->esc_hw4_current_thresold, config->esc_hw4_current_max, config->esc_hw4_is_manual_offset, config->esc_hw4_auto_detect, config->esc_hw4_offset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_hw4_task, "esc_hw4_task", STACK_ESC_HW4, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); if (config->enable_pwm_out) { xTaskCreate(pwm_out_task, "pwm_out", STACK_PWM_OUT, (void *)parameter.rpm, 2, &task_handle); context.pwm_out_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } sensor.esc[XBUS_ESC_RPM] = parameter.rpm; sensor.esc[XBUS_ESC_VOLTAGE] = parameter.voltage; sensor.esc[XBUS_ESC_CURRENT] = parameter.current; sensor.esc[XBUS_ESC_TEMPERATURE_FET] = parameter.temperature_fet; sensor.esc[XBUS_ESC_TEMPERATURE_BEC] = parameter.temperature_bec; sensor.is_enabled[XBUS_ESC] = true; i2c_multi_enable_address(XBUS_ESC_ID); // sensor->battery[XBUS_BATTERY_CURRENT1] = parameter.current; // sensor->battery[XBUS_BATTERY_CONSUMPTION1] = parameter.consumption; // sensor.is_enabled[XBUS_BATTERY] = true; // sensor_formatted->battery = malloc(sizeof(xbus_battery_t)); //*sensor_formatted->battery = (xbus_battery_t){XBUS_BATTERY_ID, 0, 0, 0, 0, 0, 0}; // i2c_multi_enable_address(XBUS_BATTERY_ID); } if (config->esc_protocol == ESC_HW5) { esc_hw5_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_hw5_task, "esc_hw5_task", STACK_ESC_HW5, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; ulTaskNotifyTake(pdTRUE, portMAX_DELAY); sensor.esc[XBUS_ESC_RPM] = parameter.rpm; sensor.esc[XBUS_ESC_VOLTAGE] = parameter.voltage; sensor.esc[XBUS_ESC_CURRENT] = parameter.current; sensor.esc[XBUS_ESC_TEMPERATURE_FET] = parameter.temperature_fet; sensor.esc[XBUS_ESC_TEMPERATURE_BEC] = parameter.temperature_bec; sensor.is_enabled[XBUS_ESC] = true; i2c_multi_enable_address(XBUS_ESC_ID); // sensor->battery[XBUS_BATTERY_CURRENT1] = parameter.current; // sensor->battery[XBUS_BATTERY_CONSUMPTION1] = parameter.consumption; // sensor->is_enabled[XBUS_BATTERY] = true; // sensor_formatted->battery = malloc(sizeof(xbus_battery_t)); //*sensor_formatted->battery = (xbus_battery_t){XBUS_BATTERY_ID, 0, 0, 0, 0, 0, 0}; // i2c_multi_enable_address(XBUS_BATTERY_ID); } if (config->esc_protocol == ESC_CASTLE) { esc_castle_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_castle_task, "esc_castle_task", STACK_ESC_CASTLE, (void *)¶meter, 2, &task_handle); sensor.esc[XBUS_ESC_RPM] = parameter.rpm; sensor.esc[XBUS_ESC_VOLTAGE] = parameter.voltage; sensor.esc[XBUS_ESC_CURRENT] = parameter.current; sensor.esc[XBUS_ESC_TEMPERATURE_FET] = parameter.temperature; sensor.esc[XBUS_ESC_CURRENT_BEC] = parameter.current_bec; sensor.esc[XBUS_ESC_VOLTAGE_BEC] = parameter.voltage_bec; sensor.is_enabled[XBUS_ESC] = true; i2c_multi_enable_address(XBUS_ESC_ID); // sensor->battery[XBUS_BATTERY_CURRENT1] = parameter.current; // sensor->battery[XBUS_BATTERY_CONSUMPTION1] = parameter.consumption; // sensor->is_enabled[XBUS_BATTERY] = true; // sensor_formatted->battery = malloc(sizeof(xbus_battery_t)); //*sensor_formatted->battery = (xbus_battery_t){XBUS_BATTERY_ID, 0, 0, 0, 0, 0, 0}; // i2c_multi_enable_address(XBUS_BATTERY_ID); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_KONTRONIK) { esc_kontronik_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_kontronik_task, "esc_kontronik_task", STACK_ESC_KONTRONIK, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; sensor.esc[XBUS_ESC_RPM] = parameter.rpm; sensor.esc[XBUS_ESC_VOLTAGE] = parameter.voltage; sensor.esc[XBUS_ESC_CURRENT] = parameter.current; sensor.esc[XBUS_ESC_TEMPERATURE_FET] = parameter.temperature_fet; sensor.esc[XBUS_ESC_TEMPERATURE_BEC] = parameter.temperature_bec; sensor.esc[XBUS_ESC_CURRENT_BEC] = parameter.current_bec; sensor.esc[XBUS_ESC_VOLTAGE_BEC] = parameter.voltage_bec; sensor.is_enabled[XBUS_ESC] = true; i2c_multi_enable_address(XBUS_ESC_ID); // sensor.battery[XBUS_BATTERY_CURRENT1] = parameter.current; // sensor.battery[XBUS_BATTERY_CONSUMPTION1] = parameter.consumption; // sensor.is_enabled[XBUS_BATTERY] = true; // sensor_formatted->battery = malloc(sizeof(xbus_battery_t)); //*sensor_formatted->battery = (xbus_battery_t){XBUS_BATTERY_ID, 0, 0, 0, 0, 0, 0}; // i2c_multi_enable_address(XBUS_BATTERY_ID); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_APD_F) { esc_apd_f_parameters_t parameter = {config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_apd_f_task, "esc_apd_f_task", STACK_ESC_APD_F, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; sensor.esc[XBUS_ESC_RPM] = parameter.rpm; sensor.esc[XBUS_ESC_VOLTAGE] = parameter.voltage; sensor.esc[XBUS_ESC_CURRENT] = parameter.current; sensor.esc[XBUS_ESC_TEMPERATURE_FET] = parameter.temperature; sensor.is_enabled[XBUS_ESC] = true; i2c_multi_enable_address(XBUS_ESC_ID); // sensor.battery[XBUS_BATTERY_CURRENT1] = parameter.current; // sensor.battery[XBUS_BATTERY_CONSUMPTION1] = parameter.consumption; // sensor.is_enabled[XBUS_BATTERY] = true; // sensor_formatted->battery = malloc(sizeof(xbus_battery_t)); //*sensor_formatted->battery = (xbus_battery_t){XBUS_BATTERY_ID, 0, 0, 0, 0, 0, 0}; // i2c_multi_enable_address(XBUS_BATTERY_ID); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_APD_HV) { esc_apd_hv_parameters_t parameter = { config->rpm_multiplier, config->alpha_rpm, config->alpha_voltage, config->alpha_current, config->alpha_temperature, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(uint8_t))}; xTaskCreate(esc_apd_hv_task, "esc_apd_hv_task", STACK_ESC_APD_HV, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; sensor.esc[XBUS_ESC_RPM] = parameter.rpm; sensor.esc[XBUS_ESC_VOLTAGE] = parameter.voltage; sensor.esc[XBUS_ESC_CURRENT] = parameter.current; sensor.esc[XBUS_ESC_TEMPERATURE_FET] = parameter.temperature; sensor.is_enabled[XBUS_ESC] = true; i2c_multi_enable_address(XBUS_ESC_ID); // sensor->battery[XBUS_BATTERY_CURRENT1] = parameter.current; // sensor->battery[XBUS_BATTERY_CONSUMPTION1] = parameter.consumption; // sensor->is_enabled[XBUS_BATTERY] = true; // sensor_formatted->battery = malloc(sizeof(xbus_battery_t)); //*sensor_formatted->battery = (xbus_battery_t){XBUS_BATTERY_ID, 0, 0, 0, 0, 0, 0}; // i2c_multi_enable_address(XBUS_BATTERY_ID); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_OMP_M4) { esc_omp_m4_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temp_esc = malloc(sizeof(float)); parameter.temp_motor = malloc(sizeof(float)); parameter.cell_voltage = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); parameter.cell_count = malloc(sizeof(uint8_t)); xTaskCreate(esc_omp_m4_task, "esc_omp_m4_task", STACK_ESC_OMP_M4, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; sensor.esc[XBUS_ESC_RPM] = parameter.rpm; sensor.esc[XBUS_ESC_VOLTAGE] = parameter.voltage; sensor.esc[XBUS_ESC_CURRENT] = parameter.current; sensor.esc[XBUS_ESC_TEMPERATURE_FET] = parameter.temp_esc; sensor.is_enabled[XBUS_ESC] = true; i2c_multi_enable_address(XBUS_ESC_ID); // sensor.battery[XBUS_BATTERY_CURRENT1] = parameter.current; // sensor.battery[XBUS_BATTERY_CONSUMPTION1] = parameter.consumption; // sensor.is_enabled[XBUS_BATTERY] = true; // sensor_formatted->battery = malloc(sizeof(xbus_battery_t)); //*sensor_formatted->battery = (xbus_battery_t){XBUS_BATTERY_ID, 0, 0, 0, 0, 0, 0}; // i2c_multi_enable_address(XBUS_BATTERY_ID); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_ZTW) { esc_ztw_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temp_esc = malloc(sizeof(float)); parameter.temp_motor = malloc(sizeof(float)); parameter.cell_voltage = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); parameter.cell_count = malloc(sizeof(uint8_t)); xTaskCreate(esc_ztw_task, "esc_ztw_task", STACK_ESC_ZTW, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; sensor.esc[XBUS_ESC_RPM] = parameter.rpm; sensor.esc[XBUS_ESC_VOLTAGE] = parameter.voltage; sensor.esc[XBUS_ESC_CURRENT] = parameter.current; sensor.esc[XBUS_ESC_TEMPERATURE_FET] = parameter.temp_esc; sensor.is_enabled[XBUS_ESC] = true; i2c_multi_enable_address(XBUS_ESC_ID); // sensor.battery[XBUS_BATTERY_CURRENT1] = parameter.current; // sensor.battery[XBUS_BATTERY_CONSUMPTION1] = parameter.consumption; // sensor.is_enabled[XBUS_BATTERY] = true; // sensor_formatted->battery = malloc(sizeof(xbus_battery_t)); //*sensor_formatted->battery = (xbus_battery_t){XBUS_BATTERY_ID, 0, 0, 0, 0, 0, 0}; // i2c_multi_enable_address(XBUS_BATTERY_ID); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->esc_protocol == ESC_OPENYGE) { esc_openyge_parameters_t parameter; parameter.rpm_multiplier = config->rpm_multiplier; parameter.pwm_out = config->enable_pwm_out; parameter.alpha_rpm = config->alpha_rpm; parameter.alpha_voltage = config->alpha_voltage; parameter.alpha_current = config->alpha_current; parameter.alpha_temperature = config->alpha_temperature; parameter.rpm = malloc(sizeof(float)); parameter.voltage = malloc(sizeof(float)); parameter.current = malloc(sizeof(float)); parameter.temperature_fet = malloc(sizeof(float)); parameter.temperature_bec = malloc(sizeof(float)); parameter.cell_voltage = malloc(sizeof(float)); parameter.consumption = malloc(sizeof(float)); parameter.voltage_bec = malloc(sizeof(float)); parameter.current_bec = malloc(sizeof(float)); parameter.throttle = malloc(sizeof(float)); parameter.pwm_percent = malloc(sizeof(float)); parameter.cell_count = malloc(sizeof(uint8_t)); xTaskCreate(esc_openyge_task, "esc_openyge_task", STACK_ESC_OPENYGE, (void *)¶meter, 2, &task_handle); context.uart1_notify_task_handle = task_handle; sensor.esc[XBUS_ESC_RPM] = parameter.rpm; sensor.esc[XBUS_ESC_VOLTAGE] = parameter.voltage; sensor.esc[XBUS_ESC_CURRENT] = parameter.current; sensor.esc[XBUS_ESC_TEMPERATURE_FET] = parameter.temperature_fet; sensor.is_enabled[XBUS_ESC] = true; i2c_multi_enable_address(XBUS_ESC_ID); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_gps) { gps_parameters_t parameter; parameter.protocol = config->gps_protocol; parameter.baudrate = config->gps_baudrate; parameter.rate = config->gps_rate; parameter.lat = malloc(sizeof(float)); parameter.lon = malloc(sizeof(float)); parameter.alt = malloc(sizeof(float)); parameter.spd = malloc(sizeof(float)); parameter.cog = malloc(sizeof(float)); parameter.hdop = malloc(sizeof(float)); parameter.sat = malloc(sizeof(float)); parameter.time = malloc(sizeof(float)); parameter.date = malloc(sizeof(float)); parameter.vspeed = malloc(sizeof(float)); parameter.dist = malloc(sizeof(float)); parameter.spd_kmh = malloc(sizeof(float)); parameter.fix = malloc(sizeof(float)); parameter.vdop = malloc(sizeof(float)); parameter.speed_acc = malloc(sizeof(float)); parameter.h_acc = malloc(sizeof(float)); parameter.v_acc = malloc(sizeof(float)); parameter.track_acc = malloc(sizeof(float)); parameter.n_vel = malloc(sizeof(float)); parameter.e_vel = malloc(sizeof(float)); parameter.v_vel = malloc(sizeof(float)); parameter.alt_elipsiod = malloc(sizeof(float)); parameter.pdop = malloc(sizeof(float)); parameter.fix_type = malloc(sizeof(uint8_t)); parameter.home_set = malloc(sizeof(uint8_t)); parameter.alt_home = malloc(sizeof(float)); xTaskCreate(gps_task, "gps_task", STACK_GPS, (void *)¶meter, 2, &task_handle); context.uart_pio_notify_task_handle = task_handle; sensor.gps_loc = malloc(sizeof(xbus_sensor_gps_loc_t)); sensor.gps_loc->altitude_low = parameter.alt; sensor.gps_loc->latitude = parameter.lat; sensor.gps_loc->longitude = parameter.lon; sensor.gps_loc->course = parameter.cog; sensor.gps_loc->hdop = parameter.hdop; sensor.gps_loc->fix_type = parameter.fix_type; sensor.gps_loc->home_set = parameter.home_set; sensor.gps_stat[XBUS_GPS_STAT_SPEED] = parameter.spd; sensor.gps_stat[XBUS_GPS_STAT_TIME] = parameter.time; sensor.gps_stat[XBUS_GPS_STAT_SATS] = parameter.sat; sensor.gps_stat[XBUS_GPS_STAT_ALTITUDE] = parameter.alt; sensor.is_enabled[XBUS_GPS_LOC] = true; sensor.is_enabled[XBUS_GPS_STAT] = true; i2c_multi_enable_address(XBUS_GPS_LOC_ID); i2c_multi_enable_address(XBUS_GPS_STAT_ID); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_voltage) { voltage_parameters_t parameter = {0, config->analog_rate, config->alpha_voltage, config->analog_voltage_multiplier, malloc(sizeof(float))}; xTaskCreate(voltage_task, "voltage_task", STACK_VOLTAGE, (void *)¶meter, 2, &task_handle); if (!config->xbus_use_alternative_volt_temp) { sensor.rpm_volt_temp[XBUS_RPMVOLTTEMP_VOLT] = parameter.voltage; sensor.is_enabled[XBUS_RPMVOLTTEMP] = true; i2c_multi_enable_address(XBUS_RPMVOLTTEMP_ID); } else { sensor.multicyl[XBUS_MULTICYL_VOLTAGE] = parameter.voltage; sensor.is_enabled[XBUS_MULTICYL] = true; i2c_multi_enable_address(XBUS_MULTICYL_ID); } ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_current) { current_parameters_t parameter = {1, config->analog_rate, config->alpha_current, config->analog_current_multiplier, config->analog_current_offset, config->analog_current_autoffset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(current_task, "current_task", STACK_CURRENT, (void *)¶meter, 2, &task_handle); sensor.energy[XBUS_ENERGY_CURRENT1] = parameter.current; sensor.energy[XBUS_ENERGY_CONSUMPTION1] = parameter.consumption; if (!sensor.is_enabled[XBUS_ENERGY]) { sensor.is_enabled[XBUS_ENERGY] = true; } i2c_multi_enable_address(XBUS_ENERGY_ID); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_ntc) { ntc_parameters_t parameter = {2, config->analog_rate, config->ntc_offset, config->alpha_temperature, malloc(sizeof(float))}; xTaskCreate(ntc_task, "ntc_task", STACK_NTC, (void *)¶meter, 2, &task_handle); if (!config->xbus_use_alternative_volt_temp) { sensor.rpm_volt_temp[XBUS_RPMVOLTTEMP_TEMP] = parameter.ntc; sensor.is_enabled[XBUS_RPMVOLTTEMP] = true; i2c_multi_enable_address(XBUS_RPMVOLTTEMP_ID); } else { sensor.multicyl[XBUS_MULTICYL_TEMP] = parameter.ntc; sensor.is_enabled[XBUS_MULTICYL] = true; i2c_multi_enable_address(XBUS_MULTICYL_ID); } ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_BMP280) { bmp280_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, 0, config->bmp280_filter, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(bmp280_task, "bmp280_task", STACK_BMP280, (void *)¶meter, 2, &task_handle); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } add_alarm_in_ms(250, interval_250_callback, NULL, false); add_alarm_in_ms(500, interval_500_callback, NULL, false); add_alarm_in_ms(1000, interval_1000_callback, NULL, false); add_alarm_in_ms(1500, interval_1500_callback, NULL, false); add_alarm_in_ms(2000, interval_2000_callback, NULL, false); add_alarm_in_ms(3000, interval_3000_callback, NULL, false); sensor.vario[XBUS_VARIO_ALTITUDE] = parameter.altitude; sensor.is_enabled[XBUS_VARIO] = true; i2c_multi_enable_address(XBUS_VARIO_ID); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_MS5611) { ms5611_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, 0, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(ms5611_task, "ms5611_task", STACK_MS5611, (void *)¶meter, 2, &task_handle); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } add_alarm_in_ms(250, interval_250_callback, NULL, false); add_alarm_in_ms(500, interval_500_callback, NULL, false); add_alarm_in_ms(1000, interval_1000_callback, NULL, false); add_alarm_in_ms(1500, interval_1500_callback, NULL, false); add_alarm_in_ms(2000, interval_2000_callback, NULL, false); add_alarm_in_ms(3000, interval_3000_callback, NULL, false); sensor.vario[XBUS_VARIO_ALTITUDE] = parameter.altitude; sensor.is_enabled[XBUS_VARIO] = true; i2c_multi_enable_address(XBUS_VARIO_ID); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->i2c_module == I2C_BMP180) { bmp180_parameters_t parameter = {config->alpha_vario, config->vario_auto_offset, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(bmp180_task, "bmp180_task", STACK_BMP180, (void *)¶meter, 2, &task_handle); add_alarm_in_ms(250, interval_250_callback, NULL, false); add_alarm_in_ms(500, interval_500_callback, NULL, false); add_alarm_in_ms(1000, interval_1000_callback, NULL, false); add_alarm_in_ms(1500, interval_1500_callback, NULL, false); add_alarm_in_ms(2000, interval_2000_callback, NULL, false); add_alarm_in_ms(3000, interval_3000_callback, NULL, false); if (config->enable_analog_airspeed) { baro_temp = parameter.temperature; baro_pressure = parameter.pressure; } sensor.vario[XBUS_VARIO_ALTITUDE] = parameter.altitude; sensor.is_enabled[XBUS_VARIO] = true; i2c_multi_enable_address(XBUS_VARIO_ID); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_analog_airspeed) { airspeed_parameters_t parameter = {3, config->analog_rate, config->alpha_airspeed, (float)config->airspeed_offset / 1000, (float)config->airspeed_vcc / 100, baro_temp, baro_pressure, malloc(sizeof(float))}; xTaskCreate(airspeed_task, "airspeed_task", STACK_AIRSPEED, (void *)¶meter, 2, &task_handle); sensor.airspeed[XBUS_AIRSPEED_AIRSPEED] = parameter.airspeed; sensor.is_enabled[XBUS_AIRSPEED] = true; i2c_multi_enable_address(XBUS_AIRSPEED_ID); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_fuel_flow) { fuel_meter_parameters_t parameter = {config->fuel_flow_ml_per_pulse, malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(fuel_meter_task, "fuel_meter_task", STACK_FUEL_METER, (void *)¶meter, 2, &task_handle); sensor.fuel_flow[XBUS_FUEL_FLOW_RATE] = parameter.consumption_instant; sensor.fuel_flow[XBUS_FUEL_FLOW_CONSUMED] = parameter.consumption_total; sensor.is_enabled[XBUS_FUEL_FLOW] = true; i2c_multi_enable_address(XBUS_FUEL_FLOW_ID); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_fuel_pressure) { xgzp68xxd_parameters_t parameter = {config->xgzp68xxd_k, malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(xgzp68xxd_task, "fuel_pressure_task", STACK_FUEL_PRESSURE, (void *)¶meter, 2, &task_handle); sensor.stru_tele_digital_air[XBUS_DIGITAL_AIR_FUEL_PRESSURE] = parameter.pressure; sensor.is_enabled[XBUS_STRU_TELE_DIGITAL_AIR] = true; i2c_multi_enable_address(XBUS_STRU_TELE_DIGITAL_AIR_ID); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->xbus_clock_stretch) { gpio_set_dir(CLOCK_STRETCH_GPIO, true); gpio_put(CLOCK_STRETCH_GPIO, false); } if (config->enable_gyro) { mpu6050_parameters_t parameter = {1, 0, config->mpu6050_acc_scale, config->mpu6050_gyro_scale, config->mpu6050_gyro_weighting, config->mpu6050_filter, malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float)), malloc(sizeof(float))}; xTaskCreate(mpu6050_task, "mpu6050_task", STACK_MPU6050, (void *)¶meter, 2, &task_handle); sensor.is_enabled[XBUS_TELE_G_METER] = true; sensor.tele_g_meter[XBUS_TELE_G_METER_X] = parameter.acc_x; sensor.tele_g_meter[XBUS_TELE_G_METER_Y] = parameter.acc_y; sensor.tele_g_meter[XBUS_TELE_G_METER_Z] = parameter.acc_z; i2c_multi_enable_address(XBUS_TELE_G_METER_ID); sensor.is_enabled[XBUS_TELE_GYRO] = true; sensor.tele_gyro[XBUS_TELE_GYRO_PITCH] = parameter.pitch; sensor.tele_gyro[XBUS_TELE_GYRO_ROLL] = parameter.roll; sensor.tele_gyro[XBUS_TELE_GYRO_YAW] = parameter.yaw; i2c_multi_enable_address(XBUS_TELE_GYRO_ID); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); } if (config->enable_lipo) { float *cell_prev = 0; sensor.is_enabled[XBUS_TELE_LIPOMON] = true; i2c_multi_enable_address(XBUS_TELE_LIPOMON_ID); if (config->lipo_cells > 0) { ina3221_parameters_t parameter = { .i2c_address = 0x40, .filter = config->ina3221_filter, .cell_count = MIN(config->lipo_cells, 3), .cell[0] = malloc(sizeof(float)), .cell[1] = malloc(sizeof(float)), .cell[2] = malloc(sizeof(float)), .cell_prev = malloc(sizeof(float)), }; *parameter.cell_prev = 0; cell_prev = parameter.cell[2]; xTaskCreate(ina3221_task, "ina3221_task", STACK_INA3221, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); for (uint8_t i = 0; i < parameter.cell_count; i++) { sensor.tele_lipomon[XBUS_TELE_LIPOMON_CELL1 + i] = parameter.cell[i]; } for (uint8_t i = parameter.cell_count; i < 3; i++) { *sensor.tele_lipomon[XBUS_TELE_LIPOMON_CELL1 + i] = 0x7FFF; } } if (config->lipo_cells > 3) { ina3221_parameters_t parameter = { .i2c_address = 0x41, .filter = config->ina3221_filter, .cell_count = MIN(config->lipo_cells - 3, 3), .cell[0] = malloc(sizeof(float)), .cell[1] = malloc(sizeof(float)), .cell[2] = malloc(sizeof(float)), .cell_prev = malloc(sizeof(float)), }; parameter.cell_prev = cell_prev; cell_prev = parameter.cell[2]; xTaskCreate(ina3221_task, "ina3221_task", STACK_INA3221, (void *)¶meter, 2, &task_handle); ulTaskNotifyTake(pdTRUE, portMAX_DELAY); for (uint8_t i = 0; i < parameter.cell_count; i++) { sensor.tele_lipomon[XBUS_TELE_LIPOMON_CELL4 + i] = parameter.cell[i]; } for (uint8_t i = parameter.cell_count; i < 3; i++) { *sensor.tele_lipomon[XBUS_TELE_LIPOMON_CELL4 + i] = 0x7FFF; } } } } static uint8_t bcd8(float value, uint8_t precision) { char buf[10] = {0}; uint8_t output = 0; for (int i = 0; i < precision; i++) value = value * 10; sprintf(buf, "%02i", (uint8_t)value); for (int i = 0; i < 2; i++) output |= (buf[i] - 48) << ((1 - i) * 4); return output; } static uint16_t bcd16(float value, uint8_t precision) { char buf[10] = {0}; uint16_t output = 0; for (int i = 0; i < precision; i++) value = value * 10; sprintf(buf, "%04i", (uint16_t)value); for (int i = 0; i < 4; i++) output |= (uint16_t)(buf[i] - 48) << ((3 - i) * 4); return output; } static uint32_t bcd32(float value, uint8_t precision) { char buf[10] = {0}; uint32_t output = 0; for (int i = 0; i < precision; i++) value = value * 10; sprintf(buf, "%08li", (uint32_t)value); for (int i = 0; i < 8; i++) output |= (uint32_t)(buf[i] - 48) << ((7 - i) * 4); return output; } static int64_t interval_250_callback(alarm_id_t id, void *parameters) { static float prev = 0; delta_0250ms = swap_16((int16_t)(round(*sensor.vario[XBUS_VARIO_ALTITUDE] - prev) * 10)); prev = *sensor.vario[XBUS_VARIO_ALTITUDE]; return 250000L; } static int64_t interval_500_callback(alarm_id_t id, void *parameters) { static float prev = 0; delta_0500ms = swap_16((int16_t)(round(*sensor.vario[XBUS_VARIO_ALTITUDE] - prev) * 10)); prev = *sensor.vario[XBUS_VARIO_ALTITUDE]; return 500000L; } static int64_t interval_1000_callback(alarm_id_t id, void *parameters) { static float prev = 0; delta_1000ms = swap_16((int16_t)round(*sensor.vario[XBUS_VARIO_ALTITUDE] - prev)); prev = *sensor.vario[XBUS_VARIO_ALTITUDE]; return 1000000L; } static int64_t interval_1500_callback(alarm_id_t id, void *parameters) { static float prev = 0; delta_1500ms = swap_16((int16_t)round(*sensor.vario[XBUS_VARIO_ALTITUDE] - prev)); prev = *sensor.vario[XBUS_VARIO_ALTITUDE]; return 1500000L; } static int64_t interval_2000_callback(alarm_id_t id, void *parameters) { static float prev = 0; delta_2000ms = swap_16((int16_t)round(*sensor.vario[XBUS_VARIO_ALTITUDE] - prev)); prev = *sensor.vario[XBUS_VARIO_ALTITUDE]; return 2000000L; } static int64_t interval_3000_callback(alarm_id_t id, void *parameters) { static float prev = 0; delta_3000ms = swap_16((int16_t)round(*sensor.vario[XBUS_VARIO_ALTITUDE] - prev)); prev = *sensor.vario[XBUS_VARIO_ALTITUDE]; return 3000000L; } ================================================ FILE: board/project/protocol/xbus.h ================================================ #ifndef XBUS_H #define XBUS_H #include "common.h" #define XBUS_AIRSPEED_ID 0x11 #define XBUS_ALTIMETER_ID 0x12 #define XBUS_GPS_LOC_ID 0x16 #define XBUS_GPS_STAT_ID 0x17 #define XBUS_ENERGY_ID 0x18 #define XBUS_ESC_ID 0x20 #define XBUS_BATTERY_ID 0x34 #define XBUS_VARIO_ID 0X40 #define XBUS_RPMVOLTTEMP_ID 0x7E #define XBUS_FUEL_FLOW_ID 0x22 #define XBUS_STRU_TELE_DIGITAL_AIR_ID 0x36 #define XBUS_TELE_LIPOMON_ID 0x3A // 6S Cell Monitor #define XBUS_TELE_G_METER_ID 0x3B // 3-axis accelerometer #define XBUS_TELE_GYRO_ID 0x1A // 3D gyro sensor #define XBUS_MULTICYL_ID 0x59 // Multi-cylinder sensor #define XBUS_GPS_INFO_FLAGS_IS_NORTH_BIT 0 #define XBUS_GPS_INFO_FLAGS_IS_EAST_BIT 1 #define XBUS_GPS_INFO_FLAGS_LONG_GREATER_99_BIT 2 #define XBUS_GPS_INFO_FLAGS_NEGATIVE_ALT_BIT 7 typedef enum xbus_sensors_t { XBUS_AIRSPEED, XBUS_ALTIMETER, XBUS_GPS_LOC, XBUS_GPS_STAT, XBUS_ESC, XBUS_BATTERY, XBUS_VARIO, XBUS_RPMVOLTTEMP, XBUS_ENERGY, XBUS_FUEL_FLOW, XBUS_STRU_TELE_DIGITAL_AIR, XBUS_TELE_LIPOMON, XBUS_TELE_G_METER, XBUS_TELE_GYRO, XBUS_MULTICYL, XBUS_SENSORS_COUNT } xbus_sensors_t; typedef enum xbus_airspeed_enum_t { XBUS_AIRSPEED_AIRSPEED, XBUS_AIRSPEED_COUNT } xbus_airspeed_enum_t; typedef enum xbus_altitude_enum_t { XBUS_ALTITUDE, XBUS_ALTITUDE_COUNT } xbus_altitude_enum_t; typedef enum xbus_gps_loc_enum_t { XBUS_GPS_LOC_ALTITUDE, XBUS_GPS_LOC_LATITUDE, XBUS_GPS_LOC_LONGITUDE, XBUS_GPS_LOC_COURSE, XBUS_GPS_LOC_HDOP, XBUS_GPS_LOC_COUNT } xbus_gps_loc_enum_t; typedef enum xbus_gps_stat_enum_t { XBUS_GPS_STAT_SPEED, XBUS_GPS_STAT_TIME, XBUS_GPS_STAT_SATS, XBUS_GPS_STAT_ALTITUDE, XBUS_GPS_STAT_FIX_TYPE, XBUS_GPS_STAT_HOME_SET, XBUS_GPS_STAT_COUNT } xbus_gps_stat_enum_t; typedef enum xbus_energy_enum_t { XBUS_ENERGY_CURRENT1, XBUS_ENERGY_CONSUMPTION1, XBUS_ENERGY_VOLTAGE1, XBUS_ENERGY_CURRENT2, XBUS_ENERGY_CONSUMPTION2, XBUS_ENERGY_VOLTAGE2, XBUS_ENERGY_COUNT } xbus_energy_enum_t; typedef enum xbus_esc_enum_t { XBUS_ESC_RPM, XBUS_ESC_VOLTAGE, XBUS_ESC_CURRENT, XBUS_ESC_TEMPERATURE_FET, XBUS_ESC_TEMPERATURE_BEC, XBUS_ESC_VOLTAGE_BEC, XBUS_ESC_CURRENT_BEC, XBUS_ESC_COUNT } xbus_esc_enum_t; typedef enum xbus_battery_enum_t { XBUS_BATTERY_CURRENT1, XBUS_BATTERY_CONSUMPTION1, XBUS_BATTERY_TEMP1, XBUS_BATTERY_CURRENT2, XBUS_BATTERY_CONSUMPTION2, XBUS_BATTERY_TEMP2, XBUS_BATTERY_COUNT } xbus_battery_enum_t; typedef enum xbus_vario_enum_t { XBUS_VARIO_ALTITUDE, XBUS_VARIO_COUNT } xbus_vario_enum_t; typedef enum xbus_rpm_volt_temp_enum_t { XBUS_RPMVOLTTEMP_MS, XBUS_RPMVOLTTEMP_VOLT, XBUS_RPMVOLTTEMP_TEMP, XBUS_RPMVOLTTEMP_COUNT } xbus_rpm_volt_temp_enum_t; typedef enum xbus_fuel_flow_enum_t { XBUS_FUEL_FLOW_CONSUMED, XBUS_FUEL_FLOW_RATE, XBUS_FUEL_FLOW_COUNT } xbus_fuel_flow_enum_t; typedef enum xbus_stru_tele_digital_air_enum_t { XBUS_DIGITAL_AIR_FUEL_PRESSURE, XBUS_DIGITAL_AIR_COUNT } xbus_stru_tele_digital_air_enum_t; typedef enum xbus_tele_lipomon_enum_t { XBUS_TELE_LIPOMON_CELL1, XBUS_TELE_LIPOMON_CELL2, XBUS_TELE_LIPOMON_CELL3, XBUS_TELE_LIPOMON_CELL4, XBUS_TELE_LIPOMON_CELL5, XBUS_TELE_LIPOMON_CELL6, XBUS_TELE_LIPOMON_COUNT } xbus_tele_lipomon_enum_t; typedef enum xbus_tele_g_meter_enum_t { XBUS_TELE_G_METER_X, XBUS_TELE_G_METER_Y, XBUS_TELE_G_METER_Z, XBUS_TELE_G_METER_MAX_X, XBUS_TELE_G_METER_MAX_Y, XBUS_TELE_G_METER_MAX_Z, XBUS_TELE_G_METER_MIN_Z, XBUS_TELE_G_METER_COUNT } xbus_tele_g_meter_enum_t; typedef enum xbus_tele_gyro_enum_t { XBUS_TELE_GYRO_PITCH, XBUS_TELE_GYRO_ROLL, XBUS_TELE_GYRO_YAW, XBUS_TELE_GYRO_COUNT } xbus_tele_gyro_enum_t; typedef enum xbus_multicyl_enum_t { XBUS_MULTICYL_TEMP, XBUS_MULTICYL_VOLTAGE, XBUS_MULTICYL_COUNT } xbus_multicyl_enum_t; typedef struct xbus_airspeed_t { uint8_t identifier; // Source device 0x11 uint8_t s_id; // Secondary ID uint16_t airspeed; // 1 km/h increments uint16_t max_airspeed; // 1 km/h increments } xbus_airspeed_t; typedef struct xbus_altitude_t { uint8_t identifier; // Source device 0x12 uint8_t s_id; // Secondary ID int16_t altitude; // .1m increments int16_t max_altitude; // .1m increments } xbus_altitude_t; typedef struct xbus_gps_loc_t { uint8_t identifier; // Source device 0x16 uint8_t s_id; // Secondary ID uint16_t altitude_low; // BCD, meters, format 3.1 (Low bits of alt) uint32_t latitude; // BCD, format 4.4, // Degrees * 100 + minutes, < 100 degrees 1234.1234 uint32_t longitude; // BCD, format 4.4, // Degrees * 100 + minutes, flag --> > 99deg uint16_t course; // BCD, 3.1 uint8_t hdop; // BCD, format 1.1 uint8_t gps_flags; // see definitions below } xbus_gps_loc_t; typedef struct xbus_gps_stat_t { uint8_t identifier; // Source device 0x17 uint8_t s_id; // Secondary ID uint16_t speed; // BCD, knots, format 3.1 uint32_t utc; // BCD, format HH:MM:SS.SS, format 6.2 uint8_t num_sats; // BCD, 0-99 uint8_t altitude_high; // BCD, meters, format 2.0 (High bits alt) } xbus_gps_stat_t; typedef struct xbus_energy_t { uint8_t identifier; // Source device 0x18 uint8_t s_id; // Secondary ID int16_t current_a; // Instantaneous current, 0.01A (0-327.7A) int16_t charge_used_a; // Integrated mAh used, 0.1mAh (0-3276.6mAh) uint16_t volts_a; // Voltage, 0.01VC (0-16.00V) int16_t current_b; // Instantaneous current, 0.01A (0-327.7A) int16_t charge_used_b; // Integrated mAh used, 0.1mAh (0-3276.6mAh) uint16_t volts_b; // Voltage, 0.01VC (0-16.00V) } xbus_energy_t; typedef struct xbus_esc_t { uint8_t identifier; // Source device 0x20 uint8_t s_id; // Secondary ID uint16_t rpm; // RPM, 10RPM (0-655340 RPM).0xFFFF --> uint16_t volts_input; // Volts, 0.01v (0-655.34V).0xFFFF --> uint16_t temp_fet; // Temperature, 0.1C (0-999.8C)0xFFFF --> uint16_t current_motor; // Current, 10mA (0-655.34A).0xFFFF --> uint16_t temp_bec; // Temperature, 0.1C (0-999.8C)0x7FFF --> uint8_t current_bec; // BEC Current, 100mA (0-25.4A). 0xFF ----> uint8_t voltage_bec; // BEC Volts, 0.05V (0-12.70V). 0xFF ----> uint8_t throttle; // 0.5% (0-127%). 0xFF ----> uint8_t power_out; // Power Output, 0.5% (0-127%). 0xFF ----> } xbus_esc_t; typedef struct xbus_battery_t { uint8_t identifier; // Source device 0x34 uint8_t s_id; // Secondary ID int16_t current_a; // Instantaneous current, 0.1A (0-3276.8A) int16_t charge_used_a; // Integrated mAh used, 1mAh (0-32.766Ah) uint16_t temp_a; // Temperature, 0.1C (0-150.0C, // 0x7FFF indicates not populated) int16_t current_b; // Instantaneous current, 0.1A (0-6553.4A) int16_t charge_used_b; // Integrated mAh used, 1mAh (0-65.534Ah) uint16_t temp_b; // Temperature, 0.1C (0-150.0C,// 0x7FFF indicates not populated) } xbus_battery_t; typedef struct xbus_vario_t { uint8_t identifier; // Source device 0x40 uint8_t s_id; // Secondary ID int16_t altitude; // .1m increments int16_t delta_0250ms, // delta last 250ms, 0.1m/s increments delta_0500ms, // delta last 500ms, 0.1m/s increments delta_1000ms, // delta last 1.0 seconds delta_1500ms, // delta last 1.5 seconds delta_2000ms, // delta last 2.0 seconds delta_3000ms; // delta last 3.0 seconds } xbus_vario_t; typedef struct xbus_rpm_volt_temp_t { uint8_t identifier; // Source device 0x7E uint8_t s_id; uint16_t microseconds; // microseconds between pulse leading edges uint16_t volts; // 0.01V increments int16_t temperature; // degrees F } xbus_rpm_volt_temp_t; typedef struct xbus_fuel_flow_t { uint8_t identifier; // Source device = 0x22 uint8_t s_id; // Secondary ID uint16_t fuel_consumed_A; // Integrated fuel consumption, 0.1mL uint16_t flow_rate_A; // Instantaneous consumption, 0.01mL/min uint16_t temp_A; // Temperature, 0.1C (0-655.34C) uint16_t fuel_consumed_B; // Integrated fuel consumption, 0.1mL uint16_t flow_rate_B; // Instantaneous consumption, 0.01mL/min uint16_t temp_B; // Temperature, 0.1C (0-655.34C) uint16_t spare; // Not used } xbus_fuel_flow_t; typedef struct xbus_stru_tele_digital_air_t { uint8_t identifier; // Source device = 0x36 uint8_t sID; // Secondary ID uint16_t digital; // Digital inputs (bit per input) uint16_t pressure; // Tank pressure, 0.1PSI (0-6553.4PSI) } xbus_stru_tele_digital_air_t; typedef struct xbus_tele_lipomon_t { uint8_t identifier; // Source device = 0x3A uint8_t sID; // Secondary ID uint16_t cell[6]; // Voltage across cell 1, .01V steps 0x7FFF --> cell not present uint8_t temp; // Temperature, 0.1C (0-655.34C) } xbus_tele_lipomon_t; typedef struct xbus_tele_g_meter_t { uint8_t identifier; // Source device = 0x14 uint8_t sID; // Secondary ID int16_t GForceX; // force is reported as .01G increments int16_t GForceY; // Range = +/-4000 (+/- 40G) in Pro model int16_t GForceZ; // Range = +/-800 (+/- 8G) in Standard model int16_t maxGForceX; // abs(max G X-axis) FORE/AFT int16_t maxGForceY; // abs (max G Y-axis) LEFT/RIGHT int16_t maxGForceZ; // max G Z-axis WING SPAR LOAD int16_t minGForceZ; // min G Z-axis WING SPAR LOAD } xbus_tele_g_meter_t; typedef struct xbus_tele_gyro_t { uint8_t identifier; // Source device = 0x1A uint8_t sID; // Secondary ID int16_t gyroX; // Units are 0.1 deg/sec - Rate is about the X Axis which is defined out the nose of the vehicle. int16_t gyroY; // Rate is about the Y Axis which is define out the right wing of the vehicle. int16_t gyroZ; // Rate is about the Z axis which is defined down from the vehicle. int16_t maxGyroX; // Max rates (absolute value) int16_t maxGyroY; int16_t maxGyroZ; } xbus_tele_gyro_t; typedef struct xbus_multi_cyl_t { uint8_t identifier; // Source device = TELE_DEVICE_MULTICYLINDER = 0x7F uint8_t sID; // Secondary ID uint8_t temperature[9]; // Temperature, 1C increments, Offset = 30C, 0xFF = NO DATA // 0x00 = 30C (86F) // 0x01 = 31C ... (88F) // 0xFE = 284C (543F) // 0xFF = NO SENSOR ATTACHED. Note that sensors must be installed cylinder 1-9 in sequence! uint8_t throttlePct; // Throttle percent (0-100% typical, 0xFF = NO DATA) uint16_t RPM; // 4 RPM increments, Offset = 400RPM, range 404-16776. // 0x000 = 0 RPM // 0x001 = 404 RPM // 0x002 = 408 RPM // 0xFFE = 16776 RPM // 0xFFF = NO SENSOR ATTACHED // NOTE: HI NYBBLE RESERVED, set to 0xF to mark "NO DATA" for now uint8_t batteryV; // Voltage, 0.1V increments, Offset = 3.5V, 0xFF = NO DATA // 0x00 = 3.5V // 0x01 = 3.6V // 0xFE = 28.9V // 0xFF = NO SENSOR ATTACHED uint8_t spare; // 0xFF --> no data } xbus_multi_cyl_t; typedef struct xbus_sensor_gps_loc_t { float *altitude_low; float *latitude; float *longitude; float *course; float *hdop; uint8_t *fix_type; uint8_t *home_set; } xbus_sensor_gps_loc_t; typedef struct xbus_sensor_t { bool is_enabled[XBUS_SENSORS_COUNT]; float *airspeed[XBUS_AIRSPEED_COUNT]; float *altimeter[XBUS_ALTITUDE_COUNT]; xbus_sensor_gps_loc_t *gps_loc; float *gps_stat[XBUS_GPS_STAT_COUNT]; float *esc[XBUS_ESC_COUNT]; float *battery[XBUS_BATTERY_COUNT]; float *vario[XBUS_VARIO_COUNT]; float *rpm_volt_temp[XBUS_RPMVOLTTEMP_COUNT]; float *energy[XBUS_ENERGY_COUNT]; float *fuel_flow[XBUS_FUEL_FLOW_COUNT]; float *stru_tele_digital_air[XBUS_DIGITAL_AIR_COUNT]; float *tele_lipomon[XBUS_TELE_LIPOMON_COUNT]; float *tele_g_meter[XBUS_TELE_G_METER_COUNT]; float *tele_gyro[XBUS_TELE_GYRO_COUNT]; float *multicyl[XBUS_MULTICYL_COUNT]; } xbus_sensor_t; extern context_t context; extern xbus_sensor_t sensor; void xbus_task(void *parameters); void xbus_format_sensor(uint8_t address, uint8_t *buffer); void xbus_set_config(void); #ifdef SIM_RX void xbus_i2c_handler(uint8_t address); #endif #endif ================================================ FILE: board/project/sensor/CMakeLists.txt ================================================ target_sources(${PROJECT_NAME} PRIVATE airspeed.c esc_hw3.c voltage.c esc_hw4.c esc_kontronik.c esc_apd_f.c esc_apd_hv.c gps.c vspeed.c distance.c esc_pwm.c bmp280.c ms5611.c bmp180.c cell_count.c auto_offset.c esc_hw5.c esc_castle.c pwm_out.c ntc.c current.c fuel_meter.c xgzp68xxd.c gpio.c smart_esc.c esc_omp_m4.c esc_ztw.c esc_openyge.c mpu6050.c ina3221.c ) ================================================ FILE: board/project/sensor/airspeed.c ================================================ #include "airspeed.h" #include #include #include "hardware/adc.h" #include "pico/stdlib.h" /* If there is not barometer sensor installed, values used to calculate air density are: Nominal pressure at sea level 101325 hPa. Defalut temperature value 20ºC */ #define KNOT_TO_KMH 1.94384 #define AIR_CONSTANT_R 287.05 // J/(kg.K) void airspeed_task(void *parameters) { airspeed_parameters_t parameter = *(airspeed_parameters_t *)parameters; adc_init(); adc_gpio_init(parameter.adc_num + 26); // gpio_pull_down(parameter.adc_num + 26); *parameter.airspeed = 0; xTaskNotifyGive(context.receiver_task_handle); float temperature, pressure, delta_pressure, air_density, airspeed; static float voltage = 0; while (1) { if (!parameter.temperature) temperature = 20; else temperature = *parameter.temperature; // ºC if (!parameter.pressure) pressure = 101325; else pressure = *parameter.pressure; // Pa voltage = voltage_read(parameter.adc_num); if (pressure < 1000) pressure = 101325; air_density = pressure / (AIR_CONSTANT_R * (temperature + 273.15)); // kg/m3 delta_pressure = ((voltage + parameter.offset) / parameter.vcc - 0.5) * 5 * 1000; // Pa // Formula: speed = sqrt(2*P/air_dens) -> Units: P (Pa=N/m2), air_dens (kg/m3), N (kg*m/s2) -> speed = // sqrt(kg*m/s2/m2*m3/kg) = sqrt(m2/s2) = m/s if (delta_pressure < 0) airspeed = -1 * sqrt(-2 * delta_pressure / air_density); else airspeed = sqrt(2 * delta_pressure / air_density); airspeed *= 3.6; // m/s to km/h *parameter.airspeed = get_average(parameter.alpha, *parameter.airspeed, airspeed); #ifdef SIM_SENSORS *parameter.airspeed = 123.34; #endif debug("\nAirspeed (%u): %.2f (Vcc %.2f Offset %.0f Vout %.3f Press %.0f Temp %.2f AirDens %.2f dPress %.0f)", uxTaskGetStackHighWaterMark(NULL), *parameter.airspeed, parameter.vcc, parameter.offset * 1000, voltage, pressure, temperature, air_density, delta_pressure); vTaskDelay(1000 / parameter.rate / portTICK_PERIOD_MS); } } ================================================ FILE: board/project/sensor/airspeed.h ================================================ #ifndef AIRSPEED_H #define AIRSPEED_H #include "common.h" typedef struct airspeed_parameters_t { uint8_t adc_num; uint8_t rate; float alpha; float offset; // accuracy = ±6.25% VFSS float vcc; // nominal is 5V float *temperature; // air temp (input from baro) float *pressure; // air pressure (input from baro) float *airspeed; } airspeed_parameters_t; extern context_t context; void airspeed_task(void *parameters); #endif ================================================ FILE: board/project/sensor/auto_offset.c ================================================ #include "auto_offset.h" #include #include "pico/stdlib.h" void auto_offset_float_task(void *parameters) { auto_offset_float_parameters_t parameter = *(auto_offset_float_parameters_t *)parameters; vTaskDelay(parameter.delay / portTICK_PERIOD_MS); *parameter.offset = *parameter.value; debug("\nAuto offset float (%u): %.2f", uxTaskGetStackHighWaterMark(NULL), *parameter.offset); vTaskDelete(NULL); } void auto_offset_int_task(void *parameters) { auto_offset_int_parameters_t parameter = *(auto_offset_int_parameters_t *)parameters; vTaskDelay(parameter.delay / portTICK_PERIOD_MS); *parameter.offset = *parameter.value; int val = *parameter.value; debug("\nAuto offset int (%u): %d", uxTaskGetStackHighWaterMark(NULL), *parameter.offset); vTaskDelete(NULL); } ================================================ FILE: board/project/sensor/auto_offset.h ================================================ #ifndef AUTO_OFFSET_H #define AUTO_OFFSET_H #include "common.h" typedef struct auto_offset_float_parameters_t { uint delay; float *value; float *offset; } auto_offset_float_parameters_t; typedef struct auto_offset_int_parameters_t { uint delay; int *value; int *offset; } auto_offset_int_parameters_t; extern context_t context; void auto_offset_float_task(void *parameters); void auto_offset_int_task(void *parameters); #endif ================================================ FILE: board/project/sensor/bmp180.c ================================================ #include "bmp180.h" #include #include "auto_offset.h" #include "hardware/i2c.h" #include "pico/stdlib.h" #include "vspeed.h" #define REGISTER_DIG_AC1 0xAA #define REGISTER_DIG_AC2 0xAC #define REGISTER_DIG_AC3 0xAD #define REGISTER_DIG_AC4 0xB0 #define REGISTER_DIG_AC5 0xB2 #define REGISTER_DIG_AC6 0xB4 #define REGISTER_DIG_B1 0xB6 #define REGISTER_DIG_B2 0xB8 #define REGISTER_DIG_MB 0xBA #define REGISTER_DIG_MC 0xBC #define REGISTER_DIG_MD 0xBE #define REGISTER_CONTROL 0xF4 #define REGISTER_CHIPID 0xD0 #define REGISTER_SOFTRESET 0xE0 #define REGISTER_DATA 0xF6 #define READ_TEMPERATURE 0x2E #define READ_PRESSURE 0x34 #define OVERSAMPLING_0 0 #define OVERSAMPLING_1 1 #define OVERSAMPLING_2 2 #define OVERSAMPLING_3 3 #define READ_PRESSURE_OVERSAMPLING_1 0x74 #define READ_PRESSURE_OVERSAMPLING_2 0xB4 #define READ_PRESSURE_OVERSAMPLING_3 0xF4 #define I2C_ADDRESS 0x77 #define PRESSURE_INTERVAL_MS 40 // min 30 #define TEMPERATURE_INTERVAL_MS 20 // min 10 #define VSPEED_INTERVAL_MS 500 static void read(bmp180_parameters_t *parameter, bmp180_calibration_t *calibration); static void begin(bmp180_parameters_t *parameter, bmp180_calibration_t *calibration); void bmp180_task(void *parameters) { bmp180_parameters_t parameter = *(bmp180_parameters_t *)parameters; xTaskNotifyGive(context.receiver_task_handle); *parameter.altitude = 0; *parameter.vspeed = 0; *parameter.temperature = 0; *parameter.pressure = 0; TaskHandle_t task_handle; vTaskDelay(500 / portTICK_PERIOD_MS); bmp180_calibration_t calibration; begin(¶meter, &calibration); while (1) { read(¶meter, &calibration); debug("\nBMP180 (%u) < Temp: %.2f Pressure: %.0f Altitude: %.2f Vspeed: %.2f", uxTaskGetStackHighWaterMark(NULL), *parameter.temperature, *parameter.pressure, *parameter.altitude, *parameter.vspeed); } } static void read(bmp180_parameters_t *parameter, bmp180_calibration_t *calibration) { uint8_t register_address, register_value; uint8_t data[3]; int32_t X1, X2, X3, B5, T, UT, UP, B6, B3, p; uint32_t B4, B7; static float pressure_initial = 0; static uint discard_readings = 5; data[0] = REGISTER_CONTROL; data[1] = READ_TEMPERATURE; i2c_write_blocking(i2c0, I2C_ADDRESS, data, 2, false); vTaskDelay(TEMPERATURE_INTERVAL_MS / portTICK_PERIOD_MS); data[0] = REGISTER_DATA; i2c_write_blocking(i2c0, I2C_ADDRESS, data, 1, true); i2c_read_blocking(i2c0, I2C_ADDRESS, data, 2, false); UT = data[0] << 8 | data[1]; X1 = ((UT - calibration->AC6) * calibration->AC5) >> 15; X2 = (calibration->MC << 11) / (X1 + calibration->MD); B5 = X1 + X2; T = (B5 + 8) >> 4; *parameter->temperature = (float)T / 10; // C calcAverage((float)alphaTemp_ / 100, temperature_, (float)T / 10); data[0] = REGISTER_CONTROL; data[1] = READ_PRESSURE | (OVERSAMPLING_3 << 6); i2c_write_blocking(i2c0, I2C_ADDRESS, data, 2, false); vTaskDelay(PRESSURE_INTERVAL_MS / portTICK_PERIOD_MS); data[0] = REGISTER_DATA; i2c_write_blocking(i2c0, I2C_ADDRESS, data, 1, true); i2c_read_blocking(i2c0, I2C_ADDRESS, data, 3, false); UP = ((uint32_t)data[0] << 16 | (uint16_t)data[1] << 8 | data[2]) >> (8 - OVERSAMPLING_3); B6 = B5 - 4000; X1 = (calibration->B2 * ((B6 * B6) >> 12)) >> 11; X2 = (calibration->AC2 * B6) >> 11; X3 = X1 + X2; B3 = (((calibration->AC1 * 4 + X3) << OVERSAMPLING_3) + 2) / 4; X1 = (calibration->AC3 * B6) >> 13; X2 = (calibration->B1 * ((B6 * B6) >> 12)) >> 16; X3 = ((X1 + X2) + 2) >> 2; B4 = calibration->AC4 * (uint32_t)(X3 + 32768) >> 15; B7 = (uint32_t)(UP - B3) * (50000 >> OVERSAMPLING_3); if (B7 < 0x80000000) { p = B7 * 2 / B4; } else { p = B7 * B4 / 2; } X1 = (p >> 8) * (p >> 8); X1 = (X1 * 3038) >> 16; X2 = (-7357 * p) >> 16; p = p + ((X1 + X2 + 3791) >> 4); *parameter->pressure = p; // Pa calcAverage((float)alphaVario_ / 100, pressure_, p); if (pressure_initial == 0 && discard_readings == 0) pressure_initial = *parameter->pressure; *parameter->altitude = get_altitude(*parameter->pressure, *parameter->temperature, pressure_initial); get_vspeed(parameter->vspeed, *parameter->altitude, VSPEED_INTERVAL_MS); if (discard_readings > 0) discard_readings--; debug("\nBMP180 P0: %.0f", pressure_initial); #ifdef SIM_SENSORS *parameter->temperature = 12.34; *parameter->pressure = 1234.56; *parameter->altitude = 2345; #endif } static void begin(bmp180_parameters_t *parameter, bmp180_calibration_t *calibration) { i2c_init(i2c0, 100 * 1000); gpio_set_function(I2C0_SDA_GPIO, GPIO_FUNC_I2C); gpio_set_function(I2C0_SCL_GPIO, GPIO_FUNC_I2C); gpio_pull_up(I2C0_SDA_GPIO); gpio_pull_up(I2C0_SCL_GPIO); uint8_t data[22]; data[0] = REGISTER_DIG_AC1; i2c_write_blocking(i2c0, I2C_ADDRESS, data, 1, true); i2c_read_blocking(i2c0, I2C_ADDRESS, data, 22, false); calibration->AC1 = ((uint16_t)data[0] << 8) | data[1]; calibration->AC2 = ((uint16_t)data[2] << 8) | data[3]; calibration->AC3 = ((uint16_t)data[4] << 8) | data[5]; calibration->AC4 = ((uint16_t)data[6] << 8) | data[7]; calibration->AC5 = ((uint16_t)data[8] << 8) | data[9]; calibration->AC6 = ((uint16_t)data[10] << 8) | data[11]; calibration->B1 = ((uint16_t)data[12] << 8) | data[13]; calibration->B2 = ((uint16_t)data[14] << 8) | data[15]; calibration->MB = ((uint16_t)data[16] << 8) | data[17]; calibration->MC = ((uint16_t)data[18] << 8) | data[19]; calibration->MD = ((uint16_t)data[20] << 8) | data[21]; } ================================================ FILE: board/project/sensor/bmp180.h ================================================ #ifndef BMP180_H #define BMP180_H #include "common.h" typedef struct bmp180_parameters_t { float alpha_vario; bool auto_offset; float *temperature, *pressure, *altitude, *vspeed; } bmp180_parameters_t; typedef struct bmp180_calibration_t { int16_t AC1, AC2, AC3, B1, B2, MB, MC, MD; uint16_t AC4, AC5, AC6; } bmp180_calibration_t; extern context_t context; void bmp180_task(void *parameters); #endif ================================================ FILE: board/project/sensor/bmp280.c ================================================ #include "bmp280.h" #include #include "auto_offset.h" #include "hardware/i2c.h" #include "pico/stdlib.h" #include "vspeed.h" #define REGISTER_DIG_T1 0x88 #define REGISTER_DIG_T2 0x8A #define REGISTER_DIG_T3 0x8C #define REGISTER_DIG_P1 0x8E #define REGISTER_DIG_P2 0x90 #define REGISTER_DIG_P3 0x92 #define REGISTER_DIG_P4 0x94 #define REGISTER_DIG_P5 0x96 #define REGISTER_DIG_P6 0x98 #define REGISTER_DIG_P7 0x9A #define REGISTER_DIG_P8 0x9C #define REGISTER_DIG_P9 0x9E #define REGISTER_CHIPID 0xD0 #define REGISTER_VERSION 0xD1 #define REGISTER_SOFTRESET 0xE0 #define REGISTER_CAL26 0xE1 #define REGISTER_STATUS 0xF3 #define REGISTER_CONTROL 0xF4 #define REGISTER_CONFIG 0xF5 #define REGISTER_PRESSUREDATA 0xF7 #define REGISTER_TEMPDATA 0xFA #define OVERSAMPLING_X0 0 #define OVERSAMPLING_X1 1 #define OVERSAMPLING_X2 2 #define OVERSAMPLING_X4 3 #define OVERSAMPLING_X8 4 #define OVERSAMPLING_X16 5 #define SLEEP 0 #define FORCED 1 #define NORMAL 3 #define FILTER_OFF 0 #define FILTER_X2 1 #define FILTER_X4 2 #define FILTER_X8 3 #define FILTER_X16 4 #define STANDBY_MS_1 0x00 #define STANDBY_MS_63 0x01 #define STANDBY_MS_125 0x02 #define STANDBY_MS_250 0x03 #define STANDBY_MS_500 0x04 #define STANDBY_MS_1000 0x05 #define STANDBY_MS_2000 0x06 #define STANDBY_MS_4000 0x07 #define I2C_ADDRESS_1 0x76 #define I2C_ADDRESS_2 0x77 #define SENSOR_INTERVAL_MS 40 // min 30 #define VSPEED_INTERVAL_MS 500 static void read(bmp280_parameters_t *parameter, bmp280_calibration_t *calibration); static void begin(bmp280_parameters_t *parameter, bmp280_calibration_t *calibration); void bmp280_task(void *parameters) { bmp280_parameters_t parameter = *(bmp280_parameters_t *)parameters; xTaskNotifyGive(context.receiver_task_handle); *parameter.altitude = 0; *parameter.vspeed = 0; *parameter.temperature = 0; *parameter.pressure = 0; TaskHandle_t task_handle; bmp280_calibration_t calibration; vTaskDelay(500 / portTICK_PERIOD_MS); begin(¶meter, &calibration); while (1) { read(¶meter, &calibration); debug("\nBMP280 (%u) < Temp: %.2f Pressure: %.0f Altitude: %0.2f Vspeed: %.2f", uxTaskGetStackHighWaterMark(NULL), *parameter.temperature, *parameter.pressure, *parameter.altitude, *parameter.vspeed); vTaskDelay(SENSOR_INTERVAL_MS / portTICK_PERIOD_MS); } } static void read(bmp280_parameters_t *parameter, bmp280_calibration_t *calibration) { int64_t var1, var2, p; uint8_t data[3]; uint32_t adc_T, adc_P, t_fine, t; static float pressure_initial = 0; static uint discard_readings = 5; data[0] = REGISTER_TEMPDATA; i2c_write_blocking(i2c0, parameter->address, data, 1, true); // vTaskDelay(50 / portTICK_PERIOD_MS); i2c_read_blocking(i2c0, parameter->address, data, 3, false); adc_T = (((uint32_t)data[0] << 16) | ((uint16_t)data[1] << 8) | data[2]) >> 4; var1 = ((((adc_T >> 3) - ((int32_t)calibration->T1 << 1))) * ((int32_t)calibration->T2)) >> 11; var2 = (int64_t)(((((adc_T >> 4) - ((int32_t)calibration->T1)) * ((adc_T >> 4) - ((int32_t)calibration->T1))))) * (calibration->T3) >> 26; t_fine = var1 + var2; t = (t_fine * 5 + 128) >> 8; *parameter->temperature = (float)t / 100; data[0] = REGISTER_PRESSUREDATA; i2c_write_blocking(i2c0, parameter->address, data, 1, true); i2c_read_blocking(i2c0, parameter->address, data, 3, false); adc_P = (((uint32_t)data[0] << 16) | ((uint16_t)data[1] << 8) | data[2]) >> 4; var1 = ((int64_t)t_fine) - 128000; var2 = var1 * var1 * (int64_t)calibration->P6; var2 = var2 + ((var1 * (int64_t)calibration->P5) << 17); var2 = var2 + (((int64_t)calibration->P4) << 35); var1 = ((var1 * var1 * (int64_t)calibration->P3) >> 8) + ((var1 * (int64_t)calibration->P2) << 12); var1 = (((((int64_t)1) << 47) + var1)) * ((int64_t)calibration->P1) >> 33; if (var1 != 0) { p = 1048576 - adc_P; p = (((p << 31) - var2) * 3125) / var1; var1 = (((int64_t)calibration->P9) * (p >> 13) * (p >> 13)) >> 25; var2 = (((int64_t)calibration->P8) * p) >> 19; p = ((p + var1 + var2) >> 8) + (((int64_t)calibration->P7) << 4); *parameter->pressure = (float)p / 256; // Pa // pressure_ = calcAverage((float)alphaVario_ / 100, pressure_, (float)p / 256); } if (pressure_initial == 0 && discard_readings == 0) pressure_initial = *parameter->pressure; *parameter->altitude = get_altitude(*parameter->pressure, *parameter->temperature, pressure_initial); get_vspeed(parameter->vspeed, *parameter->altitude, VSPEED_INTERVAL_MS); if (discard_readings > 0) discard_readings--; debug("\nBMP280 P0: %.0f", pressure_initial); #ifdef SIM_SENSORS *parameter->temperature = 12.34; *parameter->pressure = 1234.56; *parameter->altitude = 2345; #endif } static void begin(bmp280_parameters_t *parameter, bmp280_calibration_t *calibration) { i2c_init(i2c0, 100 * 1000); gpio_set_function(I2C0_SDA_GPIO, GPIO_FUNC_I2C); gpio_set_function(I2C0_SCL_GPIO, GPIO_FUNC_I2C); gpio_pull_up(I2C0_SDA_GPIO); gpio_pull_up(I2C0_SCL_GPIO); uint8_t data[24] = {0}; // Find sensor address parameter->address = I2C_ADDRESS_1; if (i2c_write_blocking(i2c0, I2C_ADDRESS_1, data, 1, false) == PICO_ERROR_GENERIC) { parameter->address = I2C_ADDRESS_2; } // Configure sensor data[0] = REGISTER_CONTROL; data[1] = (OVERSAMPLING_X2 << 5) | (OVERSAMPLING_X16 << 2) | NORMAL; i2c_write_blocking(i2c0, parameter->address, data, 2, false); data[0] = REGISTER_DIG_T1; i2c_write_blocking(i2c0, parameter->address, data, 1, true); i2c_read_blocking(i2c0, parameter->address, data, 24, false); calibration->T1 = ((uint16_t)data[1] << 8) | data[0]; calibration->T2 = ((uint16_t)data[3] << 8) | data[2]; calibration->T3 = ((uint16_t)data[5] << 8) | data[4]; calibration->P1 = ((uint16_t)data[7] << 8) | data[6]; calibration->P2 = ((uint16_t)data[9] << 8) | data[8]; calibration->P3 = ((uint16_t)data[11] << 8) | data[10]; calibration->P4 = ((uint16_t)data[13] << 8) | data[12]; calibration->P5 = ((uint16_t)data[15] << 8) | data[14]; calibration->P6 = ((uint16_t)data[17] << 8) | data[16]; calibration->P7 = ((uint16_t)data[19] << 8) | data[18]; calibration->P8 = ((uint16_t)data[21] << 8) | data[20]; calibration->P9 = ((uint16_t)data[23] << 8) | data[22]; } ================================================ FILE: board/project/sensor/bmp280.h ================================================ #ifndef BMP280_H #define BMP280_H #include "common.h" typedef struct bmp280_parameters_t { float alpha_vario; bool auto_offset; uint8_t address; uint8_t filter; float *temperature, *pressure, *altitude, *vspeed; } bmp280_parameters_t; typedef struct bmp280_calibration_t { uint16_t T1, P1; int16_t T2, T3, P2, P3, P4, P5, P6, P7, P8, P9; } bmp280_calibration_t; extern context_t context; void bmp280_task(void *parameters); #endif ================================================ FILE: board/project/sensor/cell_count.c ================================================ #include "cell_count.h" #include #include "pico/stdlib.h" void cell_count_task(void *parameters) { cell_count_parameters_t parameter = *(cell_count_parameters_t *)parameters; vTaskDelay(parameter.delay / portTICK_PERIOD_MS); float level[] = {0, 4.35, 8.7, 13.05, 17.4, 21.75, 26.1, 30.45, 34.8, 34.8, 43.5, 43.5}; int cont = 11; float voltage = *parameter.voltage; while (voltage < level[cont] && cont > 0) { cont--; } *parameter.cell_count = cont + 1; debug("\nCell count (%u): (%.2fV) %i", uxTaskGetStackHighWaterMark(NULL), voltage, *parameter.cell_count); vTaskDelete(NULL); } ================================================ FILE: board/project/sensor/cell_count.h ================================================ #ifndef CELL_COUNT_H #define CELL_COUNT_H #include "common.h" typedef struct cell_count_parameters_t { uint delay; float *voltage; uint8_t *cell_count; } cell_count_parameters_t; extern context_t context; void cell_count_task(void *parameters); #endif ================================================ FILE: board/project/sensor/current.c ================================================ #include "current.h" #include #include "auto_offset.h" #include "hardware/adc.h" #include "pico/stdlib.h" void current_task(void *parameters) { static uint32_t timestamp = 0; current_parameters_t parameter = *(current_parameters_t *)parameters; *parameter.voltage = 0; *parameter.current = 0; *parameter.consumption = 0; xTaskNotifyGive(context.receiver_task_handle); adc_init(); adc_gpio_init(parameter.adc_num + 26); //gpio_pull_down(parameter.adc_num + 26); if (parameter.auto_offset) { parameter.offset = -1; auto_offset_float_parameters_t parameter_auto_offset = {5000, parameter.voltage, ¶meter.offset}; TaskHandle_t task_handle; xTaskCreate(auto_offset_float_task, "analog_current_auto_offset_task", STACK_AUTO_OFFSET, ¶meter_auto_offset, 2, &task_handle); } while (1) { *parameter.voltage = voltage_read(parameter.adc_num); if (parameter.offset != -1) *parameter.current = get_average(parameter.alpha, *parameter.current, (*parameter.voltage - parameter.offset) * parameter.multiplier); if (time_us_32() > 6000000) { *parameter.consumption += get_consumption(*parameter.current, 0, ×tamp); } #ifdef SIM_SENSORS *parameter.current = 12.34; *parameter.consumption = 120.34; #endif debug("\nCurrent (%u): Curr %.2f Cons %.2f Volt %.2f VoltOffs %.2f Multip %.2f", uxTaskGetStackHighWaterMark(NULL), *parameter.current, *parameter.consumption, *parameter.voltage, parameter.offset, parameter.multiplier); vTaskDelay(1000 / parameter.rate / portTICK_PERIOD_MS); } } float current_read(uint8_t adc_num) { adc_select_input(adc_num); return adc_read() * BOARD_VCC / ADC_RESOLUTION; } ================================================ FILE: board/project/sensor/current.h ================================================ #ifndef CURRENT_H #define CURRENT_H #include "common.h" typedef struct current_parameters_t { uint8_t adc_num; uint8_t rate; float alpha, multiplier, offset; bool auto_offset; float *current, *consumption, *voltage; } current_parameters_t; extern context_t context; void current_task(void *parameters); #endif ================================================ FILE: board/project/sensor/distance.c ================================================ #include "distance.h" #include #include #include "pico/stdlib.h" #define INIT_DELAY_MS 1000 #define INTERVAL_MS 500 #define MIN_SATS 4 #define MAX_HDOP 5.0f static float degrees_to_radians(float degrees); static float get_distance_to_home_2d(float lat, float lon, float lat_init, float lon_init); static inline bool coord_valid(float lat, float lon) { /* Reject NaN/Inf and the common invalid (0,0) case */ if (!isfinite(lat) || !isfinite(lon)) return false; if (lat == 0.0f && lon == 0.0f) return false; return true; } void distance_task(void *parameters) { distance_parameters_t parameter = *(distance_parameters_t *)parameters; *parameter.distance = 0; float latitude_init = 0, longitude_init = 0; vTaskDelay(INIT_DELAY_MS / portTICK_PERIOD_MS); /* Home設定 */ while (1) { if (*parameter.fix && *parameter.sat >= MIN_SATS && *parameter.hdop <= MAX_HDOP && *parameter.hdop > 0.0f && coord_valid(*parameter.latitude, *parameter.longitude)) { latitude_init = *parameter.latitude; longitude_init = *parameter.longitude; *parameter.home_set = true; debug("\nDistance (%u). Set home: Lat: %.6f, Lon: %.6f, Sats: %.0f, HDOP: %.2f", uxTaskGetStackHighWaterMark(NULL), latitude_init, longitude_init, *parameter.sat, *parameter.hdop); break; } else { debug("\nDistance (%u): Invalid GPS data (Fix: %u, Sats: %.0f, HDOP: %.2f, Lat: %.6f, Lon: %.6f)", uxTaskGetStackHighWaterMark(NULL), (unsigned int)*parameter.fix, *parameter.sat, *parameter.hdop, *parameter.latitude, *parameter.longitude); } vTaskDelay(INTERVAL_MS / portTICK_PERIOD_MS); } /* 距離更新 */ while (1) { if (*parameter.fix && coord_valid(*parameter.latitude, *parameter.longitude)) { *parameter.distance = get_distance_to_home_2d(*parameter.latitude, *parameter.longitude, latitude_init, longitude_init); #ifdef SIM_SENSORS *parameter.distance = 1234.56f; #endif debug( "\nDistance (%u). Distance: %.2f, Lat: %.6f, Lon: %.6f, Sats: %.0f, HDOP: %.2f, Home Lat: %.6f, Home " "Lon: %.6f", uxTaskGetStackHighWaterMark(NULL), *parameter.distance, *parameter.latitude, *parameter.longitude, *parameter.sat, *parameter.hdop, latitude_init, longitude_init); } else { debug("\nDistance (%u): GPS invalid during distance (Fix:%u Sats:%.0f HDOP:%.2f Lat:%.6f Lon:%.6f)", uxTaskGetStackHighWaterMark(NULL), (unsigned)*parameter.fix, *parameter.sat, *parameter.hdop, *parameter.latitude, *parameter.longitude); } vTaskDelay(INTERVAL_MS / portTICK_PERIOD_MS); } } static float degrees_to_radians(float degrees) { return degrees * (float)PI / 180.0f; } static float get_distance_to_home_2d(float lat, float lon, float lat_init, float lon_init) { const float earth_radius_m = 6371000.0f; float lat1 = degrees_to_radians(lat_init); float lat2 = degrees_to_radians(lat); float dlat = degrees_to_radians(lat - lat_init); float dlon = degrees_to_radians(lon - lon_init); float s1 = sinf(dlat * 0.5f); float s2 = sinf(dlon * 0.5f); float a = s1 * s1 + cosf(lat1) * cosf(lat2) * s2 * s2; float c = 2.0f * atan2f(sqrtf(a), sqrtf(1.0f - a)); return earth_radius_m * c; } ================================================ FILE: board/project/sensor/distance.h ================================================ #ifndef DISTANCE_H #define DISTANCE_H #include "common.h" typedef struct distance_parameters_t { float *distance, *latitude, *longitude, *altitude, *sat, *hdop; uint8_t *fix, *home_set; // fix internal msrc: 0 no fix, 1 2D fix, 2 3D fix } distance_parameters_t; extern context_t context; void distance_task(void *parameters); #endif ================================================ FILE: board/project/sensor/esc_apd_f.c ================================================ #include "esc_apd_f.h" #include #include "cell_count.h" #include "pico/stdlib.h" #include "uart.h" #define APD_F_TIMEOUT_US 1000 #define APD_F_PACKET_LENGHT 12 #define KISS_PACKET_LENGHT 10 static void process(esc_apd_f_parameters_t *parameter); static uint8_t update_crc8(uint8_t crc, uint8_t crc_seed); static uint8_t get_crc8(uint8_t *buffer, uint8_t lenght); void esc_apd_f_task(void *parameters) { esc_apd_f_parameters_t parameter = *(esc_apd_f_parameters_t *)parameters; *parameter.rpm = 0; *parameter.voltage = 0; *parameter.current = 0; *parameter.temperature = 0; *parameter.cell_voltage = 0; *parameter.consumption = 0; *parameter.cell_count = 1; xTaskNotifyGive(context.receiver_task_handle); #ifdef SIM_SENSORS *parameter.temperature = 12.34; *parameter.voltage = 12.34; *parameter.current = 12.34; *parameter.consumption = 12.34; *parameter.rpm = 12345.67; *parameter.cell_voltage = 3.75; #endif TaskHandle_t task_handle; uint cell_count_delay = 15000; cell_count_parameters_t cell_count_parameters = {cell_count_delay, parameter.voltage, parameter.cell_count}; xTaskCreate(cell_count_task, "cell_count_task", STACK_CELL_COUNT, (void *)&cell_count_parameters, 1, &task_handle); uart1_begin(115200, UART1_TX_GPIO, UART_ESC_RX, APD_F_TIMEOUT_US, 8, 1, UART_PARITY_NONE, false, false); while (1) { ulTaskNotifyTakeIndexed(1, pdTRUE, portMAX_DELAY); process(¶meter); } } static void process(esc_apd_f_parameters_t *parameter) { static uint32_t timestamp = 0; uint8_t lenght = uart1_available(); if (lenght == APD_F_PACKET_LENGHT || lenght == KISS_PACKET_LENGHT) { uint8_t data[KISS_PACKET_LENGHT]; uart1_read_bytes(data, KISS_PACKET_LENGHT); if (get_crc8(data, KISS_PACKET_LENGHT - 1) == data[9]) { float temperature = data[0]; float voltage = ((uint16_t)data[1] << 8 | data[2]) / 100.0; float current = ((uint16_t)data[3] << 8 | data[4]) / 100.0; float consumption = ((uint16_t)data[5] << 8 | data[6]); float rpm = ((uint16_t)data[7] << 8 | data[8]) * 100.0; rpm *= parameter->rpm_multiplier; *parameter->temperature = get_average(parameter->alpha_temperature, *parameter->temperature, temperature); *parameter->voltage = get_average(parameter->alpha_voltage, *parameter->voltage, voltage); *parameter->current = get_average(parameter->alpha_current, *parameter->current, current); *parameter->consumption = get_average(parameter->alpha_voltage, *parameter->consumption, consumption); *parameter->rpm = get_average(parameter->alpha_rpm, *parameter->rpm, rpm); *parameter->cell_voltage = *parameter->voltage / *parameter->cell_count; debug("\nApd F (%u) < Rpm: %.0f Volt: %0.2f Curr: %.2f Temp: %.0f Cons: %.0f CellV: %.2f", uxTaskGetStackHighWaterMark(NULL), *parameter->rpm, *parameter->voltage, *parameter->current, *parameter->temperature, *parameter->consumption, *parameter->cell_voltage); } } } static uint8_t update_crc8(uint8_t crc, uint8_t crc_seed) { uint8_t crc_u, i; crc_u = crc; crc_u ^= crc_seed; for (i = 0; i < 8; i++) crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1); return (crc_u); } static uint8_t get_crc8(uint8_t *buffer, uint8_t lenght) { uint8_t crc = 0, i; for (i = 0; i < lenght; i++) crc = update_crc8(buffer[i], crc); return crc; } ================================================ FILE: board/project/sensor/esc_apd_f.h ================================================ #ifndef ESC_APD_F_H #define ESC_APD_F_H #include "common.h" typedef struct esc_apd_f_parameters_t { float rpm_multiplier; float alpha_rpm, alpha_voltage, alpha_current, alpha_temperature; float *rpm, *voltage, *current, *temperature, *cell_voltage, *consumption; uint8_t *cell_count; } esc_apd_f_parameters_t; extern context_t context; void esc_apd_f_task(void *parameters); #endif ================================================ FILE: board/project/sensor/esc_apd_hv.c ================================================ #include "esc_apd_hv.h" #include #include #include "cell_count.h" #include "pico/stdlib.h" #include "uart.h" #define ESC_KISS_PACKET_LENGHT 10 #define ESC_APD_HV_TIMEOUT_US 1000 #define ESC_APD_HV_PACKET_LENGHT 22 static void process(esc_apd_hv_parameters_t *parameter); static float get_temperature(uint16_t raw); static uint16_t get_crc16(uint8_t *buffer); void esc_apd_hv_task(void *parameters) { esc_apd_hv_parameters_t parameter = *(esc_apd_hv_parameters_t *)parameters; *parameter.rpm = 0; *parameter.voltage = 0; *parameter.current = 0; *parameter.temperature = 0; *parameter.cell_voltage = 0; *parameter.consumption = 0; *parameter.cell_count = 1; xTaskNotifyGive(context.receiver_task_handle); #ifdef SIM_SENSORS *parameter.temperature = 12.34; *parameter.voltage = 12.34; *parameter.current = 12.34; *parameter.consumption = 12.34; *parameter.rpm = 12345.67; *parameter.cell_voltage = 3.75; #endif TaskHandle_t task_handle; uint cell_count_delay = 15000; cell_count_parameters_t cell_count_parameters = {cell_count_delay, parameter.voltage, parameter.cell_count}; xTaskCreate(cell_count_task, "cell_count_task", STACK_CELL_COUNT, (void *)&cell_count_parameters, 1, &task_handle); uart1_begin(115200, UART1_TX_GPIO, UART_ESC_RX, ESC_APD_HV_TIMEOUT_US, 8, 1, UART_PARITY_NONE, false, false); while (1) { ulTaskNotifyTakeIndexed(1, pdTRUE, portMAX_DELAY); process(¶meter); } } static void process(esc_apd_hv_parameters_t *parameter) { static uint32_t timestamp = 0; if (uart1_available() == ESC_APD_HV_PACKET_LENGHT) { uint8_t data[ESC_APD_HV_PACKET_LENGHT]; uart1_read_bytes(data, ESC_APD_HV_PACKET_LENGHT); if (get_crc16(data) == (((uint16_t)data[19] << 8) | data[18])) { float voltage = ((uint16_t)data[1] << 8 | data[0]) / 100.0; float temp = get_temperature((uint16_t)data[3] << 8 | data[2]); float current = ((uint16_t)data[5] << 8 | data[4]) / 12.5; float rpm = (uint32_t)data[11] << 24 | (uint32_t)data[10] << 16 | (uint16_t)data[9] << 8 | data[8]; rpm *= parameter->rpm_multiplier; *parameter->temperature = get_average(parameter->alpha_temperature, *parameter->temperature, temp); *parameter->voltage = get_average(parameter->alpha_voltage, *parameter->voltage, voltage); *parameter->current = get_average(parameter->alpha_current, *parameter->current, current); *parameter->rpm = get_average(parameter->alpha_rpm, *parameter->rpm, rpm); *parameter->consumption += get_consumption(*parameter->current, 0, ×tamp); *parameter->cell_voltage = *parameter->voltage / *parameter->cell_count; debug("\nApd HV (%u) < Rpm: %.0f Volt: %0.2f Curr: %.2f Temp: %.0f Cons: %.0f CellV: %.2f", uxTaskGetStackHighWaterMark(NULL), *parameter->rpm, *parameter->voltage, *parameter->current, *parameter->temperature, *parameter->consumption, *parameter->cell_voltage); } } } static float get_temperature(uint16_t raw) { uint16_t SERIESRESISTOR = 10000; uint16_t NOMINAL_RESISTANCE = 10000; uint8_t NOMINAL_TEMPERATURE = 25; uint16_t BCOEFFICIENT = 3455; // convert value to resistance float Rntc = (4096 / (float)raw) - 1; Rntc = SERIESRESISTOR / Rntc; // Get the temperature float temperature = Rntc / (float)NOMINAL_RESISTANCE; // (R/Ro) temperature = (float)log(temperature); // ln(R/Ro) temperature /= BCOEFFICIENT; // 1/B * ln(R/Ro) temperature += (float)1.0 / ((float)NOMINAL_TEMPERATURE + (float)273.15); // + (1/To) temperature = (float)1.0 / temperature; // Invert temperature -= (float)273.15; return temperature; } static uint16_t get_crc16(uint8_t *buffer) { uint16_t fCCRC16; uint16_t c0 = 0; uint16_t c1 = 0; // Calculate checksum intermediate bytesUInt16 for (uint8_t i = 0; i < 18; i++) // Check only first 18 bytes, skip crc bytes { c0 = (uint16_t)(c0 + (buffer[i])) % 255; c1 = (uint16_t)(c1 + c0) % 255; } // Assemble the 16-bit checksum value fCCRC16 = (c1 << 8) | c0; return fCCRC16; } ================================================ FILE: board/project/sensor/esc_apd_hv.h ================================================ #ifndef ESC_APD_HV_H #define ESC_APD_HV_H #include "common.h" typedef struct esc_apd_hv_parameters_t { float rpm_multiplier; float alpha_rpm, alpha_voltage, alpha_current, alpha_temperature; float *rpm, *voltage, *current, *temperature, *cell_voltage, *consumption; uint8_t *cell_count; } esc_apd_hv_parameters_t; extern context_t context; void esc_apd_hv_task(void *parameters); #endif ================================================ FILE: board/project/sensor/esc_castle.c ================================================ #include "esc_castle.h" #include #include #include "cell_count.h" #include "hardware/clocks.h" #include "hardware/irq.h" #include "hardware/pio.h" #include "pico/stdlib.h" static volatile esc_castle_parameters_t parameter; static void castle_link_handler(castle_link_telemetry_t packet); void esc_castle_task(void *parameters) { parameter = *(esc_castle_parameters_t *)parameters; *parameter.rpm = 0; *parameter.voltage = 0; *parameter.ripple_voltage = 0; *parameter.current = 0; *parameter.thr = 0; *parameter.output = 0; *parameter.rpm = 0; *parameter.consumption = 0; *parameter.voltage_bec = 0; *parameter.current_bec = 0; *parameter.temperature = 0; *parameter.cell_voltage = 0; *parameter.cell_count = 1; xTaskNotifyGive(context.receiver_task_handle); #ifdef SIM_SENSORS *parameter.voltage = 12.34; *parameter.ripple_voltage = 1.23; *parameter.current = 23.45; *parameter.thr = 50; *parameter.output = 80; *parameter.rpm = 12345.67; *parameter.consumption = 345.67; *parameter.voltage_bec = 4.56; *parameter.current_bec = 5.67; *parameter.temperature = 29.9; *parameter.cell_voltage = 3.75; #endif castle_link_init(pio0, CASTLE_PWM_GPIO, PIO0_IRQ_0); castle_link_set_handler(castle_link_handler); debug("\nCastle init"); TaskHandle_t task_handle; uint cell_count_delay = 15000; cell_count_parameters_t cell_count_parameters = {cell_count_delay, parameter.voltage, parameter.cell_count}; xTaskCreate(cell_count_task, "cell_count_task", STACK_CELL_COUNT, (void *)&cell_count_parameters, 1, &task_handle); vTaskSuspend(NULL); vTaskDelete(NULL); } static void castle_link_handler(castle_link_telemetry_t packet) { *parameter.voltage = packet.voltage; *parameter.ripple_voltage = packet.ripple_voltage; *parameter.current = packet.current; *parameter.thr = packet.thr; *parameter.output = packet.output; *parameter.rpm = packet.rpm * parameter.rpm_multiplier; *parameter.voltage_bec = packet.voltage_bec; *parameter.current_bec = packet.current_bec; *parameter.temperature = packet.temperature; debug( "\nCastle (%u) < Volt(V): %.2f Ripple volt(V): %.2f Curr(A): %.2f Thr: %.0f Out: %.0f Rpm: %.0f Bec volt(V): " "%.2f Bec curr(A): %.2f Temp(C): %.0f %s", uxTaskGetStackHighWaterMark(NULL), packet.voltage, packet.ripple_voltage, packet.current, packet.thr, packet.output, packet.rpm, packet.voltage_bec, packet.current_bec, packet.temperature, packet.is_temp_ntc ? " NTC" : " Linear"); } ================================================ FILE: board/project/sensor/esc_castle.h ================================================ #ifndef ESC_CASTLE_H #define ESC_CASTLE_H #include "castle_link.h" #include "common.h" /* castleTelemetry: index element scaler 0 calib 1 (1000us) 0 1 Volt (V) 20 2 rippleVolt (V) 4 3 Curr (A) 50 4 Thr (0.1-0.2 ms esc pulse) 1 5 Output power (%) 0.2502 6 rpm 20416.7 7 becVolt (V) 4 8 becCurr (A) 4 9 temp (C) or calib 2 (500us) 30 10 temp ntc (C) or calib 2 (500us) 63.8125 */ typedef struct esc_castle_parameters_t { float rpm_multiplier; float alpha_rpm, alpha_voltage, alpha_current, alpha_temperature; float *voltage, *ripple_voltage, *current, *thr, *output, *rpm, *consumption, *voltage_bec, *current_bec, *temperature, *cell_voltage; uint8_t *cell_count; } esc_castle_parameters_t; extern context_t context; void esc_castle_task(void *parameters); #endif ================================================ FILE: board/project/sensor/esc_hw3.c ================================================ #include "esc_hw3.h" #include #include #include "pico/stdlib.h" #include "uart.h" #define TIMEOUT_US 1000 #define PACKET_LENGHT 10 #define NO_SIGNAL_TIMEOUT_MS 500 static void process(esc_hw3_parameters_t *parameter); static int64_t timeout_callback(alarm_id_t id, void *parameters); void esc_hw3_task(void *parameters) { debug("\nHW3 init"); esc_hw3_parameters_t parameter = *(esc_hw3_parameters_t *)parameters; xTaskNotifyGive(context.receiver_task_handle); *parameter.rpm = 0; #ifdef SIM_SENSORS *parameter.rpm = 12345.67; #endif uart1_begin(19200, UART1_TX_GPIO, UART_ESC_RX, TIMEOUT_US, 8, 1, UART_PARITY_NONE, false, false); while (1) { ulTaskNotifyTakeIndexed(1, pdTRUE, portMAX_DELAY); process(¶meter); } } static void process(esc_hw3_parameters_t *parameter) { static alarm_id_t timeout_alarm_id = 0; if (uart1_available() == PACKET_LENGHT) { if (timeout_alarm_id) cancel_alarm(timeout_alarm_id); uint8_t data[PACKET_LENGHT]; uart1_read_bytes(data, PACKET_LENGHT); if (data[0] == 0x9B && data[4] == 0 && data[6] == 0) { uint16_t rpmCycle = (uint16_t)data[8] << 8 | data[9]; if (rpmCycle <= 0) rpmCycle = 1; float rpm = 60000000.0 / rpmCycle * parameter->multiplier; *parameter->rpm = get_average(parameter->alpha, *parameter->rpm, rpm); uint32_t packet = (uint32_t)data[1] << 16 | (uint16_t)data[2] << 8 | data[3]; debug("\nEsc HW3 (%u) < Packet: %i Rpm: %.0f", uxTaskGetStackHighWaterMark(NULL), packet, *parameter->rpm); } timeout_alarm_id = add_alarm_in_ms(NO_SIGNAL_TIMEOUT_MS, timeout_callback, parameter->rpm, false); } } static int64_t timeout_callback(alarm_id_t id, void *parameters) { float *parameter = (float *)parameters; *parameter = 0; debug("\nEsc HW3 signal timeout. Rpm: 0"); return 0; } ================================================ FILE: board/project/sensor/esc_hw3.h ================================================ #ifndef ESC_HW3_H #define ESC_HW3_H #include "common.h" typedef struct esc_hw3_parameters_t { float multiplier; float alpha; float *rpm; } esc_hw3_parameters_t; extern context_t context; void esc_hw3_task(void *parameters); #endif ================================================ FILE: board/project/sensor/esc_hw4.c ================================================ #include "esc_hw4.h" #include #include #include "auto_offset.h" #include "cell_count.h" #include "pico/stdlib.h" #include "uart.h" #include "uart_pio.h" #define TIMEOUT_US 2000 #define PACKET_LENGHT 19 #define SIGNATURE_LENGHT 13 #define CURRENT_OFFSET_DELAY 15000 #define NTC_BETA 3950.0 #define NTC_R1 10000.0 #define NTC_R_REF 47000.0 #define DIFFAMP_SHUNT (0.25 / 1000) #define V_REF 3.3 #define ADC_RES 4096.0 float current_offset_ = -1; static void process(esc_hw4_parameters_t *parameter, uint *current_raw); float get_voltage(uint16_t voltage_raw, esc_hw4_parameters_t *parameter); float get_temperature(uint16_t temperature_raw); float get_current(uint raw, int offset, float multiplier); void esc_hw4_task(void *parameters) { esc_hw4_parameters_t parameter = *(esc_hw4_parameters_t *)parameters; *parameter.rpm = 0; *parameter.voltage = 0; *parameter.current = 0; *parameter.temperature_fet = 0; *parameter.temperature_bec = 0; *parameter.cell_voltage = 0; *parameter.consumption = 0; *parameter.cell_count = 1; xTaskNotifyGive(context.receiver_task_handle); #ifdef SIM_SENSORS *parameter.rpm = 12345.67; *parameter.consumption = 123.4; *parameter.voltage = 12.34; *parameter.current = 5.678; *parameter.temperature_fet = 12.34; *parameter.temperature_bec = 23.45; *parameter.cell_voltage = 3.75; #endif if (parameter.init_delay) vTaskDelay(15000 / portTICK_PERIOD_MS); TaskHandle_t task_handle; uint cell_count_delay = 15000; cell_count_parameters_t cell_count_parameters = {cell_count_delay, parameter.voltage, parameter.cell_count}; xTaskCreate(cell_count_task, "cell_count_task", STACK_CELL_COUNT, (void *)&cell_count_parameters, 1, &task_handle); uint current_raw = 0; if (!parameter.auto_detect && !parameter.current_is_manual_offset) { uint current_delay = CURRENT_OFFSET_DELAY; auto_offset_int_parameters_t current_offset_parameters = {current_delay, ¤t_raw, ¶meter.current_offset}; xTaskCreate(auto_offset_int_task, "esc_hw4_current_offset_task", STACK_AUTO_OFFSET, ¤t_offset_parameters, 1, &task_handle); } uart1_begin(19200, UART1_TX_GPIO, UART_ESC_RX, TIMEOUT_US, 8, 1, UART_PARITY_NONE, false, false); while (1) { ulTaskNotifyTakeIndexed(1, pdTRUE, portMAX_DELAY); process(¶meter, ¤t_raw); } } static void process(esc_hw4_parameters_t *parameter, uint *current_raw) { uint16_t pwm, throttle; static uint32_t timestamp = 0; uint8_t lenght = uart1_available(); uint8_t data[lenght]; uart1_read_bytes(data, lenght); debug("\nEsc HW4 (%u) < ", uxTaskGetStackHighWaterMark(NULL)); debug_buffer(data, lenght, "0x%X "); if (lenght == 13 && parameter->auto_detect) { if (data[1] == 0x9B && data[4] == 0x01 && data[12] == 0xB9) { parameter->voltage_multiplier = (float)data[5] / (float)data[6] / 10.0; parameter->current_multiplier = (float)data[7] / (float)data[8]; if (parameter->current_multiplier != 0) { parameter->current_offset = (float)data[9] / parameter->current_multiplier; } else parameter->current_offset = 0; debug("\nEsc HW4 signature (%u): VoltMult: %.0f CurrMult: %.0f CurrOff: %u", uxTaskGetStackHighWaterMark(NULL), parameter->voltage_multiplier * 100000, parameter->current_multiplier * 100000, parameter->current_offset); } else { debug("\nEsc HW4 signature error (%u): ", uxTaskGetStackHighWaterMark(NULL)); } } if (lenght == PACKET_LENGHT || lenght == PACKET_LENGHT + 1) { throttle = (uint16_t)data[4] << 8 | data[5]; // 0-1024 pwm = (uint16_t)data[6] << 8 | data[7]; // 0-1024 float rpm = (uint32_t)data[8] << 16 | (uint16_t)data[9] << 8 | data[10]; // try to filter invalid data frames /*if (throttle < 1024 && pwm < 1024 && rpm < 200000 && data[11] <= 0xF && data[13] <= 0xF && data[15] <= 0xF && data[17] <= 0xF)*/ if (data[0] == 0x9B) { *current_raw = (uint16_t)data[13] << 8 | data[14]; float voltage = get_voltage((uint16_t)data[11] << 8 | data[12], parameter); float current = 0; if (throttle) current = get_current(*current_raw, parameter->current_offset, parameter->current_multiplier); float temperature_fet = get_temperature((uint16_t)data[15] << 8 | data[16]); float temperature_bec = get_temperature((uint16_t)data[17] << 8 | data[18]); rpm *= parameter->rpm_multiplier; if (parameter->pwm_out) xTaskNotifyGive(context.pwm_out_task_handle); *parameter->rpm = get_average(parameter->alpha_rpm, *parameter->rpm, rpm); *parameter->consumption += get_consumption(*parameter->current, parameter->current_max, ×tamp); *parameter->voltage = get_average(parameter->alpha_voltage, *parameter->voltage, voltage); *parameter->current = get_average(parameter->alpha_current, *parameter->current, current); *parameter->temperature_fet = get_average(parameter->alpha_temperature, *parameter->temperature_fet, temperature_fet); *parameter->temperature_bec = get_average(parameter->alpha_temperature, *parameter->temperature_bec, temperature_bec); *parameter->cell_voltage = *parameter->voltage / *parameter->cell_count; uint32_t packet = (uint32_t)data[1] << 16 | (uint16_t)data[2] << 8 | data[3]; debug( "\nEsc HW4 (%u) < Packet: %i Thr: %u Rpm: %.0f Volt: %0.2f Curr: %.2f TempFet: %.0f TempBec: %.0f Cons: %.0f " "CellV: %.2f CRaw: %i CRawOffset: %u CurrMult: %.0f VoltMult: %.0f", uxTaskGetStackHighWaterMark(NULL), packet, throttle, *parameter->rpm, *parameter->voltage, *parameter->current, *parameter->temperature_fet, *parameter->temperature_bec, *parameter->consumption, *parameter->cell_voltage, *current_raw, parameter->current_offset, parameter->current_multiplier * 100000, parameter->voltage_multiplier * 100000); } else { debug("\nEsc HW4 packet error (%u) 0x%X", uxTaskGetStackHighWaterMark(NULL), data[0]); } } } float get_voltage(uint16_t voltage_raw, esc_hw4_parameters_t *parameter) { return voltage_raw * parameter->voltage_multiplier; } float get_temperature(uint16_t temperature_raw) { float voltage = temperature_raw * V_REF / ADC_RES; float ntcR_Rref = (voltage * NTC_R1 / (V_REF - voltage)) / NTC_R_REF; if (ntcR_Rref < 0.001) return 0; float temperature = 1 / (log(ntcR_Rref) / NTC_BETA + 1 / 298.15) - 273.15; if (temperature < 0) return 0; return temperature; } float get_current(uint raw, int offset, float multiplier) { // float current = (*parameter->current_raw - *parameter->current_offset) * V_REF / (parameter->ampgain * // DIFFAMP_SHUNT * ADC_RES); if ((int)raw - offset < 0) return 0; return ((int)raw - offset) * multiplier; } ================================================ FILE: board/project/sensor/esc_hw4.h ================================================ #ifndef ESC_HW4_H #define ESC_HW4_H #include "common.h" typedef struct esc_hw4_parameters_t { float rpm_multiplier; bool pwm_out; bool init_delay; float alpha_rpm, alpha_voltage, alpha_current, alpha_temperature; float voltage_multiplier, current_multiplier, current_thresold, current_max; bool current_is_manual_offset, auto_detect; uint current_offset; float *rpm, *voltage, *current, *temperature_fet, *temperature_bec, *cell_voltage, *consumption; uint8_t *cell_count; } esc_hw4_parameters_t; extern context_t context; void esc_hw4_task(void *parameters); #endif ================================================ FILE: board/project/sensor/esc_hw5.c ================================================ #include "esc_hw5.h" #include #include "auto_offset.h" #include "cell_count.h" #include "pico/stdlib.h" #include "uart.h" #include "uart_pio.h" #define TIMEOUT_US 2000 #define PACKET_LENGHT 32 static uint8_t *CRCH_, *CRCL_; static void process(esc_hw5_parameters_t *parameter); static uint16_t calculate_crc16(uint8_t const *buffer, uint lenght); void esc_hw5_task(void *parameters) { esc_hw5_parameters_t parameter = *(esc_hw5_parameters_t *)parameters; *parameter.rpm = 0; *parameter.voltage = 0; *parameter.current = 0; *parameter.temperature_fet = 0; *parameter.temperature_bec = 0; *parameter.temperature_motor = 0; *parameter.voltage_bec = 0; *parameter.current_bec = 0; *parameter.cell_voltage = 0; *parameter.consumption = 0; *parameter.cell_count = 1; xTaskNotifyGive(context.receiver_task_handle); uint8_t CRCH[] = {0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40}; uint8_t CRCL[] = {0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, 0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3, 0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32, 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26, 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1, 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, 0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5, 0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0, 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C, 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83, 0x41, 0x81, 0x80, 0x40}; CRCL_ = CRCL; CRCH_ = CRCH; #ifdef SIM_SENSORS *parameter.rpm = 12345.67; *parameter.consumption = 123.4; *parameter.voltage = 12.34; *parameter.current = 5.678; *parameter.temperature_fet = 12.34; *parameter.temperature_bec = 23.45; *parameter.cell_voltage = 3.75; #endif TaskHandle_t task_handle; uint cell_count_delay = 15000; cell_count_parameters_t cell_count_parameters = {cell_count_delay, parameter.voltage, parameter.cell_count}; xTaskCreate(cell_count_task, "cell_count_task", STACK_CELL_COUNT, (void *)&cell_count_parameters, 1, &task_handle); uart1_begin(115200, UART1_TX_GPIO, UART_ESC_RX, TIMEOUT_US, 8, 1, UART_PARITY_NONE, false, false); while (1) { ulTaskNotifyTakeIndexed(1, pdTRUE, portMAX_DELAY); process(¶meter); } } static void process(esc_hw5_parameters_t *parameter) { static uint32_t timestamp = 0; uint8_t lenght = uart1_available(); if (lenght) { uint8_t data[lenght]; uart1_read_bytes(data, lenght); if (lenght == PACKET_LENGHT) { uint16_t packet_crc = (data[31] << 8) | data[30]; uint16_t crc = calculate_crc16(data, PACKET_LENGHT - 2); if (crc == packet_crc) { uint16_t throttle; throttle = data[7 + 2]; *parameter->rpm = (data[7 + 6] | (data[7 + 7] << 8)) * 10 * parameter->rpm_multiplier; *parameter->voltage = (data[7 + 8] | (data[7 + 9] << 8)) / 10.0; *parameter->current = (data[7 + 10] | (data[7 + 11] << 8)) / 10.0; if (data[7 + 12] != 0xFF) *parameter->temperature_fet = data[7 + 12]; if (data[7 + 13] != 0xFF) *parameter->temperature_bec = data[7 + 13]; if (data[7 + 14] != 0xFF) *parameter->temperature_motor = data[7 + 14]; if (data[7 + 15] != 0xFF) *parameter->voltage_bec = data[7 + 15]; if (data[7 + 16] != 0xFF) *parameter->current_bec = data[7 + 16]; *parameter->cell_voltage = *parameter->voltage / *parameter->cell_count; *parameter->consumption += get_consumption(*parameter->current, 0, ×tamp); debug( "\nEsc HW5 (%u) < Rpm: %.0f Volt: %0.2f Curr: %.2f TempFet: %.0f TempBec: %.0f TempMotor: %.0f " "Vbec: %.1f Cbec: %.1f Cons: %.0f CellCount: %u CellV: %.2f", uxTaskGetStackHighWaterMark(NULL), *parameter->rpm, *parameter->voltage, *parameter->current, *parameter->temperature_fet, *parameter->temperature_bec, *parameter->temperature_motor, *parameter->voltage_bec, *parameter->current_bec, *parameter->consumption, *parameter->cell_count, *parameter->cell_voltage, *parameter->cell_voltage); } else { debug("\nEsc HW5 CRC error (%u): ", uxTaskGetStackHighWaterMark(NULL)); debug_buffer(data, lenght, "0x%X "); debug("\nCalculated CRC: %u", crc); } } else { debug("\nEsc HW5 packet error (%u): ", uxTaskGetStackHighWaterMark(NULL)); debug_buffer(data, lenght, "0x%X "); } } } static uint16_t calculate_crc16(uint8_t const *buffer, uint lenght) { // CRC16-MODBUS // 8005(x16+x15+x2+1) uint8_t crc_high = 0xFF; uint8_t crc_low = 0xFF; uint8_t index; while (lenght--) { index = crc_low ^ (*(buffer++)); crc_low = crc_high ^ CRCH_[index]; crc_high = CRCL_[index]; } return (uint16_t)((uint16_t)(crc_high << 8) | crc_low); } ================================================ FILE: board/project/sensor/esc_hw5.h ================================================ #ifndef ESC_HW5_H #define ESC_HW5_H #include "common.h" typedef struct esc_hw5_parameters_t { float rpm_multiplier; float alpha_rpm, alpha_voltage, alpha_current, alpha_temperature; float *rpm, *voltage, *current, *temperature_fet, *temperature_bec, *temperature_motor, *voltage_bec, *current_bec, *cell_voltage, *consumption; uint8_t *cell_count; } esc_hw5_parameters_t; extern context_t context; void esc_hw5_task(void *parameters); #endif ================================================ FILE: board/project/sensor/esc_kontronik.c ================================================ #include "esc_kontronik.h" #include #include "cell_count.h" #include "pico/stdlib.h" #include "uart.h" #define TIMEOUT_US 1000 #define PACKET_LENGHT 35 static void process(esc_kontronik_parameters_t *parameter); void esc_kontronik_task(void *parameters) { esc_kontronik_parameters_t parameter = *(esc_kontronik_parameters_t *)parameters; *parameter.rpm = 0; *parameter.voltage = 0; *parameter.current = 0; *parameter.voltage_bec = 0; *parameter.current_bec = 0; *parameter.temperature_fet = 0; *parameter.temperature_bec = 0; *parameter.cell_voltage = 0; *parameter.consumption = 0; *parameter.cell_count = 1; xTaskNotifyGive(context.receiver_task_handle); #ifdef SIM_SENSORS *parameter.rpm = 12345.67; *parameter.consumption = 123.4; *parameter.voltage = 12.34; *parameter.current = 12.34; *parameter.temperature_fet = 12.34; *parameter.temperature_bec = 12.34; *parameter.cell_voltage = 3.75; #endif TaskHandle_t task_handle; uint cell_count_delay = 15000; cell_count_parameters_t cell_count_parameters = {cell_count_delay, parameter.voltage, parameter.cell_count}; xTaskCreate(cell_count_task, "cell_count_task", STACK_CELL_COUNT, (void *)&cell_count_parameters, 1, &task_handle); uart1_begin(115200, UART1_TX_GPIO, UART_ESC_RX, TIMEOUT_US, 8, 1, UART_PARITY_EVEN, false, false); while (1) { ulTaskNotifyTakeIndexed(1, pdTRUE, portMAX_DELAY); process(¶meter); } } static void process(esc_kontronik_parameters_t *parameter) { static uint32_t timestamp = 0; if (uart1_available() == PACKET_LENGHT) { uint8_t data[PACKET_LENGHT]; uart1_read_bytes(data, PACKET_LENGHT); if (data[0] == 0x4B && data[1] == 0x4F && data[2] == 0x44 && data[3] == 0x4C) { float rpm = (uint32_t)data[7] << 24 | (uint32_t)data[6] << 16 | (uint16_t)data[5] << 8 | data[4]; rpm *= parameter->rpm_multiplier; float voltage = ((uint16_t)data[9] << 8 | data[8]) / 100.0; float current = ((uint16_t)data[11] << 8 | data[10]) / 10.0; float consumption = ((uint16_t)data[17] << 8 | data[16]); float current_bec = ((uint16_t)data[19] << 8 | data[18]) / 1000.0; float voltage_bec = ((uint16_t)data[21] << 8 | data[20]) / 1000.0; float temperature_fet = data[26]; float temperature_bec = data[27]; *parameter->rpm = get_average(parameter->alpha_rpm, *parameter->rpm, rpm); *parameter->consumption = consumption; *parameter->voltage = get_average(parameter->alpha_voltage, *parameter->voltage, voltage); *parameter->current = get_average(parameter->alpha_current, *parameter->current, current); *parameter->voltage_bec = get_average(parameter->alpha_voltage, *parameter->voltage_bec, voltage_bec); *parameter->current_bec = get_average(parameter->alpha_current, *parameter->current_bec, current_bec); *parameter->temperature_fet = get_average(parameter->alpha_temperature, *parameter->temperature_fet, temperature_fet); *parameter->temperature_bec = get_average(parameter->alpha_temperature, *parameter->temperature_bec, temperature_bec); *parameter->cell_voltage = *parameter->voltage / *parameter->cell_count; debug( "\nKontronic (%u) < Rpm: %.0f Volt: %0.2f Curr: %.2f V Bec: %0.2f C Bec: %.2f TempFet: %.0f TempBec: " "%.0f Cons: %.0f CellV: %.2f", uxTaskGetStackHighWaterMark(NULL), *parameter->rpm, *parameter->voltage, *parameter->current, *parameter->voltage_bec, *parameter->current_bec, *parameter->temperature_fet, *parameter->temperature_bec, *parameter->consumption, *parameter->cell_voltage); } } } ================================================ FILE: board/project/sensor/esc_kontronik.h ================================================ #ifndef ESC_KONTRONIK_H #define ESC_KONTRONIK_H #include "common.h" typedef struct esc_kontronik_parameters_t { float rpm_multiplier; float alpha_rpm, alpha_voltage, alpha_current, alpha_temperature; float *rpm, *voltage, *current, *voltage_bec, *current_bec, *temperature_fet, *temperature_bec, *cell_voltage, *consumption; uint8_t *cell_count; } esc_kontronik_parameters_t; extern context_t context; void esc_kontronik_task(void *parameters); #endif ================================================ FILE: board/project/sensor/esc_omp_m4.c ================================================ #include "esc_omp_m4.h" #include #include "cell_count.h" #include "pico/stdlib.h" #include "uart.h" #define OMP_M4_TIMEOUT_US 1000 #define OMP_M4_PACKET_LENGHT 32 #define OMP_M4_PACKET_HEADER 0xDD typedef struct esc_omp_m4_packet_t { uint8_t header; uint8_t version; uint8_t length; uint16_t voltage; uint16_t current; uint8_t throttle; uint16_t rpm; uint8_t temp_esc; uint8_t temp_motor; uint8_t pwm; uint16_t status; uint16_t consumption; uint8_t unused[31 - 17 + 1]; } __attribute((packed)) esc_omp_m4_packet_t; static void process(esc_omp_m4_parameters_t *parameter); void esc_omp_m4_task(void *parameters) { esc_omp_m4_parameters_t parameter = *(esc_omp_m4_parameters_t *)parameters; *parameter.rpm = 0; *parameter.voltage = 0; *parameter.current = 0; *parameter.temp_esc = 0; *parameter.temp_motor = 0; *parameter.cell_voltage = 0; *parameter.consumption = 0; *parameter.cell_count = 1; xTaskNotifyGive(context.receiver_task_handle); #ifdef SIM_SENSORS *parameter.temp_esc = 12.34; *parameter.temp_motor = 23.45; *parameter.voltage = 12.34; *parameter.current = 12.34; *parameter.consumption = 12.34; *parameter.rpm = 12345.67; *parameter.cell_voltage = 3.75; #endif TaskHandle_t task_handle; uint cell_count_delay = 15000; cell_count_parameters_t cell_count_parameters = {cell_count_delay, parameter.voltage, parameter.cell_count}; xTaskCreate(cell_count_task, "cell_count_task", STACK_CELL_COUNT, (void *)&cell_count_parameters, 1, &task_handle); uart1_begin(115200, UART1_TX_GPIO, UART_ESC_RX, OMP_M4_TIMEOUT_US, 8, 1, UART_PARITY_NONE, false, false); while (1) { ulTaskNotifyTakeIndexed(1, pdTRUE, portMAX_DELAY); process(¶meter); } } static void process(esc_omp_m4_parameters_t *parameter) { static uint32_t timestamp = 0; uint8_t lenght = uart1_available(); if (lenght == OMP_M4_PACKET_LENGHT) { esc_omp_m4_packet_t packet; uart1_read_bytes((uint8_t *)&packet, OMP_M4_PACKET_LENGHT); if (packet.header != OMP_M4_PACKET_HEADER) return; float temp_esc = packet.temp_esc; float temp_motor = packet.temp_motor; float voltage = swap_16(packet.voltage) / 10.0; float current = swap_16(packet.current) / 10.0; float consumption = swap_16(packet.consumption); float rpm = swap_16(packet.rpm) * 10.0; rpm *= parameter->rpm_multiplier; *parameter->temp_esc = get_average(parameter->alpha_temperature, *parameter->temp_esc, temp_esc); *parameter->temp_motor = get_average(parameter->alpha_temperature, *parameter->temp_motor, temp_motor); *parameter->voltage = get_average(parameter->alpha_voltage, *parameter->voltage, voltage); *parameter->current = get_average(parameter->alpha_current, *parameter->current, current); *parameter->consumption = get_average(parameter->alpha_voltage, *parameter->consumption, consumption); *parameter->rpm = get_average(parameter->alpha_rpm, *parameter->rpm, rpm); *parameter->cell_voltage = *parameter->voltage / *parameter->cell_count; debug("\nOMP M4 (%u) < Rpm: %.0f Volt: %.1f Curr: %.1f Temp esc: %.0f Temp motor: %.0f Cons: %.0f CellV: %.2f", uxTaskGetStackHighWaterMark(NULL), *parameter->rpm, *parameter->voltage, *parameter->current, *parameter->temp_esc, *parameter->temp_motor, *parameter->consumption, *parameter->cell_voltage); } } ================================================ FILE: board/project/sensor/esc_omp_m4.h ================================================ #ifndef ESC_OMP_M4_H #define ESC_OMP_M4_H #include "common.h" typedef struct esc_omp_m4_parameters_t { float rpm_multiplier; float alpha_rpm, alpha_voltage, alpha_current, alpha_temperature; float *rpm, *voltage, *current, *temp_esc, *temp_motor, *cell_voltage, *consumption; uint8_t *cell_count; } esc_omp_m4_parameters_t; extern context_t context; void esc_omp_m4_task(void *parameters); #endif ================================================ FILE: board/project/sensor/esc_openyge.c ================================================ #include "esc_openyge.h" #include #include #include "cell_count.h" #include "pico/stdlib.h" #include "uart.h" #define TIMEOUT_US 2000 #define FRAME_START 0xA5 #define CRC_POLYNOMIAL 0x1021 // CRC-16-CCITT // CRC mode tracking (based on your Arduino code) static crc_mode_t g_crc_mode = CRC_MODE_UNKNOWN; static uint16_t calculate_crc16_with_seed(uint8_t *data, uint8_t length, uint16_t seed); static crc_mode_t detect_crc_mode(uint8_t *frame, uint8_t frame_len); static bool validate_crc(uint8_t *frame, uint8_t frame_len, crc_mode_t mode); static void process(esc_openyge_parameters_t *parameter); void esc_openyge_task(void *parameters) { esc_openyge_parameters_t parameter = *(esc_openyge_parameters_t *)parameters; // Initialize all output variables *parameter.rpm = 0; *parameter.voltage = 0; *parameter.current = 0; *parameter.temperature_fet = 0; *parameter.temperature_bec = 0; *parameter.cell_voltage = 0; *parameter.consumption = 0; *parameter.voltage_bec = 0; *parameter.current_bec = 0; *parameter.throttle = 0; *parameter.pwm_percent = 0; *parameter.cell_count = 1; parameter.crc_errors = 0; parameter.crc_mode = CRC_MODE_UNKNOWN; xTaskNotifyGive(context.receiver_task_handle); #ifdef SIM_SENSORS *parameter.rpm = 12345.67; *parameter.consumption = 123.4; *parameter.voltage = 22.2; *parameter.current = 15.5; *parameter.temperature_fet = 45.6; *parameter.temperature_bec = 38.2; *parameter.cell_voltage = 3.7; *parameter.voltage_bec = 5.1; *parameter.current_bec = 0.8; *parameter.throttle = 75; *parameter.pwm_percent = 80; #endif TaskHandle_t task_handle; uint cell_count_delay = 15000; cell_count_parameters_t cell_count_parameters = {cell_count_delay, parameter.voltage, parameter.cell_count}; xTaskCreate(cell_count_task, "cell_count_task", STACK_CELL_COUNT, (void *)&cell_count_parameters, 1, &task_handle); // Initialize UART for OpenYGE protocol (115200 baud, 8N1, inverted = false) uart1_begin(115200, UART1_TX_GPIO, UART_ESC_RX, TIMEOUT_US, 8, 1, UART_PARITY_NONE, false, false); debug("\nOpenYGE ESC init"); while (1) { ulTaskNotifyTakeIndexed(1, pdTRUE, portMAX_DELAY); process(¶meter); } } static void process(esc_openyge_parameters_t *parameter) { static uint32_t timestamp = 0; uint8_t length = uart1_available(); if (length < 8) { // Minimum frame size return; } uint8_t data[length]; uart1_read_bytes(data, length); debug("\nOpenYGE (%u) < ", uxTaskGetStackHighWaterMark(NULL)); debug_buffer(data, length, "0x%02X "); // Look for frame start in the received data for (int i = 0; i <= length - 8; i++) { // Minimum 8 bytes needed if (data[i] == FRAME_START) { uint8_t *frame = &data[i]; uint8_t frame_len = (i + 4 < length) ? frame[3] : 0; // Check if we have enough data for complete frame if (frame_len == 0 || (i + frame_len) > length || frame_len < 8) { continue; } // Validate frame structure - version 3, type 0 if (frame[1] != 3 || frame[2] != 0 || frame[3] != frame_len) { continue; } // Auto-detect CRC mode if unknown if (parameter->crc_mode == CRC_MODE_UNKNOWN) { crc_mode_t detected_mode = detect_crc_mode(frame, frame_len); if (detected_mode != CRC_MODE_UNKNOWN) { parameter->crc_mode = detected_mode; debug("\nOpenYGE CRC mode auto-detected: %d", detected_mode); } } // Validate CRC using detected or known mode if (parameter->crc_mode != CRC_MODE_UNKNOWN && validate_crc(frame, frame_len, parameter->crc_mode)) { // Parse telemetry data using correct byte positions from your working code // FET Temperature - byte 7, subtract 40 for actual temperature int temp_fet_raw = (int)frame[7] - 40; float temperature_fet = (temp_fet_raw < -40 || temp_fet_raw > 215) ? 0 : temp_fet_raw; // Voltage - bytes 8-9 (little endian), scale by 0.01 uint16_t voltage_raw = (uint16_t)frame[8] | ((uint16_t)frame[9] << 8); float voltage = voltage_raw * 0.01f; // V_SCALE from your code // Current - bytes 10-11 (little endian), scale by 0.01 uint16_t current_raw = (uint16_t)frame[10] | ((uint16_t)frame[11] << 8); float current = current_raw * 0.01f; // A_SCALE from your code // Consumption - bytes 12-13 (little endian), direct mAh uint16_t consumption_raw = (uint16_t)frame[12] | ((uint16_t)frame[13] << 8); // eRPM - bytes 14-15 (little endian), scale by 0.1, then apply pole pairs uint16_t erpm_raw = (uint16_t)frame[14] | ((uint16_t)frame[15] << 8); float erpm = erpm_raw * 0.1f; // ERPM_SCALE from your code float rpm = (erpm / 2.0f) * parameter->rpm_multiplier; // Assuming 2 pole pairs like your code // PWM - byte 16, direct percentage float pwm_percent = frame[16]; // Throttle - byte 17, direct percentage float throttle = frame[17]; // BEC voltage - bytes 18-19 (little endian), in mV uint16_t bec_voltage_raw = (uint16_t)frame[18] | ((uint16_t)frame[19] << 8); float voltage_bec = bec_voltage_raw / 1000.0f; // BEC current - bytes 20-21 (little endian), in mA uint16_t bec_current_raw = (uint16_t)frame[20] | ((uint16_t)frame[21] << 8); float current_bec = bec_current_raw / 1000.0f; // BEC/Cap Temperature - byte 22 or 24, subtract 40 for actual temperature int temp_bec_raw = (int)frame[22] - 40; // Try byte 22 first float temperature_bec = (temp_bec_raw < -40 || temp_bec_raw > 215) ? 0 : temp_bec_raw; // Update outputs with averaging if (parameter->pwm_out) xTaskNotifyGive(context.pwm_out_task_handle); *parameter->rpm = get_average(parameter->alpha_rpm, *parameter->rpm, rpm); *parameter->consumption += get_consumption(*parameter->current, 65535, ×tamp); *parameter->voltage = get_average(parameter->alpha_voltage, *parameter->voltage, voltage); *parameter->current = get_average(parameter->alpha_current, *parameter->current, current); *parameter->temperature_fet = get_average(parameter->alpha_temperature, *parameter->temperature_fet, temperature_fet); *parameter->temperature_bec = get_average(parameter->alpha_temperature, *parameter->temperature_bec, temperature_bec); *parameter->cell_voltage = *parameter->voltage / *parameter->cell_count; *parameter->voltage_bec = voltage_bec; *parameter->current_bec = current_bec; *parameter->throttle = throttle; *parameter->pwm_percent = pwm_percent; debug( "\nOpenYGE (%u) < RPM: %.0f Volt: %.2fV Curr: %.2fA TempFET: %.1f°C " "TempBEC: %.1f°C Cons: %.0fmAh CellV: %.2fV BECVolt: %.2fV BECCurr: %.3fA Thr: %.0f%% PWM: %.0f%%", uxTaskGetStackHighWaterMark(NULL), *parameter->rpm, *parameter->voltage, *parameter->current, *parameter->temperature_fet, *parameter->temperature_bec, *parameter->consumption, *parameter->cell_voltage, *parameter->voltage_bec, *parameter->current_bec, *parameter->throttle, *parameter->pwm_percent); return; // Successfully processed frame } else { parameter->crc_errors++; if (parameter->crc_mode == CRC_MODE_UNKNOWN) { debug("\nOpenYGE CRC mode unknown, trying to detect..."); } else { debug("\nOpenYGE CRC error with mode %d", parameter->crc_mode); } // Debug frame contents for first few errors if (parameter->crc_errors <= 3) { debug(" Frame[%d]: ", frame_len); for (int j = 0; j < frame_len && j < 12; j++) { debug("%02X ", frame[j]); } } } } } } static uint16_t calculate_crc16_with_seed(uint8_t *data, uint8_t length, uint16_t seed) { uint16_t crc = seed; for (uint8_t i = 0; i < length; i++) { crc ^= ((uint16_t)data[i] << 8); for (uint8_t bit = 0; bit < 8; bit++) { if (crc & 0x8000) { crc = (crc << 1) ^ CRC_POLYNOMIAL; } else { crc <<= 1; } } } return crc; } static crc_mode_t detect_crc_mode(uint8_t *frame, uint8_t frame_len) { if (frame_len < 4) return CRC_MODE_UNKNOWN; // Extract CRC from frame in both byte orders uint16_t rx_be = ((uint16_t)frame[frame_len-2] << 8) | frame[frame_len-1]; // Big-endian uint16_t rx_le = ((uint16_t)frame[frame_len-1] << 8) | frame[frame_len-2]; // Little-endian // Try all CRC variants struct { crc_mode_t mode; uint8_t start_offset; uint16_t seed; bool is_big_endian; } variants[] = { {CRC_MODE_INC_SYNC_SEED_FFFF_BE, 0, 0xFFFF, true}, {CRC_MODE_INC_SYNC_SEED_FFFF_LE, 0, 0xFFFF, false}, {CRC_MODE_SKIP_SYNC_SEED_FFFF_BE, 1, 0xFFFF, true}, {CRC_MODE_SKIP_SYNC_SEED_FFFF_LE, 1, 0xFFFF, false}, {CRC_MODE_INC_SYNC_SEED_0000_BE, 0, 0x0000, true}, {CRC_MODE_INC_SYNC_SEED_0000_LE, 0, 0x0000, false}, {CRC_MODE_SKIP_SYNC_SEED_0000_BE, 1, 0x0000, true}, {CRC_MODE_SKIP_SYNC_SEED_0000_LE, 1, 0x0000, false}, }; for (int i = 0; i < 8; i++) { if (frame_len <= (2 + variants[i].start_offset)) continue; uint16_t calc_crc = calculate_crc16_with_seed( frame + variants[i].start_offset, frame_len - 2 - variants[i].start_offset, variants[i].seed ); uint16_t expected_crc = variants[i].is_big_endian ? rx_be : rx_le; if (calc_crc == expected_crc) { debug("\nOpenYGE CRC mode detected: %d", variants[i].mode); return variants[i].mode; } } return CRC_MODE_UNKNOWN; } static bool validate_crc(uint8_t *frame, uint8_t frame_len, crc_mode_t mode) { if (frame_len < 4) return false; uint16_t rx_be = ((uint16_t)frame[frame_len-2] << 8) | frame[frame_len-1]; uint16_t rx_le = ((uint16_t)frame[frame_len-1] << 8) | frame[frame_len-2]; uint8_t start_offset; uint16_t seed; bool is_big_endian; switch (mode) { case CRC_MODE_INC_SYNC_SEED_FFFF_BE: start_offset = 0; seed = 0xFFFF; is_big_endian = true; break; case CRC_MODE_INC_SYNC_SEED_FFFF_LE: start_offset = 0; seed = 0xFFFF; is_big_endian = false; break; case CRC_MODE_SKIP_SYNC_SEED_FFFF_BE: start_offset = 1; seed = 0xFFFF; is_big_endian = true; break; case CRC_MODE_SKIP_SYNC_SEED_FFFF_LE: start_offset = 1; seed = 0xFFFF; is_big_endian = false; break; case CRC_MODE_INC_SYNC_SEED_0000_BE: start_offset = 0; seed = 0x0000; is_big_endian = true; break; case CRC_MODE_INC_SYNC_SEED_0000_LE: start_offset = 0; seed = 0x0000; is_big_endian = false; break; case CRC_MODE_SKIP_SYNC_SEED_0000_BE: start_offset = 1; seed = 0x0000; is_big_endian = true; break; case CRC_MODE_SKIP_SYNC_SEED_0000_LE: start_offset = 1; seed = 0x0000; is_big_endian = false; break; default: return false; } uint16_t calc_crc = calculate_crc16_with_seed( frame + start_offset, frame_len - 2 - start_offset, seed ); uint16_t expected_crc = is_big_endian ? rx_be : rx_le; return calc_crc == expected_crc; } // Temperature function no longer needed - OpenYGE sends direct Celsius values ================================================ FILE: board/project/sensor/esc_openyge.h ================================================ #ifndef ESC_OPENYGE_H #define ESC_OPENYGE_H #include "common.h" typedef enum { CRC_MODE_UNKNOWN = 0, CRC_MODE_INC_SYNC_SEED_FFFF_BE, CRC_MODE_INC_SYNC_SEED_FFFF_LE, CRC_MODE_SKIP_SYNC_SEED_FFFF_BE, CRC_MODE_SKIP_SYNC_SEED_FFFF_LE, CRC_MODE_INC_SYNC_SEED_0000_BE, CRC_MODE_INC_SYNC_SEED_0000_LE, CRC_MODE_SKIP_SYNC_SEED_0000_BE, CRC_MODE_SKIP_SYNC_SEED_0000_LE } crc_mode_t; typedef struct esc_openyge_parameters_t { float rpm_multiplier; bool pwm_out; float alpha_rpm, alpha_voltage, alpha_current, alpha_temperature; float *rpm, *voltage, *current, *temperature_fet, *temperature_bec, *cell_voltage, *consumption; float *voltage_bec, *current_bec, *throttle, *pwm_percent; uint8_t *cell_count; uint32_t crc_errors; crc_mode_t crc_mode; } esc_openyge_parameters_t; extern context_t context; void esc_openyge_task(void *parameters); #endif ================================================ FILE: board/project/sensor/esc_pwm.c ================================================ #include "esc_pwm.h" #include #include "capture_edge.h" #include "esc_hw3.h" #include "hardware/clocks.h" #include "hardware/irq.h" #include "hardware/pio.h" #include "pico/stdlib.h" #define SIGNAL_TIMEOUT_MS 1000 #define INTERVAL_MS 100 #define CLOCK_DIV 1 static volatile bool is_timedout = false; static volatile uint pwm_cycles = 0; static void read(esc_pwm_parameters_t *parameter); static void capture_pin_0_handler(uint counter, edge_type_t edge); static int64_t timeout_callback(alarm_id_t id, void *user_data); void esc_pwm_task(void *parameters) { esc_pwm_parameters_t parameter = *(esc_pwm_parameters_t *)parameters; *parameter.rpm = 0; xTaskNotifyGive(context.receiver_task_handle); gpio_pull_up(PWM_CAPTURE_GPIO); capture_edge_init(pio0, PWM_CAPTURE_GPIO, 1, CLOCK_DIV, PIO0_IRQ_0); capture_edge_set_handler(0, capture_pin_0_handler); while (1) { read(¶meter); debug("\nEsc PWM (%u) < Rpm: %.0f", uxTaskGetStackHighWaterMark(NULL), *parameter.rpm); vTaskDelay(INTERVAL_MS / portTICK_PERIOD_MS); } } static void read(esc_pwm_parameters_t *parameter) { if (is_timedout) { *parameter->rpm = 0; return; } if (pwm_cycles > 1) { float pwm_duration = (float)pwm_cycles / clock_get_hz(clk_sys) * CLOCK_DIV * COUNTER_CYCLES; // seconds float rpm = 60 / pwm_duration * parameter->multiplier; *parameter->rpm = rpm; // get_average(parameter->alpha / 100.0F, *parameter->rpm, rpm); } #ifdef SIM_SENSORS *parameter->rpm = 12345.67; #endif } static void capture_pin_0_handler(uint counter, edge_type_t edge) { static uint counter_edge_rise = 0, counter_previous = 0; static alarm_id_t timeout_alarm_id = 0; if (timeout_alarm_id) cancel_alarm(timeout_alarm_id); is_timedout = false; if (edge == EDGE_RISE) { pwm_cycles = counter - counter_edge_rise; counter_edge_rise = counter; // printf("\nPwm cycles: %u", pwm_cycles); } timeout_alarm_id = add_alarm_in_ms(SIGNAL_TIMEOUT_MS, timeout_callback, NULL, false); } static int64_t timeout_callback(alarm_id_t id, void *parameters) { is_timedout = true; debug("\nEsc PWM signal timeout. Rpm: 0"); return 0; } ================================================ FILE: board/project/sensor/esc_pwm.h ================================================ #ifndef ESC_PWM_H #define ESC_PWM_H #include "esc_hw3.h" typedef esc_hw3_parameters_t esc_pwm_parameters_t; void esc_pwm_task(void *parameters); #endif ================================================ FILE: board/project/sensor/esc_ztw.c ================================================ #include "esc_ztw.h" #include #include "cell_count.h" #include "pico/stdlib.h" #include "uart.h" #define ZTW_TIMEOUT_US 1000 #define ZTW_PACKET_LENGHT 32 #define ZTW_PACKET_HEADER 0xDD typedef struct esc_ztw_packet_t { uint8_t header; uint8_t version; uint8_t length; uint16_t voltage; uint16_t current; uint8_t throttle; uint16_t rpm; uint8_t temp_esc; uint8_t temp_motor; uint8_t pwm; uint16_t status; uint16_t consumption; uint8_t serial_throttle; uint8_t can_throttle; uint8_t bec_voltage; uint8_t unused[29 - 20 + 1]; uint16_t crc; } __attribute((packed)) esc_ztw_packet_t; static void process(esc_ztw_parameters_t *parameter); void esc_ztw_task(void *parameters) { esc_ztw_parameters_t parameter = *(esc_ztw_parameters_t *)parameters; *parameter.rpm = 0; *parameter.voltage = 0; *parameter.bec_voltage = 0; *parameter.current = 0; *parameter.temp_esc = 0; *parameter.temp_motor = 0; *parameter.cell_voltage = 0; *parameter.consumption = 0; *parameter.cell_count = 1; xTaskNotifyGive(context.receiver_task_handle); #ifdef SIM_SENSORS *parameter.temp_esc = 12.34; *parameter.temp_motor = 23.45; *parameter.voltage = 12.34; *parameter.bec_voltage = 4.56; *parameter.current = 12.34; *parameter.consumption = 12.34; *parameter.rpm = 12345.67; *parameter.cell_voltage = 3.75; #endif TaskHandle_t task_handle; uint cell_count_delay = 15000; cell_count_parameters_t cell_count_parameters = {cell_count_delay, parameter.voltage, parameter.cell_count}; xTaskCreate(cell_count_task, "cell_count_task", STACK_CELL_COUNT, (void *)&cell_count_parameters, 1, &task_handle); uart1_begin(115200, UART1_TX_GPIO, UART_ESC_RX, ZTW_TIMEOUT_US, 8, 1, UART_PARITY_NONE, false, false); while (1) { ulTaskNotifyTakeIndexed(1, pdTRUE, portMAX_DELAY); process(¶meter); } } static void process(esc_ztw_parameters_t *parameter) { static uint32_t timestamp = 0; uint8_t lenght = uart1_available(); if (lenght == ZTW_PACKET_LENGHT) { esc_ztw_packet_t packet; uart1_read_bytes((uint8_t *)&packet, ZTW_PACKET_LENGHT); if (packet.header != ZTW_PACKET_HEADER) return; float temp_esc = packet.temp_esc; float temp_motor = packet.temp_motor; float voltage = swap_16(packet.voltage) / 10.0; float bec_voltage = packet.bec_voltage / 10.0; float current = swap_16(packet.current) / 10.0; float consumption = swap_16(packet.consumption); float rpm = swap_16(packet.rpm) * 10.0; rpm *= parameter->rpm_multiplier; *parameter->temp_esc = get_average(parameter->alpha_temperature, *parameter->temp_esc, temp_esc); *parameter->temp_motor = get_average(parameter->alpha_temperature, *parameter->temp_motor, temp_motor); *parameter->voltage = get_average(parameter->alpha_voltage, *parameter->voltage, voltage); *parameter->bec_voltage = get_average(parameter->alpha_voltage, *parameter->bec_voltage, bec_voltage); *parameter->current = get_average(parameter->alpha_current, *parameter->current, current); *parameter->consumption = get_average(parameter->alpha_voltage, *parameter->consumption, consumption); *parameter->rpm = get_average(parameter->alpha_rpm, *parameter->rpm, rpm); *parameter->cell_voltage = *parameter->voltage / *parameter->cell_count; debug("\nZTW (%u) < Rpm: %.0f Volt: %.1f Curr: %.1f Volt BEC: %.1f Temp esc: %.0f Temp motor: %.0f Cons: %.0f CellV: %.2f", uxTaskGetStackHighWaterMark(NULL), *parameter->rpm, *parameter->voltage, *parameter->current, *parameter->bec_voltage, *parameter->temp_esc, *parameter->temp_motor, *parameter->consumption, *parameter->cell_voltage); } } ================================================ FILE: board/project/sensor/esc_ztw.h ================================================ #ifndef ESC_ZTW_H #define ESC_ZTW_H #include "common.h" typedef struct esc_ztw_parameters_t { float rpm_multiplier; float alpha_rpm, alpha_voltage, alpha_current, alpha_temperature; float *rpm, *voltage, *current, *temp_esc, *temp_motor, *bec_voltage, *cell_voltage, *consumption; uint8_t *cell_count; } esc_ztw_parameters_t; extern context_t context; void esc_ztw_task(void *parameters); #endif ================================================ FILE: board/project/sensor/fuel_meter.c ================================================ #include "fuel_meter.h" #include #include "capture_edge.h" #include "hardware/clocks.h" #include "hardware/irq.h" #include "hardware/pio.h" #include "pico/stdlib.h" #define INSTANT_INTERVAL_MS 100 #define CLOCK_DIV 5 static volatile uint pwm_cycles_instant = 0; static volatile uint pwm_cycles_total = 0; static void read(fuel_meter_parameters_t *parameter); static void capture_pin_0_handler(uint counter, edge_type_t edge); void fuel_meter_task(void *parameters) { fuel_meter_parameters_t parameter = *(fuel_meter_parameters_t *)parameters; *parameter.consumption_instant = 0; // ml/min *parameter.consumption_total = 0; // ml xTaskNotifyGive(context.receiver_task_handle); gpio_pull_up(FUELMETER_CAPTURE_GPIO); capture_edge_init(pio0, FUELMETER_CAPTURE_GPIO, 1, CLOCK_DIV, PIO0_IRQ_0); capture_edge_set_handler(0, capture_pin_0_handler); while (1) { read(¶meter); debug("\nFuel sensor (%u) < ml/min %.3f ml %.3f ml/pulse %.3f pulses %u", uxTaskGetStackHighWaterMark(NULL), *parameter.consumption_instant, *parameter.consumption_total, parameter.ml_per_pulse, pwm_cycles_total); vTaskDelay(INSTANT_INTERVAL_MS / portTICK_PERIOD_MS); } } static void read(fuel_meter_parameters_t *parameter) { *parameter->consumption_total = pwm_cycles_total * parameter->ml_per_pulse; // ml *parameter->consumption_instant = pwm_cycles_instant * parameter->ml_per_pulse * 1000 / INSTANT_INTERVAL_MS * 60; // ml/min pwm_cycles_instant = 0; #ifdef SIM_SENSORS *parameter->consumption_instant = 12.23; *parameter->consumption_total = 1223; #endif } static void capture_pin_0_handler(uint counter, edge_type_t edge) { if (edge == EDGE_RISE) { pwm_cycles_total++; pwm_cycles_instant++; } } ================================================ FILE: board/project/sensor/fuel_meter.h ================================================ #ifndef FUEL_METER_H #define FUEL_METER_H #include "common.h" typedef struct fuel_meter_parameters_t { float ml_per_pulse; //float alpha; float *consumption_instant; // ml/min float *consumption_total; // ml } fuel_meter_parameters_t; extern context_t context; void fuel_meter_task(void *parameters); #endif ================================================ FILE: board/project/sensor/gpio.c ================================================ #include "gpio.h" #include #include "pico/stdlib.h" #define GPIO_MASK (1 << 17) void gpio_task(void *parameters) { gpio_parameters_t parameter = *(gpio_parameters_t *)parameters; gpio_init_mask(GPIO_MASK); gpio_set_dir_in_masked(GPIO_MASK); xTaskNotifyGive(context.receiver_task_handle); while (1) { uint8_t value = 0; for (uint i = 0; i < 6; i++) { if (parameter.mask & (1 << i)) value |= gpio_get(i + 17) << i; } #ifdef SIM_SENSORS value = 0B101; #endif *parameter.value = value; debug("\nGPIO (%u): 0x%X", uxTaskGetStackHighWaterMark(NULL), value); vTaskDelay(parameter.interval / portTICK_PERIOD_MS); } } ================================================ FILE: board/project/sensor/gpio.h ================================================ #ifndef GPIO_SENSOR_H #define GPIO_SENSOR_H #include "common.h" typedef struct gpio_parameters_t { uint8_t mask; uint16_t interval; uint8_t *value; } gpio_parameters_t; extern context_t context; void gpio_task(void *parameters); #endif ================================================ FILE: board/project/sensor/gps.c ================================================ #include "gps.h" #include #include #include #include "distance.h" #include "pico/stdlib.h" #include "stdlib.h" #include "uart_pio.h" #include "vspeed.h" #define COMMAND_GGA 0 #define COMMAND_RMC 1 #define COMMAND_GSA 2 #define COMMAND_UNK 3 #define COMMAND_VTG 4 #define COMMAND_GLL 5 #define NMEA_LON 1 #define NMEA_ALT 2 #define NMEA_SPD 3 #define NMEA_COG 4 #define NMEA_FIX 5 #define NMEA_SAT 6 #define NMEA_DATE 7 #define NMEA_TIME 8 #define NMEA_LAT_SIGN 9 #define NMEA_LON_SIGN 10 #define NMEA_END 11 #define NMEA_LAT 12 #define NMEA_GSA_FIX 13 // 1,2,3 #define NMEA_GSA_PDOP 14 #define NMEA_GSA_HDOP 15 #define NMEA_GSA_VDOP 16 #define TIMEOUT_US 5000 #define VSPEED_INTERVAL_MS 2000 typedef struct ublox_msg_info_t { uint8_t class; uint8_t id; uint16_t len; } __attribute__((packed)) ublox_msg_info_t; typedef struct ublox_navpvt_t { uint32_t iTOW; uint16_t year; uint8_t month; uint8_t day; uint8_t hour; uint8_t min; uint8_t sec; uint8_t valid; uint32_t tAcc; int32_t nano; uint8_t fixType; uint8_t flags; uint8_t flags2; uint8_t numSV; int32_t lon; int32_t lat; int32_t height; // mm int32_t hMSL; uint32_t hAcc; // mm uint32_t vAcc; // mm int32_t velN; // mm/s int32_t velE; // mm/s int32_t velD; // mm/s int32_t gSpeed; // mm/s int32_t headMot; uint32_t sAcc; // mm uint32_t headAcc; // deg * 10000 uint16_t pDOP; uint16_t flags3; uint32_t reserved1; int32_t headVeh; int16_t magDec; uint16_t magAcc; uint8_t crc_a; uint8_t crc_b; } __attribute__((packed)) ublox_navpvt_t; typedef struct ublox_navdop_t { uint32_t iTOW; uint16_t gDOP; // DOP * 100 uint16_t pDOP; // DOP * 100 uint16_t tDOP; // DOP * 100 uint16_t vDOP; // DOP * 100 uint16_t hDOP; // DOP * 100 uint16_t nDOP; // DOP * 100 uint16_t eDOP; // DOP * 100 uint8_t crc_a; uint8_t crc_b; } __attribute__((packed)) ublox_navdop_t; typedef struct alarm_parameters_t { bool is_ublox; uint rate; } alarm_parameters_t; // static alarm_id_t alarm_id_ublox = 0, alarm_id_nmea = 0; // static alarm_parameters_t alarm_parameters; static void process(gps_parameters_t *parameter); static void parser(uint8_t nmea_cmd, uint8_t cmd_field, uint8_t *buffer, gps_parameters_t *parameter); static void send_ublox_message(uint8_t *buf, uint len); static void set_baudrate(uint baudrate); static void set_nmea_config(uint rate); static void set_ublox_config(uint rate); static void nmea_msg(char *cmd, bool enable); static void ubx_cfg_msg(uint8_t class, uint8_t id, bool enable); static void ubx_cfg_rate(uint16_t rate); static void ubx_cfg_cfg(void); static bool set_home_altitude(uint fix_type); // static int64_t alarm_nmea_timeout(alarm_id_t id, void *parameters); // static int64_t alarm_ublox_timeout(alarm_id_t id, void *parameters); void gps_task(void *parameters) { gps_parameters_t parameter = *(gps_parameters_t *)parameters; xTaskNotifyGive(context.receiver_task_handle); *parameter.lat = 0; *parameter.lon = 0; *parameter.alt = 0; *parameter.spd = 0; *parameter.cog = 0; *parameter.hdop = 0; *parameter.vdop = 0; *parameter.sat = 0; *parameter.time = 0; *parameter.date = 0; *parameter.vspeed = 0; *parameter.dist = 0; *parameter.spd_kmh = 0; *parameter.fix = 0; // raw fix: nmea or ublox *parameter.fix_type = 0; // internal fix MSRC. 0 no fix, 1 2D fix, 2 3D fix *parameter.home_set = false; *parameter.n_vel = 0; *parameter.e_vel = 0; *parameter.v_vel = 0; *parameter.speed_acc = 0; *parameter.track_acc = 0; *parameter.alt_elipsiod = 0; *parameter.h_acc = 0; *parameter.v_acc = 0; *parameter.alt_home = 0; #ifdef SIM_SENSORS *parameter.lat = 123.456789; // deg * 1e7 11º32'45.67" +N, -S *parameter.lon = -123.456789; //-deg + 10e7 1251.964833333; // 20º51'57.89" +E, -W *parameter.alt = 1283; // m *parameter.spd = 158; // kts *parameter.cog = 123.45; // º *parameter.sat = 10; // *parameter.date = 141012; // yymmdd *parameter.time = 162302; // hhmmss *parameter.hdop = 12.35; // *parameter.dist = 1000; *parameter.spd_kmh = 123; #endif TaskHandle_t task_handle; static distance_parameters_t parameters_distance; parameters_distance.distance = parameter.dist; parameters_distance.altitude = parameter.alt; parameters_distance.sat = parameter.sat; parameters_distance.latitude = parameter.lat; parameters_distance.longitude = parameter.lon; parameters_distance.fix = parameter.fix_type; parameters_distance.hdop = parameter.hdop; parameters_distance.home_set = parameter.home_set; xTaskCreate(distance_task, "distance_task", STACK_DISTANCE, (void *)¶meters_distance, 2, &task_handle); /* Change GPS config. For ublox compatible devices */ vTaskDelay(1000 / portTICK_PERIOD_MS); set_baudrate(parameter.baudrate); uart_pio_begin(parameter.baudrate, UART_TX_PIO_GPIO, UART_RX_PIO_GPIO, TIMEOUT_US, pio0, PIO0_IRQ_1, 8, 1, UART_PARITY_NONE); if (parameter.protocol == UBLOX) set_ublox_config(parameter.rate); else set_nmea_config(parameter.rate); // alarm_parameters.is_ublox = true; // alarm_id_ublox = add_alarm_in_ms(20 * 1000L, alarm_ublox_timeout, &alarm_parameters, false); while (1) { ulTaskNotifyTakeIndexed(1, pdTRUE, portMAX_DELAY); process(¶meter); } } static void process(gps_parameters_t *parameter) { static char buffer[20] = {0}; if (parameter->protocol == NMEA) { static uint8_t nmea_cmd = 0; static uint8_t cmd_field = 0; while (uart_pio_available()) { static uint8_t data_pos = 0; char data = uart_pio_read(); switch (data) { case '$': cmd_field = 0; data_pos = 0; nmea_cmd = COMMAND_UNK; break; case '\r': // If we were ignoring checksum, reset cleanly if (cmd_field == 255) { cmd_field = 0; data_pos = 0; nmea_cmd = COMMAND_UNK; break; } case ',': if (cmd_field == 255) { // ignoring checksum data_pos = 0; buffer[0] = 0; break; } if (cmd_field == 0) { if (memcmp(buffer + 2, "GGA", 3) == 0) { nmea_cmd = COMMAND_GGA; } else if (memcmp(buffer + 2, "RMC", 3) == 0) { nmea_cmd = COMMAND_RMC; } else if (memcmp(buffer + 2, "GSA", 3) == 0) { nmea_cmd = COMMAND_GSA; } if (nmea_cmd != COMMAND_UNK) { // cancel_alarm(alarm_id_nmea); // alarm_id_nmea = add_alarm_in_ms(2000, alarm_nmea_timeout, &alarm_parameters, false); debug("\nGPS (%u) < %s(%i): ", uxTaskGetStackHighWaterMark(NULL), buffer + 2, nmea_cmd); } } else { if (nmea_cmd != COMMAND_UNK) parser(nmea_cmd, cmd_field, buffer, parameter); } cmd_field++; data_pos = 0; buffer[0] = 0; break; case '\n': break; case '*': // End current field; ignore checksum bytes until '\r' if (nmea_cmd != COMMAND_UNK && cmd_field != 0) { parser(nmea_cmd, cmd_field, (uint8_t *)buffer, parameter); } cmd_field = 255; // special: ignore until '\r' data_pos = 0; buffer[0] = 0; break; default: if (data_pos < 19) { buffer[data_pos] = data; data_pos++; buffer[data_pos] = 0; } } } } else { while (uart_pio_available()) { while (uart_pio_available() && uart_pio_read() != 0xB5) ; while (uart_pio_available() && uart_pio_read() != 0x62) ; if (uart_pio_available() < sizeof(ublox_msg_info_t)) return; ublox_msg_info_t msg_info; uart_pio_read_bytes((uint8_t *)&msg_info, sizeof(ublox_msg_info_t)); debug("\nGPS UBLOX MSG. Class: %u Id: %u Len: %u Avail: %u", msg_info.class, msg_info.id, msg_info.len, uart_pio_available()); if (uart_pio_available() < msg_info.len + 2) return; if (msg_info.class == 0x01 && msg_info.id == 0x07 && msg_info.len == sizeof(ublox_navpvt_t) - 2) { // cancel_alarm(alarm_id_ublox); // alarm_id_ublox = add_alarm_in_ms(2000, alarm_ublox_timeout, &alarm_parameters, false); ublox_navpvt_t navpvt; uart_pio_read_bytes((uint8_t *)&navpvt, sizeof(ublox_navpvt_t)); *parameter->alt = navpvt.hMSL / 1000.0F; *parameter->lat = navpvt.lat * 1.0e-7; *parameter->lon = navpvt.lon * 1.0e-7; *parameter->alt = navpvt.hMSL / 1000.0F; *parameter->cog = navpvt.headMot / 100000.0F; *parameter->sat = navpvt.numSV; *parameter->time = navpvt.hour * 10000L + navpvt.min * 100 + navpvt.sec; *parameter->date = navpvt.day * 10000L + navpvt.month * 100 + (navpvt.year - 2000); *parameter->vspeed = -navpvt.velD / 1000.0F; *parameter->spd_kmh = navpvt.gSpeed * 3600.0F / 1000000.0F; *parameter->spd = navpvt.gSpeed * 0.001943844F; // 1 mm/s = 0.001943844 Knot uint8_t fix = navpvt.fixType; *parameter->fix = fix; if (fix == 2) *parameter->fix_type = 1; // 2D else if (fix == 3 || fix == 4) *parameter->fix_type = 2; // 3D else *parameter->fix_type = 0; // no fix if (!(navpvt.flags & 0x01)) *parameter->fix_type = 0; *parameter->n_vel = navpvt.velN / 1000.0F; *parameter->e_vel = navpvt.velE / 1000.0F; *parameter->v_vel = -navpvt.velD / 1000.0F; *parameter->speed_acc = navpvt.sAcc / 1000.0F; *parameter->track_acc = navpvt.headAcc * 1e-5f; *parameter->alt_elipsiod = navpvt.height / 1000.0F; *parameter->h_acc = navpvt.hAcc / 1000.0F; *parameter->v_acc = navpvt.vAcc / 1000.0F; *parameter->pdop = navpvt.pDOP / 100.0F; if (set_home_altitude(*parameter->fix_type)) { *parameter->alt_home = *parameter->alt; } debug( "\nGPS (%u) < NAV-PTV: Date: %.0f Time: %.0f Fix: %.0f Sat: %.0f Lon: %.5f Lat: %.5f Alt: %.2f " "Vspeed: %.2f Speed: mm/s %i knots %.2f kmh %.2f Pdop: %.2f, Alt home: %.2f", uxTaskGetStackHighWaterMark(NULL), *parameter->date, *parameter->time, *parameter->fix, *parameter->sat, *parameter->lon, *parameter->lat, *parameter->alt, *parameter->vspeed, navpvt.gSpeed, *parameter->spd, *parameter->spd_kmh, *parameter->pdop, *parameter->alt_home); } else if (msg_info.class == 0x01 && msg_info.id == 0x04 && msg_info.len == sizeof(ublox_navdop_t) - 2) { // cancel_alarm(alarm_id_ublox); // alarm_id_ublox = add_alarm_in_ms(2000, alarm_ublox_timeout, &alarm_parameters, false); ublox_navdop_t navdop; uart_pio_read_bytes((uint8_t *)&navdop, sizeof(ublox_navdop_t)); *parameter->hdop = navdop.hDOP / 100.0F; *parameter->vdop = navdop.vDOP / 100.0F; debug("\nGPS (%u) < NAV-DOP: h: %.2f v: %.2f", uxTaskGetStackHighWaterMark(NULL), *parameter->hdop, *parameter->vdop); } } } } static bool set_home_altitude(uint fix_type) { static bool altitude_offset_set = false; if (!altitude_offset_set) { static uint cont = 0; if (fix_type == 2) { cont++; if (cont > 5) { altitude_offset_set = true; return true; } } else cont = 0; } return false; } static void parser(uint8_t nmea_cmd, uint8_t cmd_field, uint8_t *buffer, gps_parameters_t *parameter) { #define NMEA_MAX_FIELDS 18 // 0..17 (field 1..17 plus dummy 0) uint8_t nmea_field[3][NMEA_MAX_FIELDS] = { // GGA: time, lat, N/S, lon, E/W, fix, sats, hdop, alt {0, NMEA_TIME, NMEA_LAT, NMEA_LAT_SIGN, NMEA_LON, NMEA_LON_SIGN, NMEA_FIX, NMEA_SAT, 0, NMEA_ALT, 0, 0, 0, 0, 0, 0, 0, 0}, // RMC: time, status, lat, N/S, lon, E/W, spd, cog, date {0, NMEA_TIME, 0, NMEA_LAT, NMEA_LAT_SIGN, NMEA_LON, NMEA_LON_SIGN, NMEA_SPD, NMEA_COG, NMEA_DATE, 0, 0, 0, 0, 0, 0, 0, 0}, // GSA: mode1, fix, sat1..sat12, PDOP, HDOP, VDOP {0, 0, NMEA_GSA_FIX, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // sat1..sat12 ignored (fields 3..14) NMEA_GSA_PDOP, NMEA_GSA_HDOP, NMEA_GSA_VDOP}}; static int8_t lat_dir = 1, lon_dir = 1; static uint32_t timestamp_vspeed = 0, timestamp_dist = 0; if (strlen(buffer)) { if (nmea_field[nmea_cmd][cmd_field] == NMEA_TIME) { *parameter->time = atof(buffer); } else if (nmea_field[nmea_cmd][cmd_field] == NMEA_LAT) { char degrees[3] = {0}; float minutes = 0; strncpy(degrees, buffer, 2); minutes = atof(buffer + 2); *parameter->lat = atoi(degrees) + minutes / 60; } else if (nmea_field[nmea_cmd][cmd_field] == NMEA_LON) { char degrees[4] = {0}; float minutes = 0; strncpy(degrees, buffer, 3); minutes = atof(buffer + 3); *parameter->lon = atoi(degrees) + minutes / 60; } else if (nmea_field[nmea_cmd][cmd_field] == NMEA_ALT) { *parameter->alt = atof(buffer); get_vspeed_gps(parameter->vspeed, *parameter->alt, VSPEED_INTERVAL_MS); if (set_home_altitude(*parameter->fix_type)) { *parameter->alt_home = *parameter->alt; } debug("(alt home %.2f),", *parameter->alt_home); } else if (nmea_field[nmea_cmd][cmd_field] == NMEA_SPD) { *parameter->spd = atof(buffer); *parameter->spd_kmh = *parameter->spd * 1.852; } else if (nmea_field[nmea_cmd][cmd_field] == NMEA_COG) { *parameter->cog = atof(buffer); } else if (nmea_field[nmea_cmd][cmd_field] == NMEA_DATE) { *parameter->date = atof(buffer); } else if (nmea_field[nmea_cmd][cmd_field] == NMEA_SAT) { *parameter->sat = atof(buffer); } else if (nmea_field[nmea_cmd][cmd_field] == NMEA_LAT_SIGN) { lat_dir = (buffer[0] == 'N') ? 1 : -1; *parameter->lat = fabsf(*parameter->lat) * lat_dir; } else if (nmea_field[nmea_cmd][cmd_field] == NMEA_LON_SIGN) { lon_dir = (buffer[0] == 'E') ? 1 : -1; *parameter->lon = fabsf(*parameter->lon) * lon_dir; } else if (nmea_field[nmea_cmd][cmd_field] == NMEA_GSA_HDOP) { *parameter->hdop = atof(buffer); } else if (nmea_field[nmea_cmd][cmd_field] == NMEA_GSA_VDOP) { *parameter->vdop = atof(buffer); } else if (nmea_field[nmea_cmd][cmd_field] == NMEA_GSA_PDOP) { *parameter->pdop = atof(buffer); } else if (nmea_field[nmea_cmd][cmd_field] == NMEA_GSA_FIX) { uint fix = atoi((char *)buffer); if (fix >= 1 && fix <= 3) { *parameter->fix = fix; if (fix == 2) *parameter->fix_type = 1; // 2D else if (fix == 3) *parameter->fix_type = 2; // 3D else *parameter->fix_type = 0; // no fix } } debug("%s(%i),", buffer, nmea_field[nmea_cmd][cmd_field]); } } static void set_ublox_config(uint rate) { nmea_msg("GLL", false); nmea_msg("GSV", false); nmea_msg("GSA", false); nmea_msg("VTG", false); nmea_msg("ZDA", false); nmea_msg("GGA", false); nmea_msg("RMC", false); ubx_cfg_msg(0x01, 0x07, true); // Enable message UBX-NAV-PVT ubx_cfg_msg(0x01, 0x04, true); // Enable message UBX-NAV-DOP ubx_cfg_rate(rate); // Set messages rate (UBX-CFG-RATE (0x06 0x08)) ubx_cfg_cfg(); // Save changes } static void set_nmea_config(uint rate) { nmea_msg("GLL", false); nmea_msg("GSA", true); nmea_msg("GSV", false); nmea_msg("VTG", false); nmea_msg("ZDA", false); nmea_msg("GGA", true); nmea_msg("RMC", true); ubx_cfg_msg(0x01, 0x07, false); // Disable message UBX-NAV-PVT ubx_cfg_msg(0x01, 0x04, false); // Disable message UBX-NAV-DOP ubx_cfg_rate(rate); // Set messages rate (UBX-CFG-RATE (0x06 0x08)) ubx_cfg_cfg(); // Save changes } static void set_baudrate(uint baudrate) { char msg[300]; uint baudrates[] = {115200, 57600, 38400, 9600}; sprintf(msg, "$PUBX,41,1,3,3,%u,0\r\n", baudrate); for (uint i = 0; i < sizeof(baudrates) / sizeof(uint); i++) { uart_pio_begin(baudrates[i], UART_TX_PIO_GPIO, UART_RX_PIO_GPIO, TIMEOUT_US, pio0, PIO0_IRQ_1, 8, 1, UART_PARITY_NONE); vTaskDelay(10 / portTICK_PERIOD_MS); uart_pio_write_bytes(msg, strlen(msg)); vTaskDelay(200 / portTICK_PERIOD_MS); uart_pio_remove(); } } static void nmea_msg(char *cmd, bool enable) { char msg[30]; sprintf(msg, "$PUBX,40,%s,0,%u,0,0,0,0\r\n", cmd, enable); uart_pio_write_bytes(msg, strlen(msg)); } static void ubx_cfg_msg(uint8_t class, uint8_t id, bool enable) { uint8_t msg[] = {0x06, 0x01, 0x03, 0x00, class, id, enable}; send_ublox_message(msg, sizeof(msg)); } static void ubx_cfg_rate(uint16_t rate) { uint16_t ms = 1000 / rate; uint8_t msg[] = {0x06, 0x08, 0x06, 0x00, ms, ms >> 8, 0x01, 0x00, 0x01, 0x00}; send_ublox_message(msg, sizeof(msg)); } static void ubx_cfg_cfg(void) { uint8_t msg[] = {0x06, 0x09, 0x0D, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03}; send_ublox_message(msg, sizeof(msg)); } static inline void send_ublox_message(uint8_t *buf, uint len) { uint8_t a = 0, b = 0; uart_pio_write(0xB5); uart_pio_write(0x62); for (uint i = 0; i < len; i++) { a += buf[i]; b += a; uart_pio_write(buf[i]); } uart_pio_write(a); uart_pio_write(b); } /*static int64_t alarm_nmea_timeout(alarm_id_t id, void *parameters) { alarm_parameters_t *parameter; parameter = (alarm_parameters_t *)parameters; parameter->is_ublox = true; set_ublox_config(parameter->rate); return 10000 * 1000L; } static int64_t alarm_ublox_timeout(alarm_id_t id, void *parameters) { alarm_parameters_t *parameter; parameter = (alarm_parameters_t *)parameters; parameter->is_ublox = false; //set_nmea_config(parameter->rate); set_ublox_config(parameter->rate); return 10000 * 1000L; }*/ ================================================ FILE: board/project/sensor/gps.h ================================================ #ifndef GPS_H #define GPS_H #include "common.h" typedef struct gps_parameters_t { gps_protocol_t protocol; uint baudrate, rate; float *lat, *lon; float *alt, *spd, *cog, *hdop, *sat, *time, *date, *vspeed, *dist, *spd_kmh, *fix, *vdop, *speed_acc, *h_acc, *v_acc, *track_acc, *n_vel, *e_vel, *v_vel, *alt_elipsiod, *pdop, *alt_home; uint8_t *fix_type; uint8_t *home_set; } gps_parameters_t; extern context_t context; void gps_task(void *parameters); #endif ================================================ FILE: board/project/sensor/ina3221.c ================================================ #include "ina3221.h" #include #include "hardware/i2c.h" #include "pico/stdlib.h" // REGISTERS #define INA3221_CONFIGURATION (0x00) #define INA3221_SHUNT_VOLTAGE(x) (0x01 + (x * 2)) #define INA3221_BUS_VOLTAGE(x) (0x02 + (x * 2)) #define INA3221_CRITICAL_ALERT(x) (0x07 + (x * 2)) #define INA3221_WARNING_ALERT(x) (0x08 + (x * 2)) #define INA3221_SHUNT_VOLTAGE_SUM (0x0D) #define INA3221_SHUNT_VOLTAGE_LIMIT (0x0E) #define INA3221_MASK_ENABLE (0x0F) #define INA3221_POWER_VALID_UPPER (0x10) #define INA3221_POWER_VALID_LOWER (0x11) #define INA3221_MANUFACTURER (0xFE) #define INA3221_DIE_ID (0xFF) #define MODE_VOLTAGE_CONTINUOUS 0x110 // Bus continuous #define VOLTAGE_CONVERSION_TIME (0x11 << 6) // 588us //#define AVG 0x7 // 128 samples //#define CH 0x7 // Enable all channels #define RST 0x8000 // Reset bit #define I2C_ADDRESS 0x40 #define SENSOR_INTERVAL_MS 20 // 10ms min for 1024 filter. 1ms min for 0B11 filter static void begin(ina3221_parameters_t *parameter); static void read(ina3221_parameters_t *parameter); void ina3221_task(void *parameters) { ina3221_parameters_t parameter = *(ina3221_parameters_t *)parameters; xTaskNotifyGive(context.receiver_task_handle); for (uint8_t i = 0; i < parameter.cell_count; i++) { *parameter.cell[i] = 0; } vTaskDelay(500 / portTICK_PERIOD_MS); begin(¶meter); while (1) { read(¶meter); debug("\nINA3221 (%u) < Addr: 0x%02X", uxTaskGetStackHighWaterMark(NULL), parameter.i2c_address); for (uint8_t i = 0; i < parameter.cell_count; i++) { debug(" Cell %u: %.2fV", i + 1, *parameter.cell[i]); } vTaskDelay(SENSOR_INTERVAL_MS / portTICK_PERIOD_MS); } } static void begin(ina3221_parameters_t *parameter) { i2c_init(i2c0, 100 * 1000); gpio_set_function(I2C0_SDA_GPIO, GPIO_FUNC_I2C); gpio_set_function(I2C0_SCL_GPIO, GPIO_FUNC_I2C); gpio_pull_up(I2C0_SDA_GPIO); gpio_pull_up(I2C0_SCL_GPIO); uint8_t data[2] = {0}; vTaskDelay(1000 / portTICK_PERIOD_MS); while (i2c_write_blocking_until(i2c0, parameter->i2c_address, data, 1, false, make_timeout_time_ms(1000)) == PICO_ERROR_GENERIC) { debug("\nINA3221 not found at address 0x%02X. Connect and reboot", parameter->i2c_address); vTaskDelay(1000 / portTICK_PERIOD_MS); } // Configure sensor data[0] = INA3221_CONFIGURATION; data[1] = MODE_VOLTAGE_CONTINUOUS | VOLTAGE_CONVERSION_TIME | (parameter->filter << 9); if (parameter->cell_count > 3) parameter->cell_count = 3; if (parameter->cell_count < 1) parameter->cell_count = 1; for (uint8_t i = 1; i < parameter->cell_count; i++) { data[1] |= (1 << (i + 12)); } i2c_write_blocking(i2c0, parameter->i2c_address, data, 2, false); } static void read(ina3221_parameters_t *parameter) { uint8_t data[3]; int16_t bus_voltage; float cell_total[3] = {0}; for (uint8_t channel = 0; channel < parameter->cell_count; channel++) { // Read bus voltage data[0] = INA3221_BUS_VOLTAGE(channel); i2c_write_blocking(i2c0, parameter->i2c_address, data, 1, true); i2c_read_blocking(i2c0, parameter->i2c_address, data, 2, false); bus_voltage = ((int16_t)data[0] << 8) | data[1]; // Calculate voltage in volts cell_total[channel] = (float)bus_voltage * 0.001f; // Bus voltage LSB = 1mV #ifdef SIM_SENSORS *parameter->cell[channel] = 3 + channel * 0.01; #endif } for (uint8_t i = 0; i < parameter->cell_count; i++) { if (i == 0) { *parameter->cell[i] = cell_total[i] - *parameter->cell_prev; } else { *parameter->cell[i] = cell_total[i] - cell_total[i - 1]; } } } ================================================ FILE: board/project/sensor/ina3221.h ================================================ #ifndef INA3221_H #define INA3221_H #include "common.h" typedef struct ina3221_parameters_t { uint8_t i2c_address; uint8_t filter; uint8_t conversion_time; uint8_t cell_count; float *cell[3]; float *cell_prev; } ina3221_parameters_t; extern context_t context; void ina3221_task(void *parameters); #endif ================================================ FILE: board/project/sensor/mpu6050.c ================================================ #include "mpu6050.h" #include #include #include "hardware/i2c.h" #include "pico/stdlib.h" #define REGISTER_PWR_MGMT_1 0x6B #define REGISTER_SMPLRT_DIV 0x19 #define REGISTER_CONFIG 0x1A #define REGISTER_GYRO_CONFIG 0x1B #define REGISTER_ACCEL_CONFIG 0x1C #define REGISTER_INT_ENABLE 0x38 #define REGISTER_ACCEL_XOUT_H 0x3B #define REGISTER_ACCEL_XOUT_L 0x3C #define REGISTER_ACCEL_YOUT_H 0x3D #define REGISTER_ACCEL_YOUT_L 0x3E #define REGISTER_ACCEL_ZOUT_H 0x3F #define REGISTER_ACCEL_ZOUT_L 0x40 #define REGISTER_TEMP_OUT_H 0x41 #define REGISTER_TEMP_OUT_L 0x42 #define REGISTER_GYRO_XOUT_H 0x43 #define REGISTER_GYRO_XOUT_L 0x44 #define REGISTER_GYRO_YOUT_H 0x45 #define REGISTER_GYRO_YOUT_L 0x46 #define REGISTER_GYRO_ZOUT_H 0x47 #define REGISTER_GYRO_ZOUT_L 0x48 #define REGISTER_WHO_AM_I 0x75 #define I2C_ADDRESS_1 0x68 #define I2C_ADDRESS_2 0x69 #define SENSOR_READ_INTERVAL_MS 30 #define CALIBRATION_READS 50 typedef struct mpu6050_calibration_t { float acc_error_x, acc_error_y; float gyro_error_x, gyro_error_y, gyro_error_z; } mpu6050_calibration_t; typedef struct mpu6050_acceleration_t { float x, y, z; } mpu6050_acceleration_t; typedef struct mpu6050_gyro_t { float x, y, z; } mpu6050_gyro_t; static void read(mpu6050_parameters_t *parameter, mpu6050_calibration_t *calibration, uint accel_divider, uint gyro_divider); static mpu6050_acceleration_t read_acc(uint8_t address, float accel_divider); static mpu6050_gyro_t read_gyro(uint8_t address, float gyro_divider); static void begin(mpu6050_parameters_t *parameter, mpu6050_calibration_t *calibration, float accel_divider, float gyro_divider); static void calibrate_imu(uint8_t address, mpu6050_calibration_t *calibration, float accel_divider, float gyro_divider); void mpu6050_task(void *parameters) { mpu6050_parameters_t parameter = *(mpu6050_parameters_t *)parameters; xTaskNotifyGive(context.receiver_task_handle); *parameter.roll = 0; *parameter.pitch = 0; *parameter.yaw = 0; *parameter.acc_x = 0; *parameter.acc_y = 0; *parameter.acc_z = 0; *parameter.acc = 0; mpu6050_calibration_t calibration = {0}; vTaskDelay(500 / portTICK_PERIOD_MS); uint accel_divider, gyro_divider; switch (parameter.acc_scale) { case 0: accel_divider = 16384; // +-2g break; case 1: accel_divider = 8192; // +-4g break; case 2: accel_divider = 4096; // +-8g break; case 3: accel_divider = 2048; // +-16g break; default: accel_divider = 16384; // Default to +-2g break; } switch (parameter.gyro_scale) { case 0: gyro_divider = 131; // 250deg/s break; case 1: gyro_divider = 65.5; // 500deg/s break; case 2: gyro_divider = 32.8; // 1000deg/s break; case 3: gyro_divider = 16.4; // 2000deg/s break; default: gyro_divider = 131; // Default to 250deg/s break; } begin(¶meter, &calibration, accel_divider, gyro_divider); while (1) { read(¶meter, &calibration, accel_divider, gyro_divider); debug( "\nMPU6050 (%u) < Roll: %.2f Pitch: %.2f Yaw: %.2f", uxTaskGetStackHighWaterMark(NULL), calibration.acc_error_x, calibration.acc_error_y, calibration.gyro_error_x, calibration.gyro_error_y, calibration.gyro_error_z, *parameter.roll, *parameter.pitch, *parameter.yaw); vTaskDelay(SENSOR_READ_INTERVAL_MS / portTICK_PERIOD_MS); } } static void read(mpu6050_parameters_t *parameter, mpu6050_calibration_t *calibration, uint accel_divider, uint gyro_divider) { uint8_t data[6] = {0}; float acc_angle_x, acc_angle_y; static float gyro_angle_x = 0, gyro_angle_y = 0; static uint32_t lastTimestamp = 0; // Read accelerometer data mpu6050_acceleration_t acc = read_acc(parameter->address, accel_divider); acc_angle_x = (atan(acc.y / sqrt(pow(acc.x, 2) + pow(acc.z, 2))) * 180 / PI); acc_angle_y = (atan(-1 * acc.x / sqrt(pow(acc.y, 2) + pow(acc.z, 2))) * 180 / PI); acc_angle_x -= calibration->acc_error_x; acc_angle_y -= calibration->acc_error_y; *parameter->acc_x = acc.x; *parameter->acc_y = acc.y; *parameter->acc_z = acc.z; *parameter->acc = sqrt(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); // Read gyroscope data mpu6050_gyro_t gyro = read_gyro(parameter->address, gyro_divider); gyro.x -= calibration->gyro_error_x; gyro.y -= calibration->gyro_error_y; gyro.z -= calibration->gyro_error_z; // Currently the raw values are in degrees per seconds, deg/s, so we need to multiply by seconds (s) to get the // angle in degrees uint32_t now = time_us_32(); float elapsedTime = (now - lastTimestamp) / 1000000.0; gyro_angle_x += gyro.x * elapsedTime; gyro_angle_y += gyro.y * elapsedTime; // Combine accelerometer and gyro angle values *parameter->roll = parameter->gyro_weighting / 100.0 * gyro_angle_x + (100 - parameter->gyro_weighting) / 100.0 * acc_angle_x; *parameter->pitch = parameter->gyro_weighting / 100.0 * gyro_angle_y + (100 - parameter->gyro_weighting) / 100.0 * acc_angle_y; *parameter->yaw += gyro.z * elapsedTime; lastTimestamp = now; #ifdef SIM_SENSORS *parameter->roll = 12.34; *parameter->pitch = 23.56; *parameter->yaw = 45.67; #endif } static mpu6050_acceleration_t read_acc(uint8_t address, float accel_divider) { uint8_t data[6] = {0}; mpu6050_acceleration_t acc; // Read accelerometer data data[0] = REGISTER_ACCEL_XOUT_H; i2c_write_blocking(i2c0, address, data, 1, true); i2c_read_blocking(i2c0, address, data, 6, false); acc.x = (int16_t)(data[0] << 8 | data[1]) / accel_divider; acc.y = (int16_t)(data[2] << 8 | data[3]) / accel_divider; acc.z = (int16_t)(data[4] << 8 | data[5]) / accel_divider; // debug("\nAcc: X: %5.2f Y: %5.2f Z: %5.2f", acc.x, acc.y, acc.z); return acc; } static mpu6050_gyro_t read_gyro(uint8_t address, float gyro_divider) { uint8_t data[6] = {0}; mpu6050_gyro_t gyro; // Read gyroscope data data[0] = REGISTER_GYRO_XOUT_H; i2c_write_blocking(i2c0, address, data, 1, true); i2c_read_blocking(i2c0, address, data, 6, false); gyro.x = (int16_t)(data[0] << 8 | data[1]) / gyro_divider; gyro.y = (int16_t)(data[2] << 8 | data[3]) / gyro_divider; gyro.z = (int16_t)(data[4] << 8 | data[5]) / gyro_divider; // debug("\nGyro: X: %5.2f Y: %5.2f Z: %5.2f", gyro.x, gyro.y, gyro.z); return gyro; } static void begin(mpu6050_parameters_t *parameter, mpu6050_calibration_t *calibration, float accel_divider, float gyro_divider) { i2c_init(i2c0, 400 * 1000); gpio_set_function(I2C0_SDA_GPIO, GPIO_FUNC_I2C); gpio_set_function(I2C0_SCL_GPIO, GPIO_FUNC_I2C); gpio_pull_up(I2C0_SDA_GPIO); gpio_pull_up(I2C0_SCL_GPIO); // Reset uint8_t data[2] = {REGISTER_PWR_MGMT_1, 0}; i2c_write_blocking(i2c0, parameter->address, data, 2, true); vTaskDelay(100 / portTICK_PERIOD_MS); // Find sensor address parameter->address = I2C_ADDRESS_1; if (i2c_write_blocking(i2c0, I2C_ADDRESS_1, data, 1, false) == PICO_ERROR_GENERIC) { parameter->address = I2C_ADDRESS_2; } // Configure Accelerometer Sensitivity data[0] = REGISTER_ACCEL_CONFIG; data[1] = parameter->acc_scale << 3; i2c_write_blocking(i2c0, parameter->address, data, 2, true); // Configure Gyroscope Sensitivity data[0] = REGISTER_GYRO_CONFIG; data[1] = parameter->gyro_scale << 3; i2c_write_blocking(i2c0, parameter->address, data, 2, true); // Set sample rate to 1kHz by writing SMPLRT_DIV // data[0] = REGISTER_SMPLRT_DIV; // data[1] = 0x07; // Use a 1kHz gyroscope rate, set SMPLRT_DIV to 7 for a 125Hz sample rate // i2c_write_blocking(i2c0, parameter->address, data, 2, true); // Set Digital Low Pass Filter data[0] = REGISTER_CONFIG; data[1] = parameter->filter; i2c_write_blocking(i2c0, parameter->address, data, 2, true); // Calibrate IMU calibrate_imu(parameter->address, calibration, accel_divider, gyro_divider); } static void calibrate_imu(uint8_t address, mpu6050_calibration_t *calibration, float accel_divider, float gyro_divider) { // Read accelerometer values for (uint i = 0; i < CALIBRATION_READS; i++) { mpu6050_acceleration_t acc = read_acc(address, accel_divider); calibration->acc_error_x += ((atan((acc.y) / sqrt(pow((acc.x), 2) + pow((acc.z), 2))) * 180 / PI)); calibration->acc_error_y += ((atan(-1 * (acc.x) / sqrt(pow((acc.y), 2) + pow((acc.z), 2))) * 180 / PI)); vTaskDelay(SENSOR_READ_INTERVAL_MS / portTICK_PERIOD_MS); } // Divide the sum to get the error value calibration->acc_error_x /= CALIBRATION_READS; calibration->acc_error_y /= CALIBRATION_READS; // Read gyro values for (uint i = 0; i < CALIBRATION_READS; i++) { mpu6050_gyro_t gyro = read_gyro(address, gyro_divider); calibration->gyro_error_x += gyro.x; calibration->gyro_error_y += gyro.y; calibration->gyro_error_z += gyro.z; vTaskDelay(SENSOR_READ_INTERVAL_MS / portTICK_PERIOD_MS); } // Divide the sum to get the error value calibration->gyro_error_x /= CALIBRATION_READS; calibration->gyro_error_y /= CALIBRATION_READS; calibration->gyro_error_z /= CALIBRATION_READS; debug("\nMPU6050. Calibration. AccX: %.2f AccY: %.2f GyroX: %.2f GyroY: %.2f GyroZ: %.2f", calibration->acc_error_x, calibration->acc_error_y, calibration->gyro_error_x, calibration->gyro_error_y, calibration->gyro_error_z); } ================================================ FILE: board/project/sensor/mpu6050.h ================================================ #ifndef MPU6050_H #define MPU6050_H #include "common.h" typedef struct mpu6050_parameters_t { float alpha_gyro; uint8_t address, acc_scale, gyro_scale, gyro_weighting, filter; float *acc_x, *acc_y, *acc_z, *acc; float *roll, *pitch, *yaw; } mpu6050_parameters_t; extern context_t context; void mpu6050_task(void *parameters); #endif ================================================ FILE: board/project/sensor/ms5611.c ================================================ #include "ms5611.h" #include #include "auto_offset.h" #include "hardware/i2c.h" #include "pico/stdlib.h" #include "stdlib.h" #include "vspeed.h" #define CMD_ADC_READ 0x00 #define CMD_RESET 0x1E #define CMD_CONV_D1 0x40 #define CMD_CONV_D2 0x50 // 0 0xA0,0xA1 - reserved // 1 0xA2,0xA3 - C1 // 2 0xA4,0xA5 - C2 // 3 0xA6,0xA7 - C3 // 4 0xA8,0xA9 - C4 // 5 0xAA,0xAB - C5 // 6 0xAC,0xAD - C6 // 7 0xAE - CRC (4bits) #define CMD_READ_PROM 0xA0 #define OVERSAMPLING_4096 0x08 #define OVERSAMPLING_2048 0x06 #define OVERSAMPLING_1024 0x04 #define OVERSAMPLING_512 0x02 #define OVERSAMPLING_256 0x00 #define I2C_ADDRESS_1 0x77 #define I2C_ADDRESS_2 0x76 #define SENSOR_INTERVAL_MS 20 // min 10 #define VSPEED_INTERVAL_MS 500 static void read(ms5611_parameters_t *parameter, ms5611_calibration_t *calibration); static void begin(ms5611_parameters_t *parameter, ms5611_calibration_t *calibration); void ms5611_task(void *parameters) { ms5611_parameters_t parameter = *(ms5611_parameters_t *)parameters; xTaskNotifyGive(context.receiver_task_handle); *parameter.altitude = 0; *parameter.vspeed = 0; *parameter.temperature = 0; *parameter.pressure = 0; TaskHandle_t task_handle; vTaskDelay(500 / portTICK_PERIOD_MS); ms5611_calibration_t calibration; begin(¶meter, &calibration); while (1) { read(¶meter, &calibration); debug("\nMS5611 (%u) < Temp: %.2f Pressure: %.0f Altitude: %0.2f Vspeed: %.2f", uxTaskGetStackHighWaterMark(NULL), *parameter.temperature, *parameter.pressure, *parameter.altitude, *parameter.vspeed); } } static void read(ms5611_parameters_t *parameter, ms5611_calibration_t *calibration) { static float pressure_initial = 0; static uint discard_readings = 5; /* Read sensor data */ uint32_t D1, D2; uint8_t data[3]; data[0] = CMD_CONV_D1 + OVERSAMPLING_4096; i2c_write_blocking(i2c0, parameter->address, data, 1, false); vTaskDelay(SENSOR_INTERVAL_MS / portTICK_PERIOD_MS); data[0] = CMD_ADC_READ; i2c_write_blocking(i2c0, parameter->address, data, 1, true); i2c_read_blocking(i2c0, parameter->address, data, 3, false); D1 = (uint32_t)data[0] << 16 | (uint16_t)data[1] << 8 | data[2]; data[0] = CMD_CONV_D2 + OVERSAMPLING_4096; i2c_write_blocking(i2c0, parameter->address, data, 1, false); vTaskDelay(SENSOR_INTERVAL_MS / portTICK_PERIOD_MS); data[0] = CMD_ADC_READ; i2c_write_blocking(i2c0, parameter->address, data, 1, true); i2c_read_blocking(i2c0, parameter->address, data, 3, false); D2 = (uint32_t)data[0] << 16 | (uint16_t)data[1] << 8 | data[2]; /* Calculation */ int32_t dT, TEMP, T2 = 0; int64_t OFF, SENS, OFF2 = 0, SENS2 = 0; dT = D2 - ((uint32_t)calibration->C5 << 8); TEMP = 2000 + ((int64_t)dT * calibration->C6 >> 23); OFF = ((int64_t)calibration->C2 << 16) + (((int64_t)calibration->C4 * dT) >> 7); SENS = ((int64_t)calibration->C1 << 15) + (((int64_t)calibration->C3 * dT) >> 8); if (TEMP < 2000) { T2 = (dT * dT) >> 31; OFF2 = 5 * (TEMP - 2000) * (TEMP - 2000) / 2; SENS2 = 5 * (TEMP - 2000) * (TEMP - 2000) / 4; } if (TEMP < -1500) { OFF2 = OFF2 + 7 * (TEMP + 1500) * (TEMP + 1500); SENS2 = SENS2 + 11 * (TEMP + 1500) * (TEMP + 1500) / 2; } TEMP = TEMP - T2; OFF = OFF - OFF2; SENS = SENS - SENS2; int32_t P = (((D1 * SENS) >> 21) - OFF) >> 15; *parameter->temperature = (float)TEMP / 100; // °C *parameter->pressure = (float)P; // Pa if (pressure_initial == 0 && discard_readings == 0) pressure_initial = *parameter->pressure; *parameter->altitude = get_altitude(*parameter->pressure, *parameter->temperature, pressure_initial); get_vspeed(parameter->vspeed, *parameter->altitude, VSPEED_INTERVAL_MS); if (discard_readings > 0) discard_readings--; debug("\nMS5611 P0: %.0f", pressure_initial); #ifdef SIM_SENSORS *parameter->temperature = 12.34; *parameter->pressure = 1234.56; *parameter->altitude = 2345; #endif } static void begin(ms5611_parameters_t *parameter, ms5611_calibration_t *calibration) { i2c_init(i2c0, 100 * 1000); gpio_set_function(I2C0_SDA_GPIO, GPIO_FUNC_I2C); gpio_set_function(I2C0_SCL_GPIO, GPIO_FUNC_I2C); gpio_pull_up(I2C0_SDA_GPIO); gpio_pull_up(I2C0_SCL_GPIO); uint8_t data[12]; // Find sensor address parameter->address = I2C_ADDRESS_1; if (i2c_write_blocking(i2c0, I2C_ADDRESS_1, data, 1, false) == PICO_ERROR_GENERIC) { parameter->address = I2C_ADDRESS_2; } // Read calibration data data[0] = CMD_RESET; i2c_write_blocking(i2c0, parameter->address, data, 1, false); vTaskDelay(SENSOR_INTERVAL_MS / portTICK_PERIOD_MS); data[0] = CMD_READ_PROM + 2; i2c_write_blocking(i2c0, parameter->address, data, 1, true); i2c_read_blocking(i2c0, parameter->address, data, 2, false); calibration->C1 = ((uint16_t)data[0] << 8) | data[1]; data[0] = CMD_READ_PROM + 4; i2c_write_blocking(i2c0, parameter->address, data, 1, true); i2c_read_blocking(i2c0, parameter->address, data, 2, false); calibration->C2 = ((uint16_t)data[0] << 8) | data[1]; data[0] = CMD_READ_PROM + 6; i2c_write_blocking(i2c0, parameter->address, data, 1, true); i2c_read_blocking(i2c0, parameter->address, data, 2, false); calibration->C3 = ((uint16_t)data[0] << 8) | data[1]; data[0] = CMD_READ_PROM + 8; i2c_write_blocking(i2c0, parameter->address, data, 1, true); i2c_read_blocking(i2c0, parameter->address, data, 2, false); calibration->C4 = ((uint16_t)data[0] << 8) | data[1]; data[0] = CMD_READ_PROM + 10; i2c_write_blocking(i2c0, parameter->address, data, 1, true); i2c_read_blocking(i2c0, parameter->address, data, 2, false); calibration->C5 = ((uint16_t)data[0] << 8) | data[1]; data[0] = CMD_READ_PROM + 12; i2c_write_blocking(i2c0, parameter->address, data, 1, true); i2c_read_blocking(i2c0, parameter->address, data, 2, false); calibration->C6 = ((uint16_t)data[0] << 8) | data[1]; } ================================================ FILE: board/project/sensor/ms5611.h ================================================ #ifndef MS5611_H #define MS5611_H #include "common.h" typedef struct ms5611_parameters_t { float alpha_vario; bool auto_offset; uint8_t address; float *temperature, *pressure, *altitude, *vspeed; } ms5611_parameters_t; typedef struct ms5611_calibration_t { uint16_t C1, C2, C3, C4, C5, C6; } ms5611_calibration_t; extern context_t context; void ms5611_task(void *parameters); #endif ================================================ FILE: board/project/sensor/ntc.c ================================================ #include "ntc.h" #include #include #include "hardware/adc.h" // Thermistors (NTC 100k, R1 10k) #define NTC_R_REF 100000UL #define NTC_R1 10000 #define NTC_BETA 4190 // #define NTC_A1 3.35E-03 // #define NTC_B1 2.46E-04 // #define NTC_C1 3.41E-06 // #define NTC_D1 1.03E-07 void ntc_task(void *parameters) { ntc_parameters_t parameter = *(ntc_parameters_t *)parameters; adc_init(); adc_gpio_init(parameter.adc_num + 26); //gpio_pull_down(parameter.adc_num + 26); *parameter.ntc = 0; xTaskNotifyGive(context.receiver_task_handle); while (1) { float voltage = voltage_read(parameter.adc_num); float ntcR_Rref = (voltage * NTC_R1 / (BOARD_VCC - voltage)) / NTC_R_REF; if (ntcR_Rref < 0.0001) ntcR_Rref = 0.0001; float temperature = 1 / (log(ntcR_Rref) / NTC_BETA + 1 / 298.15) - 273.15; *parameter.ntc = get_average(parameter.alpha, *parameter.ntc, temperature); #ifdef SIM_SENSORS *parameter.ntc = 12.34; #endif *parameter.ntc += parameter.offset; debug("\nTemperature (%u): %.2f (offset %d)", uxTaskGetStackHighWaterMark(NULL), *parameter.ntc, parameter.offset); vTaskDelay(1000 / parameter.rate / portTICK_PERIOD_MS); } } ================================================ FILE: board/project/sensor/ntc.h ================================================ #ifndef NTC_H #define NTC_H #include "common.h" typedef struct ntc_parameters_t { uint8_t adc_num; uint8_t rate; int8_t offset; float alpha; float *ntc; } ntc_parameters_t; extern context_t context; void ntc_task(void *parameters); #endif ================================================ FILE: board/project/sensor/pwm_out.c ================================================ #include "pwm_out.h" #include #include "hardware/clocks.h" #include "hardware/pwm.h" void pwm_out_task(void *parameters) { float *rpm = (float *)parameters; xTaskNotifyGive(context.receiver_task_handle); const float clock_div = 100; gpio_set_function(PWM_OUT_GPIO, GPIO_FUNC_PWM); uint slice_num = pwm_gpio_to_slice_num(PWM_OUT_GPIO); uint wrap = 0; pwm_config c = pwm_get_default_config(); pwm_config_set_clkdiv(&c, clock_div); pwm_init(slice_num, &c, true); while (1) { ulTaskNotifyTake(pdTRUE, portMAX_DELAY); if (*rpm < 1300) pwm_set_enabled(slice_num, false); else { wrap = 60 / *rpm * clock_get_hz(clk_sys) / clock_div; pwm_set_enabled(slice_num, true); pwm_set_wrap(slice_num, wrap); pwm_set_gpio_level(PWM_OUT_GPIO, wrap / 2); } debug("\nPWM out (%u) > Rpm %.2f Wrap %i", uxTaskGetStackHighWaterMark(NULL), *rpm, wrap); } } ================================================ FILE: board/project/sensor/pwm_out.h ================================================ #ifndef PWM_OUT_H #define PWM_OUT_H #include "common.h" extern context_t context; void pwm_out_task(void *parameters); #endif ================================================ FILE: board/project/sensor/smart_esc.c ================================================ #include "smart_esc.h" #include #include "auto_offset.h" #include "capture_edge.h" #include "cell_count.h" #include "hardware/clocks.h" #include "pico/stdlib.h" #include "srxl.h" #include "srxl2.h" #include "string.h" #include "uart.h" #define SRXL2_RECEIVER_ID 0x21 #define SRXL2_ESC_ID 0x40 #define SRXL2_RECEIVER_PRIORITY 0xA #define SRXL2_RECEIVER_BAUDRATE 1 #define SRXL2_RECEIVER_INFO 0x7 #define SRXL2_INTERVAL_MS 10 #define SRXL2_RECEIVER_UID 0x27A2C29C // 0x12345678 #define XBUS_SMART_BAT 0x42 #define XBUS_SMART_BAT_REALTIME 0x00 #define XBUS_SMART_BAT_CELLS_1 0x10 #define XBUS_SMART_BAT_CELLS_2 0x20 #define XBUS_SMART_BAT_CELLS_3 0x30 #define XBUS_SMART_BAT_ID 0x80 #define XBUS_SMART_BAT_LIMITS 0x90 #define PWM_TIMEOUT_MS 100 #define CLOCK_DIV 1 typedef struct srxl2_smart_bat_realtime_t { uint8_t identifier; // Source device 0x42 uint8_t s_id; // Secondary ID uint8_t type; // 0x00 int8_t temp; uint32_t current; // A uint16_t consumption; // mAh uint16_t min_cel; uint16_t max_cel; } __attribute__((packed)) srxl2_smart_bat_realtime_t; typedef struct srxl2_smart_bat_cells_1_t { uint8_t identifier; // Source device 0x42 uint8_t s_id; // Secondary ID uint8_t type; // 0x10 int8_t temp; uint16_t cell_1; // V * 1000 uint16_t cell_2; uint16_t cell_3; uint16_t cell_4; uint16_t cell_5; uint16_t cell_6; } __attribute__((packed)) srxl2_smart_bat_cells_1_t; typedef struct srxl2_smart_bat_cells_2_t { uint8_t identifier; // Source device 0x42 uint8_t s_id; // Secondary ID uint8_t type; // 0x20 int8_t temp; uint16_t cell_7; uint16_t cell_8; uint16_t cell_9; uint16_t cell_10; uint16_t cell_11; uint16_t cell_12; } __attribute__((packed)) srxl2_smart_bat_cells_2_t; typedef struct srxl2_smart_bat_cells_3_t { uint8_t identifier; // Source device 0x42 uint8_t s_id; // Secondary ID uint8_t type; // 0x30 int8_t temp; uint16_t cell_13; uint16_t cell_14; uint16_t cell_15; uint16_t cell_16; uint16_t cell_17; uint16_t cell_18; } __attribute__((packed)) srxl2_smart_bat_cells_3_t; typedef struct srxl2_smart_bat_id_t { uint8_t identifier; // Source device 0x42 uint8_t s_id; // Secondary ID uint8_t type; // 0x80 uint8_t chemistery; uint8_t cells; uint8_t mfg_code; uint16_t cycles; uint8_t uid; } __attribute__((packed)) srxl2_smart_bat_id_t; typedef struct srxl2_smart_bat_limits_t { uint8_t identifier; // Source device 0x42 uint8_t s_id; // Secondary ID uint8_t type; // 0x90 uint8_t rfu; uint16_t capacity; uint16_t discharge_rate; uint16_t overdischarge; uint16_t zero_capacity; uint16_t fully_charged; uint8_t min_temp; uint8_t mex_temp; } __attribute__((packed)) srxl2_smart_bat_limits_t; typedef struct srxl2_channel_data_t { int8_t rssi; uint16_t frame_losses; uint32_t channel_mask; uint16_t channel_data_ch1; // throttle uint16_t channel_data_ch7; // reverse } __attribute__((packed)) srxl2_channel_data_t; typedef struct srxl2_control_packet_t { uint8_t header; uint8_t type; uint8_t len; uint8_t command; uint8_t reply_id; srxl2_channel_data_t channel_data; uint16_t crc; } __attribute__((packed)) srxl2_control_packet_t; #define SRXL2_CONTROL_LEN_CHANNEL (5 + sizeof(srxl2_channel_data_t) + 2) // header + channel data + crc static volatile uint8_t esc_id = 0, esc_priority = 10; static volatile uint16_t throttle = 0, reverse = 0; static volatile bool packet_pending = false; static void process(smart_esc_parameters_t *parameter); static void read_packet(uint8_t *buffer, smart_esc_parameters_t *parameter); static void send_packet(void); static void capture_pwm_throttle_handler(uint counter, edge_type_t edge); static void capture_pwm_reverse_handler(uint counter, edge_type_t edge); static int64_t timeout_throttle_callback(alarm_id_t id, void *user_data); static int64_t timeout_reverse_callback(alarm_id_t id, void *user_data); static int64_t alarm_packet(alarm_id_t id, void *user_data); void smart_esc_task(void *parameters) { smart_esc_parameters_t parameter = *(smart_esc_parameters_t *)parameters; *parameter.rpm = 0; *parameter.voltage = 0; *parameter.current = 0; *parameter.temperature_fet = 0; *parameter.temperature_bec = 0; *parameter.voltage_bec = 0; *parameter.current_bec = 0; *parameter.current_bat = 0; *parameter.consumption = 0; xTaskNotifyGive(context.receiver_task_handle); #ifdef SIM_SENSORS *parameter.rpm = 12345.67; *parameter.consumption = 123.4; *parameter.voltage = 12.34; *parameter.current = 5.678; *parameter.temperature_fet = 12.34; *parameter.temperature_bec = 23.45; #endif capture_edge_init(pio0, SMART_ESC_PWM_GPIO, 2, CLOCK_DIV, PIO0_IRQ_0); capture_edge_set_handler(0, capture_pwm_throttle_handler); capture_edge_set_handler(1, capture_pwm_reverse_handler); gpio_pull_up(SMART_ESC_PWM_GPIO); gpio_pull_up(SMART_ESC_PWM_GPIO + 1); uart1_begin(115200, UART1_TX_GPIO, UART_ESC_RX, SRXL2_TIMEOUT_US, 8, 1, UART_PARITY_NONE, false, true); add_alarm_in_us(0, alarm_packet, NULL, true); while (1) { ulTaskNotifyTakeIndexed(1, pdTRUE, portMAX_DELAY); process(¶meter); if (packet_pending) { packet_pending = false; send_packet(); } } } static void process(smart_esc_parameters_t *parameter) { static uint32_t timestamp = 0; uint8_t length = uart1_available(); if (length) { uint8_t data[length]; uart1_read_bytes(data, length); debug("\nSmart ESC (%u) < ", uxTaskGetStackHighWaterMark(NULL)); debug_buffer(data, length, " 0x%X"); uint16_t crc = srxl_get_crc(data, length - 2); if ((crc >> 8) != data[length - 2] || (crc & 0xFF) != data[length - 1]) { debug(" -> BAD CRC 0x%X", crc); return; } // Request for handshake to enable srxl2 mode if (data[0] == SRXL2_HEADER && data[1] == SRXL2_PACKET_TYPE_HANDSHAKE && data[4] == 0) { debug("\nSmart ESC. Handshake request from 0x%X", data[3]); if (data[3] == SRXL2_ESC_ID) srxl2_send_handshake(uart1, SRXL2_RECEIVER_ID, SRXL2_ESC_ID, SRXL2_RECEIVER_PRIORITY, SRXL2_RECEIVER_BAUDRATE, SRXL2_RECEIVER_INFO, SRXL2_RECEIVER_UID); } // Handshake confirmation else if (data[0] == SRXL2_HEADER && data[1] == SRXL2_PACKET_TYPE_HANDSHAKE && data[3] == SRXL2_ESC_ID) { esc_id = data[3]; esc_priority = data[5]; debug("\nSmart ESC. Handshake with 0x%X completed ", esc_id); } // Telemetry packet else if (data[0] == SRXL2_HEADER && data[1] == SRXL2_PACKET_TYPE_TELEMETRY && data[3] == SRXL2_RECEIVER_ID) { read_packet(data + 4, parameter); } // Re-handshake request else if (data[0] == SRXL2_HEADER && data[1] == SRXL2_PACKET_TYPE_TELEMETRY && data[3] == 0xFF) { srxl2_send_handshake(uart1, SRXL2_RECEIVER_ID, SRXL2_ESC_ID, SRXL2_RECEIVER_PRIORITY, SRXL2_RECEIVER_BAUDRATE, SRXL2_RECEIVER_INFO, SRXL2_RECEIVER_UID); } } } static void read_packet(uint8_t *buffer, smart_esc_parameters_t *parameter) { if (buffer[0] == XBUS_ESC_ID) { xbus_esc_t esc; static uint32_t timestamp = 0; memcpy(&esc, buffer, sizeof(xbus_esc_t)); *parameter->rpm = swap_16(esc.rpm) * 10 * parameter->rpm_multiplier; *parameter->voltage = swap_16(esc.volts_input) / 100.0; *parameter->current = swap_16(esc.current_motor) / 100.0; *parameter->voltage_bec = esc.voltage_bec == 0xFF ? 0 : esc.voltage_bec / 2.0; *parameter->current_bec = esc.current_bec == 0xFF ? 0 : esc.current_bec / 100.0; *parameter->temperature_fet = esc.temp_fet == 0xFFFF ? 0 : (swap_16(esc.temp_fet) / 10.0); *parameter->temperature_bec = esc.temp_bec == 0xFFFF ? 0 : swap_16(esc.temp_bec) / 10.0; if (parameter->calc_consumption) *parameter->consumption += get_consumption(*parameter->current, 0, ×tamp); debug("\nSmart ESC (%u) < Rpm: %.0f Volt: %0.2f Curr: %.2f TempFet: %.0f TempBec: %.0f Vbec: %.1f Cbec: %.1f ", uxTaskGetStackHighWaterMark(NULL), *parameter->rpm, *parameter->voltage, *parameter->current, *parameter->temperature_fet, *parameter->temperature_bec, *parameter->voltage_bec, *parameter->current_bec); } else if (buffer[0] == XBUS_SMART_BAT) { switch (buffer[2]) { case XBUS_SMART_BAT_REALTIME: { srxl2_smart_bat_realtime_t bat_realtime; memcpy(&bat_realtime, buffer, sizeof(srxl2_smart_bat_realtime_t)); *parameter->temperature_bat = bat_realtime.temp; *parameter->current_bat = bat_realtime.current / 1000.0; if (!parameter->calc_consumption) *parameter->consumption = bat_realtime.consumption / 10.0; debug("\nSmart ESC (Bat 0x%X) (%u) < Temp: %.0f Curr: %0.2f Cons: %.2f ", XBUS_SMART_BAT_REALTIME, uxTaskGetStackHighWaterMark(NULL), *parameter->temperature_bat, *parameter->current_bat, *parameter->consumption); break; } case XBUS_SMART_BAT_CELLS_1: { srxl2_smart_bat_cells_1_t bat_cells_1; memcpy(&bat_cells_1, buffer, sizeof(srxl2_smart_bat_cells_1_t)); *parameter->temperature_bat = bat_cells_1.temp; *parameter->cell[1] = bat_cells_1.cell_1 / 1000.0; *parameter->cell[2] = bat_cells_1.cell_2 / 1000.0; *parameter->cell[3] = bat_cells_1.cell_3 / 1000.0; *parameter->cell[4] = bat_cells_1.cell_4 / 1000.0; *parameter->cell[5] = bat_cells_1.cell_5 / 1000.0; *parameter->cell[6] = bat_cells_1.cell_6 / 1000.0; debug("\nSmart ESC (Bat 0x%X) (%u) < Temp: %.0f C1: %.3f C2: %.3f C3: %.3f C4: %.3f C5: %.3f C6: %.3f ", XBUS_SMART_BAT_CELLS_1, uxTaskGetStackHighWaterMark(NULL), *parameter->temperature_bat, *parameter->cell[1], *parameter->cell[2], *parameter->cell[3], *parameter->cell[4], *parameter->cell[5], *parameter->cell[6]); break; } case XBUS_SMART_BAT_CELLS_2: { srxl2_smart_bat_cells_2_t bat_cells_2; memcpy(&bat_cells_2, buffer, sizeof(srxl2_smart_bat_cells_2_t)); *parameter->temperature_bat = bat_cells_2.temp; *parameter->cell[7] = bat_cells_2.cell_7 / 1000.0; *parameter->cell[8] = bat_cells_2.cell_8 / 1000.0; *parameter->cell[9] = bat_cells_2.cell_9 / 1000.0; *parameter->cell[10] = bat_cells_2.cell_10 / 1000.0; *parameter->cell[11] = bat_cells_2.cell_11 / 1000.0; *parameter->cell[12] = bat_cells_2.cell_12 / 1000.0; debug( "\nSmart ESC (Bat 0x%X) (%u) < Temp: %.0f C7: %.3f C8: %.3f C9: %.3f C10: %.3f C11: %.3f C12: " "%.3f ", XBUS_SMART_BAT_CELLS_2, uxTaskGetStackHighWaterMark(NULL), *parameter->temperature_bat, *parameter->cell[7], *parameter->cell[8], *parameter->cell[9], *parameter->cell[10], *parameter->cell[11], *parameter->cell[12]); break; } case XBUS_SMART_BAT_CELLS_3: { srxl2_smart_bat_cells_3_t bat_cells_3; memcpy(&bat_cells_3, buffer, sizeof(srxl2_smart_bat_cells_3_t)); *parameter->temperature_bat = bat_cells_3.temp; *parameter->cell[13] = bat_cells_3.cell_13 / 1000.0; *parameter->cell[14] = bat_cells_3.cell_14 / 1000.0; *parameter->cell[15] = bat_cells_3.cell_15 / 1000.0; *parameter->cell[16] = bat_cells_3.cell_16 / 1000.0; *parameter->cell[17] = bat_cells_3.cell_17 / 1000.0; *parameter->cell[18] = bat_cells_3.cell_18 / 1000.0; debug( "\nSmart ESC (Bat 0x%X) (%u) < Temp: %.0f C13: %.3f C14: %.3f C15: %.3f C16: %.3f C17: %.3f C18: " "%.3f ", XBUS_SMART_BAT_CELLS_3, uxTaskGetStackHighWaterMark(NULL), *parameter->temperature_bat, *parameter->cell[13], *parameter->cell[14], *parameter->cell[15], *parameter->cell[16], *parameter->cell[17], *parameter->cell[18]); break; } case XBUS_SMART_BAT_ID: { srxl2_smart_bat_id_t bat_id; memcpy(&bat_id, buffer, sizeof(srxl2_smart_bat_id_t)); *parameter->cells = bat_id.cells; *parameter->cycles = bat_id.cycles; debug("\nSmart ESC (Bat 0x%X) (%u) < Cells: %u Cycles: %u ", XBUS_SMART_BAT_ID, uxTaskGetStackHighWaterMark(NULL), *parameter->cells, *parameter->cycles); break; } } } } static void send_packet(void) { static uint cont = 0; if (!esc_id) { srxl2_send_handshake(uart1, SRXL2_RECEIVER_ID, SRXL2_ESC_ID, SRXL2_RECEIVER_PRIORITY, SRXL2_RECEIVER_BAUDRATE, SRXL2_RECEIVER_INFO, SRXL2_RECEIVER_UID); } else { srxl2_control_packet_t packet; packet.header = SRXL2_HEADER; packet.type = SRXL2_PACKET_TYPE_CONTROL; packet.len = SRXL2_CONTROL_LEN_CHANNEL; packet.command = SRXL2_CONTROL_CMD_CHANNEL; packet.reply_id = 0; if (!(cont % 10)) packet.reply_id = esc_id; srxl2_channel_data_t channel_data; channel_data.rssi = 0x64; channel_data.frame_losses = 0; channel_data.channel_mask = 0B1000001; // ch1 throttle, ch7 reverse channel_data.channel_data_ch1 = throttle; channel_data.channel_data_ch7 = reverse; packet.channel_data = channel_data; uint16_t crc = srxl_get_crc((uint8_t *)&packet, sizeof(packet) - 2); packet.crc = swap_16(crc); uart1_write_bytes((uint8_t *)&packet, sizeof(packet)); cont++; static uint32_t last_time = 0; uint32_t now = time_us_32(); debug("\nSmart ESC (%u) Thr: %u Rev: %u (interval us %i)", uxTaskGetStackHighWaterMark(NULL), throttle, reverse, now - last_time); debug_buffer((uint8_t *)&packet, sizeof(packet), " 0x%X"); last_time = now; } } static void capture_pwm_throttle_handler(uint counter, edge_type_t edge) { static uint counter_edge_rise = 0; static alarm_id_t timeout_throttle_alarm_id = 0; if (timeout_throttle_alarm_id) cancel_alarm(timeout_throttle_alarm_id); if (edge == EDGE_RISE) { counter_edge_rise = counter; } else if (edge == EDGE_FALL && counter_edge_rise) { uint pulse = (float)(counter - counter_edge_rise) / clock_get_hz(clk_sys) * COUNTER_CYCLES * 1000000; int delta = pulse - 1000; if (delta < 0) delta = 0; if (delta > 1000) delta = 1000; throttle = delta / 1000.0F * 65532; // 0us->0% 1000us->100% static uint32_t last_time = 0; uint32_t now = time_us_32(); debug("\nSmart Esc. Thr (%.0f%%) %u us %u interval us %i", throttle / 65532.0F * 100, throttle, pulse, now - last_time); last_time = now; } timeout_throttle_alarm_id = add_alarm_in_ms(PWM_TIMEOUT_MS, timeout_throttle_callback, NULL, true); } static void capture_pwm_reverse_handler(uint counter, edge_type_t edge) { static uint counter_edge_rise = 0; static alarm_id_t timeout_reverse_alarm_id = 0; if (timeout_reverse_alarm_id) cancel_alarm(timeout_reverse_alarm_id); if (edge == EDGE_RISE) { counter_edge_rise = counter; } else if (edge == EDGE_FALL && counter_edge_rise) { uint pulse = (float)(counter - counter_edge_rise) / clock_get_hz(clk_sys) * COUNTER_CYCLES * 1000000; int delta = pulse - 1000; if (delta < 0) delta = 0; if (delta > 1000) delta = 1000; reverse = delta / 1000.0F * 65532; // 0us->0% 1000us->100% // debug("\nSmart Esc. Rev (%.0f%%) %u us %u", reverse / 65532.0F * 100, reverse, pulse); } timeout_reverse_alarm_id = add_alarm_in_ms(PWM_TIMEOUT_MS, timeout_reverse_callback, NULL, true); } static int64_t timeout_throttle_callback(alarm_id_t id, void *user_data) { throttle = 0; debug("\nSmart Esc. Signal timeout. Throttle 0"); return 0; } static int64_t timeout_reverse_callback(alarm_id_t id, void *user_data) { reverse = 0; debug("\nSmart Esc. Signal timeout. Reverse 0"); return 0; } static int64_t alarm_packet(alarm_id_t id, void *user_data) { BaseType_t xHigherPriorityTaskWoken = pdFALSE; packet_pending = true; vTaskNotifyGiveIndexedFromISR(context.uart1_notify_task_handle, 1, &xHigherPriorityTaskWoken); portYIELD_FROM_ISR(xHigherPriorityTaskWoken); return SRXL2_INTERVAL_MS * 1000; } ================================================ FILE: board/project/sensor/smart_esc.h ================================================ #ifndef SMART_ESC_H #define SMART_ESC_H #include "common.h" typedef struct smart_esc_parameters_t { bool calc_consumption; float rpm_multiplier; float alpha_rpm, alpha_voltage, alpha_current, alpha_temperature; float *rpm, *voltage, *current, *temperature_fet, *temperature_bec, *voltage_bec, *current_bec; // esc float *temperature_bat, *current_bat, *consumption; // bat realtime float *cell[18]; // cells uint8_t *cells; // bat id uint16_t *cycles; // bat id } smart_esc_parameters_t; extern context_t context; void smart_esc_task(void *parameters); #endif ================================================ FILE: board/project/sensor/voltage.c ================================================ #include "voltage.h" #include #include "hardware/adc.h" #include "pico/stdlib.h" void voltage_task(void *parameters) { voltage_parameters_t parameter = *(voltage_parameters_t *)parameters; adc_init(); adc_gpio_init(parameter.adc_num + 26); //gpio_pull_down(parameter.adc_num + 26); *parameter.voltage = 0; xTaskNotifyGive(context.receiver_task_handle); while (1) { *parameter.voltage = get_average(parameter.alpha, *parameter.voltage, voltage_read(parameter.adc_num) * parameter.multiplier); #ifdef SIM_SENSORS *parameter.voltage = 12.34; #endif debug("\nVoltage (%u): %.2f", uxTaskGetStackHighWaterMark(NULL), *parameter.voltage); vTaskDelay(1000 / parameter.rate / portTICK_PERIOD_MS); } } ================================================ FILE: board/project/sensor/voltage.h ================================================ #ifndef VOLTAGE_H #define VOLTAGE_H #include "common.h" typedef struct voltage_parameters_t { uint8_t adc_num; uint8_t rate; float alpha, multiplier; float *voltage; } voltage_parameters_t; extern context_t context; void voltage_task(void *parameters); #endif ================================================ FILE: board/project/sensor/vspeed.c ================================================ #include "vspeed.h" #include #include "common.h" #include "pico/stdlib.h" #define VSPEED_INIT_DELAY_MS 5000 #define VSPEED_INTERVAL_MS 1000 void vspeed_task(void *parameters) { vspeed_parameters_t *parameter = (vspeed_parameters_t *)parameters; *parameter->vspeed = 0; vTaskDelay(VSPEED_INIT_DELAY_MS / portTICK_PERIOD_MS); float altitude = *parameter->altitude; while (1) { *parameter->vspeed = (*parameter->altitude - altitude) / VSPEED_INTERVAL_MS * 1000; altitude = *parameter->altitude; #ifdef SIM_SENSORS *parameter->vspeed = 12.34; #endif debug("\nVspeed (%u): %.2f", uxTaskGetStackHighWaterMark(NULL), *parameter->vspeed); vTaskDelay(parameter->interval / portTICK_PERIOD_MS); } } ================================================ FILE: board/project/sensor/vspeed.h ================================================ #ifndef VSPEED_H #define VSPEED_H #include "common.h" typedef struct vspeed_parameters_t { uint interval; float *altitude, *vspeed; } vspeed_parameters_t; extern context_t context; void vspeed_task(void *parameters); #endif ================================================ FILE: board/project/sensor/xgzp68xxd.c ================================================ #include "xgzp68xxd.h" #include #include "hardware/i2c.h" #include "pico/stdlib.h" #include "stdlib.h" #define I2C_ADDRESS 0X6D #define REG_CMD 0x30 #define REG_DATA 0X06 #define CONTROL_SINGLE 0b10 #define CONTROL_CONT 0b11 #define SLEEP_TIME_62 0b0001 // 62.5ms #define SLEEP_TIME_125 0b0010 // 125 ms #define SLEEP_TIME_187 0b0011 // 187.5ms #define SENSOR_INTERVAL_MS 100 // min 62.5ms static void read(xgzp68xxd_parameters_t *parameter); static void begin(void); void xgzp68xxd_task(void *parameters) { xgzp68xxd_parameters_t parameter = *(xgzp68xxd_parameters_t *)parameters; xTaskNotifyGive(context.receiver_task_handle); *parameter.temperature = 0; *parameter.pressure = 0; TaskHandle_t task_handle; vTaskDelay(500 / portTICK_PERIOD_MS); begin(); while (1) { read(¶meter); debug("\nXGZP68XXD (%u) < Temp(C): %.2f Pressure(kPa): %.2f", uxTaskGetStackHighWaterMark(NULL), *parameter.temperature, *parameter.pressure / 1000); vTaskDelay(SENSOR_INTERVAL_MS / portTICK_PERIOD_MS); } } static void read(xgzp68xxd_parameters_t *parameter) { int16_t temperature_raw; int32_t pressure_raw; uint8_t data[5]; uint8_t reg[1] = {REG_DATA}; i2c_write_blocking(i2c0, I2C_ADDRESS, reg, 1, true); i2c_read_blocking(i2c0, I2C_ADDRESS, data, 5, false); pressure_raw = (((uint32_t)data[0] << 24) | ((uint32_t)data[1] << 16) | ((uint32_t)data[2] << 8)) >> 8; temperature_raw = ((uint16_t)data[3] << 8) | data[4]; *parameter->temperature = temperature_raw / 256.0; // C *parameter->pressure = (float)pressure_raw / parameter->k; // Pa #ifdef SIM_SENSORS *parameter->temperature = 12.34; // C *parameter->pressure = 101325; // Pa #endif } static void begin(void) { i2c_init(i2c0, 400 * 1000); gpio_set_function(I2C0_SDA_GPIO, GPIO_FUNC_I2C); gpio_set_function(I2C0_SCL_GPIO, GPIO_FUNC_I2C); gpio_pull_up(I2C0_SDA_GPIO); gpio_pull_up(I2C0_SCL_GPIO); // Set continuous mode at 62.5ms interval uint8_t data[2]; data[0] = REG_CMD; data[1] = SLEEP_TIME_62; i2c_write_blocking(i2c0, I2C_ADDRESS, data, 2, false); // Wait for first reading vTaskDelay(20 / portTICK_PERIOD_MS); } ================================================ FILE: board/project/sensor/xgzp68xxd.h ================================================ #ifndef XGZP68XXD_H #define XGZP68XXD_H #include "common.h" typedef struct xgzp68xxd_parameters_t { uint16_t k; float *temperature, *pressure; } xgzp68xxd_parameters_t; extern context_t context; void xgzp68xxd_task(void *parameters); #endif ================================================ FILE: board/project/serial_monitor.c ================================================ #include "serial_monitor.h" #include #include #include "config.h" #include "pico/stdlib.h" #include "uart.h" #include "uart_pio.h" static void process(config_t *config); void serial_monitor_task(void *parameters) { xTaskNotifyGive(context.receiver_task_handle); config_t *config = config_read(); debug("\nSerial monitor init. GPIO: %uBaudrate: %u Stop bits: %u Parity: %u Inverted: %u Timeout (ms): %u", config->serial_monitor_gpio, config->serial_monitor_baudrate, config->serial_monitor_stop_bits, config->serial_monitor_parity, config->serial_monitor_inverted, config->serial_monitor_timeout_ms); switch (config->serial_monitor_gpio) { case 1: uart0_begin(config->serial_monitor_baudrate, 0, 1, config->serial_monitor_timeout_ms * 1000, 8, config->serial_monitor_stop_bits, config->serial_monitor_parity, config->serial_monitor_inverted, false); break; case 5: uart1_begin(config->serial_monitor_baudrate, 4, 5, config->serial_monitor_timeout_ms * 1000, 8, config->serial_monitor_stop_bits, config->serial_monitor_parity, config->serial_monitor_inverted, false); break; case 6: uart_pio_begin(config->serial_monitor_baudrate, UART_GPIO_NONE, 6, config->serial_monitor_timeout_ms * 1000, pio0, PIO0_IRQ_0, 8, config->serial_monitor_stop_bits, config->serial_monitor_parity); } while (1) { // debug("\n>> %u", config->serial_monitor_gpio); // vTaskDelay(1000 / portTICK_PERIOD_MS); ulTaskNotifyTakeIndexed(1, pdTRUE, portMAX_DELAY); process(config); } } static void process(config_t *config) { static uint ts = 0; uint length; uint8_t data[512]; switch (config->serial_monitor_gpio) { case 1: length = uart0_available(); if (!length) return; uart0_read_bytes(data, length); break; case 5: length = uart1_available(); if (!length) return; uart1_read_bytes(data, length); break; case 6: length = uart_pio_available(); if (!length) return; uart_pio_read_bytes(data, length); break; } if (config->serial_monitor_timeout_ms) debug("\nSerial monitor (%u). GPIO: %u Length: %u (%u ms): ", uxTaskGetStackHighWaterMark(NULL), config->serial_monitor_gpio, length, (time_us_32() - ts) / 1000); if (config->serial_monitor_format == FORMAT_HEX) { debug_buffer(data, length, " 0x%X"); } else if (context.debug) debug_buffer(data, length, "%c"); } ================================================ FILE: board/project/serial_monitor.h ================================================ #ifndef SERIAL_MONITOR_H #define SERIAL_MONITOR_H #include "common.h" extern context_t context; void serial_monitor_task(void *parameters); #endif ================================================ FILE: board/project/sim_rx.c ================================================ #include "sim_rx.h" #include #include "config.h" #include "hitec.h" #include "uart.h" #include "xbus.h" #define SIM_RX_INTERVAL_MS 1000 // ms #define SIM_RX_TIMEOUT_MS 1 // ms #define IBUS_COMMAND_DISCOVER 0x8 #define IBUS_COMMAND_TYPE 0x9 #define IBUS_COMMAND_MEASURE 0xA static uint8_t sim_rx_status = 0; static QueueHandle_t uart_queue_handle; static void process(rx_protocol_t rx_protocol); static void ibus_send_data(uint8_t command, uint8_t address); static void ibus_send_byte(uint8_t c, uint16_t *crcP); static uint16_t jetiex_crc16(uint8_t *p, uint16_t len); static uint16_t jetiex_update_crc16(uint16_t crc, uint8_t data); static uint8_t smartport_get_crc(uint8_t *data); void sim_rx_task(void *parameters) { sim_rx_parameters_t *parameter = (sim_rx_parameters_t *)parameters; vTaskDelay(2000 / portTICK_PERIOD_MS); if (UART_RECEIVER == uart0) uart_queue_handle = context.uart0_queue_handle; else uart_queue_handle = context.uart1_queue_handle; debug("\nSim Rx init"); while (1) { vTaskDelay(SIM_RX_INTERVAL_MS / portTICK_PERIOD_MS); process(parameter->rx_protocol); } } static void process(rx_protocol_t rx_protocol) { #ifdef SIM_RX // printf("\nSim (%u) < ", uxTaskGetStackHighWaterMark(NULL)); xQueueReset(uart_queue_handle); if (rx_protocol == RX_SMARTPORT) { vTaskResume(context.led_task_handle); uint8_t c[10] = {0}; c[0] = 0x7E; c[1] = 0x71; // sensor id 18 = 0x71 (10 = 0xE9) xQueueSendToBack(uart_queue_handle, &c[0], 0); xQueueSendToBack(uart_queue_handle, &c[1], 0); #ifdef SIM_SMARTPORT_SEND_CONFIG_LUA if (sim_rx_status == 0) // maintenance mode on { c[2] = 0x21; // type_id c[3] = 0xFF; // data_id c[4] = 0xFF; c[5] = 0x80; // value c[6] = 0x00; c[7] = 0x00; c[8] = 0x00; c[9] = smartport_get_crc(c); for (uint i = 2; i < 10; i++) xQueueSendToBack(uart_queue_handle, &c[i], 0); } else if (sim_rx_status == 1) // request config { c[2] = 0x30; // type_id c[3] = 0x00; // data_id c[4] = 0x50; c[5] = 0x00; // value c[6] = 0x00; c[7] = 0x00; c[8] = 0x00; c[9] = smartport_get_crc(c); for (uint i = 2; i < 10; i++) xQueueSendToBack(uart_queue_handle, &c[i], 0); } else if (sim_rx_status == 6) // maintenance mode off { c[2] = 0x20; // type_id c[3] = 0xFF; // data_id c[4] = 0xFF; c[5] = 0x80; // value c[6] = 0x00; c[7] = 0x00; c[8] = 0x00; c[9] = smartport_get_crc(c); for (uint i = 2; i < 10; i++) xQueueSendToBack(uart_queue_handle, &c[i], 0); } sim_rx_status++; #endif #ifdef SIM_SMARTPORT_RECEIVE_CONFIG_LUA if (sim_rx_status == 0) // maintenance mode on { c[2] = 0x21; // type_id c[3] = 0xFF; // data_id c[4] = 0xFF; c[5] = 0x80; // value c[6] = 0x00; c[7] = 0x00; c[8] = 0x00; c[9] = smartport_get_crc(c); for (uint i = 2; i < 10; i++) xQueueSendToBack(uart_queue_handle, &c[i], 0); } if (sim_rx_status == 1) // packet 1 { c[2] = 0x31; // type_id c[3] = 0x00; // data_id c[4] = 0x50; c[5] = 0xF1; // value c[6] = 0x00; c[7] = 0x00; c[8] = 0x00; c[9] = smartport_get_crc(c); for (uint i = 2; i < 10; i++) xQueueSendToBack(uart_queue_handle, &c[i], 0); } if (sim_rx_status == 2) // packet 2 { c[2] = 0x31; // type_id c[3] = 0x00; // data_id c[4] = 0x50; c[5] = 0xF2; // value c[6] = 0x00; c[7] = 0x00; c[8] = 0x00; c[9] = smartport_get_crc(c); for (uint i = 2; i < 10; i++) xQueueSendToBack(uart_queue_handle, &c[i], 0); } if (sim_rx_status == 3) // packet 3 { c[2] = 0x31; // type_id c[3] = 0x00; // data_id c[4] = 0x50; c[5] = 0xF3; // value c[6] = 0x00; c[7] = 0x00; c[8] = 0x00; c[9] = smartport_get_crc(c); for (uint i = 2; i < 10; i++) xQueueSendToBack(uart_queue_handle, &c[i], 0); } if (sim_rx_status == 5) // maintenance mode off { c[2] = 0x20; // type_id c[3] = 0xFF; // data_id c[4] = 0xFF; c[5] = 0x80; // value c[6] = 0x00; c[7] = 0x00; c[8] = 0x00; c[9] = smartport_get_crc(c); for (uint i = 2; i < 10; i++) xQueueSendToBack(uart_queue_handle, &c[i], 0); } sim_rx_status++; #endif #ifdef SIM_SMARTPORT_SEND_SENSOR_ID if (sim_rx_status == 0) // maintenance mode on { c[2] = 0x21; // type_id c[3] = 0xFF; // data_id c[4] = 0xFF; c[5] = 0x80; // value c[6] = 0x00; c[7] = 0x00; c[8] = 0x00; c[9] = smartport_get_crc(c); for (uint i = 2; i < 10; i++) xQueueSendToBack(uart_queue_handle, &c[i], 0); } else if (sim_rx_status == 1) // request sensor id { c[2] = 0x30; // type_id c[3] = 0x00; // data_id c[4] = 0x50; c[5] = 0x01; // value c[6] = 0x00; c[7] = 0x00; c[8] = 0x00; c[9] = smartport_get_crc(c); for (uint i = 2; i < 10; i++) xQueueSendToBack(uart_queue_handle, &c[i], 0); } else if (sim_rx_status == 3) // maintenance mode off { c[2] = 0x20; // type_id c[3] = 0xFF; // data_id c[4] = 0xFF; c[5] = 0x80; // value c[6] = 0x00; c[7] = 0x00; c[8] = 0x00; c[9] = smartport_get_crc(c); for (uint i = 2; i < 10; i++) xQueueSendToBack(uart_queue_handle, &c[i], 0); } sim_rx_status++; #endif #ifdef SIM_SMARTPORT_RECEIVE_SENSOR_ID if (sim_rx_status == 0) // maintenance mode on { c[2] = 0x21; // type_id c[3] = 0xFF; // data_id c[4] = 0xFF; c[5] = 0x80; // value c[6] = 0x00; c[7] = 0x00; c[8] = 0x00; c[9] = smartport_get_crc(c); for (uint i = 2; i < 10; i++) xQueueSendToBack(uart_queue_handle, &c[i], 0); } else if (sim_rx_status == 1) // change sensor id { c[2] = 0x31; // type_id c[3] = 0x00; // data_id c[4] = 0x50; c[5] = 0x01; // value c[6] = 10 - 1; // sensor id = 10 -> lua sensor id = 9 c[7] = 0x00; c[8] = 0x00; c[9] = smartport_get_crc(c); for (uint i = 2; i < 10; i++) xQueueSendToBack(uart_queue_handle, &c[i], 0); } else if (sim_rx_status == 3) // maintenance mode off { c[2] = 0x20; // type_id c[3] = 0xFF; // data_id c[4] = 0xFF; c[5] = 0x80; // value c[6] = 0x00; c[7] = 0x00; c[8] = 0x00; c[9] = smartport_get_crc(c); for (uint i = 2; i < 10; i++) xQueueSendToBack(uart_queue_handle, &c[i], 0); } sim_rx_status++; #endif } else if (rx_protocol == RX_XBUS) { uint8_t sensor_id[] = { XBUS_AIRSPEED_ID, XBUS_ALTIMETER_ID, XBUS_GPS_LOC_ID, XBUS_GPS_STAT_ID, XBUS_ESC_ID, XBUS_BATTERY_ID, XBUS_VARIO_ID, XBUS_RPMVOLTTEMP_ID, XBUS_FUEL_FLOW_ID, XBUS_STRU_TELE_DIGITAL_AIR_ID}; static uint8_t sensor_index = 0; xbus_i2c_handler(sensor_id[sensor_index]); sensor_index++; if (sensor_index > sizeof(sensor_id)) sensor_index = 0; } else if (rx_protocol == RX_SRXL) { static uint8_t data[] = {0xA5, 0x12, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; for (uint8_t i = 0; i < sizeof(data); i++) { xQueueSendToBack(uart_queue_handle, &data[i], 0); } } else if (rx_protocol == RX_SRXL2) { static uint8_t status = 0; static uint8_t handshake_request[] = {0xA6, 0x21, 0xE, 0x10, 0x30, 0xA, 0x1, 0x1, 0xFC, 0x96, 0x8C, 0x4B, 0x30, 0xD9}; static uint8_t telemetry_request[] = {0xA6, 0xCD, 0x14, 0x0, 0x30, 0xEC, 0x0, 0x0, 0x91, 0x0, 0x0, 0x0, 0x60, 0x80, 0x0, 0x80, 0x0, 0x80, 0xB2, 0xA0}; if (status < 10) { for (uint8_t i = 0; i < sizeof(handshake_request); i++) { xQueueSendToBack(uart_queue_handle, &handshake_request[i], 0); } status++; } else if (status == 10) { for (uint8_t i = 0; i < sizeof(telemetry_request); i++) { xQueueSendToBack(uart_queue_handle, &telemetry_request[i], 0); } } } else if (rx_protocol == RX_FRSKY_D) { } else if (rx_protocol == RX_IBUS) { static uint8_t command = IBUS_COMMAND_DISCOVER; static uint8_t address = 0; command = IBUS_COMMAND_MEASURE; ibus_send_data(command, address); address++; if (address == 16) { address = 0; if (command < IBUS_COMMAND_MEASURE) command++; } } else if (rx_protocol == RX_SBUS) { static uint8_t data[] = {0x0F, 0x0F, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x04}; for (uint8_t i = 0; i < sizeof(data); i++) { xQueueSendToBack(uart_queue_handle, &data[i], 0); } data[24] += 0x10; if (data[24] == 0x44) data[24] = 0x04; } else if (rx_protocol == RX_MULTIPLEX) { static uint8_t cont = 0; xQueueSendToBack(uart_queue_handle, &cont, 0); cont++; cont = cont % 16; } else if (rx_protocol == RX_JETIEX) { uint8_t data[] = {0x3E, 0x3, 0x28, 0x2, 0x31, 0x20, 0x80, 0x3E, 0xDD, 0x2E, 0xEB, 0x2E, 0xEC, 0x2E, 0xE0, 0x2E, 0xE0, 0x2E, 0xE0, 0x2E, 0xE0, 0x2E, 0xE0, 0x2E, 0xE0, 0x2E, 0xE0, 0x2E, 0xE0, 0x2E, 0xE0, 0x2E, 0xE0, 0x2E, 0xE0, 0x2E, 0xE0, 0x2E, 0xC5, 0xFE, 0x3D, 0x1, 0x8, 0x2, 0x3A, 0x0, 0xF9, 0xE2}; for (uint8_t i = 0; i < sizeof(data); i++) { xQueueSendToBack(uart_queue_handle, &data[i], 0); } } else if (rx_protocol == RX_HITEC) { hitec_i2c_handler(); } else if (rx_protocol == RX_CRSF) { // uint8_t data[] = {0xC8, 0x06, 0x2C, 0xEE, 0xEF, 0x01, 0x00, 0x76}; // uint8_t data[] = {0xC8, 0x4, 0x7, 0x0, 0x5, 0x8}; uint8_t data[] = {0xC8, 0x18, 0x16, 0xDB, 0xE3, 0x62, 0xF9, 0xB6, 0xF7, 0x8B, 0xF2, 0x95, 0xAF, 0x7C, 0xE5, 0x2B, 0x5F, 0xF9, 0xCA, 0x7, 0x0, 0x0, 0x4C, 0x7C, 0xE2, 0xA7}; /*uint8_t data[] = {0xC8, 0xC, 0x14, 0x11, 0x0, 0x64, 0xB, 0x0, 0x9, 0x7, 0x0, 0x0, 0x0, 0xF8, 0xC8, 0x18, 0x16, 0xDB, 0xE3, 0x62, 0xF9, 0xB6, 0xF7, 0x8B, 0xF2, 0x95, 0xAF, 0x7C, 0xE5, 0x2B, 0x5F, 0xF9, 0xCA, 0x7, 0x0, 0x0, 0x4C, 0x7C, 0xE2, 0xA7};*/ for (uint8_t i = 0; i < sizeof(data); i++) { xQueueSendToBack(uart_queue_handle, &data[i], 0); } } else if (rx_protocol == RX_SANWA) { static uint8_t type = 0; static const uint8_t data[5][10] = {{0x1, 0x4, 0x82, 0x2, 0xFF, 0x0, 0x3, 0x3, 0xFF, 0x8D}, {0x1, 0x1, 0x30, 0x4, 0xA7, 0x3, 0xFF, 0x3, 0xFF, 0xE1}, {0x1, 0x1, 0x31, 0x4, 0xA7, 0x0, 0x3, 0x3, 0xFF, 0xE3}, {0x1, 0x1, 0x30, 0x4, 0xA7, 0x3, 0xFF, 0x3, 0xFF, 0xE1}, {0x1, 0x1, 0x31, 0x4, 0xA8, 0x0, 0x3, 0x3, 0xFF, 0xE4}}; for (uint8_t i = 0; i < sizeof(data[type % 5]); i++) xQueueSendToBack(uart_queue_handle, &data[type % 5][i], 0); type++; } else if (rx_protocol == RX_HOTT) { uint8_t address[5] = {0x89, 0x8A, 0x8C, 0x8D, 0x8E}; static uint index = 0; uint8_t type = 0x80; xQueueSendToBack(uart_queue_handle, &type, 0); xQueueSendToBack(uart_queue_handle, &address[index % 5], 0); index++; } else if (rx_protocol == RX_JR_PROPO) { static uint address = 0; xQueueSendToBack(uart_queue_handle, &address, 0); address++; if (address > 10) address = 0; } else if (rx_protocol == RX_FPORT) { uint8_t data[] = {0x7E, 0x19, 0x00, 0xAB, 0x00, 0x1F, 0x2B, 0xA2, 0x07, 0x3E, 0xF0, 0x81, 0xAF, 0x7C, 0x08, 0x40, 0x00, 0x02, 0x10, 0x80, 0x00, 0x04, 0x20, 0x00, 0x01, 0x00, 0x64, 0x06, 0x7E, 0x7E, 0x08, 0x01, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xE6, 0x7E}; for (uint8_t i = 0; i < sizeof(data); i++) { xQueueSendToBack(uart_queue_handle, &data[i], 0); } } else if (rx_protocol == RX_FBUS) { // len 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 crc uint8_t data[] = {0x18, 0xFF, 0x02, 0x07, 0x1F, 0x2B, 0xA2, 0x07, 0x3E, 0xF0, 0x81, 0xAF, 0x7C, 0xDC, 0x03, 0x1F, 0xF8, 0xC0, 0x07, 0x3E, 0xF0, 0x09, 0x7C, 0x15, 0x00, 0x64, 0x38, 0x08, 0x2F, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xC0}; for (uint8_t i = 0; i < sizeof(data); i++) { xQueueSendToBack(uart_queue_handle, &data[i], 0); } } else if (rx_protocol == RX_GHST) { uint8_t data[] = {0x89, 0x0C, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xEA}; for (uint8_t i = 0; i < sizeof(data); i++) { xQueueSendToBack(uart_queue_handle, &data[i], 0); } } uart0_set_timestamp(); vTaskDelay(SIM_RX_TIMEOUT_MS / portTICK_PERIOD_MS); xTaskNotifyGiveIndexed(context.uart0_notify_task_handle, 1); #endif } static void ibus_send_data(uint8_t command, uint8_t address) { uint16_t crc = 0; uint8_t *u8P; // lenght ibus_send_byte(4, &crc); // command & address ibus_send_byte(command << 4 | address, &crc); // crc crc = 0xFFFF - crc; u8P = (uint8_t *)&crc; ibus_send_byte(u8P[0], NULL); ibus_send_byte(u8P[1], NULL); } void ibus_send_byte(uint8_t c, uint16_t *crcP) { if (crcP != NULL) { uint16_t crc = *crcP; crc += c; *crcP = crc; } xQueueSendToBack(uart_queue_handle, &c, 0); } static uint16_t jetiex_crc16(uint8_t *p, uint16_t len) { uint16_t crc16_data = 0; while (len--) { crc16_data = jetiex_update_crc16(crc16_data, p[0]); p++; } return (crc16_data); } static uint16_t jetiex_update_crc16(uint16_t crc, uint8_t data) { uint16_t ret_val; data ^= (uint8_t)(crc) & (uint8_t)(0xFF); data ^= data << 4; ret_val = ((((uint16_t)data << 8) | ((crc & 0xFF00) >> 8)) ^ (uint8_t)(data >> 4) ^ ((uint16_t)data << 3)); return ret_val; } static uint8_t smartport_get_crc(uint8_t *data) { uint16_t crc = 0; for (uint8_t i = 2; i < 9; i++) { crc += data[i]; crc += crc >> 8; crc &= 0x00FF; } return 0xFF - (uint8_t)crc; } ================================================ FILE: board/project/sim_rx.h ================================================ #ifndef SIM_RX_H #define SIM_RX_H #include "common.h" extern context_t context; typedef struct sim_rx_parameters_t { rx_protocol_t rx_protocol; } sim_rx_parameters_t; void sim_rx_task(void *parameters); #endif ================================================ FILE: board/project/uart.c ================================================ #include "uart.h" #include #include "hardware/irq.h" #include "hardware/uart.h" #define UART0_BUFFER_SIZE 512 #define UART1_BUFFER_SIZE 512 #define enable_rx(UART) hw_set_bits(&uart_get_hw(UART)->cr, 0x00000200) #define disable_rx(UART) hw_clear_bits(&uart_get_hw(UART)->cr, 0x00000200) static volatile uint uart0_timeout, uart1_timeout, uart0_timestamp, uart1_timestamp; static volatile bool uart0_is_timedout = true, uart1_is_timedout = true; static bool half_duplex0, half_duplex1, inverted0, inverted1; static int gpio_tx0, gpio_rx0, gpio_tx1, gpio_rx1; static int64_t uart0_timeout_callback(alarm_id_t id, void *user_data); static int64_t uart1_timeout_callback(alarm_id_t id, void *user_data); static void uart0_rx_handler(); static void uart1_rx_handler(); static void uart_reset_queue_from_isr(QueueHandle_t queue_handle, BaseType_t *xHigherPriorityTaskWoken); void uart0_begin(uint baudrate, uint gpio_tx, uint gpio_rx, uint timeout, uint databits, uint stopbits, uart_parity_t parity, bool inverted, bool half_duplex) { half_duplex0 = half_duplex; gpio_tx0 = gpio_tx; gpio_rx0 = gpio_rx; inverted0 = inverted; if (context.uart_alarm_pool == NULL) context.uart_alarm_pool = alarm_pool_create(2, 10); uart_init(uart0, baudrate); uart_set_fifo_enabled(uart0, false); gpio_set_function(gpio_rx, GPIO_FUNC_UART); if (!half_duplex) gpio_set_function(gpio_tx, GPIO_FUNC_UART); if (!inverted) { gpio_pull_up(gpio_tx); gpio_pull_up(gpio_rx); } else { gpio_pull_down(gpio_tx); gpio_pull_down(gpio_rx); gpio_set_outover(gpio_tx, GPIO_OVERRIDE_INVERT); gpio_set_inover(gpio_rx, GPIO_OVERRIDE_INVERT); } uart_set_format(uart0, databits, stopbits, parity); irq_set_exclusive_handler(UART0_IRQ, uart0_rx_handler); irq_set_enabled(UART0_IRQ, true); uart0_timeout = timeout; context.uart0_queue_handle = xQueueCreate(UART0_BUFFER_SIZE, sizeof(uint8_t)); uart_set_irq_enables(uart0, true, false); } void uart1_begin(uint baudrate, uint gpio_tx, uint gpio_rx, uint timeout, uint databits, uint stopbits, uart_parity_t parity, bool inverted, bool half_duplex) { half_duplex1 = half_duplex; gpio_tx1 = gpio_tx; gpio_rx1 = gpio_rx; inverted1 = inverted; if (context.uart_alarm_pool == NULL) context.uart_alarm_pool = alarm_pool_create(2, 10); uart_init(uart1, baudrate); uart_set_fifo_enabled(uart1, false); gpio_set_function(gpio_rx, GPIO_FUNC_UART); if (!half_duplex) gpio_set_function(gpio_tx, GPIO_FUNC_UART); if (!inverted) { gpio_pull_up(gpio_tx); gpio_pull_up(gpio_rx); } else { gpio_pull_down(gpio_tx); gpio_pull_down(gpio_rx); gpio_set_outover(gpio_tx, GPIO_OVERRIDE_INVERT); gpio_set_inover(gpio_rx, GPIO_OVERRIDE_INVERT); } uart_set_format(uart1, databits, stopbits, parity); irq_set_exclusive_handler(UART1_IRQ, uart1_rx_handler); irq_set_enabled(UART1_IRQ, true); uart1_timeout = timeout; context.uart1_queue_handle = xQueueCreate(UART1_BUFFER_SIZE, sizeof(uint8_t)); uart_set_irq_enables(uart1, true, false); } static int64_t uart0_timeout_callback(alarm_id_t id, void *user_data) { BaseType_t xHigherPriorityTaskWoken = pdFALSE; uart0_is_timedout = true; vTaskNotifyGiveIndexedFromISR(context.uart0_notify_task_handle, 1, &xHigherPriorityTaskWoken); portYIELD_FROM_ISR(xHigherPriorityTaskWoken); return 0; } static int64_t uart1_timeout_callback(alarm_id_t id, void *user_data) { BaseType_t xHigherPriorityTaskWoken = pdFALSE; uart1_is_timedout = true; vTaskNotifyGiveIndexedFromISR(context.uart1_notify_task_handle, 1, &xHigherPriorityTaskWoken); portYIELD_FROM_ISR(xHigherPriorityTaskWoken); return 0; } static void uart_reset_queue_from_isr(QueueHandle_t queue_handle, BaseType_t *xHigherPriorityTaskWoken) { uint8_t discarded; while (uxQueueMessagesWaitingFromISR(queue_handle) > 0) { xQueueReceiveFromISR(queue_handle, &discarded, xHigherPriorityTaskWoken); } } static void uart0_rx_handler() { BaseType_t xHigherPriorityTaskWoken = pdFALSE; static alarm_id_t uart0_timeout_alarm_id = 0; if (uart0_timeout_alarm_id) alarm_pool_cancel_alarm(context.uart_alarm_pool, uart0_timeout_alarm_id); if (uart0_is_timedout) { uart_reset_queue_from_isr(context.uart0_queue_handle, &xHigherPriorityTaskWoken); uart0_is_timedout = false; } while (uart_is_readable(uart0)) { uint8_t data = uart_getc(uart0); // debug("-%X-", data); xQueueSendToBackFromISR(context.uart0_queue_handle, &data, &xHigherPriorityTaskWoken); if (uart0_timeout == 0) { vTaskNotifyGiveIndexedFromISR(context.uart0_notify_task_handle, 1, &xHigherPriorityTaskWoken); portYIELD_FROM_ISR(xHigherPriorityTaskWoken); } } if (uart0_timeout) { uart0_timeout_alarm_id = alarm_pool_add_alarm_in_us(context.uart_alarm_pool, uart0_timeout, uart0_timeout_callback, NULL, true); } uart0_timestamp = time_us_32(); portYIELD_FROM_ISR(xHigherPriorityTaskWoken); } static void uart1_rx_handler() { BaseType_t xHigherPriorityTaskWoken = pdFALSE; static alarm_id_t uart1_timeout_alarm_id = 0; if (uart1_timeout_alarm_id) { alarm_pool_cancel_alarm(context.uart_alarm_pool, uart1_timeout_alarm_id); } if (uart1_is_timedout) { uart_reset_queue_from_isr(context.uart1_queue_handle, &xHigherPriorityTaskWoken); uart1_is_timedout = false; } while (uart_is_readable(uart1)) { uint8_t data = uart_getc(uart1); // debug("%X ", data); xQueueSendToBackFromISR(context.uart1_queue_handle, &data, &xHigherPriorityTaskWoken); if (uart1_timeout == 0) { vTaskNotifyGiveIndexedFromISR(context.uart1_notify_task_handle, 1, &xHigherPriorityTaskWoken); portYIELD_FROM_ISR(xHigherPriorityTaskWoken); } } if (uart1_timeout) { uart1_timeout_alarm_id = alarm_pool_add_alarm_in_us(context.uart_alarm_pool, uart1_timeout, uart1_timeout_callback, NULL, true); } uart1_timestamp = time_us_32(); portYIELD_FROM_ISR(xHigherPriorityTaskWoken); } uint8_t uart0_read() { uint8_t value = 0; xQueueReceive(context.uart0_queue_handle, &value, 0); return value; } uint8_t uart1_read() { uint8_t value = 0; xQueueReceive(context.uart1_queue_handle, &value, 0); return value; } void uart0_read_bytes(uint8_t *data, uint8_t lenght) { for (uint8_t i = 0; i < lenght; i++) xQueueReceive(context.uart0_queue_handle, data + i, 0); } void uart1_read_bytes(uint8_t *data, uint8_t lenght) { for (uint8_t i = 0; i < lenght; i++) xQueueReceive(context.uart1_queue_handle, data + i, 0); } void uart0_write(uint8_t data) { if (half_duplex0) { disable_rx(uart0); gpio_set_function(gpio_tx0, GPIO_FUNC_UART); if (inverted0) gpio_set_outover(gpio_tx0, GPIO_OVERRIDE_INVERT); } uart_putc_raw(uart0, data); if (half_duplex0) { uart_tx_wait_blocking(uart0); enable_rx(uart0); gpio_set_function(gpio_tx0, GPIO_FUNC_NULL); } } void uart1_write(uint8_t data) { if (half_duplex1) { disable_rx(uart1); gpio_set_function(gpio_tx1, GPIO_FUNC_UART); if (inverted1) gpio_set_outover(gpio_tx1, GPIO_OVERRIDE_INVERT); } uart_putc_raw(uart1, data); if (half_duplex1) { uart_tx_wait_blocking(uart1); enable_rx(uart1); gpio_set_function(gpio_tx1, GPIO_FUNC_NULL); } } void uart0_write_bytes(uint8_t *data, uint8_t lenght) { if (half_duplex0) { disable_rx(uart0); gpio_set_function(gpio_tx0, GPIO_FUNC_UART); if (inverted0) gpio_set_outover(gpio_tx0, GPIO_OVERRIDE_INVERT); } uart_write_blocking(uart0, data, lenght); if (half_duplex0) { uart_tx_wait_blocking(uart0); enable_rx(uart0); gpio_set_function(gpio_tx0, GPIO_FUNC_NULL); } } void uart1_write_bytes(uint8_t *data, uint8_t lenght) { if (half_duplex1) { disable_rx(uart1); gpio_set_function(gpio_tx1, GPIO_FUNC_UART); if (inverted1) gpio_set_outover(gpio_tx1, GPIO_OVERRIDE_INVERT); } uart_write_blocking(uart1, data, lenght); if (half_duplex1) { uart_tx_wait_blocking(uart1); enable_rx(uart1); gpio_set_function(gpio_tx1, GPIO_FUNC_NULL); } } uint8_t uart0_available() { return uxQueueMessagesWaiting(context.uart0_queue_handle); } uint8_t uart1_available() { return uxQueueMessagesWaiting(context.uart1_queue_handle); } uint uart0_get_time_elapsed() { return time_us_32() - uart0_timestamp; } uint uart1_get_time_elapsed() { return time_us_32() - uart1_timestamp; } /* Use with sim rx */ void uart0_set_timestamp() { uart0_timestamp = time_us_32(); } void uart1_set_timestamp() { uart1_timestamp = time_us_32(); } ================================================ FILE: board/project/uart.h ================================================ #ifndef UART_H #define UART_H #include "common.h" extern context_t context; void uart0_begin(uint baudrate, uint gpio_tx, uint gpio_rx, uint timeout, uint databits, uint stopbits, uart_parity_t parity, bool inverted, bool half_duplex); uint8_t uart0_read(); void uart0_read_bytes(uint8_t *data, uint8_t lenght); uint8_t uart0_available(); uint uart0_get_time_elapsed(); void uart0_write(uint8_t data); void uart0_write_bytes(uint8_t *data, uint8_t lenght); void uart1_begin(uint baudrate, uint gpio_tx, uint gpio_rx, uint timeout, uint databits, uint stopbits, uart_parity_t parity, bool inverted, bool half_duplex); uint8_t uart1_read(); void uart1_read_bytes(uint8_t *data, uint8_t lenght); uint8_t uart1_available(); uint uart1_get_time_elapsed(); void uart1_write(uint8_t data); void uart1_write_bytes(uint8_t *data, uint8_t lenght); void uart0_set_timestamp(); void uart1_set_timestamp(); #endif ================================================ FILE: board/project/uart_pio.c ================================================ #include "uart_pio.h" #include #include "common.h" #include "hardware/irq.h" #include "hardware/pio.h" static volatile uint uart_pio_timeout, uart_pio_timestamp; static volatile bool uart_pio_is_timedout = true; static uint uart_pio_sm; static int64_t uart_pio_timeout_callback(alarm_id_t id, void *user_data); static void uart_pio_handler(uint8_t data); static void uart_pio_reset_queue_from_isr(BaseType_t *xHigherPriorityTaskWoken); void uart_pio_begin(uint baudrate, int gpio_tx, int gpio_rx, uint timeout, PIO pio, uint irq, uint8_t data_bits, uint8_t stop_bits, uint8_t parity) { if (gpio_rx != UART_GPIO_NONE) { if (context.uart_alarm_pool == NULL) context.uart_alarm_pool = alarm_pool_create(2, 10); uart_pio_sm = uart_rx_init(pio, gpio_rx, baudrate, irq); uart_rx_set_handler(uart_pio_handler); uart_pio_timeout = timeout; context.uart_rx_pio_queue_handle = xQueueCreate(UART_PIO_BUFFER_SIZE, sizeof(uint8_t)); } if (gpio_tx != UART_GPIO_NONE) { uart_pio_sm = uart_tx_init(pio, gpio_tx, baudrate, data_bits, stop_bits, parity); } } static int64_t uart_pio_timeout_callback(alarm_id_t id, void *user_data) { BaseType_t xHigherPriorityTaskWoken = pdFALSE; uart_pio_is_timedout = true; // printf("\n"); vTaskNotifyGiveIndexedFromISR(context.uart_pio_notify_task_handle, 1, &xHigherPriorityTaskWoken); portYIELD_FROM_ISR(xHigherPriorityTaskWoken); return 0; } static void uart_pio_reset_queue_from_isr(BaseType_t *xHigherPriorityTaskWoken) { uint8_t discarded; while (uxQueueMessagesWaitingFromISR(context.uart_rx_pio_queue_handle) > 0) { xQueueReceiveFromISR(context.uart_rx_pio_queue_handle, &discarded, xHigherPriorityTaskWoken); } } static void uart_pio_handler(uint8_t data) { BaseType_t xHigherPriorityTaskWoken = pdFALSE; static alarm_id_t uart_pio_timeout_alarm_id = 0; if (uart_pio_timeout_alarm_id) alarm_pool_cancel_alarm(context.uart_alarm_pool, uart_pio_timeout_alarm_id); if (uart_pio_is_timedout) { uart_pio_reset_queue_from_isr(&xHigherPriorityTaskWoken); uart_pio_is_timedout = false; } // debug("-%X-", data); xQueueSendToBackFromISR(context.uart_rx_pio_queue_handle, &data, &xHigherPriorityTaskWoken); if (uart_pio_timeout == 0) { BaseType_t xHigherPriorityTaskWoken = pdFALSE; vTaskNotifyGiveIndexedFromISR(context.uart_pio_notify_task_handle, 1, &xHigherPriorityTaskWoken); portYIELD_FROM_ISR(xHigherPriorityTaskWoken); } if (uart_pio_timeout) { uart_pio_timeout_alarm_id = alarm_pool_add_alarm_in_us(context.uart_alarm_pool, uart_pio_timeout, uart_pio_timeout_callback, NULL, true); } uart_pio_timestamp = time_us_32(); portYIELD_FROM_ISR(xHigherPriorityTaskWoken); } uint8_t uart_pio_read(void) { uint8_t value = 0; xQueueReceive(context.uart_rx_pio_queue_handle, &value, 0); return value; } void uart_pio_read_bytes(uint8_t *data, uint8_t lenght) { for (uint8_t i = 0; i < lenght; i++) { xQueueReceive(context.uart_rx_pio_queue_handle, data + i, 0); } } void uart_pio_write(uint32_t c) { uart_tx_write(c); } void uart_pio_write_bytes(void *data, uint8_t lenght) { uart_tx_write_bytes(data, lenght); } uint8_t uart_pio_available(void) { return uxQueueMessagesWaiting(context.uart_rx_pio_queue_handle); } uint8_t uart_pio_tx_available(void) { return UART_PIO_BUFFER_SIZE - uxQueueMessagesWaiting(context.uart_rx_pio_queue_handle); } uint uart_pio_get_time_elapsed(void) { return time_us_32() - uart_pio_timestamp; } void uart_pio_remove(void) { uart_rx_remove(); uart_tx_remove(); } ================================================ FILE: board/project/uart_pio.h ================================================ #ifndef UART_PIO_H #define UART_PIO_H #include "common.h" #include "uart_rx.h" #include "uart_tx.h" #define UART_PIO_BUFFER_SIZE 2048 #define UART_GPIO_NONE -1 extern context_t context; void uart_pio_begin(uint baudrate, int gpio_tx, int gpio_rx, uint timeout, PIO pio, uint irq, uint8_t data_bits, uint8_t stop_bits, uint8_t parity); uint8_t uart_pio_read(void); void uart_pio_read_bytes(uint8_t *data, uint8_t lenght); void uart_pio_write(uint32_t c); void uart_pio_write_bytes(void *data, uint8_t lenght); uint8_t uart_pio_available(void); uint8_t uart_pio_tx_available(void); void uart_pio_remove(void); #endif ================================================ FILE: board/project/usb.c ================================================ #include "usb.h" #include #include #include "config.h" #include "pico/stdlib.h" #include "string.h" #define AIRCR_Register (*((volatile uint32_t *)(PPB_BASE + 0x0ED0C))) #define USB_BUFFER_LENGTH 256 #define USB_INTERVAL_MS 1000 static uint8_t buffer_rx[USB_BUFFER_LENGTH]; static int read_usb(); static void process_usb(int lenght); void usb_task() { while (1) { int length = read_usb(); if (length) process_usb(length); debug("\nUSB (%u)", uxTaskGetStackHighWaterMark(NULL)); vTaskDelay(USB_INTERVAL_MS / portTICK_PERIOD_MS); } } static void process_usb(int lenght) { debug("\nUSB. Processing (%i) 0x%X 0x%X", lenght, buffer_rx[0], buffer_rx[1]); /* header - 0x30 command - 0x30 - read config from usb 0x31 - request to send config to usb 0x32 - answer to send config 0x33 - debug on 0x34 - debug off */ if (buffer_rx[0] == 0x30) { if (buffer_rx[1] == 0x30 && lenght == sizeof(config_t) + 2) { // read config from usb, write to flash and reboot config_t *config = (config_t *)(buffer_rx + 2); if (config->version == CONFIG_VERSION) { config->rpm_multiplier = config->pinionTeeth / (1.0 * config->mainTeeth * config->pairOfPoles); context.led_cycles = 3; context.led_cycle_duration = 1000; vTaskResume(context.led_task_handle); vTaskDelay(3000 / portTICK_PERIOD_MS); context.led_cycles = 1; context.led_cycle_duration = 6; vTaskResume(context.led_task_handle); config_write(config); debug("\nUSB. Updated config"); // sleep_ms(1000); // AIRCR_Register = 0x5FA0004; } else debug("\nUSB. Incompatible config version"); } else if (buffer_rx[1] == 0x31 && lenght == 2) { // send config uint8_t debug_state = context.debug; context.debug = 0; context.led_cycles = 2; context.led_cycle_duration = 1000; vTaskResume(context.led_task_handle); vTaskDelay(2000 / portTICK_PERIOD_MS); context.led_cycles = 1; context.led_cycle_duration = 6; vTaskResume(context.led_task_handle); config_t *config = config_read(); putchar_raw(0x30); putchar_raw(0x32); uint8_t data[sizeof(config_t)]; memcpy(data, config, sizeof(config_t)); for (int i = 0; i < sizeof(config_t); i++) putchar_raw(data[i]); if (debug_state) vTaskDelay(1000 / portTICK_PERIOD_MS); context.debug = debug_state; debug("\nUSB. Send config"); } else if (buffer_rx[1] == 0x33 && lenght == 2) { // debug enable context.debug = 1; debug("\nUSB. Debug enabled. MSRC %s", PROJECT_VERSION); } else if (buffer_rx[1] == 0x34 && lenght == 2) { // debug disable context.debug = 0; debug("\nUSB. Debug disabled"); } else if (buffer_rx[1] == 0x35 && lenght == 2) { // force save default config to flash config_forze_write(); debug("\nUSB. Default config saved to flash"); } } } static int read_usb() { int buffer_index = 0; while (1) { int c = getchar_timeout_us(1000); if (c != PICO_ERROR_TIMEOUT && buffer_index < USB_BUFFER_LENGTH) { buffer_rx[buffer_index++] = (c & 0xFF); } else { break; } } return buffer_index; } ================================================ FILE: board/project/usb.h ================================================ #ifndef USB_H #define USB_H #include "common.h" extern context_t context; void usb_task(); extern context_t context; #endif ================================================ FILE: case/MSRC_case_zero-Lower case.stl ================================================ version https://git-lfs.github.com/spec/v1 oid sha256:706d1712a05c26d363e756b3c37a33e08091c530aa5a3a4d4ae283afff7a6ff0 size 123084 ================================================ FILE: case/MSRC_case_zero-Upper case.stl ================================================ version https://git-lfs.github.com/spec/v1 oid sha256:9e29e04059e759b96d55ae8b1698df50249703a28a3ffe5457d4ccf6f9c29b4c size 160284 ================================================ FILE: case/MSRC_case_zero.3mf ================================================ version https://git-lfs.github.com/spec/v1 oid sha256:df72a711ca0eb78f81886c5d42da3f04503d07448e117a83678408008134164a size 84013 ================================================ FILE: case/MSRC_case_zero.FCStd ================================================ version https://git-lfs.github.com/spec/v1 oid sha256:f958637abcdd52ebfe8e2f99acd9b631e951af492a2a80fb3f67ae941ddf843e size 3487729 ================================================ FILE: case/MSRC_case_zero_PLA_1h27m.gcode ================================================ version https://git-lfs.github.com/spec/v1 oid sha256:11042343aad94bb6fef96a9717a1ab6ead31a9674e0f2fc6b89eadc720c977dd size 2216642 ================================================ FILE: include/shared.h ================================================ #ifndef SHARED_H #define SHARED_H #include #ifdef __cplusplus typedef enum rx_protocol_t : uint8_t { RX_SMARTPORT, RX_FRSKY_D, RX_XBUS, RX_SRXL, RX_IBUS, RX_SBUS, RX_MULTIPLEX, RX_JETIEX, RX_HITEC, RX_SRXL2, SERIAL_MONITOR, RX_CRSF, RX_HOTT, RX_SANWA, RX_JR_PROPO, RX_FPORT, RX_FBUS, RX_GHST, RX_JETIEX_SENSOR } rx_protocol_t; typedef enum esc_protocol_t : uint8_t { ESC_NONE, ESC_HW3, ESC_HW4, ESC_PWM, ESC_CASTLE, ESC_KONTRONIK, ESC_APD_F, ESC_APD_HV, ESC_HW5, ESC_SMART, ESC_OMP_M4, ESC_ZTW, ESC_OPENYGE } esc_protocol_t; typedef enum i2c_module_t : uint8_t { I2C_NONE, I2C_BMP280, I2C_MS5611, I2C_BMP180 } i2c_module_t; typedef enum analog_current_type_t : uint8_t { CURRENT_TYPE_HALL, CURRENT_TYPE_SHUNT } analog_current_type_t; typedef enum serial_monitor_format_t : uint8_t { FORMAT_HEX, FORMAT_STRING } serial_monitor_format_t; typedef enum gps_protocol_t : uint8_t { UBLOX, NMEA } gps_protocol_t; #else typedef enum rx_protocol_t { RX_SMARTPORT, RX_FRSKY_D, RX_XBUS, RX_SRXL, RX_IBUS, RX_SBUS, RX_MULTIPLEX, RX_JETIEX, RX_HITEC, RX_SRXL2, SERIAL_MONITOR, RX_CRSF, RX_HOTT, RX_SANWA, RX_JR_PROPO, RX_FPORT, RX_FBUS, RX_GHST, RX_JETIEX_SENSOR } rx_protocol_t; typedef enum esc_protocol_t { ESC_NONE, ESC_HW3, ESC_HW4, ESC_PWM, ESC_CASTLE, ESC_KONTRONIK, ESC_APD_F, ESC_APD_HV, ESC_HW5, ESC_SMART, ESC_OMP_M4, ESC_ZTW, ESC_OPENYGE } esc_protocol_t; typedef enum i2c_module_t { I2C_NONE, I2C_BMP280, I2C_MS5611, I2C_BMP180 } i2c_module_t; typedef enum analog_current_type_t { CURRENT_TYPE_HALL, CURRENT_TYPE_SHUNT } analog_current_type_t; typedef enum serial_monitor_format_t { FORMAT_HEX, FORMAT_STRING } serial_monitor_format_t; typedef enum gps_protocol_t { UBLOX, NMEA } gps_protocol_t; #endif typedef struct config_t { // smartport data_id uint16_t version; // 0x5101 enum rx_protocol_t rx_protocol; // 0x5102 enum esc_protocol_t esc_protocol; // 0x5103 bool enable_gps; // 0x5104 uint32_t gps_baudrate; // 0x5105 bool enable_analog_voltage; // 0x5106 bool enable_analog_current; // 0x5107 bool enable_analog_ntc; // 0x5108 bool enable_analog_airspeed; // 0x5109 enum i2c_module_t i2c_module; // 0x510A uint8_t ina3221_filter; // 0x510B float alpha_rpm; // 0x510C float alpha_voltage; // 0x510D float alpha_current; // 0x510E float alpha_temperature; // 0x510F float alpha_vario; // 0x5110 float alpha_airspeed; // 0x5111 uint16_t refresh_rate_rpm; // 0x5112 uint16_t refresh_rate_voltage; // 0x5113 uint16_t refresh_rate_current; // 0x5114 uint16_t refresh_rate_temperature; // 0x5115 uint16_t refresh_rate_gps; // 0x5116 uint16_t refresh_rate_consumption; // 0x5117 uint16_t refresh_rate_vario; // 0x5118 uint16_t refresh_rate_airspeed; // 0x5119 uint16_t refresh_rate_default; // 0x511A float analog_voltage_multiplier; // 0x511B enum analog_current_type_t analog_current_type; // 0x511C uint16_t gpio_interval; // 0x511D float analog_current_quiescent_voltage; // 0x511E float analog_current_multiplier; // 0x511F float analog_current_offset; // 0x5120 bool analog_current_autoffset; // 0x5121 uint8_t pairOfPoles; // 0x5122 uint8_t mainTeeth; // 0x5123 uint8_t pinionTeeth; // 0x5124 float rpm_multiplier; // 0x5125 uint8_t bmp280_filter; // 0x5126 bool enable_pwm_out; // 0x5127 uint8_t smartport_sensor_id; // 0x5128 uint16_t smartport_data_id; // 0x5129 bool vario_auto_offset; // 0x512A bool xbus_clock_stretch; // 0x512B bool jeti_gps_speed_units_kmh; // 0x512C bool enable_esc_hw4_init_delay; // 0x512D uint16_t esc_hw4_init_delay_duration; // 0x512E uint8_t esc_hw4_current_thresold; // 0x512F uint16_t esc_hw4_current_max; // 0x5130 float esc_hw4_voltage_multiplier; // 0x5131 float esc_hw4_current_multiplier; // 0x5132 bool ibus_alternative_coordinates; // 0x5133 uint8_t debug; // 0x5134 bool esc_hw4_is_manual_offset; // 0x5135 uint8_t analog_rate; // 0x5136 bool xbus_use_alternative_volt_temp; // 0x5137 uint8_t gpio_mask; // 0x5138 float esc_hw4_offset; // 0x5139 uint32_t serial_monitor_baudrate; // 0x513A uint8_t serial_monitor_stop_bits; // 0x513B uint8_t serial_monitor_parity; // 0x513C uint16_t serial_monitor_timeout_ms; // 0x513D bool serial_monitor_inverted; // 0x513E bool esc_hw4_auto_detect; // 0x514B int16_t airspeed_vcc; // 0x5140 float fuel_flow_ml_per_pulse; // 0x5141 bool enable_fuel_flow; // 0x5142 uint16_t xgzp68xxd_k; // 0x5143 uint8_t enable_fuel_pressure; // 0x5144 bool smart_esc_calc_consumption; // 0x5145 uint8_t serial_monitor_gpio; // 0x5146 uint8_t gps_rate; // 0x5147 serial_monitor_format_t serial_monitor_format; // 0x5148 uint8_t gps_protocol; // 0x5149 bool sbus_battery_slot; // 0x514A bool fport_inverted; bool fbus_inverted; int16_t airspeed_offset; // 0x513F uint8_t mpu6050_acc_scale; // 0x514C uint8_t mpu6050_gyro_scale; // 0x514D uint8_t mpu6050_gyro_weighting; // 0x514E bool enable_gyro; // 0x514F uint8_t enable_lipo; // 0x5150 uint8_t mpu6050_filter; // 0x5151 uint8_t lipo_cells; // 0x5152 uint8_t sensor_id_srxl2; int8_t ntc_offset; // 0x5153 uint8_t spare7; uint32_t spare8; uint32_t spare9; uint32_t spare10; uint32_t spare11; uint32_t spare12; uint32_t spare13; uint32_t spare14; uint32_t spare15; uint32_t spare16; uint32_t spare17; uint32_t spare18; uint32_t spare19; uint32_t spare20; } config_t; #endif ================================================ FILE: lua/MSRC.lua ================================================ local toolName = "TNS|MSRC config|TNE" local scriptVersion = "v1.4" local statusEnum = { start = 1, getConfig = 2, config = 3, exitScr = 4, saveConfig = 5, startSave = 6, maintOff = 7, exit = 8, } local pageEnum = { sensorId = 1, rate = 2, avg = 3, esc = 4, gps = 5, vario = 6, fuelmeter = 7, gpio = 8, analogRate = 9, analogTemp = 10, analogVolt = 11, analogCurr = 12, analogAirspeed = 13, gyro = 14, } local varEnum = { str = 1, val = 2, min = 3, max = 4, incr = 5, dataId = 6, list = 7, } local firmwareVersion local sensorIdTx = 18 local page = 0 local pageLong = false local pageItem = 1 local isSelected = false local status = statusEnum.start local saveChanges = true local ts = 0 local vars = {} local onOffStr = { "Off", "On" } local pageName = { "Sensor Id", "Refresh rate", "Average elements", "ESC", "GPS", "Vario", "Fuel meter", "GPIO", "Analog rate", "Temperature analog", "Voltage analog", "Current analog", "Airspeed analog", "Gyro", } -- Page 1 - SensorId local sensorId = { "Sensor Id", nil, 1, 28, 1, 0x5128 } -- str, val, min, max, incr, dataId vars[pageEnum.sensorId] = { sensorId } -- Page 2 - Refresh interval (ms 1-2000) local rateRpm = { "RPM", nil, 1, 2000, 1, 0x513A } local rateVolt = { "Voltage", nil, 1, 2000, 1, 0x5113 } local rateCurr = { "Current", nil, 1, 2000, 1, 0x5114 } local rateTemp = { "Temperature", nil, 1, 2000, 1, 0x5115 } local rateGps = { "GPS", nil, 1, 2000, 1, 0x5116 } local rateCons = { "Consumption", nil, 1, 2000, 1, 0x5117 } local rateVario = { "Vario", nil, 1, 2000, 1, 0x5118 } local rateAirspeed = { "Airspeed", nil, 1, 2000, 1, 0x5119 } vars[pageEnum.rate] = { rateRpm, rateVolt, rateCurr, rateTemp, rateGps, rateCons, rateVario, rateAirspeed } -- Page 3 - Averaging elements (1-16) local avgRpm = { "RPM", nil, 1, 16, 1, 0x510C } local avgVolt = { "Voltage", nil, 1, 16, 1, 0x510D } local avgCurr = { "Current", nil, 1, 16, 1, 0x510E } local avgTemp = { "Temperature", nil, 1, 16, 1, 0x510F } local avgVario = { "Vario", nil, 1, 16, 1, 0x5110 } local avgAirspeed = { "Airspeed", nil, 1, 16, 1, 0x5111 } vars[pageEnum.avg] = { avgRpm, avgVolt, avgCurr, avgTemp, avgVario, avgAirspeed } -- Page 4 - ESC local pairPoles = { "Pair of Poles", nil, 1, 20, 1, 0x5122 } local mainGear = { "Main Gear", nil, 1, 1000, 1, 0x5123 } local pinionGear = { "Pinion Gear", nil, 1, 1000, 1, 0x5124 } local escProtocolStr = { "None", "Hobbywing V3", "Hobbywing V4", "PWM", "Castle Link", "Kontronik", "Kiss", "APD HV", "HobbyWing V5", "Smart ESC/BAT", "OMP M4", "ZTW", "OpenYGE", } local escProtocol = { "Protocol", nil, 0, 11, 1, 0x5103, escProtocolStr } local hw4InitDelay = { "Init Delay", nil, 0, 1, 1, 0x512E, onOffStr } local hw4AutoDetect = { "Auto detect", nil, 0, 1, 1, 0x514B, onOffStr } local hw4VoltMult = { "Volt mult", nil, 0, 100000, 1, 0x5131 } local hw4CurrMult = { "Curr mult", nil, 0, 100000, 1, 0x5132 } local hw4AutoOffset = { "Auto offset", nil, 0, 1, 1, 0x5135, onOffStr } local hw4Offset = { "Curr offset", nil, 0, 2000, 1, 0x5139 } local smartEscConsumption = { "Calc cons", nil, 0, 1, 1, 0x5145, onOffStr } vars[pageEnum.esc] = { pairPoles, mainGear, pinionGear, escProtocol, hw4InitDelay, hw4AutoDetect, hw4VoltMult, hw4CurrMult, hw4AutoOffset, hw4Offset, smartEscConsumption, } -- Page 5 - GPS local gpsEnable = { "Enable", nil, 0, 1, 1, 0x5104, onOffStr } local gpsProtocolStr = { "UBLOX", "NMEA" } local gpsProtocol = { "Protocol", nil, 0, 1, 1, 0x5149, gpsProtocolStr } local gpsBaudrateVal = { 9600, 38400, 57600, 115200 } local gpsBaudrate = { "Baudrate", nil, 0, 3, 1, 0x5105, gpsBaudrateVal } local gpsRateVal = { 1, 5, 10, 20 } local gpsRate = { "Rate", nil, 0, 3, 1, 0x5147, gpsRateVal } vars[pageEnum.gps] = { gpsEnable, gpsProtocol, gpsBaudrate, gpsRate } -- Page 6 - Vario local varioModelStr = { "None", "BMP280", "MS5611", "BMP180" } local varioModel = { "Model", nil, 0, 3, 1, 0x510A, varioModelStr } local varioFilterStr = { "Low", "Medium", "High" } local varioFilter = { "Filter", nil, 1, 3, 1, 0x5126, varioFilterStr } vars[pageEnum.vario] = { varioModel, varioFilter } -- Page 7 - Fuel meter local fuelMeter = { "Enable", nil, 0, 1, 1, 0x5142, onOffStr } local mlPulse = { "ml/pulse", nil, 0, 1, 0.0001, 0x5141 } vars[pageEnum.fuelmeter] = { fuelMeter, mlPulse } -- Page 8 - GPIO local gpioInterval = { "Interval(ms)", nil, 10, 10000, 1, 0x511D } local gpio = { "", nil, 0, 0x3F, 1, 0x5138 } local gpio17 = { "17", 0, 0, 1, 1, 0, onOffStr } local gpio18 = { "18", 0, 0, 1, 1, 0, onOffStr } local gpio19 = { "19", 0, 0, 1, 1, 0, onOffStr } local gpio20 = { "20", 0, 0, 1, 1, 0, onOffStr } local gpio21 = { "21", 0, 0, 1, 1, 0, onOffStr } local gpio22 = { "22", 0, 0, 1, 1, 0, onOffStr } vars[pageEnum.gpio] = { gpioInterval, gpio } -- Page 9 - Analog rate local analogRate = { "Rate(Hz)", nil, 1, 100, 1, 0x5136 } vars[pageEnum.analogRate] = { analogRate } -- Page 10 - Temperature analog local analogTemp = { "Enable", nil, 0, 1, 1, 0x5108, onOffStr } vars[pageEnum.analogTemp] = { analogTemp } -- Page 11 - Voltage analog local analogVolt = { "Enable", nil, 0, 1, 1, 0x5106, onOffStr } local analogVoltMult = { "Multiplier", nil, 1, 1000, 0.01, 0x511B } vars[pageEnum.analogVolt] = { analogVolt, analogVoltMult } -- Page 12 - Current analog local analogCurr = { "Enable", nil, 0, 1, 1, 0x5107, onOffStr } local analogCurrTypeStr = { "Hall Effect", "Shunt Resistor" } local analogCurrType = { "Type", nil, 0, 1, 1, 0x511C, analogCurrTypeStr } local analogCurrMult = { "Mult", nil, 0, 100, 0.01, 0x511F } local analogCurrSens = { "Sens(mV/A)", 0, 0, 100, 0.01, 0 } local analogCurrAutoOffset = { "Auto Offset", nil, 0, 1, 1, 0x5121, onOffStr } local analogCurrOffset = { "Offset", nil, 0, 3.3, 0.01, 0x5120 } vars[pageEnum.analogCurr] = { analogCurr, analogCurrType, analogCurrMult, analogCurrAutoOffset, analogCurrOffset } -- Page 13 - Airspeed analog local analogAirspeed = { "Enable", nil, 0, 1, 1, 0x5109, onOffStr } local analogAirspeedVcc = { "Vcc(V)", nil, 3, 6, 0.01, 0x5140 } local analogAirspeedOffset = { "Offset(mV)", nil, -1000, 1000, 1, 0x513F } vars[pageEnum.analogAirspeed] = { analogAirspeed, analogAirspeedVcc, analogAirspeedOffset } -- Page 14 - Gyro local gyro = { "Enable", nil, 0, 1, 1, 0x514F, onOffStr } local gyroAccSens = { "Acc sens", nil, 0, 3, 1, 0x514C, { 2, 4, 8, 16 } } local gyroGyroSens = { "Gyro sens", nil, 0, 3, 1, 0x514D, { 250, 500, 1000, 2000 } } local gyroGyroWeight = { "Gyro weight", nil, 0, 100, 1, 0x514E } local gyroFilter = { "Filter", nil, 0, 6, 1, 0x5151 } vars[pageEnum.gyro] = { gyro, gyroAccSens, gyroGyroSens, gyroGyroWeight, gyroFilter } local function getTextFlags(item) local value = 0 if item == pageItem then value = INVERS if isSelected == true then value = value + BLINK end end return value end local function changeValue(isIncremented) local mult = 1 if getRotEncSpeed() == ROTENC_MIDSPEED then mult = 10 elseif getRotEncSpeed() == ROTENC_HIGHSPEED then mult = 100 end if isIncremented == true then vars[page][pageItem][varEnum.val] = vars[page][pageItem][varEnum.val] + vars[page][pageItem][varEnum.incr] * mult if vars[page][pageItem][varEnum.val] > vars[page][pageItem][varEnum.max] then vars[page][pageItem][varEnum.val] = vars[page][pageItem][varEnum.max] end else vars[page][pageItem][varEnum.val] = vars[page][pageItem][varEnum.val] - vars[page][pageItem][varEnum.incr] * mult if vars[page][pageItem][varEnum.val] < vars[page][pageItem][varEnum.min] then vars[page][pageItem][varEnum.val] = vars[page][pageItem][varEnum.min] end end end local function getValue(list, index) -- index starts at 1 if index > #list then return list[1] end return list[index] end local function getIndex(list, value) for i = 1, #list do if value == list[i] then return i end end return 1 end local function handleEvents(event) -- Check one-time script if event == nil then return 2 end -- Handle events if page == 0 then if event == EVT_PAGE_BREAK or event == 513 then page = 1 status = statusEnum.getConfig end elseif event == EVT_EXIT_BREAK then if isSelected == true then isSelected = false else status = statusEnum.exitScr end elseif (event == EVT_PAGE_BREAK or event == 513) and (status ~= statusEnum.exitScr) then if pageLong then page = page - 1 else page = page + 1 end if page > #vars then page = 1 elseif page < 1 then page = #vars end pageLong = false pageItem = 1 isSelected = false status = statusEnum.getConfig ts = 0 elseif event == EVT_PAGE_LONG or event == 2049 then pageLong = true elseif event == EVT_ROT_RIGHT then if status == statusEnum.exitScr then saveChanges = not saveChanges elseif isSelected == false then pageItem = pageItem + 1 if pageItem > #vars[page] then pageItem = 1 end else changeValue(true) end elseif event == EVT_ROT_LEFT then if status == statusEnum.exitScr then saveChanges = not saveChanges end if isSelected == false then pageItem = pageItem - 1 if pageItem < 1 then pageItem = #vars[page] end else changeValue(false) end elseif event == EVT_ROT_BREAK then if status == statusEnum.exitScr then pageItem = 1 if saveChanges == true then status = statusEnum.saveConfig page = 1 vars[pageEnum.gpio] = { gpioInterval, gpio } else saveChanges = true status = statusEnum.getConfig end else isSelected = not isSelected end end end local function getConfig() local sensor, frameId, dataId, value = sportTelemetryPop() if firmwareVersion == nil then if dataId ~= nil and dataId == 0x5101 then firmwareVersion = "v" .. bit32.rshift(value, 16) .. "." .. bit32.band(bit32.rshift(value, 8), 0xF) .. "." .. bit32.band(value, 0xF) status = statusEnum.config elseif (getTime() - ts > 200) and sportTelemetryPush(sensorIdTx - 1, 0x30, 0x5101, 1) then ts = getTime() end elseif dataId ~= nil and dataId == vars[page][pageItem][varEnum.dataId] then if dataId == 0x511E or dataId == 0x5140 or dataId == 0x511B or dataId == 0x5120 then value = value / 100.0 elseif dataId == 0x5141 then value = value / 10000.0 elseif dataId == 0x511F then value = value / 100.0 if analogCurrTypeStr[analogCurrType[varEnum.val] + 1] == "Hall Effect" then value = 1000.0 / value analogCurrSens[varEnum.val] = math.floor(value * 100) / 100.0 end elseif dataId == 0x5105 then value = getIndex(gpsBaudrateVal, value) - 1 elseif dataId == 0x5147 then value = getIndex(gpsRateVal, value) - 1 elseif dataId == 0x5138 then gpio17[varEnum.val] = bit32.extract(value, 0) gpio18[varEnum.val] = bit32.extract(value, 1) gpio19[varEnum.val] = bit32.extract(value, 2) gpio20[varEnum.val] = bit32.extract(value, 3) gpio21[varEnum.val] = bit32.extract(value, 4) gpio22[varEnum.val] = bit32.extract(value, 5) elseif dataId == 0x5126 then value = value - 1 elseif dataId == 0x5135 then if value == 0 then value = 1 else value = 0 end elseif dataId == 0x513F then value = value - 1000 end if value < vars[page][pageItem][varEnum.min] then value = vars[page][pageItem][varEnum.min] end if value > vars[page][pageItem][varEnum.max] then value = vars[page][pageItem][varEnum.max] end vars[page][pageItem][varEnum.val] = value pageItem = pageItem + 1 ts = 0 if pageItem > #vars[page] then pageItem = 1 status = statusEnum.config end elseif vars[page][pageItem][varEnum.val] ~= nil then pageItem = pageItem + 1 ts = 0 if pageItem > #vars[page] then pageItem = 1 status = statusEnum.config end elseif (getTime() - ts > 200) and sportTelemetryPush(sensorIdTx - 1, 0x30, vars[page][pageItem][varEnum.dataId], 1) then ts = getTime() end if page == pageEnum.gpio and status == statusEnum.config then vars[pageEnum.gpio] = { gpioInterval, gpio17, gpio18, gpio19, gpio20, gpio21, gpio22 } end end local function saveConfig() if status == statusEnum.saveConfig then if sportTelemetryPush(sensorIdTx - 1, 0x31, 0x5201, 0) then status = statusEnum.startSave end return end if page > #vars then if sportTelemetryPush(sensorIdTx - 1, 0x31, 0x5201, 1) then status = statusEnum.maintOff end return end local value = vars[page][pageItem][varEnum.val] local dataId = vars[page][pageItem][varEnum.dataId] if value ~= nil and dataId ~= 0 then if dataId == 0x511E or dataId == 0x511F or dataId == 0x5140 or dataId == 0x511B then value = value * 100 elseif dataId == 0x5141 then value = value * 10000 elseif dataId == 0x5105 then value = getValue(gpsBaudrateVal, gpsBaudrate[varEnum.val] + 1) elseif dataId == 0x5147 then value = getValue(gpsRateVal, gpsRate[varEnum.val] + 1) elseif dataId == 0x5138 then value = gpio17[varEnum.val] -- bit 1 value = bit32.bor(value, bit32.lshift(gpio18[varEnum.val], 1)) -- bit 2 value = bit32.bor(value, bit32.lshift(gpio19[varEnum.val], 2)) -- bit 3 value = bit32.bor(value, bit32.lshift(gpio20[varEnum.val], 3)) -- bit 4 value = bit32.bor(value, bit32.lshift(gpio21[varEnum.val], 4)) -- bit 5 value = bit32.bor(value, bit32.lshift(gpio22[varEnum.val], 5)) -- bit 6 elseif dataId == 0x5126 then value = vars[page][pageItem][varEnum.val] + 1 elseif dataId == 0x5135 then if vars[page][pageItem][varEnum.val] == 1 then value = 0 else value = 1 end elseif dataId == 0x513F then value = vars[page][pageItem][varEnum.val] + 1000 end value = math.floor(value) if sportTelemetryPush(sensorIdTx - 1, 0x31, dataId, value) then pageItem = pageItem + 1 end else pageItem = pageItem + 1 end if pageItem > #vars[page] then page = page + 1 pageItem = 1 end end local function setPageItems() if page == pageEnum.esc then if escProtocol[varEnum.val] + 1 > #escProtocolStr then escProtocol[varEnum.val] = 0 end if hw4AutoDetect[varEnum.val] + 1 > #onOffStr then hw4AutoDetect[varEnum.val] = 0 end if hw4AutoOffset[varEnum.val] + 1 > #onOffStr then hw4AutoOffset[varEnum.val] = 0 end vars[page] = { pairPoles, mainGear, pinionGear, escProtocol } if escProtocolStr[escProtocol[varEnum.val] + 1] == "Hobbywing V4" then vars[page] = { pairPoles, mainGear, pinionGear, escProtocol, hw4InitDelay, hw4AutoDetect, } if getValue(onOffStr, hw4AutoDetect[varEnum.val] + 1) == "Off" then vars[page] = { pairPoles, mainGear, pinionGear, escProtocol, hw4InitDelay, hw4AutoDetect, hw4VoltMult, hw4CurrMult, hw4AutoOffset, } if getValue(onOffStr, hw4AutoOffset[varEnum.val] + 1) == "Off" then vars[page] = { pairPoles, mainGear, pinionGear, escProtocol, hw4InitDelay, hw4AutoDetect, hw4VoltMult, hw4CurrMult, hw4AutoOffset, hw4Offset, } end end elseif escProtocolStr[escProtocol[varEnum.val] + 1] == "Smart ESC/BAT" then vars[page] = { pairPoles, mainGear, pinionGear, escProtocol, smartEscConsumption } end elseif page == pageEnum.vario then if varioModel[varEnum.val] + 1 > #varioModelStr then varioModel[varEnum.val] = 0 end if varioModelStr[varioModel[varEnum.val] + 1] == "BMP280" then vars[page] = { varioModel, varioFilter } else vars[page] = { varioModel } end elseif page == pageEnum.analogCurr then if analogCurrType[varEnum.val] + 1 > #analogCurrTypeStr then analogCurrType[varEnum.val] = 0 end if analogCurrAutoOffset[varEnum.val] + 1 > #onOffStr then analogCurrAutoOffset[varEnum.val] = 0 end if analogCurrTypeStr[analogCurrType[varEnum.val] + 1] == "Hall Effect" then vars[page] = { analogCurr, analogCurrType, analogCurrSens, analogCurrAutoOffset } if getValue(onOffStr, analogCurrAutoOffset[varEnum.val] + 1) == "Off" then vars[page] = { analogCurr, analogCurrType, analogCurrSens, analogCurrAutoOffset, analogCurrOffset } end else vars[page] = { analogCurr, analogCurrType, analogCurrMult } end end end local function drawTitle(str, page, pages) if LCD_H == 64 then lcd.drawScreenTitle(str, page, pages) else lcd.drawText(1, 1, str) if page ~= 0 and pages ~= 0 then lcd.drawText(200, 1, page .. "/" .. pages) end end end local function drawPage() lcd.clear() if page == 0 then drawTitle("MSRC " .. scriptVersion, 0, 0) if firmwareVersion ~= nil then lcd.drawText(1, 20, "Firmware " .. firmwareVersion, SMLSIZE) lcd.drawText(1, 35, "Press Page", SMLSIZE) end elseif status == statusEnum.getConfig then drawTitle(pageName[page], page, #vars) lcd.drawText(60, 30, pageItem .. "/" .. #vars[page], 0) elseif status == statusEnum.config then drawTitle(pageName[page], page, #vars) local scroll, fileHeight, fileStart if LCD_H == 64 then scroll = pageItem - 8 if scroll < 0 then scroll = 0 end fileStart = 9 fileHeight = 7 else scroll = 0 fileStart = 20 fileHeight = 15 end for i = 1, #vars[page] - scroll do lcd.drawText(1, fileStart + fileHeight * (i - 1), vars[page][i + scroll][1], SMLSIZE) if #vars[page][i + scroll] == 6 then local val = vars[page][i + scroll][varEnum.val] if val == nil then val = -1 end lcd.drawText(LCD_W / 2, fileStart + fileHeight * (i - 1), val, SMLSIZE + getTextFlags(i + scroll)) elseif #vars[page][i + scroll] == 7 then local val = 1 if vars[page][i + scroll][varEnum.val] ~= nil then val = vars[page][i + scroll][varEnum.val] + 1 end lcd.drawText( LCD_W / 2, fileStart + fileHeight * (i - 1), getValue(vars[page][i + scroll][varEnum.list], val), SMLSIZE + getTextFlags(i + scroll) ) end end elseif status == statusEnum.exitScr then drawTitle("Exit", 0, 0) lcd.drawText(1, 20, "Save changes?", SMLSIZE) local flag_yes = 0 local flag_cancel = 0 if saveChanges == true then flag_yes = INVERS else flag_cancel = INVERS end lcd.drawText(1, 30, "Yes", SMLSIZE + flag_yes) lcd.drawText(60, 30, "Cancel", SMLSIZE + flag_cancel) elseif status == statusEnum.startSave then drawTitle("Saving ", 0, 0) lcd.drawText(1, 20, "Update " .. page .. "/" .. #vars, SMLSIZE) elseif status == statusEnum.exit then drawTitle("MSRC " .. scriptVersion, 0, 0) lcd.drawText(1, 20, "Completed!", SMLSIZE) lcd.drawText(1, 40, "Reboot MSRC to apply changes.", SMLSIZE) end end local function run_func(event) if status == statusEnum.start then if sportTelemetryPush(sensorIdTx - 1, 0x21, 0xFFFF, 0x80) then status = statusEnum.getConfig end elseif status == statusEnum.getConfig then handleEvents(event) getConfig() elseif status == statusEnum.config or status == statusEnum.exitScr then handleEvents(event) if status == statusEnum.config then setPageItems() end elseif status == statusEnum.saveConfig or status == statusEnum.startSave then saveConfig() elseif status == statusEnum.maintOff then if sportTelemetryPush(sensorIdTx - 1, 0x20, 0xFFFF, 0x80) then status = statusEnum.exit end end drawPage() return 0 end return { run = run_func } ================================================ FILE: msrc_gui/appdir/msrc.desktop ================================================ [Desktop Entry] Type=Application Name=MSRC Exec=msrc_gui %F Icon=msrc Comment=Sensors for RC Terminal=false Categories = Utility; ================================================ FILE: msrc_gui/circuitdialog.cpp ================================================ #include "circuitdialog.h" #include "ui_circuitdialog.h" CircuitDialog::CircuitDialog(QWidget *parent) : QDialog(parent), ui(new Ui::CircuitDialog) { ui->setupUi(this); this->setWindowFlags(Qt::WindowMaximizeButtonHint | Qt::WindowCloseButtonHint); ui->saCircuit->takeWidget(); ui->saCircuit->setWidget(ui->lbCircuit); } CircuitDialog::~CircuitDialog() { delete ui; } void CircuitDialog::on_btClose_clicked() { this->hide(); } void CircuitDialog::resizeEvent(QResizeEvent* event) { Q_UNUSED(event); if (ui->lbCircuit->height() < ui->saCircuit->height()) ui->lbCircuit->setFixedHeight(ui->saCircuit->height()); if (ui->lbCircuit->width() < ui->saCircuit->width()) ui->lbCircuit->setFixedWidth(ui->saCircuit->width()); mainWindow->generateCircuit(ui->lbCircuit); } void CircuitDialog::on_btZoomIn_released() { ui->lbCircuit->setFixedHeight(ui->lbCircuit->height() * 1.2); ui->lbCircuit->setFixedWidth(ui->lbCircuit->width() * 1.2); mainWindow->generateCircuit(ui->lbCircuit); } void CircuitDialog::on_btZoomOut_released() { ui->lbCircuit->setFixedHeight(ui->lbCircuit->height() / 1.2); ui->lbCircuit->setFixedWidth(ui->lbCircuit->width() / 1.2); if (ui->lbCircuit->height() < ui->saCircuit->height()) ui->lbCircuit->setFixedHeight(ui->saCircuit->height()); if (ui->lbCircuit->width() < ui->saCircuit->width()) ui->lbCircuit->setFixedWidth(ui->saCircuit->width()); mainWindow->generateCircuit(ui->lbCircuit); } ================================================ FILE: msrc_gui/circuitdialog.h ================================================ #ifndef CIRCUITDIALOG_H #define CIRCUITDIALOG_H #include #include #include #include "mainwindow.h" namespace Ui { class CircuitDialog; } class CircuitDialog : public QDialog { Q_OBJECT public: MainWindow *mainWindow; explicit CircuitDialog(QWidget *parent = nullptr); ~CircuitDialog(); void resizeEvent(QResizeEvent* event); private slots: void on_btClose_clicked(); void on_btZoomIn_released(); void on_btZoomOut_released(); private: Ui::CircuitDialog *ui; }; #endif // CIRCUITDIALOG_H ================================================ FILE: msrc_gui/circuitdialog.ui ================================================ CircuitDialog 0 0 763 559 600 400 Circuit true 0 0 743 506 0 0 121 71 TextLabel + - Close ================================================ FILE: msrc_gui/main.cpp ================================================ #include "mainwindow.h" #include int main(int argc, char *argv[]) { QApplication a(argc, argv); MainWindow w; w.show(); return a.exec(); } ================================================ FILE: msrc_gui/mainwindow.cpp ================================================ #include "mainwindow.h" #include "circuitdialog.h" #include "qobject.h" #include "ui_mainwindow.h" MainWindow::MainWindow(QWidget *parent) : QMainWindow(parent), ui(new Ui::MainWindow), serial(new QSerialPort()) { ui->setupUi(this); this->setWindowTitle(QString::asprintf("MSRC Link %s", PROJECT_VERSION)); ui->tbViews->setCurrentIndex(0); ui->ptDebug->ensureCursorVisible(); ui->tbConfig->setEnabled(true); ui->tbConfig->setCurrentIndex(0); ui->btDebug->setDisabled(true); ui->btUpdate->setDisabled(true); enableWidgets(ui->scrollAreaWidgetContentsReceiver, false); enableWidgets(ui->scrollAreaWidgetContentsSensors, false); ui->cbEsc->addItems({"Hobbywing V3", "Hobbywing V4/Flyfun (not VBAR firmware)", "PWM", "Castle Link", "Kontronic", "Kiss", "APD HV", "HobbyWing V5", "Smart ESC/BAT", "OMP M4", "ZTW", "OpenYGE"}); ui->cbGpsBaudrate->addItems({"115200", "57600", "38400", "9600"}); ui->cbGpsBaudrate->setCurrentIndex(5); ui->cbReceiver->addItem("Frsky Smartport", RX_SMARTPORT); ui->cbReceiver->addItem("Frsky D", RX_FRSKY_D); ui->cbReceiver->addItem("Frsky FPort", RX_FPORT); ui->cbReceiver->addItem("Frsky FBUS", RX_FBUS); ui->cbReceiver->addItem("Spektrum XBUS", RX_XBUS); ui->cbReceiver->addItem("Spektrum SRXL", RX_SRXL); ui->cbReceiver->addItem("Spektrum SRXL2", RX_SRXL2); ui->cbReceiver->addItem("Flysky IBUS", RX_IBUS); ui->cbReceiver->addItem("Futaba SBUS2", RX_SBUS); ui->cbReceiver->addItem("Jeti Ex Bus", RX_JETIEX); ui->cbReceiver->addItem("Jeti Ex Sensor", RX_JETIEX_SENSOR); ui->cbReceiver->addItem("Multiplex Sensor Bus", RX_MULTIPLEX); ui->cbReceiver->addItem("ELRS/CRSF", RX_CRSF); ui->cbReceiver->addItem("Sanwa", RX_SANWA); ui->cbReceiver->addItem("HOTT", RX_HOTT); ui->cbReceiver->addItem("Hitec", RX_HITEC); ui->cbReceiver->addItem("JR Propo", RX_JR_PROPO); ui->cbReceiver->addItem("GHST", RX_GHST); ui->cbReceiver->addItem("Serial Monitor", SERIAL_MONITOR); ui->sbEscOffset->setVisible(false); ui->cbCurrentSensorType->addItems({"Hall effect", "Shunt resistor"}); ui->cbCurrentAutoOffset->setChecked(true); ui->cbBarometerType->addItems({"BMP280", "MS5611", "BMP180"}); ui->cbAltitudeFilter->addItems({"Low", "Medium", "High"}); ui->cbAltitudeFilter->setCurrentIndex(2); ui->cbGpsRate->addItems({"1", "5", "10", "20"}); ui->cbGpsProtocol->addItems({"UBLOX", "NMEA"}); ui->cbSpeedUnitsGps->addItems({"km/h", "kts"}); ui->lbQuiescentVoltage->setText("Zero current output voltage, VIOUT (V)"); ui->cbVarioAutoOffset->setVisible(false); ui->cbSerialMonitorGpio->addItems({"1", "5", "6"}); ui->cbBaudrate->addItems({"115200", "57600", "38400", "19200", "9600", "4800"}); ui->cbStopbits->addItems({"1", "2"}); ui->cbParity->addItems({"None", "Odd", "Even"}); ui->cbSerialFormat->addItems({"Hex", "String"}); ui->cbLipoType->addItem({"INA3221"}); ui->cbIna3221Filter->addItem("1", 0B000); ui->cbIna3221Filter->addItem("4", 0B001); ui->cbIna3221Filter->addItem("16", 0B010); ui->cbIna3221Filter->addItem("64", 0B011); ui->cbIna3221Filter->addItem("128", 0B100); ui->cbIna3221Filter->addItem("256", 0B101); ui->cbIna3221Filter->addItem("512", 0B110); ui->cbIna3221Filter->addItem("1024", 0B111); ui->cbMaxPressure->addItems({"< 1 kPa (K = 8192)", "< 2 kPa (K = 4096)", "< 4 kPa (K = 2048)", "< 8 kPa (K = 1024)", "< 16 kPa (K = 512)", "< 32 kPa (K = 256)", "< 65 kPa (K = 128)", "< 130 kPa (K = 64)", "< 260 kPa (K = 32)", "< 500 kPa (K = 16)", "< 1000 kPa (K = 8)", "> 1000 kPa (K = 4)"}); ui->cbGyroAccSens->addItems({"2", "4", "8", "16"}); ui->cbGyroSens->addItems({"250", "500", "1000", "2000"}); ui->cbGyroSamplerate->addItems({"1000", "500", "333", "250", "200", "167", "144", "125"}); ui->cbGyroSamplerate->setVisible(false); ui->lbGyroSamplerate->setVisible(false); ui->lbConnections->setText( "| Sensor/Receiver | Board GPIO|" "\n| :---: | :---: |" "\n| 3.3-5v | 5v |" "\n| GND | GND |" "\n| Smartport, SBUS, SRXL, IBUS, SB, Jeti Ex, Sanwa, Hott, SRXL2, FPort, FBUS, GHST | 0(1) & 1 |" "\n| Frsky D, ELRS/CRSF Rx | 0 |" "\n| Serial monitor | 1 |" "\n| Hitec, XBUS SDA | 2(2) |" "\n| Hitec, XBUS SCL | 3(2) |" "\n| ESC serial, Serial monitor, Smart ESC | 5 |" "\n| Phase sensor (PWM in), Smart ESC | 4 |" "\n| Castle. Receiver signal | 4 |" "\n| Castle. ESC signal | 5(2) |" "\n| GPS Tx | 6 |" "\n| GPS Rx (3)(4) | 14 |" "\n| XBUS. NPN clock stretch(3) | 7 |" "\n| Sensor SDA | 8(2) |" "\n| Sensor SCL | 9(2) |" "\n| PWM out | 10 |" "\n| Fuel meter (PWM in) | 11 |" "\n| Throttle PWM (Smart ESC) | 12 |" "\n| Reverse PWM (Smart ESC) | 13 |" "\n| Rsteore default config | 15 |" "\n| Voltage | 26 |" "\n| Current | 27 |" "\n| NTC | 28 |" "\n| Airspeed | 29 |" "\n| GPIOs | 17 to 22 |" "\n " "\n(1) with 100Ω resistor. This resistor is optional as RP2040 has internal protection resistors for GPIOs " "\n(2) Pullups. See [6.3 Spektrum " "XBUS](https://github.com/dgatf/msrc/wiki/06.-Receiver-protocol#63-spektrum-xbus) and [6.9 " "Hitec](https://github.com/dgatf/msrc/wiki/06.-Receiver-protocol#69-hitec) " "\n(3) Optional " "\n(4) Only UBLOX compatible "); connect(ui->btConnect, SIGNAL(released()), this, SLOT(buttonSerialPort())); connect(ui->btDebug, SIGNAL(released()), this, SLOT(buttonDebug())); connect(ui->btClearDebug, SIGNAL(released()), this, SLOT(buttonClearDebug())); connect(ui->actionExit, SIGNAL(triggered()), this, SLOT(exitApp())); connect(serial, &QSerialPort::readyRead, this, &MainWindow::readSerial); connect(ui->btUpdate, SIGNAL(released()), this, SLOT(writeSerialConfig())); connect(ui->actionUpdateConfig, SIGNAL(triggered()), this, SLOT(writeSerialConfig())); connect(ui->actionOpen, SIGNAL(triggered()), this, SLOT(openConfig())); connect(ui->actionSave, SIGNAL(triggered()), this, SLOT(saveConfig())); connect(ui->actionAbout, SIGNAL(triggered()), this, SLOT(showAbout())); connect(ui->actionDefaultConfig, SIGNAL(triggered()), this, SLOT(defaultConfig())); ui->lbCircuit->resize(600, 400); //(ui->lbCircuit->parentWidget()->width(), // ui->lbCircuit->parentWidget()->height()); generateCircuit(ui->lbCircuit); portsList = fillPortsInfo(); statusBar()->showMessage("Not connected"); QTimer *timer = new QTimer(this); connect(timer, &QTimer::timeout, this, &MainWindow::checkPorts); timer->start(1000); } MainWindow::~MainWindow() { delete ui; } void MainWindow::generateCircuit(QLabel *label) { QSize *size = new QSize(label->width() - 10, label->height() - 10); QPixmap *pix = new QPixmap(*size); QPainter *paint = new QPainter(pix); QImage image; paint->fillRect(0, 0, size->width(), size->height(), label->palette().color(QPalette::Base)); image.load(":/res/rp2040_zero.png"); paint->drawImage(QPoint(0, 0), image.scaled(*size, Qt::IgnoreAspectRatio)); if (ui->cbReceiver->currentText() != "Serial Monitor") { if (ui->gbCurrent->isChecked()) { image.load(":/res/current_rp2040_zero.png"); paint->drawImage(QPoint(0, 0), image.scaled(*size, Qt::IgnoreAspectRatio)); } if (ui->gbVoltage1->isChecked()) { image.load(":/res/voltage_rp2040_zero.png"); paint->drawImage(QPoint(0, 0), image.scaled(*size, Qt::IgnoreAspectRatio)); } if (ui->gbTemp1->isChecked()) { image.load(":/res/ntc_rp2040_zero.png"); paint->drawImage(QPoint(0, 0), image.scaled(*size, Qt::IgnoreAspectRatio)); } if (ui->gbAirspeed->isChecked()) { image.load(":/res/airspeed_rp2040_zero.png"); paint->drawImage(QPoint(0, 0), image.scaled(*size, Qt::IgnoreAspectRatio)); } if (ui->gbGps->isChecked()) { image.load(":/res/gps_rp2040_zero.png"); paint->drawImage(QPoint(0, 0), image.scaled(*size, Qt::IgnoreAspectRatio)); } if (ui->gbEsc->isChecked()) { // ui->lbEsc->setEnabled(true); // ui->cbEsc->setEnabled(true); // ui->lbEscModel->setEnabled(true); // ui->lbEscModel->setEnabled(true); // ui->gbRpmMultipliers->setEnabled(true); if (ui->cbEsc->currentText() == "Hobbywing V3" || ui->cbEsc->currentText() == "Hobbywing V4/Flyfun (not VBAR firmware)" || ui->cbEsc->currentText() == "Kontronic" || ui->cbEsc->currentText() == "Kiss" || ui->cbEsc->currentText() == "APD HV" || ui->cbEsc->currentText() == "HobbyWing V5" || ui->cbEsc->currentText() == "OMP M4" || ui->cbEsc->currentText() == "ZTW" || ui->cbEsc->currentText() == "OpenYGE") image.load(":/res/esc_rp2040_zero.png"); else if (ui->cbEsc->currentText() == "PWM") image.load(":/res/pwm_rp2040_zero.png"); else if (ui->cbEsc->currentText() == "Castle Link") image.load(":/res/castle_rp2040_zero.png"); else if (ui->cbEsc->currentText() == "Smart ESC/BAT") image.load(":/res/smart_esc.png"); paint->drawImage(QPoint(0, 0), image.scaled(*size, Qt::IgnoreAspectRatio)); } else { } if (ui->gbAltitude->isChecked()) { image.load(":/res/vario_rp2040_zero.png"); paint->drawImage(QPoint(0, 0), image.scaled(*size, Qt::IgnoreAspectRatio)); } if (ui->gbGyro->isChecked()) { image.load(":/res/vario_rp2040_zero.png"); paint->drawImage(QPoint(0, 0), image.scaled(*size, Qt::IgnoreAspectRatio)); } if (ui->gbFuelPressure->isChecked()) { image.load(":/res/fuel_pressure.png"); paint->drawImage(QPoint(0, 0), image.scaled(*size, Qt::IgnoreAspectRatio)); } if (ui->gbFuelmeter->isChecked()) { image.load(":/res/fuel_meter.png"); paint->drawImage(QPoint(0, 0), image.scaled(*size, Qt::IgnoreAspectRatio)); } if (ui->gbLipo->isChecked()) { image.load(":/res/vario_rp2040_zero.png"); paint->drawImage(QPoint(0, 0), image.scaled(*size, Qt::IgnoreAspectRatio)); } if (ui->cbReceiver->currentText() == "Frsky D" || ui->cbReceiver->currentText() == "ELRS/CRSF" || ui->cbReceiver->currentText() == "Jeti Ex Sensor") { image.load(":/res/receiver_frsky_d_rp2040_zero.png"); } else if (ui->cbReceiver->currentText() == "Spektrum XBUS") { image.load(":/res/receiver_xbus_rp2040_zero.png"); } else if (ui->cbReceiver->currentText() == "Hitec") { image.load(":/res/receiver_hitec_rp2040_zero.png"); } else { image.load(":/res/receiver_serial_rp2040_zero.png"); } if (ui->cbReceiver->currentText() == "Spektrum XBUS" && ui->cbClockStretch->isChecked() == true) { image.load(":/res/clock_stretch_xbus_rp2040_zero.png"); paint->drawImage(QPoint(0, 0), image.scaled(*size, Qt::IgnoreAspectRatio)); } } paint->drawImage(QPoint(0, 0), image.scaled(*size, Qt::IgnoreAspectRatio)); label->setPixmap(*pix); } void MainWindow::openConfig() { QString filename = QFileDialog::getOpenFileName(this, "Open Config", "", "Config Files (*.cfg)"); QFile file(filename); if (file.open(QIODevice::ReadOnly)) { file.read((char *)&config, sizeof(config_t)); file.close(); if (config.version > CONFIG_VERSION) { QMessageBox::warning(this, tr("Information"), tr("Firmware config version is ") + QString::number(config.version) + ". mscr_gui config version is " + QString::number(CONFIG_VERSION) + ". Download latest msrc_gui. Please save the config in case the conversion fails.", QMessageBox::Close); saveConfig(); closeSerialPort(); return; } if (config.version < CONFIG_VERSION) { QMessageBox::warning(this, tr("Information"), tr("Firmware config version is ") + QString::number(config.version) + ". mscr_gui config version is " + QString::number(CONFIG_VERSION) + ". Converting config version to " + QString::number(CONFIG_VERSION) + "\nPlease press Update button to update MSRC config with new config " "version.\nAlso is needed to update MSRC firmware, if not already done.", QMessageBox::Close); config.version = CONFIG_VERSION; } setUiFromConfig(); } } void MainWindow::saveConfig() { QFileDialog dialog(this, "Save Config", QString(), "Config Files (*.cfg)"); dialog.setDefaultSuffix(".cfg"); dialog.setAcceptMode(QFileDialog::AcceptSave); if (dialog.exec()) { QString filename = dialog.selectedFiles().front(); QFile file(filename); if (file.open(QIODevice::WriteOnly)) { getConfigFromUi(); file.write((char *)&config, sizeof(config_t)); } } } void MainWindow::showAbout() { QMessageBox::information(this, tr("About"), QString::asprintf("MSRC Link %s\n\rDaniel Gorbea © 2020/2025", PROJECT_VERSION), QMessageBox::Close); } void MainWindow::buttonSerialPort() { if (isConnected) closeSerialPort(); else openSerialPort(); } void MainWindow::buttonDebug() { if (ui->btDebug->text() == "Enable Log") { if (!isConnected) return; ui->btDebug->setText("Disable Log"); serial->readAll(); char header = 0x30; serial->write(&header, 1); char command = 0x33; serial->write(&command, 1); isDebug = true; } else if (ui->btDebug->text() == "Disable Log") { if (!isConnected) return; ui->btDebug->setText("Enable Log"); char header = 0x30; serial->write(&header, 1); char command = 0x34; serial->write(&command, 1); isDebug = false; } } void MainWindow::buttonClearDebug() { ui->ptDebug->clear(); } void MainWindow::openSerialPort() { QString portName = ui->cbPortList->currentData().toString(); serial->setPortName(portName); serial->setBaudRate(QSerialPort::BaudRate::Baud115200); serial->setDataBits(QSerialPort::DataBits::Data8); serial->setParity(QSerialPort::Parity::NoParity); serial->setStopBits(QSerialPort::StopBits::OneStop); serial->setFlowControl(QSerialPort::FlowControl::NoFlowControl); if (serial->open(QIODevice::ReadWrite)) { statusBar()->showMessage("Connected " + ui->cbPortList->currentText()); isConnected = true; requestSerialConfig(); serial->setDataTerminalReady(true); ui->btUpdate->setEnabled(true); ui->btConnect->setText("Disconnect"); ui->actionUpdateConfig->setEnabled(true); ui->actionDefaultConfig->setEnabled(true); enableWidgets(ui->scrollAreaWidgetContentsReceiver, true); enableWidgets(ui->scrollAreaWidgetContentsSensors, true); ui->cbPortList->setDisabled(true); ui->btDebug->setEnabled(true); ui->btDebug->setText("Enable Log"); } else { statusBar()->showMessage("Not connected: " + serial->errorString()); isConnected = false; ui->btUpdate->setEnabled(false); ui->btConnect->setText("Connect"); ui->actionUpdateConfig->setEnabled(false); ui->actionDefaultConfig->setEnabled(false); enableWidgets(ui->scrollAreaWidgetContentsReceiver, false); enableWidgets(ui->scrollAreaWidgetContentsSensors, false); ui->btDebug->setEnabled(false); ui->btDebug->setText("Enable Log"); } } void MainWindow::closeSerialPort() { serial->close(); statusBar()->showMessage("Not connected"); isConnected = false; ui->btUpdate->setEnabled(false); ui->btConnect->setText("Connect"); ui->actionUpdateConfig->setEnabled(false); ui->actionDefaultConfig->setEnabled(false); enableWidgets(ui->scrollAreaWidgetContentsReceiver, false); enableWidgets(ui->scrollAreaWidgetContentsSensors, false); ui->cbPortList->setDisabled(false); ui->btDebug->setDisabled(true); ui->btDebug->setText("Enable Log"); } void MainWindow::readSerial() { /* header - 0x30 command - 0x30 - msrc to read config from usb 0x31 - msrc to request to send config to usb 0x32 - msrc answer to send config 0x33 - debug on 0x34 - debug off 0x35 - save default config to rp2040 flash */ if (isDebug == false) { if (serial->bytesAvailable() == sizeof(config_t) + 2) { data = serial->readAll(); if (data.at(0) == 0x30 && data.at(1) == 0x32) { memcpy(&config, data.data() + 2, sizeof(config_t)); if (config.version > CONFIG_VERSION) { QMessageBox::warning( this, tr("Information"), tr("Firmware config version is ") + QString::number(config.version) + ". mscr_gui config version is " + QString::number(CONFIG_VERSION) + ". Download latest msrc_gui. Please save the config in case the conversion fails.", QMessageBox::Close); saveConfig(); closeSerialPort(); return; } if (config.version < CONFIG_VERSION) { QMessageBox::warning( this, tr("Information"), tr("Firmware config version is ") + QString::number(config.version) + ". mscr_gui config version is " + QString::number(CONFIG_VERSION) + ". Converting config version to " + QString::number(CONFIG_VERSION) + "\nIt is needed to update MSRC firmware first or you will lose your config to the default " "values. Press Update button to update MSRC config with new config version only if MSRC " "firmware is updated to latest version first.", QMessageBox::Close); config.version = CONFIG_VERSION; } setUiFromConfig(); } } } else { data = serial->readAll(); ui->ptDebug->insertPlainText(data); if (autoscroll) ui->ptDebug->ensureCursorVisible(); } } void MainWindow::writeSerialConfig() { if (!isConnected) return; getConfigFromUi(); char header = 0x30; serial->write(&header, 1); char command = 0x30; serial->write(&command, 1); serial->write((char *)&config, sizeof(config_t)); /*QMessageBox msgBox; msgBox.setText("Reset RP2040 to apply settings."); msgBox.exec();*/ QMessageBox::warning(this, tr("Information"), tr("Reset RP2040 to apply settings."), QMessageBox::Close); } void MainWindow::defaultConfig() { if (!isConnected) return; char header = 0x30; serial->write(&header, 1); char command = 0x35; serial->write(&command, 1); QMessageBox::warning(this, tr("Information"), tr("Reset RP2040 to apply settings."), QMessageBox::Close); } void MainWindow::setUiFromConfig() { /* Receiver protocol */ int index = ui->cbReceiver->findData(config.rx_protocol); ui->cbReceiver->setCurrentIndex(index); /* Serial Monitor */ if (config.serial_monitor_gpio == 1) ui->cbSerialMonitorGpio->setCurrentText("1"); else if (config.serial_monitor_gpio == 5) ui->cbSerialMonitorGpio->setCurrentText("5"); else ui->cbSerialMonitorGpio->setCurrentText("6"); int item = ui->cbBaudrate->findText(QString::number(config.serial_monitor_baudrate)); if (item == -1) ui->cbBaudrate->setCurrentText(QString::number(config.serial_monitor_baudrate)); else ui->cbBaudrate->setCurrentIndex(item); if (config.serial_monitor_parity > 2) config.serial_monitor_parity = 0; ui->cbParity->setCurrentIndex(config.serial_monitor_parity); if (config.serial_monitor_stop_bits > 2 || config.serial_monitor_stop_bits < 1) config.serial_monitor_stop_bits = 1; ui->cbStopbits->setCurrentIndex(config.serial_monitor_stop_bits - 1); if (config.serial_monitor_timeout_ms > 100) config.serial_monitor_timeout_ms = 100; ui->sbTimeout->setValue(config.serial_monitor_timeout_ms); ui->cbInverted->setChecked(config.serial_monitor_inverted); if (config.serial_monitor_format == FORMAT_HEX) ui->cbSerialFormat->setCurrentText("Hex"); else ui->cbSerialFormat->setCurrentText("String"); /* Sensors */ // ESC if (config.esc_protocol == esc_protocol_t::ESC_NONE) ui->gbEsc->setChecked(false); else { ui->gbEsc->setChecked(true); ui->cbEsc->setCurrentIndex(config.esc_protocol - 1); } // GPS ui->gbGps->setChecked(config.enable_gps); ui->cbGpsBaudrate->setCurrentText(QString::number(config.gps_baudrate)); ui->cbGpsRate->setCurrentText(QString::number(config.gps_rate)); ui->cbGpsProtocol->setCurrentIndex(config.gps_protocol); // Analog rate ui->sbAnalogRate->setValue(config.analog_rate); // Voltage ui->gbVoltage1->setChecked(config.enable_analog_voltage); if (config.sbus_battery_slot == true) { ui->ckSbusBattery->setChecked(true); ui->ckSbusExtVolt->setChecked(false); } else { ui->ckSbusBattery->setChecked(false); ui->ckSbusExtVolt->setChecked(true); } // Temperature ui->gbTemp1->setChecked(config.enable_analog_ntc); ui->sbTempOffset->setValue(config.ntc_offset); // Current ui->gbCurrent->setChecked(config.enable_analog_current); ui->cbCurrentSensorType->setCurrentIndex(config.analog_current_type); ui->cbCurrentAutoOffset->setChecked(config.analog_current_autoffset); ui->sbQuiescentVoltage->setValue(config.analog_current_quiescent_voltage); if (config.analog_current_type == analog_current_type_t::CURRENT_TYPE_HALL) ui->sbCurrentSens->setValue(1000 / config.analog_current_multiplier); else ui->sbAnalogCurrentMultiplier->setValue(config.analog_current_multiplier); // Airspeed ui->gbAirspeed->setChecked(config.enable_analog_airspeed); ui->sbAirspeedVcc->setValue(config.airspeed_vcc / 100.0); ui->sbAirspeedOffset->setValue(config.airspeed_offset); // Vario if (config.i2c_module == i2c_module_t::I2C_NONE) ui->gbAltitude->setChecked(false); else ui->gbAltitude->setChecked(true); ui->cbBarometerType->setCurrentIndex(config.i2c_module - 1); ui->cbAltitudeFilter->setCurrentIndex(config.bmp280_filter - 1); ui->cbVarioAutoOffset->setChecked(config.vario_auto_offset); // Refresh rate ui->sbRpmRate->setValue(config.refresh_rate_rpm); ui->sbVoltageRate->setValue(config.refresh_rate_voltage); ui->sbCurrentRate->setValue(config.refresh_rate_current); ui->sbTemperatureRate->setValue(config.refresh_rate_temperature); ui->sbGpsRate->setValue(config.refresh_rate_gps); ui->sbConsumptionRate->setValue(config.refresh_rate_consumption); ui->sbVarioRate->setValue(config.refresh_rate_vario); ui->sbAirspeedRate->setValue(config.refresh_rate_airspeed); // ui->sbDefRate->setValue(config.refresh_rate_def); // Averaging ui->sbRpmAvg->setValue(qRound(2 / config.alpha_rpm - 1)); ui->sbVoltageAvg->setValue(qRound(2 / config.alpha_voltage - 1)); ui->sbCurrentAvg->setValue(qRound(2 / config.alpha_current - 1)); ui->sbTemperatureAvg->setValue(qRound(2 / config.alpha_temperature - 1)); ui->sbVarioAvg->setValue(qRound(2 / config.alpha_vario - 1)); ui->sbAirspeedAvg->setValue(qRound(2 / config.alpha_airspeed - 1)); // Analog voltage multipliers ui->sbVoltage1Mult->setValue(config.analog_voltage_multiplier); // config.analog_voltage2_multiplier = ui->sbVoltage2Mult->value(); // RPM Multipliers ui->sbPairOfPoles->setValue(config.pairOfPoles); ui->sbMainTeeth->setValue(config.mainTeeth); ui->sbPinionTeeth->setValue(config.pinionTeeth); // PWM out ui->cbPwmOut->setChecked(config.enable_pwm_out); // Smartport // config.smartport_data_id; ui->sbSensorId->setValue(config.smartport_sensor_id); // XBUS Clock stretch ui->cbClockStretch->setChecked(config.xbus_clock_stretch); ui->cbAlternativePacket->setChecked(config.xbus_use_alternative_volt_temp); // Ibus ui->cbAlternativeCoordinates->setChecked(config.ibus_alternative_coordinates); // Jeti Ex if (config.jeti_gps_speed_units_kmh == true) ui->cbSpeedUnitsGps->setCurrentIndex(0); else ui->cbSpeedUnitsGps->setCurrentIndex(1); // FPort ui->cbFPortInverted->setChecked(config.fport_inverted); // FBUS ui->cbFbusInverted->setChecked(config.fbus_inverted); // HW V4/V5 parameters ui->cbInitDelay->setChecked(config.enable_esc_hw4_init_delay); ui->cbEscAutoOffset->setChecked(!config.esc_hw4_is_manual_offset); ui->sbEscOffset->setValue(config.esc_hw4_offset); // config.esc_hw4_init_delay_duration = 10000; ui->sbVoltageMultiplier->setValue(config.esc_hw4_voltage_multiplier * 100000); ui->sbCurrentMultiplier->setValue(config.esc_hw4_current_multiplier * 100000); ui->cbHw4AutoDetect->setChecked(config.esc_hw4_auto_detect); // Smart esc ui->cbCalculateConsumption->setChecked(config.smart_esc_calc_consumption); // Fuel flow ui->gbFuelmeter->setChecked(config.enable_fuel_flow); ui->sbMlPulse->setValue(config.fuel_flow_ml_per_pulse); // Fuel pressure ui->gbFuelPressure->setChecked(config.enable_fuel_pressure); if (config.xgzp68xxd_k == 8192) ui->cbMaxPressure->setCurrentIndex(0); else if (config.xgzp68xxd_k == 4096) ui->cbMaxPressure->setCurrentIndex(1); else if (config.xgzp68xxd_k == 2048) ui->cbMaxPressure->setCurrentIndex(2); else if (config.xgzp68xxd_k == 1024) ui->cbMaxPressure->setCurrentIndex(3); else if (config.xgzp68xxd_k == 512) ui->cbMaxPressure->setCurrentIndex(4); else if (config.xgzp68xxd_k == 256) ui->cbMaxPressure->setCurrentIndex(5); else if (config.xgzp68xxd_k == 128) ui->cbMaxPressure->setCurrentIndex(6); else if (config.xgzp68xxd_k == 64) ui->cbMaxPressure->setCurrentIndex(7); else if (config.xgzp68xxd_k == 32) ui->cbMaxPressure->setCurrentIndex(8); else if (config.xgzp68xxd_k == 16) ui->cbMaxPressure->setCurrentIndex(9); else if (config.xgzp68xxd_k == 8) ui->cbMaxPressure->setCurrentIndex(10); else ui->cbMaxPressure->setCurrentIndex(11); // GPIOs ui->cbGpio17->setChecked(config.gpio_mask & 1); ui->cbGpio18->setChecked(config.gpio_mask & (1 << 1)); ui->cbGpio19->setChecked(config.gpio_mask & (1 << 2)); ui->cbGpio20->setChecked(config.gpio_mask & (1 << 3)); ui->cbGpio21->setChecked(config.gpio_mask & (1 << 4)); ui->cbGpio22->setChecked(config.gpio_mask & (1 << 5)); ui->sbGpioInterval->setValue(config.gpio_interval); // Gyro MPU6050 ui->gbGyro->setChecked(config.enable_gyro); ui->cbGyroAccSens->setCurrentIndex(config.mpu6050_acc_scale); ui->cbGyroSens->setCurrentIndex(config.mpu6050_gyro_scale); ui->sbGyroWeight->setValue(config.mpu6050_gyro_weighting); ui->sbGyroFilter->setValue(config.mpu6050_filter); // ui->cbGpsRate->setValue // INA3221 (lipo) ui->cbIna3221Filter->currentData(config.ina3221_filter); ui->sbLipoCells->setValue(config.lipo_cells); ui->gbLipo->setChecked(config.enable_lipo); // SRXL2 uint sensor_id_srxl2 = config.sensor_id_srxl2 ; if (sensor_id_srxl2 < 0x01 || sensor_id_srxl2 > 0x0F) sensor_id_srxl2 = 0x01; ui->sbSensorIdSrxl2->setValue(sensor_id_srxl2); } void MainWindow::getConfigFromUi() { /* Config version */ config.version = CONFIG_VERSION; /* Receiver protocol */ config.rx_protocol = (rx_protocol_t)ui->cbReceiver->currentData().toInt(); /* Serial Monitor */ config.serial_monitor_baudrate = ui->cbBaudrate->currentText().toInt(); config.serial_monitor_gpio = ui->cbSerialMonitorGpio->currentText().toInt(); config.serial_monitor_stop_bits = ui->cbStopbits->currentText().toInt(); if (ui->cbParity->currentText() == "None") config.serial_monitor_parity = 0; else if (ui->cbParity->currentText() == "Odd") config.serial_monitor_parity = 1; else config.serial_monitor_parity = 2; config.serial_monitor_timeout_ms = ui->sbTimeout->value(); config.serial_monitor_inverted = ui->cbInverted->isChecked(); config.serial_monitor_format = ui->cbSerialFormat->currentText() == "Hex" ? FORMAT_HEX : FORMAT_STRING; /* Sensors */ // ESC if (ui->gbEsc->isChecked()) config.esc_protocol = (esc_protocol_t)(ui->cbEsc->currentIndex() + 1); else config.esc_protocol = esc_protocol_t::ESC_NONE; // GPS config.enable_gps = ui->gbGps->isChecked(); config.gps_baudrate = ui->cbGpsBaudrate->currentText().toInt(); config.gps_rate = ui->cbGpsRate->currentText().toInt(); config.gps_protocol = ui->cbGpsProtocol->currentIndex(); // Voltage config.enable_analog_voltage = ui->gbVoltage1->isChecked(); config.analog_voltage_multiplier = ui->sbVoltage1Mult->value(); config.sbus_battery_slot = ui->ckSbusBattery->isChecked(); // Current config.enable_analog_current = ui->gbCurrent->isChecked(); config.analog_current_type = (analog_current_type_t)ui->cbCurrentSensorType->currentIndex(); config.analog_current_quiescent_voltage = ui->sbQuiescentVoltage->value(); if (ui->cbCurrentSensorType->currentIndex() == analog_current_type_t::CURRENT_TYPE_HALL) { if (ui->cbCurrentAutoOffset->isChecked()) { config.analog_current_autoffset = true; config.analog_current_offset = 0; } else { config.analog_current_autoffset = false; config.analog_current_offset = ui->sbQuiescentVoltage->value(); } config.analog_current_multiplier = 1000 / ui->sbCurrentSens->value(); } else if (ui->cbCurrentSensorType->currentIndex() == analog_current_type_t::CURRENT_TYPE_SHUNT) { config.analog_current_autoffset = false; config.analog_current_offset = 0; config.analog_current_multiplier = ui->sbAnalogCurrentMultiplier->value(); } // Temperature config.enable_analog_ntc = ui->gbTemp1->isChecked(); config.ntc_offset = ui->sbTempOffset->value(); // Airspeed config.enable_analog_airspeed = ui->gbAirspeed->isChecked(); config.airspeed_vcc = ui->sbAirspeedVcc->value() * 100; config.airspeed_offset = ui->sbAirspeedOffset->value(); // Vario if (ui->gbAltitude->isChecked()) config.i2c_module = (i2c_module_t)(ui->cbBarometerType->currentIndex() + 1); else config.i2c_module = i2c_module_t::I2C_NONE; config.bmp280_filter = ui->cbAltitudeFilter->currentIndex() + 1; config.vario_auto_offset = ui->cbVarioAutoOffset->isChecked(); // Refresh rate config.refresh_rate_rpm = ui->sbRpmRate->value(); config.refresh_rate_voltage = ui->sbVoltageRate->value(); config.refresh_rate_current = ui->sbCurrentRate->value(); config.refresh_rate_temperature = ui->sbTemperatureRate->value(); config.refresh_rate_gps = ui->sbGpsRate->value(); config.refresh_rate_consumption = ui->sbConsumptionRate->value(); config.refresh_rate_vario = ui->sbVarioRate->value(); config.refresh_rate_airspeed = ui->sbAirspeedRate->value(); // Averaging config.alpha_rpm = 2.0 / (ui->sbRpmAvg->value() + 1); config.alpha_voltage = 2.0 / (ui->sbVoltageAvg->value() + 1); config.alpha_current = 2.0 / (ui->sbCurrentAvg->value() + 1); config.alpha_temperature = 2.0 / (ui->sbTemperatureAvg->value() + 1); config.alpha_vario = 2.0 / (ui->sbVarioAvg->value() + 1); config.alpha_airspeed = 2.0 / (ui->sbAirspeedAvg->value() + 1); // Analog rate config.analog_rate = ui->sbAnalogRate->value(); // RPM Multipliers config.pairOfPoles = ui->sbPairOfPoles->value(); config.mainTeeth = ui->sbMainTeeth->value(); config.pinionTeeth = ui->sbPinionTeeth->value(); // PWM out config.enable_pwm_out = ui->cbPwmOut->isChecked(); // Smartport // config.smartport_data_id = 0x5000; config.smartport_sensor_id = ui->sbSensorId->value(); // XBUS Clock stretch config.xbus_clock_stretch = ui->cbClockStretch->isChecked(); config.xbus_use_alternative_volt_temp = ui->cbAlternativePacket->isChecked(); // Jeti Ex if (ui->cbSpeedUnitsGps->currentText() == "km/h") config.jeti_gps_speed_units_kmh = true; else config.jeti_gps_speed_units_kmh = false; // Ibus config.ibus_alternative_coordinates = ui->cbAlternativeCoordinates->isChecked(); // FPort config.fport_inverted = ui->cbFPortInverted->isChecked(); // FBUS config.fbus_inverted = ui->cbFbusInverted->isChecked(); // HW V4/V5 parameters config.enable_esc_hw4_init_delay = ui->cbInitDelay->isChecked(); config.esc_hw4_is_manual_offset = !ui->cbEscAutoOffset->isChecked(); config.esc_hw4_offset = ui->sbEscOffset->value(); config.esc_hw4_voltage_multiplier = ui->sbVoltageMultiplier->value() / 100000.0; config.esc_hw4_current_multiplier = ui->sbCurrentMultiplier->value() / 100000.0; config.esc_hw4_auto_detect = ui->cbHw4AutoDetect->isChecked(); // Smart esc config.smart_esc_calc_consumption = ui->cbCalculateConsumption->isChecked(); // Fuel flow config.enable_fuel_flow = ui->gbFuelmeter->isChecked(); config.fuel_flow_ml_per_pulse = ui->sbMlPulse->value(); // Fuel pressure config.enable_fuel_pressure = ui->gbFuelPressure->isChecked(); if (ui->cbMaxPressure->currentIndex() == 0) config.xgzp68xxd_k = 8192; else if (ui->cbMaxPressure->currentIndex() == 1) config.xgzp68xxd_k = 4096; else if (ui->cbMaxPressure->currentIndex() == 2) config.xgzp68xxd_k = 2048; else if (ui->cbMaxPressure->currentIndex() == 3) config.xgzp68xxd_k = 1024; else if (ui->cbMaxPressure->currentIndex() == 4) config.xgzp68xxd_k = 512; else if (ui->cbMaxPressure->currentIndex() == 5) config.xgzp68xxd_k = 256; else if (ui->cbMaxPressure->currentIndex() == 6) config.xgzp68xxd_k = 128; else if (ui->cbMaxPressure->currentIndex() == 7) config.xgzp68xxd_k = 64; else if (ui->cbMaxPressure->currentIndex() == 8) config.xgzp68xxd_k = 32; else if (ui->cbMaxPressure->currentIndex() == 9) config.xgzp68xxd_k = 16; else if (ui->cbMaxPressure->currentIndex() == 10) config.xgzp68xxd_k = 8; else config.xgzp68xxd_k = 4; // GPIOs config.gpio_mask = ui->cbGpio17->isChecked(); config.gpio_mask |= ui->cbGpio18->isChecked() << 1; config.gpio_mask |= ui->cbGpio19->isChecked() << 2; config.gpio_mask |= ui->cbGpio20->isChecked() << 3; config.gpio_mask |= ui->cbGpio21->isChecked() << 4; config.gpio_mask |= ui->cbGpio22->isChecked() << 5; config.gpio_interval = ui->sbGpioInterval->value(); // Gyro MPU6050 config.enable_gyro = ui->gbGyro->isChecked(); config.mpu6050_acc_scale = ui->cbGyroAccSens->currentIndex(); config.mpu6050_gyro_scale = ui->cbGyroSens->currentIndex(); config.mpu6050_gyro_weighting = ui->sbGyroWeight->value(); config.mpu6050_filter = ui->sbGyroFilter->value(); // config.mpu6050_rate = ui->cbGyroSamplerate->currentText().toInt(); // INA3221 (lipo) config.ina3221_filter = ui->cbIna3221Filter->itemData(ui->cbIna3221Filter->currentIndex()).toUInt(); config.lipo_cells = ui->sbLipoCells->value(); config.enable_lipo = ui->gbLipo->isChecked(); // SRXL2 config.sensor_id_srxl2 = ui->sbSensorIdSrxl2->value(); if (config.sensor_id_srxl2 < 0x01 || config.sensor_id_srxl2 > 0x0F) config.sensor_id_srxl2 = 0x01; // Debug config.debug = 0; // disabled from msrc_gui } void MainWindow::requestSerialConfig() { // disable debug char header = 0x30; serial->write(&header, 1); char command = 0x34; serial->write(&command, 1); isDebug = false; QTimer::singleShot(2000, this, SLOT(readSerialConfig())); } void MainWindow::readSerialConfig() { serial->readAll(); // request config char header = 0x30; serial->write(&header, 1); char command = 0x31; serial->write(&command, 1); } QStringList MainWindow::fillPortsInfo() { const QList infos = QSerialPortInfo::availablePorts(); QStringList list; ui->cbPortList->clear(); for (const QSerialPortInfo &info : infos) { list.append(info.portName() + " (" + info.manufacturer() + " " + info.description() + ")"); ui->cbPortList->addItem(info.portName() + " (" + info.manufacturer() + " " + info.description() + ")", info.portName()); } return list; } void MainWindow::checkPorts() { const QList infos = QSerialPortInfo::availablePorts(); QStringList list; for (const QSerialPortInfo &info : infos) { list.append(info.portName() + " (" + info.manufacturer() + " " + info.description() + ")"); } std::sort(portsList.begin(), portsList.end()); std::sort(list.begin(), list.end()); if (portsList != list) { QString currentPort = ui->cbPortList->currentText(); if (isConnected && !list.contains(currentPort)) closeSerialPort(); portsList = fillPortsInfo(); ui->cbPortList->setCurrentIndex(ui->cbPortList->findText(currentPort)); if (ui->cbPortList->currentIndex() == -1 && ui->cbPortList->count()) ui->cbPortList->setCurrentIndex(0); } } void MainWindow::exitApp() { QApplication::quit(); } void MainWindow::enableWidgets(QWidget *widget, bool enable) { QList widgets = widget->findChildren(); QWidget *child; foreach (child, widgets) child->setEnabled(enable); } void MainWindow::on_cbReceiver_currentTextChanged(const QString &arg1) { if (arg1 == "Spektrum XBUS") { ui->cbClockStretch->setVisible(true); } else { ui->cbClockStretch->setVisible(false); } if (arg1 == "Spektrum SRXL2") { ui->lbSensorIdSrxl2->setVisible(true); ui->sbSensorIdSrxl2->setVisible(true); } else { ui->lbSensorIdSrxl2->setVisible(false); ui->sbSensorIdSrxl2->setVisible(false); } if (arg1 == "Spektrum XBUS" || arg1 == "Spektrum SRXL" || arg1 == "Spektrum SRXL2") { ui->cbAlternativePacket->setVisible(true); } else { ui->cbAlternativePacket->setVisible(false); } if (arg1 == "Frsky Smartport" || arg1 == "Frsky D" || arg1 == "Frsky FPort" || arg1 == "Frsky FBUS") { ui->gbRate->setVisible(true); } else { ui->gbRate->setVisible(false); } if (arg1 == "Frsky FPort") { ui->cbFPortInverted->setVisible(true); } else { ui->cbFPortInverted->setVisible(false); } if (arg1 == "Frsky FBUS") { ui->cbFbusInverted->setVisible(true); } else { ui->cbFbusInverted->setVisible(false); } if (arg1 == "Frsky Smartport" || arg1 == "Frsky FBUS") { ui->lbSensorId->setVisible(true); ui->sbSensorId->setVisible(true); } else { ui->lbSensorId->setVisible(false); ui->sbSensorId->setVisible(false); } if (arg1 == "Flysky IBUS") { ui->cbAlternativeCoordinates->setVisible(true); } else { ui->cbAlternativeCoordinates->setVisible(false); } if (arg1 == "Jeti Ex Bus" || arg1 == "Jeti Ex Sensor") { ui->cbSpeedUnitsGps->setVisible(true); ui->lbSpeedUnitsGps->setVisible(true); } else { ui->cbSpeedUnitsGps->setVisible(false); ui->lbSpeedUnitsGps->setVisible(false); } if (arg1 == "Futaba SBUS2") { ui->ckSbusBattery->setVisible(true); ui->ckSbusExtVolt->setVisible(true); } else { ui->ckSbusBattery->setVisible(false); ui->ckSbusExtVolt->setVisible(false); } if (arg1 == "Serial Monitor") { ui->cbBaudrate->setVisible(true); ui->cbStopbits->setVisible(true); ui->cbParity->setVisible(true); ui->sbTimeout->setVisible(true); ui->cbInverted->setVisible(true); ui->lbBaudrate->setVisible(true); ui->lbStopbits->setVisible(true); ui->lbParity->setVisible(true); ui->lbTimeout->setVisible(true); ui->lbSerialFormat->setVisible(true); ui->cbSerialFormat->setVisible(true); ui->gbSensors->setVisible(false); ui->gbAverage->setVisible(false); ui->lbSerialMonitorGpio->setVisible(true); ui->cbSerialMonitorGpio->setVisible(true); } else { ui->cbBaudrate->setVisible(false); ui->cbStopbits->setVisible(false); ui->cbParity->setVisible(false); ui->sbTimeout->setVisible(false); ui->cbInverted->setVisible(false); ui->lbBaudrate->setVisible(false); ui->lbStopbits->setVisible(false); ui->lbParity->setVisible(false); ui->lbTimeout->setVisible(false); ui->lbSerialFormat->setVisible(false); ui->cbSerialFormat->setVisible(false); ui->gbSensors->setVisible(true); ui->gbAverage->setVisible(true); ui->lbSerialMonitorGpio->setVisible(false); ui->cbSerialMonitorGpio->setVisible(false); } // Fuel meter if (arg1 == "Frsky Smartport" || arg1 == "Jeti Ex Bus" || arg1 == "Jeti Ex Sensor" || arg1 == "Spektrum XBUS" || arg1 == "HOTT" || arg1 == "Frsky FPort" || arg1 == "Frsky FBUS") { ui->gbFuelmeter->setVisible(true); } else { ui->gbFuelmeter->setVisible(false); } // Fuel pressure if (arg1 == "Spektrum SRXL" || arg1 == "Spektrum SRXL2" || arg1 == "Jeti Ex Bus" || arg1 == "Jeti Ex Sensor" || arg1 == "Spektrum XBUS" || arg1 == "HOTT") { ui->gbFuelPressure->setVisible(true); } else { ui->gbFuelPressure->setVisible(false); } // GPIO if (arg1 == "Frsky Smartport" || arg1 == "Frsky FPort" || arg1 == "Frsky FBUS") { ui->gbGpio->setVisible(true); } else { ui->gbGpio->setVisible(false); } // Airspeed if (arg1 == "Sanwa" || arg1 == "GHST") { ui->gbAirspeed->setVisible(false); } else { ui->gbAirspeed->setVisible(true); } // Temperature if (arg1 == "GHST") { ui->gbTemp1->setVisible(false); } else { ui->gbTemp1->setVisible(true); } // GPS, current, vario if (arg1 == "Sanwa") { ui->gbGps->setVisible(false); ui->gbCurrent->setVisible(false); ui->gbAltitude->setVisible(false); } else { ui->gbGps->setVisible(true); ui->gbCurrent->setVisible(true); ui->gbAltitude->setVisible(true); } // Lipo if (arg1 == "ELRS/CRSF" || arg1 == "Frsky Smartport" || arg1 == "Frsky FPort" || arg1 == "Frsky FBUS" || arg1 == "HOTT" || arg1 == "JetiEx Bus" || arg1 == "JetiEx Bus" || arg1 == "JetiEx Sensor" || arg1 == "Spektrum SRXL" || arg1 == "Spektrum SRXL2") { ui->gbLipo->setVisible(true); } else { ui->gbLipo->setVisible(false); } // Average elements if (arg1 == "Sanwa") { ui->lbRpmAvg->setVisible(true); ui->sbRpmAvg->setVisible(true); ui->lbVoltageAvg->setVisible(true); ui->sbVoltageAvg->setVisible(true); ui->lbCurrentAvg->setVisible(false); ui->sbCurrentAvg->setVisible(false); ui->lbTemperatureAvg->setVisible(true); ui->sbTemperatureAvg->setVisible(true); ui->lbVarioAvg->setVisible(false); ui->sbVarioAvg->setVisible(false); ui->lbAirspeedAvg->setVisible(false); ui->sbAirspeedAvg->setVisible(false); } else if (arg1 == "GHST") { ui->lbRpmAvg->setVisible(false); ui->sbRpmAvg->setVisible(false); ui->lbVoltageAvg->setVisible(true); ui->sbVoltageAvg->setVisible(true); ui->lbCurrentAvg->setVisible(true); ui->sbCurrentAvg->setVisible(true); ui->lbTemperatureAvg->setVisible(false); ui->sbTemperatureAvg->setVisible(false); ui->lbVarioAvg->setVisible(true); ui->sbVarioAvg->setVisible(true); ui->lbAirspeedAvg->setVisible(false); ui->sbAirspeedAvg->setVisible(false); } else { ui->lbRpmAvg->setVisible(true); ui->sbRpmAvg->setVisible(true); ui->lbVoltageAvg->setVisible(true); ui->sbVoltageAvg->setVisible(true); ui->lbCurrentAvg->setVisible(true); ui->sbCurrentAvg->setVisible(true); ui->lbTemperatureAvg->setVisible(true); ui->sbTemperatureAvg->setVisible(true); ui->lbVarioAvg->setVisible(true); ui->sbVarioAvg->setVisible(true); ui->lbAirspeedAvg->setVisible(true); ui->sbAirspeedAvg->setVisible(true); } generateCircuit(ui->lbCircuit); } void MainWindow::on_cbEsc_currentTextChanged(const QString &arg1) { if (arg1 == "Smart ESC/BAT") ui->cbCalculateConsumption->setVisible(true); else ui->cbCalculateConsumption->setVisible(false); if (arg1 == "Hobbywing V4/Flyfun (not VBAR firmware)") { ui->gbEscParameters->setVisible(true); ui->cbPwmOut->setVisible(true); ui->cbHw4AutoDetect->setVisible(true); } else { ui->gbEscParameters->setVisible(false); ui->cbPwmOut->setVisible(false); ui->cbHw4AutoDetect->setVisible(false); } generateCircuit(ui->lbCircuit); } void MainWindow::on_gbEsc_toggled(bool enabled) { enableWidgets(ui->gbEsc, enabled); generateCircuit(ui->lbCircuit); } void MainWindow::on_gbVoltage1_toggled(bool enabled) { enableWidgets(ui->gbVoltage1, enabled); generateCircuit(ui->lbCircuit); } void MainWindow::on_gbTemp1_toggled(bool checked) { Q_UNUSED(checked); generateCircuit(ui->lbCircuit); } void MainWindow::on_gbAltitude_toggled(bool enabled) { enableWidgets(ui->gbAltitude, enabled); generateCircuit(ui->lbCircuit); } void MainWindow::on_gbCurrent_toggled(bool enabled) { enableWidgets(ui->gbCurrent, enabled); generateCircuit(ui->lbCircuit); } void MainWindow::on_cbBarometerType_currentTextChanged(const QString &arg1) { if (arg1 == "BMP280") { ui->cbAltitudeFilter->setVisible(true); ui->lbAltitudeFilter->setVisible(true); } else { ui->cbAltitudeFilter->setVisible(false); ui->lbAltitudeFilter->setVisible(false); } // Q_UNUSED(arg1); // generateCircuit(ui->lbCircuit); } void MainWindow::on_btCircuit_clicked() { CircuitDialog circuitDialog; circuitDialog.setModal(true); circuitDialog.mainWindow = this; circuitDialog.exec(); } void MainWindow::on_gbGps_toggled(bool enabled) { enableWidgets(ui->gbGps, enabled); generateCircuit(ui->lbCircuit); } void MainWindow::on_gbAirspeed_toggled(bool enabled) { enableWidgets(ui->gbAirspeed, enabled); generateCircuit(ui->lbCircuit); } void MainWindow::on_gbFuelmeter_toggled(bool enabled) { enableWidgets(ui->gbFuelmeter, enabled); generateCircuit(ui->lbCircuit); } void MainWindow::on_cbCurrentAutoOffset_toggled(bool checked) { if (checked) { ui->lbQuiescentVoltage->setVisible(false); ui->sbQuiescentVoltage->setVisible(false); } else if (ui->cbCurrentSensorType->currentIndex() == analog_current_type_t::CURRENT_TYPE_HALL) { ui->lbQuiescentVoltage->setVisible(true); ui->sbQuiescentVoltage->setVisible(true); } } void MainWindow::on_cbCurrentSensorType_currentTextChanged(const QString &arg1) { if (arg1 == "Hall effect") { ui->cbCurrentAutoOffset->setVisible(true); ui->lbCurrentSens->setVisible(true); ui->sbCurrentSens->setVisible(true); ui->lbAnalogCurrentMultiplier->setVisible(false); ui->sbAnalogCurrentMultiplier->setVisible(false); if (ui->cbCurrentAutoOffset->isChecked()) { ui->lbQuiescentVoltage->setVisible(false); ui->sbQuiescentVoltage->setVisible(false); } else { ui->lbQuiescentVoltage->setVisible(true); ui->sbQuiescentVoltage->setVisible(true); } } if (arg1 == "Shunt resistor") { ui->cbCurrentAutoOffset->setVisible(false); ui->lbCurrentSens->setVisible(false); ui->sbCurrentSens->setVisible(false); ui->lbQuiescentVoltage->setVisible(false); ui->sbQuiescentVoltage->setVisible(false); ui->lbAnalogCurrentMultiplier->setVisible(true); ui->sbAnalogCurrentMultiplier->setVisible(true); } } void MainWindow::on_cbClockStretch_toggled(bool checked) { Q_UNUSED(checked); generateCircuit(ui->lbCircuit); } void MainWindow::on_cbEscAutoOffset_stateChanged(int arg1) { if (arg1) ui->sbEscOffset->setVisible(false); else ui->sbEscOffset->setVisible(true); } void MainWindow::on_btScroll_clicked() { autoscroll = !autoscroll; if (autoscroll) ui->btScroll->setText("No scroll"); else ui->btScroll->setText("Autoscroll"); } void MainWindow::on_gbFuelPressure_toggled(bool enabled) { enableWidgets(ui->gbFuelPressure, enabled); generateCircuit(ui->lbCircuit); } void MainWindow::on_ckSbusBattery_toggled(bool checked) { ui->ckSbusExtVolt->setChecked(!checked); } void MainWindow::on_ckSbusExtVolt_toggled(bool checked) { ui->ckSbusBattery->setChecked(!checked); } void MainWindow::on_cbHw4AutoDetect_toggled(bool checked) { if (checked) { ui->gbEscParameters->setVisible(false); } else { ui->gbEscParameters->setVisible(true); } } void MainWindow::on_gbGyro_toggled(bool enabled) { enableWidgets(ui->gbGyro, enabled); generateCircuit(ui->lbCircuit); } ================================================ FILE: msrc_gui/mainwindow.h ================================================ #ifndef MAINWINDOW_H #define MAINWINDOW_H #define CONFIG_VERSION 2 #include #include #include #include #include #include #include #include #include #include #include #include "shared.h" QT_BEGIN_NAMESPACE namespace Ui { class MainWindow; } QT_END_NAMESPACE class MainWindow : public QMainWindow { Q_OBJECT public: MainWindow(QWidget *parent = nullptr); ~MainWindow(); void generateCircuit(QLabel *label); private: Ui::MainWindow *ui; QSerialPort *serial = nullptr; QByteArray data; bool isConnected = false; QStringList portsList; config_t config; bool isDebug = false; bool autoscroll = true; void requestSerialConfig(); void getConfigFromUi(); void setUiFromConfig(); void openSerialPort(); void closeSerialPort(); void enableWidgets(QWidget *widget, bool enable); private slots: void buttonSerialPort(); void buttonDebug(); void buttonClearDebug(); void readSerial(); void readSerialConfig(); QStringList fillPortsInfo(); void checkPorts(); void writeSerialConfig(); void defaultConfig(); void openConfig(); void saveConfig(); void showAbout(); void exitApp(); void on_cbReceiver_currentTextChanged(const QString &arg1); void on_cbEsc_currentTextChanged(const QString &arg1); void on_gbEsc_toggled(bool arg1); void on_gbVoltage1_toggled(bool arg1); void on_gbTemp1_toggled(bool checked); void on_gbAltitude_toggled(bool arg1); void on_gbCurrent_toggled(bool arg1); void on_cbBarometerType_currentTextChanged(const QString &arg1); void on_btCircuit_clicked(); void on_gbGps_toggled(bool arg1); void on_cbCurrentAutoOffset_toggled(bool checked); void on_cbCurrentSensorType_currentTextChanged(const QString &arg1); void on_cbClockStretch_toggled(bool checked); void on_cbEscAutoOffset_stateChanged(int arg1); void on_gbAirspeed_toggled(bool arg1); void on_gbFuelmeter_toggled(bool arg1); void on_btScroll_clicked(); void on_gbFuelPressure_toggled(bool arg1); void on_ckSbusBattery_toggled(bool checked); void on_ckSbusExtVolt_toggled(bool checked); void on_cbHw4AutoDetect_toggled(bool checked); void on_gbGyro_toggled(bool arg1); }; #endif // MAINWINDOW_H ================================================ FILE: msrc_gui/mainwindow.ui ================================================ MainWindow true 0 0 1267 765 MSRC Link :/res/msrc.png:/res/msrc.png QLayout::SetDefaultConstraint true Port true true Connect true Update config 1 Receiver true 0 0 581 774 true true true Refresh rate (ms) true GPS true Vario true 1 2000 1 100 true RPM true Current true Airspeed true 1 2000 1 100 true 1 2000 1 100 true 1 2000 1 100 true Temperature true 1 2000 1 100 true 1 2000 1 100 true 1 2000 1 100 true 1 2000 1 100 true Voltage true Consumption Qt::Vertical 20 40 true Parity true Inverted Inverted true true true Alternative coordinates true Baudrate true Protocol true 1 30 true Timeout (ms) Output format true true 100 Inverted true Stop bits true Clock stretch true Sensor ID true Alternative packet GPIO Sensor ID (0x3n) 1 15 Qt::Vertical 20 40 Sensors true 0 -1230 581 2242 true 0 0 ESC true false false RPM multipliers false 1 1000 1 false Main gear teeth false Pair of Poles false 1 1000 1 false 1 20 1 false Pinion gear teeth false PWM out false Protocol Auto detect false Parameters false 0 100000.000000000000000 1.000000000000000 false Current multiplier (x1e5) false 0 0.000000000000000 100000.000000000000000 1.000000000000000 false Auto offset true false Voltage multiplier (x1e5) false 0 2000.000000000000000 false Calculate consumption (no Smart Battery) false Init delay true 0 0 GPS true false false false Speed units Rate (Hz) false false Baud rate Protocol true 0 0 16777215 16777215 Vario true false false Auto offset false false false Filter false Model Fuel pressure (XGZP68XXD) true false Max pressure (kPa) Gyro (MPU6050) true false Sample rate 6 Low pass filter (0 none, 6 max) Accel sensitivity (g) Gyro sensitivity (°/s) Gyro weight (%) 100 Lipo true false Type Filter Cells 1 6 true 0 0 Analog false true Analog rate (Hz) true 1 100 10 Temperature true Offset -10 10 true Voltage true false false 1000.000000000000000 7.800000000000000 false <html><head/><body><p>(R1+R2)/R2</p></body></html> Multiplier Battery Ext-volt true Current true false false Zero current output voltage, Viout (V) false Type false 2 3.300000000000000 0.010000000000000 false Auto offset false false false Multiplier false Sensitivity (mV/A) false 0.010000000000000 false 2 Airspeed true false Vcc 2 3.000000000000000 6.000000000000000 0.010000000000000 5.000000000000000 Offset (mV) 0 -1000.000000000000000 1000.000000000000000 1.000000000000000 0.000000000000000 Fuel meter true false ml/pulse 4 0.000100000000000 GPIOs 17-22 false false 21 10 10000 17 Interval (ms) 19 20 18 22 true Averaging elements true Temperature true Voltage true 1 16 true RPM true 1 16 true Airspeed true Vario true 1 16 1 true 1 16 1 true 1 16 1 true Current true 1 16 1 QLayout::SetDefaultConstraint true 1 Circuit true 0 0 500 0 Circuit image true true Open in window Conections Ubuntu 10 connections Qt::MarkdownText false Log Ubuntu Mono true Enable Log Clear Log No scroll 0 0 1267 22 File About false Update config Exit Open config... Save config... About... false Restore default config ================================================ FILE: msrc_gui/msrc_gui.pro ================================================ QT += core gui serialport greaterThan(QT_MAJOR_VERSION, 4): QT += widgets system(git describe --tags > VERSION) PROJECT_VERSION = "\\\"$$cat(VERSION)"\\\" system(rm VERSION) message($${PROJECT_VERSION}) DEFINES += PROJECT_VERSION=$${PROJECT_VERSION} QMAKE_APPLE_DEVICE_ARCHS = x86_64 arm64 CONFIG += c++11 # You can make your code fail to compile if it uses deprecated APIs. # In order to do so, uncomment the following line. #DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000 # disables all the APIs deprecated before Qt 6.0.0 INCLUDEPATH += ../include SOURCES += \ circuitdialog.cpp \ main.cpp \ mainwindow.cpp HEADERS += \ circuitdialog.h \ mainwindow.h FORMS += \ circuitdialog.ui \ mainwindow.ui # Default rules for deployment. qnx: target.path = /tmp/$${TARGET}/bin else: unix:!android: target.path = /opt/$${TARGET}/bin !isEmpty(target.path): INSTALLS += target RESOURCES += \ resources.qrc ================================================ FILE: msrc_gui/resources.qrc ================================================ res/castle_rp2040_zero.png res/gps_rp2040_zero.png res/ntc_rp2040_zero.png res/pwm_rp2040_zero.png res/receiver_frsky_d_rp2040_zero.png res/receiver_serial_rp2040_zero.png res/rp2040_zero.png res/vario_rp2040_zero.png res/voltage_rp2040_zero.png res/esc_rp2040_zero.png res/current_rp2040_zero.png res/airspeed_rp2040_zero.png res/receiver_xbus_rp2040_zero.png res/receiver_hitec_rp2040_zero.png res/clock_stretch_xbus_rp2040_zero.png res/msrc.png res/msrc.ico res/smart_esc.png res/fuel_pressure.png res/fuel_meter.png ================================================ FILE: msrc_gui_web/README.md ================================================ # MSRC Link Web A web-based alternative for the Qt desktop configurator (`msrc_gui/`), built with vanilla JavaScript, the **Web Serial API**, and **PWA** offline support. Inspired by the Betaflight Configurator approach. ## Why - No native app installation required — runs in Chrome/Edge (v89+). - Works offline once loaded (Service Worker + cache). - Identical binary protocol and `config_t` struct layout as the desktop GUI — `.cfg` files are interchangeable. ## Project structure ``` msrc_gui_web/ ├── index.html # Full UI (493 lines) — all widgets from the Qt GUI ├── css/ │ └── style.css # Dark theme (387 lines) ├── js/ │ ├── config_struct.js # Binary config_t serialization (308 lines) │ ├── serial.js # Web Serial API communication (168 lines) │ ├── ui.js # Bidirectional config↔UI mapping (534 lines) │ ├── circuit.js # Canvas-based circuit diagram (137 lines) │ └── app.js # Main orchestration (218 lines) ├── manifest.json # PWA manifest ├── sw.js # Service Worker (cache-first) ├── res/ # 19 PNG images (copied from msrc_gui/res/) └── test/ └── verify_layout.js # Struct layout verification test ``` No build step, no bundler, no npm — just static files served by any HTTP server. ## How each file was built ### `js/config_struct.js` — Binary struct serialization This is the most critical file. It replicates the exact memory layout of `config_t` from `include/shared.h`. **Approach:** 1. Every field of `config_t` is listed in declaration order with its C type (`u8`, `u16`, `u32`, `f32`, `bool`, `i16`). 2. A `buildLayout()` function walks the fields applying **natural alignment rules** (matching GCC ARM / Clang defaults): each field is aligned to its own size boundary (1-byte for `u8`/`bool`, 2-byte for `u16`/`i16`, 4-byte for `u32`/`f32`). 3. Byte offsets are computed automatically — no hardcoded offsets. 4. `configEncode(obj)` → `Uint8Array` and `configDecode(Uint8Array)` → `obj` use `DataView` with **little-endian** byte order (matching ARM Cortex-M0+). 5. All enum types (`rx_protocol_t`, `esc_protocol_t`, etc.) are mirrored as JS constants with the same integer values. **Verification:** The computed layout was verified against the actual C++ compiled struct: ```sh # Host: compiled include/shared.h with clang++ -std=c++11 (typed uint8_t enums) # and printed offsetof() for 29 key fields. # Docker: ran test/verify_layout.js docker run --rm -v $(pwd):/app -w /app node:20-alpine node test/verify_layout.js # Result: # CONFIG_SIZE=224 expected=224 OK # 29 passed, 0 failed (offset checks) # Roundtrip: 99 passed, 0 failed (encode→decode for all 99 fields) ``` The 29 checked offsets span the entire struct — from `version` at offset 0 through `spare20` at offset 220 — including all alignment-sensitive transitions (e.g. `bool` → `uint32_t` at `enable_gps` → `gps_baudrate`, `uint8_t` → `float` at `ina3221_filter` → `alpha_rpm`). The roundtrip test encodes the default config, decodes it, and compares every field. ### `js/serial.js` — Web Serial API Implements the same USB CDC binary protocol as the Qt GUI (`board/project/usb.c`): | Bytes sent | Direction | Meaning | |---|---|---| | `0x30 0x30` + 224 bytes | App → Board | Write config to flash | | `0x30 0x31` | App → Board | Request current config | | `0x30 0x32` + 224 bytes | Board → App | Config response | | `0x30 0x33` | App → Board | Enable debug text stream | | `0x30 0x34` | App → Board | Disable debug | | `0x30 0x35` | App → Board | Force write default config | Connection parameters: **115200 baud, 8N1** (matching `QSerialPort` in mainwindow.cpp). The reader accumulates bytes and searches for the `[0x30, 0x32]` header pattern before extracting exactly `CONFIG_SIZE` bytes. In debug mode, raw bytes are decoded as UTF-8 text and forwarded to the debug panel. ### `js/ui.js` — Config ↔ UI mapping Reimplements the two core functions from `mainwindow.cpp`: - **`setUiFromConfig(cfg)`** — maps every `config_t` field to the corresponding DOM element. Ported from `MainWindow::setUiFromConfig()` (lines ~452–680 of mainwindow.cpp). - **`getConfigFromUi()`** — reads all DOM widgets back into a config object. Ported from `MainWindow::getConfigFromUi()` (lines ~682–901). All conversion formulas are preserved: | Conversion | Formula | |---|---| | Averaging alpha ↔ UI elements | `alpha = 2 / (n + 1)`, `n = round(2/alpha - 1)` | | Hall sensor sensitivity ↔ multiplier | `multiplier = 1000 / sensitivity_mV_A` | | Shunt resistor | multiplier passed through directly | | Airspeed VCC | config stores centivolts (`V × 100`), UI shows volts | | HW V4/V5 ESC voltage/current multipliers | config stores raw float, UI shows `× 100000` | | ESC combo → `esc_protocol_t` | `enum_value = combo_index + 1` (ESC_NONE = 0 = unchecked) | | Barometer combo → `i2c_module_t` | `enum_value = combo_index + 1` (I2C_NONE = 0 = unchecked) | | BMP280 filter | `config_value = combo_index + 1` | | GPIO bitmask | bits 0–5 → GPIO 17–22 | **Protocol-dependent visibility** (`updateVisibility()`) replicates `MainWindow::on_cbReceiver_currentTextChanged()`: | Feature | Visible for protocols | |---|---| | Refresh rates | SmartPort, Frsky D, FPort, FBUS | | Fuel meter | SmartPort, JetiEx, JetiEx Sensor, XBUS, HOTT, FPort, FBUS | | Fuel pressure | SRXL, SRXL2, JetiEx, JetiEx Sensor, XBUS, HOTT | | GPIO switches | SmartPort, FPort, FBUS | | Airspeed | Hidden for Sanwa, GHST | | GPS, Current, Vario | Hidden for Sanwa | | LiPo (INA3221) | CRSF, SmartPort, FPort, FBUS, HOTT, SRXL, SRXL2, JetiEx, JetiEx Sensor | | Averaging subsets | Sanwa: RPM+Volt+Temp only. GHST: Volt+Curr+Vario only | | Serial Monitor | hides all sensor sections, shows baudrate/parity/stop bits/GPIO | ### `js/circuit.js` — Circuit diagram Replicates `MainWindow::generateCircuit()` on an HTML5 Canvas: 1. Preloads all 18 PNG overlays from `res/`. 2. Draws the base board image (`rp2040_zero.png`) always. 3. Composites sensor overlays based on current UI state: - Each enabled sensor (voltage, current, NTC, airspeed, GPS) draws its overlay. - ESC overlays: `esc_rp2040_zero.png` for serial ESCs, `pwm_rp2040_zero.png` for PWM, `castle_rp2040_zero.png` for Castle Link, `smart_esc.png` for Smart ESC. - I2C bus: `vario_rp2040_zero.png` shared by vario, gyro, and lipo (same physical bus). - Receiver: `receiver_frsky_d_rp2040_zero.png` for Frsky D / CRSF / JetiEx Sensor (single-wire), `receiver_xbus_rp2040_zero.png` for XBUS (I2C), `receiver_hitec_rp2040_zero.png` for Hitec, `receiver_serial_rp2040_zero.png` for all others (UART). - XBUS + clock stretch → additional `clock_stretch_xbus_rp2040_zero.png`. 4. Zoom in/out buttons scale the canvas (0.5× – 3×). The 19 PNG images in `res/` are copied verbatim from `msrc_gui/res/`. ### `js/app.js` — Application entry point Orchestrates everything: - **Connect/Disconnect** via Web Serial API (port selection dialog is browser-native). - On connect: disables debug, waits 2 seconds (matching Qt GUI timing), then requests config. - **Update button**: calls `getConfigFromUi()` → `configEncode()` → `serial.writeConfig()`. - **Tab switching** between Configuration, Circuit, Debug. - **Debug panel**: toggles debug mode, displays streaming text, auto-scroll toggle. - **File Open/Save**: reads/writes raw `.cfg` binary files (just the `config_t` bytes — same format as Qt GUI's save/load). - **Default Config**: sends `0x35` command after confirmation dialog. - **PWA Service Worker** registration. ### `index.html` — UI structure Every widget from the Qt GUI (`mainwindow.ui` / `mainwindow.cpp`) has a corresponding DOM element: - **Receiver protocol** dropdown with all 19 protocols, each `