diff --git a/refloat/.github/actions/build/action.yml b/refloat/.github/actions/build/action.yml index 3f843353..b2392038 100644 --- a/refloat/.github/actions/build/action.yml +++ b/refloat/.github/actions/build/action.yml @@ -15,8 +15,8 @@ runs: authToken: '${{ inputs.cache-auth-token }}' - uses: rrbutani/use-nix-shell-action@v1 with: - flakes: nixpkgs/nixos-24.11#gcc-arm-embedded-7, nixpkgs#gnumake, github:lukash/vesc_tool-flake/release_6_05 + flakes: nixpkgs/nixos-24.11#gcc-arm-embedded-7, nixpkgs#gnumake, github:lukash/vesc_tool-flake/release_6_06 - name: Build shell: bash - run: make -j OLDVT=1 + run: make -j diff --git a/refloat/.github/workflows/ci.yml b/refloat/.github/workflows/ci.yml index 76d9be5c..fcdbf73d 100644 --- a/refloat/.github/workflows/ci.yml +++ b/refloat/.github/workflows/ci.yml @@ -15,11 +15,11 @@ jobs: - uses: cachix/install-nix-action@v25 - uses: rrbutani/use-nix-shell-action@v1 with: - flakes: nixpkgs#clang-tools_18, nixpkgs#lefthook + flakes: nixpkgs#llvmPackages_18.clang-tools, nixpkgs#pre-commit - name: Run clang-format id: clang-format - run: lefthook run clang-format-check + run: pre-commit run --all-files --show-diff-on-failure --color always - name: Build Package if: success() || (failure() && steps.clang-format.conclusion == 'failure') diff --git a/refloat/.github/workflows/create_release.yml b/refloat/.github/workflows/create_release.yml index 47dbf2ed..c6fb4c71 100644 --- a/refloat/.github/workflows/create_release.yml +++ b/refloat/.github/workflows/create_release.yml @@ -55,7 +55,9 @@ jobs: run: cat src/conf/confparser.h | sed -n 's/^#define .\+_SIGNATURE\W\+\([0-9]*\)/\1/p' > config_signature.txt - name: Rename Package File - run: mv refloat.vescpkg refloat-${{env.VERSION}}.vescpkg + run: | + mv refloat.vescpkg refloat-${{env.VERSION}}.vescpkg + mv src/package_lib.elf refloat-${{env.VERSION}}.elf - name: Install Changelog Generation Dependencies run: sudo apt-get install python3-git @@ -74,7 +76,7 @@ jobs: tag: ${{github.ref}} name: Refloat ${{env.VERSION}} draft: true - artifacts: "refloat-${{env.VERSION}}.vescpkg,config_signature.txt" + artifacts: "refloat-${{env.VERSION}}.vescpkg,refloat-${{env.VERSION}}.elf,config_signature.txt" bodyFile: "release_notes.md" prerelease: ${{env.PRERELEASE}} allowUpdates: true diff --git a/refloat/.lefthook.yml b/refloat/.lefthook.yml deleted file mode 100644 index 04ccf6ec..00000000 --- a/refloat/.lefthook.yml +++ /dev/null @@ -1,27 +0,0 @@ -pre-commit: - commands: - clang-format-check: - glob: "*.{c,h}" - exclude: '^vesc_pkg_lib/' - run: clang-format -i --dry-run --Werror {staged_files} - skip: false - clang-format-fix: - glob: "*.{c,h}" - exclude: '^vesc_pkg_lib/' - stage_fixed: true - run: clang-format -i {staged_files} - skip: true - -clang-format-check: - commands: - check: - glob: "*.{c,h}" - exclude: '^vesc_pkg_lib/' - run: clang-format -i --dry-run --Werror {all_files} - -clang-format-fix: - commands: - fix: - glob: "*.{c,h}" - exclude: '^vesc_pkg_lib/' - run: clang-format -i {all_files} diff --git a/refloat/.pre-commit-config.yaml b/refloat/.pre-commit-config.yaml new file mode 100644 index 00000000..80455480 --- /dev/null +++ b/refloat/.pre-commit-config.yaml @@ -0,0 +1,9 @@ +repos: + - repo: local + hooks: + - id: clang-format + name: clang-format + entry: clang-format -i + language: system + types: [c] +exclude: ^vesc_pkg_lib/ diff --git a/refloat/CONTRIBUTING.md b/refloat/CONTRIBUTING.md index eacd27bb..b199e97f 100644 --- a/refloat/CONTRIBUTING.md +++ b/refloat/CONTRIBUTING.md @@ -73,17 +73,12 @@ clang-format -i src/*.{h,c} But, it's better to use git hooks. ## Git Hooks -Refloat uses `lefthook` for managing git hooks. Again, you need to install this dependency in your development environment. Then run this to install the commit hook in the repo: +Refloat uses `pre-commit` for managing git hooks. Again, you need to install this dependency in your development environment. Then run this to install the commit hook in the repo: ```sh -lefthook install +pre-commit install ``` -After this, `lefthook` will automatically run clang-format on every commit you make and check your formatting. - -To auto-fix all formatting of all files in your working tree, run: -```sh -lefthook run clang-format-fix -``` +After this, `pre-commit` will automatically run clang-format on every commit you make, check and automatically fix your formatting. ## Naming Conventions - Type names are in `PascalCase`. diff --git a/refloat/Makefile b/refloat/Makefile index 9e2a7594..b069effd 100644 --- a/refloat/Makefile +++ b/refloat/Makefile @@ -7,9 +7,9 @@ OLDVT ?= 0 all: refloat.vescpkg -refloat.vescpkg: src package.lisp package_README-gen.md ui.qml +refloat.vescpkg: src lisp/package.lisp package_README-gen.md ui.qml ifeq ($(OLDVT), 1) - $(VESC_TOOL) --buildPkg "refloat.vescpkg:package.lisp:ui.qml:0:package_README-gen.md:Refloat" + $(VESC_TOOL) --buildPkg "refloat.vescpkg:lisp/package.lisp:ui.qml:0:package_README-gen.md:Refloat" else $(VESC_TOOL) --buildPkgFromDesc pkgdesc.qml endif diff --git a/refloat/doc/commands/ALERTS_CONTROL.md b/refloat/doc/commands/ALERTS_CONTROL.md new file mode 100644 index 00000000..22cfa8b3 --- /dev/null +++ b/refloat/doc/commands/ALERTS_CONTROL.md @@ -0,0 +1,11 @@ +# Command: ALERTS_CONTROL + +**ID**: 36 + +**Status**: **unstable** + +The alerts control command. + +| Offset | Size | Name | Mandatory | Description | +|--------|------|-----------|-----------|---------------| +| 0 | 1 | `command` | Yes | `1`: Clear Fatal Error
| diff --git a/refloat/doc/commands/ALERTS_LIST.md b/refloat/doc/commands/ALERTS_LIST.md new file mode 100644 index 00000000..0389aa72 --- /dev/null +++ b/refloat/doc/commands/ALERTS_LIST.md @@ -0,0 +1,34 @@ +# Command: ALERTS_LIST + +**ID**: 35 + +**Status**: **unstable** + +Returns a list of active alerts and a log of alert history. The package maintains a number of last occurred alerts in a circular buffer. This command can return all of them, or, optionally provide a starting timestamp in ticks in the `since` argument. In such case only alerts which occurred after the timestamp are returned (note: the caller needs to take into account the possibility of the ticks overflowing, which occurs once every ~5 days). + +The response is limited to 511B in size, in case the alerts wouldn't fit in the message, the alert list will be trucncated (hence it's recommended to use the `since` argument to deal with this eventuality). + +## Request + +| Offset | Size | Name | Mandatory | Description | +|--------|------|---------|-----------|---------------| +| 0 | 4 | `since` | No | Timestamp in ticks from which to list alerts in the log as `uint32`. | + +## Response + +| Offset | Size | Name | Description | +|--------|------|-----------------------|---------------| +| 0 | 4 | `active_alert_mask_1` | Bits 0..31 of the `active_alert_mask`, indicating which [alert_id](alert_id.md)s are active. | +| 4 | 8 | `active_alert_mask_2` | Bits 32..63 of the `active_alert_mask`, indicating which [alert_id](alert_id.md)s are active. | +| 8 | 1 | `firmware_fault_code` | In case `ALERT_FW_FAULT` is active, the VESC firmware fault code, otherwise 0. | +| 9 | ? | `firmware_fault_name` | Iff `firmware_fault_code` is non-zero, a [string](string.md) containing the firmware fault name. +| ? | ? | `alert_log` | A sequence of `alert_record`s containing the alert history stored in the package. | + +**`alert_record`**: +| Offset | Size | Name | Description | +|--------|------|----------|---------------| +| 0 | 4 | `time` | Timestamp of the alert in ticks, as `uint32`. | +| 4 | 1 | `id` | The ID of the alert, see [alert_id](alert_id.md). | +| 5 | 1 | `active` | `1` if the event represents the alert becoming active, `0` if the event marks the end of the alert. | +| 6 | 1 | `code` | If the Alert ID is `ALERT_FW_FAULT`, the code of the fault from the firmware, otherwise 0. | +| 7 | ? | `firmware_fault_name` | Iff Alert ID is `ALERT_FW_FAULT` and `firmware_fault_code` is non-zero, a [string](string.md) containing the firmware fault name. diff --git a/refloat/doc/commands/DATA_RECORD.md b/refloat/doc/commands/DATA_RECORD.md index 89d9417a..70f70c18 100644 --- a/refloat/doc/commands/DATA_RECORD.md +++ b/refloat/doc/commands/DATA_RECORD.md @@ -61,19 +61,13 @@ The client is responsible to request all the chunks of data by repeatedly callin **ID**: 42 -Response with the metadata for the recorded data. Sends the total number of samples and then a list of string IDs for the values in each sample, same as in [REALTIME_DATA](REALTIME_DATA.md). +Response with the metadata for the recorded data. Sends the total number of samples and then a list of [string](string.md) IDs for the values in each sample, same as in [REALTIME_DATA](REALTIME_DATA.md). | Offset | Size | Name | Description | |--------|------|--------------------------|---------------| | 0 | 4 | `size` | Size of the buffer in number of samples. The client is expected to fetch up to this number of samples. | | 4 | 1 | `recorded_data_id_count` | Number of the recorded items per sample (sample size). | -| 5 | ? | `recorded_data_ids` | A `string` sequence repeated `recorded_data_id_count` times. | - -**`string`**: -| Offset | Size | Name | Description | -|--------|------|----------|---------------| -| 0 | 1 | `length` | Length of the string. | -| 1 | ? | `string` | `length` number of characters of the string (not null-terminated). | +| 5 | ? | `recorded_data_ids` | A [string](string.md) sequence repeated `recorded_data_id_count` times. | ## DATA_RECORD_DATA (Response) diff --git a/refloat/doc/commands/LIGHTS_CONTROL.md b/refloat/doc/commands/LIGHTS_CONTROL.md new file mode 100644 index 00000000..d2020630 --- /dev/null +++ b/refloat/doc/commands/LIGHTS_CONTROL.md @@ -0,0 +1,26 @@ +# Command: LIGHTS_CONTROL + +**ID**: 20 + +Controls the lights and returns current lighting state values. + +## Request + +| Offset | Size | Name | Mandatory | Description | +|--------|------|-----------|-----------|---------------| +| 0 | 4 | `mask` | No | Mask determining which values are being set in the command. Bits 0..7 correspond to the `flags` bits. Bits 8..31 are unused. | +| 4 | 1 | `flags` | No | Values of lighting flags to set. | + +## Response + +| Offset | Size | Name | Description | +|--------|------|---------|---------------| +| 0 | 1 | `flags` | Current values of lighting flags. | + +## Common Structures + +#### flags + +| 7-2 | 1 | 0 | +|-----|-----------------|-----------| +| 0 | `headlights_on` | `leds_on` | diff --git a/refloat/doc/commands/REALTIME_DATA.md b/refloat/doc/commands/REALTIME_DATA.md index fc08f664..bfb467f5 100644 --- a/refloat/doc/commands/REALTIME_DATA.md +++ b/refloat/doc/commands/REALTIME_DATA.md @@ -42,9 +42,16 @@ In case the following bits are set in the `mask`, the listed data follow in the | 0 | 2 | `charging_current` | The charging current encoded as [float16](float16.md). | | 2 | 2 | `charging_voltage` | The charging voltage encoded as [float16](float16.md). | +**`0x4`: Alerts** +| Offset | Size | Name | Description | +|--------|------|-----------------------|---------------| +| 0 | 4 | `active_alert_mask_1` | Bits 0..31 of the `active_alert_mask`, indicating which [alert_id](alert_id.md)s are active. | +| 4 | 8 | `active_alert_mask_2` | Bits 32..63 of the `active_alert_mask`, indicating which [alert_id](alert_id.md)s are active. | +| 8 | 1 | `firmware_fault_code` | In case `ALERT_FW_FAULT` is active, the VESC firmware fault code, otherwise 0. | + #### extra_flags -| 7-5 | 2 | 1 | 0 | +| 7-3 | 2 | 1 | 0 | |-----|------------------------|-------------------------|-------------------------| | 0 | `data_record_autostop` | `data_record_autostart` | `data_record_recording` | diff --git a/refloat/doc/commands/REALTIME_DATA_IDS.md b/refloat/doc/commands/REALTIME_DATA_IDS.md index 81074975..7007d5d8 100644 --- a/refloat/doc/commands/REALTIME_DATA_IDS.md +++ b/refloat/doc/commands/REALTIME_DATA_IDS.md @@ -17,12 +17,6 @@ The actual list of IDs that are sent is in [rt_data.h](/src/rt_data.h). See [Rea | Offset | Size | Name | Description | |--------|------|----------------------------------|---------------| | 0 | 1 | `realtime_data_id_count` | Number of IDs of the `REALTIME_DATA` items which are always sent. | -| 1 | ? | `realtime_data_ids` | A `string` sequence repeated `realtime_data_id_count` times. | +| 1 | ? | `realtime_data_ids` | A [string](string.md) sequence repeated `realtime_data_id_count` times. | | ? | 1 | `realtime_runtime_data_id_count` | Number of IDs of the `REALTIME_DATA` items which are sent only when the package is running. | -| ? | ? | `realtime_runtime_data_ids` | A `string` sequence repeated `realtime_runtime_data_id_count` times. | - -**`string`**: -| Offset | Size | Name | Description | -|--------|------|----------|---------------| -| 0 | 1 | `length` | Length of the string. | -| 1 | ? | `string` | `length` number of characters of the string (not null-terminated). | +| ? | ? | `realtime_runtime_data_ids` | A [string](string.md) sequence repeated `realtime_runtime_data_id_count` times. | diff --git a/refloat/doc/commands/alert_id.md b/refloat/doc/commands/alert_id.md new file mode 100644 index 00000000..3888909f --- /dev/null +++ b/refloat/doc/commands/alert_id.md @@ -0,0 +1,4 @@ +# `alert_id` + +- `1: ALERT_FW_FAULT` + diff --git a/refloat/doc/commands/index.md b/refloat/doc/commands/index.md index f1e0ac9a..af4d445d 100644 --- a/refloat/doc/commands/index.md +++ b/refloat/doc/commands/index.md @@ -24,6 +24,9 @@ All commands on the VESC bus are checksummed (so they shouldn't get mangled). In In the commands' documentation, the first two bytes with `package_interface_id` and `command_id` are omitted, so while their offsets start at 0, in the full message their data are always preceded by them. - [INFO](INFO.md) +- [LIGHTS_CONTROL](LIGHTS_CONTROL.md) - [REALTIME_DATA](REALTIME_DATA.md) - [REALTIME_DATA_IDS](REALTIME_DATA_IDS.md) - [DATA_RECORD](DATA_RECORD.md) +- [ALERTS_LIST](ALERTS_LIST.md) +- [ALERTS_CONTROL](ALERTS_CONTROL.md) diff --git a/refloat/doc/commands/string.md b/refloat/doc/commands/string.md new file mode 100644 index 00000000..592565ff --- /dev/null +++ b/refloat/doc/commands/string.md @@ -0,0 +1,7 @@ +# `string` + +| Offset | Size | Name | Description | +|--------|------|----------|---------------| +| 0 | 1 | `length` | Length of the string. | +| 1 | ? | `string` | `length` number of characters of the string (not null-terminated). | + diff --git a/refloat/lisp/bms.lisp b/refloat/lisp/bms.lisp new file mode 100644 index 00000000..e1b58e0d --- /dev/null +++ b/refloat/lisp/bms.lisp @@ -0,0 +1,37 @@ +(defun bms-loop () { + (var v-cell-support (eq (first (trap (get-bms-val 'bms-v-cell-min))) 'exit-ok)) + (var v-min 0) + (var v-max 0) + (var temp-min 0) + (var temp-max 0) + (var temp-fet -281) + (loopwhile t { + (if (and (>= (get-bms-val 'bms-can-id) 0) (ext-bms)) { + (var msg-age (get-bms-val 'bms-msg-age)) + (setq temp-max (get-bms-val 'bms-temp-cell-max)) + (setq temp-min temp-max) + + (if v-cell-support { + (if (= (get-bms-val 'bms-data-version) 1) { + (setq temp-min (get-bms-val 'bms-temps-adc 1)) + (setq temp-fet (get-bms-val 'bms-temps-adc 3)) + }) + (setq v-min (get-bms-val 'bms-v-cell-min)) + (setq v-max (get-bms-val 'bms-v-cell-max)) + } { + (var num-cells (get-bms-val 'bms-cell-num)) + (if (> num-cells 0) { + (setq v-min (get-bms-val 'bms-v-cell 0)) + (setq v-max v-min) + (looprange i 1 num-cells { + (var cell-v (get-bms-val 'bms-v-cell i)) + (if (< cell-v v-min) (setq v-min cell-v)) + (if (> cell-v v-max) (setq v-max cell-v)) + }) + }) + }) + (ext-bms v-min v-max temp-min temp-max temp-fet msg-age) + }) + (sleep 0.2) + }) +}) diff --git a/refloat/lisp/package.lisp b/refloat/lisp/package.lisp new file mode 100644 index 00000000..1ed2c383 --- /dev/null +++ b/refloat/lisp/package.lisp @@ -0,0 +1,17 @@ +(import "src/package_lib.bin" 'package-lib) +(load-native-lib package-lib) + +(define fw-ver (sysinfo 'fw-ver)) +(apply ext-set-fw-version fw-ver) + +; Start the BMS polling loop in a thread if enabled +(if (ext-bms) + (if (or (>= (first fw-ver) 7) (and (= (first fw-ver) 6) (>= (second fw-ver) 5))) + (progn + (import "bms.lisp" 'bms) + (read-eval-program bms) + (spawn "Refloat BMS" 50 bms-loop) + ) + (print "[refloat] BMS Integration: Unsupported firmware version, 6.05+ required.") + ) +) diff --git a/refloat/package.lisp b/refloat/package.lisp deleted file mode 100644 index 40f6df3c..00000000 --- a/refloat/package.lisp +++ /dev/null @@ -1,27 +0,0 @@ -(import "src/package_lib.bin" 'package-lib) - -(load-native-lib package-lib) - -; Switch Balance App to UART App -(if (= (conf-get 'app-to-use) 9) (conf-set 'app-to-use 3)) - -; Set firmware version: -(apply ext-set-fw-version (sysinfo 'fw-ver)) - -; Set to 1 to monitor debug variables -(define debug 1) - -(if (= debug 1) - (loopwhile t - (progn - (define pid_value (ext-dbg 1)) - (define proportional (ext-dbg 2)) - (define integral (ext-dbg 3)) - (define rate_p (ext-dbg 4)) - (define setpoint (ext-dbg 5)) - (define atr_offset (ext-dbg 6)) - (define erpm (ext-dbg 7)) - (define current (ext-dbg 8)) - (define atr_filtered_current (ext-dbg 9)) - (sleep 0.1) -))) diff --git a/refloat/package_README.md b/refloat/package_README.md index 8abd3931..c42e70cd 100644 --- a/refloat/package_README.md +++ b/refloat/package_README.md @@ -2,37 +2,30 @@ A full-featured self-balancing skateboard package. -## New in 1.1 -- Haptic Feedback: Audible and vibrating feedback generated by the motor -- Parking Brake: A strong stand-still brake on 6.05 firmware -- Automatic backup and per-board backup in Settings -> Setup -- Improved setpoint smoothing for ATR and Torque Tilt -- Configurable order of LED strips and color order per strip -- Improved tune handling in AppUI -- Realtime Data Plotting and Recording: For developers and power users to analyze board behavior -- Many minor features and fixes - -For more details, read the [1.1 release post](https://pev.dev/t/refloat-version-1-1/2480). +## New in 1.2 +- BMS alerting support (Pushback and Haptic) +- Speed-based alerting (Pushback and Haptic) +- Automatic config migration when updating the package +- Support for per-cell High/Low Voltage Thresholds (no more needing to adjust them in Specs after a fresh install, 6.05+ only) +- Groundworks for a new alerting system, for now only providing firmware fault reporting +- New Rainbow Fade, Rainbow Cycle and Rainbow Roll LED effects + +For more details, read the [1.2 release post](https://pev.dev/t/refloat-version-1-2/2795). ## Installation ### Upgrading -Back up your package config (either by **Backup Configs** on the Start page, or by saving the XML on **Refloat Cfg**), install the package and restore your config (**Restore Configs** or load the XML). +Back up your package config (either by **Backup Configs** on the Start page, or by saving the XML on **Refloat Cfg**). + +If upgrading from Refloat 1.1 to 1.2, an automatic config restore dialog should pop up. Confirm it to restore. This is the preferred way to update the config from now on, as it will also migrate any changed options to the new version. Other methods of restoring the config will not do that. -If you use Refloat for LED control, you'll need to re-configure a few options, these won't restore from your backup (make note of your current settings before upgrading): -- _Specs -> LED Mode_: set Internal if your _LED Type_ was **RGB** or **RGBW** -- _Specs -> Status / Front / Rear LED Color Order_ - - Pick an option with a **W** if your _LED Type_ was **RGBW**; Pick the RGB order that you had in your _LED Color Order_ - - Or, just keep trying them until the colors look correct (remember a restart is needed) - - You need to set this for all of the three strips that you use +In case the automatic config restore fails, restore it the traditional way and please report the issue, so that it can be fixed. ### Fresh Installation If doing a fresh board installation, you need to do the **motor** and **IMU** calibration and configuration. If you install the package before that, you need to disable the package before running the **motor** _calibration_ and re-enable it afterwards. For a detailed guide, read the [Initial Board Setup guide on pev.dev](https://pev.dev/t/initial-board-setup-in-vesc-tool/2190). -To configure the package, you only need to set the **Low and High Tiltback voltages** on the **Specs** tab of **Refloat Cfg**. These need to be set according to your battery specs. - -The options in the other tabs are set to good starting values that should provide you with a well behaving, rideable board. +On 6.05+ firmware, the package should ride well without the need to configure anything in Refloat Cfg. On 6.02, the **Low and High Tiltback voltages** on the **Specs** tab of **Refloat Cfg** still need to be set according to your battery specs. ## Disclaimer **Use at your own risk!** Electric vehicles are inherently dangerous, authors of this package shall not be liable for any damage or harm caused by errors in the software. Not endorsed by the VESC project. diff --git a/refloat/pkgdesc.qml b/refloat/pkgdesc.qml index f5812fc0..647cc20e 100644 --- a/refloat/pkgdesc.qml +++ b/refloat/pkgdesc.qml @@ -3,7 +3,7 @@ import QtQuick 2.15 Item { property string pkgName: "Refloat" property string pkgDescriptionMd: "package_README-gen.md" - property string pkgLisp: "package.lisp" + property string pkgLisp: "lisp/package.lisp" property string pkgQml: "ui.qml" property bool pkgQmlIsFullscreen: false property string pkgOutput: "refloat.vescpkg" diff --git a/refloat/src/Makefile b/refloat/src/Makefile index 4501f1fd..2c994b2d 100644 --- a/refloat/src/Makefile +++ b/refloat/src/Makefile @@ -8,7 +8,7 @@ TARGET = package_lib all: $(TARGET) -REFLOAT_SOURCES = $(wildcard *.c) +REFLOAT_SOURCES = $(wildcard *.c) $(wildcard lib/*.c) CONF_GEN_HEADERS = conf/conf_default.h conf/confparser.h conf/confxml.h CONF_GEN_SOURCES = conf/confparser.c conf/confxml.c CONF_GEN_FILES = $(CONF_GEN_HEADERS) $(CONF_GEN_SOURCES) @@ -18,7 +18,6 @@ DEPS = $(SOURCES:.c=.d) ADD_TO_CLEAN = $(CONF_GEN_FILES) $(DEPS) conf/conf_general.h VESC_C_LIB_PATH = ../vesc_pkg_lib/ -USE_STLIB = yes include $(VESC_C_LIB_PATH)rules.mk CFLAGS += -MMD -flto diff --git a/refloat/src/alert_tracker.c b/refloat/src/alert_tracker.c new file mode 100644 index 00000000..a7001a1a --- /dev/null +++ b/refloat/src/alert_tracker.c @@ -0,0 +1,99 @@ +// Copyright 2025 Lukas Hrazky +// +// This file is part of the Refloat VESC package. +// +// Refloat VESC package 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. +// +// Refloat VESC package 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 . + +#include "alert_tracker.h" + +static uint32_t alert_id_to_mask(AlertId alert_id) { + return 1 << (alert_id - 1); +} + +static const AlertProperties alert_properties[] = { + [ALERT_FW_FAULT] = {.type = ATYPE_FATAL}, +}; + +const AlertProperties *alert_tracker_properties(AlertId alert_id) { + return &alert_properties[alert_id]; +} + +void alert_tracker_init(AlertTracker *at) { + at->persistent_fatal_error = true; + + at->active_alert_mask = 0; + at->new_active_alert_mask = 0; + at->fw_fault_code = FAULT_CODE_NONE; + at->fatal_error = false; + + circular_buffer_init( + &at->alert_buffer, sizeof(AlertRecord), ALERT_TRACKER_SIZE, &at->alert_buffer_array + ); +} + +void alert_tracker_configure(AlertTracker *at, const RefloatConfig *config) { + at->persistent_fatal_error = config->persistent_fatal_error; +} + +void alert_tracker_add(AlertTracker *at, const Time *time, uint8_t id, uint8_t code) { + uint32_t mask = alert_id_to_mask(ALERT_FW_FAULT); + bool already_active = at->active_alert_mask & mask; + if (id == ALERT_FW_FAULT) { + already_active = already_active && code == at->fw_fault_code; + at->fw_fault_code = code; + } + + if (!already_active) { + AlertRecord alert = {.time = time->now, .id = id, .code = code, .active = true}; + circular_buffer_push(&at->alert_buffer, &alert); + } + + if (alert_tracker_properties(id)->type == ATYPE_FATAL) { + at->fatal_error = true; + } + + at->new_active_alert_mask |= mask; +} + +void alert_tracker_finalize(AlertTracker *at, const Time *time) { + // Check for alerts that ended and create a record of it + bool clear_fatal = !at->persistent_fatal_error; + for (uint8_t id = 1; id <= ALERT_LAST; ++id) { + uint32_t mask = alert_id_to_mask(id); + if (at->active_alert_mask & mask && !(at->new_active_alert_mask & mask)) { + AlertRecord alert = {.time = time->now, .id = id, .code = 0, .active = false}; + circular_buffer_push(&at->alert_buffer, &alert); + if (id == ALERT_FW_FAULT) { + at->fw_fault_code = 0; + } + } + + if (at->active_alert_mask & mask && alert_tracker_properties(id)->type == ATYPE_FATAL) { + clear_fatal = false; + } + } + + at->active_alert_mask = at->new_active_alert_mask; + at->new_active_alert_mask = 0; + + at->fatal_error &= !clear_fatal; +} + +bool alert_tracker_is_alert_active(AlertTracker *at, AlertId alert) { + return at->active_alert_mask & alert_id_to_mask(alert); +} + +void alert_tracker_clear_fatal(AlertTracker *at) { + at->fatal_error = false; +} diff --git a/refloat/src/alert_tracker.h b/refloat/src/alert_tracker.h new file mode 100644 index 00000000..856bc009 --- /dev/null +++ b/refloat/src/alert_tracker.h @@ -0,0 +1,73 @@ +// Copyright 2025 Lukas Hrazky +// +// This file is part of the Refloat VESC package. +// +// Refloat VESC package 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. +// +// Refloat VESC package 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 . + +#pragma once + +#include "conf/datatypes.h" +#include "lib/circular_buffer.h" +#include "time.h" + +#define ALERT_TRACKER_SIZE 20 + +typedef enum { + ATYPE_FATAL = 1, + ATYPE_ERROR = 2, + ATYPE_WARNING = 3, +} AlertType; + +typedef enum { + ALERT_NONE = 0, + ALERT_FW_FAULT = 1, + ALERT_LAST = ALERT_FW_FAULT +} AlertId; + +typedef struct { + uint8_t type; +} AlertProperties; + +typedef struct { + time_t time; + uint8_t id; + bool active; + uint8_t code; +} AlertRecord; + +typedef struct { + bool persistent_fatal_error; + + uint32_t active_alert_mask; + uint32_t new_active_alert_mask; + mc_fault_code fw_fault_code; + bool fatal_error; + + uint8_t alert_buffer_array[ALERT_TRACKER_SIZE * sizeof(AlertRecord)]; + CircularBuffer alert_buffer; +} AlertTracker; + +const AlertProperties *alert_tracker_properties(AlertId alert_id); + +void alert_tracker_init(AlertTracker *at); + +void alert_tracker_configure(AlertTracker *at, const RefloatConfig *config); + +void alert_tracker_add(AlertTracker *at, const Time *time, uint8_t id, uint8_t data); + +void alert_tracker_finalize(AlertTracker *at, const Time *time); + +bool alert_tracker_is_alert_active(AlertTracker *at, AlertId alert); + +void alert_tracker_clear_fatal(AlertTracker *at); diff --git a/refloat/src/atr.c b/refloat/src/atr.c index 9eabf929..960c93f3 100644 --- a/refloat/src/atr.c +++ b/refloat/src/atr.c @@ -22,12 +22,20 @@ #include +void atr_init(ATR *atr) { + atr->on_step_size = 0.0f; + atr->off_step_size = 0.0f; + atr->speed_boost_mult = 0.0f; + atr_reset(atr); +} + void atr_reset(ATR *atr) { - atr->accel_diff = 0; - atr->speed_boost = 0; - atr->target = 0; - atr->setpoint = 0; - atr->ramped_step_size = 0; + atr->accel_diff = 0.0f; + atr->speed_boost = 0.0f; + + atr->target = 0.0f; + atr->ramped_step_size = 0.0f; + atr->setpoint = 0.0f; } void atr_configure(ATR *atr, const RefloatConfig *config) { diff --git a/refloat/src/atr.h b/refloat/src/atr.h index 8aaaa712..caff97e5 100644 --- a/refloat/src/atr.h +++ b/refloat/src/atr.h @@ -24,17 +24,18 @@ typedef struct { float on_step_size; float off_step_size; - float ramped_step_size; + float speed_boost_mult; float accel_diff; float speed_boost; float target; + float ramped_step_size; float setpoint; - - float speed_boost_mult; } ATR; +void atr_init(ATR *atr); + void atr_reset(ATR *atr); void atr_configure(ATR *atr, const RefloatConfig *config); diff --git a/refloat/src/bms.c b/refloat/src/bms.c new file mode 100644 index 00000000..acf08e45 --- /dev/null +++ b/refloat/src/bms.c @@ -0,0 +1,86 @@ +// Copyright 2024 Syler Clayton +// Copyright 2025 Lukas Hrazky +// +// This file is part of the Refloat VESC package. +// +// Refloat VESC package 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. +// +// Refloat VESC package 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 . + +#include "bms.h" + +#include + +void bms_init(BMS *bms) { + bms->cell_lv = 0.0f; + bms->cell_hv = 0.0f; + bms->cell_lt = 0; + bms->cell_ht = 0; + bms->bms_ht = 0; + bms->msg_age = 42.0f; + bms->fault_mask = BMSF_NONE; +} + +static inline void set_fault(uint32_t *fault_mask, BMSFaultCode fault_code) { + *fault_mask |= 1u << (fault_code - 1); +} + +void bms_update(BMS *bms, const CfgBMS *cfg, const Time *time) { + if (!cfg->enabled) { + bms->fault_mask = BMSF_NONE; + return; + } + + uint32_t fault_mask = BMSF_NONE; + const float timeout = 5.0f; + + // Before the first BMS update occurs right after startup, msg_age has its + // init value. We need to wait the `timeout` time before issuing errors. + if (bms->msg_age > timeout && time_elapsed(time, start, timeout)) { + set_fault(&fault_mask, BMSF_CONNECTION); + bms->fault_mask = fault_mask; + return; + } + + if (bms->cell_lv < cfg->cell_lv_threshold) { + set_fault(&fault_mask, BMSF_CELL_UNDER_VOLTAGE); + } + + if (bms->cell_hv > cfg->cell_hv_threshold) { + set_fault(&fault_mask, BMSF_CELL_OVER_VOLTAGE); + } + + // Setting high temp threshold to 0 disables both high and low temp checking + if (cfg->cell_ht_threshold > 0) { + if (bms->cell_ht > cfg->cell_ht_threshold) { + set_fault(&fault_mask, BMSF_CELL_OVER_TEMP); + } + + if (bms->cell_lt < cfg->cell_lt_threshold) { + set_fault(&fault_mask, BMSF_CELL_UNDER_TEMP); + } + } + + if (cfg->bms_ht_threshold > 0 && bms->bms_ht > cfg->bms_ht_threshold) { + set_fault(&fault_mask, BMSF_OVER_TEMP); + } + + if (fabsf(bms->cell_lv - bms->cell_hv) > cfg->cell_balance_threshold) { + set_fault(&fault_mask, BMSF_CELL_BALANCE); + } + + bms->fault_mask = fault_mask; +} + +bool bms_is_fault(const BMS *bms, BMSFaultCode fault_code) { + return (bms->fault_mask & (1u << (fault_code - 1))) != 0; +} diff --git a/refloat/src/bms.h b/refloat/src/bms.h new file mode 100644 index 00000000..b3fb98ad --- /dev/null +++ b/refloat/src/bms.h @@ -0,0 +1,53 @@ +// Copyright 2024 Syler Clayton +// Copyright 2025 Lukas Hrazky +// +// This file is part of the Refloat VESC package. +// +// Refloat VESC package 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. +// +// Refloat VESC package 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 . + +#pragma once + +#include "conf/datatypes.h" +#include "time.h" + +#include +#include + +typedef enum { + BMSF_NONE = 0, + BMSF_CONNECTION = 1, + BMSF_OVER_TEMP = 2, + BMSF_CELL_OVER_VOLTAGE = 3, + BMSF_CELL_UNDER_VOLTAGE = 4, + BMSF_CELL_OVER_TEMP = 5, + BMSF_CELL_UNDER_TEMP = 6, + BMSF_CELL_BALANCE = 7 +} BMSFaultCode; + +typedef struct { + float cell_lv; + float cell_hv; + int16_t cell_lt; + int16_t cell_ht; + int16_t bms_ht; + float msg_age; + + uint32_t fault_mask; +} BMS; + +void bms_init(BMS *bms); + +void bms_update(BMS *bms, const CfgBMS *cfg, const Time *time); + +bool bms_is_fault(const BMS *bms, BMSFaultCode fault_code); diff --git a/refloat/src/booster.c b/refloat/src/booster.c index 2c514b2f..6dadad7f 100644 --- a/refloat/src/booster.c +++ b/refloat/src/booster.c @@ -21,8 +21,12 @@ #include +void booster_init(Booster *b) { + booster_reset(b); +} + void booster_reset(Booster *b) { - b->current = 0; + b->current = 0.0f; } void booster_update( diff --git a/refloat/src/booster.h b/refloat/src/booster.h index 3e364ee6..6a5a10f4 100644 --- a/refloat/src/booster.h +++ b/refloat/src/booster.h @@ -24,6 +24,8 @@ typedef struct { float current; } Booster; +void booster_init(Booster *b); + void booster_reset(Booster *b); void booster_update( diff --git a/refloat/src/brake_tilt.c b/refloat/src/brake_tilt.c index d2f90c2c..f47ec96e 100644 --- a/refloat/src/brake_tilt.c +++ b/refloat/src/brake_tilt.c @@ -22,9 +22,15 @@ #include +void brake_tilt_init(BrakeTilt *bt) { + bt->factor = 0.0f; + brake_tilt_reset(bt); +} + void brake_tilt_reset(BrakeTilt *bt) { - bt->target = 0; - bt->setpoint = 0; + bt->ramped_step_size = 0.0f; + bt->target = 0.0f; + bt->setpoint = 0.0f; } void brake_tilt_configure(BrakeTilt *bt, const RefloatConfig *config) { @@ -76,7 +82,8 @@ void brake_tilt_update( braketilt_step_size /= 2; } - rate_limitf(&bt->setpoint, bt->target, braketilt_step_size); + // Smoothen changes in tilt angle by ramping the step size + smooth_rampf(&bt->setpoint, &bt->ramped_step_size, bt->target, braketilt_step_size, 0.05, 1.5); } void brake_tilt_winddown(BrakeTilt *bt) { diff --git a/refloat/src/brake_tilt.h b/refloat/src/brake_tilt.h index c57e5abb..2d5cf1d1 100644 --- a/refloat/src/brake_tilt.h +++ b/refloat/src/brake_tilt.h @@ -25,9 +25,12 @@ typedef struct { float factor; float target; + float ramped_step_size; float setpoint; } BrakeTilt; +void brake_tilt_init(BrakeTilt *bt); + void brake_tilt_reset(BrakeTilt *bt); void brake_tilt_configure(BrakeTilt *bt, const RefloatConfig *config); diff --git a/refloat/src/clangd_verbose b/refloat/src/clangd_verbose deleted file mode 100644 index e69de29b..00000000 diff --git a/refloat/src/conf/buffer.c b/refloat/src/conf/buffer.c index 85a67a77..c4a555de 100644 --- a/refloat/src/conf/buffer.c +++ b/refloat/src/conf/buffer.c @@ -155,6 +155,17 @@ void buffer_append_string(uint8_t *buffer, const char *str, int32_t *index) { *index = idx; } +void buffer_append_string_max(uint8_t *buffer, const char *str, int32_t *index, uint8_t length) { + int32_t idx = *index + 1; + size_t i = 0; + while (i < length && str[i] != '\0') { + buffer[idx++] = str[i++]; + } + + buffer[*index] = i; + *index = idx; +} + void buffer_append_string_fixed(uint8_t *buffer, const char *str, int32_t *index, uint8_t length) { int32_t idx = *index; size_t i = 0; diff --git a/refloat/src/conf/buffer.h b/refloat/src/conf/buffer.h index 8376ae07..5e996d1b 100644 --- a/refloat/src/conf/buffer.h +++ b/refloat/src/conf/buffer.h @@ -31,6 +31,7 @@ void buffer_append_float32(uint8_t *buffer, float number, float scale, int32_t * void buffer_append_float32_auto(uint8_t *buffer, float number, int32_t *index); void buffer_append_float16_auto(uint8_t *buffer, float number, int32_t *index); void buffer_append_string(uint8_t *buffer, const char *str, int32_t *index); +void buffer_append_string_max(uint8_t *buffer, const char *str, int32_t *index, uint8_t length); void buffer_append_string_fixed(uint8_t *buffer, const char *str, int32_t *index, uint8_t length); int16_t buffer_get_int16(const uint8_t *buffer, int32_t *index); diff --git a/refloat/src/conf/datatypes.h b/refloat/src/conf/datatypes.h index 9808a0b0..a03c68a6 100644 --- a/refloat/src/conf/datatypes.h +++ b/refloat/src/conf/datatypes.h @@ -35,15 +35,23 @@ typedef enum { typedef enum { LED_MODE_OFF = 0, - LED_MODE_INTERNAL, - LED_MODE_EXTERNAL, + LED_MODE_INTERNAL = 0x1, + LED_MODE_EXTERNAL = 0x2, + LED_MODE_BOTH = 0x3, } LedMode; typedef enum { LED_PIN_B6 = 0, - LED_PIN_B7 + LED_PIN_B7, + LED_PIN_C9, + LED_PIN_LAST = LED_PIN_C9 } LedPin; +typedef enum { + LED_PIN_CFG_PULLUP_TO_5V = 0, + LED_PIN_CFG_NO_PULLUP +} LedPinConfig; + typedef enum { LED_COLOR_GRB = 0, LED_COLOR_GRBW, @@ -100,6 +108,9 @@ typedef enum { LED_ANIM_STROBE, LED_ANIM_KNIGHT_RIDER, LED_ANIM_FELONY, + LED_ANIM_RAINBOW_CYCLE, + LED_ANIM_RAINBOW_FADE, + LED_ANIM_RAINBOW_ROLL, } LedAnimMode; typedef enum { @@ -154,6 +165,7 @@ typedef struct { typedef struct { LedMode mode; LedPin pin; + LedPinConfig pin_config; CfgLedStrip status; CfgLedStrip front; CfgLedStrip rear; @@ -179,6 +191,16 @@ typedef struct { float current_threshold; } CfgHapticFeedback; +typedef struct { + bool enabled; + float cell_lv_threshold; + float cell_hv_threshold; + float cell_balance_threshold; + int8_t cell_lt_threshold; + int8_t cell_ht_threshold; + int8_t bms_ht_threshold; +} CfgBMS; + typedef struct { bool is_default; } CfgMeta; @@ -204,11 +226,13 @@ typedef struct { uint16_t fault_adc_half_erpm; bool fault_is_dual_switch; bool fault_moving_fault_disabled; + bool enable_quickstop; bool fault_darkride_enabled; bool fault_reversestop_enabled; float tiltback_duty_angle; float tiltback_duty_speed; float tiltback_duty; + uint8_t tiltback_speed; float tiltback_hv_angle; float tiltback_hv_speed; float tiltback_hv; @@ -216,6 +240,7 @@ typedef struct { float tiltback_lv_speed; float tiltback_lv; float tiltback_return_speed; + bool persistent_fatal_error; float tiltback_constant; uint16_t tiltback_constant_erpm; float tiltback_variable; @@ -279,7 +304,7 @@ typedef struct { bool is_footbeep_enabled; CfgHapticFeedback haptic; - + CfgBMS bms; CfgLeds leds; CfgHardware hardware; diff --git a/refloat/src/conf/settings.xml b/refloat/src/conf/settings.xml index da408102..b9def903 100644 --- a/refloat/src/conf/settings.xml +++ b/refloat/src/conf/settings.xml @@ -124,7 +124,7 @@ p, li { white-space: pre-wrap; } <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><br /></p> <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Technically, this value affects how the board behaves when experiencing acceleration. With a higher value, harder acceleration will make the nose softer, and harder deceleration/braking will make the tail softer.</p> <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><br /></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">This parameter is amongst those that have the most impact on the feel of the ride and how the board behaves in ondulated terrain.</p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">This parameter is amongst those that have the most impact on the feel of the ride and how the board behaves in undulated terrain.</p> <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><br /></p> <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-style:italic;">Recommended Values: 1.5 - 2.5 (Caution with higher or lower values!)</span></p></body></html> CFG_DFLT_MAHONY_KP @@ -184,7 +184,7 @@ p, li { white-space: pre-wrap; } <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Angle P (Braking) = 0.5x,</p> <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">then your brakes will use an Angle P of 10, half the strength of your acceleration Angle P.</p></body></html> CFG_DFLT_KP_BRAKE - 1 + 2 1 0 3 @@ -192,7 +192,7 @@ p, li { white-space: pre-wrap; } 0 0.1 1 - 10 + 100 x 7 @@ -209,7 +209,7 @@ p, li { white-space: pre-wrap; } <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Rate P (Braking) = 0.5x,</p> <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">then your brakes will use a Rate P of 0.3, half the strength of your acceleration Rate P.</p></body></html> CFG_DFLT_KP2_BRAKE - 1 + 2 1 0 3 @@ -217,7 +217,7 @@ p, li { white-space: pre-wrap; } 0 0.1 1 - 10 + 100 x 7 @@ -479,6 +479,18 @@ p, li { white-space: pre-wrap; } CFG_DFLT_FAULT_MOVING_FAULT_DISABLED 0 + + Enable Quickstop + 5 + 1 + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'Roboto'; ; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Enable disengaging the board immediately by braking hard and lifting the foot off the sensor as the board stops. Requires the board to be angled back more than 14° and the wheel coming to a complete stop.</p></body></html> + CFG_DFLT_FAULT_ENABLE_QUICKSTOP + 1 + Enable Darkride 5 @@ -510,14 +522,14 @@ p, li { white-space: pre-wrap; } 0 - Angle + Pushback Angle 1 1 <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> <html><head><meta name="qrichtext" content="1" /><style type="text/css"> p, li { white-space: pre-wrap; } </style></head><body style=" font-family:'Roboto'; ; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Desired setpoint angle for Duty Cycle Pushback.</p></body></html> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Setpoint angle for Speed and Duty Cycle Pushback.</p></body></html> CFG_DFLT_TILTBACK_DUTY_ANGLE 1 1 @@ -532,14 +544,14 @@ p, li { white-space: pre-wrap; } 7 - Speed + Pushback Speed 1 1 <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> <html><head><meta name="qrichtext" content="1" /><style type="text/css"> p, li { white-space: pre-wrap; } </style></head><body style=" font-family:'Roboto'; ; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Rate at which nose is tilted back when exceeding Duty Cycle limit (fast pushback can be dangerous!).</p></body></html> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Rate at which nose is tilted up during Speed and Duty Cycle Pushback (fast pushback can be dangerous!).</p></body></html> CFG_DFLT_TILTBACK_DUTY_SPEED 1 1 @@ -563,9 +575,9 @@ p, li { white-space: pre-wrap; } </style></head><body style=" font-family:'Roboto'; ; font-weight:400; font-style:normal;"> <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Duty Cycle threshold to trigger a safety pushback (Pushback raises the nose of the board, informing you to slow down).</p> <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><br /></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-weight:600; font-style:italic;">*Note: Can be disabled by setting to 100%</span></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-style:italic;">Set to 100% to disable.</span></p> <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><br /></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-weight:600; font-style:italic;">*</span><span style=" font-weight:600;">DISCLAIMER</span><span style=" font-weight:600; font-style:italic;">: Max Duty Cycle on a VESC is </span><span style=" font-weight:600;">95%</span><span style=" font-weight:600; font-style:italic;">, </span><span style=" font-weight:600;">NOT 100%! </span><span style=" font-weight:600; font-style:italic;">Reaching 95% Duty Cycle and pushing beyond </span><span style=" font-weight:600;">WILL </span><span style=" font-weight:600; font-style:italic;">result in a nosedive. Use this knowledge to leave yourself the headroom you desire.</span></p></body></html> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-weight:600; font-style:italic;">*</span><span style=" font-weight:600;">DISCLAIMER</span><span style=" font-weight:600; font-style:italic;">: Max Duty Cycle on a VESC is </span><span style=" font-weight:600;">95%</span><span style=" font-weight:600; font-style:italic;">, </span><span style=" font-weight:600;">NOT 100%! </span><span style=" font-weight:600; font-style:italic;">Reaching 95% Duty Cycle and pushing beyond </span><span style=" font-weight:600;">WILL </span><span style=" font-weight:600; font-style:italic;">result in a nosedive.</span></p></body></html> CFG_DFLT_TILTBACK_DUTY 2 1 @@ -587,21 +599,21 @@ p, li { white-space: pre-wrap; } <html><head><meta name="qrichtext" content="1" /><style type="text/css"> p, li { white-space: pre-wrap; } </style></head><body style=" font-family:'Roboto'; ; font-weight:400; font-style:normal;"> -<p align="justify" style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Beep during duty cycle pushback. It is recommended to disable this beep when enabling the surge beep or else it will be impossible to recognize the surge beep.</p> +<p align="justify" style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Beep during duty cycle pushback.</p> <p align="justify" style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><br /></p> <p align="justify" style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-weight:600;">Warning: beeper may not be easily audible at high speeds!</span></p></body></html> CFG_DFLT_IS_DUTYBEEP_ENABLED 0 - Angle + Pushback Angle 1 1 <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> <html><head><meta name="qrichtext" content="1" /><style type="text/css"> p, li { white-space: pre-wrap; } </style></head><body style=" font-family:'Roboto'; ; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Desired setpoint angle for High Voltage Pushback.</p></body></html> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Setpoint angle for High Voltage Pushback.</p></body></html> CFG_DFLT_TILTBACK_HV_ANGLE 1 1 @@ -623,7 +635,7 @@ p, li { white-space: pre-wrap; } <html><head><meta name="qrichtext" content="1" /><style type="text/css"> p, li { white-space: pre-wrap; } </style></head><body style=" font-family:'Roboto'; ; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Rate at which nose is tilted back when exceeding High Voltage limit (fast pushback can be dangerous!).</p></body></html> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Rate at which nose is tilted up during High Voltage Pushback (fast pushback can be dangerous!).</p></body></html> CFG_DFLT_TILTBACK_HV_SPEED 1 1 @@ -647,29 +659,33 @@ p, li { white-space: pre-wrap; } </style></head><body style=" font-family:'Roboto'; ; font-weight:400; font-style:normal;"> <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">High Voltage threshold to trigger a safety pushback (Pushback raises the nose of the vehicle to alert you). High voltage pushback is most likely to be triggered when braking or going downhill on a full battery, sometimes resulting in a tail drag (take caution!).</p> <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><br /></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-style:italic;">Recommended Value: 4.3V * Cell Count (i.e. for a 15s battery, 4.3V * 15 = 64.5V)</span></p></body></html> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Since Refloat 1.2, per-cell value is supported if on firmware version 6.05+.</p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-style:italic;">On firmware 6.05+: Set to a per-cell value (recommended 4.3V, which is now default).</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-style:italic;">On firmware 6.02: Set to total voltage value, e.g. 4.3V * Cell Count (i.e. for a 15s battery, 4.3V * 15 = 64.5V).</span></p></body></html> CFG_DFLT_TILTBACK_HV - 1 + 2 1 0 150 0 0 - 0.5 - 64.5 - 10 + 0.05 + 4.3 + 100 V 7 - Angle + Pushback Angle 1 1 <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> <html><head><meta name="qrichtext" content="1" /><style type="text/css"> p, li { white-space: pre-wrap; } </style></head><body style=" font-family:'Roboto'; ; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Desired setpoint angle for Low Voltage Pushback.</p></body></html> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Setpoint angle for Low Voltage Pushback.</p></body></html> CFG_DFLT_TILTBACK_LV_ANGLE 1 1 @@ -691,7 +707,7 @@ p, li { white-space: pre-wrap; } <html><head><meta name="qrichtext" content="1" /><style type="text/css"> p, li { white-space: pre-wrap; } </style></head><body style=" font-family:'Roboto'; ; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Rate at which nose is tilted back when exceeding Low Voltage threshold (fast pushback can be dangerous!).</p></body></html> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Rate at which nose is tilted up during Low Voltage Pushback (fast pushback can be dangerous!).</p></body></html> CFG_DFLT_TILTBACK_LV_SPEED 1 1 @@ -715,24 +731,50 @@ p, li { white-space: pre-wrap; } </style></head><body style=" font-family:'Roboto'; ; font-weight:400; font-style:normal;"> <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Low Voltage threshold to trigger a safety pushback (Pushback raises the nose of the vehicle informing you to slow down).</p> <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><br /></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-style:italic;">Recommended Value: 3.0V * Cell Count (i.e. for a 15s battery, 3.0V * 15 = 45V)</span></p> -<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-style:italic;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Since Refloat 1.2, per-cell value is supported if on firmware version 6.05+.</p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-style:italic;">On firmware 6.05+: Set to a per-cell value (recommended 3.0V, which is now default).</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-style:italic;">On firmware 6.02: Set to total voltage value, e.g. 3.0V * Cell Count (i.e. for a 15s battery, 3.0V * 15 = 45V).</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><br /></p> <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-weight:600;">*DISCLAIMER: Make sure your Voltage Cutoff Start and End (Motor CFG -&gt; General -&gt; Voltage) are below this threshold! LV Pushback should be used as a notice to stop riding immediately, while the Voltage Cutoffs should be used as a last resort measure to protect the battery.</span></p> <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-weight:600;"><br /></p> <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-weight:600;">ALWAYS RESPECT LOW VOLTAGE PUSHBACK!</span></p></body></html> CFG_DFLT_TILTBACK_LV - 1 + 2 1 0 150 0 0 - 0.5 - 45 - 10 + 0.05 + 3 + 100 V 7 + + Speed Threshold + 2 + 1 + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'Roboto'; ; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Speed Threshold to trigger pushback (Pushback raises the nose of the board, informing you to slow down).</p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-style:italic;">Set to 0 to disable.</span></p></body></html> + CFG_DFLT_TILTBACK_SPEED + 1 + 0 + 100 + 0 + 0 + 5 + 0 + km/h + 1 + Return To Level Speed 1 @@ -741,7 +783,7 @@ p, li { white-space: pre-wrap; } <html><head><meta name="qrichtext" content="1" /><style type="text/css"> p, li { white-space: pre-wrap; } </style></head><body style=" font-family:'Roboto'; ; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Speed at which nose is returned back to normal after a pushback condition has been cleared.</p></body></html> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Rate at which nose is returned from pushback back to normal.</p></body></html> CFG_DFLT_TILTBACK_RETURN_SPEED 1 1 @@ -755,6 +797,20 @@ p, li { white-space: pre-wrap; } °/s 7 + + Persistent Fatal Error + 5 + 1 + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'Roboto'; ; font-weight:400; font-style:normal;"> +<p align="justify" style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">When enabled, any serious problem (even if only momentary) like a firmware fault will set a Fatal Error state which will need to be cleared via Vesc Tool or power cycling the board.</p> +<p align="justify" style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><br /></p> +<p align="justify" style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Fatal Error state is signalled via Pushback and Haptic Feedback (four beeps).</p></body></html> + CFG_DFLT_PERSISTENT_FATAL_ERROR + 1 + Constant Tiltback 1 @@ -863,15 +919,163 @@ p, li { white-space: pre-wrap; } ERPM 3 + + Enable BMS Integration + 5 + 1 + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'Roboto'; ; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:12px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Enable BMS integration.</p> +<p style=" margin-top:0px; margin-bottom:12px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">If enabled, the board will alert via pushback and haptics if any of the configured thresholds are crossed.</p> +<p style=" margin-top:0px; margin-bottom:12px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Note: Board restart required. After enabling and before restarting, the package will start checking for BMS values without establishing the communication and will warn about BMS Connection Error. After a restart, the warning should go away if BMS communication is established. This can be used to check the warning mechanism is in place and working.</p></body></html> + CFG_DFLT_BMS_ENABLED + 0 + + + Cell Low Voltage Threshold + 1 + 1 + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'Roboto'; ; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Low voltage threshold per cell to trigger a safety alert.</p></body></html> + CFG_DFLT_BMS_CELL_LV_THRESHOLD + 2 + 1 + 0 + 4.5 + 2.5 + 0 + 0.1 + 2.7 + 1000 + V + 7 + + + Cell High Voltage Threshold + 1 + 1 + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'Roboto'; ; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">High voltage threshold per cell to trigger a safety alert.</p></body></html> + CFG_DFLT_BMS_CELL_HV_THRESHOLD + 2 + 1 + 0 + 4.5 + 2.5 + 0 + 0.1 + 4.3 + 1000 + V + 7 + + + Cell Balance Threshold + 1 + 1 + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'Roboto'; ; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Cell balance threshold to trigger a warning beep while idle.</p></body></html> + CFG_DFLT_BMS_CELL_BALANCE_THRESHOLD + 2 + 1 + 0 + 1 + 0.01 + 0 + 0.05 + 0.2 + 1000 + V + 7 + + + Cell High Temp Threshold + 2 + 1 + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'Roboto'; ; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">High temp threshold per cell to trigger a safety alert.</p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-style:italic;">Set to 0 to disable </span><span style=" font-weight:600; font-style:italic;">both High and Low Cell Temperature</span><span style=" font-style:italic;"> alert.</span></p></body></html> + CFG_DFLT_BMS_CELL_HT_THRESHOLD + 1 + 0 + 60 + 0 + 0 + 5 + 45 + °C + 2 + + + Cell Low Temp Threshold + 2 + 1 + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'Roboto'; ; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Low temp threshold per cell to trigger a safety alert.</p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-weight:600;">Note: It is very bad for battery cells to be </span><span style=" font-weight:600; font-style:italic;">charged</span><span style=" font-weight:600;"> at below 0 °C! It is possible to set this value to a negative number, but you should ensure no significant regeneration occurs at these temperatures, otherwise you </span><span style=" font-weight:600; font-style:italic;">will</span><span style=" font-weight:600;"> permanently damage your battery.</span></p></body></html> + CFG_DFLT_BMS_CELL_LT_THRESHOLD + 1 + 0 + 20 + -20 + 0 + 1 + 0 + °C + 2 + + + BMS High Temp Threshold + 2 + 1 + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'Roboto'; ; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">High temp threshold for the BMS temperature to trigger a safety alert.</p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-style:italic;">Set to 0 to disable this alert.</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-style:italic;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-style:italic;">Note: Requires VESC firmware version 6.06+ and a recent enough BMS firmware.</span></p></body></html> + CFG_DFLT_BMS_BMS_HT_THRESHOLD + 1 + 0 + 80 + 0 + 0 + 5 + 60 + °C + 2 + - Duty Cycle Audible Frequency + Warning Audible Frequency 2 1 <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> <html><head><meta name="qrichtext" content="1" /><style type="text/css"> p, li { white-space: pre-wrap; } </style></head><body style=" font-family:'Roboto'; ; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Audible frequency of the Duty Cycle haptic feedback. Also used for Current Limit haptic Feedback, if enabled.</p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Audible frequency of the Speed and Duty Cycle Alert haptic feedback. Also used for Current Limit haptic feedback, if enabled.</p> <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><br /></p> <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-style:italic;">(generated by foc_play_tone of the 6.05 firmware)</span></p></body></html> CFG_DFLT_HAPTIC_DUTY_FREQUENCY @@ -886,14 +1090,14 @@ p, li { white-space: pre-wrap; } 3 - Duty Cycle Audible Strength + Warning Audible Strength 1 1 <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> <html><head><meta name="qrichtext" content="1" /><style type="text/css"> p, li { white-space: pre-wrap; } </style></head><body style=" font-family:'Roboto'; ; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Maximum strength of the audible Duty Cycle haptic feedback, in Volts. Also used for Current Limit haptic Feedback, if enabled.</p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Maximum strength of the audible Speed and Duty Cycle haptic feedback, in Volts. Also used for Current Limit haptic feedback, if enabled.</p> <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><br /></p> <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Actual strength is determined by strength scaling, which depends on speed. This value is the maximum strength, which will be reached at your configured Maximum Strength Speed.</p> <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-style:italic;"><br /></p> @@ -967,7 +1171,7 @@ p, li { white-space: pre-wrap; } <html><head><meta name="qrichtext" content="1" /><style type="text/css"> p, li { white-space: pre-wrap; } </style></head><body style=" font-family:'Roboto'; ; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Vibrating frequency of both Duty Cycle and Error haptic feedback.</p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Vibrating frequency of both Warning and Error haptic feedback.</p> <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><br /></p> <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-style:italic;">Note: When changing this option, the actual frequency will be changing in larger discrete steps, as the configured frequency is being rounded down to the nearest division of Loop Hertz / 2.</span></p> <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><br /></p> @@ -1676,7 +1880,7 @@ p, li { white-space: pre-wrap; } 7 - Tilitback Angle Limit + Tiltback Angle Limit 1 1 <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> @@ -1917,7 +2121,7 @@ p, li { white-space: pre-wrap; } 7 - Tilitback Angle Limit + Tiltback Angle Limit 1 1 <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> @@ -2179,7 +2383,7 @@ p, li { white-space: pre-wrap; } 7 - Tilitback Angle Limit + Tiltback Angle Limit 1 1 <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> @@ -2433,6 +2637,9 @@ p, li { white-space: pre-wrap; } Strobe Knight Rider Felony + Rainbow Cycle + Rainbow Fade + Rainbow Roll Front Brightness @@ -2590,6 +2797,9 @@ p, li { white-space: pre-wrap; } Strobe Knight Rider Felony + Rainbow Cycle + Rainbow Fade + Rainbow Roll Rear Brightness @@ -2747,6 +2957,9 @@ p, li { white-space: pre-wrap; } Strobe Knight Rider Felony + Rainbow Cycle + Rainbow Fade + Rainbow Roll Headlights Brightness @@ -2906,6 +3119,9 @@ p, li { white-space: pre-wrap; } Strobe Knight Rider Felony + Rainbow Cycle + Rainbow Fade + Rainbow Roll Taillights Brightness @@ -3193,6 +3409,9 @@ p, li { white-space: pre-wrap; } Strobe Knight Rider Felony + Rainbow Cycle + Rainbow Fade + Rainbow Roll Status Idle Brightness @@ -3275,7 +3494,7 @@ p, li { white-space: pre-wrap; } <html><head><meta name="qrichtext" content="1" /><style type="text/css"> p, li { white-space: pre-wrap; } </style></head><body style=" font-family:'Roboto'; ; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Satus Idle secondary color.</p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Status Idle secondary color.</p> <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><br /></p> <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Note: There are three whites due to the extra white channel on RGBW LEDs:</p> <ul style="margin-top: 0px; margin-bottom: 0px; margin-left: 0px; margin-right: 0px; -qt-list-indent: 1;"><li style="" style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">White (full) is full brightness on all four channels, the brightest it can be. It also consumes the most power. On RGB LEDs, this is the same as White (rgb).</li> @@ -3317,7 +3536,7 @@ p, li { white-space: pre-wrap; } Lavender - Satus Idle Speed + Status Idle Speed 1 1 <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> @@ -3350,7 +3569,8 @@ p, li { white-space: pre-wrap; } <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><br /></p> <ul style="margin-top: 0px; margin-bottom: 0px; margin-left: 0px; margin-right: 0px; -qt-list-indent: 1;"><li style="" style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Off: Disable LED controls</li></ul> <ul style="margin-top: 0px; margin-bottom: 0px; margin-left: 0px; margin-right: 0px; -qt-list-indent: 1;"><li style="" style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Internal: LEDs driven by the Refloat package</li> -<li style="" style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">External: External LED control modules, such as the Floatwheel LCM</li></ul> +<li style="" style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">External: External LED control modules, such as the Floatwheel LCM</li> +<li style="" style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Internal and External: Both of the above (e.g. connect status bar directly to VESC and front/rear lights to VESC Express)</li></ul> <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><br /></p> <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-weight:600;">External (LCM)</span></p> <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">External Modules have their own simple LED configuration, which uses three brightness levels:</p> @@ -3378,6 +3598,7 @@ p, li { white-space: pre-wrap; } Off Internal External + Internal and External LED Pin @@ -3390,7 +3611,8 @@ p, li { white-space: pre-wrap; } <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">The pin to which the LEDs are connected on the VESC.</p> <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><br /></p> <ul style="margin-top: 0px; margin-bottom: 0px; margin-left: 0px; margin-right: 0px; -qt-list-indent: 1;"><li style="" style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">PPM/Servo pin: Little FOCer v3.x, Tronic 250R, Ubox V2 75V/100V (pin B6)</li> -<li style="" style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Dedicated LED pin: Little FOCer v4.x, Thor 300 (pin B7)</li></ul> +<li style="" style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Dedicated LED pin: Little FOCer v4.x, Thor 300/301/400 (pin B7)</li></ul> +<ul style="margin-top: 0px; margin-bottom: 0px; margin-left: 0px; margin-right: 0px; -qt-list-indent: 1;"><li style="" style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">JetFleet F6 v1 AUX pin (pin C9)</li></ul> <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><br /></p> <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Board restart required for changes to take effect.</p> <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><br /></p> @@ -3399,7 +3621,29 @@ p, li { white-space: pre-wrap; } 1 PPM/Servo pin Dedicated LED pin + JetFleet F6 v1 pin + + LED Pin Configuration + 4 + 1 + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'Roboto'; ; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">The configuration of the LED pin:</p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><br /></p> +<ul style="margin-top: 0px; margin-bottom: 0px; margin-left: 0px; margin-right: 0px; -qt-list-indent: 1;"><li style="" style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Pullup to 5V: Most controllers with a dedicated LED pin (Little FOCer v4, all Thors), custom solutions on the PPM/Servo pin with an external 1kOhm pullup resistor to 5V</li> +<li style="" style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">No Pullup: JetFleet F6 v2, custom solutions with no external pullup</li></ul> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Board restart required for changes to take effect.</p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-style:italic;">Note: The No Pullup option may work for the Pullup to 5V controllers, but the signal will be limited to 3.3V.</span></p></body></html> + CFG_DFLT_HARDWARE_LEDS_PIN_CONFIG + 0 + Pullup to 5V + No Pullup + Status LED Strip Order 4 @@ -3690,6 +3934,7 @@ p, li { white-space: pre-wrap; } fault_adc_half_erpm fault_is_dual_switch fault_moving_fault_disabled + enable_quickstop fault_darkride_enabled fault_reversestop_enabled tiltback_duty_angle @@ -3702,7 +3947,9 @@ p, li { white-space: pre-wrap; } tiltback_lv_angle tiltback_lv_speed tiltback_lv + tiltback_speed tiltback_return_speed + persistent_fatal_error tiltback_constant tiltback_constant_erpm tiltback_variable @@ -3800,6 +4047,7 @@ p, li { white-space: pre-wrap; } leds.status_idle.speed hardware.leds.mode hardware.leds.pin + hardware.leds.pin_config hardware.leds.status.order hardware.leds.status.count hardware.leds.status.color_order @@ -3825,6 +4073,13 @@ p, li { white-space: pre-wrap; } haptic.min_strength haptic.max_strength_speed haptic.strength_curvature + bms.enabled + bms.cell_lv_threshold + bms.cell_hv_threshold + bms.cell_balance_threshold + bms.cell_ht_threshold + bms.cell_lt_threshold + bms.bms_ht_threshold meta.is_default @@ -3938,6 +4193,7 @@ p, li { white-space: pre-wrap; } fault_adc_half_erpm fault_is_dual_switch fault_moving_fault_disabled + enable_quickstop ::sep::Features fault_darkride_enabled fault_reversestop_enabled @@ -3947,12 +4203,13 @@ p, li { white-space: pre-wrap; } Pushback Alerts ::sep::General Config - tiltback_return_speed - ::sep::Duty Cycle Alert Pushback + tiltback_speed tiltback_duty tiltback_duty_angle tiltback_duty_speed + tiltback_return_speed is_dutybeep_enabled + persistent_fatal_error ::sep::High Voltage Alert Pushback tiltback_hv_angle tiltback_hv_speed @@ -4062,6 +4319,7 @@ p, li { white-space: pre-wrap; } ::sep::Leds (changes require reboot) hardware.leds.mode hardware.leds.pin + hardware.leds.pin_config hardware.leds.status.order hardware.leds.status.count hardware.leds.status.color_order @@ -4076,6 +4334,18 @@ p, li { white-space: pre-wrap; } hardware.leds.rear.reverse + + BMS + + bms.enabled + bms.cell_lv_threshold + bms.cell_hv_threshold + bms.cell_balance_threshold + bms.cell_ht_threshold + bms.cell_lt_threshold + bms.bms_ht_threshold + + Internal diff --git a/refloat/src/data.h b/refloat/src/data.h index e81a8744..cbdf1b11 100644 --- a/refloat/src/data.h +++ b/refloat/src/data.h @@ -17,7 +17,9 @@ #pragma once +#include "alert_tracker.h" #include "atr.h" +#include "bms.h" #include "booster.h" #include "brake_tilt.h" #include "charging.h" @@ -48,11 +50,15 @@ typedef struct { // Firmware version, passed in from Lisp int fw_version_major, fw_version_minor, fw_version_beta; + // IMU data for the balancing filter + BalanceFilterData balance_filter; + Time time; MotorData motor; IMU imu; PID pid; MotorControl motor_control; + TorqueTilt torque_tilt; ATR atr; BrakeTilt brake_tilt; @@ -60,6 +66,22 @@ typedef struct { Booster booster; Remote remote; + State state; + FootpadSensor footpad; + HapticFeedback haptic_feedback; + AlertTracker alert_tracker; + + Leds leds; + LcmData lcm; + Charging charging; + BMS bms; + + DataRecord data_record; + + Konami flywheel_konami; + Konami headlights_on_konami; + Konami headlights_off_konami; + // Beeper int beep_num_left; int beep_duration; @@ -67,13 +89,6 @@ typedef struct { int beep_reason; bool beeper_enabled; - Leds leds; - - // Lights Control Module - external lights control - LcmData lcm; - - Charging charging; - // Config values uint32_t loop_time_us; float startup_pitch_trickmargin, startup_pitch_tolerance; @@ -81,23 +96,13 @@ typedef struct { float tiltback_duty_step_size, tiltback_hv_step_size, tiltback_lv_step_size, tiltback_return_step_size; float tiltback_variable, tiltback_variable_max_erpm, noseangling_step_size; - float mc_max_temp_fet, mc_max_temp_mot; bool duty_beeping; - // IMU data for the balancing filter - BalanceFilterData balance_filter; - - float max_duty_with_margin; - - FootpadSensor footpad; - - // Rumtime state values - State state; - float balance_current; float setpoint, setpoint_target, setpoint_target_interpolated; float noseangling_interpolated; + time_t alert_timer; time_t nag_timer; float idle_voltage; time_t fault_angle_pitch_timer, fault_angle_roll_timer, fault_switch_timer, @@ -128,11 +133,4 @@ typedef struct { int rc_counter; float rc_current_target; float rc_current; - - HapticFeedback haptic_feedback; - DataRecord data_record; - - Konami flywheel_konami; - Konami headlights_on_konami; - Konami headlights_off_konami; } Data; diff --git a/refloat/src/data_record.h b/refloat/src/data_record.h index f5ae4f33..e637a061 100644 --- a/refloat/src/data_record.h +++ b/refloat/src/data_record.h @@ -17,6 +17,7 @@ #pragma once +#include "lib/circular_buffer.h" #include "rt_data.h" #include "time.h" @@ -30,12 +31,9 @@ typedef struct { } Sample; typedef struct { - Sample *buffer; - size_t size; - size_t head; - size_t tail; - bool empty; + bool enabled; bool recording; bool autostart; bool autostop; + CircularBuffer buffer; } DataRecord; diff --git a/refloat/src/data_recorder.c b/refloat/src/data_recorder.c index 86eb2cb0..4887ffe7 100644 --- a/refloat/src/data_recorder.c +++ b/refloat/src/data_recorder.c @@ -20,46 +20,8 @@ #include "utils.h" #include "vesc_c_if.h" -static void circular_buffer_reset(DataRecord *dr) { - dr->head = 0; - dr->tail = 0; - dr->empty = true; -} - -static inline void increment(const DataRecord *buffer, size_t *i) { - ++(*i); - if (*i >= buffer->size) { - *i -= buffer->size; - } -} - -static void circular_buffer_push(DataRecord *buffer, const Sample *sample) { - buffer->buffer[buffer->head] = *sample; - - if (!buffer->empty && buffer->head == buffer->tail) { - increment(buffer, &buffer->tail); - } - increment(buffer, &buffer->head); - - buffer->empty = false; -} - -static void circular_buffer_iterate( - const DataRecord *buffer, void (*cb)(const Sample *sample, void *data), void *data -) { - if (buffer->empty) { - return; - } - - size_t i = buffer->tail; - do { - cb(&buffer->buffer[i], data); - increment(buffer, &i); - } while (i != buffer->head); -} - static void start_recording(DataRecord *dr) { - circular_buffer_reset(dr); + circular_buffer_clear(&dr->buffer); dr->recording = true; } @@ -74,34 +36,32 @@ typedef struct { } DataBufferInfo; void data_recorder_init(DataRecord *dr) { + dr->recording = false; + dr->autostart = true; + dr->autostop = true; + // fetch information about the data buffer, it's stored at the end of the // VESC interface memory area DataBufferInfo *buffer_info = (DataBufferInfo *) ((uint8_t *) VESC_IF + 2036); - if (buffer_info->magic == 0xcafe1111) { - size_t size = buffer_info->length; - size_t sample_nr = size / sizeof(Sample); - log_msg("Data Record buffer size: %uB (%u samples)", size, sample_nr); - dr->buffer = (Sample *) buffer_info->buffer; - dr->size = sample_nr; - } else { - dr->buffer = 0; - dr->size = 0; + if (buffer_info->magic != 0xcafe1111) { + dr->enabled = false; + return; } - dr->head = 0; - dr->tail = 0; - dr->empty = true; - dr->recording = false; - dr->autostart = true; - dr->autostop = true; + dr->enabled = true; + size_t size = buffer_info->length; + size_t sample_nr = size / sizeof(Sample); + uint8_t *buffer = buffer_info->buffer; + circular_buffer_init(&dr->buffer, sizeof(Sample), sample_nr, buffer); + log_msg("Data Record buffer size: %uB (%u samples)", size, sample_nr); } bool data_recorder_has_capability(const DataRecord *dr) { - return dr->buffer != NULL; + return dr->enabled; } void data_recorder_trigger(DataRecord *dr, bool engage) { - if (!dr->buffer) { + if (!dr->enabled) { return; } @@ -112,28 +72,29 @@ void data_recorder_trigger(DataRecord *dr, bool engage) { } } -void data_recorder_sample(DataRecord *dr, const Data *d) { - if (!dr->buffer || !dr->recording) { +void data_recorder_sample(DataRecord *dr, const Data *d, time_t time) { + if (!dr->enabled || !dr->recording) { return; } uint8_t flags = d->state.sat << 4 | d->footpad.state << 2; flags |= d->state.wheelslip << 1 | (d->state.state == STATE_RUNNING); - Sample p = - {.time = d->time.now, + Sample sample = + {.time = time, .flags = flags, .values = { #define ARRAY_VALUE(id) to_float16(d->id), VISIT_REC(RT_DATA_ALL_ITEMS, ARRAY_VALUE) #undef ARRAY_VALUE }}; - circular_buffer_push(dr, &p); + circular_buffer_push(&dr->buffer, &sample); } -static void send_point_vt_experiment(const Sample *sample, void *data) { +static void send_point_vt_experiment(const void *item, void *data) { unused(data); + Sample *sample = (Sample *) item; for (uint8_t i = 0; i < ITEMS_COUNT_REC(RT_DATA_ALL_ITEMS); ++i) { VESC_IF->plot_set_graph(i); VESC_IF->plot_send_points(sample->time, sample->values[i]); @@ -141,7 +102,7 @@ static void send_point_vt_experiment(const Sample *sample, void *data) { } void data_recorder_send_experiment_plot(DataRecord *dr) { - if (!dr->buffer) { + if (!dr->enabled) { return; } @@ -151,7 +112,7 @@ void data_recorder_send_experiment_plot(DataRecord *dr) { VISIT_REC(RT_DATA_ALL_ITEMS, ADD_GRAPH); #undef ADD_GRAPH - circular_buffer_iterate(dr, &send_point_vt_experiment, 0); + circular_buffer_iterate(&dr->buffer, &send_point_vt_experiment, 0); } typedef enum { @@ -167,15 +128,7 @@ static void send_header(DataRecord *dr) { buf[ind++] = 101; // Package ID buf[ind++] = COMMAND_DATA_RECORD_HEADER; - size_t size = 0; - if (!dr->empty) { - size = (dr->head - dr->tail) % dr->size; - if (size == 0) { - size = dr->size; - } - } - - buffer_append_uint32(buf, size, &ind); + buffer_append_uint32(buf, circular_buffer_size(&dr->buffer), &ind); buf[ind++] = ITEMS_COUNT_REC(RT_DATA_ALL_ITEMS); #define ADD_ID(id) buffer_append_string(buf, #id, &ind); @@ -186,7 +139,7 @@ static void send_header(DataRecord *dr) { } static void send_data(const DataRecord *dr, size_t offset) { - if (dr->empty) { + if (!dr->enabled || circular_buffer_size(&dr->buffer) == 0) { return; } @@ -199,29 +152,26 @@ static void send_data(const DataRecord *dr, size_t offset) { buffer_append_uint32(buf, offset, &ind); - bool start = true; - for (size_t i = (dr->tail + offset) % dr->size; start || i != dr->head; increment(dr, &i)) { - // 4 bytes for time, 1 byte for flags, 2 bytes for rest of the values - if (ind + 4 + 1 + 2 * ITEMS_COUNT_REC(RT_DATA_ALL_ITEMS) > bufsize) { - break; - } - - const Sample *sample = &dr->buffer[i]; - buffer_append_uint32(buf, sample->time, &ind); - buf[ind++] = sample->flags; + Sample sample; + while (circular_buffer_get(&dr->buffer, offset++, &sample)) { + buffer_append_uint32(buf, sample.time, &ind); + buf[ind++] = sample.flags; for (size_t i = 0; i < ITEMS_COUNT_REC(RT_DATA_ALL_ITEMS); ++i) { - buffer_append_uint16(buf, sample->values[i], &ind); + buffer_append_uint16(buf, sample.values[i], &ind); } - start = false; + // 4 bytes for time, 1 byte for flags, 2 bytes for rest of the values + if (ind + 4 + 1 + 2 * ITEMS_COUNT_REC(RT_DATA_ALL_ITEMS) > bufsize) { + break; + } } SEND_APP_DATA(buf, bufsize, ind); } void data_recorder_request(DataRecord *dr, uint8_t *buffer, size_t len) { - if (!dr->buffer) { + if (!dr->enabled) { log_error("Data Record not supported."); return; } diff --git a/refloat/src/data_recorder.h b/refloat/src/data_recorder.h index 0284fad0..134dcb05 100644 --- a/refloat/src/data_recorder.h +++ b/refloat/src/data_recorder.h @@ -35,7 +35,7 @@ bool data_recorder_has_capability(const DataRecord *dr); void data_recorder_trigger(DataRecord *dr, bool engage); -void data_recorder_sample(DataRecord *dr, const Data *data); +void data_recorder_sample(DataRecord *dr, const Data *data, time_t time); void data_recorder_request(DataRecord *dr, uint8_t *buffer, size_t len); diff --git a/refloat/src/footpad_sensor.c b/refloat/src/footpad_sensor.c index 9de4ef67..bc8c90ed 100644 --- a/refloat/src/footpad_sensor.c +++ b/refloat/src/footpad_sensor.c @@ -19,6 +19,12 @@ #include "vesc_c_if.h" +void footpad_sensor_init(FootpadSensor *fs) { + fs->adc1 = 0.0f; + fs->adc2 = 0.0f; + fs->state = FS_NONE; +} + void footpad_sensor_update(FootpadSensor *fs, const RefloatConfig *config) { fs->adc1 = VESC_IF->io_read_analog(VESC_PIN_ADC1); // Returns -1.0 if the pin is missing on the hardware diff --git a/refloat/src/footpad_sensor.h b/refloat/src/footpad_sensor.h index 1d5ce0bd..ff6fa05a 100644 --- a/refloat/src/footpad_sensor.h +++ b/refloat/src/footpad_sensor.h @@ -31,6 +31,8 @@ typedef struct { FootpadSensorState state; } FootpadSensor; +void footpad_sensor_init(FootpadSensor *fs); + void footpad_sensor_update(FootpadSensor *fs, const RefloatConfig *config); int footpad_sensor_state_to_switch_compat(FootpadSensorState v); diff --git a/refloat/src/haptic_feedback.c b/refloat/src/haptic_feedback.c index 34953d2e..5681047b 100644 --- a/refloat/src/haptic_feedback.c +++ b/refloat/src/haptic_feedback.c @@ -29,6 +29,7 @@ void haptic_feedback_init(HapticFeedback *hf) { hf->type_playing = HAPTIC_FEEDBACK_NONE; hf->tone_timer = 0; hf->is_playing = false; + hf->can_change_type = true; } void haptic_feedback_configure(HapticFeedback *hf, const RefloatConfig *cfg) { @@ -43,24 +44,31 @@ void haptic_feedback_configure(HapticFeedback *hf, const RefloatConfig *cfg) { } static HapticFeedbackType haptic_feedback_get_type( - const HapticFeedback *hf, const State *state, const MotorData *md + const HapticFeedback *hf, const State *state, const MotorData *md, const AlertTracker *at ) { // TODO: Ideally we don't even do pushback in handtest, as it can be confusing if (state->state != STATE_RUNNING || state->mode == MODE_HANDTEST) { return HAPTIC_FEEDBACK_NONE; } + if (at->fatal_error) { + return HAPTIC_FEEDBACK_ERROR_FATAL; + } + switch (state->sat) { case SAT_PB_DUTY: if (md->duty_cycle > hf->duty_solid_threshold) { return HAPTIC_FEEDBACK_DUTY_CONTINUOUS; } else { - return HAPTIC_FEEDBACK_DUTY; + return HAPTIC_FEEDBACK_DUTY_SPEED; } + case SAT_PB_SPEED: + return HAPTIC_FEEDBACK_DUTY_SPEED; case SAT_PB_TEMPERATURE: return HAPTIC_FEEDBACK_ERROR_TEMPERATURE; case SAT_PB_LOW_VOLTAGE: case SAT_PB_HIGH_VOLTAGE: + case SAT_PB_ERROR: return HAPTIC_FEEDBACK_ERROR_VOLTAGE; default: break; @@ -79,7 +87,7 @@ static HapticFeedbackType haptic_feedback_get_type( // skipped, giving a certain number of "beeps" followed by a pause. static uint8_t get_beats(HapticFeedbackType type) { switch (type) { - case HAPTIC_FEEDBACK_DUTY: + case HAPTIC_FEEDBACK_DUTY_SPEED: return 2; case HAPTIC_FEEDBACK_DUTY_CONTINUOUS: return 0; @@ -87,6 +95,8 @@ static uint8_t get_beats(HapticFeedbackType type) { return 6; case HAPTIC_FEEDBACK_ERROR_VOLTAGE: return 8; + case HAPTIC_FEEDBACK_ERROR_FATAL: + return 10; case HAPTIC_FEEDBACK_NONE: break; } @@ -96,11 +106,12 @@ static uint8_t get_beats(HapticFeedbackType type) { static const CfgHapticTone *get_haptic_tone(const HapticFeedback *hf) { switch (hf->type_playing) { - case HAPTIC_FEEDBACK_DUTY: + case HAPTIC_FEEDBACK_DUTY_SPEED: case HAPTIC_FEEDBACK_DUTY_CONTINUOUS: return &hf->cfg->duty; case HAPTIC_FEEDBACK_ERROR_TEMPERATURE: case HAPTIC_FEEDBACK_ERROR_VOLTAGE: + case HAPTIC_FEEDBACK_ERROR_FATAL: return &hf->cfg->error; case HAPTIC_FEEDBACK_NONE: break; @@ -122,11 +133,16 @@ static inline void foc_play_tone(int channel, float freq, float voltage) { } void haptic_feedback_update( - HapticFeedback *hf, MotorControl *mc, const State *state, const MotorData *md, const Time *time + HapticFeedback *hf, + MotorControl *mc, + const State *state, + const MotorData *md, + const AlertTracker *at, + const Time *time ) { - HapticFeedbackType type_to_play = haptic_feedback_get_type(hf, state, md); + HapticFeedbackType type_to_play = haptic_feedback_get_type(hf, state, md, at); - if (type_to_play != hf->type_playing && timer_older(time, hf->tone_timer, TONE_LENGTH)) { + if (type_to_play != hf->type_playing && hf->can_change_type) { hf->type_playing = type_to_play; timer_refresh(time, &hf->tone_timer); } @@ -136,6 +152,7 @@ void haptic_feedback_update( uint8_t beats = get_beats(hf->type_playing); if (beats == 0) { should_be_playing = true; + hf->can_change_type = true; } else { float period = TONE_LENGTH * beats; float tone_time = fmodf(timer_age(time, hf->tone_timer), period); @@ -143,7 +160,12 @@ void haptic_feedback_update( uint8_t off_beat = beats > 2 ? beats - 2 : 0; should_be_playing = beat % 2 == 0 && (off_beat == 0 || beat != off_beat); + // Only allow changing type (another pattern or stop alerting) + // if we just finished a period + hf->can_change_type = !hf->is_playing && beat == 0; } + } else { + hf->can_change_type = true; } if (hf->is_playing && !should_be_playing) { diff --git a/refloat/src/haptic_feedback.h b/refloat/src/haptic_feedback.h index 3661a90a..683c1cdc 100644 --- a/refloat/src/haptic_feedback.h +++ b/refloat/src/haptic_feedback.h @@ -25,10 +25,11 @@ typedef enum { HAPTIC_FEEDBACK_NONE = 0, - HAPTIC_FEEDBACK_DUTY, + HAPTIC_FEEDBACK_DUTY_SPEED, HAPTIC_FEEDBACK_DUTY_CONTINUOUS, HAPTIC_FEEDBACK_ERROR_TEMPERATURE, HAPTIC_FEEDBACK_ERROR_VOLTAGE, + HAPTIC_FEEDBACK_ERROR_FATAL, } HapticFeedbackType; typedef struct { @@ -40,6 +41,7 @@ typedef struct { HapticFeedbackType type_playing; time_t tone_timer; bool is_playing; + bool can_change_type; } HapticFeedback; void haptic_feedback_init(HapticFeedback *hf); @@ -47,5 +49,10 @@ void haptic_feedback_init(HapticFeedback *hf); void haptic_feedback_configure(HapticFeedback *hf, const RefloatConfig *cfg); void haptic_feedback_update( - HapticFeedback *hf, MotorControl *mc, const State *state, const MotorData *md, const Time *time + HapticFeedback *hf, + MotorControl *mc, + const State *state, + const MotorData *md, + const AlertTracker *at, + const Time *time ); diff --git a/refloat/src/imu.c b/refloat/src/imu.c index 00732847..6a765ef9 100644 --- a/refloat/src/imu.c +++ b/refloat/src/imu.c @@ -33,14 +33,22 @@ void imu_init(IMU *imu) { } void imu_update(IMU *imu, const BalanceFilterData *bf, const State *state) { + float roll_rad = VESC_IF->imu_get_roll(); // in Radians + imu->pitch = rad2deg(VESC_IF->imu_get_pitch()); - imu->roll = rad2deg(VESC_IF->imu_get_roll()); + imu->roll = rad2deg(roll_rad); imu->yaw = rad2deg(VESC_IF->imu_get_yaw()); imu->balance_pitch = rad2deg(balance_filter_get_pitch(bf)); float gyro[3]; VESC_IF->imu_get_gyro(gyro); - imu->pitch_rate = gyro[1]; + + float sin_roll = sinf(roll_rad); + float cos_roll = cosf(roll_rad); + + // Rotated to diminish influence of Yaw Change on Gyro Y when board is rolled + // (Estimates Pitch Rate solely due to rider input, without influence from board turning) + imu->pitch_rate = cos_roll * cos_roll * gyro[1] + sin_roll * cos_roll * gyro[2]; if (state->darkride) { imu->pitch_rate = -imu->pitch_rate; } diff --git a/refloat/src/lcm.c b/refloat/src/lcm.c index b0e1f284..e3f0430d 100644 --- a/refloat/src/lcm.c +++ b/refloat/src/lcm.c @@ -25,7 +25,7 @@ #include void lcm_init(LcmData *lcm, CfgHwLeds *hw_cfg) { - lcm->enabled = hw_cfg->mode == LED_MODE_EXTERNAL; + lcm->enabled = hw_cfg->mode & LED_MODE_EXTERNAL; lcm->brightness = 0; lcm->brightness_idle = 0; lcm->status_brightness = 0; diff --git a/refloat/src/led_driver.c b/refloat/src/led_driver.c index 452a4ab6..8c044a0f 100644 --- a/refloat/src/led_driver.c +++ b/refloat/src/led_driver.c @@ -18,113 +18,152 @@ #include "led_driver.h" -#include "st_types.h" - #include "utils.h" #include "vesc_c_if.h" -#include #include #define WS2812_CLK_HZ 800000 #define TIM_PERIOD ((168000000 / 2 / WS2812_CLK_HZ) - 1) #define WS2812_ZERO (((uint32_t) TIM_PERIOD) * 0.3) #define WS2812_ONE (((uint32_t) TIM_PERIOD) * 0.7) -#define BITBUFFER_PAD 1000 -static DMA_Stream_TypeDef *get_dma_stream(LedPin pin) { - if (pin == LED_PIN_B6) { - return DMA1_Stream0; - } else { - return DMA1_Stream3; +static const PinHwConfig pin_hw_configs[] = { + { + .pin_port = GPIOB, + .pin_nr = 6, + .timer = TIM4, + .ccr_address = (uint32_t) &TIM4->CCR1, + .rcc_apb1_periph = RCC_APB1Periph_TIM4, + .timer_ccmr = &TIM4->CCMR1, + .timer_ccmr_shift = 0, + .timer_ccer_shift = 0, + .dma_stream = DMA1_Stream0, + .dma_channel = DMA_Channel_2, + .dma_if_shift = 0, + .dma_source = TIM_DMA_CC1, + }, + { + .pin_port = GPIOB, + .pin_nr = 7, + .timer = TIM4, + .ccr_address = (uint32_t) &TIM4->CCR2, + .rcc_apb1_periph = RCC_APB1Periph_TIM4, + .timer_ccmr = &TIM4->CCMR1, + .timer_ccmr_shift = 8, + .timer_ccer_shift = 4, + .dma_stream = DMA1_Stream3, + .dma_channel = DMA_Channel_2, + .dma_if_shift = 22, + .dma_source = TIM_DMA_CC2, + }, + { + .pin_port = GPIOC, + .pin_nr = 9, + .timer = TIM3, + .ccr_address = (uint32_t) &TIM3->CCR4, + .rcc_apb1_periph = RCC_APB1Periph_TIM3, + .timer_ccmr = &TIM3->CCMR2, + .timer_ccmr_shift = 8, + .timer_ccer_shift = 12, + .dma_stream = DMA1_Stream2, + .dma_channel = DMA_Channel_5, + .dma_if_shift = 16, + .dma_source = TIM_DMA_CC4, } +}; + +static void reset_timer(const PinHwConfig *pin_hw_cfg) { + RCC->APB1RSTR |= pin_hw_cfg->rcc_apb1_periph; + RCC->APB1RSTR &= ~pin_hw_cfg->rcc_apb1_periph; } -static void init_dma(LedPin pin, uint16_t *buffer, uint32_t length) { - TIM_TypeDef *tim = TIM4; - uint32_t dma_ch = DMA_Channel_2; - DMA_Stream_TypeDef *dma_stream = get_dma_stream(pin); - - // settings for different pins in one place, can be extended if more pins are added - uint8_t pin_nr; - uint32_t CCR_address; - uint16_t DMA_source; - if (pin == LED_PIN_B6) { - pin_nr = 6; - CCR_address = (uint32_t) &tim->CCR1; - DMA_source = TIM_DMA_CC1; - } else { - pin_nr = 7; - CCR_address = (uint32_t) &tim->CCR2; - DMA_source = TIM_DMA_CC2; - } +static void init_timer(const PinHwConfig *pin_hw_cfg) { + // enable the Low Speed APB (APB1) peripheral clock + // see: RCC_APB1PeriphClockCmd() + RCC->APB1ENR |= pin_hw_cfg->rcc_apb1_periph; + + // init timer registers + // see: TIM_TimeBaseInit() + pin_hw_cfg->timer->CR1 |= TIM_CounterMode_Up; + pin_hw_cfg->timer->ARR = TIM_PERIOD; + + // see: TIM_OCInit() and TIM_OCPreloadConfig() + *pin_hw_cfg->timer_ccmr |= TIM_OCMode_PWM1 << pin_hw_cfg->timer_ccmr_shift; + pin_hw_cfg->timer->CCER |= (TIM_OCPolarity_High | TIM_OutputState_Enable) + << pin_hw_cfg->timer_ccer_shift; + *pin_hw_cfg->timer_ccmr |= TIM_OCPreload_Enable << pin_hw_cfg->timer_ccmr_shift; + + // enable timer peripheral Preload register on ARR and enable the timer + // see: RCC_APB1PeriphClockCmd() + pin_hw_cfg->timer->CR1 |= TIM_CR1_ARPE | TIM_CR1_CEN; +} - VESC_IF->set_pad_mode( - GPIOB, pin_nr, PAL_MODE_ALTERNATE(2) | PAL_STM32_OTYPE_OPENDRAIN | PAL_STM32_OSPEED_MID1 - ); - - TIM_DeInit(tim); - - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA1, ENABLE); - DMA_DeInit(dma_stream); - - DMA_InitTypeDef DMA_InitStructure; - DMA_InitStructure.DMA_PeripheralBaseAddr = CCR_address; - DMA_InitStructure.DMA_Channel = dma_ch; - DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t) (buffer); - DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral; - DMA_InitStructure.DMA_BufferSize = length; - DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; - DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; - DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; - DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord; - DMA_InitStructure.DMA_Mode = DMA_Mode_Circular; - DMA_InitStructure.DMA_Priority = DMA_Priority_High; - DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable; - DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_Full; - DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single; - DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; - - DMA_Init(dma_stream, &DMA_InitStructure); - - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); - - TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; - TIM_TimeBaseStructure.TIM_Prescaler = 0; - TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; - TIM_TimeBaseStructure.TIM_Period = TIM_PERIOD; - TIM_TimeBaseStructure.TIM_ClockDivision = 0; - TIM_TimeBaseStructure.TIM_RepetitionCounter = 0; - - TIM_TimeBaseInit(tim, &TIM_TimeBaseStructure); - - TIM_OCInitTypeDef TIM_OCInitStructure; - TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; - TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; - TIM_OCInitStructure.TIM_Pulse = buffer[0]; - TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; - - if (CCR_address == (uint32_t) &tim->CCR1) { - TIM_OC1Init(tim, &TIM_OCInitStructure); - TIM_OC1PreloadConfig(tim, TIM_OCPreload_Enable); - } else { - TIM_OC2Init(tim, &TIM_OCInitStructure); - TIM_OC2PreloadConfig(tim, TIM_OCPreload_Enable); - } +static void disable_dma_stream(const PinHwConfig *pin_hw_cfg) { + pin_hw_cfg->dma_stream->CR &= ~DMA_SxCR_EN; +} - TIM_ARRPreloadConfig(tim, ENABLE); +static void enable_dma_stream(const PinHwConfig *pin_hw_cfg) { + pin_hw_cfg->dma_stream->CR |= DMA_SxCR_EN; +} - TIM_Cmd(tim, ENABLE); +static void init_dma_stream(const PinHwConfig *pin_hw_cfg, uint16_t *buf, uint32_t buf_len) { + // enable the AHB1 peripheral clock for DMA1 + // see: RCC_AHB1PeriphClockCmd() + RCC->AHB1ENR |= RCC_AHB1Periph_DMA1; - DMA_Cmd(dma_stream, ENABLE); + // see: DMA_Init() + disable_dma_stream(pin_hw_cfg); - TIM_DMACmd(tim, DMA_source, ENABLE); + pin_hw_cfg->dma_stream->M1AR = 0; + + const uint32_t dma_stream0_it_mask = + DMA_LISR_FEIF0 | DMA_LISR_DMEIF0 | DMA_LISR_TEIF0 | DMA_LISR_HTIF0 | DMA_LISR_TCIF0; + DMA1->LIFCR = dma_stream0_it_mask << pin_hw_cfg->dma_if_shift; + + pin_hw_cfg->dma_stream->CR = pin_hw_cfg->dma_channel | DMA_DIR_MemoryToPeripheral | + DMA_MemoryInc_Enable | DMA_PeripheralDataSize_HalfWord | DMA_MemoryDataSize_HalfWord | + DMA_Mode_Normal | DMA_Priority_High | DMA_MemoryBurst_Single | DMA_PeripheralBurst_Single; + + pin_hw_cfg->dma_stream->FCR = 0x00000020 | DMA_FIFOThreshold_Full; + + pin_hw_cfg->dma_stream->M0AR = (uint32_t) buf; + pin_hw_cfg->dma_stream->NDTR = buf_len; + pin_hw_cfg->dma_stream->PAR = pin_hw_cfg->ccr_address; + + enable_dma_stream(pin_hw_cfg); } -static void deinit_dma(LedPin pin) { - TIM_DeInit(TIM4); - DMA_DeInit(get_dma_stream(pin)); +static void reset_dma_stream_transfer_complete(const PinHwConfig *pin_hw_cfg) { + DMA1->LIFCR |= DMA_LIFCR_CTCIF0 << pin_hw_cfg->dma_if_shift; +} + +static void disable_timer_dma(const PinHwConfig *pin_hw_cfg) { + pin_hw_cfg->timer->DIER &= ~pin_hw_cfg->dma_source; +} + +static void enable_timer_dma(const PinHwConfig *pin_hw_cfg) { + pin_hw_cfg->timer->DIER |= pin_hw_cfg->dma_source; +} + +static void init_hw( + const PinHwConfig *cfg, LedPinConfig pin_config, uint16_t *buffer, uint32_t length +) { + uint32_t pin_mode = PAL_MODE_ALTERNATE(2) | PAL_STM32_OSPEED_MID1; + pin_mode |= pin_config == LED_PIN_CFG_PULLUP_TO_5V ? PAL_STM32_OTYPE_OPENDRAIN + : PAL_STM32_OTYPE_PUSHPULL; + VESC_IF->set_pad_mode(cfg->pin_port, cfg->pin_nr, pin_mode); + + reset_timer(cfg); + init_dma_stream(cfg, buffer, length); + init_timer(cfg); + enable_timer_dma(cfg); +} + +static void deinit_hw(const PinHwConfig *cfg) { + reset_timer(cfg); + disable_dma_stream(cfg); } inline static uint8_t color_order_bits(LedColorOrder order) { @@ -146,7 +185,15 @@ void led_driver_init(LedDriver *driver) { driver->bitbuffer = NULL; } -bool led_driver_setup(LedDriver *driver, LedPin pin, const LedStrip **led_strips) { +bool led_driver_setup( + LedDriver *driver, LedPin pin, LedPinConfig pin_config, const LedStrip **led_strips +) { + if (pin > LED_PIN_LAST) { + log_error("Invalid LED pin configured: %u", pin); + return false; + } + driver->pin_hw_config = &pin_hw_configs[pin]; + driver->bitbuffer_length = 0; size_t offsets[3] = {0}; @@ -163,9 +210,8 @@ bool led_driver_setup(LedDriver *driver, LedPin pin, const LedStrip **led_strips driver->bitbuffer_length += color_order_bits(strip->color_order) * strip->length; } - uint32_t padding_offset = driver->bitbuffer_length; - - driver->bitbuffer_length += BITBUFFER_PAD; + // An extra array item to set the output to 0 PWM + ++driver->bitbuffer_length; driver->bitbuffer = VESC_IF->malloc(sizeof(uint16_t) * driver->bitbuffer_length); driver->pin = pin; @@ -180,12 +226,12 @@ bool led_driver_setup(LedDriver *driver, LedPin pin, const LedStrip **led_strips } } - for (uint32_t i = 0; i < padding_offset; ++i) { + for (uint32_t i = 0; i < driver->bitbuffer_length - 1; ++i) { driver->bitbuffer[i] = WS2812_ZERO; } + driver->bitbuffer[driver->bitbuffer_length - 1] = 0; - memset(driver->bitbuffer + padding_offset, 0, sizeof(uint16_t) * BITBUFFER_PAD); - init_dma(pin, driver->bitbuffer, driver->bitbuffer_length); + init_hw(driver->pin_hw_config, pin_config, driver->bitbuffer, driver->bitbuffer_length); return true; } @@ -256,12 +302,19 @@ void led_driver_paint(LedDriver *driver) { } } } + + const PinHwConfig *cfg = driver->pin_hw_config; + disable_timer_dma(cfg); + disable_dma_stream(cfg); + reset_dma_stream_transfer_complete(cfg); + enable_dma_stream(cfg); + enable_timer_dma(cfg); } void led_driver_destroy(LedDriver *driver) { if (driver->bitbuffer) { // only touch the timer/DMA if we inited it - something else could be using it - deinit_dma(driver->pin); + deinit_hw(driver->pin_hw_config); VESC_IF->free(driver->bitbuffer); driver->bitbuffer = NULL; diff --git a/refloat/src/led_driver.h b/refloat/src/led_driver.h index 4f3c88d8..a04f1e65 100644 --- a/refloat/src/led_driver.h +++ b/refloat/src/led_driver.h @@ -21,20 +21,40 @@ #include "conf/datatypes.h" #include "led_strip.h" +#include "st_types.h" + #include #include +typedef struct { + void *pin_port; + TIM_TypeDef *timer; + uint32_t ccr_address; + uint32_t rcc_apb1_periph; + volatile uint32_t *timer_ccmr; + DMA_Stream_TypeDef *dma_stream; + uint32_t dma_channel; + uint16_t dma_source; + uint8_t dma_if_shift; + uint8_t timer_ccmr_shift; + uint8_t timer_ccer_shift; + uint8_t pin_nr; +} PinHwConfig; + typedef struct { uint16_t *bitbuffer; uint32_t bitbuffer_length; LedPin pin; + const PinHwConfig *pin_hw_config; const LedStrip *strips[STRIP_COUNT]; uint16_t *strip_bitbuffs[STRIP_COUNT]; } LedDriver; void led_driver_init(LedDriver *driver); -bool led_driver_setup(LedDriver *driver, LedPin pin, const LedStrip **led_strips); +bool led_driver_setup( + LedDriver *driver, LedPin pin, LedPinConfig pin_config, const LedStrip **led_strips +); void led_driver_paint(LedDriver *driver); diff --git a/refloat/src/leds.c b/refloat/src/leds.c index 5adc118f..efc7be9e 100644 --- a/refloat/src/leds.c +++ b/refloat/src/leds.c @@ -102,18 +102,42 @@ static uint32_t color_blend(uint32_t color1, uint32_t color2, float blend) { return RGBW(r, g, b, w); } -// Borrowed from WLED -static uint32_t color_wheel(uint8_t pos) { - pos = 255 - pos; - if (pos < 85) { - return ((uint32_t) (255 - pos * 3) << 16) | ((uint32_t) 0 << 8) | (pos * 3); - } else if (pos < 170) { - pos -= 85; - return ((uint32_t) 0 << 16) | ((uint32_t) (pos * 3) << 8) | (255 - pos * 3); +// Bhaskara cosine approximation. +// Returns a cosine wave oscillating from 0 to 1, starting at 0, with a period of 2s: +// (1 - cos(x)) / 2 +static float cosine_progress(float time) { + uint32_t rounded = lroundf(time); + float x = (time - rounded) * M_PI; + x *= x; + float cos = 2.5f * x / (x + M_PI * M_PI); + if (rounded % 2 == 1) { + return 1 - cos; } else { - pos -= 170; - return ((uint32_t) (pos * 3) << 16) | ((uint32_t) (255 - pos * 3) << 8) | 0; + return cos; + } +} + +// Tweaks the color channel value to make the hue more uniform. +static float tweak_color(float x, float a) { + if (x < 1.0f) { + return 1.0f - powf(1.0f - x, a); + } else if (x < 2.0f) { + return 1.0f + powf(x - 1.0f, a); } + return 0.0f; +} + +// Returns color for hue in range [0..255]. +static uint32_t hue_to_color(uint8_t hue) { + float norm = (float) hue / 255.0f * 3.0f; + float r_norm = fmodf(norm + 0.5f, 3.0f); + float g_norm = fmodf(norm + 2.5f, 3.0f); + float b_norm = fmodf(norm + 1.5f, 3.0f); + float r = cosine_progress(tweak_color(r_norm, 3.2f)); + float g = cosine_progress(tweak_color(g_norm, 2.4f)); + float b = cosine_progress(tweak_color(b_norm, 2.2f)); + + return ((uint8_t) (r * 255) << 16) | ((uint8_t) (g * 255) << 8) | (uint8_t) (b * 255); } static void sattolo_shuffle(uint32_t seed, uint8_t *array, uint8_t length) { @@ -192,18 +216,6 @@ static void strip_set_color( strip_set_color_range(leds, strip, color, brightness, blend, 0, strip->length); } -// Returns a cosine wave oscillating from 0 to 1, starting at 0, with a period of 2s. -static float cosine_progress(float time) { - uint32_t rounded = lroundf(time); - float x = (time - rounded) * M_PI; - // Bhaskara cosine approximation, optimized to return: (1 - cos(x)) / 2 - float cos = 10 * x * x / (4 * x * x + 4 * M_PI * M_PI); - if (rounded % 2 == 1) { - cos = 1 - cos; - } - return cos; -} - static void anim_fade(Leds *leds, const LedStrip *strip, const LedBar *bar, float time) { float p = cosine_progress(time); uint32_t color = color_blend(colors[bar->color2], colors[bar->color1], p); @@ -310,6 +322,32 @@ static void anim_felony(Leds *leds, const LedStrip *strip, const LedBar *bar, fl } } +static void anim_rainbow_cycle(Leds *leds, const LedStrip *strip, float time) { + const uint8_t count = 10; + const float segment = 255.0f / count; + uint8_t color_idx = ((uint8_t) (time * count) % count) * segment; + strip_set_color(leds, strip, hue_to_color(color_idx), strip->brightness, 1.0f); +} + +static void anim_rainbow_fade(Leds *leds, const LedStrip *strip, float time) { + uint8_t offset = fmodf(time, 1.0f) * 255.0f; + strip_set_color(leds, strip, hue_to_color(offset), strip->brightness, 1.0f); +} + +static void anim_rainbow_roll(Leds *leds, const LedStrip *strip, float time) { + uint8_t offset = fmodf(time, 1.0f) * 255.0f; + for (int i = 0; i < strip->length; ++i) { + led_set_color( + leds, + strip, + i, + hue_to_color(255.0f / strip->length * i + offset), + strip->brightness, + 1.0f + ); + } +} + static void led_strip_animate(Leds *leds, const LedStrip *strip, const LedBar *bar, float time) { time *= bar->speed; @@ -332,6 +370,15 @@ static void led_strip_animate(Leds *leds, const LedStrip *strip, const LedBar *b case LED_ANIM_FELONY: anim_felony(leds, strip, bar, time); break; + case LED_ANIM_RAINBOW_CYCLE: + anim_rainbow_cycle(leds, strip, time); + break; + case LED_ANIM_RAINBOW_FADE: + anim_rainbow_fade(leds, strip, time); + break; + case LED_ANIM_RAINBOW_ROLL: + anim_rainbow_roll(leds, strip, time); + break; } } @@ -606,7 +653,7 @@ static void trans_cipher( } else { // random fade to white uint8_t wf = rnd(j + target_j + 23) % 128 + 80; - color = color_wheel(r) | RGB(wf, wf, wf); + color = hue_to_color(r) | RGB(wf, wf, wf); } } @@ -780,7 +827,7 @@ void leds_setup(Leds *leds, CfgHwLeds *hw_cfg, const CfgLeds *cfg, FootpadSensor log_msg("Both sensors pressed, not initializing LEDs."); } else if (leds->front_strip.length + leds->rear_strip.length > LEDS_FRONT_AND_REAR_COUNT_MAX) { log_error("Front and rear LED counts exceed maximum."); - } else if (hw_cfg->mode == LED_MODE_INTERNAL && led_count > 0) { + } else if (hw_cfg->mode & LED_MODE_INTERNAL && led_count > 0) { led_data = VESC_IF->malloc(sizeof(uint32_t) * led_count); if (!led_data) { log_error("Failed to init LED data, out of memory."); @@ -813,7 +860,7 @@ void leds_setup(Leds *leds, CfgHwLeds *hw_cfg, const CfgLeds *cfg, FootpadSensor leds_configure(leds, cfg); if (led_data) { - if (led_driver_setup(&leds->led_driver, hw_cfg->pin, strip_array)) { + if (led_driver_setup(&leds->led_driver, hw_cfg->pin, hw_cfg->pin_config, strip_array)) { leds->led_data = led_data; } else { VESC_IF->free(led_data); diff --git a/refloat/src/lib/circular_buffer.c b/refloat/src/lib/circular_buffer.c new file mode 100644 index 00000000..5bd837e1 --- /dev/null +++ b/refloat/src/lib/circular_buffer.c @@ -0,0 +1,102 @@ +// Copyright 2025 Lukas Hrazky +// +// This file is part of the Refloat VESC package. +// +// Refloat VESC package 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. +// +// Refloat VESC package 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 . + +#include "circular_buffer.h" + +#include + +void circular_buffer_clear(CircularBuffer *cb) { + cb->head = 0; + cb->tail = 0; + cb->empty = true; +} + +void circular_buffer_init(CircularBuffer *cb, size_t item_size, size_t item_number, void *buffer) { + cb->buffer = buffer; + cb->length = item_number; + cb->item_size = item_size; + circular_buffer_clear(cb); +} + +static inline void increment(const CircularBuffer *cb, size_t *i) { + ++(*i); + if (*i >= cb->length) { + *i -= cb->length; + } +} + +size_t circular_buffer_size(const CircularBuffer *cb) { + size_t size = cb->head - cb->tail; + if (size > cb->length) { + return size + cb->length; + } else if (size == 0 && !cb->empty) { + return cb->length; + } + return size; +} + +void circular_buffer_push(CircularBuffer *cb, const void *item) { + if (!cb->empty && cb->head == cb->tail) { + increment(cb, &cb->tail); + } + + memcpy(cb->buffer + cb->head * cb->item_size, item, cb->item_size); + increment(cb, &cb->head); + + cb->empty = false; +} + +bool circular_buffer_get(const CircularBuffer *cb, size_t i, void *item) { + if (i >= circular_buffer_size(cb)) { + return false; + } + + size_t offset = cb->tail + i; + if (offset >= cb->length) { + offset -= cb->length; + } + memcpy(item, cb->buffer + offset * cb->item_size, cb->item_size); + + return true; +} + +bool circular_buffer_pop(CircularBuffer *cb, size_t i, void *item) { + if (!circular_buffer_get(cb, i, item)) { + return false; + } + + increment(cb, &cb->tail); + if (cb->tail == cb->head) { + cb->empty = true; + } + + return true; +} + +void circular_buffer_iterate( + const CircularBuffer *cb, void (*callback)(const void *item, void *data), void *data +) { + if (cb->empty) { + return; + } + + size_t i = cb->tail; + do { + callback(&cb->buffer[i], data); + increment(cb, &i); + } while (i != cb->head); +} diff --git a/refloat/src/lib/circular_buffer.h b/refloat/src/lib/circular_buffer.h new file mode 100644 index 00000000..df5c5529 --- /dev/null +++ b/refloat/src/lib/circular_buffer.h @@ -0,0 +1,67 @@ +// Copyright 2025 Lukas Hrazky +// +// This file is part of the Refloat VESC package. +// +// Refloat VESC package 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. +// +// Refloat VESC package 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 . + +#pragma once + +#include +#include +#include + +// Note: The circular buffer is not synchronized. Provide your own locking, or +// at least insure it's only being written from a single thread and know that +// data may not be consistent on reading. + +typedef struct { + uint8_t *buffer; + size_t length; + size_t item_size; + size_t head; + size_t tail; + bool empty; +} CircularBuffer; + +void circular_buffer_init(CircularBuffer *cb, size_t item_size, size_t item_number, void *buffer); + +void circular_buffer_clear(CircularBuffer *cb); + +size_t circular_buffer_size(const CircularBuffer *cb); + +/** + * Pushes an item into the buffer. In case it's full, replaces the last item. + */ +void circular_buffer_push(CircularBuffer *cb, const void *item); + +/** + * Gets an item at index i (a position relative to the current tail) from the + * buffer without removing it. If there's no item at that index, leaves item + * unchanged and returns false, otherwise returns true. + */ +bool circular_buffer_get(const CircularBuffer *cb, size_t i, void *item); + +/** + * Pops an item at index i (a position relative to the current tail) from the + * buffer. If there's no item at that index, leaves item unchanged and returns + * false, otherwise returns true. + */ +bool circular_buffer_pop(CircularBuffer *cb, size_t i, void *item); + +/** + * Iterates over the buffer, calling the callback function on each item. + */ +void circular_buffer_iterate( + const CircularBuffer *cb, void (*callback)(const void *item, void *data), void *data +); diff --git a/refloat/src/main.c b/refloat/src/main.c index b20399d0..befba005 100644 --- a/refloat/src/main.c +++ b/refloat/src/main.c @@ -22,6 +22,7 @@ #include "vesc_c_if.h" #include "atr.h" +#include "bms.h" #include "booster.h" #include "brake_tilt.h" #include "charging.h" @@ -67,7 +68,16 @@ typedef enum { BEEP_SENSORS = 7, BEEP_LOWBATT = 8, BEEP_IDLE = 9, - BEEP_ERROR = 10 + BEEP_ERROR = 10, + BEEP_SPEED = 11, + BEEP_TEMP_CELL_UNDER = 12, + BEEP_TEMP_CELL_OVER = 13, + BEEP_CELL_LV = 14, + BEEP_CELL_HV = 15, + BEEP_CELL_BALANCE = 16, + BEEP_BMS_CONNECTION = 17, + BEEP_BMS_TEMP_OVER = 18, + BEEP_FW_FAULT = 19 } BeepReason; static const FootpadSensorState flywheel_konami_sequence[] = { @@ -146,23 +156,27 @@ void beep_on(Data *d, bool force) { } static void reconfigure(Data *d) { - d->startup_step_size = d->float_conf.startup_speed / d->float_conf.hertz; - d->noseangling_step_size = d->float_conf.noseangling_speed / d->float_conf.hertz; - d->startup_pitch_trickmargin = d->float_conf.startup_dirtylandings_enabled ? 10 : 0; - d->tiltback_variable = - d->float_conf.tiltback_variable / 1000 * sign(d->float_conf.tiltback_variable_max); - d->tiltback_variable_max_erpm = - fabsf(d->float_conf.tiltback_variable_max / d->tiltback_variable); + balance_filter_configure(&d->balance_filter, &d->float_conf); motor_data_configure(&d->motor, d->float_conf.atr_filter / d->float_conf.hertz); motor_control_configure(&d->motor_control, &d->float_conf); - balance_filter_configure(&d->balance_filter, &d->float_conf); + torque_tilt_configure(&d->torque_tilt, &d->float_conf); atr_configure(&d->atr, &d->float_conf); brake_tilt_configure(&d->brake_tilt, &d->float_conf); turn_tilt_configure(&d->turn_tilt, &d->float_conf); remote_configure(&d->remote, &d->float_conf); + haptic_feedback_configure(&d->haptic_feedback, &d->float_conf); + alert_tracker_configure(&d->alert_tracker, &d->float_conf); + + d->startup_step_size = d->float_conf.startup_speed / d->float_conf.hertz; + d->noseangling_step_size = d->float_conf.noseangling_speed / d->float_conf.hertz; + d->startup_pitch_trickmargin = d->float_conf.startup_dirtylandings_enabled ? 10 : 0; + d->tiltback_variable = + d->float_conf.tiltback_variable / 1000 * sign(d->float_conf.tiltback_variable_max); + d->tiltback_variable_max_erpm = + fabsf(d->float_conf.tiltback_variable_max / d->tiltback_variable); time_refresh_idle(&d->time); } @@ -195,11 +209,6 @@ static void configure(Data *d) { VESC_IF->set_cfg_float(CFG_PARAM_IMU_accel_confidence_decay, 0.1); } - d->mc_max_temp_fet = VESC_IF->get_cfg_float(CFG_PARAM_l_temp_fet_start) - 3; - d->mc_max_temp_mot = VESC_IF->get_cfg_float(CFG_PARAM_l_temp_motor_start) - 3; - - d->max_duty_with_margin = VESC_IF->get_cfg_float(CFG_PARAM_l_max_duty) - 0.05; - // Feature: Reverse Stop d->reverse_tolerance = 20000; d->reverse_stop_step_size = 100.0 / d->float_conf.hertz; @@ -225,14 +234,15 @@ static void leds_headlights_switch(CfgLeds *cfg_leds, LcmData *lcm, bool headlig static void reset_runtime_vars(Data *d) { motor_data_reset(&d->motor); + pid_init(&d->pid); + + torque_tilt_reset(&d->torque_tilt); atr_reset(&d->atr); brake_tilt_reset(&d->brake_tilt); - torque_tilt_reset(&d->torque_tilt); turn_tilt_reset(&d->turn_tilt); remote_reset(&d->remote); booster_reset(&d->booster); - pid_init(&d->pid); d->balance_current = 0; // Set values for startup @@ -303,9 +313,12 @@ static float get_setpoint_adjustment_step_size(Data *d) { return d->tiltback_duty_step_size; case (SAT_PB_HIGH_VOLTAGE): case (SAT_PB_TEMPERATURE): + case (SAT_PB_ERROR): return d->tiltback_hv_step_size; case (SAT_PB_LOW_VOLTAGE): return d->tiltback_lv_step_size; + case (SAT_PB_SPEED): + return d->tiltback_duty_step_size; default: return 0; } @@ -402,8 +415,9 @@ static bool check_faults(Data *d) { } } - if (d->motor.abs_erpm < 200 && fabsf(d->imu.pitch) > 14 && - fabsf(d->remote.setpoint) < 30 && sign(d->imu.pitch) == d->motor.erpm_sign) { + if (d->float_conf.enable_quickstop && d->motor.abs_erpm < 200 && + fabsf(d->imu.pitch) > 14 && fabsf(d->remote.setpoint) < 30 && + sign(d->imu.pitch) == d->motor.erpm_sign) { state_stop(&d->state, STOP_QUICKSTOP); return true; } @@ -495,7 +509,8 @@ static bool check_faults(Data *d) { } static void calculate_setpoint_target(Data *d) { - if (d->motor.batt_voltage < d->float_conf.tiltback_hv) { + if (d->motor.batt_voltage < d->motor.hv_threshold && + !bms_is_fault(&d->bms, BMSF_CELL_OVER_VOLTAGE)) { timer_refresh(&d->time, &d->tb_highvoltage_timer); } @@ -516,7 +531,6 @@ static void calculate_setpoint_target(Data *d) { d->state.sat = SAT_NONE; d->reverse_total_erpm = 0; d->setpoint_target = 0; - pid_reset_integral(&d->pid); } } } @@ -551,7 +565,7 @@ static void calculate_setpoint_target(Data *d) { d->traction_control = false; } // Remain in wheelslip state for a bit to avoid any overreactions - if (d->motor.duty_cycle > d->max_duty_with_margin) { + if (d->motor.duty_cycle > d->motor.duty_max_with_margin) { timer_refresh(&d->time, &d->wheelslip_timer); } else if (timer_older(&d->time, d->wheelslip_timer, 0.2)) { if (d->motor.duty_raw < 0.85) { @@ -575,11 +589,18 @@ static void calculate_setpoint_target(Data *d) { if (d->state.mode != MODE_FLYWHEEL) { d->state.sat = SAT_PB_DUTY; } - } else if (d->motor.duty_cycle > 0.05 && d->motor.batt_voltage > d->float_conf.tiltback_hv) { - d->beep_reason = BEEP_HV; + } else if (d->motor.duty_cycle > 0.05 && + (d->motor.batt_voltage > d->motor.hv_threshold || + bms_is_fault(&d->bms, BMSF_CELL_OVER_VOLTAGE))) { + if (bms_is_fault(&d->bms, BMSF_CELL_OVER_VOLTAGE)) { + d->beep_reason = BEEP_CELL_HV; + } else { + d->beep_reason = BEEP_HV; + } beep_alert(d, 3, false); if (timer_older(&d->time, d->tb_highvoltage_timer, 0.5) || - d->motor.batt_voltage > d->float_conf.tiltback_hv + 1) { + d->motor.batt_voltage > d->motor.hv_threshold + 1 || + bms_is_fault(&d->bms, BMSF_CELL_OVER_VOLTAGE)) { // 500ms have passed or voltage is another volt higher, time for some tiltback if (d->motor.erpm > 0) { d->setpoint_target = d->float_conf.tiltback_hv_angle; @@ -592,11 +613,21 @@ static void calculate_setpoint_target(Data *d) { // The rider has 500ms to react to the triple-beep, or maybe it was just a short spike d->state.sat = SAT_NONE; } - } else if (d->motor.mosfet_temp > d->mc_max_temp_fet) { + } else if (bms_is_fault(&d->bms, BMSF_CONNECTION)) { + beep_alert(d, 3, true); + d->beep_reason = BEEP_BMS_CONNECTION; + + if (d->motor.erpm > 0) { + d->setpoint_target = d->float_conf.tiltback_hv_angle; + } else { + d->setpoint_target = -d->float_conf.tiltback_hv_angle; + } + d->state.sat = SAT_PB_ERROR; + } else if (d->motor.mosfet_temp > d->motor.mosfet_temp_max) { // Use the angle from Low-Voltage tiltback, but slower speed from High-Voltage tiltback beep_alert(d, 3, true); d->beep_reason = BEEP_TEMPFET; - if (d->motor.mosfet_temp > d->mc_max_temp_fet + 1) { + if (d->motor.mosfet_temp > d->motor.mosfet_temp_max + 1) { if (d->motor.erpm > 0) { d->setpoint_target = d->float_conf.tiltback_lv_angle; } else { @@ -607,11 +638,11 @@ static void calculate_setpoint_target(Data *d) { // The rider has 1 degree Celsius left before we start tilting back d->state.sat = SAT_NONE; } - } else if (d->motor.motor_temp > d->mc_max_temp_mot) { + } else if (d->motor.motor_temp > d->motor.motor_temp_max) { // Use the angle from Low-Voltage tiltback, but slower speed from High-Voltage tiltback beep_alert(d, 3, true); d->beep_reason = BEEP_TEMPMOT; - if (d->motor.motor_temp > d->mc_max_temp_mot + 1) { + if (d->motor.motor_temp > d->motor.motor_temp_max + 1) { if (d->motor.erpm > 0) { d->setpoint_target = d->float_conf.tiltback_lv_angle; } else { @@ -622,17 +653,42 @@ static void calculate_setpoint_target(Data *d) { // The rider has 1 degree Celsius left before we start tilting back d->state.sat = SAT_NONE; } - } else if (d->motor.duty_cycle > 0.05 && d->motor.batt_voltage < d->float_conf.tiltback_lv) { + } else if (bms_is_fault(&d->bms, BMSF_CELL_OVER_TEMP) || + bms_is_fault(&d->bms, BMSF_CELL_UNDER_TEMP) || + bms_is_fault(&d->bms, BMSF_OVER_TEMP)) { + // Use the angle from Low-Voltage tiltback, but slower speed from High-Voltage tiltback + beep_alert(d, 3, true); + if (bms_is_fault(&d->bms, BMSF_CELL_OVER_TEMP)) { + d->beep_reason = BEEP_TEMP_CELL_OVER; + } else if (bms_is_fault(&d->bms, BMSF_CELL_UNDER_TEMP)) { + d->beep_reason = BEEP_TEMP_CELL_UNDER; + } else { + d->beep_reason = BEEP_BMS_TEMP_OVER; + } + if (d->motor.erpm > 0) { + d->setpoint_target = d->float_conf.tiltback_lv_angle; + } else { + d->setpoint_target = -d->float_conf.tiltback_lv_angle; + } + d->state.sat = SAT_PB_TEMPERATURE; + } else if (d->motor.duty_cycle > 0.05 && + (d->motor.batt_voltage < d->motor.lv_threshold || + bms_is_fault(&d->bms, BMSF_CELL_UNDER_VOLTAGE))) { beep_alert(d, 3, false); - d->beep_reason = BEEP_LV; + if (bms_is_fault(&d->bms, BMSF_CELL_UNDER_VOLTAGE)) { + d->beep_reason = BEEP_CELL_LV; + } else { + d->beep_reason = BEEP_LV; + } float abs_motor_current = fabsf(d->motor.dir_current); - float vdelta = d->float_conf.tiltback_lv - d->motor.batt_voltage; + float vdelta = d->motor.lv_threshold - d->motor.batt_voltage; float ratio = vdelta * 20 / abs_motor_current; // When to do LV tiltback: // a) we're 2V below lv threshold // b) motor current is small (we cannot assume vsag) // c) we have more than 20A per Volt of difference (we tolerate some amount of vsag) - if ((vdelta > 2) || (abs_motor_current < 5) || (ratio > 1)) { + if ((vdelta > 2) || (abs_motor_current < 5) || (ratio > 1) || + bms_is_fault(&d->bms, BMSF_CELL_UNDER_VOLTAGE)) { if (d->motor.erpm > 0) { d->setpoint_target = d->float_conf.tiltback_lv_angle; } else { @@ -644,13 +700,22 @@ static void calculate_setpoint_target(Data *d) { d->state.sat = SAT_NONE; d->setpoint_target = 0; } + } else if (d->float_conf.tiltback_speed > 0.0 && + fabsf(d->motor.speed) > d->float_conf.tiltback_speed) { + if (d->motor.speed > 0) { + d->setpoint_target = d->float_conf.tiltback_duty_angle; + } else { + d->setpoint_target = -d->float_conf.tiltback_duty_angle; + } + d->beep_reason = BEEP_SPEED; + d->state.sat = SAT_PB_SPEED; } else { // Normal running d->state.sat = SAT_NONE; d->setpoint_target = 0; } - if (d->state.wheelslip && d->motor.duty_cycle > d->max_duty_with_margin) { + if (d->state.wheelslip && d->motor.duty_cycle > d->motor.duty_max_with_margin) { d->setpoint_target = 0; } if (d->state.darkride) { @@ -747,9 +812,22 @@ static void refloat_thd(void *arg) { } haptic_feedback_update( - &d->haptic_feedback, &d->motor_control, &d->state, &d->motor, &d->time + &d->haptic_feedback, + &d->motor_control, + &d->state, + &d->motor, + &d->alert_tracker, + &d->time ); + bms_update(&d->bms, &d->float_conf.bms, &d->time); + + motor_data_evaluate_alerts(&d->motor, &d->alert_tracker, &d->time); + alert_tracker_finalize(&d->alert_tracker, &d->time); + if (alert_tracker_is_alert_active(&d->alert_tracker, ALERT_FW_FAULT)) { + d->beep_reason = BEEP_FW_FAULT; + } + // Control Loop State Logic switch (d->state.state) { case (STATE_STARTUP): @@ -759,7 +837,7 @@ static void refloat_thd(void *arg) { d->state.state = STATE_READY; // if within 5V of LV tiltback threshold, issue 1 beep for each volt below that - float threshold = d->float_conf.tiltback_lv + 5; + float threshold = d->motor.lv_threshold + 5; if (d->motor.batt_voltage < threshold) { int beeps = (int) fminf(6, threshold - d->motor.batt_voltage); beep_alert(d, beeps + 1, true); @@ -803,22 +881,13 @@ static void refloat_thd(void *arg) { // in case of wheelslip, don't change torque tilts, instead slightly decrease each // cycle if (d->state.wheelslip) { + turn_tilt_winddown(&d->turn_tilt); torque_tilt_winddown(&d->torque_tilt); atr_winddown(&d->atr); brake_tilt_winddown(&d->brake_tilt); } else { apply_noseangling(d); - d->setpoint += d->noseangling_interpolated; - - turn_tilt_update( - &d->turn_tilt, - &d->motor, - &d->atr, - d->imu.balance_pitch, - d->noseangling_interpolated, - &d->float_conf - ); - d->setpoint += d->turn_tilt.setpoint; + turn_tilt_update(&d->turn_tilt, &d->motor, &d->float_conf); torque_tilt_update(&d->torque_tilt, &d->motor, &d->float_conf); atr_update(&d->atr, &d->motor, &d->float_conf); @@ -831,6 +900,9 @@ static void refloat_thd(void *arg) { ); } + d->setpoint += d->noseangling_interpolated; + d->setpoint += d->turn_tilt.setpoint; + // aggregated torque tilts: // if signs match between torque tilt and ATR + brake tilt, use the more significant // one if signs do not match, they are simply added together @@ -843,7 +915,7 @@ static void refloat_thd(void *arg) { } } - pid_update(&d->pid, d->setpoint, &d->motor, &d->imu, &d->state, &d->float_conf); + pid_update(&d->pid, d->setpoint, &d->motor, &d->imu, &d->float_conf); float booster_proportional = d->setpoint - d->brake_tilt.setpoint - d->imu.pitch; booster_update(&d->booster, &d->motor, &d->float_conf, booster_proportional); @@ -857,7 +929,14 @@ static void refloat_thd(void *arg) { } float new_current = d->pid.p + d->pid.i + pitch_based; - float current_limit = d->motor.braking ? d->motor.current_min : d->motor.current_max; + float current_limit; + if (d->state.mode == MODE_HANDTEST) { + current_limit = 7; + } else if (d->state.mode == MODE_FLYWHEEL) { + current_limit = 40; + } else { + current_limit = d->motor.braking ? d->motor.current_min : d->motor.current_max; + } if (fabsf(new_current) > current_limit) { new_current = sign(new_current) * current_limit; } @@ -912,6 +991,22 @@ static void refloat_thd(void *arg) { d->state.darkride = false; } + // alert if cells are out of balance or bms connection failed + if (bms_is_fault(&d->bms, BMSF_CONNECTION) || + (bms_is_fault(&d->bms, BMSF_CELL_BALANCE) && time_elapsed(&d->time, disengage, 5) + )) { + if (timer_older(&d->time, d->alert_timer, 15)) { + timer_refresh(&d->time, &d->alert_timer); + beep_alert(d, 4, false); + + if (bms_is_fault(&d->bms, BMSF_CONNECTION)) { + d->beep_reason = BEEP_BMS_CONNECTION; + } else { + d->beep_reason = BEEP_CELL_BALANCE; + } + } + } + if (time_elapsed(&d->time, idle, 1800)) { // alert user after 30 minutes if (timer_older(&d->time, d->nag_timer, 60)) { // beep every 60 seconds timer_refresh(&d->time, &d->nag_timer); @@ -962,8 +1057,12 @@ static void refloat_thd(void *arg) { if ((fabsf(d->imu.balance_pitch) < 45) && (fabsf(d->imu.roll) < 45)) { // 45 to prevent board engaging when upright or laying sideways // 45 degree tolerance is more than plenty for tricks / extreme mounts - engage(d); - break; + if (d->float_conf.fault_reversestop_enabled && d->motor.erpm < 0) { + // ignore push start backwards if reverse stop is enabled + } else { + engage(d); + break; + } } } @@ -975,26 +1074,42 @@ static void refloat_thd(void *arg) { motor_control_apply(&d->motor_control, d->motor.abs_erpm_smooth, d->state.state, &d->time); - data_recorder_sample(&d->data_record, d); + data_recorder_sample(&d->data_record, d, d->time.now); VESC_IF->sleep_us(d->loop_time_us); } } +// TODO: The required buffer size is not provided by the confparser. Until it's +// added, use a number that should always be bigger. On a config write, we +// check if we've written past the buffer end and crash. On a config read, +// there's no way to check and the trailing end of the config will have bogus +// numbers read from the EEPROM. +#ifndef SERIALIZED_CONFIG_LENGTH +#define SERIALIZED_CONFIG_LENGTH 320 +#endif + static void write_cfg_to_eeprom(Data *d) { - uint32_t ints = sizeof(RefloatConfig) / 4 + 1; - uint32_t *buffer = VESC_IF->malloc(ints * sizeof(uint32_t)); + const size_t words = (SERIALIZED_CONFIG_LENGTH - 1) / 4 + 1; + const size_t bufsize = words * 4; + uint32_t *buffer = VESC_IF->malloc(bufsize); if (!buffer) { - log_error("Failed to write config to EEPROM: Out of memory."); + log_error("Failed to write config: Out of memory."); return; } + memset(buffer, 0, bufsize); + + uint32_t written_bytes = confparser_serialize_refloatconfig((uint8_t *) buffer, &d->float_conf); + if (written_bytes > bufsize) { + log_error("Config write buffer overflow, terminating."); + fatal_error_terminate(); + } bool write_ok = true; - memcpy(buffer, &(d->float_conf), sizeof(RefloatConfig)); - for (uint32_t i = 0; i < ints; i++) { + for (uint32_t i = 0; i < words; ++i) { eeprom_var v; v.as_u32 = buffer[i]; - if (!VESC_IF->store_eeprom_var(&v, i + 1)) { + if (!VESC_IF->store_eeprom_var(&v, i)) { write_ok = false; break; } @@ -1003,20 +1118,23 @@ static void write_cfg_to_eeprom(Data *d) { VESC_IF->free(buffer); if (write_ok) { - eeprom_var v; - v.as_u32 = REFLOATCONFIG_SIGNATURE; - VESC_IF->store_eeprom_var(&v, 0); - + log_msg("Config written: %uB", written_bytes); beep_alert(d, 1, 0); leds_status_confirm(&d->leds); } else { - log_error("Failed to write config to EEPROM."); + log_error("Failed to write config."); } } static void aux_thd(void *arg) { Data *d = (Data *) arg; + if (VESC_IF->thread_set_priority) { + VESC_IF->thread_set_priority(-1); + } + + time_t motor_config_refresh_timer = 0; + while (!VESC_IF->should_terminate()) { leds_update(&d->leds, &d->state, d->footpad.state); @@ -1026,40 +1144,42 @@ static void aux_thd(void *arg) { d->odometer = VESC_IF->mc_get_odometer(); } + if (timer_older(&d->time, motor_config_refresh_timer, 0.5)) { + motor_data_refresh_motor_config( + &d->motor, d->float_conf.tiltback_lv, d->float_conf.tiltback_hv + ); + timer_refresh(&d->time, &motor_config_refresh_timer); + } + VESC_IF->sleep_us(1e6 / LEDS_REFRESH_RATE); } } static void read_cfg_from_eeprom(Data *d) { - uint32_t ints = sizeof(RefloatConfig) / 4 + 1; - uint32_t *buffer = VESC_IF->malloc(ints * sizeof(uint32_t)); + uint32_t words = (SERIALIZED_CONFIG_LENGTH - 1) / 4 + 1; + uint32_t *buffer = VESC_IF->malloc(words * sizeof(uint32_t)); if (!buffer) { - log_error("Failed to read config from EEPROM: Out of memory."); + log_error("Failed to read config: Out of memory."); return; } eeprom_var v; - bool read_ok = VESC_IF->read_eeprom_var(&v, 0); - if (read_ok) { - if (v.as_u32 == REFLOATCONFIG_SIGNATURE) { - for (uint32_t i = 0; i < ints; i++) { - if (!VESC_IF->read_eeprom_var(&v, i + 1)) { - read_ok = false; - break; - } - buffer[i] = v.as_u32; - } - } else { - log_error("Failed signature check while reading config from EEPROM, using defaults."); - confparser_set_defaults_refloatconfig(&d->float_conf); - return; + bool read_ok = true; + for (uint32_t i = 0; i < words; ++i) { + if (!VESC_IF->read_eeprom_var(&v, i)) { + read_ok = false; + break; } + buffer[i] = v.as_u32; } if (read_ok) { - memcpy(&d->float_conf, buffer, sizeof(RefloatConfig)); + if (!confparser_deserialize_refloatconfig((uint8_t *) buffer, &d->float_conf)) { + log_error("Failed to deserialize config, using defaults."); + confparser_set_defaults_refloatconfig(&d->float_conf); + } } else { - log_error("Failed to read config from EEPROM, using defaults."); + log_error("Failed to read config, using defaults."); confparser_set_defaults_refloatconfig(&d->float_conf); } @@ -1071,18 +1191,31 @@ static void data_init(Data *d) { read_cfg_from_eeprom(d); - d->odometer = VESC_IF->mc_get_odometer(); - balance_filter_init(&d->balance_filter); - state_init(&d->state); + time_init(&d->time); + motor_data_init(&d->motor); + imu_init(&d->imu); pid_init(&d->pid); motor_control_init(&d->motor_control); + + torque_tilt_init(&d->torque_tilt); + atr_init(&d->atr); + brake_tilt_init(&d->brake_tilt); + turn_tilt_init(&d->turn_tilt); + booster_init(&d->booster); + remote_init(&d->remote); + + state_init(&d->state); + footpad_sensor_init(&d->footpad); haptic_feedback_init(&d->haptic_feedback); + alert_tracker_init(&d->alert_tracker); + + leds_init(&d->leds); lcm_init(&d->lcm, &d->float_conf.hardware.leds); charging_init(&d->charging); - remote_init(&d->remote); - leds_init(&d->leds); + bms_init(&d->bms); + data_recorder_init(&d->data_record); konami_init(&d->flywheel_konami, flywheel_konami_sequence, sizeof(flywheel_konami_sequence)); @@ -1096,33 +1229,8 @@ static void data_init(Data *d) { headlights_off_konami_sequence, sizeof(headlights_off_konami_sequence) ); -} - -static float app_get_debug(int index) { - Data *d = (Data *) ARG; - switch (index) { - case (1): - return d->balance_current; - case (2): - return d->pid.p; - case (3): - return d->pid.i; - case (4): - return d->pid.rate_p; - case (5): - return d->setpoint; - case (6): - return d->atr.setpoint; - case (7): - return d->motor.erpm; - case (8): - return d->motor.dir_current; - case (9): - return d->motor.filt_current; - default: - return 0; - } + d->odometer = VESC_IF->mc_get_odometer(); } // See also: @@ -1144,13 +1252,15 @@ enum { COMMAND_LOCK = 12, COMMAND_HANDTEST = 13, COMMAND_TUNE_TILT = 14, + COMMAND_LIGHTS_CONTROL = 20, COMMAND_FLYWHEEL = 22, COMMAND_REALTIME_DATA = 31, COMMAND_REALTIME_DATA_IDS = 32, + COMMAND_ALERTS_LIST = 35, + COMMAND_ALERTS_CONTROL = 36, COMMAND_DATA_RECORD_REQUEST = 41, // commands above 200 are unstable and can change protocol at any time - COMMAND_LIGHTS_CONTROL = 202, } Commands; static void send_realtime_data(Data *d) { @@ -1318,8 +1428,6 @@ static void cmd_handtest(Data *d, unsigned char *cfg) { d->state.mode = cfg[0] ? MODE_HANDTEST : MODE_NORMAL; if (d->state.mode == MODE_HANDTEST) { - // temporarily reduce max currents to make hand test safer / gentler - d->motor.current_max = d->motor.current_min = 7; // Disable I-term and all tune modifiers and tilts d->float_conf.ki = 0; d->float_conf.kp_brake = 1; @@ -1447,15 +1555,7 @@ static void cmd_runtime_tune(Data *d, unsigned char *cfg, int len) { d->float_conf.braketilt_strength = h1; d->float_conf.braketilt_lingering = h2; - split(cfg[11], &h1, &h2); - d->motor.current_max = h1 * 5 + 55; - d->motor.current_min = h2 * 5 + 55; - if (h1 == 0) { - d->motor.current_max = VESC_IF->get_cfg_float(CFG_PARAM_l_current_max); - } - if (h2 == 0) { - d->motor.current_min = fabsf(VESC_IF->get_cfg_float(CFG_PARAM_l_current_min)); - } + // cfg[11] used to be current min and max limits } if (len >= 16) { split(cfg[12], &h1, &h2); @@ -1741,9 +1841,6 @@ static void cmd_flywheel_toggle(Data *d, unsigned char *cfg, int len) { d->tiltback_duty_step_size = d->float_conf.tiltback_duty_speed / d->float_conf.hertz; d->tiltback_return_step_size = d->float_conf.tiltback_return_speed / d->float_conf.hertz; - // Limit amps - d->motor.current_max = d->motor.current_min = 40; - // Disable I-term and all tune modifiers and tilts d->float_conf.ki = 0; d->float_conf.kp_brake = 1; @@ -1804,7 +1901,7 @@ static void cmd_realtime_data_ids() { } static void cmd_realtime_data(Data *d) { - static const int bufsize = 79; + static const int bufsize = 16 + ITEMS_COUNT(RT_DATA_ALL_ITEMS) * 2 + 9; uint8_t buffer[bufsize]; int32_t ind = 0; @@ -1822,10 +1919,13 @@ static void cmd_realtime_data(Data *d) { mask |= 0x2; } + mask |= 0x4; // Append active alerts mask, always appended from now on + buffer[ind++] = mask; const DataRecord *rd = &d->data_record; - uint8_t extra_flags = rd->autostop << 2 | rd->autostart << 1 | rd->recording; + uint8_t extra_flags = + d->alert_tracker.fatal_error << 3 | rd->autostop << 2 | rd->autostart << 1 | rd->recording; buffer[ind++] = extra_flags; buffer_append_uint32(buffer, d->time.now, &ind); @@ -1852,18 +1952,96 @@ static void cmd_realtime_data(Data *d) { buffer_append_float16_auto(buffer, d->charging.voltage, &ind); } + buffer_append_uint32(buffer, d->alert_tracker.active_alert_mask, &ind); + buffer_append_uint32(buffer, 0, &ind); // extra 32 bits for more flags if needed + buffer[ind++] = d->alert_tracker.fw_fault_code; + SEND_APP_DATA(buffer, bufsize, ind); } +static void buffer_append_fault_name(uint8_t *buffer, mc_fault_code code, int32_t *index) { + const char *str = VESC_IF->mc_fault_to_string(code); + uint32_t length = strlen(str); + if (length > 11 && str[0] == 'F') { + str += 11; + } + buffer_append_string_max(buffer, str, index, 50); +} + +static void cmd_alerts_list(const AlertTracker *at, uint8_t *buf, size_t len) { + // Note: Accessing the circular buffer from multiple threads is racey. + // Needs locking, but locking in the control loop is undesirable. Once + // alert checking is outside the control loop thread, locking can be added. + time_t since = 0; + if (len >= 4) { + int32_t ind = 0; + since = buffer_get_uint32(buf, &ind); + } + + uint8_t buffer[SEND_BUF_MAX_SIZE]; + int32_t ind = 0; + + buffer[ind++] = 101; // Package ID + buffer[ind++] = COMMAND_ALERTS_LIST; + + buffer_append_uint32(buffer, at->active_alert_mask, &ind); + buffer_append_uint32(buffer, 0, &ind); // extra 32 bits for more flags if needed + buffer[ind++] = at->fw_fault_code; + if (at->fw_fault_code != FAULT_CODE_NONE) { + buffer_append_fault_name(buffer, at->fw_fault_code, &ind); + } + + int size_index = ind++; + uint8_t size = circular_buffer_size(&at->alert_buffer); + uint8_t real_size = 0; + for (uint8_t i = 0; i < size; ++i) { + AlertRecord alert; + circular_buffer_get(&at->alert_buffer, i, &alert); + + if (alert.time > since) { + // 7 bytes fixed and at most 50 bytes string with one byte for size + if (ind + 7 + 51 > SEND_BUF_MAX_SIZE) { + // if we can't fit into the message buffer, we only send what we can + break; + } + + buffer_append_uint32(buffer, alert.time, &ind); + buffer[ind++] = alert.id; + buffer[ind++] = alert.active; + buffer[ind++] = alert.code; + if (alert.code != FAULT_CODE_NONE) { + buffer_append_fault_name(buffer, alert.code, &ind); + } + ++real_size; + } + } + + buffer[size_index] = real_size; + + SEND_APP_DATA(buffer, SEND_BUF_MAX_SIZE, ind); +} + +static void cmd_alerts_control(AlertTracker *at, uint8_t *buf, size_t len) { + if (len < 1) { + return; + } + uint8_t command = buf[0]; + if (command == 1) { // clear fatal error + alert_tracker_clear_fatal(at); + } +} + static void lights_control_request(CfgLeds *leds, uint8_t *buffer, size_t len, LcmData *lcm) { - if (len < 2) { + if (len < 5) { return; } - uint8_t mask = buffer[0]; - uint8_t value = buffer[1]; + int32_t ind = 0; + uint32_t mask = buffer_get_uint32(buffer, &ind); + + if ((mask & 0xff) != 0) { + uint8_t value = buffer[ind++]; - if (mask != 0) { if (mask & 0x1) { leds->on = value & 0x1; } @@ -1909,7 +2087,7 @@ static void cmd_info(const Data *d, unsigned char *buf, int len) { // Backwards compatibility for the LED type - external used to be 3 uint8_t led_type = d->float_conf.hardware.leds.mode; - if (led_type == LED_MODE_EXTERNAL) { + if (led_type & LED_MODE_EXTERNAL) { led_type = 3; } @@ -1945,7 +2123,7 @@ static void cmd_info(const Data *d, unsigned char *buf, int len) { } if (d->float_conf.hardware.leds.mode != LED_MODE_OFF) { capabilities |= 1; - if (d->float_conf.hardware.leds.mode == LED_MODE_EXTERNAL) { + if (d->float_conf.hardware.leds.mode & LED_MODE_EXTERNAL) { capabilities |= 1 << 1; } } @@ -2106,6 +2284,14 @@ static void on_command_received(unsigned char *buffer, unsigned int len) { data_recorder_request(&d->data_record, &buffer[2], len - 2); return; } + case COMMAND_ALERTS_LIST: { + cmd_alerts_list(&d->alert_tracker, &buffer[2], len - 2); + return; + } + case COMMAND_ALERTS_CONTROL: { + cmd_alerts_control(&d->alert_tracker, &buffer[2], len - 2); + return; + } default: { if (!VESC_IF->app_is_output_disabled()) { log_error("Unknown command received: %u", command); @@ -2114,15 +2300,6 @@ static void on_command_received(unsigned char *buffer, unsigned int len) { } } -// Register get_debug as a lisp extension -static lbm_value ext_dbg(lbm_value *args, lbm_uint argn) { - if (argn != 1 || !VESC_IF->lbm_is_number(args[0])) { - return VESC_IF->lbm_enc_sym_eerror; - } - - return VESC_IF->lbm_enc_float(app_get_debug(VESC_IF->lbm_dec_as_i32(args[0]))); -} - // Called from Lisp on init to pass in the version info of the firmware static lbm_value ext_set_fw_version(lbm_value *args, lbm_uint argn) { Data *d = (Data *) ARG; @@ -2134,6 +2311,25 @@ static lbm_value ext_set_fw_version(lbm_value *args, lbm_uint argn) { return VESC_IF->lbm_enc_sym_true; } +// Called from Lisp to pass in values from the bms. Stores BMS values if +// arguments are passed in and BMS integration is enabled. +// +// Always returns whether BMS integration is enabled. +static lbm_value ext_bms(lbm_value *args, lbm_uint argn) { + Data *d = (Data *) ARG; + + if (argn > 5 && d->float_conf.bms.enabled) { + d->bms.cell_lv = VESC_IF->lbm_dec_as_float(args[0]); + d->bms.cell_hv = VESC_IF->lbm_dec_as_float(args[1]); + d->bms.cell_lt = VESC_IF->lbm_dec_as_i32(args[2]); + d->bms.cell_ht = VESC_IF->lbm_dec_as_i32(args[3]); + d->bms.bms_ht = VESC_IF->lbm_dec_as_i32(args[4]); + d->bms.msg_age = VESC_IF->lbm_dec_as_float(args[5]); + } + + return d->float_conf.bms.enabled ? VESC_IF->lbm_enc_sym_true : VESC_IF->lbm_enc_sym_nil; +} + // Used to send the current or default configuration to VESC Tool. static int get_cfg(uint8_t *buffer, bool is_default) { Data *d = (Data *) ARG; @@ -2226,6 +2422,11 @@ INIT_FUN(lib_info *info) { } data_init(d); + // Periodically called from the aux thread. Do the first refresh here to avoid races. + motor_data_refresh_motor_config( + &d->motor, d->float_conf.tiltback_lv, d->float_conf.tiltback_hv + ); + info->stop_fun = stop; info->arg = d; @@ -2253,12 +2454,12 @@ INIT_FUN(lib_info *info) { VESC_IF->imu_set_read_callback(imu_ref_callback); VESC_IF->conf_custom_add_config(get_cfg, set_cfg, get_cfg_xml); VESC_IF->set_app_data_handler(on_command_received); - VESC_IF->lbm_add_extension("ext-dbg", ext_dbg); VESC_IF->lbm_add_extension("ext-set-fw-version", ext_set_fw_version); + VESC_IF->lbm_add_extension("ext-bms", ext_bms); return true; } -void send_app_data_overflow_terminate() { +void fatal_error_terminate() { stop(ARG); } diff --git a/refloat/src/motor_data.c b/refloat/src/motor_data.c index 8e866429..38311c10 100644 --- a/refloat/src/motor_data.c +++ b/refloat/src/motor_data.c @@ -23,9 +23,45 @@ #include +void motor_data_init(MotorData *m) { + m->erpm = 0.0f; + m->abs_erpm = 0.0f; + m->abs_erpm_smooth = 0.0f; + m->last_erpm = 0.0f; + m->erpm_sign = 1; + + m->speed = 0.0f; + + m->current = 0.0f; + m->dir_current = 0.0f; + m->filt_current = 0.0f; + m->braking = false; + + m->duty_raw = 0.0f; + + m->batt_current = 0.0f; + m->batt_voltage = 0.0f; + + m->mosfet_temp = 0.0f; + m->motor_temp = 0.0f; + + m->current_min = 0.0f; + m->current_max = 0.0f; + m->battery_current_min = 0.0f; + m->battery_current_max = 0.0f; + m->mosfet_temp_max = 0.0f; + m->motor_temp_max = 0.0f; + m->duty_max_with_margin = 0.0f; + m->lv_threshold = 0.0f; + m->hv_threshold = 0.0f; + + m->current_filter_enabled = false; + + motor_data_reset(m); +} + void motor_data_reset(MotorData *m) { - m->abs_erpm_smooth = 0; - m->duty_raw = 0; + m->duty_cycle = 0; m->acceleration = 0; m->accel_idx = 0; @@ -36,19 +72,37 @@ void motor_data_reset(MotorData *m) { biquad_reset(&m->current_biquad); } -void motor_data_configure(MotorData *m, float frequency) { - if (frequency > 0) { - biquad_configure(&m->current_biquad, BQ_LOWPASS, frequency); - m->current_filter_enabled = true; - } else { - m->current_filter_enabled = false; +void motor_data_refresh_motor_config(MotorData *m, float lv_threshold, float hv_threshold) { + uint8_t battery_cells = VESC_IF->get_cfg_int(CFG_PARAM_si_battery_cells); + if (battery_cells > 0) { + if (lv_threshold < 10) { + lv_threshold *= battery_cells; + } + if (hv_threshold < 10) { + hv_threshold *= battery_cells; + } } + m->lv_threshold = lv_threshold; + m->hv_threshold = hv_threshold; + // min motor current is a positive value here! m->current_min = fabsf(VESC_IF->get_cfg_float(CFG_PARAM_l_current_min)); m->current_max = VESC_IF->get_cfg_float(CFG_PARAM_l_current_max); m->battery_current_min = VESC_IF->get_cfg_float(CFG_PARAM_l_in_current_min); m->battery_current_max = VESC_IF->get_cfg_float(CFG_PARAM_l_in_current_max); + m->mosfet_temp_max = VESC_IF->get_cfg_float(CFG_PARAM_l_temp_fet_start) - 3; + m->motor_temp_max = VESC_IF->get_cfg_float(CFG_PARAM_l_temp_motor_start) - 3; + m->duty_max_with_margin = VESC_IF->get_cfg_float(CFG_PARAM_l_max_duty) - 0.05; +} + +void motor_data_configure(MotorData *m, float frequency) { + if (frequency > 0) { + biquad_configure(&m->current_biquad, BQ_LOWPASS, frequency); + m->current_filter_enabled = true; + } else { + m->current_filter_enabled = false; + } } void motor_data_update(MotorData *m) { @@ -61,6 +115,7 @@ void motor_data_update(MotorData *m) { // including four divisions. In theory multiplying by a single constant is // enough, we just need to calculate the constant (and keep it up to date // when motor config changes, there's no way to know, we'll have to poll). + // And it's only possible on 6.05+. m->speed = VESC_IF->mc_get_speed() * 3.6; m->current = VESC_IF->mc_get_tot_current_filtered(); @@ -90,6 +145,15 @@ void motor_data_update(MotorData *m) { m->motor_temp = VESC_IF->mc_temp_motor_filtered(); } +void motor_data_evaluate_alerts(const MotorData *m, AlertTracker *at, const Time *time) { + unused(m); + + mc_fault_code fault_code = VESC_IF->mc_get_fault(); + if (fault_code != FAULT_CODE_NONE) { + alert_tracker_add(at, time, ALERT_FW_FAULT, fault_code); + } +} + float motor_data_get_current_saturation(const MotorData *m) { float motor_saturation = fabsf(m->filt_current) / (m->braking ? m->current_min : m->current_max); diff --git a/refloat/src/motor_data.h b/refloat/src/motor_data.h index 4297da7b..b1a3582d 100644 --- a/refloat/src/motor_data.h +++ b/refloat/src/motor_data.h @@ -17,6 +17,7 @@ #pragma once +#include "alert_tracker.h" #include "biquad.h" #include @@ -50,10 +51,16 @@ typedef struct { float mosfet_temp; float motor_temp; + // The following values are periodically updated from the aux thread float current_min; float current_max; float battery_current_min; float battery_current_max; + float mosfet_temp_max; + float motor_temp_max; + float duty_max_with_margin; + float lv_threshold; + float hv_threshold; float accel_history[ACCEL_ARRAY_SIZE]; uint8_t accel_idx; @@ -62,10 +69,16 @@ typedef struct { Biquad current_biquad; } MotorData; +void motor_data_init(MotorData *m); + void motor_data_reset(MotorData *m); +void motor_data_refresh_motor_config(MotorData *m, float lv_threshold, float hv_threshold); + void motor_data_configure(MotorData *m, float frequency); void motor_data_update(MotorData *m); +void motor_data_evaluate_alerts(const MotorData *m, AlertTracker *at, const Time *time); + float motor_data_get_current_saturation(const MotorData *m); diff --git a/refloat/src/pid.c b/refloat/src/pid.c index 73f0b899..79b9f8b3 100644 --- a/refloat/src/pid.c +++ b/refloat/src/pid.c @@ -35,12 +35,7 @@ void pid_init(PID *pid) { } void pid_update( - PID *pid, - float setpoint, - const MotorData *md, - const IMU *imu, - const State *state, - const RefloatConfig *config + PID *pid, float setpoint, const MotorData *md, const IMU *imu, const RefloatConfig *config ) { pid->p = setpoint - imu->balance_pitch; pid->i = pid->i + pid->p * config->ki; @@ -49,10 +44,6 @@ void pid_update( if (config->ki_limit > 0 && fabsf(pid->i) > config->ki_limit) { pid->i = config->ki_limit * sign(pid->i); } - // quickly ramp down integral component during reverse stop - if (state->sat == SAT_REVERSESTOP) { - pid->i *= 0.9; - } // brake scale coefficient smoothing if (md->abs_erpm < 500) { @@ -80,7 +71,3 @@ void pid_update( pid->rate_p = -imu->pitch_rate * config->kp2; pid->rate_p *= pid->rate_p > 0 ? pid->kp2_accel_scale : pid->kp2_brake_scale; } - -void pid_reset_integral(PID *pid) { - pid->i = 0; -} diff --git a/refloat/src/pid.h b/refloat/src/pid.h index 027000ec..74ab54f5 100644 --- a/refloat/src/pid.h +++ b/refloat/src/pid.h @@ -20,7 +20,6 @@ #include "conf/datatypes.h" #include "imu.h" #include "motor_data.h" -#include "state.h" typedef struct { float p; @@ -37,12 +36,5 @@ typedef struct { void pid_init(PID *pid); void pid_update( - PID *pid, - float setpoint, - const MotorData *md, - const IMU *imu, - const State *state, - const RefloatConfig *config + PID *pid, float setpoint, const MotorData *md, const IMU *imu, const RefloatConfig *config ); - -void pid_reset_integral(PID *pid); diff --git a/refloat/src/state.c b/refloat/src/state.c index e17ee636..39958837 100644 --- a/refloat/src/state.c +++ b/refloat/src/state.c @@ -106,6 +106,10 @@ uint8_t sat_compat(const State *state) { return 5; // TILTBACK_LV case SAT_PB_TEMPERATURE: return 6; // TILTBACK_TEMP + case SAT_PB_SPEED: + return 7; + case SAT_PB_ERROR: + return 8; } return 0; // CENTERING } diff --git a/refloat/src/state.h b/refloat/src/state.h index fd1db6c9..e8e2c30c 100644 --- a/refloat/src/state.h +++ b/refloat/src/state.h @@ -49,7 +49,9 @@ typedef enum { SAT_NONE = 0, SAT_CENTERING = 1, SAT_REVERSESTOP = 2, + SAT_PB_SPEED = 5, SAT_PB_DUTY = 6, + SAT_PB_ERROR = 7, SAT_PB_HIGH_VOLTAGE = 10, SAT_PB_LOW_VOLTAGE = 11, SAT_PB_TEMPERATURE = 12 diff --git a/refloat/src/time.c b/refloat/src/time.c index 04d71620..fb7176c5 100644 --- a/refloat/src/time.c +++ b/refloat/src/time.c @@ -27,6 +27,7 @@ static inline void time_now(Time *t) { void time_init(Time *t) { time_now(t); + t->start_timer = t->now; t->engage_timer = t->now; // Workaround: After startup (assume the time is very close to 0), we don't // want anything to be thinking we have just disengaged. Set disengage time diff --git a/refloat/src/time.h b/refloat/src/time.h index 85c4b44f..87d34741 100644 --- a/refloat/src/time.h +++ b/refloat/src/time.h @@ -25,6 +25,7 @@ typedef uint32_t time_t; typedef struct { time_t now; + time_t start_timer; time_t engage_timer; time_t disengage_timer; time_t idle_timer; diff --git a/refloat/src/torque_tilt.c b/refloat/src/torque_tilt.c index 4d11a2a2..dd3eac13 100644 --- a/refloat/src/torque_tilt.c +++ b/refloat/src/torque_tilt.c @@ -22,9 +22,15 @@ #include +void torque_tilt_init(TorqueTilt *tt) { + tt->on_step_size = 0.0f; + tt->off_step_size = 0.0f; + torque_tilt_reset(tt); +} + void torque_tilt_reset(TorqueTilt *tt) { - tt->setpoint = 0; - tt->ramped_step_size = 0; + tt->ramped_step_size = 0.0f; + tt->setpoint = 0.0f; } void torque_tilt_configure(TorqueTilt *tt, const RefloatConfig *config) { @@ -49,9 +55,15 @@ void torque_tilt_update(TorqueTilt *tt, const MotorData *motor, const RefloatCon sign(motor->filt_current); float step_size = 0; - if ((tt->setpoint - target > 0 && target > 0) || (tt->setpoint - target < 0 && target < 0)) { + if (tt->setpoint * target < 0) { + // Moving towards opposite sign (crossing zero); + // Use the faster tilt speed until 0 is reached + step_size = fmaxf(tt->off_step_size, tt->on_step_size); + } else if (fabsf(tt->setpoint) > fabsf(target)) { + // Moving towards smaller angle of same sign or zero step_size = tt->off_step_size; } else { + // Moving towards larger angle of same sign step_size = tt->on_step_size; } diff --git a/refloat/src/torque_tilt.h b/refloat/src/torque_tilt.h index f1d2fff9..affc5909 100644 --- a/refloat/src/torque_tilt.h +++ b/refloat/src/torque_tilt.h @@ -24,11 +24,13 @@ typedef struct { float on_step_size; float off_step_size; - float ramped_step_size; + float ramped_step_size; float setpoint; } TorqueTilt; +void torque_tilt_init(TorqueTilt *tt); + void torque_tilt_reset(TorqueTilt *tt); void torque_tilt_configure(TorqueTilt *tt, const RefloatConfig *config); diff --git a/refloat/src/turn_tilt.c b/refloat/src/turn_tilt.c index 85a56a5e..b44f1a6a 100644 --- a/refloat/src/turn_tilt.c +++ b/refloat/src/turn_tilt.c @@ -21,14 +21,22 @@ #include -void turn_tilt_reset(TurnTilt *tt) { - tt->last_yaw_angle = 0; - tt->last_yaw_change = 0; - tt->yaw_change = 0; - tt->yaw_aggregate = 0; +void turn_tilt_init(TurnTilt *tt) { + tt->step_size = 0.0f; + tt->boost_per_erpm = 0.0f; + turn_tilt_reset(tt); +} - tt->target = 0; - tt->setpoint = 0; +void turn_tilt_reset(TurnTilt *tt) { + tt->last_yaw_angle = 0.0f; + tt->last_yaw_change = 0.0f; + tt->yaw_change = 0.0f; + tt->abs_yaw_change = 0.0f; + tt->yaw_aggregate = 0.0f; + + tt->ramped_step_size = 0.0f; + tt->target = 0.0f; + tt->setpoint = 0.0f; } void turn_tilt_configure(TurnTilt *tt, const RefloatConfig *config) { @@ -65,14 +73,7 @@ void turn_tilt_aggregate(TurnTilt *tt, const IMU *imu) { } } -void turn_tilt_update( - TurnTilt *tt, - const MotorData *md, - const ATR *atr, - float balance_pitch, - float noseangling, - const RefloatConfig *config -) { +void turn_tilt_update(TurnTilt *tt, const MotorData *md, const RefloatConfig *config) { if (config->turntilt_strength == 0) { return; } @@ -119,32 +120,12 @@ void turn_tilt_update( } else { tt->target *= md->erpm_sign; } - - // ATR interference: Reduce target during moments of high torque response - float atr_min = 2; - float atr_max = 5; - if (sign(atr->target) != sign(tt->target)) { - // further reduced turntilt during moderate to steep downhills - atr_min = 1; - atr_max = 4; - } - if (fabsf(atr->target) > atr_min) { - // Start scaling turntilt when ATR>2, down to 0 turntilt for ATR > 5 degrees - float atr_scaling = (atr_max - fabsf(atr->target)) / (atr_max - atr_min); - if (atr_scaling < 0) { - atr_scaling = 0; - // during heavy torque response clear the yaw aggregate too - tt->yaw_aggregate = 0; - } - tt->target *= atr_scaling; - } - if (fabsf(balance_pitch - noseangling) > 4) { - // no setpoint changes during heavy acceleration or braking - tt->target = 0; - tt->yaw_aggregate = 0; - } } - // Move towards target limited by max speed - rate_limitf(&tt->setpoint, tt->target, tt->step_size); + // Smoothen changes in tilt angle by ramping the step size + smooth_rampf(&tt->setpoint, &tt->ramped_step_size, tt->target, tt->step_size, 0.04, 1.5); +} + +void turn_tilt_winddown(TurnTilt *tt) { + tt->setpoint *= 0.995; } diff --git a/refloat/src/turn_tilt.h b/refloat/src/turn_tilt.h index 00e10330..9b2f5670 100644 --- a/refloat/src/turn_tilt.h +++ b/refloat/src/turn_tilt.h @@ -17,13 +17,13 @@ #pragma once -#include "atr.h" #include "conf/datatypes.h" #include "imu.h" #include "motor_data.h" typedef struct { float step_size; + float ramped_step_size; float boost_per_erpm; float last_yaw_angle; @@ -36,17 +36,14 @@ typedef struct { float setpoint; } TurnTilt; +void turn_tilt_init(TurnTilt *tt); + void turn_tilt_reset(TurnTilt *tt); void turn_tilt_configure(TurnTilt *tt, const RefloatConfig *config); void turn_tilt_aggregate(TurnTilt *tt, const IMU *imu); -void turn_tilt_update( - TurnTilt *tt, - const MotorData *md, - const ATR *atr, - float balance_pitch, - float noseangling, - const RefloatConfig *config -); +void turn_tilt_update(TurnTilt *tt, const MotorData *md, const RefloatConfig *config); + +void turn_tilt_winddown(TurnTilt *tt); diff --git a/refloat/src/utils.h b/refloat/src/utils.h index ba89e804..5068109c 100644 --- a/refloat/src/utils.h +++ b/refloat/src/utils.h @@ -57,7 +57,7 @@ #endif // Declaration for the SEMD_APP_DATA macro, definition needs to be in main.c. -void send_app_data_overflow_terminate(); +void fatal_error_terminate(); #define SEND_BUF_MAX_SIZE 511 @@ -77,7 +77,7 @@ void send_app_data_overflow_terminate(); ind \ ); \ /* terminate the main thread, the memory has just been corrupted by buffer overflow */ \ - send_app_data_overflow_terminate(); \ + fatal_error_terminate(); \ } \ VESC_IF->send_app_data(buffer, ind); \ } while (0) diff --git a/refloat/ui.qml.in b/refloat/ui.qml.in index 3a51f3d0..c0eef26a 100644 --- a/refloat/ui.qml.in +++ b/refloat/ui.qml.in @@ -25,6 +25,7 @@ import QtQuick.Window 2.2 import QtGraphicalEffects 1.15 import Vedder.vesc.vescinterface 1.0 +import Vedder.vesc.codeloader 1.0 import Vedder.vesc.commands 1.0 import Vedder.vesc.configparams 1.0 import Vedder.vesc.utility 1.0 @@ -84,6 +85,45 @@ Item { return Math.abs(a - b); } + function parseVersion(ver) { + var dotSplit = ver.split("."); + if (dotSplit.length < 3) { + return [0, 0, 0, ""]; + } + + var lastPart = dotSplit[2]; + var dashPos = lastPart.search("-"); + var patch = lastPart; + var suffix = ""; + if (dashPos > 0) { + patch = lastPart.substring(0, dashPos); + suffix = lastPart.substring(dashPos + 1); + } + + return [Number(dotSplit[0]), Number(dotSplit[1]), Number(patch), suffix]; + } + + function versionMajorMinor(ver) { + var arr = ver.split("."); + if (arr.length < 2) { + return [0, 0]; + } + + return [Number(arr[0]), Number(arr[1])]; + } + + // Expects arrays containing 3 numbers, returns -1 if a < b, 1 if a > b and 0 if a == b + function cmpVersion(a, b) { + for (var i = 0; i < 3; i++) { + if (a[i] < b[i]) { + return -1; + } else if (a[i] > b[i]) { + return 1; + } + } + return 0; + } + function getDarkThemeColor(name) { switch (name) { case "normalBackground": @@ -213,6 +253,11 @@ Item { VescIf.storeBleName(VescIf.getLastBleAddr(), name); VescIf.storeSettings(); } + + function getFwVersion() { + var params = VescIf.getLastFwRxParams(); + return [params.major, params.minor]; + } } Connections { @@ -404,34 +449,40 @@ Item { writeConfig(); } - function paramNameToObject(name) { - var obj = { - "name": name - }; + function getParamValue(name) { + if (pkgConfig.isParamDouble(name)) { + return pkgConfig.getParamDouble(name); + } else if (pkgConfig.isParamInt(name)) { + return pkgConfig.getParamInt(name); + } else if (pkgConfig.isParamEnum(name)) { + return pkgConfig.getParamEnum(name); + } else if (pkgConfig.isParamBool(name)) { + return pkgConfig.getParamBool(name); + } else if (pkgConfig.isParamQString(name)) { + return pkgConfig.getParamQString(name); + } else if (pkgConfig.isParamBitfield(name)) { + return pkgConfig.getParamInt(name); + } + + return null; + } + function getParamType(name) { if (pkgConfig.isParamDouble(name)) { - obj.type = "Double"; - obj.value = pkgConfig.getParamDouble(name); + return "Double"; } else if (pkgConfig.isParamInt(name)) { - obj.type = "Int"; - obj.value = pkgConfig.getParamInt(name); + return "Int"; } else if (pkgConfig.isParamEnum(name)) { - obj.type = "Enum"; - obj.value = pkgConfig.getParamEnum(name); + return "Enum"; } else if (pkgConfig.isParamBool(name)) { - obj.type = "Bool"; - obj.value = pkgConfig.getParamBool(name); + return "Bool"; } else if (pkgConfig.isParamQString(name)) { - obj.type = "String"; - obj.value = pkgConfig.getParamQString(name); + return "String"; } else if (pkgConfig.isParamBitfield(name)) { - obj.type = "Bitfield"; - obj.value = pkgConfig.getParamInt(name); - } else { - return null; + return "Bitfield"; } - return obj; + return null; } function getDisplayName(name) { @@ -463,6 +514,7 @@ Item { "is_beeper_enabled", "tiltback_hv", "tiltback_lv", + "tiltback_speed", ]; return !name.startsWith("haptic.") && !name.startsWith("hardware.") && !tuneBlacklist.includes(name); @@ -471,17 +523,16 @@ Item { function fetchConfig(tuneOnly) { if (!checkConfig()) return null; - var settings = []; + var settings = {}; for (let subGroup of pkgConfig.getParamSubgroups("General")) { for (let param of pkgConfig.getParamsFromSubgroup("General", subGroup)) { if (tuneOnly && !paramIsTune(param)) { continue; } - var item = paramNameToObject(param); - - if (item) { - settings.push(item); + var value = getParamValue(param); + if (value !== null) { + settings[param] = getParamValue(param); } } } @@ -490,9 +541,7 @@ Item { } function versionCompatible(config) { - var verArray = config.version.split("."); - var major = Number(verArray[0]); - var minor = Number(verArray[1]); + var [major, minor] = versionMajorMinor(config.version); if (major !== tuneManager.formatVersionMajor || minor > tuneManager.formatVersionMinor) { VescIf.emitStatusMessage("Error: Incompatible version: %1".arg(config.version), false); return false; @@ -508,19 +557,19 @@ Item { return; } - for (let item of config.settings) { - if (item.type === "Double") { - pkgConfig.updateParamDouble(item.name, item.value); - } else if (item.type === "Int") { - pkgConfig.updateParamInt(item.name, item.value); - } else if (item.type === "Bool") { - pkgConfig.updateParamBool(item.name, item.value); - } else if (item.type === "Enum") { - pkgConfig.updateParamEnum(item.name, item.value); - } else if (item.type === "String") { - pkgConfig.updateParamString(item.name, item.value); - } else if (item.type === "Bitfield") { - pkgConfig.updateParamInt(item.name, item.value); + for (let [key, value] of Object.entries(config.settings)) { + if (pkgConfig.isParamDouble(key)) { + pkgConfig.updateParamDouble(key, value); + } else if (pkgConfig.isParamInt(key)) { + pkgConfig.updateParamInt(key, value); + } else if (pkgConfig.isParamBool(key)) { + pkgConfig.updateParamBool(key, value); + } else if (pkgConfig.isParamEnum(key)) { + pkgConfig.updateParamEnum(key, value); + } else if (pkgConfig.isParamQString(key)) { + pkgConfig.updateParamString(key, value); + } else if (pkgConfig.isParamBitfield(key)) { + pkgConfig.updateParamInt(key, value); } } @@ -528,35 +577,32 @@ Item { } function diffConfig(other) { - var typeDiffers = []; var differs = []; var otherMissing = []; + var otherSettings = {}; if (other && other.settings) { - var otherSettings = {}; - for (let item of other.settings) { - otherSettings[item.name] = item; - } + otherSettings = Object.assign({}, other.settings); if (VescIf.customConfigsLoaded()) { for (let subGroup of pkgConfig.getParamSubgroups("General")) { for (let paramName of pkgConfig.getParamsFromSubgroup("General", subGroup)) { - var param = paramNameToObject(paramName); - if (!param) { + var value = getParamValue(paramName); + if (value === null) { continue; } if (otherSettings.hasOwnProperty(paramName)) { - var otherParam = otherSettings[paramName]; - if (param.type !== otherParam.type) { - param.otherType = otherParam.type; - typeDiffers.push(param); - } else if (param.value !== otherParam.value) { - param.otherValue = otherParam.value; - differs.push(param); + var otherValue = otherSettings[paramName]; + if (value !== otherValue) { + differs.push({ + "name": paramName, + "value": value, + "otherValue": otherValue + }); } delete otherSettings[paramName]; } else { - otherMissing.push(param); + otherMissing.push(paramName); } } } @@ -565,7 +611,7 @@ Item { return { "differs": differs, - "missing": otherSettings, + "missing": Object.values(otherSettings), "otherMissing": otherMissing, } } @@ -588,10 +634,12 @@ Item { readonly property int c_INFO: 0 readonly property int c_RC_MOVE: 7 readonly property int c_HANDTEST: 13 + readonly property int c_LIGHTS_CONTROL: 20 readonly property int c_FLYWHEEL: 22 readonly property int c_REALTIME_DATA: 31 readonly property int c_REALTIME_DATA_IDS: 32 - readonly property int c_LIGHTS_CONTROL: 202 + readonly property int c_ALERTS_LIST: 35 + readonly property int c_ALERTS_CONTROL: 36 readonly property int c_DATA_RECORD_REQUEST: 41 readonly property int c_DATA_RECORD_HEADER: 42 readonly property int c_DATA_RECORD_DATA: 43 @@ -658,7 +706,7 @@ Item { } function sendLightsControl(on, headlightsOn) { - var data = createData(4, c_LIGHTS_CONTROL); + var data = createData(7, c_LIGHTS_CONTROL); var mask = 0; var value = 0; @@ -676,8 +724,26 @@ Item { value |= 0b10; } } - data.setUint8(2, mask); - data.setUint8(3, value); + data.setUint32(2, mask); + data.setUint8(6, value); + + vescCommands.sendCustomAppData(data.buffer); + } + + function sendAlertsList(startTime) { + var data = createData(6, c_ALERTS_LIST); + + if (startTime !== undefined && startTime !== null) { + data.setUint32(2, startTime); + } + + vescCommands.sendCustomAppData(data.buffer); + } + + function sendAlertsClearFatal() { + var data = createData(3, c_ALERTS_CONTROL); + + data.setUint8(2, 0x1); // clear the fatal error flag vescCommands.sendCustomAppData(data.buffer); } @@ -747,10 +813,6 @@ Item { } function onValuesSetupReceived(values, mask) { - var fault = values.fault_str; - fault = fault.replace("FAULT_CODE_", ""); - state.vescFault = fault != "NONE" ? fault : ""; - batteryBar.value = values.battery_level * 100; odometer.value = values.odometer * 0.001 * vescConfig.imperialFactor; @@ -786,6 +848,8 @@ Item { var values = dv.getUint8(ind++); lights.on = values & 0b01; lights.headlightsOn = values & 0b10; + } else if (msgtype === c_ALERTS_LIST) { + alerts.receiveAlertsList(dv, ind); } else if (msgtype === c_REALTIME_DATA) { if (state.commsState !== state.comms_Initialized) { // rt data came because another client app requested them, @@ -796,13 +860,16 @@ Item { var mask = dv.getUint8(ind++); var hasRuntime = !!(mask & 0x1); var hasCharging = !!(mask & 0x2); + var hasAlerts = !!(mask & 0x4); var extraFlags = dv.getUint8(ind++); state.extraRecording = extraFlags & 0x1; state.extraRecAutostart = extraFlags & 0x2; state.extraRecAutostop = extraFlags & 0x4; + state.fatalError = extraFlags & 0x8; var time = dv.getUint32(ind); ind += 4; + state.setTimeReference(time); var modeAndState = dv.getUint8(ind++); state.pkgMode = modeAndState >> 4; @@ -845,8 +912,12 @@ Item { state.rtData = state.newRtData(newRt); if (hasCharging) { - chargingInfo.current = dv.getFloat32(ind); ind += 4; - chargingInfo.voltage = dv.getFloat32(ind); ind += 4; + chargingInfo.current = getFloat16(dv, ind); ind += 2; + chargingInfo.voltage = getFloat16(dv, ind); ind += 2; + } + + if (hasAlerts) { + ind += 9; // unused here } var running = +(state.pkgState === state.s_Running); @@ -871,26 +942,366 @@ Item { } } - // Old Float package settings, used to retrieve quicksaves - Settings { - id: floatSettings - } - Settings { id: preferences category: "preferences" property int speedDialMax: vescConfig.useImperial ? 30 : 40 - property alias swapAdcs: prefSwapAdcs.checked property alias showBattVoltage: prefShowBattVoltage.checked property alias showBattVoltagePerCell: prefShowBattVoltagePerCell.checked property alias battCurrentLog: prefBattCurrentLog.checked property int tempWarningOffset: 10 property alias tuneSlotCount: prefTuneSlotCount.value property alias showTuneDiffCount: prefShowTuneDiffCount.checked - property alias showRTPlotByDefault: prefShowRTPlotByDefault.checked property int plotMaxWindowMinutes: 30 property alias showWelcomeDialog: prefShowWelcomeDialog.checked + property alias checkForNewVersions: prefCheckForNewVersions.checked + } + + Settings { + id: persistentData + category: "persistentData" + + property var lastPackageUpdateCheck + property var packageUpdateSnooze + } + + QtObject { + id: updateChecker + + property CodeLoader packageLoader: CodeLoader { + id: packageLoader + + function getPackageVersion(pkgName) { + var pkgs = reloadPackageArchive(); + + for (var pkg of pkgs) { + if (!pkg.isLibrary && shouldShowPackage(pkg) && pkg.name === pkgName) { + // HTML text + var li = pkg.description.match(/
  • Version: [a-zA-Z0-9._-]+<\/li>/g); + if (li) { + var version = li[0]; + return version.substring(13, version.length - 5); + } + + // markdown text + var md_bullet = pkg.description.match(/\n- Version: [a-zA-Z0-9._-]+\n/g); + if (md_bullet) { + var version = md_bullet[0]; + return version.substring(11, version.length); + } + } + } + + return null; + } + + Component.onCompleted: { + setVesc(VescIf) + } + } + + function check() { + if (!preferences.checkForNewVersions) { + return; + } + + var pkgName = "{{PACKAGE_NAME}}"; + var pkgVersion = "{{VERSION}}"; + if (pkgName.includes("PACKAGE_NAME")) { + // Don't run when loading the teplate itself for development/debugging + return; + } + + var now = Date.now(); + var oneDayMs = 24 * 60 * 60 * 1000; + + var lpuc = persistentData.lastPackageUpdateCheck; + if (!lpuc || now - lpuc > oneDayMs) { + packageLoader.downloadPackageArchive(); + persistentData.lastPackageUpdateCheck = Date.now(); + } + + var pus = persistentData.packageUpdateSnooze; + if (pus && now - pus < 30 * oneDayMs) { + return; + } + + var version = packageLoader.getPackageVersion(pkgName); + if (!version) { + print("{{PACKAGE_NAME}}: Failed to retrieve package version"); + return; + } + + if (cmpVersion(parseVersion(version), parseVersion(pkgVersion)) == 1) { + newPackageVersionDialog.show(version); + } + } + } + + component UIHint : Item { + id: hint + property var target + property real targetX + property real targetY + property real targetWidth + property real targetHeight + + property Settings settings + property string settingName + property bool shown + + function updateTarget() { + if (target !== null) { + var p = mapFromItem(target, 0, 0); + targetX = p.x; + targetY = p.y; + } + } + + onTargetChanged: { + targetWidth = target.width; + targetHeight = target.height; + updateTarget(); + } + + Connections { + target: hint.target + + function onXChanged() { + hint.updateTarget(); + } + + function onYChanged() { + hint.updateTarget(); + } + + function onWidthChanged() { + hint.targetWidth = target.width; + hint.updateTarget(); + } + + function onHeightChanged() { + hint.targetHeight = target.height; + hint.updateTarget(); + } + } + + Component.onCompleted: { + shown = settings[settingName]; + } + + onShownChanged: { + settings[settingName] = shown; + } + + MouseArea { + z: 10 + anchors.fill: parent + } + } + + component UIHintText : NText { + anchors.margins: 50 + width: parent.width - 50 + font.pointSize: 15 + horizontalAlignment: TextInput.AlignHCenter + wrapMode: Text.WordWrap + } + + component UIHintFrame : Rectangle { + anchors.fill: parent + anchors.margins: -5 + color: "transparent" + border.color: Utility.getAppHexColor("lightAccent") + border.width: 1 + radius: 8 + opacity: 0.7 + } + + component UIHintUnderline : Rectangle { + anchors.left: parent.left + anchors.top: parent.top + width: unit * 0.8 + height: 3 + color: Utility.getAppHexColor("lightAccent") + opacity: 0.7 + } + + // Workaround for `state` getting resolved to all kinds of different things in different contexts + property var gState: state + Component { + id: statusTextComp + + UIHint { + target: statusText + + StatusText { + id: statusTextSubject + x: parent.targetX + y: parent.targetY + width: parent.targetWidth + height: parent.targetHeight + globState: gState + + UIHintFrame {} + } + + UIHintText { + anchors.horizontalCenter: statusTextSubject.horizontalCenter + anchors.bottom: statusTextSubject.top + text: "Tap the state text to show more board state details like Stop Condition, Last Beep Reason, errors and faults." + } + } + } + + Component { + id: plotComp + + UIHint { + target: rtPlot + + property int stage: settingName === "plotCursor" ? 0 : settingName === "drGeneral" ? 1 : settingName === "drStartStop" ? 2 : 3 + + Plot { + id: subject + x: parent.targetX + y: parent.targetY + width: parent.targetWidth + height: parent.targetHeight + + showRecordDownload: gState.capDataRecord + dotsEnabled: false + + plotData: PlotData { + id: pd + seriesMetaData: plotSeriesMetaData + } + + Component.onCompleted: { + pd.init(["motor.current"], 1); + subject.init(1); + pd.addSample([0, 0,0,0,0, 10], 0); + pd.addSample([2,0,0,0,0, 22], 1); + pd.addSample([4,0,0,0,0, 15], 2); + pd.addSample([9,0,0,0,0, 30], 3); + pd.makeReady(); + if (stage === 0) { + subject.spawnCursor(4); + } + } + + UIHintFrame {} + + UIHintUnderline { + anchors.topMargin: unit + anchors.leftMargin: unit + width: unit * 1.7 + visible: stage === 1 + } + + UIHintUnderline { + anchors.topMargin: unit + anchors.leftMargin: 1.9 * unit + visible: stage === 2 + } + + UIHintUnderline { + anchors.topMargin: unit + anchors.leftMargin: unit + visible: stage === 3 + } + } + + UIHintText { + anchors.horizontalCenter: subject.horizontalCenter + anchors.bottom: subject.top + text: "View a plot of realtime data (log) from the package on the Data tab.\n\nLong press anywhere in the plot area to spawn a cursor.\n\nYou can dismiss the cursor by a swipe up gesture on it." + visible: stage === 0 + } + + UIHintText { + anchors.horizontalCenter: subject.horizontalCenter + anchors.bottom: subject.top + text: "Congratulations, you have the Data Record firmware installed. You are now part of the secret Data Recording society.\n\nYour board has the ability to store a short detailed data \"log\" by itself, you don't need to be connected.\n\nIt's controlled by these two buttons." + visible: stage === 1 + } + + UIHintText { + anchors.horizontalCenter: subject.horizontalCenter + anchors.bottom: subject.top + text: "This button indicates whether the board is recording. You can use it to start and stop the recording manually.\n\nThe board automatically starts recording when engaged and stops recording on disengage. You can disable the autostart and autostop functionality by long-pressing the button.\n\nIf you nosedive, check the recording before engaging again!" + visible: stage == 2 + } + + UIHintText { + anchors.horizontalCenter: subject.horizontalCenter + anchors.bottom: subject.top + text: "You can download the stored data using this button. It automatically stops recording. Do not start it while you're downloading, or you'll corrupt your data." + visible: stage == 3 + } + } + } + + QtObject { + id: uiHints + + property Settings uiHintsSettings: Settings { + id: uiHintsSettings + category: "uiHints" + + property bool statusText + property bool plotCursor + + property bool drGeneral + property bool drStartStop + property bool drDownload + } + + readonly property var hints: new Map([ + ["statusText", statusTextComp], + ["plotCursor", plotComp], + ]) + + readonly property var dataRecordHints: new Map([ + ["drGeneral", plotComp], + ["drStartStop", plotComp], + ["drDownload", plotComp], + ]) + + function fill(container) { + for (let [key, value] of hints) { + if (!uiHintsSettings[key]) { + if (key == "plotCursor") { + bottomStackTabs.currentIndex = 2; + } + value.createObject(container, {settings: uiHintsSettings, settingName: key}); + } + } + } + + function fillDataRecord(container) { + if (state.capDataRecord) { + for (let [key, value] of dataRecordHints) { + if (!uiHintsSettings[key]) { + bottomStackTabs.currentIndex = 2; + value.createObject(container, {settings: uiHintsSettings, settingName: key}); + } + } + } + } + + function reset(container) { + for (let key of hints.keys()) { + uiHintsSettings[key] = false; + } + + if (state.capDataRecord) { + for (let key of dataRecordHints.keys()) { + uiHintsSettings[key] = false; + } + } + } } Connections { @@ -915,11 +1326,6 @@ Item { property var tuneArchive property var tuneArchiveDownloadDate - // An array storing Float tune names that have been converted for - // each slot. Prevents converting tunes over and over again, until - // the name changes for the Float tune. - property var floatTunesConverted: Array(6) - property var fullBackup } @@ -935,6 +1341,24 @@ Item { return tunes; } + // Major package versions require a migration, minor don't. + // Migration functions should return: [tuneObject, [newMajor, newMinor]] + property var migrations: { + "1.1": function(tune) { + var [fwMajor, fwMinor] = board.getFwVersion(); + if (fwMajor > 6 || (fwMajor == 6 && fwMinor >= 5) && motorConfig.batteryCells > 0) { + if (tune.settings["tiltback_hv"] > 10) { + tune.settings["tiltback_hv"] /= motorConfig.batteryCells; + } + if (tune.settings["tiltback_lv"] > 10) { + tune.settings["tiltback_lv"] /= motorConfig.batteryCells; + } + } + + return [tune, [1, 2]]; + }, + } + function onUpdate() { triggerAutoRestore(); // force recalculation of dependent variables @@ -943,6 +1367,36 @@ Item { property alias fullBackup: tuneStorage.fullBackup + function convertToOldFormat(tune) { + if (Array.isArray(tune.settings)) { + return tune; + } + + var res = Object.assign({}, tune); + res.settings = []; + for (let [name, value] of Object.entries(tune.settings)) { + res.settings.push({ + "name": name, + "value": value, + "type": packageConfig.getParamType(name) + }); + } + return res; + } + + function convertFromOldFormat(tune) { + if (!Array.isArray(tune.settings)) { + return tune; + } + + var res = Object.assign({}, tune); + res.settings = {}; + for (let item of tune.settings) { + res.settings[item.name] = item.value; + } + return res; + } + function tuneId(slot) { return "tune_slot_" + slot; } @@ -992,6 +1446,26 @@ Item { return true; } + function migrateTune(tune) { + var [major, minor] = versionMajorMinor(tune.package.version); + // Zero or NaN + if (!major) { + return tune; + } + + var [pMajor, pMinor] = versionMajorMinor(packageVersion); + + while (major < pMajor || (major == pMajor && minor < pMinor)) { + var versionKey = [major, minor].join("."); + if (migrations.hasOwnProperty(versionKey)) { + [tune, [major, minor]] = migrations[versionKey](tune); + } else { + minor++; + } + } + return tune; + } + function createBackup() { var config = packageConfig.fetchConfig(false); if (!config) { @@ -1000,6 +1474,19 @@ Item { return createTune("Full Backup", "", config); } + function loadBackup(name) { + var backup = tuneStorage.value(name); + if (!backup) { + return null; + } + + return migrateTune(convertFromOldFormat(backup)); + } + + function saveBackup(name, backup) { + tuneStorage.setValue(name, convertToOldFormat(backup)); + } + function globalBackup() { var backup = createBackup(); if (!backup) { @@ -1009,10 +1496,11 @@ Item { } function globalRestore() { - if (!fullBackup) { + var backup = loadBackup("fullBackup"); + if (!backup) { return; } - applyConfigDialog.show(fullBackup); + applyConfigDialog.show(backup, false, false); } function idBackup() { @@ -1020,16 +1508,16 @@ Item { if (!backup) { return; } - tuneStorage.setValue("backup_" + board.getId(), backup); + saveBackup("backup_" + board.getId(), backup); idBackupFinished(backup); } function idRestore() { - var backup = tuneStorage.value("backup_" + board.getId()); + var backup = loadBackup("backup_" + board.getId()); if (!backup) { return; } - applyConfigDialog.show(backup); + applyConfigDialog.show(backup, false, false); } function autoBackup() { @@ -1038,16 +1526,16 @@ Item { return; } - tuneStorage.setValue("auto_backup_" + board.getId(), backup); + saveBackup("auto_backup_" + board.getId(), backup); autoBackupFinished(backup); } function autoRestore() { - var backup = tuneStorage.value("auto_backup_" + board.getId()); + var backup = loadBackup("auto_backup_" + board.getId()); if (!backup) { return; } - applyConfigDialog.show(backup); + applyConfigDialog.show(backup, false, false); } function triggerAutoRestore() { @@ -1056,10 +1544,10 @@ Item { } if (packageConfig.isDefault()) { - var backup = tuneStorage.value("auto_backup_" + board.getId()); + var backup = loadBackup("auto_backup_" + board.getId()); if (backup) { print("Refloat: Automatic backup restore."); - applyConfigDialog.show(backup, true); + applyConfigDialog.show(backup, true, false); } else { print("Refloat: No automatic backup to restore."); } @@ -1071,8 +1559,7 @@ Item { function saveTune(slot, tune) { var id = tuneId(slot); - - tuneStorage.setValue(id, JSON.stringify(tune)); + tuneStorage.setValue(id, JSON.stringify(convertToOldFormat(tune))); tunes[slot] = tune; // workaround needed so that the change is written through to the storage @@ -1080,9 +1567,12 @@ Item { } function loadTune(slot) { - var tuneJson = tuneStorage.value(tuneId(slot), ""); - if (tuneJson) { - return JSON.parse(tuneJson); + var tune = tuneStorage.value(tuneId(slot)); + if (tune) { + if (typeof tune === "string") { + tune = JSON.parse(tune); + } + return migrateTune(convertFromOldFormat(tune)); } return null; } @@ -1105,7 +1595,11 @@ Item { function loadTuneArchive() { if (tuneStorage.tuneArchive) { - return JSON.parse(tuneStorage.tuneArchive); + if (typeof tuneStorage.tuneArchive === "string") { + return JSON.parse(tuneStorage.tuneArchive); + } else { + return tuneStorage.tuneArchive; + } } return null; } @@ -1114,40 +1608,6 @@ Item { return Qt.formatDateTime(tuneStorage.tuneArchiveDownloadDate); } - function convertFloatQuicksaves() { - var namesJson = floatSettings.value("quicksave_names", null); - if (namesJson) { - var names = JSON.parse(namesJson); - for (var slotName in names) { - if (!slotName.startsWith("Float Quicksave ")) { - continue; - } - - var tuneSettingsJson = floatSettings.value(slotName, null); - if (!tuneSettingsJson) { - continue; - } - - var slot = Number(slotName.replace("Float Quicksave ", "")); - // skipping if the tune has been converted before - if (tuneStorage.floatTunesConverted[slot - 1] === names[slotName]) { - continue; - } - - // at this stage mark slot as converted, if target slot is not empty, - // we don't want it converted at the moment it gets emptied - tuneStorage.floatTunesConverted[slot - 1] = names[slotName]; - // workaround needed so that the change is written through to the storage - tuneStorage.floatTunesConverted = tuneStorage.floatTunesConverted; - - if (slotEmpty(slot)) { - var tuneSettings = JSON.parse(tuneSettingsJson); - tuneManager.saveTune(slot, tuneManager.createTune(names[slotName], "", tuneSettings)); - } - } - } - } - function parseCsv(csv) { var lines = csv.split("\r\n"); var tuneCount = lines[0].split(",").length - 1; @@ -1155,7 +1615,7 @@ Item { var result = []; for (var i = 0; i < tuneCount; i++) { // set the version to the current one to always pass the version check - result.push({"version": formatVersion, "settings": []}); + result.push({"version": formatVersion, "settings": {}}); } for (var i in lines) { @@ -1166,30 +1626,29 @@ Item { if (value) { var key = currentLine[0]; var name; - var type; if (key === "_name") { result[j].name = value; continue; } else if (key.startsWith("double_")) { name = key.substring(7); - type = "Double"; + value = parseFloat(value); } else if (key.startsWith("int_")) { name = key.substring(4); - type = "Int"; + value = parseInt(value); } else if (key.startsWith("bool_")) { name = key.substring(5); - type = "Bool"; + value = !!parseInt(value); } else if (key.startsWith("enum_")) { name = key.substring(5); - type = "Enum"; + value = parseInt(value); } - result[j].settings.push({ - "name": name, - "type": type, - "value": value - }); + if (Number.isNaN(value)) { + continue; + } + + result[j].settings[name] = value; } } } @@ -1198,12 +1657,11 @@ Item { } Component.onCompleted: { - convertFloatQuicksaves(); - var idBackup = tuneStorage.value("backup_" + board.getId()); + var idBackup = loadBackup("backup_" + board.getId()); if (idBackup) { idBackupFinished(idBackup); } - var autoBackup = tuneStorage.value("auto_backup_" + board.getId()); + var autoBackup = loadBackup("auto_backup_" + board.getId()); if (autoBackup) { autoBackupFinished(autoBackup); } @@ -1270,10 +1728,6 @@ Item { property int pkgState: s_Connecting property string pkgStateString: pkgStateToString.get(pkgState) ?? "UNKNOWN (%1)".arg(pkgState) - onPkgStateChanged: { - updateErrorStrings(); - } - // package mode readonly property int m_Normal: 0 readonly property int m_Handtest: 1 @@ -1312,18 +1766,15 @@ Item { if (fpState & fs_Right) { rightSensor.on = true; } - - if (preferences.swapAdcs && leftSensor.on !== rightSensor.on) { - leftSensor.on = !leftSensor.on; - rightSensor.on = !rightSensor.on; - } } // setpoint adjustment type readonly property int sat_None: 0 readonly property int sat_Centering: 1 readonly property int sat_ReverseStop: 2 + readonly property int sat_Speed: 5 readonly property int sat_Duty: 6 + readonly property int sat_PushbackError: 7 readonly property int sat_HighVoltage: 10 readonly property int sat_LowVoltage: 11 readonly property int sat_Temperature: 12 @@ -1337,7 +1788,9 @@ Item { [sat_None, ""], [sat_Centering, "centering"], [sat_ReverseStop, "reverse stop"], + [sat_Speed, "pushback: speed"], [sat_Duty, "pushback: duty"], + [sat_PushbackError, "pushback: error"], [sat_HighVoltage, "pushback: high voltage"], [sat_LowVoltage, "pushback: low voltage"], [sat_Temperature, "pushback: temperature"], @@ -1348,7 +1801,7 @@ Item { property int setpointAdjustmentClass onSetpointAdjustmentTypeChanged: { // do these in one function to keep them consistent, order is important to display and fade correctly in pitchPushbackText - setpointAdjustmentClass = setpointAdjustmentType >= sat_HighVoltage ? sac_Error : setpointAdjustmentType >= sat_Duty ? sac_Warning : sac_Normal; + setpointAdjustmentClass = setpointAdjustmentType >= sat_HighVoltage ? sac_Error : setpointAdjustmentType >= sat_Speed ? sac_Warning : sac_Normal; setpointAdjustmentTypeString = setpointAdjustmentTypeToString.get(setpointAdjustmentType) ?? "unknown (%1)".arg(setpointAdjustmentType); } @@ -1390,42 +1843,44 @@ Item { [8, "Low Battery"], [9, "Board Idle"], [10, "Other"], + [11, "Speed"], + [12, "BMS: Cell Under Temp"], + [13, "BMS: Cell Over Temp"], + [14, "BMS: Cell Low Voltage"], + [15, "BMS: Cell High Voltage"], + [16, "BMS: Cell Balance"], + [17, "BMS: Connection Error"], + [18, "BMS: BMS Temp"], + [19, "Firmware Fault"], ]) property int beepReason: 0 property string beepReasonString: beepReasonToString.get(beepReason) ?? "unknown (%1)".arg(beepReason) property bool pkgStateIsError: ![s_Startup, s_Ready, s_Running, s_Connecting].includes(pkgState) - property string vescFault: "" property bool configLoadingError: false + property bool fatalError: false - property bool isError: pkgStateIsError || !!vescFault || configLoadingError - - onVescFaultChanged: updateErrorStrings() - onConfigLoadingErrorChanged: updateErrorStrings() - - property string errorText: "" - property string errorDescription: "" - - function updateErrorStrings() { - if (!!vescFault) { - errorText = "VESC fault: %1".arg(vescFault); - errorDescription = "VESC reported a fault. Be very careful. You should not continue riding before you fix the root cause." - } else if (configLoadingError) { - errorText = "Error: Refloat Cfg failed to load."; - errorDescription = "Reconnecting to the board usually fixes this. If problem persists, try reinstalling the package."; - } else if (pkgState == s_Disconnected) { - errorText = ""; - errorDescription = "Package is not responding. May occur temporarily with spotty connection. If board doesn't engage, the package is dead. Restart the board, if problem persists, try reinstalling the package."; - } else if (pkgState == s_Disabled) { - errorText = ""; - errorDescription = "Refloat package is disabled. After you're done with board setup, You can re-enable it in Settings > Setup." - } else if (![s_Startup, s_Ready, s_Running, s_Connecting].includes(pkgState)) { - errorText = ""; - errorDescription = "Your Refloat package state is unknown. This should never happen."; - } else { - errorText = ""; - errorDescription = ""; + property bool isError: pkgStateIsError || configLoadingError || fatalError + + property var timeReference + + function ticksToMsSinceEpoch(t) { + if (timeReference === undefined) { + return undefined; + } + + return timeReference + t * 1000 / ticksPerSecond; + } + + function setTimeReference(t) { + var timeRef = ticksToMsSinceEpoch(t); + var now = Date.now(); + + // if time is in the past, it's either a board reboot or the ticks have overflown + // reset the time reference either way + if (timeRef === undefined || timeRef < now - 8000) { + timeReference = now - t * 1000 / ticksPerSecond; } } @@ -1454,6 +1909,138 @@ Item { } } + QtObject { + id: alerts + + property int activeAlertsMask: 0 + property int activeAlertHighlight: 0 + property int activeAlertCount: 0 + property bool isFatalActive: false + property var activeAlerts: [] + property int activeFwFaultCode: 0 + property string activeFwFaultName: "" + property var alertsLog: new Map() + property int lastAlertsLogTicks: 0 + + signal alertAdded(var alert) + + property Connections connections: Connections { + target: state + + function onTimeReferenceChanged() { + alerts.lastAlertsLogTicks = 0; + } + } + + readonly property int alert_fwFault: 1 + + readonly property var alertData: new Map([ + [0, { + "name": "None", + "severity": "WARNING", + }], + [alert_fwFault, { + "name": "Fault", + "severity": "FATAL", + }], + ]) + + readonly property var severityColors: new Map([ + ["FATAL", Utility.getAppHexColor("red")], + ["ERROR", Utility.getAppHexColor("orange")], + ["WARNING", Utility.getAppHexColor("lightText")], + ]) + + property Timer alertsListPollTimer: Timer { + running: state.commsState === state.comms_Initialized + repeat: true + interval: 500 + triggeredOnStart: true + onTriggered: { + commands.sendAlertsList(alerts.lastAlertsLogTicks ? alerts.lastAlertsLogTicks : undefined); + } + } + + function getActiveAlertData(alertId) { + if (alertId == alert_fwFault) { + var data = Object.assign({}, alertData.get(alertId)); + data.name = data.name + ": " + activeFwFaultName; + return data; + } else { + return alertData.get(alertId); + } + } + + function getAlertLogData(alertId, data) { + return alertData.get(alertId); + } + + function updateActiveAlerts() { + if (activeAlertsMask & alert_fwFault) { + activeAlertHighlight = alert_fwFault; + activeAlertCount = 1; + isFatalActive = true; + activeAlerts = [ + alert_fwFault, + ]; + } else { + activeAlertHighlight = 0; + activeAlertCount = 0; + isFatalActive = false; + activeAlerts = []; + } + } + + function receiveAlertsList(dv, ind) { + activeAlertsMask = dv.getUint32(ind); ind += 4; + ind += 4; // skip unused 4 bytes + activeFwFaultCode = dv.getUint8(ind++); + if (activeFwFaultCode > 0) { + [activeFwFaultName, ind] = commands.getString(dv, ind); + } else { + activeFwFaultName = ""; + } + updateActiveAlerts(); + + var count = dv.getUint8(ind++); + + var ids = []; + for (var i = 0; i < count; i++) { + var ticks = dv.getUint32(ind); ind += 4; + var record = { + "ticks": ticks, + "id": dv.getUint8(ind++), + "active": dv.getUint8(ind++) & 0x1, + "code": dv.getUint8(ind++), + }; + + if (record.code > 0) { + [record.text, ind] = commands.getString(dv, ind); + } + + var realTime = state.ticksToMsSinceEpoch(ticks); + if (realTime === undefined) { + break; + } + + record.time = realTime; + + var key = "%1_%2_%3".arg(ticks).arg(record.id).arg(record.active); + if (alertsLog.has(key) && Math.abs(realTime - alertsLog.get(key).time) < 5000) { + continue; + } + + alertsLog.set(key, record); + lastAlertsLogTicks = ticks; + alertAdded(record); + } + } + + function clearFatalError() { + commands.sendAlertsClearFatal(); + } + } + QtObject { id: lights @@ -1877,7 +2464,7 @@ Item { modal: true focus: true closePolicy: Popup.CloseOnEscape - parent: ApplicationWindow.overlay + parent: mainItem Overlay.modal: Rectangle { color: "#AA000000" @@ -2223,6 +2810,7 @@ Item { property bool showRecordDownload: false property int maxWindowMinutes + property bool dotsEnabled: true function init(ticksPerSec) { ticksPerSecond = ticksPerSec; @@ -2235,6 +2823,14 @@ Item { plotCanvas.reset(); } + function spawnCursor(cursorPos) { + plotCanvas.spawnCursor(cursorPos); + } + + function hideCursor() { + plotCanvas.fadeCursor(); + } + onVisibleChanged: { if (!visible) { menu.visible = false; @@ -2470,7 +3066,6 @@ Item { MouseArea { id: plotMouse anchors.fill: parent - propagateComposedEvents: true preventStealing: true property real initialX @@ -2535,7 +3130,7 @@ Item { var d = initialX - x(mouse); if (holdCursor) { if (moveCursor) { - plotCanvas.spawnCursor(x(mouse)); + plotCanvas.spawnCursorCoord(x(mouse)); } else { var yd = initialY - mouse.y; var dist = Math.sqrt(yd * yd + d * d); @@ -2574,7 +3169,7 @@ Item { if (!moving && (dist2d(mouse) < snap || !holdCursor)) { holdCursor = true; moveCursor = true; - plotCanvas.spawnCursor(x(mouse)); + plotCanvas.spawnCursorCoord(x(mouse)); } } } @@ -2923,9 +3518,11 @@ Item { return true; } - function spawnCursor(x) { - var cursorPos = toPlotPos(position, x); + function spawnCursorCoord(x) { + spawnCursor(toPlotPos(position, x)); + } + function spawnCursor(cursorPos) { if (!isCursorPosValid(cursorPos)) { return; } @@ -2961,7 +3558,7 @@ Item { return toCanvasPos(pos); } - function fadeCursor(a) { + function fadeCursor(a=0) { a = a * a * a; if (a <= 0.05) { cursor = null; @@ -3187,6 +3784,7 @@ Item { var pos = position; var win = window; var xLen = xLength; + var dotsEnbld = plot.dotsEnabled; // calculate start and end indices according to window var start = 0; @@ -3336,7 +3934,7 @@ Item { } if (!minMaxMode) { - if (origX - lastX > halfASecondPixels) { + if (dotsEnbld && origX - lastX > halfASecondPixels) { ctx.moveTo(x, y); } else { ctx.lineTo(x, y); @@ -3348,7 +3946,7 @@ Item { justOutOfMinMax = false; } - if (i > start && origX >= 0 && (paintDots || x - lastSampleX > dotDistThreshold || origX - lastSampleX > halfASecondPixels)) { + if (dotsEnbld && i > start && origX >= 0 && (paintDots || x - lastSampleX > dotDistThreshold || origX - lastSampleX > halfASecondPixels)) { ctx.stroke(); ctx.beginPath(); if (!drawnLastDot && lastX >= 0) { @@ -3438,6 +4036,109 @@ Item { } } + component StatusText : Item { + property var globState + + MouseArea { + anchors.fill: parent + onClicked: alertsDialog.show() + } + + states: [ + State { + name: "specialMode" + when: statusTextMode.visible || statusTextAlert.visible + AnchorChanges {target: statusTextState; anchors.top: parent.top; anchors.verticalCenter: undefined;} + } + ] + + LText { + id: statusTextState + anchors.verticalCenter: parent.verticalCenter + width: parent.width + font.pixelSize: (statusTextMode.visible && statusTextAlert.visible ? 0.3 : globState.pkgStateString.length >= 10 ? 0.42 : 0.55) * unit + font.family: "Exan" + horizontalAlignment: Text.AlignHCenter + verticalAlignment: Text.AlignVCenter + text: globState.pkgStateString + } + + Glow { + property bool red: globState.isError + property bool dark: globState.darkride + property bool blue: globState.pkgState == globState.s_Running + + anchors.fill: statusTextState + radius: Math.round((dark ? 0.2 : 0.12) * unit) + samples: radius * 2 + 1 + spread: red ? 0.6 : (dark ? 0.7 : 0.5) + opacity: red || dark || blue ? 0.8 : 0.3 + + color: { + if (red) { + return Utility.getAppHexColor("red"); + } else if (dark) { + return Utility.isDarkMode() ? Utility.getAppHexColor("black") : Utility.getAppHexColor("disabledText"); + } else if (blue) { + return Utility.getAppHexColor("lightAccent"); + } else { + return Utility.getAppHexColor("lightText"); + } + } + source: statusTextState + } + + Text { + id: statusTextMode + anchors.top: statusTextState.bottom + width: parent.width + color: globState.pkgMode == globState.m_Flywheel ? Utility.getAppHexColor("lightAccent") : Utility.getAppHexColor("lightText") + font.pixelSize: statusTextAlert.visible ? fontSizeSuperSmall : fontSizeSmall + font.family: "Exan" + horizontalAlignment: Text.AlignHCenter + verticalAlignment: Text.AlignVCenter + visible: globState.pkgMode != globState.m_Normal + text: globState.pkgModeString + } + + Glow { + anchors.fill: statusTextMode + radius: Math.round(0.12 * unit) + samples: radius * 2 + 1 + opacity: 0.3 + color: statusTextMode.color + visible: globState.pkgMode != globState.m_Normal + source: statusTextMode + } + + Text { + id: statusTextAlert + + property var alert: alerts.getActiveAlertData(alerts.activeAlertHighlight) + anchors.horizontalCenter: parent.horizontalCenter + anchors.top: statusTextMode.visible ? statusTextMode.bottom : statusTextState.bottom + width: parent.width * 1.2 + color: alerts.severityColors.get(alert.severity) + font.pixelSize: fontSizeSuperSmall + font.family: "Mono" + horizontalAlignment: Text.AlignHCenter + verticalAlignment: Text.AlignVCenter + elide: Text.ElideMiddle + visible: alerts.activeAlertCount > 0 + text: alert.name + } + + Glow { + anchors.fill: statusTextAlert + radius: Math.round(0.12 * unit) + samples: radius * 2 + 1 + opacity: 0.3 + color: statusTextAlert.color + visible: statusTextAlert.visible + source: statusTextAlert + } + } + function locateButton(comp, name) { for (var i = 0; i < comp.children.length; i++) { var child = comp.children[i]; @@ -3453,6 +4154,15 @@ Item { return null; } + Timer { + id: onStartupTimer + interval: 10 + + onTriggered: { + updateChecker.check(); + } + } + property var customConfigWriteButton: null Component.onCompleted: { @@ -3472,6 +4182,8 @@ Item { if (preferences.showWelcomeDialog) { welcomeDialog.show(); } + + onStartupTimer.start(); } Component.onDestruction: { @@ -3935,16 +4647,6 @@ Item { } } - RightSwitch { - id: prefSwapAdcs - label: "Swap Footpad Sensor ADCs" - } - - OptionDescription { - visible: prefSwapAdcs.checked - text: "Note: This swaps only the sensor visualization. You should swap your ADC1/2 physical connection, as this way, for example, your Konamies (sensor tap codes that control the board) will be reversed as well." - } - RightSwitch { id: prefShowBattVoltage checked: true @@ -4005,12 +4707,6 @@ Item { label: "Show changed options count on tunes" } - RightSwitch { - id: prefShowRTPlotByDefault - checked: false - label: "Show RT plot on Data tab by default" - } - IntTextField { label: "Realtime plot maximum window" valueUnit: "minutes" @@ -4027,12 +4723,59 @@ Item { checked: true label: "Show Welcome dialog" } + + RightSwitch { + id: prefCheckForNewVersions + checked: true + label: "Check for new package versions" + } + + Button { + Layout.alignment: Qt.AlignHCenter + + text: "Show Tips && Tricks" + + onClicked: { + uiHints.reset(); + uiHintsRect.fill(true); + settingsDialog.close(); + } + } } } } } } + BaseDialog { + id: newPackageVersionDialog + parent: ApplicationWindow.overlay + + title: "New Package Version" + standardButtons: Dialog.Ok | Dialog.Abort + + property string version + + function show(newVersion) { + version = newVersion; + standardButton(Dialog.Abort).text = "Snooze for one month" + + open(); + } + + onRejected: { + persistentData.packageUpdateSnooze = Date.now(); + } + + NText { + width: newPackageVersionDialog.availableWidth + font.pointSize: 12 + horizontalAlignment: Text.AlignHCenter + wrapMode: TextEdit.WordWrap + text: "New {{PACKAGE_NAME}} package version available: %1\n\nIt's recommended you update it in the Package Store.\n\nYour package configuration will be migrated automatically, no extra steps needed unless mentioned in package description. Having a manual XML or Start Page \"Backup Configs\" backup is still recommended.".arg(newPackageVersionDialog.version) + } + } + BaseDialog { id: editBoardNameDialog @@ -4199,7 +4942,8 @@ Item { open(); var tuneArchive = tuneManager.loadTuneArchive(); - if (tuneArchive) { + // backwards compatibility: Check for the first item settings not being an array (if it is, redownload) + if (tuneArchive && tuneArchive.length > 0 && !Array.isArray(tuneArchive[0].settings)) { downloadedTunesModel.setTunes(tuneArchive); tuneArchiveDownloadStatus.text = "Tunes downloaded on %1".arg(tuneManager.getTuneArchiveDate()); } else { @@ -4255,9 +4999,248 @@ Item { width: tuneArchiveTunesList.width text: tune.name onClicked: { - applyConfigDialog.show(tune); + applyConfigDialog.show(tune, false, true); + } + } + } + } + } + + BaseDialog { + id: alertsDialog + + title: "Alerts" + standardButtons: Dialog.Close + + function show() { + alertsDialogListModel.set(alerts.alertsLog); + activeAlertsItem.visible = alerts.activeAlertCount > 0; + open(); + } + + Connections { + target: alerts + + function onAlertAdded(alert) { + if (alertsDialog.visible) { + alertsDialogListModel.add(alert); + } + } + + function onActiveAlertCountChanged() { + if (alerts.activeAlertCount > 0) { + activeAlertsItem.visible = true; + } + } + } + + ScrollView { + anchors.fill: parent + contentWidth: alertsDialog.availableWidth + clip: true + + ColumnLayout { + anchors.fill: parent + + component ErrHeader : NText { + Layout.fillWidth: true + font.pixelSize: 16 + horizontalAlignment: Text.AlignHCenter + } + + component ErrText : NText { + Layout.fillWidth: true + font.pixelSize: 12 + font.family: "Mono" + color: Utility.getAppHexColor("red") + } + + component ErrDesc : NText { + Layout.fillWidth: true + font.pixelSize: 12 + horizontalAlignment: Text.AlignHCenter + wrapMode: Text.WordWrap + } + + ErrHeader { + text: "Package Disabled" + visible: state.pkgState === state.s_Disabled + } + + ErrDesc { + text: "Refloat package is disabled. After you're done with board setup, re-enable it." + visible: state.pkgState === state.s_Disabled + } + + Button { + Layout.alignment: Qt.AlignHCenter + + text: "Enable Refloat" + visible: state.pkgState === state.s_Disabled + + onClicked: { + packageConfig.setDisabled(false); + } + } + + ErrHeader { + text: "Connection Errors" + visible: state.configLoadingError || state.pkgState === state.s_Disconnected + } + + ErrText { + text: "Refloat Cfg failed to load" + visible: state.configLoadingError + } + + ErrDesc { + text: "Reconnecting to the board usually fixes this. If problem persists, try reinstalling the package." + visible: state.configLoadingError + } + + ErrText { + text: "Package disconnected" + visible: state.pkgState === state.s_Disconnected + } + + ErrDesc { + text: "Package is not responding. May occur temporarily with spotty connection. If board doesn't engage, the package is dead. Restart the board, if problem persists, try reinstalling the package."; + visible: state.pkgState === state.s_Disconnected + } + + Item { + id: activeAlertsItem + Layout.fillWidth: true + + Column { + id: activeAlertsColumn + width: parent.width + property bool resetHeight: true + + ErrHeader { + width: parent.width + text: "Active Alerts" + } + + Item { + width: parent.width + height: 5 + } + + Repeater { + model: alerts.activeAlerts + + ErrText { + property var alert: alerts.getActiveAlertData(modelData) + text: alert.name + color: alerts.severityColors.get(alert.severity) + } + } + + onHeightChanged: { + if (resetHeight || !activeAlertsItem.visible || activeAlertsItem.implicitHeight < height) { + activeAlertsItem.implicitHeight = height; + resetHeight = false; + } + } + } + + onVisibleChanged: { + activeAlertsColumn.resetHeight = true; + } + } + + ErrHeader { + text: "Fatal Error" + visible: state.fatalError + } + + // Fatal Error is so far only used for FW Fault + ErrDesc { + text: "VESC reported a fault. Be very careful. You should not continue riding before you fix the root cause. " + (alerts.isFatalActive ? "The Fault is still active, you need to resolve the fault and then clear the error." : "The package will stay in an error state until you clear the error.") + visible: state.fatalError + } + + Button { + Layout.alignment: Qt.AlignHCenter + + text: "Clear Fatal Error" + visible: state.fatalError + enabled: !alerts.isFatalActive + + onClicked: { + alerts.clearFatalError(); + } + } + + ErrHeader { + text: "Alert Log" + } + + Rectangle { + color: Utility.getAppHexColor("darkBackground") + Layout.fillWidth: true + Layout.minimumHeight: 200 + + ListView { + id: alertsDialogList + anchors.fill: parent + anchors.margins: 6 + clip: true + + model: ListModel { + id: alertsDialogListModel + + function add(alert) { + var wasAtEnd = alertsDialogList.atYEnd; + + var model = Object.assign({}, alert); + Object.assign(model, alerts.getAlertLogData(alert.id)); + if (!alert.active) { + model.name = model.name + " ended"; + } else { + if (alert.id === alerts.alert_fwFault) { + model.name = model.name + ": %1 (%2)".arg(model.text).arg(model.code); + } + } + append(model); + + if (wasAtEnd) { + alertsDialogList.positionViewAtEnd(); + } + } + + function set(alertList) { + clear(); + for (var alert of alertList.values()) { + add(alert); + } + } + } + + delegate: NText { + id: alertRecordText + width: alertsDialogList.width + + font.pixelSize: 12 + font.family: "Mono" + wrapMode: Text.WordWrap + text: "%1: [%3] %5".arg(Qt.formatTime(new Date(time), "hh:mm:ss.zzz")).arg(alerts.severityColors.get(severity)).arg(severity).arg(active ? Utility.getAppHexColor("lightText") : Utility.getAppHexColor("disabledText")).arg(name) + } } } + + ErrText { + text: "Stop Condition: %2".arg(Utility.getAppHexColor("lightText")).arg(state.stopConditionString) + visible: state.stopConditionString + color: Utility.getAppHexColor("normalText") + } + + ErrText { + text: "Last Beep Reason: %2".arg(Utility.getAppHexColor("lightText")).arg(state.beepReasonString) + visible: state.beepReasonString + color: Utility.getAppHexColor("normalText") + } } } } @@ -4301,15 +5284,16 @@ Item { } function displayValue(item, value) { - if (item.type === "Double" || item.type === "Int") { + var type = packageConfig.getParamType(item.name); + if (type === "Double" || type === "Int") { return value.toString(); - } else if (item.type === "Bool") { + } else if (type === "Bool") { return value ? "On" : "Off"; - } else if (item.type === "Enum") { + } else if (type === "Enum") { return packageConfig.getEnumNames(item.name)[value]; - } else if (item.type === "String") { + } else if (type === "String") { return value; - } else if (item.type === "Bitfield") { + } else if (type === "Bitfield") { return "[option set]"; } @@ -4479,8 +5463,8 @@ Item { NText { Layout.fillWidth: true horizontalAlignment: Text.AlignHCenter - visible: isEmpty && !isTune - text: "Current config is the same as this backup." + visible: isEmpty + text: "There are no differences to your current config." } Item { @@ -4490,11 +5474,13 @@ Item { BaseDialog { id: applyConfigDialog + parent: ApplicationWindow.overlay - title: "Restore Config" + title: isTune ? "Apply Tune" : "Restore Config" property var config property bool automatic: false + property bool isTune: false property var name: config ? config.name : "(Unknown)" property var date: config && config.date ? Qt.formatDateTime(new Date(config.date)) : "" @@ -4506,11 +5492,13 @@ Item { applyConfigDiff.setModel(diff); } - function show(cfg, autoRestore) { + function show(cfg, auto, tune) { config = cfg; - automatic = !!autoRestore; + automatic = !!auto; + isTune = !!tune; applyConfigDiff.reset(); - if (!applyConfigDiff.isEmpty) { + applyConfigDiff.isTune = isTune; + if (!applyConfigDiff.isEmpty && !isTune) { standardButton(Dialog.Apply).text = "Restore"; } open(); @@ -4595,7 +5583,7 @@ Item { horizontalAlignment: Text.AlignHCenter verticalAlignment: Text.AlignVCenter wrapMode: Text.WordWrap - visible: !applyConfigDiff.isEmpty + visible: !applyConfigDiff.isEmpty && !applyConfigDialog.isTune text: "Restoring this backup will overwrite your current config." } } @@ -4666,6 +5654,113 @@ Item { } } + Rectangle { + id: uiHintsRect + anchors.fill: parent + z: 90 + visible: uiHintsSwipe.count > 0 + + color: "#CC000000" + + SwipeView { + id: uiHintsSwipe + anchors.fill: parent + clip: true + + property bool seenAll + + onCurrentIndexChanged: { + if (currentIndex > 0) { + itemAt(currentIndex - 1).shown = true; + } + + if (currentIndex == count - 1) { + seenAll = true; + } + } + + function clear() { + for (let i = count - 1; i >= 0; i--) { + takeItem(i); + } + } + + function markAllShown() { + for (let i = 0; i < count; i++) { + itemAt(i).shown = true; + } + } + } + + NText { + anchors.horizontalCenter: parent.horizontalCenter + anchors.top: parent.top + anchors.margins: 20 + font.pixelSize: 24 + text: "Tips & Tricks" + } + + PageIndicator { + anchors.horizontalCenter: parent.horizontalCenter + anchors.bottom: uiHintsClose.top + anchors.bottomMargin: 10 + + currentIndex: uiHintsSwipe.currentIndex + count: uiHintsSwipe.count + } + + Button { + anchors.bottom: parent.bottom + anchors.bottomMargin: 20 + anchors.horizontalCenter: parent.horizontalCenter + anchors.horizontalCenterOffset: -parent.width * 0.25 + visible: !uiHintsSwipe.seenAll + text: "Skip now" + onClicked: { + uiHintsSwipe.clear(); + } + } + + Button { + id: uiHintsClose + anchors.bottom: parent.bottom + anchors.bottomMargin: 20 + anchors.horizontalCenter: parent.horizontalCenter + anchors.horizontalCenterOffset: uiHintsSwipe.seenAll ? 0 : parent.width * 0.25 + text: uiHintsSwipe.seenAll ? "Close" : "Don't show again" + onClicked: { + uiHintsSwipe.markAllShown(); + uiHintsSwipe.clear(); + } + } + + function fill(doDataRecord=false) { + uiHints.fill(uiHintsSwipe); + if (doDataRecord) { + uiHints.fillDataRecord(uiHintsSwipe); + } + + uiHintsSwipe.seenAll = uiHintsSwipe.currentIndex == uiHintsSwipe.count - 1; + } + + function fillDataRecord() { + uiHints.fillDataRecord(uiHintsSwipe); + uiHintsSwipe.seenAll = uiHintsSwipe.currentIndex == uiHintsSwipe.count - 1; + } + + Component.onCompleted: { + fill(); + } + + Connections { + target: state + + function onCommsInitialized() { + uiHintsRect.fillDataRecord(); + } + } + } + // Color Preview dialog with an overview of the colors. Uncomment this and the // FloatingToolButton that shows it to enable it. // Rectangle { @@ -5273,8 +6368,8 @@ Item { width: parent.height height: width - property real leftVoltage: preferences.swapAdcs ? state.rtData["footpad.adc2"] : state.rtData["footpad.adc1"] - property real rightVoltage: preferences.swapAdcs ? state.rtData["footpad.adc1"] : state.rtData["footpad.adc2"] + property real leftVoltage: state.rtData["footpad.adc1"] + property real rightVoltage: state.rtData["footpad.adc2"] property real canvasWidth: width * 0.85 property real scale: canvasWidth / footpadPath.width @@ -5404,10 +6499,22 @@ Item { width: parent.height height: width - property real value: state.rtData["imu.roll"] + property real newValue: state.rtData["imu.roll"] + property real preValue: newValue + onNewValueChanged: { + if (Math.abs(newValue - preValue) > 180) { + rollEasingBehavior.enabled = false; + value += newValue > 0 ? 360 : -360; + rollEasingBehavior.enabled = true; + } + value = newValue; + preValue = newValue; + } + property real value: newValue onValueChanged: rollCanvas.requestPaint(); Behavior on value { + id: rollEasingBehavior NumberAnimation { easing.type: Easing.OutCirc duration: 100 @@ -5738,79 +6845,13 @@ Item { value: state.rtData["motor.mosfet_temp"] } - Item { + StatusText { id: statusText anchors.verticalCenter: parent.verticalCenter anchors.horizontalCenter: parent.horizontalCenter width: parent.width - 4 * parent.verticalValueItemWidth height: parent.height * 0.83 - - property bool red: state.pkgStateIsError - property bool dark: state.darkride - property bool blue: state.pkgState == state.s_Running - - states: [ - State { - name: "specialMode" - when: state.pkgMode != state.m_Normal - AnchorChanges {target: statusTextState; anchors.top: parent.top; anchors.verticalCenter: undefined;} - } - ] - - LText { - id: statusTextState - anchors.verticalCenter: parent.verticalCenter - width: parent.width - font.pixelSize: (state.pkgStateString.length >= 10 ? 0.42 : 0.55) * unit - font.family: "Exan" - horizontalAlignment: Text.AlignHCenter - verticalAlignment: Text.AlignVCenter - text: state.pkgStateString - } - - Glow { - anchors.fill: statusTextState - radius: Math.round((parent.dark ? 0.2 : 0.12) * unit) - samples: radius * 2 + 1 - spread: parent.red ? 0.6 : (parent.dark ? 0.7 : 0.5) - opacity: parent.red || parent.dark || parent.blue ? 0.8 : 0.3 - color: { - if (parent.red) { - return Utility.getAppHexColor("red"); - } else if (parent.dark) { - return Utility.isDarkMode() ? Utility.getAppHexColor("black") : Utility.getAppHexColor("disabledText"); - } else if (parent.blue) { - return Utility.getAppHexColor("lightAccent"); - } else { - return Utility.getAppHexColor("lightText"); - } - } - source: statusTextState - } - - Text { - id: statusTextMode - anchors.bottom: parent.bottom - width: parent.width - color: state.pkgMode == state.m_Flywheel ? Utility.getAppHexColor("lightAccent") : Utility.getAppHexColor("lightText") - font.pixelSize: fontSizeSmall - font.family: "Exan" - horizontalAlignment: Text.AlignHCenter - verticalAlignment: Text.AlignVCenter - visible: state.pkgMode != state.m_Normal - text: state.pkgModeString - - } - - Glow { - anchors.fill: statusTextMode - radius: Math.round(0.12 * unit) - samples: radius * 2 + 1 - opacity: 0.3 - color: statusTextMode.color - visible: state.pkgMode != state.m_Normal - source: statusTextMode - } + globState: state } VerticalValue { @@ -5828,53 +6869,16 @@ Item { unit: vescConfig.distanceUnit } } - - Text { - Layout.fillWidth: true - property string fault - - color: Utility.getAppHexColor("red") - font.pixelSize: fontSizeNormal - horizontalAlignment: Text.AlignHCenter - visible: state.isError && !state.pkgStateIsError - text: state.errorText - } } TabBar { + id: bottomStackTabs Layout.fillWidth: true clip: true TabButton {text: "Tunes"} - TabButton {text: "Control"} - - TabButton { - id: tabBarDebugButton - text: "Data" - - property bool isError: state.isError - property bool glowVisible: false - - onIsErrorChanged: { - glowVisible = isError && !checked - } - - onToggled: { - glowVisible = false - } - - Glow { - anchors.fill: tabBarDebugButton.contentItem - radius: Math.round(0.12 * unit) - samples: radius * 2 + 1 - spread: 0.8 - opacity: 0.8 - visible: parent.glowVisible - color: Utility.getAppHexColor("red") - source: tabBarDebugButton.contentItem - } - } + TabButton {text: "Data"} background: Rectangle { color: Utility.getAppHexColor("lightBackground"); @@ -6192,6 +7196,7 @@ Item { width: 0.8 * unit height: 0.8 * unit z: 1 + value: true trueIconPath: Path { PathSvg {path: "M 6 13 L 31 13 M 6 21 L 21 21 M 6 29 L 21 29 M 6 37 L 21 37 M 29 21 L 44 21 M 29 29 L 44 29 M 29 37 L 44 37"} @@ -6200,10 +7205,6 @@ Item { falseIconPath: Path { PathSvg {path: "M 10 12 L 10 38 L 42 38 M 10 30 L 22 22 L 32 28 L 39 18"} } - - Component.onCompleted: { - value = preferences.showRTPlotByDefault; - } } ScrollView { @@ -6252,28 +7253,6 @@ Item { height: 0.1 * unit } - LText { - width: parent.width - - font.pixelSize: fontSizeBig - horizontalAlignment: Text.AlignHCenter - visible: state.isError - text: "Error" - } - - LText { - width: parent.width - - font.pixelSize: fontSizeNormal - horizontalAlignment: Text.AlignHCenter - visible: state.isError - wrapMode: Text.WordWrap - text: state.errorDescription - } - - Value {id: debugStopReason; label: "Stop Condition"; value: state.stopConditionString} - Value {id: debugLastBeepReason; label: "Last Beep Reason"; value: state.beepReasonString} - Item { width: parent.width height: 0.1 * unit diff --git a/refloat/version b/refloat/version index 45a1b3f4..26aaba0e 100644 --- a/refloat/version +++ b/refloat/version @@ -1 +1 @@ -1.1.2 +1.2.0 diff --git a/refloat/vesc_pkg_lib/rules.mk b/refloat/vesc_pkg_lib/rules.mk index 66247832..2cf229ee 100644 --- a/refloat/vesc_pkg_lib/rules.mk +++ b/refloat/vesc_pkg_lib/rules.mk @@ -33,7 +33,7 @@ ifeq ($(USE_OPT),) endif CFLAGS = -fpic -Os -Wall -Wextra -Wundef -std=gnu99 -I$(VESC_C_LIB_PATH) -CFLAGS += -I$(STLIB_PATH)/CMSIS/include -I$(STLIB_PATH)/CMSIS/ST -I$(UTILS_PATH)/ +CFLAGS += -I$(STLIB_PATH)/CMSIS/include -I$(STLIB_PATH)/CMSIS/ST -I$(STLIB_PATH)/inc -I$(UTILS_PATH)/ CFLAGS += -fomit-frame-pointer -falign-functions=16 -mthumb CFLAGS += -fsingle-precision-constant -Wdouble-promotion CFLAGS += -mfloat-abi=hard -mfpu=fpv4-sp-d16 -mcpu=cortex-m4 @@ -42,7 +42,7 @@ CFLAGS += -DIS_VESC_LIB CFLAGS += $(USE_OPT) ifeq ($(USE_STLIB),yes) - CFLAGS += -DUSE_STLIB -I$(STLIB_PATH)/inc + CFLAGS += -DUSE_STLIB endif LDFLAGS = -nostartfiles -static -mfloat-abi=hard -mfpu=fpv4-sp-d16 -mcpu=cortex-m4 diff --git a/refloat/vesc_pkg_lib/st_types.h b/refloat/vesc_pkg_lib/st_types.h index 96444d00..8599b446 100755 --- a/refloat/vesc_pkg_lib/st_types.h +++ b/refloat/vesc_pkg_lib/st_types.h @@ -23,10 +23,7 @@ #include #include #include "system_stm32f4xx.h" - -#ifdef USE_STLIB #include "stm32f4xx_conf.h" -#endif typedef struct { volatile uint32_t MODER; diff --git a/refloat/vesc_pkg_lib/vesc_c_if.h b/refloat/vesc_pkg_lib/vesc_c_if.h index f612bb90..c9fc356c 100755 --- a/refloat/vesc_pkg_lib/vesc_c_if.h +++ b/refloat/vesc_pkg_lib/vesc_c_if.h @@ -297,6 +297,11 @@ typedef enum { CFG_PARAM_si_battery_cells, CFG_PARAM_si_battery_ah, CFG_PARAM_si_motor_nl_current, + + // Motor FOC Parameters + CFG_PARAM_foc_motor_r, + CFG_PARAM_foc_motor_l, + CFG_PARAM_foc_motor_flux_linkage, } CFG_PARAM; typedef struct { @@ -374,7 +379,7 @@ typedef struct { int (*printf)(const char *str, ...); void* (*malloc)(size_t bytes); void (*free)(void *prt); - lib_thread (*spawn)(void (*fun)(void *arg), size_t stack_size, char *name, void *arg); + lib_thread (*spawn)(void (*fun)(void *arg), size_t stack_size, const char *name, void *arg); void (*request_terminate)(lib_thread thd); bool (*should_terminate)(void); void** (*get_arg)(uint32_t prog_addr); @@ -490,7 +495,7 @@ typedef struct { // UART bool (*uart_start)(uint32_t baudrate, bool half_duplex); - bool (*uart_write)(uint8_t *data, uint32_t size); + bool (*uart_write)(const uint8_t *data, uint32_t size); int32_t (*uart_read)(void); // Packets @@ -510,7 +515,7 @@ typedef struct { void (*imu_get_accel)(float *accel); void (*imu_get_gyro)(float *gyro); void (*imu_get_mag)(float *mag); - void (*imu_derotate)(float *input, float *output); + void (*imu_derotate)(const float *input, float *output); void (*imu_get_accel_derotated)(float *accel); void (*imu_get_gyro_derotated)(float *gyro); void (*imu_get_quaternions)(float *q); @@ -535,8 +540,8 @@ typedef struct { float (*timeout_secs_since_update)(void); // Plot - void (*plot_init)(char *namex, char *namey); - void (*plot_add_graph)(char *name); + void (*plot_init)(const char *namex, const char *namey); + void (*plot_add_graph)(const char *name); void (*plot_set_graph)(int graph); void (*plot_send_points)(float x, float y); @@ -580,12 +585,12 @@ typedef struct { // IMU AHRS functions and read callback void (*imu_set_read_callback)(void (*func)(float *acc, float *gyro, float *mag, float dt)); void (*ahrs_init_attitude_info)(ATTITUDE_INFO *att); - void (*ahrs_update_initial_orientation)(float *accelXYZ, float *magXYZ, ATTITUDE_INFO *att); - void (*ahrs_update_mahony_imu)(float *gyroXYZ, float *accelXYZ, float dt, ATTITUDE_INFO *att); - void (*ahrs_update_madgwick_imu)(float *gyroXYZ, float *accelXYZ, float dt, ATTITUDE_INFO *att); - float (*ahrs_get_roll)(ATTITUDE_INFO *att); - float (*ahrs_get_pitch)(ATTITUDE_INFO *att); - float (*ahrs_get_yaw)(ATTITUDE_INFO *att); + void (*ahrs_update_initial_orientation)(const float *accelXYZ, const float *magXYZ, ATTITUDE_INFO *att); + void (*ahrs_update_mahony_imu)(const float *gyroXYZ, const float *accelXYZ, float dt, ATTITUDE_INFO *att); + void (*ahrs_update_madgwick_imu)(const float *gyroXYZ, const float *accelXYZ, float dt, ATTITUDE_INFO *att); + float (*ahrs_get_roll)(const ATTITUDE_INFO *att); + float (*ahrs_get_pitch)(const ATTITUDE_INFO *att); + float (*ahrs_get_yaw)(const ATTITUDE_INFO *att); // Set custom encoder callbacks void (*encoder_set_custom_callbacks)( @@ -647,7 +652,7 @@ typedef struct { bool (*foc_beep)(float freq, float time, float voltage); bool (*foc_play_tone)(int channel, float freq, float voltage); void (*foc_stop_audio)(bool reset); - bool (*foc_set_audio_sample_table)(int channel, float *samples, int len); + bool (*foc_set_audio_sample_table)(int channel, const float *samples, int len); const float* (*foc_get_audio_sample_table)(int channel); bool (*foc_play_audio_samples)(const int8_t *samples, int num_samp, float f_samp, float voltage); @@ -657,6 +662,14 @@ typedef struct { void (*sem_signal)(lib_semaphore); bool (*sem_wait_to)(lib_semaphore, systime_t); // Returns false on timeout void (*sem_reset)(lib_semaphore); + + // Functions below were added in firmware 6.06 + + // Set priority of current thread + // Range: -5 to 5, -5 is lowest, 0 is normal, 5 is highest + void (*thread_set_priority)(int priority); + // Disable shutdown (for hw with momentary button / auto shutdown support) + void (*shutdown_disable)(bool disable); } vesc_c_if; typedef struct { @@ -672,7 +685,7 @@ typedef struct { #define VESC_IF ((vesc_c_if*)(0x1000F800)) // Put this at the beginning of your source file -#define HEADER static volatile int __attribute__((__section__(".program_ptr"))) prog_ptr; +#define HEADER volatile int __attribute__((__section__(".program_ptr"))) prog_ptr; // Init function #define INIT_FUN bool __attribute__((__section__(".init_fun"))) init @@ -686,5 +699,7 @@ typedef struct { // The argument that was set in the init function (same as the one you get in stop_fun) #define ARG (*VESC_IF->get_arg(PROG_ADDR)) +extern volatile int prog_ptr; + #endif // VESC_C_IF_H