diff --git a/cmake/main.cmake b/cmake/main.cmake index d8e2dddf3ec..1b987204b25 100644 --- a/cmake/main.cmake +++ b/cmake/main.cmake @@ -108,10 +108,10 @@ function(setup_firmware_target exe name) endfunction() function(exclude_from_all target) - set_property(TARGET ${target} PROPERTY + set_target_properties(${target} PROPERTIES TARGET_MESSAGES OFF - EXCLUDE_FROM_ALL 1 - EXCLUDE_FROM_DEFAULT_BUILD 1) + EXCLUDE_FROM_ALL ON + EXCLUDE_FROM_DEFAULT_BUILD ON) endfunction() function(collect_targets) diff --git a/cmake/sitl.cmake b/cmake/sitl.cmake index c94fca21209..1983175c8de 100644 --- a/cmake/sitl.cmake +++ b/cmake/sitl.cmake @@ -19,8 +19,8 @@ main_sources(SITL_SRC target/SITL/sim/realFlight.h target/SITL/sim/simHelper.c target/SITL/sim/simHelper.h - target/SITL/sim/simple_soap_client.c - target/SITL/sim/simple_soap_client.h + target/SITL/sim/soap_client.c + target/SITL/sim/soap_client.h target/SITL/sim/xplane.c target/SITL/sim/xplane.h ) @@ -58,6 +58,12 @@ if(DEBUG) list(APPEND SITL_COMPILE_OPTIONS -g) endif() +if(ASAN) + message(STATUS "AddressSanitizer enabled.") + list(APPEND SITL_COMPILE_OPTIONS -fsanitize=address -fno-omit-frame-pointer) + list(APPEND SITL_LINK_OPTIONS -fsanitize=address) +endif() + if(NOT MACOSX) set(SITL_COMPILE_OPTIONS ${SITL_COMPILE_OPTIONS} -Wno-return-local-addr @@ -163,8 +169,8 @@ function (target_sitl name) WORKING_DIRECTORY ${CMAKE_BINARY_DIR} COMMAND ${generator_cmd} clean COMMENT "Removing intermediate files for ${name}") - set_property(TARGET ${clean_target} PROPERTY - EXCLUDE_FROM_ALL 1 - EXCLUDE_FROM_DEFAULT_BUILD 1) + set_target_properties(${clean_target} PROPERTIES + EXCLUDE_FROM_ALL ON + EXCLUDE_FROM_DEFAULT_BUILD ON) endif() endfunction() diff --git a/docs/Backup and Restore.md b/docs/Backup and Restore.md new file mode 100644 index 00000000000..771b129762d --- /dev/null +++ b/docs/Backup and Restore.md @@ -0,0 +1,107 @@ +# Backup and Restore + +INAV Configurator can automatically back up your configuration before flashing firmware and offer to restore it afterwards. When upgrading across major versions (e.g. 7.x → 8.x → 9.x), settings are automatically migrated to the new firmware format. + +For manual CLI-based backup and restore, see the [CLI documentation](Cli.md#backup-via-cli). + +## Automatic Backup & Restore During Firmware Flash + +### What happens automatically + +1. **Before flashing** (with or without Full Chip Erase enabled): Your current CLI configuration (`diff all`) is automatically captured and saved to the backup directory. +2. **After flashing**: Depending on the situation, the Configurator offers to restore your settings if Full Chip Erase was enabled: + +| Scenario | Behavior | +|----------|----------| +| **Patch update** (e.g. 8.0.0 → 8.0.1) | Auto-restore offered immediately | +| **Minor update** (e.g. 8.0.0 → 8.1.0) | Auto-restore offered immediately | +| **Major upgrade** (e.g. 7.x → 8.x) with migration profile available | Migration preview shown — confirm to restore with converted settings | +| **Major upgrade** without migration profile | Warning shown — restore still possible but some settings may fail | +| **Major downgrade** (e.g. 9.x → 7.x) | Auto-restore blocked — manual restore only (settings may be incompatible) | +| **Local firmware file** (loaded from disk) | No auto-restore offered — backup is still saved | +| **Flash without Full Chip Erase** | Backup taken, no restore offered | + +### Migration Preview + +When updating across major versions (e.g. 7.x → 9.x), the Configurator shows a **migration preview overlay** before restoring. This lists: + +- **Removed settings** — settings that no longer exist in the new firmware (will be skipped) +- **Renamed settings** — settings whose name changed (automatically converted) +- **Renamed commands** — CLI commands that were renamed (automatically converted) +- **Value replacements** — setting values that changed meaning (automatically converted) +- **Setting remappings** — numeric IDs that were renumbered (automatically converted) +- **Warnings** — settings whose semantics changed and require manual review + +You can review all changes before confirming or cancelling the restore. + +Multi-step migrations are handled automatically. For example, a 7.x → 9.x upgrade applies migration profiles in sequence (7→8, then 8→9). + +## Manual Backup & Restore + +The Firmware Flasher tab provides three buttons: + +- **Backup Config** — saves your current CLI configuration to a file (opens save dialog) +- **Restore Config** — loads a backup file and restores it to your flight controller + - If the backup is from a different major version, the migration preview is shown first + - If no migration profile exists for the version gap, a warning is shown but you can still proceed +- **Open Backups Folder** — opens the backup directory in your file manager + +For CLI-based backup and restore procedures, see [Backup via CLI](Cli.md#backup-via-cli) and [Restore via CLI](Cli.md#restore-via-cli). + +## Backup File Location + +Backups are stored in your OS-specific application data directory: + +| OS | Path | +|----|------| +| **Windows** | `%APPDATA%/inav-configurator/backups/` | +| **macOS** | `~/Library/Application Support/inav-configurator/backups/` | +| **Linux** | `~/.config/inav-configurator/backups/` | + +Use the **Open Backups Folder** button in the Firmware Flasher tab to open this directory. + +## Backup File Naming + +| Type | Format | +|------|--------| +| Auto-backups | `UPDATE_inav_backup_{version}_{board}_{YYYY-MM-DD_HHMMSS}.txt` | +| Manual backups | `inav_backup_{version}_{board}_{YYYY-MM-DD_HHMMSS}.txt` | + +Auto-backups are pruned automatically — only the 10 most recent are kept. Files you rename are never pruned. + +## Backup File Format + +Backup files are plain-text CLI dumps with a metadata header: + +``` +# INAV Backup +# Version: 8.0.0 +# Board: SPEEDYBEEF405V4 +# Date: 2026-04-11T10:30:00.000Z +# Craft: MyQuad +# +# INAV/SPEEDYBEEF405V4 8.0.0 Apr 1 2026 / 12:00:00 (abc1234) +# GCC-13.2.1 +# ... +set gyro_main_lpf_hz = 110 +set acc_hardware = AUTO +... +``` + +You can open and edit these files with any text editor. + +## Restore Error Handling + +If errors occur during restore (e.g. unknown settings, invalid values): + +- An error dialog shows the affected lines +- You can choose: + - **Save anyway** — saves the successfully applied settings and reboots + - **Abort** — discards all changes and exits CLI mode + +## Tips + +- **Always flash with Full Chip Erase** when upgrading to a new version. This ensures clean defaults and triggers automatic backup and restore. +- **Review migration previews carefully** — especially the warnings section, which highlights settings whose meaning may have changed. +- **Keep manual backups** before major upgrades. While auto-backup handles this, having an extra copy in a known location gives peace of mind. +- **Use `diff` over `dump`** for backups. The `diff` format only stores settings that differ from defaults, which makes restoring safer across versions. The auto-backup feature already uses `diff all`. diff --git a/docs/Cli.md b/docs/Cli.md index d949937a61b..d27029ee1b5 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -22,6 +22,8 @@ See the other documentation sections for details of the cli commands and setting ## Backup via CLI +> **Note:** The INAV Configurator now performs automatic backups before flashing and can restore settings afterwards, including migration across major versions. See [Backup and Restore](Backup%20and%20Restore.md) for details. The CLI method below remains available for manual backup. + Disconnect main power, connect to cli via USB/FTDI. dump using cli diff --git a/docs/Configuration.md b/docs/Configuration.md index 1ccea15d110..f48de1a10d6 100644 --- a/docs/Configuration.md +++ b/docs/Configuration.md @@ -10,7 +10,7 @@ See the Serial section for more information and see the Board specific sections The GUI cannot currently configure all aspects of the system, the CLI must be used to enable or configure some features and settings. -__Due to ongoing development, the fact that the GUI cannot yet backup all your settings and automatic chrome updates of the GUI app it is highly advisable to backup your settings (using the CLI) so that when a new version of the configurator or firmware is released you can re-apply your settings.__ +__The INAV Configurator now (versions after 9.0.x) automatically backs up your settings before flashing firmware and can restore them afterwards (when Full Chip Erase is enabled), including automatic migration across major versions. For details, see [Backup and Restore](Backup%20and%20Restore.md). You can also manually backup and restore settings using the [CLI](Cli.md#backup-via-cli).__ ## GUI diff --git a/docs/Controls.md b/docs/Controls.md index 6dbc26df5fc..a84267ffbfe 100644 --- a/docs/Controls.md +++ b/docs/Controls.md @@ -31,7 +31,7 @@ The stick positions are combined to activate different functions: | Battery profile 3 | HIGH | LOW | CENTER | HIGH | | Calibrate Gyro | LOW | LOW | LOW | CENTER | | Calibrate Acc | HIGH | LOW | LOW | CENTER | -| Calibrate Mag/Compass | HIGH | HIGH | LOW | CENTER | +| Calibrate Compass/Zero Yaw | HIGH | HIGH | LOW | CENTER | | Trim Acc Left | HIGH | CENTER | CENTER | LOW | | Trim Acc Right | HIGH | CENTER | CENTER | HIGH | | Trim Acc Forwards | HIGH | CENTER | HIGH | CENTER | @@ -52,6 +52,14 @@ The stick positions are combined to activate different functions: For graphical stick position in all transmitter modes, check out [this page](https://www.mrd-rc.com/tutorials-tools-and-testing/inav-flight/inav-stick-commands-for-all-transmitter-modes/). ![Stick Positions](assets/images/StickPositions.png) +## Compass Calibration and Yaw Zero Reset + +The stick function `Calibrate Compass/Zero Yaw` provides 2 functions depending on whether or not a compass is available. + +If a compass is available the stick function initiates the compass calibration routine. + +If no compass is available the stick function will reset the current yaw/heading estimate to zero (North) and also set the heading as trusted. This is useful on multirotors, allowing the craft yaw/heading to be correctly aligned to actual North simply by physically pointing the craft North then using the stick function to zero the yaw estimate. Since this also sets the heading as trusted Nav modes reliant on heading will be available immediately after arming without the need to fly fast enough to obtain a valid heading from GPS ground course. + ## Yaw control While arming/disarming with sticks, your yaw stick will be moving to extreme values. In order to prevent your craft from trying to yaw during arming/disarming while on the ground, your yaw input will not cause the craft to yaw when the throttle is LOW (i.e. below the `min_check` setting). diff --git a/docs/Installation.md b/docs/Installation.md index 38e9470fd81..140f47bd6c9 100644 --- a/docs/Installation.md +++ b/docs/Installation.md @@ -27,4 +27,8 @@ When upgrading be sure to backup / dump your existing settings. Some firmware r ## Backup/Restore process -See the CLI section of the docs for details on how to backup and restore your configuration via the CLI. +The INAV Configurator (after version 9.0.x) automatically backs up your configuration before flashing and offers to restore it afterwards (when Full Chip Erase is enabled) — including automatic settings migration when upgrading across major versions. + +For details on automatic and manual backup/restore, see [Backup and Restore](Backup%20and%20Restore.md). + +For CLI-based backup and restore, see the [CLI documentation](Cli.md#backup-via-cli). diff --git a/docs/Rx.md b/docs/Rx.md index 0f0ae64a453..3c9f5497448 100644 --- a/docs/Rx.md +++ b/docs/Rx.md @@ -201,7 +201,7 @@ bind_msp_rx ## MultiWii serial protocol (MSP RX) -Allows you to use MSP commands as the RC input. Up to 18 channels are supported. +Allows you to use MSP commands as the RC input. Up to 34 channels are supported. Note: * It is necessary to update `MSP_SET_RAW_RC` at 5Hz or faster. * `MSP_SET_RAW_RC` uses the defined RC channel map @@ -213,6 +213,31 @@ Note: Enables the use of a joystick in the INAV SITL with a flight simulator. See the [SITL documentation](SITL/SITL.md). +## MSP Auxiliary RC Channel Overlay (MSP2_INAV_SET_AUX_RC) + +Allows extending the available RC channel count beyond the native RC link capacity using `MSP2_INAV_SET_AUX_RC` (`0x2230`). This is a lightweight, bandwidth-efficient alternative to `MSP_SET_RAW_RC` for auxiliary channels only. + +**Key properties:** +- Controls **CH13–CH32** only (CH1–CH12 are protected and rejected) +- Configurable resolution: 2-bit (3 positions), 4-bit (~71µs steps), 8-bit (~3.9µs steps), or 16-bit (raw PWM) +- Value `0` = skip (no update) — previous value persists indefinitely +- No flight mode or special configuration required — always active +- Does **not** affect failsafe detection +- Recommended to send with `MSP_FLAG_DONT_REPLY` (`flags=0x01`) on telemetry passthrough links + +**Typical use case:** A Lua script on the radio sends `MSP2_INAV_SET_AUX_RC` via SmartPort/CRSF/ELRS telemetry passthrough to control auxiliary functions (lights, camera triggers, gimbal modes) on channels beyond the RC link's native capacity. + +**Priority order** (last writer wins): +1. Primary RX (SBUS, CRSF, FPort, etc.) +2. MSP RC Override (if active) +3. **MSP AUX Overlay** (CH13–CH32) + +**Important:** For serial RX protocols, the firmware cannot detect which channels the sender actively uses. If AUX_RC targets a channel that the RX link also sends, AUX_RC will override it. Configure the start channel above your RC link's active channel range. + +When MSP is the primary RX provider (`receiver_type = MSP`), channels covered by `MSP_SET_RAW_RC` are automatically protected. Channels in the `msp_override_channels` bitmask are also protected when MSP RC Override mode is active. + +See the [MSP documentation](development/msp/README.md) for the full message format. + ## Configuration The receiver type can be set from the configurator or CLI. diff --git a/docs/Settings.md b/docs/Settings.md index ad03f282e3f..39e3de90d52 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -5624,7 +5624,7 @@ Defines rotation rate on PITCH axis that UAV will try to archive on max. stick d ### pitot_hardware -Selection of pitot hardware. +Selection of pitot hardware. VIRTUAL only works if a GPS is enabled. | Default | Min | Max | | --- | --- | --- | diff --git a/docs/Telemetry.md b/docs/Telemetry.md index 67e43312406..0adc36cb929 100644 --- a/docs/Telemetry.md +++ b/docs/Telemetry.md @@ -413,4 +413,4 @@ It runs at a fixed baud rate of 100000, so it needs a hardware uart capable of i \_______/ \-------------/ ``` -For more information and sensor slot numbering, refer to [SBUS2 Documentation](SBUS2_telemetry.md) \ No newline at end of file +For more information and sensor slot numbering, refer to [SBUS2 Documentation](SBUS2_Telemetry.md) diff --git a/docs/development/Backup Restore Architecture.md b/docs/development/Backup Restore Architecture.md new file mode 100644 index 00000000000..9dd971230c6 --- /dev/null +++ b/docs/development/Backup Restore Architecture.md @@ -0,0 +1,251 @@ +# Backup, Restore & Settings Migration — Architecture + +> **Note:** This document describes the internals of the INAV Configurator's backup/restore and settings migration system. It is intended for Configurator developers, not end users. For user-facing documentation, see the [INAV docs](https://github.com/iNavFlight/inav/blob/master/docs/Backup%20and%20Restore.md). + +## Architecture Overview + +``` +User → Firmware Flasher Tab → STM32.connect(onCliReady) → CLI mode + ↓ + BackupRestore.captureCliDiffAll() + ↓ + Save to file, prune old backups + ↓ + STM32 flash (DFU or serial) + ↓ + onFlashComplete() callback + ↓ + Version check → Migration check → UI overlay + ↓ + User confirms → Poll for FC reconnect + ↓ + BackupRestore.performRestore() or performRestoreWithMigration() + ↓ + saveAndReboot() or abortRestore() +``` + +## Files + +| File | Purpose | +|------|---------| +| `js/backup_restore.js` | Core backup/restore module — CLI protocol, file I/O, auto-backup | +| `js/migration/migration_handler.js` | Version migration engine — profile chaining, line transformation | +| `js/migration/7_to_8.json` | Migration profile: INAV 7.x → 8.0 | +| `js/migration/8_to_9.json` | Migration profile: INAV 8.0 → 9.0 | +| `tabs/firmware_flasher.js` | Flash integration — auto-backup trigger, restore UI, version gating | +| `tabs/firmware_flasher.html` | Overlays and buttons for backup/restore/migration UI | +| `src/css/tabs/firmware_flasher.css` | Overlay styles | +| `js/protocols/stm32.js` | STM32 flash protocol — `onCliReady` callback, DFU timeout fix | +| `js/main/main.js` | Electron main process — IPC handlers for file operations | +| `js/main/preload.js` | IPC bridge — exposes backup API to renderer | +| `locale/en/messages.json` | All i18n translation keys | + +## Adding a New Migration Profile + +When a new major INAV version is released (e.g. 9.x → 10.x), create a migration profile: + +### Step 1: Create the JSON profile + +Create `js/migration/9_to_10.json`: + +```json +{ + "fromVersion": "9", + "toVersion": "10", + "description": "INAV 9.x → 10.0 migration profile", + + "commandRenames": { + "old_command_name": "new_command_name" + }, + + "settingRenames": { + "old_setting_name": "new_setting_name" + }, + + "valueReplacements": { + "setting_name": { + "OLD_VALUE": "NEW_VALUE" + } + }, + + "removed": [ + "deleted_setting_1", + "deleted_setting_2" + ], + + "settingPatternMappings": [ + { + "pattern": "^regex_matching_setting_names$", + "valueMap": { "old_numeric_id": "new_numeric_id" }, + "description": "Human-readable description of remapping" + } + ], + + "warnings": [ + "Human-readable warning about settings whose semantics changed and need manual review." + ] +} +``` + +### Step 2: Register the profile + +In `js/migration/migration_handler.js`, add the import and append to the array: + +```javascript +import profile_9_to_10 from './9_to_10.json'; + +const MIGRATION_PROFILES = [ + profile_7_to_8, + profile_8_to_9, + profile_9_to_10, // ← add here +]; +``` + +The migration engine automatically chains profiles. A 7.x → 10.x upgrade will apply all three profiles in sequence (7→8, 8→9, 9→10). + +### How to determine what goes into a migration profile + +Compare CLI settings between the old and new firmware version: + +1. **Removed settings**: Run `diff all` on old and new firmware with default settings. Settings present in old but not in new → add to `removed` +2. **Renamed settings**: Check INAV release notes and source code for renamed settings → add to `settingRenames` +3. **Renamed commands**: Check for CLI command name changes (e.g. `profile` → `control_profile`) → add to `commandRenames` +4. **Value replacements**: Check for enum value name changes → add to `valueReplacements` +5. **Pattern mappings**: Check for bulk ID renumbering (OSD elements, etc.) → add to `settingPatternMappings` +6. **Warnings**: Check for settings where the meaning/units changed but name stayed the same → add to `warnings` + +Key INAV source files to check: +- `src/main/fc/settings.yaml` — all CLI settings definitions +- `src/main/fc/cli.c` — CLI command implementations +- Release notes on GitHub + +## Migration Profile Schema Reference + +| Field | Type | Required | Description | +|-------|------|----------|-------------| +| `fromVersion` | `string` | Yes | Source major version number (e.g. `"9"`) | +| `toVersion` | `string` | Yes | Target major version number (e.g. `"10"`) | +| `description` | `string` | Yes | Human-readable description | +| `commandRenames` | `object` | Yes | Maps old CLI command names to new names. Applied to any token in the command line. E.g. `"profile" → "control_profile"` transforms `profile 2` to `control_profile 2` | +| `settingRenames` | `object` | Yes | Maps old `set` setting names to new names. Only applies to `set = ` lines | +| `valueReplacements` | `object` | Yes | Maps setting names to value replacement objects `{ "oldval": "newval" }`. Only modifies the value portion after `=` | +| `removed` | `string[]` | Yes | List of setting names to remove entirely. Lines with `set = ...` matching these are dropped | +| `settingPatternMappings` | `array` | Yes | Array of pattern-based value remappings for settings matching a regex. Each entry has `pattern` (regex), `valueMap` (object), `description` (string) | +| `warnings` | `string[]` | Yes | Warning messages about semantic changes requiring manual review. Displayed in migration preview overlay | + +## Existing Migration Profile Details + +### 7_to_8.json (INAV 7.x → 8.0) + +| Category | Changes | +|----------|---------| +| **Command renames** | `profile` → `control_profile` | +| **Value replacements** | `gps_provider`: `UBLOX7` → `UBLOX` | +| **Removed settings** (18) | `control_deadband`, `cpu_underclock`, `disarm_kill_switch`, `dji_workarounds`, `fw_iterm_limit_stick_position`, `gyro_anti_aliasing_lpf_type`, `gyro_hardware_lpf`, `gyro_main_lpf_type`, `gyro_use_dyn_lpf`, `inav_use_gps_no_baro`, `inav_use_gps_velned`, `ledstrip_visual_beeper`, `max_throttle`, `nav_auto_climb_rate`, `nav_manual_climb_rate`, `osd_stats_min_voltage_unit`, `pidsum_limit`, `pidsum_limit_yaw` | +| **Pattern mappings** | `osd_custom_element_N_type`: IDs remapped 4→9, 5→16, 6→7, 7→10 | +| **Warnings** | `nav_fw_wp_tracking_accuracy` semantics changed: was arbitrary tracking response, now distance in meters | + +### 8_to_9.json (INAV 8.0 → 9.0) + +| Category | Changes | +|----------|---------| +| **Command renames** | `controlrate_profile` → `use_control_profile` | +| **Setting renames** | `mixer_pid_profile_linking` → `mixer_control_profile_linking`, `osd_pan_servo_pwm2centideg` → `osd_pan_servo_range_decadegrees` | +| **Value replacements** | None | +| **Removed settings** | None | +| **Pattern mappings** | None | +| **Warnings** | Position estimator defaults changed (`w_z_baro_v`, `inav_w_z_gps_p`, `inav_w_z_gps_v`). `ahrs_acc_ignore_rate` default changed 20→15 | + +## Migration Engine Internals + +### Profile chaining + +`buildMigrationChain(fromVersion, toVersion)` selects all profiles where `profileFrom >= fromMajor` and `profileTo <= toMajor`, sorted by `fromVersion`. A 7.x → 9.x migration applies both 7→8 and 8→9 profiles in sequence. + +### Line processing + +Each non-comment, non-empty line passes through every profile in the chain. For each profile, transformations are applied in this order: + +1. Command renames (any token in the line) +2. Removed settings (line dropped if `set ` matches) +3. Setting renames (`set ` replacement) +4. Value replacements (value after `=` replaced) +5. Setting pattern mappings (regex-matched settings with value remapping) + +### Missing profile detection + +`hasMissingProfiles()` returns `true` when the number of profiles in the chain is fewer than the number of major version steps. The UI shows a warning but still allows restore — some settings may fail. + +## Edge Cases Handled + +1. **Stale FC version after flash**: Real FC version is queried via `MSP_FC_VERSION` after connect, not the cached value +2. **DFU mode (no MSP)**: `FC.CONFIG` null checks prevent crashes when connected in DFU mode +3. **DFU timeout**: UI unlock and progress label update on timeout (no permanent lock) +4. **Local firmware files**: `localFirmwareLoaded` flag prevents stale dropdown version from triggering wrong migration +5. **Backup pruning with mixed versions**: Sort by timestamp portion, not full filename +6. **Multi-step migration**: 7.x → 9.x automatically chains 7→8 + 8→9 profiles +7. **Missing migration profiles**: Warning shown but restore allowed — graceful degradation +8. **Version detection from backup**: Parsed from backup header (`# Version: X.Y.Z`), not from FC state + +## i18n Keys + +All backup/restore/migration translation keys in `locale/en/messages.json`: + +### Backup status +| Key | Text | +|-----|------| +| `backupRestoreStatusEnteringCli` | Entering CLI | +| `backupRestoreStatusReadingConfig` | Reading configuration via CLI | +| `backupRestoreStatusSavingFile` | Saving backup file... | +| `backupRestoreStatusExitingCli` | Exiting CLI mode | +| `backupRestoreBackupSaved` | Backup saved $1 | +| `backupRestoreAutoBackupSaved` | Auto-backup saved to $1 | +| `backupRestoreBackupComplete` | Backup complete | +| `backupRestoreBackupCancelled` | Backup cancelled | +| `backupRestoreBackupFailed` | Backup failed | + +### Restore status +| Key | Text | +|-----|------| +| `backupRestoreStatusConnecting` | Connecting to flight controller | +| `backupRestoreStatusRestoringConfig` | Restoring configuration | +| `backupRestoreStatusRestoringProgress` | Restoring... $1 / $2 | +| `backupRestoreStatusSaving` | Saving configuration | +| `backupRestoreRestoreComplete` | Configuration restored. Flight controller is rebooting. | +| `backupRestoreRestoreCancelled` | Restore cancelled. | +| `backupRestoreRestoreFailed` | Restore failed. | + +### Auto-restore UI +| Key | Text | +|-----|------| +| `backupRestoreAutoRestoreConfirm` | Restore confirmation prompt | +| `backupRestoreAutoRestoreWaiting` | Waiting for FC to reboot after flash | +| `backupRestoreAutoRestoreYes` | Yes, restore settings | +| `backupRestoreAutoRestoreNo` | No, keep current settings | +| `backupRestoreAutoRestoreWaitingPort` | Waiting for port $1 to reconnect | +| `backupRestoreDowngradeNoAutoRestore` | Major downgrade warning | +| `backupRestoreFlashCompleteBackupSaved` | Backup saved (local firmware, no restore offer) | +| `backupRestoreMigrationApplied` | Migration applied: $1 → $2 ($3 changes) | +| `backupRestoreMigrationWarningsHeader` | Migration Warnings: | + +### Migration preview +| Key | Text | +|-----|------| +| `migrationPreviewTitle` | Settings Migration Required | +| `migrationPreviewSubtitle` | Conversion explanation | +| `migrationPreviewRemovedHeader` | Removed Settings: | +| `migrationPreviewRenamedSettingsHeader` | Renamed Settings: | +| `migrationPreviewRenamedCommandsHeader` | Renamed Commands: | +| `migrationPreviewValueReplacementsHeader` | Value Replacements: | +| `migrationPreviewSettingRemappingsHeader` | Setting Remappings: | +| `migrationPreviewContinue` | Continue with migration | +| `migrationPreviewCancel` | Cancel restore | +| `migrationMissingProfileWarning` | Missing profile warning | + +### Error messages +| Key | Text | +|-----|------| +| `backupRestoreErrorTitle` | Restore Errors Detected | +| `backupRestoreErrorText` | Error explanation | +| `backupRestoreErrorAbort` | Abort | +| `backupRestoreErrorSave` | Save anyway | diff --git a/docs/development/msp/README.md b/docs/development/msp/README.md index d85974a65c0..833a9f6873c 100644 --- a/docs/development/msp/README.md +++ b/docs/development/msp/README.md @@ -9,7 +9,8 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i -**JSON file rev: 4** +**JSON file rev: 5 +** **Warning: Verification needed, exercise caution until completely verified for accuracy and cleared, especially for integer signs. Source-based generation/validation is forthcoming. Refer to source for absolute certainty** @@ -66,6 +67,7 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i **variable_len**: Optional boolean, if true, message does not have a predefined fixed length and needs appropriate handling\ **variants**: Optional special case, message has different cases of reply/request. Key/description is not a strict expression or code; just a readable condition\ **not_implemented**: Optional special case, message is not implemented (never or deprecated)\ +**replaced_by**: Optional array of MSP message names that replace this command. Present when a command is deprecated and scheduled for removal. Empty array if no replacement is needed\ **notes**: String with details of message ## Data dict fields: @@ -403,6 +405,7 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i [8448 - MSP2_INAV_CUSTOM_OSD_ELEMENTS](#msp2_inav_custom_osd_elements) [8449 - MSP2_INAV_CUSTOM_OSD_ELEMENT](#msp2_inav_custom_osd_element) [8450 - MSP2_INAV_SET_CUSTOM_OSD_ELEMENTS](#msp2_inav_set_custom_osd_elements) +[8451 - MSP2_INAV_GET_LINK_STATS](#msp2_inav_get_link_stats) [8461 - MSP2_INAV_OUTPUT_MAPPING_EXT2](#msp2_inav_output_mapping_ext2) [8704 - MSP2_INAV_SERVO_CONFIG](#msp2_inav_servo_config) [8705 - MSP2_INAV_SET_SERVO_CONFIG](#msp2_inav_set_servo_config) @@ -412,8 +415,10 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i [8723 - MSP2_INAV_SET_GEOZONE_VERTEX](#msp2_inav_set_geozone_vertex) [8724 - MSP2_INAV_SET_GVAR](#msp2_inav_set_gvar) [8736 - MSP2_INAV_FULL_LOCAL_POSE](#msp2_inav_full_local_pose) -[12288 - MSP2_BETAFLIGHT_BIND](#msp2_betaflight_bind) -[12289 - MSP2_RX_BIND](#msp2_rx_bind) +[8737 - MSP2_INAV_SET_WP_INDEX](#msp2_inav_set_wp_index) +[8739 - MSP2_INAV_SET_CRUISE_HEADING](#msp2_inav_set_cruise_heading) +[12288 - MSP2_BETAFLIGHT_BIND](#msp2_betaflight_bind) +[12289 - MSP2_RX_BIND](#msp2_rx_bind) ## `MSP_API_VERSION (1 / 0x1)` **Description:** Provides the MSP protocol version and the INAV API version. @@ -2206,7 +2211,7 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i | `armingFlags` | `uint16_t` | 2 | Bitmask | Bitmask: Flight controller arming flags (`armingFlags`). Note: Truncated to 16 bits | | `accCalibAxisFlags` | `uint8_t` | 1 | Bitmask | Bitmask: Accelerometer calibrated axes flags (`accGetCalibrationAxisFlags()`) | -**Notes:** Superseded by `MSP2_INAV_STATUS` which provides the full 32-bit `armingFlags` and other enhancements. +**Notes:** Superseded by `MSP2_INAV_STATUS` which provides the full 32-bit `armingFlags` and other enhancements. The `accCalibAxisFlags` field is not present in `MSP2_INAV_STATUS` but is available via `MSP_CALIBRATION_DATA`. ## `MSP_SENSOR_STATUS (151 / 0x97)` **Description:** Provides the hardware status for each individual sensor system. @@ -2273,6 +2278,7 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i | `hdop` | `uint16_t` | 2 | HDOP * 100 | Horizontal Dilution of Precision (`gpsSol.hdop`) | | `eph` | `uint16_t` | 2 | cm | Estimated Horizontal Position Accuracy (`gpsSol.eph`) | | `epv` | `uint16_t` | 2 | cm | Estimated Vertical Position Accuracy (`gpsSol.epv`) | +| `hwVersion` | `uint32_t` | 4 | Version code | GPS hardware version (`gpsState.hwVersion`). Values: 500=UBLOX5, 600=UBLOX6, 700=UBLOX7, 800=UBLOX8, 900=UBLOX9, 1000=UBLOX10, 0=UNKNOWN | **Notes:** Requires `USE_GPS`. @@ -4355,6 +4361,20 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i **Notes:** Payload length must be (OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1) + (CUSTOM_ELEMENTS_PARTS * 3) + 4 bytes including elementIndex. elementIndex must be < MAX_CUSTOM_ELEMENTS. Each partType must be < CUSTOM_ELEMENT_TYPE_END. Firmware NUL-terminates elementText internally. +## `MSP2_INAV_GET_LINK_STATS (8451 / 0x2103)` +**Description:** Provides uplink RC link statistics for monitoring on a GCS. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `uplinkRSSI_dBm` | `uint8_t` | 1 | -dBm | Uplink RSSI in dBm, sent as a positive magnitude (`getRSSI()`). For example, 70 means -70dBm. | +| `uplinkLQ` | `uint8_t` | 1 | % | Uplink Link Quality (`rxLinkStatistics.uplinkLQ`) | +| `uplinkSNR` | `int8_t` | 1 | dB | Uplink Signal-to-Noise Ratio (`rxLinkStatistics.uplinkSNR`) | + +**Notes:** Useful for GCS monitoring of the active RC link quality and signal margin. + ## `MSP2_INAV_OUTPUT_MAPPING_EXT2 (8461 / 0x210d)` **Description:** Retrieves the full extended output mapping configuration (timer ID, full 32-bit usage flags, and pin label). Supersedes `MSP2_INAV_OUTPUT_MAPPING_EXT`. @@ -4526,6 +4546,30 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i **Notes:** All attitude angles are in deci-degrees. +## `MSP2_INAV_SET_WP_INDEX (8737 / 0x2221)` +**Description:** Jumps to a specific waypoint during an active waypoint mission, causing the aircraft to immediately begin navigating toward the new target waypoint. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `wp_index` | `uint8_t` | 1 | - | 0-based waypoint index to jump to, relative to the mission start waypoint (`posControl.startWpIndex`) | + +**Reply Payload:** **None** + +**Notes:** Returns error if the aircraft is not armed, `NAV_WP_MODE` is not active, or the index is outside the valid mission range (`startWpIndex` to `startWpIndex + waypointCount - 1`). On success, sets `posControl.activeWaypointIndex` to the requested index and fires `NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP`, transitioning the navigation FSM back to `NAV_STATE_WAYPOINT_PRE_ACTION` so the flight controller re-initialises navigation for the new target. + +## `MSP2_INAV_SET_CRUISE_HEADING (8739 / 0x2223)` +**Description:** Sets the course heading target while Cruise or Course Hold mode is active, causing the aircraft to turn to and maintain the new heading. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `heading_centidegrees` | `int32_t` | 4 | centidegrees | Target heading in centidegrees (0-35999). Values are wrapped modulo 36000 before being applied. | + +**Reply Payload:** **None** + +**Notes:** Returns error if the aircraft is not armed or `NAV_COURSE_HOLD_MODE` is not active. On success, sets both `posControl.cruise.course` and `posControl.cruise.previousCourse` to the normalised value, preventing spurious heading adjustments from `getCruiseHeadingAdjustment()` on the next control cycle. + ## `MSP2_BETAFLIGHT_BIND (12288 / 0x3000)` **Description:** Initiates the receiver binding procedure for supported serial protocols (CRSF, SRXL2). @@ -4536,14 +4580,14 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i **Notes:** Requires `rxConfig()->receiverType == RX_TYPE_SERIAL`. Requires `USE_SERIALRX_CRSF` or `USE_SERIALRX_SRXL2`. Calls `crsfBind()` or `srxl2Bind()` respectively. Returns error if receiver type or provider is not supported for binding. ## `MSP2_RX_BIND (12289 / 0x3001)` -**Description:** Initiates binding for MSP receivers (mLRS). - +**Description:** Initiates binding for MSP receivers (mLRS). + **Request Payload:** |Field|C Type|Size (Bytes)|Description| |---|---|---|---| | `port_id` | `uint8_t` | 1 | Port ID | | `reserved_for_custom_use` | `uint8_t[3]` | 3 | Reserved for custom use | - + **Reply Payload:** |Field|C Type|Size (Bytes)|Description| |---|---|---|---| diff --git a/docs/development/msp/gen_docs.sh b/docs/development/msp/gen_docs.sh index cd368cf7f00..84aa852841f 100755 --- a/docs/development/msp/gen_docs.sh +++ b/docs/development/msp/gen_docs.sh @@ -11,7 +11,7 @@ expected="$(awk '{print $1}' msp_messages.checksum)" echo "Hash:" $actual if [[ "$actual" != "$expected" ]]; then n="$(cat rev)" - printf '%d' "$(n + 1)" > rev + printf '%d' "$((n + 1))" > rev echo "File changed, incrementing revision" echo $actual > msp_messages.checksum fi diff --git a/docs/development/msp/inav_enums.json b/docs/development/msp/inav_enums.json index 04a045ad3b9..63f3eb2941b 100644 --- a/docs/development/msp/inav_enums.json +++ b/docs/development/msp/inav_enums.json @@ -12,7 +12,8 @@ "ACC_ICM42605": "8", "ACC_BMI270": "9", "ACC_LSM6DXX": "10", - "ACC_FAKE": "11", + "ACC_ICM45686": "11", + "ACC_FAKE": "12", "ACC_MAX": "ACC_FAKE" }, "accEvent_t": { @@ -736,6 +737,9 @@ "CRSF_FRAMETYPE_VARIO_SENSOR": "7", "CRSF_FRAMETYPE_BATTERY_SENSOR": "8", "CRSF_FRAMETYPE_BAROMETER_ALTITUDE": "9", + "CRSF_FRAMETYPE_AIRSPEED_SENSOR": "10", + "CRSF_FRAMETYPE_RPM": "12", + "CRSF_FRAMETYPE_TEMP": "13", "CRSF_FRAMETYPE_LINK_STATISTICS": "20", "CRSF_FRAMETYPE_RC_CHANNELS_PACKED": "22", "CRSF_FRAMETYPE_ATTITUDE": "30", @@ -760,6 +764,9 @@ "CRSF_FRAME_GPS_INDEX": "", "CRSF_FRAME_VARIO_SENSOR_INDEX": "", "CRSF_FRAME_BAROMETER_ALTITUDE_INDEX": "", + "CRSF_FRAME_TEMP_INDEX": "", + "CRSF_FRAME_RPM_INDEX": "", + "CRSF_FRAME_AIRSPEED_INDEX": "", "CRSF_SCHEDULE_COUNT_MAX": "" }, "crsrRfMode_e": { @@ -801,53 +808,54 @@ "DEVHW_ICM42605": "7", "DEVHW_BMI270": "8", "DEVHW_LSM6D": "9", - "DEVHW_MPU9250": "10", - "DEVHW_BMP085": "11", - "DEVHW_BMP280": "12", - "DEVHW_MS5611": "13", - "DEVHW_MS5607": "14", - "DEVHW_LPS25H": "15", - "DEVHW_SPL06": "16", - "DEVHW_BMP388": "17", - "DEVHW_DPS310": "18", - "DEVHW_B2SMPB": "19", - "DEVHW_HMC5883": "20", - "DEVHW_AK8963": "21", - "DEVHW_AK8975": "22", - "DEVHW_IST8310_0": "23", - "DEVHW_IST8310_1": "24", - "DEVHW_IST8308": "25", - "DEVHW_QMC5883": "26", - "DEVHW_QMC5883P": "27", - "DEVHW_MAG3110": "28", - "DEVHW_LIS3MDL": "29", - "DEVHW_RM3100": "30", - "DEVHW_VCM5883": "31", - "DEVHW_MLX90393": "32", - "DEVHW_LM75_0": "33", - "DEVHW_LM75_1": "34", - "DEVHW_LM75_2": "35", - "DEVHW_LM75_3": "36", - "DEVHW_LM75_4": "37", - "DEVHW_LM75_5": "38", - "DEVHW_LM75_6": "39", - "DEVHW_LM75_7": "40", - "DEVHW_DS2482": "41", - "DEVHW_MAX7456": "42", - "DEVHW_SRF10": "43", - "DEVHW_VL53L0X": "44", - "DEVHW_VL53L1X": "45", - "DEVHW_US42": "46", - "DEVHW_TOF10120_I2C": "47", - "DEVHW_TERARANGER_EVO_I2C": "48", - "DEVHW_MS4525": "49", - "DEVHW_DLVR": "50", - "DEVHW_M25P16": "51", - "DEVHW_W25N": "52", - "DEVHW_UG2864": "53", - "DEVHW_SDCARD": "54", - "DEVHW_IRLOCK": "55", - "DEVHW_PCF8574": "56" + "DEVHW_ICM45686": "10", + "DEVHW_MPU9250": "11", + "DEVHW_BMP085": "12", + "DEVHW_BMP280": "13", + "DEVHW_MS5611": "14", + "DEVHW_MS5607": "15", + "DEVHW_LPS25H": "16", + "DEVHW_SPL06": "17", + "DEVHW_BMP388": "18", + "DEVHW_DPS310": "19", + "DEVHW_B2SMPB": "20", + "DEVHW_HMC5883": "21", + "DEVHW_AK8963": "22", + "DEVHW_AK8975": "23", + "DEVHW_IST8310_0": "24", + "DEVHW_IST8310_1": "25", + "DEVHW_IST8308": "26", + "DEVHW_QMC5883": "27", + "DEVHW_QMC5883P": "28", + "DEVHW_MAG3110": "29", + "DEVHW_LIS3MDL": "30", + "DEVHW_RM3100": "31", + "DEVHW_VCM5883": "32", + "DEVHW_MLX90393": "33", + "DEVHW_LM75_0": "34", + "DEVHW_LM75_1": "35", + "DEVHW_LM75_2": "36", + "DEVHW_LM75_3": "37", + "DEVHW_LM75_4": "38", + "DEVHW_LM75_5": "39", + "DEVHW_LM75_6": "40", + "DEVHW_LM75_7": "41", + "DEVHW_DS2482": "42", + "DEVHW_MAX7456": "43", + "DEVHW_SRF10": "44", + "DEVHW_VL53L0X": "45", + "DEVHW_VL53L1X": "46", + "DEVHW_US42": "47", + "DEVHW_TOF10120_I2C": "48", + "DEVHW_TERARANGER_EVO_I2C": "49", + "DEVHW_MS4525": "50", + "DEVHW_DLVR": "51", + "DEVHW_M25P16": "52", + "DEVHW_W25N": "53", + "DEVHW_UG2864": "54", + "DEVHW_SDCARD": "55", + "DEVHW_IRLOCK": "56", + "DEVHW_PCF8574": "57" }, "deviceFlags_e": { "_source": "inav/src/main/drivers/bus.h", @@ -1527,7 +1535,8 @@ "GYRO_ICM42605": "8", "GYRO_BMI270": "9", "GYRO_LSM6DXX": "10", - "GYRO_FAKE": "11" + "GYRO_ICM45686": "11", + "GYRO_FAKE": "12" }, "HardwareMotorTypes_e": { "_source": "inav/src/main/drivers/pwm_esc_detect.h", @@ -1861,7 +1870,8 @@ "LED_MODE_ANGLE": "3", "LED_MODE_MAG": "4", "LED_MODE_BARO": "5", - "LED_SPECIAL": "6" + "LED_MODE_LOITER": "6", + "LED_SPECIAL": "7" }, "ledOverlayId_e": { "_source": "inav/src/main/io/ledstrip.h", @@ -2325,8 +2335,7 @@ "MULTI_FUNC_3": "3", "MULTI_FUNC_4": "4", "MULTI_FUNC_5": "5", - "MULTI_FUNC_6": "6", - "MULTI_FUNC_END": "7" + "MULTI_FUNC_END": "6" }, "multiFunctionFlags_e": { "_source": "inav/src/main/fc/multifunction.h", @@ -2407,6 +2416,7 @@ "NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME": "NAV_FSM_EVENT_STATE_SPECIFIC_1", "NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND": "NAV_FSM_EVENT_STATE_SPECIFIC_2", "NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED": "NAV_FSM_EVENT_STATE_SPECIFIC_3", + "NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP": "NAV_FSM_EVENT_STATE_SPECIFIC_4", "NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE": "NAV_FSM_EVENT_STATE_SPECIFIC_1", "NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK": "NAV_FSM_EVENT_STATE_SPECIFIC_2", "NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME": "NAV_FSM_EVENT_STATE_SPECIFIC_3", @@ -3291,9 +3301,8 @@ "_source": "inav/src/main/io/displayport_msp_osd.c", "SD_3016": "0", "HD_5018": "1", - "HD_3016": "2", - "HD_6022": "3", - "HD_5320": "4" + "HD_6022": "2", + "HD_5320": "3" }, "resourceOwner_e": { "_source": "inav/src/main/drivers/resource.h", @@ -3829,7 +3838,7 @@ "THR_HI": "(2 << (2 * THROTTLE))" }, "systemState_e": { - "_source": "inav/src/main/fc/fc_init.c", + "_source": "inav/src/main/fc/fc_init.h", "SYSTEM_STATE_INITIALISING": "0", "SYSTEM_STATE_CONFIG_LOADED": "(1 << 0)", "SYSTEM_STATE_SENSORS_READY": "(1 << 1)", diff --git a/docs/development/msp/inav_enums_ref.md b/docs/development/msp/inav_enums_ref.md index b3aabb41467..802e813ab43 100644 --- a/docs/development/msp/inav_enums_ref.md +++ b/docs/development/msp/inav_enums_ref.md @@ -370,7 +370,8 @@ | `ACC_ICM42605` | 8 | | | `ACC_BMI270` | 9 | | | `ACC_LSM6DXX` | 10 | | -| `ACC_FAKE` | 11 | | +| `ACC_ICM45686` | 11 | | +| `ACC_FAKE` | 12 | | | `ACC_MAX` | ACC_FAKE | | --- @@ -1317,6 +1318,9 @@ | `CRSF_FRAMETYPE_VARIO_SENSOR` | 7 | | | `CRSF_FRAMETYPE_BATTERY_SENSOR` | 8 | | | `CRSF_FRAMETYPE_BAROMETER_ALTITUDE` | 9 | | +| `CRSF_FRAMETYPE_AIRSPEED_SENSOR` | 10 | | +| `CRSF_FRAMETYPE_RPM` | 12 | | +| `CRSF_FRAMETYPE_TEMP` | 13 | | | `CRSF_FRAMETYPE_LINK_STATISTICS` | 20 | | | `CRSF_FRAMETYPE_RC_CHANNELS_PACKED` | 22 | | | `CRSF_FRAMETYPE_ATTITUDE` | 30 | | @@ -1346,6 +1350,9 @@ | `CRSF_FRAME_GPS_INDEX` | | | | `CRSF_FRAME_VARIO_SENSOR_INDEX` | | | | `CRSF_FRAME_BAROMETER_ALTITUDE_INDEX` | | | +| `CRSF_FRAME_TEMP_INDEX` | | | +| `CRSF_FRAME_RPM_INDEX` | | | +| `CRSF_FRAME_AIRSPEED_INDEX` | | | | `CRSF_SCHEDULE_COUNT_MAX` | | | --- @@ -1407,53 +1414,54 @@ | `DEVHW_ICM42605` | 7 | | | `DEVHW_BMI270` | 8 | | | `DEVHW_LSM6D` | 9 | | -| `DEVHW_MPU9250` | 10 | | -| `DEVHW_BMP085` | 11 | | -| `DEVHW_BMP280` | 12 | | -| `DEVHW_MS5611` | 13 | | -| `DEVHW_MS5607` | 14 | | -| `DEVHW_LPS25H` | 15 | | -| `DEVHW_SPL06` | 16 | | -| `DEVHW_BMP388` | 17 | | -| `DEVHW_DPS310` | 18 | | -| `DEVHW_B2SMPB` | 19 | | -| `DEVHW_HMC5883` | 20 | | -| `DEVHW_AK8963` | 21 | | -| `DEVHW_AK8975` | 22 | | -| `DEVHW_IST8310_0` | 23 | | -| `DEVHW_IST8310_1` | 24 | | -| `DEVHW_IST8308` | 25 | | -| `DEVHW_QMC5883` | 26 | | -| `DEVHW_QMC5883P` | 27 | | -| `DEVHW_MAG3110` | 28 | | -| `DEVHW_LIS3MDL` | 29 | | -| `DEVHW_RM3100` | 30 | | -| `DEVHW_VCM5883` | 31 | | -| `DEVHW_MLX90393` | 32 | | -| `DEVHW_LM75_0` | 33 | | -| `DEVHW_LM75_1` | 34 | | -| `DEVHW_LM75_2` | 35 | | -| `DEVHW_LM75_3` | 36 | | -| `DEVHW_LM75_4` | 37 | | -| `DEVHW_LM75_5` | 38 | | -| `DEVHW_LM75_6` | 39 | | -| `DEVHW_LM75_7` | 40 | | -| `DEVHW_DS2482` | 41 | | -| `DEVHW_MAX7456` | 42 | | -| `DEVHW_SRF10` | 43 | | -| `DEVHW_VL53L0X` | 44 | | -| `DEVHW_VL53L1X` | 45 | | -| `DEVHW_US42` | 46 | | -| `DEVHW_TOF10120_I2C` | 47 | | -| `DEVHW_TERARANGER_EVO_I2C` | 48 | | -| `DEVHW_MS4525` | 49 | | -| `DEVHW_DLVR` | 50 | | -| `DEVHW_M25P16` | 51 | | -| `DEVHW_W25N` | 52 | | -| `DEVHW_UG2864` | 53 | | -| `DEVHW_SDCARD` | 54 | | -| `DEVHW_IRLOCK` | 55 | | -| `DEVHW_PCF8574` | 56 | | +| `DEVHW_ICM45686` | 10 | | +| `DEVHW_MPU9250` | 11 | | +| `DEVHW_BMP085` | 12 | | +| `DEVHW_BMP280` | 13 | | +| `DEVHW_MS5611` | 14 | | +| `DEVHW_MS5607` | 15 | | +| `DEVHW_LPS25H` | 16 | | +| `DEVHW_SPL06` | 17 | | +| `DEVHW_BMP388` | 18 | | +| `DEVHW_DPS310` | 19 | | +| `DEVHW_B2SMPB` | 20 | | +| `DEVHW_HMC5883` | 21 | | +| `DEVHW_AK8963` | 22 | | +| `DEVHW_AK8975` | 23 | | +| `DEVHW_IST8310_0` | 24 | | +| `DEVHW_IST8310_1` | 25 | | +| `DEVHW_IST8308` | 26 | | +| `DEVHW_QMC5883` | 27 | | +| `DEVHW_QMC5883P` | 28 | | +| `DEVHW_MAG3110` | 29 | | +| `DEVHW_LIS3MDL` | 30 | | +| `DEVHW_RM3100` | 31 | | +| `DEVHW_VCM5883` | 32 | | +| `DEVHW_MLX90393` | 33 | | +| `DEVHW_LM75_0` | 34 | | +| `DEVHW_LM75_1` | 35 | | +| `DEVHW_LM75_2` | 36 | | +| `DEVHW_LM75_3` | 37 | | +| `DEVHW_LM75_4` | 38 | | +| `DEVHW_LM75_5` | 39 | | +| `DEVHW_LM75_6` | 40 | | +| `DEVHW_LM75_7` | 41 | | +| `DEVHW_DS2482` | 42 | | +| `DEVHW_MAX7456` | 43 | | +| `DEVHW_SRF10` | 44 | | +| `DEVHW_VL53L0X` | 45 | | +| `DEVHW_VL53L1X` | 46 | | +| `DEVHW_US42` | 47 | | +| `DEVHW_TOF10120_I2C` | 48 | | +| `DEVHW_TERARANGER_EVO_I2C` | 49 | | +| `DEVHW_MS4525` | 50 | | +| `DEVHW_DLVR` | 51 | | +| `DEVHW_M25P16` | 52 | | +| `DEVHW_W25N` | 53 | | +| `DEVHW_UG2864` | 54 | | +| `DEVHW_SDCARD` | 55 | | +| `DEVHW_IRLOCK` | 56 | | +| `DEVHW_PCF8574` | 57 | | --- ## `deviceFlags_e` @@ -2488,7 +2496,8 @@ | `GYRO_ICM42605` | 8 | | | `GYRO_BMI270` | 9 | | | `GYRO_LSM6DXX` | 10 | | -| `GYRO_FAKE` | 11 | | +| `GYRO_ICM45686` | 11 | | +| `GYRO_FAKE` | 12 | | --- ## `HardwareMotorTypes_e` @@ -2924,7 +2933,8 @@ | `LED_MODE_ANGLE` | 3 | | | `LED_MODE_MAG` | 4 | | | `LED_MODE_BARO` | 5 | | -| `LED_SPECIAL` | 6 | | +| `LED_MODE_LOITER` | 6 | | +| `LED_SPECIAL` | 7 | | --- ## `ledOverlayId_e` @@ -3531,8 +3541,7 @@ | `MULTI_FUNC_3` | 3 | | | `MULTI_FUNC_4` | 4 | | | `MULTI_FUNC_5` | 5 | | -| `MULTI_FUNC_6` | 6 | | -| `MULTI_FUNC_END` | 7 | | +| `MULTI_FUNC_END` | 6 | | --- ## `multiFunctionFlags_e` @@ -3658,6 +3667,7 @@ | `NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME` | NAV_FSM_EVENT_STATE_SPECIFIC_1 | | | `NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND` | NAV_FSM_EVENT_STATE_SPECIFIC_2 | | | `NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED` | NAV_FSM_EVENT_STATE_SPECIFIC_3 | | +| `NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP` | NAV_FSM_EVENT_STATE_SPECIFIC_4 | | | `NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE` | NAV_FSM_EVENT_STATE_SPECIFIC_1 | | | `NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK` | NAV_FSM_EVENT_STATE_SPECIFIC_2 | | | `NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME` | NAV_FSM_EVENT_STATE_SPECIFIC_3 | | @@ -4819,9 +4829,8 @@ |---|---:|---| | `SD_3016` | 0 | | | `HD_5018` | 1 | | -| `HD_3016` | 2 | | -| `HD_6022` | 3 | | -| `HD_5320` | 4 | | +| `HD_6022` | 2 | | +| `HD_5320` | 3 | | --- ## `resourceOwner_e` @@ -5627,7 +5636,7 @@ --- ## `systemState_e` -> Source: ../../../src/main/fc/fc_init.h +> Source: ../../../src/main/fc/fc_init.c | Enumerator | Value | Condition | |---|---:|---| @@ -5641,7 +5650,7 @@ --- ## `systemState_e` -> Source: ../../../src/main/fc/fc_init.c +> Source: ../../../src/main/fc/fc_init.h | Enumerator | Value | Condition | |---|---:|---| diff --git a/docs/development/msp/msp_messages.checksum b/docs/development/msp/msp_messages.checksum index ff3e21a1d69..031f3b14179 100644 --- a/docs/development/msp/msp_messages.checksum +++ b/docs/development/msp/msp_messages.checksum @@ -1 +1 @@ -82a3d2eee9d0d1fe609363a08405ed21 +c9458e9a712b7a4f3bc9333aa7bc3dcb diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json index 12d67369bd7..e1b128f305d 100644 --- a/docs/development/msp/msp_messages.json +++ b/docs/development/msp/msp_messages.json @@ -4914,9 +4914,9 @@ }, { "name": "hwVersion", - "ctype": "uint32_t", - "desc": "GPS hardware version (`gpsState.hwVersion`). Values: 500=UBLOX5, 600=UBLOX6, 700=UBLOX7, 800=UBLOX8, 900=UBLOX9, 1000=UBLOX10, 0=UNKNOWN", - "units": "Version code" + "ctype": "uint8_t", + "desc": "GPS hardware version bit-field: bits[7:6]=series (0b01=u-blox Neo/M), bits[5:0]=generation. E.g. 0x48=M8, 0x49=M9, 0x4A=M10, 0=unknown.", + "units": "" } ] }, @@ -6953,6 +6953,35 @@ "notes": "Requires `USE_CURRENT_METER`/`USE_ADC` for current-related fields; values fall back to zero when unavailable. Capacity fields are reported in the units configured by `batteryMetersConfig()->capacity_unit` (mAh or mWh).", "description": "Provides detailed analog sensor readings, superseding `MSP_ANALOG` with higher precision and additional fields." }, + "MSP2_INAV_GET_LINK_STATS": { + "code": 8451, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "uplinkRSSI_dBm", + "ctype": "uint8_t", + "desc": "Uplink RSSI in dBm, sent as a positive magnitude (`getRSSI()`). For example, 70 means -70dBm.", + "units": "-dBm" + }, + { + "name": "uplinkLQ", + "ctype": "uint8_t", + "desc": "Uplink Link Quality (`rxLinkStatistics.uplinkLQ`)", + "units": "%" + }, + { + "name": "uplinkSNR", + "ctype": "int8_t", + "desc": "Uplink Signal-to-Noise Ratio (`rxLinkStatistics.uplinkSNR`)", + "units": "dB" + } + ] + }, + "notes": "Useful for GCS monitoring of the active RC link quality and signal margin.", + "description": "Provides uplink RC link statistics for monitoring on a GCS." + }, "MSP2_INAV_MISC": { "code": 8195, "mspv": 2, @@ -10943,6 +10972,66 @@ "notes": "All attitude angles are in deci-degrees.", "description": "Provides estimates of current attitude, local NEU position, and velocity." }, + "MSP2_INAV_SET_WP_INDEX": { + "code": 8737, + "mspv": 2, + "request": { + "payload": [ + { + "name": "wp_index", + "ctype": "uint8_t", + "desc": "0-based waypoint index to jump to, relative to the mission start waypoint (`posControl.startWpIndex`)", + "units": "-" + } + ] + }, + "reply": null, + "notes": "Returns error if the aircraft is not armed, `NAV_WP_MODE` is not active, or the index is outside the valid mission range (`startWpIndex` to `startWpIndex + waypointCount - 1`). On success, sets `posControl.activeWaypointIndex` to the requested index and fires `NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP`, transitioning the navigation FSM back to `NAV_STATE_WAYPOINT_PRE_ACTION` so the flight controller re-initialises navigation for the new target.", + "description": "Jumps to a specific waypoint during an active waypoint mission, causing the aircraft to immediately begin navigating toward the new target waypoint." + }, + "MSP2_INAV_SET_CRUISE_HEADING": { + "code": 8739, + "mspv": 2, + "request": { + "payload": [ + { + "name": "heading_centidegrees", + "ctype": "int32_t", + "desc": "Target heading in centidegrees (0-35999). Values are wrapped modulo 36000 before being applied.", + "units": "centidegrees" + } + ] + }, + "reply": null, + "notes": "Returns error if the aircraft is not armed or `NAV_COURSE_HOLD_MODE` is not active. On success, sets both `posControl.cruise.course` and `posControl.cruise.previousCourse` to the normalised value, preventing spurious heading adjustments from `getCruiseHeadingAdjustment()` on the next control cycle.", + "description": "Sets the course heading target while Cruise or Course Hold mode is active, causing the aircraft to turn to and maintain the new heading." + }, + "MSP2_INAV_SET_AUX_RC": { + "code": 8752, + "mspv": 2, + "request": { + "payload": [ + { + "name": "definitionByte", + "ctype": "uint8_t", + "desc": "Packed start channel and resolution. Bits 7-3: start channel index (valid range 12-31 for CH13-CH32; 0-11 rejected as error). Bits 2-0: resolution mode (0=2-bit, 1=4-bit, 2=8-bit, 3=16-bit; 4-7 reserved/error).", + "units": "" + }, + { + "name": "channelData", + "ctype": "uint8_t", + "desc": "Packed channel values, sequential from start channel. Number of channels is derived from data size and resolution. Value 0 means skip (no update). Sub-byte modes (2-bit, 4-bit) are packed MSB-first. 2-bit values 1-3 map to 1000/1500/2000us. 4-bit values 1-15 map to 1000 + (val-1)*1000/14 us. 8-bit values 1-255 map to 1000 + (val-1)*1000/254 us. 16-bit values are direct PWM, clamped to 750-2250us.", + "units": "PWM (encoded)", + "array": true, + "array_size": 0 + } + ] + }, + "reply": null, + "variable_len": true, + "notes": "CH1-CH12 (index 0-11) are protected and will return `MSP_RESULT_ERROR`. Payload size must be 2-49 bytes. Constraint: `startChannel + channelCount <= 32`. Values persist until overwritten; no timeout. Applied as a post-RX overlay in `calculateRxChannelsAndUpdateFailsafe()` after MSP RC Override but before failsafe. Does not require `USE_RX_MSP` or MSP-RC-OVERRIDE flight mode. Does not affect failsafe detection. When MSP is the primary RX provider, channels covered by `MSP_SET_RAW_RC` are automatically skipped. Channels in the `mspOverrideChannels` bitmask are skipped when MSP RC Override mode is active. Recommended to send with `MSP_FLAG_DONT_REPLY` (flags=0x01) to save bandwidth on telemetry passthrough links. 16-bit mode requires even number of data bytes and values are clamped to 750-2250us.", + "description": "Bandwidth-efficient auxiliary RC channel update. Sets CH13-CH32 with configurable resolution (2/4/8/16-bit) without affecting primary flight controls. Designed for extending channel count beyond native RC link capacity via MSP passthrough." + }, "MSP2_BETAFLIGHT_BIND": { "code": 12288, "mspv": 2, diff --git a/docs/development/msp/rev b/docs/development/msp/rev index bf0d87ab1b2..7ed6ff82de6 100644 --- a/docs/development/msp/rev +++ b/docs/development/msp/rev @@ -1 +1 @@ -4 \ No newline at end of file +5 diff --git a/docs/development/release-create.md b/docs/development/release-create.md index 16dccb6292d..f1ba7cf2369 100644 --- a/docs/development/release-create.md +++ b/docs/development/release-create.md @@ -109,6 +109,7 @@ Version numbers are set in: - [ ] Release notes drafted - [ ] Breaking changes documented - [ ] New features documented +- [ ] **Configurator migration profile created** for major version bumps (see [Backup Restore Architecture](Backup%20Restore%20Architecture.md#adding-a-new-migration-profile)) ## Release Workflow @@ -492,6 +493,7 @@ gh api repos/iNavFlight/inav-configurator/git/refs -f ref="refs/heads/maintenanc - **Changes maintaining backward compatibility** → PR to maintenance-X.x (e.g., maintenance-9.x) - **Breaking changes** (MSP protocol, settings structure) → PR to maintenance-(X+1).x (e.g., maintenance-10.x) + - When breaking changes affect CLI settings (renames, removals, value changes), a **Configurator migration profile** must be created. See [Backup Restore Architecture](Backup%20Restore%20Architecture.md#adding-a-new-migration-profile) - **Master** → NOT a PR target (receives merges only) Lower version branches are periodically merged into higher version branches (e.g., maintenance-9.x → master → maintenance-10.x). diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 101c7223372..c243d9215f3 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -87,6 +87,8 @@ main_sources(COMMON_SRC drivers/accgyro/accgyro_icm20689.h drivers/accgyro/accgyro_icm42605.c drivers/accgyro/accgyro_icm42605.h + drivers/accgyro/accgyro_icm45686.c + drivers/accgyro/accgyro_icm45686.h drivers/accgyro/accgyro_mpu.c drivers/accgyro/accgyro_mpu.h drivers/accgyro/accgyro_mpu6000.c @@ -217,6 +219,8 @@ main_sources(COMMON_SRC drivers/pitotmeter/pitotmeter_adc.h drivers/pitotmeter/pitotmeter_ms4525.c drivers/pitotmeter/pitotmeter_ms4525.h + drivers/pitotmeter/pitotmeter_ms5525.c + drivers/pitotmeter/pitotmeter_ms5525.h drivers/pitotmeter/pitotmeter_dlvr_l10d.c drivers/pitotmeter/pitotmeter_dlvr_l10d.h drivers/pitotmeter/pitotmeter_msp.c diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index e4c2afb3eec..f5225ca3e1e 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -504,7 +504,7 @@ typedef struct blackboxMainState_s { int16_t gyroPeaksYaw[DYN_NOTCH_PEAK_COUNT]; int16_t accADC[XYZ_AXIS_COUNT]; - int16_t accVib; + uint16_t accVib; int16_t attitude[XYZ_AXIS_COUNT]; int32_t debug[DEBUG32_VALUE_COUNT]; int16_t motor[MAX_SUPPORTED_MOTORS]; @@ -1642,7 +1642,7 @@ static void loadMainState(timeUs_t currentTimeUs) blackboxCurrent->axisPID_D[i] = axisPID_D[i]; blackboxCurrent->axisPID_F[i] = axisPID_F[i]; blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]); - blackboxCurrent->accADC[i] = lrintf(acc.accADCf[i] * acc.dev.acc_1G); + blackboxCurrent->accADC[i] = constrain(lrintf(acc.accADCf[i] * acc.dev.acc_1G), -32678, 32767); blackboxCurrent->gyroRaw[i] = lrintf(gyro.gyroRaw[i]); #ifdef USE_DYNAMIC_FILTERS @@ -1668,7 +1668,8 @@ static void loadMainState(timeUs_t currentTimeUs) blackboxCurrent->mcVelAxisOutput[i] = lrintf(nav_pids->vel[i].output_constrained); } } - blackboxCurrent->accVib = lrintf(accGetVibrationLevel() * acc.dev.acc_1G); + + blackboxCurrent->accVib = constrain(lrintf(accGetVibrationLevel() * acc.dev.acc_1G), 0, 65535); if (STATE(FIXED_WING_LEGACY)) { diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 5fd72965d85..2e9e4574e71 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -44,7 +44,7 @@ float nullFilterApply4(void *filter, float input, float f_cut, float dt) // PT1 Low Pass filter -static float pt1ComputeRC(const float f_cut) +static float FAST_CODE pt1ComputeRC(const float f_cut) { return 1.0f / (2.0f * M_PIf * f_cut); } diff --git a/src/main/common/log.c b/src/main/common/log.c index 050422561d7..c2cc30a3a48 100644 --- a/src/main/common/log.c +++ b/src/main/common/log.c @@ -54,7 +54,7 @@ static serialPort_t * logPort = NULL; static mspPort_t * mspLogPort = NULL; -PG_REGISTER(logConfig_t, logConfig, PG_LOG_CONFIG, 0); +PG_REGISTER_WITH_RESET_TEMPLATE(logConfig_t, logConfig, PG_LOG_CONFIG, 0); PG_RESET_TEMPLATE(logConfig_t, logConfig, .level = SETTING_LOG_LEVEL_DEFAULT, diff --git a/src/main/common/maths.c b/src/main/common/maths.c index d591e40de81..308fb3eca9a 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -165,7 +165,7 @@ int32_t applyDeadbandRescaled(int32_t value, int32_t deadband, int32_t min, int3 return value; } -int32_t constrain(int32_t amt, int32_t low, int32_t high) +int32_t FAST_CODE constrain(int32_t amt, int32_t low, int32_t high) { if (amt < low) return low; @@ -175,7 +175,7 @@ int32_t constrain(int32_t amt, int32_t low, int32_t high) return amt; } -float constrainf(float amt, float low, float high) +float FAST_CODE constrainf(float amt, float low, float high) { if (amt < low) return low; @@ -225,7 +225,7 @@ int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax) { return ((a / b) + destMin); } -float scaleRangef(float x, float srcMin, float srcMax, float destMin, float destMax) { +float FAST_CODE scaleRangef(float x, float srcMin, float srcMax, float destMin, float destMax) { float a = (destMax - destMin) * (x - srcMin); float b = srcMax - srcMin; return ((a / b) + destMin); diff --git a/src/main/config/config_eeprom.c b/src/main/config/config_eeprom.c index 6c1dfc3dc7e..608a08a0a84 100755 --- a/src/main/config/config_eeprom.c +++ b/src/main/config/config_eeprom.c @@ -33,6 +33,7 @@ #include "drivers/system.h" #include "drivers/flash.h" +#include "drivers/pwm_output.h" #include "fc/config.h" @@ -321,6 +322,13 @@ static bool writeSettingsToEEPROM(void) void writeConfigToEEPROM(void) { +#if !defined(SITL_BUILD) && defined(USE_DSHOT) + // Enable circular DMA so hardware keeps repeating zero-throttle DShot + // packets during flash writes (which block the CPU for 20-200ms). + // Without this, ESCs lose signal and may spin up or reboot. + pwmSetMotorDMACircular(true); +#endif + bool success = false; // write it for (int attempt = 0; attempt < 3 && !success; attempt++) { @@ -333,6 +341,10 @@ void writeConfigToEEPROM(void) } } +#if !defined(SITL_BUILD) && defined(USE_DSHOT) + pwmSetMotorDMACircular(false); +#endif + if (success && isEEPROMContentValid()) { return; } diff --git a/src/main/config/config_streamer_file.c b/src/main/config/config_streamer_file.c index 379155c22e1..bce0f5be522 100644 --- a/src/main/config/config_streamer_file.c +++ b/src/main/config/config_streamer_file.c @@ -47,7 +47,6 @@ bool configFileSetPath(char* path) void config_streamer_impl_unlock(void) { if (eepromFd != NULL) { - fprintf(stderr, "[EEPROM] Unable to load %s\n", eepromPath); return; } @@ -103,7 +102,6 @@ int config_streamer_impl_write_word(config_streamer_t *c, config_streamer_buffer if ((c->address >= (uintptr_t)eepromData) && (c->address < (uintptr_t)ARRAYEND(eepromData))) { *((uint32_t*)c->address) = *buffer; - fprintf(stderr, "[EEPROM] Program word %p = %08x\n", (void*)c->address, *((uint32_t*)c->address)); } else { fprintf(stderr, "[EEPROM] Program word %p out of range!\n", (void*)c->address); } diff --git a/src/main/drivers/accgyro/accgyro_icm45686.c b/src/main/drivers/accgyro/accgyro_icm45686.c new file mode 100644 index 00000000000..9b249fb607f --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_icm45686.c @@ -0,0 +1,492 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#include "build/debug.h" + +#include "common/axis.h" +#include "common/maths.h" +#include "common/utils.h" +#include "common/log.h" + +#include "drivers/system.h" +#include "drivers/time.h" + +#include "drivers/sensor.h" +#include "drivers/accgyro/accgyro_mpu.h" +#include "drivers/accgyro/accgyro.h" +#include "accgyro_icm45686.h" + +#if defined(USE_IMU_ICM45686) +/* +reference: https://github.com/tdk-invn-oss/motion.mcu.icm45686.driver +Datasheet: https://invensense.tdk.com/wp-content/uploads/documentation/DS-000577_ICM-45686.pdf +Datasheet: https://invensense.tdk.com/wp-content/uploads/documentation/DS-000576_ICM-45605.pdf + +Note: ICM456xx has two modes of operation: Low-Power Mode Low-Noise Mode +Note: Now implemented only UI Interface with Low-Noise Mode + + The following diagram shows the signal path for each mode: + The cut-off frequency of the filters is determined by the ODR setting. + + Low-Noise Mode + +------+ +--------------+ +-------------+ +--------------+ +------------------+ + | ADC |---->| Anti-Alias |--->| Interpolator|--->| LPF |--->| Sensor Registers |---> UI Interface + | | | Filter (AAF) | | | +->| & ODR Select | | | + +--|---+ +--------------+ +-------------+ | +--------------+ +------------------+ + | | + | Low-Power Mode | + | +--------------+ | + |-------->| Notch Filter |--------------------| + | | | + | +--------------+ + | + | + +--|---+ +--------------+ +------+ +------+ +------------------+ + | ADC | --------> | Notch Filter | ---> | HPF | ---> | LPF | ---> | Sensor Registers | ---> AUX1 Interface + | | | | | | | | | | + +------+ +--------------+ +------+ +------+ +------------------+ + + The AUX1 interface default configuration can be checked by read only register IOC_PAD_SCENARIO through host interface. + By default, AUX1 interface is enabled, and default interface for AUX1 is SPI3W or I3CSM. + + In Low-Noise Mode, the ADC output is sent through an Anti-Alias Filter (AAF). The AAF is an FIR filter with fixed + coefficients (not user configurable). The AAF can be enabled or disabled by the user using GYRO_SRC_CTRL and + ACCEL_SRC_CTRL. + + The AUX1 signal path includes a Notch Filter. The notch filter is not user programmable. The usage of the notch + filter in the auxiliary path is recommended for sharper roll-off and for the cases where user is asynchronously + sampling the auxiliary interface data output at integer multiples of 1 kHz rate. The notch filter may be bypassed + using GYRO_OIS_M6_BYP. + + The notch filter is followed by an HPF on the AUX1 signal path. HPF cut-off frequency can be selected using + GYRO_OIS_HPFBW_SEL and ACCEL_OIS_HPFBW_SEL. HPF can be bypassed using GYRO_OIS_HPF1_BYP and + ACCEL_OIS_HPF1_BYP. + + The HPF is followed by LPF on the AUX1 signal path. The AUX1 LPF BW is set by register bit field + GYRO_OIS_LPF1BW_SEL and ACCEL_OIS_LPF1BW_SEL for gyroscope and accelerometer respectively. This is + followed by Full Scale Range (FSR) selection based on user configurable settings for register fields + GYRO_AUX1_FS_SEL and ACCEL_AUX1_FS_SEL. AUX1 output is fixed at 6.4kHz ODR. +*/ + +// NOTE: ICM-45686 does NOT have a bank select register like ICM-426xx +// The ICM-45686 uses Indirect Register (IREG) access for internal registers +// Register 0x75 is RESERVED/UNDEFINED in the ICM-45686 datasheet +// DO NOT use bank switching on this device + +// Register map User Bank 0 (UI Interface) +#define ICM456XX_WHO_AM_REGISTER 0x72 +#define ICM456XX_REG_MISC2 0x7F +#define ICM456XX_INT1_CONFIG0 0x16 +#define ICM456XX_INT1_CONFIG2 0x18 +#define ICM456XX_INT1_STATUS0 0x19 +#define ICM456XX_INT1_STATUS1 0x1A +#define ICM456XX_GYRO_CONFIG0 0x1C +#define ICM456XX_ACCEL_CONFIG0 0x1B +#define ICM456XX_PWR_MGMT0 0x10 +#define ICM456XX_TEMP_DATA1 0x0C +// MGMT0 - 0x10 - Gyro +#define ICM456XX_GYRO_MODE_OFF (0x00 << 2) +#define ICM456XX_GYRO_MODE_STANDBY (0x01 << 2) +#define ICM456XX_GYRO_MODE_LP (0x02 << 2) // Low Power Mode +#define ICM456XX_GYRO_MODE_LN (0x03 << 2) // Low Noise Mode + +// MGMT0 - 0x10 - Accel +#define ICM456XX_ACCEL_MODE_OFF (0x00) +#define ICM456XX_ACCEL_MODE_OFF2 (0x01) +#define ICM456XX_ACCEL_MODE_LP (0x02) // Low Power Mode +#define ICM456XX_ACCEL_MODE_LN (0x03) // Low Noise Mode + +// INT1_CONFIG0 - 0x16 +#define ICM456XX_INT1_STATUS_EN_RESET_DONE (1 << 7) +#define ICM456XX_INT1_STATUS_EN_AUX1_AGC_RDY (1 << 6) +#define ICM456XX_INT1_STATUS_EN_AP_AGC_RDY (1 << 5) +#define ICM456XX_INT1_STATUS_EN_AP_FSYNC (1 << 4) +#define ICM456XX_INT1_STATUS_EN_AUX1_DRDY (1 << 3) +#define ICM456XX_INT1_STATUS_EN_DRDY (1 << 2) +#define ICM456XX_INT1_STATUS_EN_FIFO_THS (1 << 1) +#define ICM456XX_INT1_STATUS_EN_FIFO_FULL (1 << 0) + +// INT1_CONFIG2 - 0x18 +#define ICM456XX_INT1_DRIVE_CIRCUIT_PP (0 << 2) +#define ICM456XX_INT1_DRIVE_CIRCUIT_OD (1 << 2) +#define ICM456XX_INT1_MODE_PULSED (0 << 1) +#define ICM456XX_INT1_MODE_LATCHED (1 << 1) +#define ICM456XX_INT1_POLARITY_ACTIVE_LOW (0 << 0) +#define ICM456XX_INT1_POLARITY_ACTIVE_HIGH (1 << 0) + +// INT1_STATUS0 - 0x19 +#define ICM456XX_INT1_STATUS_RESET_DONE (1 << 7) +#define ICM456XX_INT1_STATUS_AUX1_AGC_RDY (1 << 6) +#define ICM456XX_INT1_STATUS_AP_AGC_RDY (1 << 5) +#define ICM456XX_INT1_STATUS_AP_FSYNC (1 << 4) +#define ICM456XX_INT1_STATUS_AUX1_DRDY (1 << 3) +#define ICM456XX_INT1_STATUS_DRDY (1 << 2) +#define ICM456XX_INT1_STATUS_FIFO_THS (1 << 1) +#define ICM456XX_INT1_STATUS_FIFO_FULL (1 << 0) + +// REG_MISC2 - 0x7F +#define ICM456XX_SOFT_RESET (1 << 1) +#define ICM456XX_RESET_TIMEOUT_US 20000 // 20 ms + +#define ICM456XX_ACCEL_DATA_X1_UI 0x00 +#define ICM456XX_GYRO_DATA_X1_UI 0x06 + +// ACCEL_CONFIG0 - 0x1B +#define ICM456XX_ACCEL_FS_SEL_32G (0x00 << 4) +#define ICM456XX_ACCEL_FS_SEL_16G (0x01 << 4) +#define ICM456XX_ACCEL_FS_SEL_8G (0x02 << 4) +#define ICM456XX_ACCEL_FS_SEL_4G (0x03 << 4) +#define ICM456XX_ACCEL_FS_SEL_2G (0x04 << 4) + +// ACCEL_CONFIG0 - 0x1B +#define ICM456XX_ACCEL_ODR_6K4_LN 0x03 +#define ICM456XX_ACCEL_ODR_3K2_LN 0x04 +#define ICM456XX_ACCEL_ODR_1K6_LN 0x05 +#define ICM456XX_ACCEL_ODR_800_LN 0x06 +#define ICM456XX_ACCEL_ODR_400_LP_LN 0x07 +#define ICM456XX_ACCEL_ODR_200_LP_LN 0x08 +#define ICM456XX_ACCEL_ODR_100_LP_LN 0x09 +#define ICM456XX_ACCEL_ODR_50_LP_LN 0x0A +#define ICM456XX_ACCEL_ODR_25_LP_LN 0x0B +#define ICM456XX_ACCEL_ODR_12_5_LP_LN 0x0C +#define ICM456XX_ACCEL_ODR_6_25_LP 0x0D +#define ICM456XX_ACCEL_ODR_3_125_LP 0x0E +#define ICM456XX_ACCEL_ODR_1_5625_LP 0x0F + +// GYRO_CONFIG0 - 0x1C +#define ICM456XX_GYRO_FS_SEL_4000DPS (0x00 << 4) +#define ICM456XX_GYRO_FS_SEL_2000DPS (0x01 << 4) +#define ICM456XX_GYRO_FS_SEL_1000DPS (0x02 << 4) +#define ICM456XX_GYRO_FS_SEL_500DPS (0x03 << 4) +#define ICM456XX_GYRO_FS_SEL_250DPS (0x04 << 4) +#define ICM456XX_GYRO_FS_SEL_125DPS (0x05 << 4) +#define ICM456XX_GYRO_FS_SEL_62_5DPS (0x06 << 4) +#define ICM456XX_GYRO_FS_SEL_31_25DPS (0x07 << 4) +#define ICM456XX_GYRO_FS_SEL_15_625DPS (0x08 << 4) + +// GYRO_CONFIG0 - 0x1C +#define ICM456XX_GYRO_ODR_6K4_LN 0x03 +#define ICM456XX_GYRO_ODR_3K2_LN 0x04 +#define ICM456XX_GYRO_ODR_1K6_LN 0x05 +#define ICM456XX_GYRO_ODR_800_LN 0x06 +#define ICM456XX_GYRO_ODR_400_LP_LN 0x07 +#define ICM456XX_GYRO_ODR_200_LP_LN 0x08 +#define ICM456XX_GYRO_ODR_100_LP_LN 0x09 +#define ICM456XX_GYRO_ODR_50_LP_LN 0x0A +#define ICM456XX_GYRO_ODR_25_LP_LN 0x0B +#define ICM456XX_GYRO_ODR_12_5_LP_LN 0x0C +#define ICM456XX_GYRO_ODR_6_25_LP 0x0D +#define ICM456XX_GYRO_ODR_3_125_LP 0x0E +#define ICM456XX_GYRO_ODR_1_5625_LP 0x0F + +// Accel IPREG_SYS2_REG_123 - 0x7B +#define ICM456XX_SRC_CTRL_AAF_ENABLE_BIT (1 << 0) // Anti-Alias Filter - AAF +#define ICM456XX_SRC_CTRL_INTERP_ENABLE_BIT (1 << 1) // Interpolator + +// IPREG_SYS2_REG_123 - 0x7B +#define ICM456XX_ACCEL_SRC_CTRL_IREG_ADDR 0xA57B // To access register in IPREG_SYS2, add base address 0xA500 + offset + +// IPREG_SYS1_REG_166 - 0xA6 +#define ICM456XX_GYRO_SRC_CTRL_IREG_ADDR 0xA4A6 // To access register in IPREG_SYS1, add base address 0xA400 + offset + +// HOST INDIRECT ACCESS REGISTER (IREG) +#define ICM456XX_REG_IREG_ADDR_15_8 0x7C +#define ICM456XX_REG_IREG_ADDR_7_0 0x7D +#define ICM456XX_REG_IREG_DATA 0x7E + + +// IPREG_SYS1_REG_172 - 0xAC +#define ICM456XX_GYRO_UI_LPF_CFG_IREG_ADDR 0xA4AC // To access register in IPREG_SYS1, add base address 0xA400 + offset + +// LPF UI - 0xAC PREG_SYS1_REG_172 (bits 2:0) +#define ICM456XX_GYRO_UI_LPFBW_BYPASS 0x00 +#define ICM456XX_GYRO_UI_LPFBW_ODR_DIV_4 0x01 // 1600 Hz ODR = 6400 Hz: +#define ICM456XX_GYRO_UI_LPFBW_ODR_DIV_8 0x02 // 800 Hz ODR = 6400 Hz: +#define ICM456XX_GYRO_UI_LPFBW_ODR_DIV_16 0x03 // 400 Hz ODR = 6400 Hz: +#define ICM456XX_GYRO_UI_LPFBW_ODR_DIV_32 0x04 // 200 Hz ODR = 6400 Hz +#define ICM456XX_GYRO_UI_LPFBW_ODR_DIV_64 0x05 // 100 Hz ODR = 6400 Hz +#define ICM456XX_GYRO_UI_LPFBW_ODR_DIV_128 0x06 // 50 Hz ODR = 6400 Hz + +// IPREG_SYS2_REG_131 - 0x83 +#define ICM456XX_ACCEL_UI_LPF_CFG_IREG_ADDR 0xA583 // To access register in IPREG_SYS2, add base address 0xA500 + offset + +// Accel UI path LPF - 0x83 IPREG_SYS2_REG_131 (bits 2:0) +#define ICM456XX_ACCEL_UI_LPFBW_BYPASS 0x00 +#define ICM456XX_ACCEL_UI_LPFBW_ODR_DIV_4 0x01 // 400 Hz ODR = 1600 Hz: +#define ICM456XX_ACCEL_UI_LPFBW_ODR_DIV_8 0x02 // 200 Hz ODR = 1600 Hz: +#define ICM456XX_ACCEL_UI_LPFBW_ODR_DIV_16 0x03 // 100 Hz ODR = 1600 Hz: +#define ICM456XX_ACCEL_UI_LPFBW_ODR_DIV_32 0x04 // 50 Hz ODR = 1600 Hz +#define ICM456XX_ACCEL_UI_LPFBW_ODR_DIV_64 0x05 // 25 Hz ODR = 1600 Hz +#define ICM456XX_ACCEL_UI_LPFBW_ODR_DIV_128 0x06 // 12.5 Hz ODR = 1600 Hz + +#ifndef ICM456XX_CLOCK +// Default: 24 MHz max SPI frequency +#define ICM456XX_MAX_SPI_CLK_HZ 24000000 +#else +#define ICM456XX_MAX_SPI_CLK_HZ ICM456XX_CLOCK +#endif + +#define HZ_TO_US(hz) ((int32_t)((1000 * 1000) / (hz))) + +#define ICM456XX_BIT_IREG_DONE (1 << 0) + +// Startup timing constants (DS-000577 Table 9-6) +#define ICM456XX_ACCEL_STARTUP_TIME_MS 10 // Min accel startup from OFF/STANDBY/LP +#define ICM456XX_GYRO_STARTUP_TIME_MS 35 // Min gyro startup from OFF/STANDBY/LP +#define ICM456XX_SENSOR_ENABLE_DELAY_MS 1 // Allow sensors to power on and stabilize +#define ICM456XX_INT_CONFIG_DELAY_MS 15 // Register settle time after interrupt config (matches ICM-426xx convention) +#define ICM456XX_IREG_TIMEOUT_US 5000 // IREG operation timeout (5ms max) + +#define ICM456XX_DATA_LENGTH 6 // 3 axes * 2 bytes per axis +#define ICM456XX_SPI_BUFFER_SIZE (1 + ICM456XX_DATA_LENGTH) // 1 byte register + 6 bytes data + +static const gyroFilterAndRateConfig_t icm45xxGyroConfigs[] = { + /* LPF ODR { lpfBits, odrReg } */ + { GYRO_LPF_NONE, 6000, { ICM456XX_GYRO_UI_LPFBW_BYPASS, 0x03 } }, + { GYRO_LPF_256HZ, 6000, { ICM456XX_GYRO_UI_LPFBW_ODR_DIV_16, 0x03 } }, // ≈400 Hz + { GYRO_LPF_188HZ, 6000, { ICM456XX_GYRO_UI_LPFBW_ODR_DIV_32, 0x03 } }, // ≈200 Hz + { GYRO_LPF_98HZ, 6000, { ICM456XX_GYRO_UI_LPFBW_ODR_DIV_64, 0x03 } }, // ≈100 Hz + { GYRO_LPF_42HZ, 6000, { ICM456XX_GYRO_UI_LPFBW_ODR_DIV_128, 0x03 } }, // ≈50 Hz +}; + +/** + * @brief This function follows the IREG WRITE procedure (Section 14.1-14.4 of the datasheet) + * using indirect addressing via IREG_ADDR_15_8, IREG_ADDR_7_0, and IREG_DATA registers. + * After writing, an internal operation transfers the data to the target IREG address. + * Ensures compliance with the required minimum time gap and checks the IREG_DONE bit. + * + * @param dev Pointer to the SPI device structure. + * @param reg 16-bit internal IREG register address. + * @param value Value to be written to the register. + * @return true if the write was successful + */ +static bool icm45686WriteIREG(const busDevice_t *dev, uint16_t reg, uint8_t value) +{ + const uint8_t msb = (reg >> 8) & 0xFF; + const uint8_t lsb = reg & 0xFF; + + busWrite(dev, ICM456XX_REG_IREG_ADDR_15_8, msb); + busWrite(dev, ICM456XX_REG_IREG_ADDR_7_0, lsb); + busWrite(dev, ICM456XX_REG_IREG_DATA, value); + + // Check IREG_DONE (bit 0 of REG_MISC2 = 0x7F) with elapsed-time tracking + for (uint32_t waited_us = 0; waited_us < ICM456XX_IREG_TIMEOUT_US; waited_us += 10) { + uint8_t misc2 = 0; + busRead(dev, ICM456XX_REG_MISC2, &misc2); + if (misc2 & ICM456XX_BIT_IREG_DONE) { + return true; + } + delayMicroseconds(10); + } + + return false; // timeout +} + +static void icm45686AccInit(accDev_t *acc) +{ + acc->acc_1G = 512 * 4; // 16g scale +} + +static bool icm45686AccRead(accDev_t *acc) +{ + uint8_t data[6]; + + const bool ack = busReadBuf(acc->busDev, ICM456XX_ACCEL_DATA_X1_UI, data, 6); + if (!ack) { + return false; + } + + acc->ADCRaw[X] = (float) int16_val_little_endian(data, 0); + acc->ADCRaw[Y] = (float) int16_val_little_endian(data, 1); + acc->ADCRaw[Z] = (float) int16_val_little_endian(data, 2); + + return true; +} + +static bool icm45686GyroRead(gyroDev_t *gyro) +{ + uint8_t data[6]; + + const bool ack = busReadBuf(gyro->busDev, ICM456XX_GYRO_DATA_X1_UI, data, 6); + if (!ack) { + return false; + } + + gyro->gyroADCRaw[X] = (float) int16_val_little_endian(data, 0); + gyro->gyroADCRaw[Y] = (float) int16_val_little_endian(data, 1); + gyro->gyroADCRaw[Z] = (float) int16_val_little_endian(data, 2); + + return true; +} + +static bool icm45686ReadTemperature(gyroDev_t *gyro, int16_t * temp) +{ + uint8_t data[2]; + + const bool ack = busReadBuf(gyro->busDev, ICM456XX_TEMP_DATA1, data, 2); + if (!ack) { + return false; + } + // From datasheet: Temperature in Degrees Centigrade = (TEMP_DATA / 128) + 25 + *temp = ( int16_val_little_endian(data, 0) / 12.8f ) + 250.0f; // Temperature stored as degC*10 + + return true; +} + +static void icm45686AccAndGyroInit(gyroDev_t *gyro) +{ + busDevice_t * dev = gyro->busDev; + const gyroFilterAndRateConfig_t * config = chooseGyroConfig(gyro->lpf, 1000000 / gyro->requestedSampleIntervalUs, + &icm45xxGyroConfigs[0], ARRAYLEN(icm45xxGyroConfigs)); + gyro->sampleRateIntervalUs = 1000000 / config->gyroRateHz; + + busSetSpeed(dev, BUS_SPEED_INITIALIZATION); + + // enable sensors + busWrite(dev, ICM456XX_PWR_MGMT0, (ICM456XX_GYRO_MODE_LN | ICM456XX_ACCEL_MODE_LN)); + // Allow sensors to power on and stabilize + delay(ICM456XX_SENSOR_ENABLE_DELAY_MS); + // Configure accelerometer full-scale range (16g mode) + busWrite(dev, ICM456XX_ACCEL_CONFIG0, ICM456XX_ACCEL_FS_SEL_16G | ICM456XX_ACCEL_ODR_1K6_LN); + // Per datasheet Table 9-6: 10ms minimum startup time + delay(ICM456XX_ACCEL_STARTUP_TIME_MS); + // gyro filters + // Enable Anti-Alias (AAF) Filter and Interpolator for Gyro (Section 7.2 of datasheet) + if (!icm45686WriteIREG(dev, ICM456XX_GYRO_SRC_CTRL_IREG_ADDR, ICM456XX_SRC_CTRL_AAF_ENABLE_BIT | ICM456XX_SRC_CTRL_INTERP_ENABLE_BIT)) { + // AAF/Interpolator initialization failed, fallback to disabled state + icm45686WriteIREG(dev, ICM456XX_GYRO_SRC_CTRL_IREG_ADDR, 0); + } + // Set the Gyro UI LPF bandwidth cut-off (Section 7.3 of datasheet) + if (!icm45686WriteIREG(dev, ICM456XX_GYRO_UI_LPF_CFG_IREG_ADDR, config->gyroConfigValues[0])) { + // If LPF configuration fails, fallback to BYPASS + icm45686WriteIREG(dev, ICM456XX_GYRO_UI_LPF_CFG_IREG_ADDR, ICM456XX_GYRO_UI_LPFBW_BYPASS); + } + // accel filters + // Enable Anti-Alias Filter and Interpolator for Accel (Section 7.2 of datasheet) + if (!icm45686WriteIREG(dev, ICM456XX_ACCEL_SRC_CTRL_IREG_ADDR, ICM456XX_SRC_CTRL_AAF_ENABLE_BIT | ICM456XX_SRC_CTRL_INTERP_ENABLE_BIT)) { + icm45686WriteIREG(dev, ICM456XX_ACCEL_SRC_CTRL_IREG_ADDR, 0); + } + // Set the Accel UI LPF bandwidth cut-off to ODR/8 (Section 7.3 of datasheet) + if (!icm45686WriteIREG(dev, ICM456XX_ACCEL_UI_LPF_CFG_IREG_ADDR, ICM456XX_ACCEL_UI_LPFBW_ODR_DIV_8)) { + // If LPF configuration fails, fallback to BYPASS + icm45686WriteIREG(dev, ICM456XX_ACCEL_UI_LPF_CFG_IREG_ADDR, ICM456XX_ACCEL_UI_LPFBW_BYPASS); + } + // Setup scale and odr values for gyro + busWrite(dev, ICM456XX_GYRO_CONFIG0, ICM456XX_GYRO_FS_SEL_2000DPS | config->gyroConfigValues[1]); + // Per datasheet Table 9-6: 35ms minimum startup time + delay(ICM456XX_GYRO_STARTUP_TIME_MS); + + busWrite(dev, ICM456XX_INT1_CONFIG2, ICM456XX_INT1_MODE_PULSED | ICM456XX_INT1_DRIVE_CIRCUIT_PP | + ICM456XX_INT1_POLARITY_ACTIVE_HIGH); + busWrite(dev, ICM456XX_INT1_CONFIG0, ICM456XX_INT1_STATUS_EN_DRDY); + + delay(ICM456XX_INT_CONFIG_DELAY_MS); + busSetSpeed(dev, BUS_SPEED_FAST); +} + +static bool icm45686DeviceDetect(busDevice_t * dev) +{ + uint8_t tmp = 0xFF; + uint8_t attemptsRemaining = 5; + uint32_t waitedMs = 0; + + busSetSpeed(dev, BUS_SPEED_INITIALIZATION); + // ICM-45686 does not use bank switching (register 0x75 is reserved) + // Perform soft reset directly + // Soft reset + busWrite(dev, ICM456XX_REG_MISC2, ICM456XX_SOFT_RESET); + // Poll until soft reset completes (SOFT_RESET bit clears) per datasheet Section 9.4 + do { + busRead(dev, ICM456XX_REG_MISC2, &tmp); + if (!(tmp & ICM456XX_SOFT_RESET)) { + break; + } + delay(1); + waitedMs++; + } while (waitedMs < 20); + + if (tmp & ICM456XX_SOFT_RESET) { + return false; + } + // Initialize power management to a known state after reset + // This ensures sensors are off and ready for proper initialization + busWrite(dev, ICM456XX_PWR_MGMT0, 0x00); + + do { + delay(150); + busRead(dev, ICM456XX_WHO_AM_REGISTER, &tmp); + if (tmp == ICM45686_WHO_AM_I_CONST) { + return true; + } + } while (attemptsRemaining--); + + return false; +} + +bool icm45686AccDetect(accDev_t *acc) +{ + acc->busDev = busDeviceOpen(BUSTYPE_ANY, DEVHW_ICM45686, acc->imuSensorToUse); + if (acc->busDev == NULL) { + return false; + } + + mpuContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev); + if (ctx->chipMagicNumber != 0x4265) { + return false; + } + + acc->initFn = icm45686AccInit; + acc->readFn = icm45686AccRead; + acc->accAlign = acc->busDev->param; + + return true; +} + +bool icm45686GyroDetect(gyroDev_t *gyro) +{ + gyro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_ICM45686, gyro->imuSensorToUse, OWNER_MPU); + if (gyro->busDev == NULL) { + return false; + } + + if (!icm45686DeviceDetect(gyro->busDev)) { + busDeviceDeInit(gyro->busDev); + return false; + } + + // Magic number for ACC detection to indicate that we have detected icm45686 gyro + mpuContextData_t * ctx = busDeviceGetScratchpadMemory(gyro->busDev); + ctx->chipMagicNumber = 0x4265; + + gyro->initFn = icm45686AccAndGyroInit; + gyro->readFn = icm45686GyroRead; + gyro->intStatusFn = gyroCheckDataReady; + gyro->temperatureFn = icm45686ReadTemperature; + gyro->scale = 1.0f / 16.4f; // 16.4 dps/lsb scalefactor + gyro->gyroAlign = gyro->busDev->param; + + return true; +} + + +#endif diff --git a/src/main/drivers/accgyro/accgyro_icm45686.h b/src/main/drivers/accgyro/accgyro_icm45686.h new file mode 100644 index 00000000000..aac195aa220 --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_icm45686.h @@ -0,0 +1,22 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#pragma once + + +bool icm45686AccDetect(accDev_t *acc); +bool icm45686GyroDetect(gyroDev_t *gyro); diff --git a/src/main/drivers/accgyro/accgyro_lsm6dxx.c b/src/main/drivers/accgyro/accgyro_lsm6dxx.c index 5e7d24f44e4..874f32eacb3 100644 --- a/src/main/drivers/accgyro/accgyro_lsm6dxx.c +++ b/src/main/drivers/accgyro/accgyro_lsm6dxx.c @@ -222,6 +222,7 @@ bool lsm6dGyroDetect(gyroDev_t *gyro) gyro->initFn = lsm6dxxSpiGyroInit; gyro->readFn = lsm6dxxGyroRead; gyro->intStatusFn = gyroCheckDataReady; + gyro->gyroAlign = gyro->busDev->param; gyro->scale = 1.0f / 16.4f; // 2000 dps return true; diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h index 62c2778b45b..0f59d10d3e8 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.h +++ b/src/main/drivers/accgyro/accgyro_mpu.h @@ -30,6 +30,7 @@ #define ICM20689_WHO_AM_I_CONST (0x98) #define ICM42605_WHO_AM_I_CONST (0x42) #define ICM42688P_WHO_AM_I_CONST (0x47) +#define ICM45686_WHO_AM_I_CONST (0xE9) // RA = Register Address diff --git a/src/main/drivers/bus.c b/src/main/drivers/bus.c index 0da3f76d307..71410fb2748 100644 --- a/src/main/drivers/bus.c +++ b/src/main/drivers/bus.c @@ -283,7 +283,7 @@ bool busWriteBuf(const busDevice_t * dev, uint8_t reg, const uint8_t * data, uin return spiBusWriteBuffer(dev, reg, data, length); } else { - return spiBusWriteBuffer(dev, reg | 0x80, data, length); + return spiBusWriteBuffer(dev, reg & 0x7F, data, length); } #else return false; diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index 5a3e6dba453..385fbd36c0e 100644 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -85,6 +85,7 @@ typedef enum { DEVHW_ICM42605, DEVHW_BMI270, DEVHW_LSM6D, + DEVHW_ICM45686, /* Combined ACC/GYRO/MAG chips */ DEVHW_MPU9250, @@ -140,6 +141,7 @@ typedef enum { /* Other hardware */ DEVHW_MS4525, // Pitot meter + DEVHW_MS5525, // Pitot meter DEVHW_DLVR, // Pitot meter DEVHW_M25P16, // SPI NOR flash DEVHW_W25N, // SPI 128MB or 256MB flash from Winbond W25N family diff --git a/src/main/drivers/flash.h b/src/main/drivers/flash.h index 4dfa8cb1953..0e522b87890 100644 --- a/src/main/drivers/flash.h +++ b/src/main/drivers/flash.h @@ -40,6 +40,8 @@ typedef struct flashGeometry_s { uint32_t totalSize; // This is just sectorSize * sectors uint16_t pagesPerSector; flashType_e flashType; + int32_t bbReplacementBlocks; + uint8_t bblutTableEntryCount; // Used by W25N_BBLUT_TABLE_ENTRY_COUNT for bad-block scanning } flashGeometry_t; typedef struct diff --git a/src/main/drivers/flash_w25n.c b/src/main/drivers/flash_w25n.c index 2791d3d89e1..8eef2e6944f 100644 --- a/src/main/drivers/flash_w25n.c +++ b/src/main/drivers/flash_w25n.c @@ -23,7 +23,7 @@ #include "platform.h" -#if defined(USE_FLASH_W25N01G) || defined(USE_FLASH_W25N02K) +#if defined(USE_FLASH_W25N01G) || defined(USE_FLASH_W25N02K) || defined(USE_FLASH_MX35LF2G) #include "drivers/bus.h" #include "drivers/io.h" @@ -37,26 +37,24 @@ #define W25N01GV_BLOCKS_PER_DIE 1024 #define W25N02KV_BLOCKS_PER_DIE 2048 - +#define MX35LF2G_BLOCKS_PER_DIE 2048 // BB replacement area #define W25N_BB_MARKER_BLOCKS 1 -#define W25N_BB_REPLACEMENT_BLOCKS 20 +#define W25N_BB_REPLACEMENT_BLOCKS (geometry.bbReplacementBlocks) #define W25N_BB_MANAGEMENT_BLOCKS (W25N_BB_REPLACEMENT_BLOCKS + W25N_BB_MARKER_BLOCKS) // blocks are zero-based index -#define W25N_BB_REPLACEMENT_START_BLOCK (W25N_BLOCKS_PER_DIE - W25N_BB_REPLACEMENT_BLOCKS) -#define W25N_BB_MANAGEMENT_START_BLOCK (W25N_BLOCKS_PER_DIE - W25N_BB_MANAGEMENT_BLOCKS) +#define W25N_BB_REPLACEMENT_START_BLOCK (geometry.sectors - W25N_BB_REPLACEMENT_BLOCKS) +#define W25N_BB_MANAGEMENT_START_BLOCK (geometry.sectors - W25N_BB_MANAGEMENT_BLOCKS) #define W25N_BB_MARKER_BLOCK (W25N_BB_REPLACEMENT_START_BLOCK - W25N_BB_MARKER_BLOCKS) // Instructions #define W25N_INSTRUCTION_RDID 0x9F #define W25N_INSTRUCTION_DEVICE_RESET 0xFF -#define W25N_INSTRUCTION_READ_STATUS_REG 0x05 -#define W25N_INSTRUCTION_READ_STATUS_ALTERNATE_REG 0x0F -#define W25N_INSTRUCTION_WRITE_STATUS_REG 0x01 -#define W25N_INSTRUCTION_WRITE_STATUS_ALTERNATE_REG 0x1F +#define W25N_INSTRUCTION_READ_STATUS_REG 0x0F +#define W25N_INSTRUCTION_WRITE_STATUS_REG 0x1F #define W25N_INSTRUCTION_WRITE_ENABLE 0x06 #define W25N_INSTRUCTION_DIE_SELECT 0xC2 #define W25N_INSTRUCTION_BLOCK_ERASE 0xD8 @@ -99,7 +97,7 @@ #define W25N_STATUS_ERASE_FAIL (1 << 2) #define W25N_STATUS_FLAG_WRITE_ENABLED (1 << 1) #define W25N_STATUS_FLAG_BUSY (1 << 0) -#define W25N_BBLUT_TABLE_ENTRY_COUNT 20 +#define W25N_BBLUT_TABLE_ENTRY_COUNT geometry.bblutTableEntryCount #define W25N_BBLUT_TABLE_ENTRY_SIZE 4 // in bytes // Bits in LBA for BB LUT @@ -110,8 +108,8 @@ // Some useful defs and macros #define W25N_LINEAR_TO_COLUMN(laddr) ((laddr) % W25N_PAGE_SIZE) #define W25N_LINEAR_TO_PAGE(laddr) ((laddr) / W25N_PAGE_SIZE) -#define W25N_LINEAR_TO_BLOCK(laddr) (W25N_LINEAR_TO_PAGE(laddr) / W25N_PAGES_PER_BLOCK) -#define W25N_BLOCK_TO_PAGE(block) ((block) * W25N_PAGES_PER_BLOCK) +#define W25N_LINEAR_TO_BLOCK(laddr) (W25N_LINEAR_TO_PAGE(laddr) / geometry.pagesPerSector) +#define W25N_BLOCK_TO_PAGE(block) ((block) * geometry.pagesPerSector) #define W25N_BLOCK_TO_LINEAR(block) (W25N_BLOCK_TO_PAGE(block) * W25N_PAGE_SIZE) // IMPORTANT: Timeout values are currently required to be set to the highest value required by any of the supported flash chips by this driver @@ -129,6 +127,7 @@ // JEDEC ID #define JEDEC_ID_WINBOND_W25N01GV 0xEFAA21 #define JEDEC_ID_WINBOND_W25N02KV 0xEFAA22 +#define JEDEC_ID_MACRONIX_MX35LF2G 0xC22603 static busDevice_t *busDev = NULL; static flashGeometry_t geometry; @@ -242,19 +241,31 @@ bool w25n_detect(uint32_t chipID) geometry.sectors = W25N01GV_BLOCKS_PER_DIE; // Blocks geometry.pagesPerSector = W25N_PAGES_PER_BLOCK; // Pages/Blocks geometry.pageSize = W25N_PAGE_SIZE; + geometry.bbReplacementBlocks = 20; + geometry.bblutTableEntryCount = 20; break; case JEDEC_ID_WINBOND_W25N02KV: geometry.sectors = W25N02KV_BLOCKS_PER_DIE; // Blocks geometry.pagesPerSector = W25N_PAGES_PER_BLOCK; // Pages/Blocks geometry.pageSize = W25N_PAGE_SIZE; + geometry.bbReplacementBlocks = 20; + geometry.bblutTableEntryCount = 20; + break; + case JEDEC_ID_MACRONIX_MX35LF2G: + geometry.sectors = MX35LF2G_BLOCKS_PER_DIE; // Blocks + geometry.pagesPerSector = W25N_PAGES_PER_BLOCK; // Pages/Blocks + geometry.pageSize = W25N_PAGE_SIZE; + geometry.bbReplacementBlocks = 40; + geometry.bblutTableEntryCount = 40; break; - default: // Unsupported chip geometry.sectors = 0; geometry.pagesPerSector = 0; geometry.sectorSize = 0; geometry.totalSize = 0; + geometry.bbReplacementBlocks = 0; + geometry.bblutTableEntryCount = 0; return false; } diff --git a/src/main/drivers/io.c b/src/main/drivers/io.c index b13b73cbfd2..c602a2df492 100644 --- a/src/main/drivers/io.c +++ b/src/main/drivers/io.c @@ -244,10 +244,11 @@ void IOToggle(IO_t io) IO_GPIO(io)->BSRRL = mask; } #elif defined(AT32F43x) - if (IO_GPIO(io)->odt & mask) - mask <<= 16; // bit is set, shift mask to reset half - - IO_GPIO(io)->scr = IO_Pin(io); + if (IO_GPIO(io)->odt & mask) { + IO_GPIO(io)->clr = mask; + } else { + IO_GPIO(io)->scr = mask; + } #else if (IO_GPIO(io)->ODR & mask) mask <<= 16; // bit is set, shift mask to reset half diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index 4d541d32027..73493b50f6e 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -293,6 +293,17 @@ static int max7456PrepareBuffer(uint8_t * buf, size_t bufsize, int bufPtr, uint8 return bufPtr; } +static void max7456ApplyBusSpeed(void) +{ +#if defined(MAX7456_SPI_SPEED) + busSpeed_e speed = (MAX7456_SPI_SPEED <= BUS_SPEED_ULTRAFAST) ? MAX7456_SPI_SPEED : BUS_SPEED_STANDARD; + busSetSpeed(state.dev, speed); +#else + // Default safe speed for MAX7456 + busSetSpeed(state.dev, BUS_SPEED_STANDARD); +#endif +} + uint16_t max7456GetScreenSize(void) { // Default to PAL while the display is not yet initialized. This @@ -386,7 +397,7 @@ void max7456Init(const videoSystem_e videoSystem) return; } - busSetSpeed(state.dev, BUS_SPEED_STANDARD); + max7456ApplyBusSpeed(); // force soft reset on Max7456 busWrite(state.dev, MAX7456ADD_VM0, MAX7456_RESET); diff --git a/src/main/drivers/pitotmeter/pitotmeter_ms5525.c b/src/main/drivers/pitotmeter/pitotmeter_ms5525.c new file mode 100644 index 00000000000..303c4becb5f --- /dev/null +++ b/src/main/drivers/pitotmeter/pitotmeter_ms5525.c @@ -0,0 +1,226 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include +#include + +#include "platform.h" +#include "build/debug.h" + +#include "common/utils.h" +#include "common/maths.h" +#include "drivers/time.h" +#include "drivers/bus_i2c.h" +#include "drivers/pitotmeter/pitotmeter.h" +#include "drivers/pitotmeter/pitotmeter_ms5525.h" + +// MS5525 I2C Addresses +#define MS5525_ADDR_1 0x76 +#define MS5525_ADDR_2 0x77 + +#define CMD_RESET 0x1E // ADC reset command +#define CMD_ADC_READ 0x00 // ADC read command +#define CMD_ADC_CONV 0x40 // ADC conversion command +#define CMD_ADC_D1 0x00 // ADC D1 conversion (Pressure) +#define CMD_ADC_D2 0x10 // ADC D2 conversion (Temperature) +#define CMD_ADC_4096 0x08 // ADC OSR=4096 +#define CMD_PROM_RD 0xA0 // Prom read command +#define PROM_NB 8 + +typedef struct __attribute__ ((__packed__)) ms5525Ctx_s { + uint16_t c[6]; // c1 through c6 + uint32_t up; // up (24 bits) + step (top 8 bits) + uint32_t ut; // ut (24 bits) +} ms5525Ctx_t; + +STATIC_ASSERT(sizeof(ms5525Ctx_t) <= BUS_SCRATCHPAD_MEMORY_SIZE, busDevice_scratchpad_memory_too_small); + +static int8_t ms5525_crc(uint16_t *prom) +{ + int32_t i, j; + uint32_t res = 0; + uint8_t crc = prom[7] & 0xF; + prom[7] &= 0xFF00; + + bool blankEeprom = true; + + for (i = 0; i < 16; i++) { + if (prom[i >> 1]) { + blankEeprom = false; + } + if (i & 1) + res ^= ((prom[i >> 1]) & 0x00FF); + else + res ^= (prom[i >> 1] >> 8); + for (j = 8; j > 0; j--) { + if (res & 0x8000) + res ^= 0x1800; + res <<= 1; + } + } + prom[7] |= crc; + if (!blankEeprom && crc == ((res >> 12) & 0xF)) + return 0; + + return -1; +} + +static bool ms5525_read_adc(pitotDev_t *pitot, uint32_t *result) +{ + uint8_t rxbuf[3]; + if (busReadBuf(pitot->busDev, CMD_ADC_READ, rxbuf, 3)) { + *result = (rxbuf[0] << 16) | (rxbuf[1] << 8) | rxbuf[2]; + return true; + } + return false; +} + +static bool ms5525_start(pitotDev_t * pitot) +{ + ms5525Ctx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev); + uint8_t step = ctx->up >> 24; + + if (step == 0) { + return busWrite(pitot->busDev, CMD_ADC_CONV + CMD_ADC_D1 + CMD_ADC_4096, 1); + } else { + return busWrite(pitot->busDev, CMD_ADC_CONV + CMD_ADC_D2 + CMD_ADC_4096, 1); + } +} + +static bool ms5525_read(pitotDev_t * pitot) +{ + ms5525Ctx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev); + uint8_t step = ctx->up >> 24; + + uint32_t adc_val = 0; + if (!ms5525_read_adc(pitot, &adc_val)) { + return false; + } + + if (step == 0) { + ctx->up = adc_val | (1 << 24); + return true; + } else { + ctx->ut = adc_val; + ctx->up &= 0x00FFFFFF; // clear step back to 0 + return true; + } +} + +static void ms5525_calculate(pitotDev_t * pitot, float *pressure, float *temperature) +{ + ms5525Ctx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev); + + uint32_t up = ctx->up & 0x00FFFFFF; + uint32_t ut = ctx->ut; + + if (up == 0 || ut == 0) { + return; // Wait until both are read at least once + } + + // 5525DSO-pp001DS coefficients (1 psi): + // Q1=15, Q2=17, Q3=7, Q4=5, Q5=7, Q6=21 + int64_t dT = (int64_t)ut - ((int64_t)ctx->c[4] << 7); // c[5] is c[4] (0-indexed) + int64_t off = ((int64_t)ctx->c[1] << 17) + (((int64_t)ctx->c[3] * dT) >> 5); // c[2]=c[1], c[4]=c[3] + int64_t sens = ((int64_t)ctx->c[0] << 15) + (((int64_t)ctx->c[2] * dT) >> 7); // c[1]=c[0], c[3]=c[2] + int64_t temp = 2000 + ((dT * (int64_t)ctx->c[5]) >> 21); // c[6]=c[5] + + int64_t p_raw = ((((int64_t)up * sens) >> 21) - off) >> 15; + + // Convert 1 PSI sensor output to Pa + // 1 psi = 6894.757 Pa. p_raw is 0.0001 psi per bit -> 0.6894757 Pa per bit. + + if (pressure) { + *pressure = (float)p_raw * 0.6894757f; + } + + if (temperature) { + *temperature = C_TO_KELVIN(temp / 100.0f); + } +} + +static bool deviceDetect(busDevice_t * dev) +{ + // Verify an I2C transaction works: read PROM + uint8_t rxbuf[2]; + bool ack = busReadBuf(dev, CMD_PROM_RD, rxbuf, 2); + if (ack) { + return true; + } + return false; +} + +bool ms5525Detect(pitotDev_t * pitot) +{ + pitot->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_MS5525, 0, OWNER_AIRSPEED); + if (pitot->busDev == NULL) { + return false; + } + + // Try primary address 0x76 +#ifdef USE_I2C + pitot->busDev->busdev.i2c.address = MS5525_ADDR_1; +#endif + if (!deviceDetect(pitot->busDev)) { + // Fallback to secondary 0x77 +#ifdef USE_I2C + pitot->busDev->busdev.i2c.address = MS5525_ADDR_2; +#endif + if (!deviceDetect(pitot->busDev)) { + busDeviceDeInit(pitot->busDev); + return false; + } + } + + // Sensor found, initialize + busWrite(pitot->busDev, CMD_RESET, 1); + delay(5); + + ms5525Ctx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev); + + // Read PROM + uint16_t prom[8]; + for (int i = 0; i < PROM_NB; i++) { + uint8_t rxbuf[2] = { 0, 0 }; + busReadBuf(pitot->busDev, CMD_PROM_RD + i * 2, rxbuf, 2); + prom[i] = (rxbuf[0] << 8 | rxbuf[1]); + } + + // Check CRC + if (ms5525_crc(prom) != 0) { + busDeviceDeInit(pitot->busDev); + return false; + } + + // Copy to ctx starting from c1 to c6 + for (int i = 0; i < 6; i++) { + ctx->c[i] = prom[i+1]; + } + + // Setup Context + ctx->up = 0; + ctx->ut = 0; + + // Pitot delays + pitot->delay = 10000; // max 9.04ms for OSR4096 + pitot->calibThreshold = 0.00005f; + pitot->start = ms5525_start; + pitot->get = ms5525_read; + pitot->calculate = ms5525_calculate; + + return true; +} diff --git a/src/main/drivers/pitotmeter/pitotmeter_ms5525.h b/src/main/drivers/pitotmeter/pitotmeter_ms5525.h new file mode 100644 index 00000000000..822ba72e2ed --- /dev/null +++ b/src/main/drivers/pitotmeter/pitotmeter_ms5525.h @@ -0,0 +1,22 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#pragma once + +#include "drivers/pitotmeter/pitotmeter.h" + +bool ms5525Detect(pitotDev_t *pitot); diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 619f4b95db5..a4efb9e3008 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -124,6 +124,18 @@ static timeUs_t commandPostDelay = 0; static circularBuffer_t commandsCircularBuffer; static uint8_t commandsBuff[DHSOT_COMMAND_QUEUE_SIZE]; static currentExecutingCommand_t currentExecutingCommand; + +static uint16_t prepareDshotPacket(const uint16_t value, bool requestTelemetry); +#ifndef USE_DSHOT_DMAR +static void loadDmaBufferDshot(timerDMASafeType_t *dmaBuffer, uint16_t packet); +#else +static void loadDmaBufferDshotStride(timerDMASafeType_t *dmaBuffer, int stride, uint16_t packet); +#endif + +#ifdef USE_DSHOT_DMAR +burstDmaTimer_t burstDmaTimers[MAX_DMA_TIMERS]; +uint8_t burstDmaTimersCount = 0; +#endif #endif static void pwmOutConfigTimer(pwmOutputPort_t * p, TCH_t * tch, uint32_t hz, uint16_t period, uint16_t value) @@ -226,6 +238,56 @@ void pwmEnableMotors(void) pwmMotorsEnabled = true; } +void pwmSetMotorDMACircular(bool circular) +{ +#ifdef USE_DSHOT + if (!isMotorProtocolDshot()) { + return; + } + + int motorCount = getMotorCount(); + + if (circular) { + // Load zero-throttle packets directly into DMA buffers, + // bypassing the rate limiter in pwmCompleteMotorUpdate() + uint16_t packet = prepareDshotPacket(0, false); + for (int i = 0; i < motorCount; i++) { + if (motors[i].pwmPort && motors[i].pwmPort->configured) { +#ifdef USE_DSHOT_DMAR + loadDmaBufferDshotStride(&motors[i].pwmPort->dmaBurstBuffer[motors[i].pwmPort->tch->timHw->channelIndex], 4, packet); +#else + loadDmaBufferDshot(motors[i].pwmPort->dmaBuffer, packet); +#endif + } + } + } + +#ifdef USE_DSHOT_DMAR + // Burst DMA: one DMA stream per timer, shared across channels + for (int i = 0; i < burstDmaTimersCount; i++) { + burstDmaTimer_t *burstDmaTimer = &burstDmaTimers[i]; + // Find the first motor using this timer to get the TCH for DMA state + for (int m = 0; m < motorCount; m++) { + if (motors[m].pwmPort && motors[m].pwmPort->configured && motors[m].pwmPort->tch + && motors[m].pwmPort->tch->timHw->tim == burstDmaTimer->timer) { + impl_pwmBurstDMASetCircular(burstDmaTimer, motors[m].pwmPort->tch, circular, DSHOT_DMA_BUFFER_SIZE * 4); + break; + } + } + } +#else + // Per-channel DMA: one DMA stream per motor + for (int i = 0; i < motorCount; i++) { + if (motors[i].pwmPort && motors[i].pwmPort->configured && motors[i].pwmPort->tch) { + impl_timerPWMSetDMACircular(motors[i].pwmPort->tch, circular, DSHOT_DMA_BUFFER_SIZE); + } + } +#endif +#else + UNUSED(circular); +#endif +} + bool isMotorBrushed(uint16_t motorPwmRateHz) { return (motorPwmRateHz > 500); @@ -264,9 +326,6 @@ uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType) } #ifdef USE_DSHOT_DMAR -burstDmaTimer_t burstDmaTimers[MAX_DMA_TIMERS]; -uint8_t burstDmaTimersCount = 0; - static uint8_t getBurstDmaTimerIndex(TIM_TypeDef *timer) { for (int i = 0; i < burstDmaTimersCount; i++) { diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 1041ace04fa..1dc644f7f4e 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -50,6 +50,7 @@ void pwmWriteServo(uint8_t index, uint16_t value); void pwmDisableMotors(void); void pwmEnableMotors(void); +void pwmSetMotorDMACircular(bool circular); struct timerHardware_s; void pwmMotorPreconfigure(void); diff --git a/src/main/drivers/serial_uart_hal.c b/src/main/drivers/serial_uart_hal.c index cce38422848..f25b2cbafb6 100644 --- a/src/main/drivers/serial_uart_hal.c +++ b/src/main/drivers/serial_uart_hal.c @@ -52,6 +52,13 @@ static void usartConfigurePinInversion(uartPort_t *uartPort) { uartPort->Handle.AdvancedInit.TxPinLevelInvert = UART_ADVFEATURE_TXINV_ENABLE; } } + +#ifdef USE_UART4_SWAP + if (uartPort->Handle.Instance == UART4) { + uartPort->Handle.AdvancedInit.AdvFeatureInit |= UART_ADVFEATURE_SWAP_INIT; + uartPort->Handle.AdvancedInit.Swap = UART_ADVFEATURE_SWAP_ENABLE; + } +#endif } static void uartReconfigure(uartPort_t *uartPort) diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index d87e0400d52..512b0d7c106 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -138,6 +138,7 @@ typedef enum { TCH_DMA_IDLE = 0, TCH_DMA_READY, TCH_DMA_ACTIVE, + TCH_DMA_CIRCULAR, } tchDmaState_e; // Some forward declarations for types diff --git a/src/main/drivers/timer_impl.h b/src/main/drivers/timer_impl.h index 4d0a87f9aa5..6a302f57cb4 100644 --- a/src/main/drivers/timer_impl.h +++ b/src/main/drivers/timer_impl.h @@ -84,8 +84,10 @@ bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBuf void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferElementCount); void impl_timerPWMStartDMA(TCH_t * tch); void impl_timerPWMStopDMA(TCH_t * tch); +void impl_timerPWMSetDMACircular(TCH_t * tch, bool circular, uint32_t dmaBufferSize); #ifdef USE_DSHOT_DMAR bool impl_timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount); void impl_pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstLength); +void impl_pwmBurstDMASetCircular(burstDmaTimer_t * burstDmaTimer, TCH_t * tch, bool circular, uint32_t dmaBufferSize); #endif \ No newline at end of file diff --git a/src/main/drivers/timer_impl_hal.c b/src/main/drivers/timer_impl_hal.c index 8df0f7024d3..9c4093bafa8 100644 --- a/src/main/drivers/timer_impl_hal.c +++ b/src/main/drivers/timer_impl_hal.c @@ -319,6 +319,12 @@ static void impl_timerDMA_IRQHandler(DMA_t descriptor) if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) { TCH_t * tch = (TCH_t *)descriptor->userParam; + // In circular mode, let DMA keep running - don't disable the stream + if (tch->dmaState == TCH_DMA_CIRCULAR) { + DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF); + return; + } + // If it was ACTIVE - switch to IDLE if (tch->dmaState == TCH_DMA_ACTIVE) { tch->dmaState = TCH_DMA_IDLE; @@ -512,6 +518,46 @@ void impl_pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstLength //LL_TIM_EnableDMAReq_UPDATE(burstDmaTimer->timer); LL_TIM_EnableDMAReq_CCx(burstDmaTimer->timer, burstDmaTimer->burstRequestSource); } + +void impl_pwmBurstDMASetCircular(burstDmaTimer_t * burstDmaTimer, TCH_t * tch, bool circular, uint32_t dmaBufferSize) +{ + if (!tch->dma || !tch->dma->dma) { + return; + } + + ATOMIC_BLOCK(NVIC_PRIO_MAX) { + LL_TIM_DisableDMAReq_CCx(burstDmaTimer->timer, burstDmaTimer->burstRequestSource); + LL_DMA_DisableStream(burstDmaTimer->dma, burstDmaTimer->streamLL); + + uint32_t timeout = 10000; + while (LL_DMA_IsEnabledStream(burstDmaTimer->dma, burstDmaTimer->streamLL) && timeout--) { + __NOP(); + } + + if (LL_DMA_IsEnabledStream(burstDmaTimer->dma, burstDmaTimer->streamLL)) { + LL_TIM_EnableDMAReq_CCx(burstDmaTimer->timer, burstDmaTimer->burstRequestSource); + return; + } + + DMA_CLEAR_FLAG(tch->dma, DMA_IT_TCIF); + + if (circular) { + LL_DMA_SetMode(burstDmaTimer->dma, burstDmaTimer->streamLL, LL_DMA_MODE_CIRCULAR); + LL_DMA_SetDataLength(burstDmaTimer->dma, burstDmaTimer->streamLL, dmaBufferSize); + LL_DMA_DisableIT_TC(burstDmaTimer->dma, burstDmaTimer->streamLL); + tch->dmaState = TCH_DMA_CIRCULAR; + } else { + LL_DMA_SetMode(burstDmaTimer->dma, burstDmaTimer->streamLL, LL_DMA_MODE_NORMAL); + LL_DMA_EnableIT_TC(burstDmaTimer->dma, burstDmaTimer->streamLL); + tch->dmaState = TCH_DMA_IDLE; + } + + __DSB(); + + LL_DMA_EnableStream(burstDmaTimer->dma, burstDmaTimer->streamLL); + LL_TIM_EnableDMAReq_CCx(burstDmaTimer->timer, burstDmaTimer->burstRequestSource); + } +} #endif void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferElementCount) @@ -528,6 +574,20 @@ void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferElementCount) DMA_CLEAR_FLAG(tch->dma, DMA_IT_TCIF); } + // Wait for EN bit to actually clear before reconfiguring DMA registers. + // Per STM32F7 RM: writes to DMA_SxNDTR and DMA_SxM0AR are ignored while EN=1. + // The EN bit does not clear synchronously - hardware may still be completing an + // in-progress burst when software writes 0 to EN. + for (uint32_t timeout = 10000; timeout && LL_DMA_IsEnabledStream(dmaBase, streamLL); timeout--) { + __NOP(); + } + if (LL_DMA_IsEnabledStream(dmaBase, streamLL)) { + // EN did not clear - cannot reconfigure this cycle. Skip frame (ESC holds + // last command); EN should clear before the next call. + tch->dmaState = TCH_DMA_IDLE; + return; + } + LL_DMA_SetDataLength(dmaBase, streamLL, dmaBufferElementCount); LL_DMA_ConfigAddresses(dmaBase, streamLL, (uint32_t)tch->dmaBuffer, (uint32_t)impl_timerCCR(tch), LL_DMA_DIRECTION_MEMORY_TO_PERIPH); LL_DMA_EnableIT_TC(dmaBase, streamLL); @@ -580,3 +640,52 @@ void impl_timerPWMStopDMA(TCH_t * tch) HAL_TIM_Base_Start(tch->timCtx->timHandle); } + +void impl_timerPWMSetDMACircular(TCH_t * tch, bool circular, uint32_t dmaBufferSize) +{ + if (!tch->dma || !tch->dma->dma) { + return; + } + + const uint32_t streamLL = lookupDMALLStreamTable[DMATAG_GET_STREAM(tch->timHw->dmaTag)]; + DMA_TypeDef *dmaBase = tch->dma->dma; + + ATOMIC_BLOCK(NVIC_PRIO_MAX) { + // Stop new transfer triggers before reconfiguring + LL_TIM_DisableDMAReq_CCx(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex]); + LL_DMA_DisableStream(dmaBase, streamLL); + + // STM32H7 RM: poll EN bit until stream is actually disabled + uint32_t timeout = 10000; // ~20us at 480MHz, well above worst-case disable latency + while (LL_DMA_IsEnabledStream(dmaBase, streamLL) && timeout--) { + __NOP(); + } + + if (LL_DMA_IsEnabledStream(dmaBase, streamLL)) { + LL_TIM_EnableDMAReq_CCx(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex]); + return; + } + + DMA_CLEAR_FLAG(tch->dma, DMA_IT_TCIF); + + if (circular) { + LL_DMA_SetMode(dmaBase, streamLL, LL_DMA_MODE_CIRCULAR); + // Circular mode requires non-zero NDTR (STM32H7 RM constraint) + LL_DMA_SetDataLength(dmaBase, streamLL, dmaBufferSize); + // Disable TC interrupt — in circular mode, TC fires every cycle + // and the IRQ handler would otherwise disable the stream + LL_DMA_DisableIT_TC(dmaBase, streamLL); + tch->dmaState = TCH_DMA_CIRCULAR; + } else { + LL_DMA_SetMode(dmaBase, streamLL, LL_DMA_MODE_NORMAL); + LL_DMA_EnableIT_TC(dmaBase, streamLL); + tch->dmaState = TCH_DMA_IDLE; + } + + // Ensure register writes are visible to DMA before re-enabling + __DSB(); + + LL_DMA_EnableStream(dmaBase, streamLL); + LL_TIM_EnableDMAReq_CCx(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex]); + } +} diff --git a/src/main/drivers/timer_impl_stdperiph.c b/src/main/drivers/timer_impl_stdperiph.c index d2bd35dd521..5b52cb862a4 100644 --- a/src/main/drivers/timer_impl_stdperiph.c +++ b/src/main/drivers/timer_impl_stdperiph.c @@ -270,6 +270,13 @@ static void impl_timerDMA_IRQHandler(DMA_t descriptor) { if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) { TCH_t * tch = (TCH_t *)descriptor->userParam; + + // In circular mode, let DMA keep running - don't disable the stream + if (tch->dmaState == TCH_DMA_CIRCULAR) { + DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF); + return; + } + tch->dmaState = TCH_DMA_IDLE; DMA_Cmd(tch->dma->ref, DISABLE); @@ -463,6 +470,46 @@ void impl_pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstLength TIM_DMAConfig(burstDmaTimer->timer, TIM_DMABase_CCR1, TIM_DMABurstLength_4Transfers); TIM_DMACmd(burstDmaTimer->timer, burstDmaTimer->burstRequestSource, ENABLE); } + +void impl_pwmBurstDMASetCircular(burstDmaTimer_t * burstDmaTimer, TCH_t * tch, bool circular, uint32_t dmaBufferSize) +{ + if (!tch->dma || !tch->dma->ref) { + return; + } + + ATOMIC_BLOCK(NVIC_PRIO_MAX) { + TIM_DMACmd(burstDmaTimer->timer, burstDmaTimer->burstRequestSource, DISABLE); + DMA_Cmd(burstDmaTimer->dmaBurstStream, DISABLE); + + uint32_t timeout = 10000; + while ((burstDmaTimer->dmaBurstStream->CR & DMA_SxCR_EN) && timeout--) { + __NOP(); + } + + if (burstDmaTimer->dmaBurstStream->CR & DMA_SxCR_EN) { + TIM_DMACmd(burstDmaTimer->timer, burstDmaTimer->burstRequestSource, ENABLE); + return; + } + + DMA_CLEAR_FLAG(tch->dma, DMA_IT_TCIF); + + if (circular) { + burstDmaTimer->dmaBurstStream->CR |= DMA_SxCR_CIRC; + DMA_SetCurrDataCounter(burstDmaTimer->dmaBurstStream, dmaBufferSize); + DMA_ITConfig(burstDmaTimer->dmaBurstStream, DMA_IT_TC, DISABLE); + tch->dmaState = TCH_DMA_CIRCULAR; + } else { + burstDmaTimer->dmaBurstStream->CR &= ~DMA_SxCR_CIRC; + DMA_ITConfig(burstDmaTimer->dmaBurstStream, DMA_IT_TC, ENABLE); + tch->dmaState = TCH_DMA_IDLE; + } + + __DSB(); + + DMA_Cmd(burstDmaTimer->dmaBurstStream, ENABLE); + TIM_DMACmd(burstDmaTimer->timer, burstDmaTimer->burstRequestSource, ENABLE); + } +} #endif void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferElementCount) @@ -519,3 +566,48 @@ void impl_timerPWMStopDMA(TCH_t * tch) tch->dmaState = TCH_DMA_IDLE; TIM_Cmd(tch->timHw->tim, ENABLE); } + +void impl_timerPWMSetDMACircular(TCH_t * tch, bool circular, uint32_t dmaBufferSize) +{ + if (!tch->dma || !tch->dma->ref) { + return; + } + + ATOMIC_BLOCK(NVIC_PRIO_MAX) { + // Stop new transfer triggers before reconfiguring + TIM_DMACmd(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex], DISABLE); + DMA_Cmd(tch->dma->ref, DISABLE); + + // STM32F4/F7 RM: poll EN bit until stream is actually disabled + uint32_t timeout = 10000; // ~60us at 168MHz, well above worst-case disable latency + while ((tch->dma->ref->CR & DMA_SxCR_EN) && timeout--) { + __NOP(); + } + + if (tch->dma->ref->CR & DMA_SxCR_EN) { + TIM_DMACmd(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex], ENABLE); + return; + } + + DMA_CLEAR_FLAG(tch->dma, DMA_IT_TCIF); + + if (circular) { + tch->dma->ref->CR |= DMA_SxCR_CIRC; + DMA_SetCurrDataCounter(tch->dma->ref, dmaBufferSize); + // Disable TC interrupt — in circular mode, TC fires every cycle + // and the IRQ handler would otherwise disable the stream + DMA_ITConfig(tch->dma->ref, DMA_IT_TC, DISABLE); + tch->dmaState = TCH_DMA_CIRCULAR; + } else { + tch->dma->ref->CR &= ~DMA_SxCR_CIRC; + DMA_ITConfig(tch->dma->ref, DMA_IT_TC, ENABLE); + tch->dmaState = TCH_DMA_IDLE; + } + + // Ensure register writes are visible to DMA before re-enabling + __DSB(); + + DMA_Cmd(tch->dma->ref, ENABLE); + TIM_DMACmd(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex], ENABLE); + } +} diff --git a/src/main/drivers/timer_impl_stdperiph_at32.c b/src/main/drivers/timer_impl_stdperiph_at32.c index 0cc194897d9..782943ab25d 100644 --- a/src/main/drivers/timer_impl_stdperiph_at32.c +++ b/src/main/drivers/timer_impl_stdperiph_at32.c @@ -269,6 +269,13 @@ static void impl_timerDMA_IRQHandler(DMA_t descriptor) { if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) { TCH_t * tch = (TCH_t *)descriptor->userParam; + + // In circular mode, let DMA keep running - don't disable the channel + if (tch->dmaState == TCH_DMA_CIRCULAR) { + DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF); + return; + } + tch->dmaState = TCH_DMA_IDLE; dma_channel_enable(tch->dma->ref,FALSE); tmr_dma_request_enable(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex], FALSE); @@ -407,3 +414,48 @@ void impl_timerPWMStopDMA(TCH_t * tch) tch->dmaState = TCH_DMA_IDLE; tmr_counter_enable(tch->timHw->tim, TRUE); } + +void impl_timerPWMSetDMACircular(TCH_t * tch, bool circular, uint32_t dmaBufferSize) +{ + if (!tch->dma || !tch->dma->ref) { + return; + } + + ATOMIC_BLOCK(NVIC_PRIO_MAX) { + // Stop new transfer triggers before reconfiguring + tmr_dma_request_enable(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex], FALSE); + dma_channel_enable(tch->dma->ref, FALSE); + + // AT32: poll enable bit until channel is actually disabled + uint32_t timeout = 10000; // ~40us at 288MHz, well above worst-case disable latency + while (tch->dma->ref->ctrl_bit.chen && timeout--) { + __NOP(); + } + + if (tch->dma->ref->ctrl_bit.chen) { + tmr_dma_request_enable(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex], TRUE); + return; + } + + DMA_CLEAR_FLAG(tch->dma, DMA_IT_TCIF); + + if (circular) { + tch->dma->ref->ctrl_bit.lm = TRUE; + dma_data_number_set(tch->dma->ref, dmaBufferSize); + // Disable TC interrupt — in circular mode, TC fires every cycle + // and the IRQ handler would otherwise disable the channel + dma_interrupt_enable(tch->dma->ref, DMA_IT_TCIF, FALSE); + tch->dmaState = TCH_DMA_CIRCULAR; + } else { + tch->dma->ref->ctrl_bit.lm = FALSE; + dma_interrupt_enable(tch->dma->ref, DMA_IT_TCIF, TRUE); + tch->dmaState = TCH_DMA_IDLE; + } + + // Ensure register writes are visible to DMA before re-enabling + __DSB(); + + dma_channel_enable(tch->dma->ref, TRUE); + tmr_dma_request_enable(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex], TRUE); + } +} diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 3e8165dcf32..0d6e75627a1 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -230,7 +230,7 @@ static const char *debugModeNames[DEBUG_COUNT] = { // sync with gyroSensor_e static const char *const gyroNames[] = { "NONE", "AUTO", "MPU6000", "MPU6500", "MPU9250", "BMI160", - "ICM20689", "BMI088", "ICM42605", "BMI270", "LSM6DXX", "FAKE"}; + "ICM20689", "BMI088", "ICM42605", "BMI270", "LSM6DXX", "ICM45686", "FAKE"}; // sync this with sensors_e static const char * const sensorTypeNames[] = { @@ -4142,13 +4142,14 @@ static void cliStatus(char *cmdline) #endif // for if at32 #endif // for SITL - cliPrintLinef("Sensor status: GYRO=%s, ACC=%s, MAG=%s, BARO=%s, RANGEFINDER=%s, OPFLOW=%s, GPS=%s", + cliPrintLinef("Sensor status: GYRO=%s, ACC=%s, MAG=%s, BARO=%s, RANGEFINDER=%s, OPFLOW=%s, PITOT=%s, GPS=%s", hardwareSensorStatusNames[getHwGyroStatus()], hardwareSensorStatusNames[getHwAccelerometerStatus()], hardwareSensorStatusNames[getHwCompassStatus()], hardwareSensorStatusNames[getHwBarometerStatus()], hardwareSensorStatusNames[getHwRangefinderStatus()], hardwareSensorStatusNames[getHwOpticalFlowStatus()], + hardwareSensorStatusNames[getHwPitotmeterStatus()], hardwareSensorStatusNames[getHwGPSStatus()] ); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 11bbd6cf3cc..3223aca497e 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -177,7 +177,7 @@ bool areSensorsCalibrating(void) return false; } -int16_t getAxisRcCommand(int16_t rawData, int16_t rate, int16_t deadband) +int16_t FAST_CODE getAxisRcCommand(int16_t rawData, int16_t rate, int16_t deadband) { int16_t stickDeflection = 0; @@ -390,10 +390,25 @@ static void processPilotAndFailSafeActions(float dT) failsafeApplyControlInput(); } else { - // Compute ROLL PITCH and YAW command - rcCommand[ROLL] = getAxisRcCommand(rxGetChannelValue(ROLL), FLIGHT_MODE(MANUAL_MODE) ? currentControlProfile->manual.rcExpo8 : currentControlProfile->stabilized.rcExpo8, rcControlsConfig()->deadband); - rcCommand[PITCH] = getAxisRcCommand(rxGetChannelValue(PITCH), FLIGHT_MODE(MANUAL_MODE) ? currentControlProfile->manual.rcExpo8 : currentControlProfile->stabilized.rcExpo8, rcControlsConfig()->deadband); - rcCommand[YAW] = -getAxisRcCommand(rxGetChannelValue(YAW), FLIGHT_MODE(MANUAL_MODE) ? currentControlProfile->manual.rcYawExpo8 : currentControlProfile->stabilized.rcYawExpo8, rcControlsConfig()->yaw_deadband); + // Compute ROLL PITCH and YAW command. + // Only recompute when the RX task has delivered new data (~50 Hz). + { + static int16_t cachedCmd[3] = {0, 0, 0}; + if (isRXDataNew) { + cachedCmd[ROLL] = getAxisRcCommand(rxGetChannelValue(ROLL), + FLIGHT_MODE(MANUAL_MODE) ? currentControlProfile->manual.rcExpo8 : currentControlProfile->stabilized.rcExpo8, + rcControlsConfig()->deadband); + cachedCmd[PITCH] = getAxisRcCommand(rxGetChannelValue(PITCH), + FLIGHT_MODE(MANUAL_MODE) ? currentControlProfile->manual.rcExpo8 : currentControlProfile->stabilized.rcExpo8, + rcControlsConfig()->deadband); + cachedCmd[YAW] = -getAxisRcCommand(rxGetChannelValue(YAW), + FLIGHT_MODE(MANUAL_MODE) ? currentControlProfile->manual.rcYawExpo8 : currentControlProfile->stabilized.rcYawExpo8, + rcControlsConfig()->yaw_deadband); + } + rcCommand[ROLL] = cachedCmd[ROLL]; + rcCommand[PITCH] = cachedCmd[PITCH]; + rcCommand[YAW] = cachedCmd[YAW]; + } // Apply manual control rates if (FLIGHT_MODE(MANUAL_MODE)) { @@ -418,8 +433,10 @@ static void processPilotAndFailSafeActions(float dT) //Compute THROTTLE command rcCommand[THROTTLE] = throttleStickMixedValue(); - // Signal updated rcCommand values to Failsafe system - failsafeUpdateRcCommandValues(); + // Signal updated rcCommand values to Failsafe system when new RC data arrived + if (isRXDataNew) { + failsafeUpdateRcCommandValues(); + } if (FLIGHT_MODE(HEADFREE_MODE)) { const float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index cf5308067e9..03a24d8f5c4 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -742,6 +742,12 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU16(dst, getRSSI()); break; + case MSP2_INAV_GET_LINK_STATS: + sbufWriteU8(dst, (uint8_t)-rxLinkStatistics.uplinkRSSI); + sbufWriteU8(dst, rxLinkStatistics.uplinkLQ); + sbufWriteU8(dst, (uint8_t)rxLinkStatistics.uplinkSNR); + break; + case MSP_LOOP_TIME: sbufWriteU16(dst, gyroConfig()->looptime); break; @@ -1009,7 +1015,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU16(dst, gpsSol.hdop); sbufWriteU16(dst, gpsSol.eph); sbufWriteU16(dst, gpsSol.epv); - sbufWriteU32(dst, gpsState.hwVersion); + sbufWriteU8(dst, gpsState.hwVersion); break; #endif case MSP2_ADSB_VEHICLE_LIST: @@ -2412,6 +2418,111 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) } break; #endif + + case MSP2_INAV_SET_AUX_RC: + { + // Max valid payload: 1 def byte + 24 channels × 2 bytes (16-bit) = 49 bytes + if (dataSize < 2 || dataSize > 49) { + return MSP_RESULT_ERROR; + } + + const uint8_t defByte = sbufReadU8(src); + const uint8_t startChannel = defByte >> 3; // Bits 7-3: start channel index (0-31) + const uint8_t resolutionMode = defByte & 0x07; // Bits 2-0: resolution + + // Safety: CH1-CH12 (index 0-11) are protected + if (startChannel < 12) { + return MSP_RESULT_ERROR; + } + + const uint8_t dataBytes = dataSize - 1; + uint8_t channelCount; + uint8_t bitsPerChannel; + + switch (resolutionMode) { + case 0: // 2-bit + bitsPerChannel = 2; + channelCount = dataBytes * 4; + break; + case 1: // 4-bit + bitsPerChannel = 4; + channelCount = dataBytes * 2; + break; + case 2: // 8-bit + bitsPerChannel = 8; + channelCount = dataBytes; + break; + case 3: // 16-bit + bitsPerChannel = 16; + if (dataBytes % 2 != 0) { + return MSP_RESULT_ERROR; + } + channelCount = dataBytes / 2; + break; + default: + return MSP_RESULT_ERROR; + } + + if (channelCount == 0 || startChannel + channelCount > 32) { + return MSP_RESULT_ERROR; + } + + // Decode and apply channel values + if (bitsPerChannel >= 8) { + // Byte-aligned modes: 8-bit and 16-bit + for (int i = 0; i < channelCount; i++) { + uint16_t rawValue; + if (bitsPerChannel == 16) { + rawValue = sbufReadU16(src); + } else { + rawValue = sbufReadU8(src); + } + + if (rawValue == 0) { + continue; // skip: no update + } + + uint16_t pwmValue; + if (bitsPerChannel == 16) { + pwmValue = constrain(rawValue, 750, 2250); + } else { + // 8-bit: 1-255 → 1000-2000 + pwmValue = 1000 + ((uint32_t)(rawValue - 1) * 1000) / 254; + } + + rxMspAuxOverlaySet(startChannel + i, pwmValue); + } + } else { + // Sub-byte modes: 2-bit and 4-bit + const uint8_t mask = (1 << bitsPerChannel) - 1; + const uint8_t channelsPerByte = 8 / bitsPerChannel; + int ch = 0; + + for (int byteIdx = 0; byteIdx < (int)dataBytes && ch < channelCount; byteIdx++) { + const uint8_t dataByte = sbufReadU8(src); + for (int sub = channelsPerByte - 1; sub >= 0 && ch < channelCount; sub--, ch++) { + const uint8_t rawValue = (dataByte >> (sub * bitsPerChannel)) & mask; + + if (rawValue == 0) { + continue; // skip: no update + } + + uint16_t pwmValue; + if (bitsPerChannel == 2) { + // 2-bit: 1→1000, 2→1500, 3→2000 + pwmValue = 1000 + (rawValue - 1) * 500; + } else { + // 4-bit: 1-15 → 1000-2000 + pwmValue = 1000 + ((uint32_t)(rawValue - 1) * 1000) / 14; + } + + rxMspAuxOverlaySet(startChannel + ch, pwmValue); + } + } + } + } + break; + case MSP2_COMMON_SET_MOTOR_MIXER: sbufReadU8Safe(&tmp_u8, src); if ((dataSize == 9) && (tmp_u8 < MAX_SUPPORTED_MOTORS)) { @@ -3684,6 +3795,28 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) } break; + case MSP2_INAV_SET_CRUISE_HEADING: + // Set heading while Cruise / Course Hold is active. + // Payload: I32 heading_centidegrees (0–35999) + if (dataSize == 4) { + int32_t headingCd; + if (sbufReadI32Safe(&headingCd, src) && navSetCruiseHeading(headingCd)) { + break; + } + } + return MSP_RESULT_ERROR; + + case MSP2_INAV_SET_WP_INDEX: + // Jump to waypoint N during an active WP mission. + // Payload: U8 wp_index (0-based, relative to mission start waypoint) + if (dataSize == 1) { + uint8_t wpIndex; + if (sbufReadU8Safe(&wpIndex, src) && navSetActiveWaypointIndex(wpIndex)) { + break; + } + } + return MSP_RESULT_ERROR; + default: return MSP_RESULT_ERROR; } @@ -4486,6 +4619,7 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro sbuf_t *dst = &reply->buf; sbuf_t *src = &cmd->buf; const uint16_t cmdMSP = cmd->cmd; + // initialize reply by default reply->cmd = cmd->cmd; diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index a20a23e0b24..65654ccd97b 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -190,9 +190,7 @@ void initActiveBoxIds(void) RESET_BOX_ID_COUNT; ADD_ACTIVE_BOX(BOXARM); ADD_ACTIVE_BOX(BOXPREARM); -#ifdef USE_MULTI_FUNCTIONS ADD_ACTIVE_BOX(BOXMULTIFUNCTION); -#endif if (sensors(SENSOR_ACC) && STATE(ALTITUDE_CONTROL)) { ADD_ACTIVE_BOX(BOXANGLE); diff --git a/src/main/fc/multifunction.c b/src/main/fc/multifunction.c index 2ac3a558278..50cf3af1aea 100644 --- a/src/main/fc/multifunction.c +++ b/src/main/fc/multifunction.c @@ -26,19 +26,31 @@ #include "build/debug.h" #include "drivers/time.h" -#ifdef USE_MULTI_FUNCTIONS - +#include "fc/config.h" #include "fc/fc_core.h" #include "fc/multifunction.h" #include "fc/rc_modes.h" -#include "fc/runtime_config.h" + +#include "flight/imu.h" #include "io/osd.h" + #include "navigation/navigation.h" +#include "navigation/navigation_private.h" + +#include "sensors/acceleration.h" +#include "sensors/battery.h" +#include "sensors/compass.h" +#include "sensors/diagnostics.h" +#include "sensors/pitotmeter.h" +#include "sensors/sensors.h" + +textAttributes_t osdGetMultiFunctionMessage(char *buff); +multiFunctionWarning_t multiFunctionWarning; +#ifdef USE_MULTI_FUNCTIONS multi_function_e selectedItem = MULTI_FUNC_NONE; uint8_t multiFunctionFlags; -bool nextItemIsAvailable = false; static void multiFunctionApply(multi_function_e selectedItem) { @@ -50,80 +62,286 @@ static void multiFunctionApply(multi_function_e selectedItem) break; case MULTI_FUNC_2: // toggle Safehome suspend #if defined(USE_SAFE_HOME) - if (navConfig()->general.flags.safehome_usage_mode != SAFEHOME_USAGE_OFF) { - MULTI_FUNC_FLAG(MF_SUSPEND_SAFEHOMES) ? MULTI_FUNC_FLAG_DISABLE(MF_SUSPEND_SAFEHOMES) : MULTI_FUNC_FLAG_ENABLE(MF_SUSPEND_SAFEHOMES); - } + MULTI_FUNC_FLAG(MF_SUSPEND_SAFEHOMES) ? MULTI_FUNC_FLAG_DISABLE(MF_SUSPEND_SAFEHOMES) : MULTI_FUNC_FLAG_ENABLE(MF_SUSPEND_SAFEHOMES); #endif break; case MULTI_FUNC_3: // toggle RTH Trackback suspend - if (navConfig()->general.flags.rth_trackback_mode != RTH_TRACKBACK_OFF) { - MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK) ? MULTI_FUNC_FLAG_DISABLE(MF_SUSPEND_TRACKBACK) : MULTI_FUNC_FLAG_ENABLE(MF_SUSPEND_TRACKBACK); - } + MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK) ? MULTI_FUNC_FLAG_DISABLE(MF_SUSPEND_TRACKBACK) : MULTI_FUNC_FLAG_ENABLE(MF_SUSPEND_TRACKBACK); break; - case MULTI_FUNC_4: + case MULTI_FUNC_4: // toggle Turtle mode #ifdef USE_DSHOT - if (STATE(MULTIROTOR)) { // toggle Turtle mode - MULTI_FUNC_FLAG(MF_TURTLE_MODE) ? MULTI_FUNC_FLAG_DISABLE(MF_TURTLE_MODE) : MULTI_FUNC_FLAG_ENABLE(MF_TURTLE_MODE); - } + MULTI_FUNC_FLAG(MF_TURTLE_MODE) ? MULTI_FUNC_FLAG_DISABLE(MF_TURTLE_MODE) : MULTI_FUNC_FLAG_ENABLE(MF_TURTLE_MODE); #endif break; case MULTI_FUNC_5: // emergency ARM - if (!ARMING_FLAG(ARMED)) { - emergencyArmingUpdate(true, true); - } + emergencyArmingUpdate(true, true); + break; + case MULTI_FUNC_6: // Calibrate compass/Zero yaw heading +#if defined(USE_GPS) || defined(USE_MAG) + ENABLE_STATE(CALIBRATE_MAG); +#endif + break; case MULTI_FUNC_END: break; } } -bool isNextMultifunctionItemAvailable(void) -{ - return nextItemIsAvailable; -} - void setMultifunctionSelection(multi_function_e item) { selectedItem = item == MULTI_FUNC_END ? MULTI_FUNC_1 : item; - nextItemIsAvailable = false; } multi_function_e multiFunctionSelection(void) { - static timeMs_t startTimer; - static timeMs_t selectTimer; - static bool toggle = true; + static timeMs_t functionTimer; const timeMs_t currentTime = millis(); + static uint8_t functionTracker = 0; if (IS_RC_MODE_ACTIVE(BOXMULTIFUNCTION)) { - if (selectTimer) { - if (currentTime - selectTimer > 3000) { // 3s selection duration to activate selected function + if (!functionTimer) { // initiate function on first BOXMULTIFUNCTION activation + functionTimer = currentTime; + selectedItem = MULTI_FUNC_1; + } else if (functionTracker && selectedItem != MULTI_FUNC_END) { + functionTracker = 2; + if (currentTime - functionTimer > 3000) { // 3s BOXMULTIFUNCTION activation to trigger selected function multiFunctionApply(selectedItem); - selectTimer = 0; - selectedItem = MULTI_FUNC_NONE; - nextItemIsAvailable = false; - } - } else if (toggle) { - if (selectedItem == MULTI_FUNC_NONE) { - selectedItem++; - } else { - selectTimer = currentTime; - nextItemIsAvailable = true; + selectedItem = MULTI_FUNC_END; } } - startTimer = currentTime; - toggle = false; - } else if (startTimer) { - if (!toggle && selectTimer) { - setMultifunctionSelection(++selectedItem); + } else if (functionTimer) { + if (!functionTracker) { + functionTimer = currentTime; + } else if (functionTracker == 2 || selectedItem == MULTI_FUNC_NONE) { + // cancel and reset function after second BOXMULTIFUNCTION deactivation or if no functions available + functionTimer = 0; + functionTracker = 0; + return selectedItem = MULTI_FUNC_NONE; } - if (currentTime - startTimer > 4000) { // 4s reset delay after mode deselected - startTimer = 0; - selectedItem = MULTI_FUNC_NONE; + + if (currentTime - functionTimer > 1500) { // display available functions on 1.5s rolling cycle + setMultifunctionSelection(++selectedItem); + functionTimer = currentTime; } - selectTimer = 0; - toggle = true; + + functionTracker = 1; } return selectedItem; } +#endif // multifunction + +static bool osdCheckWarning(bool condition, uint8_t warningFlag) +{ + static timeMs_t newWarningEndTime = 0; + static uint8_t newWarningFlags = 0; // bitfield + const timeMs_t currentTimeMs = millis(); + + /* New warnings dislayed individually for 10s with blinking after which + * all current warnings displayed without blinking on 1 second cycle */ + if (condition) { // condition required to trigger warning + if (!(multiFunctionWarning.osdWarningsFlags & warningFlag)) { // check for new warnings + multiFunctionWarning.osdWarningsFlags |= warningFlag; + newWarningFlags |= warningFlag; + newWarningEndTime = currentTimeMs + 10000; + multiFunctionWarning.newWarningActive = true; + } +#ifdef USE_DEV_TOOLS + if (systemConfig()->groundTestMode) { + return true; + } +#endif + if (currentTimeMs < newWarningEndTime) { + return (newWarningFlags & warningFlag); // filter out new warnings excluding older warnings + } else { + newWarningFlags = 0; + multiFunctionWarning.newWarningActive = false; + } + return true; + } else if (multiFunctionWarning.osdWarningsFlags & warningFlag) { + multiFunctionWarning.osdWarningsFlags &= ~warningFlag; + } + + return false; +} + +textAttributes_t osdGetMultiFunctionMessage(char *buff) +{ + /* Message length limit 10 char max */ + + textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE; + const char *message = NULL; + +#ifdef USE_MULTI_FUNCTIONS + /* --- FUNCTIONS --- */ + multi_function_e selectedFunction = multiFunctionSelection(); + + if (selectedFunction) { + multi_function_e activeFunction = selectedFunction; + + switch (selectedFunction) { + case MULTI_FUNC_NONE: + case MULTI_FUNC_1: + if (ARMING_FLAG(ARMED)) { + message = posControl.flags.manualEmergLandActive ? "ABORT LAND" : "EMERG LAND"; + break; + } + activeFunction++; + FALLTHROUGH; + case MULTI_FUNC_2: +#if defined(USE_SAFE_HOME) + if (navConfig()->general.flags.safehome_usage_mode != SAFEHOME_USAGE_OFF) { + message = MULTI_FUNC_FLAG(MF_SUSPEND_SAFEHOMES) ? "USE SFHOME" : "SUS SFHOME"; + break; + } +#endif + activeFunction++; + FALLTHROUGH; + case MULTI_FUNC_3: + if (navConfig()->general.flags.rth_trackback_mode != RTH_TRACKBACK_OFF) { + message = MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK) ? "USE TKBACK" : "SUS TKBACK"; + break; + } + activeFunction++; + FALLTHROUGH; + case MULTI_FUNC_4: +#ifdef USE_DSHOT + if (!ARMING_FLAG(ARMED) && STATE(MULTIROTOR)) { + message = MULTI_FUNC_FLAG(MF_TURTLE_MODE) ? "END TURTLE" : "USE TURTLE"; + break; + } +#endif + activeFunction++; + FALLTHROUGH; + case MULTI_FUNC_5: + if (!ARMING_FLAG(ARMED)) { + message = "EMERG ARM "; + break; + } + activeFunction++; + FALLTHROUGH; + case MULTI_FUNC_6: + if (!ARMING_FLAG(ARMED)) { +#if defined(USE_MAG) + if (sensors(SENSOR_MAG)) { + message = "CAL COMPAS"; + break; + } +#endif +#if defined(USE_GPS) + if (isYawZeroResetAllowed()) { + message = "SET HEADIN"; + break; + } #endif + } + activeFunction++; + break; + case MULTI_FUNC_END: + message = "*FUNC SET*"; + break; + } + + if (activeFunction != selectedFunction) { + if (selectedFunction == MULTI_FUNC_1 && activeFunction == MULTI_FUNC_END) { // no functions available so end process + message = "*NO FUNCS*"; + setMultifunctionSelection(MULTI_FUNC_NONE); + } else { + setMultifunctionSelection(activeFunction); + if (activeFunction == MULTI_FUNC_END) { // no messages to display so return + return elemAttr; + } + } + } + + strcpy(buff, message); + + return elemAttr; + } +#endif // MULTIFUNCTION - functions only, warnings always defined + + /* --- WARNINGS --- */ + const char *messages[8]; + uint8_t messageCount = 0; + bool warningCondition = false; + uint8_t warningFlagID = 1; + + // Low Battery Voltage + const batteryState_e batteryVoltageState = checkBatteryVoltageState(); + warningCondition = batteryVoltageState == BATTERY_CRITICAL || batteryVoltageState == BATTERY_WARNING; + if (osdCheckWarning(warningCondition, warningFlagID)) { + messages[messageCount++] = batteryVoltageState == BATTERY_CRITICAL ? "VBATT LAND" : "VBATT LOW "; + } + + // Low Battery Capacity + if (batteryUsesCapacityThresholds()) { + const batteryState_e batteryState = getBatteryState(); + warningCondition = batteryState == BATTERY_CRITICAL || batteryState == BATTERY_WARNING; + if (osdCheckWarning(warningCondition, warningFlagID <<= 1)) { + messages[messageCount++] = batteryState == BATTERY_CRITICAL ? "BATT EMPTY" : "BATT DYING"; + } + } +#if defined(USE_GPS) + // GPS Fix and Failure + if (feature(FEATURE_GPS)) { + if (osdCheckWarning(!STATE(GPS_FIX), warningFlagID <<= 1)) { + bool gpsFailed = getHwGPSStatus() == HW_SENSOR_UNAVAILABLE; + messages[messageCount++] = gpsFailed ? "GPS FAILED" : "NO GPS FIX"; + } + } + + // RTH sanity (warning if RTH heads 200m further away from home than closest point) + warningCondition = NAV_Status.state == MW_NAV_STATE_RTH_ENROUTE && !posControl.flags.rthTrackbackActive && + (posControl.homeDistance - posControl.rthSanityChecker.minimalDistanceToHome) > 20000; + if (osdCheckWarning(warningCondition, warningFlagID <<= 1)) { + messages[messageCount++] = "RTH SANITY"; + } + + // Altitude sanity (warning if significant mismatch between estimated and GPS altitude) + if (osdCheckWarning(posControl.flags.gpsCfEstimatedAltitudeMismatch, warningFlagID <<= 1)) { + messages[messageCount++] = "ALT SANITY"; + } +#endif + +#if defined(USE_MAG) + // Magnetometer failure + if (requestedSensors[SENSOR_INDEX_MAG] != MAG_NONE) { + hardwareSensorStatus_e magStatus = getHwCompassStatus(); + if (osdCheckWarning(magStatus == HW_SENSOR_UNAVAILABLE || magStatus == HW_SENSOR_UNHEALTHY, warningFlagID <<= 1)) { + messages[messageCount++] = "MAG FAILED"; + } + } +#endif + +#if defined(USE_PITOT) + // Pitot sensor validation failure (blocked/failed pitot tube) + if (sensors(SENSOR_PITOT) && detectedSensors[SENSOR_INDEX_PITOT] != PITOT_VIRTUAL) { + if (osdCheckWarning(pitotHasFailed(), warningFlagID <<= 1)) { + messages[messageCount++] = "PITOT FAIL"; + } + } +#endif + + // Vibration levels TODO - needs better vibration measurement to be useful + // const float vibrationLevel = accGetVibrationLevel(); + // warningCondition = vibrationLevel > 1.5f; + // if (osdCheckWarning(warningCondition, warningFlagID <<= 1, &warningsCount)) { + // messages[messageCount++] = vibrationLevel > 2.5f ? "BAD VIBRTN" : "VIBRATION!"; + // } + +#ifdef USE_DEV_TOOLS + if (osdCheckWarning(systemConfig()->groundTestMode, warningFlagID <<= 1)) { + messages[messageCount++] = "GRD TEST !"; + } +#endif + + if (messageCount) { + message = messages[(millis() / 1000) % messageCount]; // display each warning on 1s cycle + strcpy(buff, message); + if (multiFunctionWarning.newWarningActive) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + } + + return elemAttr; +} + diff --git a/src/main/fc/multifunction.h b/src/main/fc/multifunction.h index 6e60da2bdd8..cdbdd17ea0f 100644 --- a/src/main/fc/multifunction.h +++ b/src/main/fc/multifunction.h @@ -25,6 +25,7 @@ #pragma once #include +#include "drivers/display.h" typedef struct multiFunctionWarning_s { uint8_t osdWarningsFlags; // bitfield @@ -54,10 +55,11 @@ typedef enum { MULTI_FUNC_3, MULTI_FUNC_4, MULTI_FUNC_5, + MULTI_FUNC_6, MULTI_FUNC_END, } multi_function_e; multi_function_e multiFunctionSelection(void); -bool isNextMultifunctionItemAvailable(void); void setMultifunctionSelection(multi_function_e item); #endif +textAttributes_t osdGetMultiFunctionMessage(char *buff); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 3e353e9dfcd..6ef471e3bb0 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2,7 +2,7 @@ tables: - name: alignment values: ["DEFAULT", "CW0", "CW90", "CW180", "CW270", "CW0FLIP", "CW90FLIP", "CW180FLIP", "CW270FLIP"] - name: acc_hardware - values: ["NONE", "AUTO", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "BMI270","LSM6DXX", "FAKE"] + values: ["NONE", "AUTO", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "BMI270","LSM6DXX", "ICM45686", "FAKE"] enum: accelerationSensor_e - name: rangefinder_hardware values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C", "FAKE", "TERARANGER_EVO", "USD1_V0", "NRA"] @@ -17,7 +17,7 @@ tables: values: ["NONE", "AUTO", "BMP085", "MS5611", "BMP280", "MS5607", "LPS25H", "SPL06", "BMP388", "DPS310", "B2SMPB", "MSP", "FAKE"] enum: baroSensor_e - name: pitot_hardware - values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE", "MSP", "DLVR-L10D"] + values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE", "MSP", "DLVR-L10D", "MS5525"] enum: pitotSensor_e - name: receiver_type values: ["NONE", "SERIAL", "MSP", "SIM (SITL)"] @@ -667,7 +667,7 @@ groups: condition: USE_PITOT members: - name: pitot_hardware - description: "Selection of pitot hardware." + description: "Selection of pitot hardware. VIRTUAL only works if a GPS is enabled." default_value: "NONE" table: pitot_hardware - name: pitot_lpf_milli_hz diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index b012c040d27..379a023ffd8 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -56,6 +56,7 @@ #endif #include "io/gps.h" +#include "io/beeper.h" #include "sensors/acceleration.h" #include "sensors/barometer.h" @@ -85,7 +86,7 @@ FASTRAM fpVector3_t imuMeasuredAccelBF; FASTRAM fpVector3_t imuMeasuredRotationBF; //centrifugal force compensated using gps -FASTRAM fpVector3_t compansatedGravityBF;// cm/s/s +FASTRAM fpVector3_t compensatedGravityBF;// cm/s/s STATIC_FASTRAM float smallAngleCosZ; @@ -113,8 +114,6 @@ STATIC_FASTRAM pt1Filter_t HeadVecEFFilterY; STATIC_FASTRAM pt1Filter_t HeadVecEFFilterZ; FASTRAM fpVector3_t HeadVecEFFiltered = {.v = {0.0f, 0.0f, 0.0f}}; -STATIC_FASTRAM float GPS3DspeedFiltered=0.0f; -STATIC_FASTRAM pt1Filter_t GPS3DspeedFilter; FASTRAM bool gpsHeadingInitialized; @@ -171,6 +170,11 @@ void imuConfigure(void) imuRuntimeConfig.dcm_kp_mag = imuConfig()->dcm_kp_mag / 10000.0f; imuRuntimeConfig.dcm_ki_mag = imuConfig()->dcm_ki_mag / 10000.0f; imuRuntimeConfig.small_angle = imuConfig()->small_angle; + /* Precompute the Mahony anti-windup clamp. The PID loop reads this value + * 1000× per second; the kP gains only change when the user saves settings, + * so computing it here (called once per save) avoids one add, one multiply, + * and one divide on every PID cycle. */ + imuRuntimeConfig.dcm_i_limit = DEGREES_TO_RADIANS(2.0f) * (imuRuntimeConfig.dcm_kp_acc + imuRuntimeConfig.dcm_kp_mag) * 0.5f; } void imuInit(void) @@ -188,7 +192,7 @@ void imuInit(void) // Create magnetic declination matrix #ifdef USE_MAG const int deg = compassConfig()->mag_declination / 100; - const int min = compassConfig()->mag_declination % 100; + const int min = compassConfig()->mag_declination % 100; #else const int deg = 0; const int min = 0; @@ -210,8 +214,6 @@ void imuInit(void) pt1FilterReset(&HeadVecEFFilterX, 0); pt1FilterReset(&HeadVecEFFilterY, 0); pt1FilterReset(&HeadVecEFFilterZ, 0); - // Initialize 3d speed filter - pt1FilterReset(&GPS3DspeedFilter, 0); } void imuSetMagneticDeclination(float declinationDeg) @@ -360,7 +362,7 @@ static float imuCalculateMcCogAccWeight(void) { fpVector3_t accBFNorm; vectorScale(&accBFNorm, &imuMeasuredAccelBFFiltered, 1.0f / GRAVITY_CMSS); - float wCoGAcc = constrainf((accBFNorm.z - 1.0f)* 2, 0.0f, 1.0f); //z direction is verified via SITL + float wCoGAcc = constrainf((accBFNorm.z - 1.0f) * 2.0f, 0.0f, 1.0f); //z direction is verified via SITL wCoGAcc = wCoGAcc * imuCalculateAccelerometerWeightRateIgnore(4.0f); //reduce weight when fast angular rate, gps acc is considered lagging return wCoGAcc; } @@ -368,7 +370,19 @@ static float imuCalculateMcCogAccWeight(void) static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVector3_t * accBF, const fpVector3_t * magBF, const fpVector3_t * vCOG, const fpVector3_t * vCOGAcc, float accWScaler, float magWScaler) { STATIC_FASTRAM fpVector3_t vGyroDriftEstimate = { 0 }; - fpQuaternion_t prevOrientation = orientation; + + /* Opt 5: snapshot prevOrientation every 100 PID cycles instead of every cycle. + * The snapshot is only used by the fault-recovery path in + * imuCheckAndResetOrientationQuaternion(), which should never fire in normal + * flight. Copying 4 floats 1000×/s just to support a near-zero-probability + * reset path is wasteful; 100 ms staleness is a safe recovery point. */ + static uint8_t prevOrientationSnapshotCount = 0; + static fpQuaternion_t prevOrientation = { .q0 = 1.0f }; // identity quaternion safe default + if (++prevOrientationSnapshotCount >= 100) { + prevOrientationSnapshotCount = 0; + prevOrientation = orientation; + } + fpVector3_t vRotation = *gyroBF; /* Calculate general spin rate (rad/s) */ @@ -423,7 +437,7 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe //vForward as trust vector if (STATE(MULTIROTOR) && (!isMixerTransitionMixing)){ vForward.z = 1.0f; - }else{ + } else { vForward.x = 1.0f; } fpVector3_t vHeadingEF; @@ -434,23 +448,22 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe float airSpeed = gpsSol.groundSpeed; #if defined(USE_WIND_ESTIMATOR) // remove wind elements in vCoGlocal for better heading estimation - if (isEstimatedWindSpeedValid() && imuConfig()->gps_yaw_windcomp) - { + if (isEstimatedWindSpeedValid() && imuConfig()->gps_yaw_windcomp) { vectorScale(&vCoGlocal, &vCoGlocal, gpsSol.groundSpeed); vCoGlocal.x += getEstimatedWindSpeed(X); vCoGlocal.y -= getEstimatedWindSpeed(Y); airSpeed = fast_fsqrtf(vectorNormSquared(&vCoGlocal)); } #endif - wCoG *= scaleRangef(constrainf((airSpeed+gpsSol.groundSpeed)/2, 400, 1000), 400, 1000, 0.0f, 1.0f); + wCoG *= scaleRangef(constrainf((airSpeed+gpsSol.groundSpeed) / 2.0f, 400.0f, 1000.0f), 400.0f, 1000.0f, 0.0f, 1.0f); } else { //vCOG is not avaliable and vCOGAcc is avaliable, set the weight of vCOG to zero wCoG = 0.0f; } - if (STATE(MULTIROTOR)){ + if (STATE(MULTIROTOR)) { //when multicopter`s orientation or speed is changing rapidly. less weight on gps heading wCoG *= imuCalculateMcCogWeight(); //handle acc based vector - if(vCOGAcc){ + if (vCOGAcc) { float wCoGAcc = imuCalculateMcCogAccWeight();//stronger weight on acc if body frame z axis greate than 1G if (wCoGAcc > wCoG){ //when copter is accelerating use gps acc vector instead of gps speed vector @@ -501,11 +514,17 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe /* Step 2: Roll and pitch correction - use measured acceleration vector */ if (accBF) { - static const fpVector3_t vGravity = { .v = { 0.0f, 0.0f, 1.0f } }; fpVector3_t vEstGravity, vAcc, vErr; - // Calculate estimated gravity vector in body frame - quaternionRotateVector(&vEstGravity, &vGravity, &orientation); // EF -> BF + /* Opt 1: imuComputeRotationMatrix() is called at the END of every + * imuMahonyAHRSupdate() and keeps rMat in sync with orientation. + * Rotating the constant unit-gravity vector {0,0,1} from EF to BF by + * the current orientation quaternion yields exactly the third row of rMat + * (rMat[2][0..2]). Reading those three floats replaces a quaternionRotateVector() + * call that costs 2× quaternionMultiply = ~32 multiplies + 24 adds. */ + vEstGravity.x = rMat[2][0]; + vEstGravity.y = rMat[2][1]; + vEstGravity.z = rMat[2][2]; // Error is sum of cross product between estimated direction and measured direction of gravity vectorNormalize(&vAcc, accBF); @@ -528,7 +547,9 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe vectorAdd(&vRotation, &vRotation, &vErr); } // Anti wind-up - float i_limit = DEGREES_TO_RADIANS(2.0f) * (imuRuntimeConfig.dcm_kp_acc + imuRuntimeConfig.dcm_kp_mag) / 2.0f; + /* Opt 4: dcm_i_limit is computed once in imuConfigure() (called on settings save), + * not recomputed each PID cycle. The kP gains do not change at runtime. */ + const float i_limit = imuRuntimeConfig.dcm_i_limit; vGyroDriftEstimate.x = constrainf(vGyroDriftEstimate.x, -i_limit, i_limit); vGyroDriftEstimate.y = constrainf(vGyroDriftEstimate.y, -i_limit, i_limit); vGyroDriftEstimate.z = constrainf(vGyroDriftEstimate.z, -i_limit, i_limit); @@ -551,7 +572,10 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe // Proper quaternion from axis/angle involves computing sin/cos, but the formula becomes numerically unstable as Theta approaches zero. // For near-zero cases we use the first 3 terms of the Taylor series expansion for sin/cos. We check if fourth term is less than machine precision - // then we can safely use the "low angle" approximated version without loss of accuracy. - if (thetaMagnitudeSq < fast_fsqrtf(24.0f * 1e-6f)) { + /* Opt 2: original condition was "thetaMagnitudeSq < sqrt(24e-6)". + * Squaring both sides (both are non-negative) gives an equivalent + * condition without a sqrt() call: thetaMagnitudeSq² < 24e-6. */ + if (thetaMagnitudeSq * thetaMagnitudeSq < 24.0e-6f) { quaternionScale(&deltaQ, &deltaQ, 1.0f - thetaMagnitudeSq / 6.0f); deltaQ.q0 = 1.0f - thetaMagnitudeSq / 2.0f; } @@ -563,7 +587,20 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe // Calculate final orientation and renormalize quaternionMultiply(&orientation, &orientation, &deltaQ); - quaternionNormalize(&orientation, &orientation); + /* Opt 3: first-order Newton renormalization avoids sqrt() and 4 divides. + * At 1 kHz the quaternion norm drifts by < 1e-6 per step, so normSq = 1 + ε + * with |ε| ≪ 1. The identity 1/sqrt(x) ≈ (3-x)/2 is accurate to O(ε²) ≈ 1e-12 + * — well within float precision. imuCheckAndResetOrientationQuaternion() below + * catches any catastrophic norm deviation that this approximation cannot correct. */ + { + const float normSq = orientation.q0 * orientation.q0 + orientation.q1 * orientation.q1 + + orientation.q2 * orientation.q2 + orientation.q3 * orientation.q3; + const float scale = (3.0f - normSq) * 0.5f; + orientation.q0 *= scale; + orientation.q1 *= scale; + orientation.q2 *= scale; + orientation.q3 *= scale; + } } // Check for invalid quaternion and reset to previous known good one @@ -589,8 +626,7 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void) attitude.values.yaw = RADIANS_TO_DECIDEGREES(-atan2_approx(rMat[1][0], rMat[0][0])); } - if (attitude.values.yaw < 0) - attitude.values.yaw += 3600; + if (attitude.values.yaw < 0) attitude.values.yaw += 3600; /* Update small angle state */ if (calculateCosTiltAngle() > smallAngleCosZ) { @@ -629,23 +665,17 @@ static float imuCalculateAccelerometerWeightRateIgnore(const float acc_ignore_sl // Default - don't apply rate/ignore scaling float accWeight_RateIgnore = 1.0f; - if (ARMING_FLAG(ARMED) && imuConfig()->acc_ignore_rate) - { + if (ARMING_FLAG(ARMED) && imuConfig()->acc_ignore_rate) { float rotRateMagnitude = fast_fsqrtf(vectorNormSquared(&imuMeasuredRotationBFFiltered)); rotRateMagnitude = rotRateMagnitude / (acc_ignore_slope_multipiler + 0.001f); - if (imuConfig()->acc_ignore_slope) - { + + if (imuConfig()->acc_ignore_slope) { const float rateSlopeMin = DEGREES_TO_RADIANS((imuConfig()->acc_ignore_rate - imuConfig()->acc_ignore_slope)); const float rateSlopeMax = DEGREES_TO_RADIANS((imuConfig()->acc_ignore_rate + imuConfig()->acc_ignore_slope)); accWeight_RateIgnore = scaleRangef(constrainf(rotRateMagnitude, rateSlopeMin, rateSlopeMax), rateSlopeMin, rateSlopeMax, 1.0f, 0.0f); - } - else - { - if (rotRateMagnitude > DEGREES_TO_RADIANS(imuConfig()->acc_ignore_rate)) - { - accWeight_RateIgnore = 0.0f; - } + } else if (rotRateMagnitude > DEGREES_TO_RADIANS(imuConfig()->acc_ignore_rate)) { + accWeight_RateIgnore = 0.0f; } } @@ -658,7 +688,7 @@ static void imuCalculateFilters(float dT) imuMeasuredRotationBFFiltered.x = pt1FilterApply4(&rotRateFilterX, imuMeasuredRotationBF.x, IMU_ROTATION_LPF, dT); imuMeasuredRotationBFFiltered.y = pt1FilterApply4(&rotRateFilterY, imuMeasuredRotationBF.y, IMU_ROTATION_LPF, dT); imuMeasuredRotationBFFiltered.z = pt1FilterApply4(&rotRateFilterZ, imuMeasuredRotationBF.z, IMU_ROTATION_LPF, dT); - + imuMeasuredAccelBFFiltered.x = pt1FilterApply4(&accelFilterX, imuMeasuredAccelBF.x, IMU_ROTATION_LPF, dT); imuMeasuredAccelBFFiltered.y = pt1FilterApply4(&accelFilterY, imuMeasuredAccelBF.y, IMU_ROTATION_LPF, dT); imuMeasuredAccelBFFiltered.z = pt1FilterApply4(&accelFilterZ, imuMeasuredAccelBF.z, IMU_ROTATION_LPF, dT); @@ -667,9 +697,6 @@ static void imuCalculateFilters(float dT) HeadVecEFFiltered.y = pt1FilterApply4(&HeadVecEFFilterY, rMat[1][0], IMU_ROTATION_LPF, dT); HeadVecEFFiltered.z = pt1FilterApply4(&HeadVecEFFilterZ, rMat[2][0], IMU_ROTATION_LPF, dT); - //anti aliasing - float GPS3Dspeed = calc_length_pythagorean_3D(gpsSol.velNED[X],gpsSol.velNED[Y],gpsSol.velNED[Z]); - GPS3DspeedFiltered = pt1FilterApply4(&GPS3DspeedFilter, GPS3Dspeed, IMU_ROTATION_LPF, dT); } static void imuCalculateGPSacceleration(fpVector3_t *vEstAccelEF,fpVector3_t *vEstcentrifugalAccelBF, float *acc_ignore_slope_multipiler) @@ -683,8 +710,7 @@ static void imuCalculateGPSacceleration(fpVector3_t *vEstAccelEF,fpVector3_t *vE // on first gps data acquired, time_delta_ms will be large, vEstcentrifugalAccelBF will be minimal to disable the compensation rtcTime_t time_delta_ms = currenttime - lastGPSNewDataTime; - if (lastGPSHeartbeat != gpsSol.flags.gpsHeartbeat && time_delta_ms > 0) - { + if (lastGPSHeartbeat != gpsSol.flags.gpsHeartbeat && time_delta_ms > 0) { // on new gps frame, update accEF and estimate centrifugal accleration vEstAccelEF->x = -(currentGPSvel.x - lastGPSvel.x) / (MS2S(time_delta_ms)); // the x axis of accerometer is pointing backward vEstAccelEF->y = (currentGPSvel.y - lastGPSvel.y) / (MS2S(time_delta_ms)); @@ -699,18 +725,25 @@ static void imuCalculateGPSacceleration(fpVector3_t *vEstAccelEF,fpVector3_t *vE } static void imuCalculateTurnRateacceleration(fpVector3_t *vEstcentrifugalAccelBF, float dT, float *acc_ignore_slope_multipiler) -{ +{ //fixed wing only static float lastspeed = -1.0f; float currentspeed = 0; - if (isGPSTrustworthy()){ + if (isGPSTrustworthy()) { //first speed choice is gps + static bool lastGPSHeartbeat; + static pt1Filter_t GPS3DspeedFilter; + static float GPS3DspeedFiltered = 0.0f; + if (gpsSol.flags.gpsHeartbeat != lastGPSHeartbeat) { + lastGPSHeartbeat = gpsSol.flags.gpsHeartbeat; + float GPS3Dspeed = calc_length_pythagorean_3D(gpsSol.velNED[X], gpsSol.velNED[Y], gpsSol.velNED[Z]); + GPS3DspeedFiltered = pt1FilterApply4(&GPS3DspeedFilter, GPS3Dspeed, IMU_ROTATION_LPF, dT); + } currentspeed = GPS3DspeedFiltered; *acc_ignore_slope_multipiler = 4.0f; } #ifdef USE_PITOT - else if (sensors(SENSOR_PITOT) && pitotIsHealthy()) - { + else if (sensors(SENSOR_PITOT) && pitotIsHealthy()) { // second choice is pitot currentspeed = getAirspeedEstimate(); *acc_ignore_slope_multipiler = 2.0f; @@ -733,7 +766,7 @@ fpQuaternion_t* getTailSitterQuaternion(bool normal2tail){ static bool firstRun = true; static fpQuaternion_t qNormal2Tail; static fpQuaternion_t qTail2Normal; - if(firstRun){ + if (firstRun) { fpAxisAngle_t axisAngle; axisAngle.axis.x = 0; axisAngle.axis.y = 1; @@ -749,7 +782,7 @@ fpQuaternion_t* getTailSitterQuaternion(bool normal2tail){ void imuUpdateTailSitter(void) { static bool lastTailSitter=false; - if (((bool)STATE(TAILSITTER)) != lastTailSitter){ + if (((bool)STATE(TAILSITTER)) != lastTailSitter) { fpQuaternion_t* rotation_for_tailsitter= getTailSitterQuaternion(STATE(TAILSITTER)); quaternionMultiply(&orientation, &orientation, rotation_for_tailsitter); } @@ -799,65 +832,76 @@ static void imuCalculateEstimatedAttitude(float dT) resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw)); } } else if (!ARMING_FLAG(ARMED)) { - gpsHeadingInitialized = false; + if (STATE(AIRPLANE)) { + gpsHeadingInitialized = false; // required for fixed wing flight detection to work correctly + } else if (!sensors(SENSOR_MAG) && STATE(CALIBRATE_MAG)) { + // When no compass available allow yaw to be set to 0 (North) as required using compass calibration stick command + DISABLE_STATE(CALIBRATE_MAG); + beeper(BEEPER_ACTION_SUCCESS); + + // Re-initialize quaternion from known Roll, Pitch with yaw set to 0 (North) + imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, 0); + gpsHeadingInitialized = true; + } } imuCalculateFilters(dT); + // centrifugal force compensation static fpVector3_t vEstcentrifugalAccelBF_velned; static fpVector3_t vEstcentrifugalAccelBF_turnrate; float acc_ignore_slope_multipiler = 1.0f; // when using gps centrifugal_force_compensation, AccelerometerWeightRateIgnore slope will be multiplied by this value - if (isGPSTrustworthy()) - { + + if (isGPSTrustworthy()) { imuCalculateGPSacceleration(&vCOGAcc, &vEstcentrifugalAccelBF_velned, &acc_ignore_slope_multipiler); useCOGAcc = true; //currently only for multicopter } - if (STATE(AIRPLANE)) - { + + if (STATE(AIRPLANE)) { imuCalculateTurnRateacceleration(&vEstcentrifugalAccelBF_turnrate, dT, &acc_ignore_slope_multipiler); } - if (imuConfig()->inertia_comp_method == COMPMETHOD_ADAPTIVE && isGPSTrustworthy() && STATE(AIRPLANE)) - { + + if (imuConfig()->inertia_comp_method == COMPMETHOD_ADAPTIVE && isGPSTrustworthy() && STATE(AIRPLANE)) { //pick the best centrifugal acceleration between velned and turnrate - fpVector3_t compansatedGravityBF_velned; - vectorAdd(&compansatedGravityBF_velned, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_velned); - float velned_error = fabsf(fast_fsqrtf(vectorNormSquared(&compansatedGravityBF_velned)) - GRAVITY_CMSS); + fpVector3_t compensatedGravityBF_velned; + vectorAdd(&compensatedGravityBF_velned, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_velned); + float velned_error = fabsf(fast_fsqrtf(vectorNormSquared(&compensatedGravityBF_velned)) - GRAVITY_CMSS); - fpVector3_t compansatedGravityBF_turnrate; - vectorAdd(&compansatedGravityBF_turnrate, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_turnrate); - float turnrate_error = fabsf(fast_fsqrtf(vectorNormSquared(&compansatedGravityBF_turnrate)) - GRAVITY_CMSS); + fpVector3_t compensatedGravityBF_turnrate; + vectorAdd(&compensatedGravityBF_turnrate, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_turnrate); + float turnrate_error = fabsf(fast_fsqrtf(vectorNormSquared(&compensatedGravityBF_turnrate)) - GRAVITY_CMSS); - compansatedGravityBF = velned_error > turnrate_error? compansatedGravityBF_turnrate:compansatedGravityBF_velned; + compensatedGravityBF = velned_error > turnrate_error ? compensatedGravityBF_turnrate : compensatedGravityBF_velned; } else if (((imuConfig()->inertia_comp_method == COMPMETHOD_VELNED) || (imuConfig()->inertia_comp_method == COMPMETHOD_ADAPTIVE)) && isGPSTrustworthy()) { //velned centrifugal force compensation, quad will use this method - vectorAdd(&compansatedGravityBF, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_velned); + vectorAdd(&compensatedGravityBF, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_velned); } else if (STATE(AIRPLANE)) { //turnrate centrifugal force compensation - vectorAdd(&compansatedGravityBF, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_turnrate); + vectorAdd(&compensatedGravityBF, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_turnrate); } else { - compansatedGravityBF = imuMeasuredAccelBF; + compensatedGravityBF = imuMeasuredAccelBF; } #else // In absence of GPS MAG is the only option if (canUseMAG) { useMag = true; } - compansatedGravityBF = imuMeasuredAccelBF + compensatedGravityBF = imuMeasuredAccelBF; #endif - float accWeight = imuGetPGainScaleFactor() * imuCalculateAccelerometerWeightNearness(&compansatedGravityBF); + float accWeight = imuGetPGainScaleFactor() * imuCalculateAccelerometerWeightNearness(&compensatedGravityBF); accWeight = accWeight * imuCalculateAccelerometerWeightRateIgnore(acc_ignore_slope_multipiler); const bool useAcc = (accWeight > 0.001f); const float magWeight = imuGetPGainScaleFactor() * 1.0f; fpVector3_t measuredMagBF = {.v = {mag.magADC[X], mag.magADC[Y], mag.magADC[Z]}}; imuMahonyAHRSupdate(dT, &imuMeasuredRotationBF, - useAcc ? &compansatedGravityBF : NULL, + useAcc ? &compensatedGravityBF : NULL, useMag ? &measuredMagBF : NULL, useCOG ? &vCOG : NULL, useCOGAcc ? &vCOGAcc : NULL, @@ -907,7 +951,6 @@ void imuUpdateAttitude(timeUs_t currentTimeUs) acc.accADCf[Z] = 0.0f; } } - bool isImuReady(void) { @@ -923,9 +966,13 @@ float calculateCosTiltAngle(void) { return 1.0f - 2.0f * sq(orientation.q1) - 2.0f * sq(orientation.q2); } - +#if defined(USE_GPS) +bool isYawZeroResetAllowed(void) +{ + return !ARMING_FLAG(ARMED) && !STATE(AIRPLANE) && sensors(SENSOR_GPS) && !sensors(SENSOR_MAG); +} +#endif #if defined(SITL_BUILD) || defined (USE_SIMULATOR) - void imuSetAttitudeRPY(int16_t roll, int16_t pitch, int16_t yaw) { attitude.values.roll = roll; diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 6d285d0f794..60eb964bd68 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -25,10 +25,7 @@ #include "config/parameter_group.h" extern fpVector3_t imuMeasuredAccelBF; // cm/s/s -extern fpVector3_t imuMeasuredAccelBFFiltered; // cm/s/s extern fpVector3_t imuMeasuredRotationBF; // rad/s -extern fpVector3_t imuMeasuredRotationBFFiltered; // rad/s -extern fpVector3_t compansatedGravityBF; // cm/s/s extern fpVector3_t HeadVecEFFiltered; typedef union { @@ -65,6 +62,11 @@ typedef struct imuRuntimeConfig_s { float dcm_ki_acc; float dcm_kp_mag; float dcm_ki_mag; + /* Precomputed anti-windup limit for imuMahonyAHRSupdate(): equals + * DEGREES_TO_RADIANS(2) * (dcm_kp_acc + dcm_kp_mag) / 2. + * Updated once by imuConfigure() whenever settings are saved, so the + * hot PID path reads a single float instead of doing arithmetic. */ + float dcm_i_limit; uint8_t small_angle; } imuRuntimeConfig_t; @@ -83,6 +85,7 @@ void imuUpdateAccelerometer(void); float calculateCosTiltAngle(void); bool isImuReady(void); bool isImuHeadingValid(void); +bool isYawZeroResetAllowed(void); void imuTransformVectorBodyToEarth(fpVector3_t * v); void imuTransformVectorEarthToBody(fpVector3_t * v); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 0a2faa648ca..354ffeff371 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -117,6 +117,7 @@ typedef struct { smithPredictor_t smithPredictor; fwPidAttenuation_t attenuation; + uint16_t pidSumLimit; } pidState_t; STATIC_FASTRAM bool pidFiltersConfigured = false; @@ -413,7 +414,7 @@ float getAxisIterm(uint8_t axis) return pidState[axis].errorGyroIf; } -static float pidRcCommandToAngle(int16_t stick, int16_t maxInclination) +static float FAST_CODE pidRcCommandToAngle(int16_t stick, int16_t maxInclination) { stick = constrain(stick, -500, 500); return scaleRangef((float) stick, -500.0f, 500.0f, (float) -maxInclination, (float) maxInclination); @@ -436,7 +437,7 @@ float pidRateToRcCommand(float rateDPS, uint8_t rate) return scaleRangef(rateDPS, -maxRateDPS, maxRateDPS, -500.0f, 500.0f); } -float pidRcCommandToRate(int16_t stick, uint8_t rate) +float FAST_CODE pidRcCommandToRate(int16_t stick, uint8_t rate) { const float maxRateDPS = rate * 10.0f; return scaleRangef((float) stick, -500.0f, 500.0f, -maxRateDPS, maxRateDPS); @@ -725,7 +726,7 @@ static void pidLevel(const float angleTarget, pidState_t *pidState, flight_dynam } /* Apply angular acceleration limit to rate target to limit extreme stick inputs to respect physical capabilities of the machine */ -static void pidApplySetpointRateLimiting(pidState_t *pidState, flight_dynamics_index_t axis, float dT) +static void FAST_CODE pidApplySetpointRateLimiting(pidState_t *pidState, flight_dynamics_index_t axis, float dT) { const uint32_t axisAccelLimit = (axis == FD_YAW) ? pidProfile()->axisAccelerationLimitYaw : pidProfile()->axisAccelerationLimitRollPitch; @@ -734,7 +735,7 @@ static void pidApplySetpointRateLimiting(pidState_t *pidState, flight_dynamics_i } } -static float pTermProcess(pidState_t *pidState, float rateError, float dT) { +static float FAST_CODE pTermProcess(pidState_t *pidState, float rateError, float dT) { float newPTerm = rateError * pidState->kP; return pidState->ptermFilterApplyFn(&pidState->ptermLpfState, newPTerm, yawLpfHz, dT); @@ -770,7 +771,7 @@ static float applyDBoost(pidState_t *pidState, float dT) { } #endif -static float dTermProcess(pidState_t *pidState, float currentRateTarget, float dT, float dT_inv) { +static float FAST_CODE dTermProcess(pidState_t *pidState, float currentRateTarget, float dT, float dT_inv) { // Calculate new D-term float newDTerm = 0; if (pidState->kD == 0) { @@ -787,7 +788,7 @@ static float dTermProcess(pidState_t *pidState, float currentRateTarget, float d return(newDTerm); } -static void applyItermLimiting(pidState_t *pidState) { +static void FAST_CODE applyItermLimiting(pidState_t *pidState) { if (pidState->itermLimitActive) { pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit); } else @@ -861,7 +862,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, float applyItermLimiting(pidState); - const uint16_t limit = getPidSumLimit(pidState->axis); + const uint16_t limit = pidState->pidSumLimit; if (pidProfile()->pidItermLimitPercent != 0){ float itermLimit = limit * pidProfile()->pidItermLimitPercent * 0.01f; @@ -927,7 +928,7 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid */ const float newCDTerm = rateTargetDeltaFiltered * pidState->kCD; - const uint16_t limit = getPidSumLimit(pidState->axis); + const uint16_t limit = pidState->pidSumLimit; // TODO: Get feedback from mixer on available correction range for each axis const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf + newCDTerm; @@ -1136,7 +1137,7 @@ static void pidApplyFpvCameraAngleMix(pidState_t *pidState, uint8_t fpvCameraAng pidState[YAW].rateTarget = constrainf(yawRate * cosCameraAngle + rollRate * sinCameraAngle, -GYRO_SATURATION_LIMIT, GYRO_SATURATION_LIMIT); } -void checkItermLimitingActive(pidState_t *pidState) +void FAST_CODE checkItermLimitingActive(pidState_t *pidState) { bool shouldActivate = false; @@ -1147,7 +1148,7 @@ void checkItermLimitingActive(pidState_t *pidState) pidState->itermLimitActive = STATE(ANTI_WINDUP) || shouldActivate; } -void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis) +void FAST_CODE checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis) { if (usedPidControllerType == PID_TYPE_PIFF && pidProfile()->fixedWingYawItermBankFreeze != 0 && axis == FD_YAW) { // Do not allow yaw I-term to grow when bank angle is too large @@ -1376,6 +1377,7 @@ void pidInit(void) #endif pidState[axis].axis = axis; + pidState[axis].pidSumLimit = getPidSumLimit(axis); if (axis == FD_YAW) { if (yawLpfHz) { pidState[axis].ptermFilterApplyFn = (filterApply4FnPtr) pt1FilterApply4; diff --git a/src/main/flight/smith_predictor.c b/src/main/flight/smith_predictor.c index 6a363c4f231..78151215ff5 100644 --- a/src/main/flight/smith_predictor.c +++ b/src/main/flight/smith_predictor.c @@ -34,7 +34,7 @@ #include "flight/smith_predictor.h" #include "build/debug.h" -float applySmithPredictor(uint8_t axis, smithPredictor_t *predictor, float sample) { +float FAST_CODE applySmithPredictor(uint8_t axis, smithPredictor_t *predictor, float sample) { UNUSED(axis); if (predictor->enabled) { predictor->data[predictor->idx] = sample; diff --git a/src/main/io/gps_private.h b/src/main/io/gps_private.h index e5234e70248..bad8992ce45 100755 --- a/src/main/io/gps_private.h +++ b/src/main/io/gps_private.h @@ -42,7 +42,7 @@ typedef struct { const serialConfig_t * serialConfig; serialPort_t * gpsPort; // Serial GPS only - uint32_t hwVersion; + uint8_t hwVersion; // See UBX_HW_VERSION_* in gps_ublox.h uint8_t swVersionMajor; uint8_t swVersionMinor; diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 102284723df..4fd0c332a21 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -556,7 +556,7 @@ static void gpsDecodeProtocolVersion(const char *proto, size_t bufferLength) } } -static uint32_t gpsDecodeHardwareVersion(const char * szBuf, unsigned nBufSize) +static uint8_t gpsDecodeHardwareVersion(const char * szBuf, unsigned nBufSize) { // ublox_5 hwVersion 00040005 if (strncmp(szBuf, "00040005", nBufSize) == 0) { diff --git a/src/main/io/gps_ublox.h b/src/main/io/gps_ublox.h index ab0ab930275..d54f4997be0 100644 --- a/src/main/io/gps_ublox.h +++ b/src/main/io/gps_ublox.h @@ -62,13 +62,25 @@ STATIC_ASSERT(MAX_UBLOX_PAYLOAD_SIZE >= 256, ubx_size_too_small); #define UBX_VALID_GPS_TIME(valid) (valid & 1 << 1) #define UBX_VALID_GPS_DATE_TIME(valid) (UBX_VALID_GPS_DATE(valid) && UBX_VALID_GPS_TIME(valid)) -#define UBX_HW_VERSION_UNKNOWN 0 -#define UBX_HW_VERSION_UBLOX5 500 -#define UBX_HW_VERSION_UBLOX6 600 -#define UBX_HW_VERSION_UBLOX7 700 -#define UBX_HW_VERSION_UBLOX8 800 -#define UBX_HW_VERSION_UBLOX9 900 -#define UBX_HW_VERSION_UBLOX10 1000 +/* + * hwVersion encoding (fits in uint8_t): + * bits [7:6] series: 0b00=unknown, 0b01=u-blox Neo/M series + * bits [5:0] generation within series (e.g. 8=M8, 9=M9, 10=M10) + * + * This leaves 0b10 and 0b11 available for future series (e.g. u-blox F9, + * other manufacturers). + */ +#define UBX_HW_SERIES_MASK 0xC0 +#define UBX_HW_GEN_MASK 0x3F +#define UBX_HW_SERIES_UBLOX_NM 0x40 // 0b01 << 6: u-blox Neo/M series + +#define UBX_HW_VERSION_UNKNOWN 0 +#define UBX_HW_VERSION_UBLOX5 (UBX_HW_SERIES_UBLOX_NM | 5) // 0x45 +#define UBX_HW_VERSION_UBLOX6 (UBX_HW_SERIES_UBLOX_NM | 6) // 0x46 +#define UBX_HW_VERSION_UBLOX7 (UBX_HW_SERIES_UBLOX_NM | 7) // 0x47 +#define UBX_HW_VERSION_UBLOX8 (UBX_HW_SERIES_UBLOX_NM | 8) // 0x48 +#define UBX_HW_VERSION_UBLOX9 (UBX_HW_SERIES_UBLOX_NM | 9) // 0x49 +#define UBX_HW_VERSION_UBLOX10 (UBX_HW_SERIES_UBLOX_NM | 10) // 0x4A #define UBLOX_CFG_MSGOUT_NAV_POSLLH_UART1 0x2091002a // U1 #define UBLOX_CFG_MSGOUT_NAV_SAT_UART1 0x20910016 // U1 diff --git a/src/main/io/osd.c b/src/main/io/osd.c index f4fe19a4040..55bca838f34 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -84,8 +84,6 @@ #include "fc/multifunction.h" #include "fc/rc_adjustments.h" #include "fc/rc_controls.h" -#include "fc/rc_modes.h" -#include "fc/runtime_config.h" #include "fc/settings.h" #include "flight/imu.h" @@ -107,7 +105,6 @@ #include "sensors/boardalignment.h" #include "sensors/compass.h" #include "sensors/diagnostics.h" -#include "sensors/sensors.h" #include "sensors/pitotmeter.h" #include "sensors/temperature.h" #include "sensors/esc_sensor.h" @@ -203,10 +200,6 @@ static bool fullRedraw = false; static uint8_t armState; -// Multifunction -static textAttributes_t osdGetMultiFunctionMessage(char *buff); -multiFunctionWarning_t multiFunctionWarning; - typedef struct osdMapData_s { uint32_t scale; char referenceSymbol; @@ -228,6 +221,53 @@ static bool osdDisplayHasCanvas; PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 15); PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 3); +/* OSD formatting helpers replacing common tfp_sprintf patterns + * for reduced code size and CPU overhead. */ + +static int i2a_len(int num, char *bf) +{ + i2a(num, bf); + return (int)strlen(bf); +} + +static void osdFormatIntUnit(char *buff, int width, int value, char unit) +{ + char tmp[12]; + int len = i2a_len(value, tmp); + int pad = width - len; + int i = 0; + while (pad-- > 0) buff[i++] = ' '; + const char *src = tmp; + while (*src) buff[i++] = *src++; + if (unit) buff[i++] = unit; + buff[i] = '\0'; +} + +static inline void osdWriteChar(char *buff, char c) +{ + buff[0] = c; + buff[1] = '\0'; +} + +static inline void osdWriteChar2(char *buff, char c1, char c2) +{ + buff[0] = c1; + buff[1] = c2; + buff[2] = '\0'; +} + +static void osdFormatTime_MMSS(char *buff, int m, int s) +{ + m %= 100; // clamp to two digits to prevent writing non-ASCII to display + s %= 100; + buff[0] = '0' + m / 10; + buff[1] = '0' + m % 10; + buff[2] = ':'; + buff[3] = '0' + s / 10; + buff[4] = '0' + s % 10; + buff[5] = '\0'; +} + void osdStartedSaveProcess(void) { savingSettings = true; } @@ -244,11 +284,7 @@ bool osdDisplayIsPAL(void) bool osdDisplayIsHD(void) { - if (displayScreenSize(osdDisplayPort) >= VIDEO_BUFFER_CHARS_HDZERO) - { - return true; - } - return false; + return displayScreenSize(osdDisplayPort) >= VIDEO_BUFFER_CHARS_HDZERO; } bool osdIsNotMetric(void) { @@ -660,7 +696,7 @@ static void osdFormatTime(char *buff, uint32_t seconds, char sym_m, char sym_h) value = seconds / 60; } buff[0] = sym; - tfp_sprintf(buff + 1, "%02d:%02d", (int)(value / 60), (int)(value % 60)); + osdFormatTime_MMSS(buff + 1, (int)(value / 60), (int)(value % 60)); } static inline void osdFormatOnTime(char *buff) @@ -771,7 +807,7 @@ static void osdDisplayTemperature(uint8_t elemPosX, uint8_t elemPosY, uint16_t s if ((temperature <= alarm_min) || (temperature >= alarm_max)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); if (osdConfig()->units == OSD_UNIT_IMPERIAL) temperature = temperature * 9 / 5.0f + 320; - tfp_sprintf(buff, "%3d", temperature / 10); + osdFormatIntUnit(buff, 3, temperature / 10, 0); } else strcpy(buff, "---"); @@ -1205,7 +1241,7 @@ static void osdFormatThrottlePosition(char *buff, bool useScaled, textAttributes strcpy(buff + 1, message); return; } - tfp_sprintf(buff + 2, "%3d", throttlePercent); + osdFormatIntUnit(buff + 2, 3, throttlePercent, 0); } /** @@ -1653,25 +1689,25 @@ static void osdDisplayFlightPIDValues(uint8_t elemPosX, uint8_t elemPosY, const } elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", pid->P); + osdFormatIntUnit(buff, 3, pid->P, 0); if ((isAdjustmentFunctionSelected(adjFuncP)) || (((adjFuncP == ADJUSTMENT_ROLL_P) || (adjFuncP == ADJUSTMENT_PITCH_P)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_P)))) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", pid->I); + osdFormatIntUnit(buff, 3, pid->I, 0); if ((isAdjustmentFunctionSelected(adjFuncI)) || (((adjFuncI == ADJUSTMENT_ROLL_I) || (adjFuncI == ADJUSTMENT_PITCH_I)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_I)))) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 8, elemPosY, buff, elemAttr); elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", pid->D); + osdFormatIntUnit(buff, 3, pid->D, 0); if ((isAdjustmentFunctionSelected(adjFuncD)) || (((adjFuncD == ADJUSTMENT_ROLL_D) || (adjFuncD == ADJUSTMENT_PITCH_D)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_D)))) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 12, elemPosY, buff, elemAttr); elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", pid->FF); + osdFormatIntUnit(buff, 3, pid->FF, 0); if ((isAdjustmentFunctionSelected(adjFuncFF)) || (((adjFuncFF == ADJUSTMENT_ROLL_FF) || (adjFuncFF == ADJUSTMENT_PITCH_FF)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_FF)))) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 16, elemPosY, buff, elemAttr); @@ -1695,19 +1731,19 @@ static void osdDisplayNavPIDValues(uint8_t elemPosX, uint8_t elemPosY, const cha } elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", pid->P); + osdFormatIntUnit(buff, 3, pid->P, 0); if ((isAdjustmentFunctionSelected(adjFuncP)) || (((adjFuncP == ADJUSTMENT_ROLL_P) || (adjFuncP == ADJUSTMENT_PITCH_P)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_P)))) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", pid->I); + osdFormatIntUnit(buff, 3, pid->I, 0); if ((isAdjustmentFunctionSelected(adjFuncI)) || (((adjFuncI == ADJUSTMENT_ROLL_I) || (adjFuncI == ADJUSTMENT_PITCH_I)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_I)))) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 8, elemPosY, buff, elemAttr); elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", pidType == PID_TYPE_PIFF ? pid->FF : pid->D); + osdFormatIntUnit(buff, 3, pidType == PID_TYPE_PIFF ? pid->FF : pid->D, 0); if ((isAdjustmentFunctionSelected(adjFuncD)) || (((adjFuncD == ADJUSTMENT_ROLL_D) || (adjFuncD == ADJUSTMENT_PITCH_D)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_D)))) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 12, elemPosY, buff, elemAttr); @@ -1849,7 +1885,7 @@ static bool osdDrawSingleElement(uint8_t item) uint8_t osdRssi = osdConvertRSSI(); buff[0] = SYM_RSSI; if (osdRssi < 100) - tfp_sprintf(buff + 1, "%2d", osdRssi); + osdFormatIntUnit(buff + 1, 2, osdRssi, 0); else tfp_sprintf(buff + 1, "%c ", SYM_MAX); @@ -1899,7 +1935,7 @@ static bool osdDrawSingleElement(uint8_t item) #ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it if (isDJICompatibleVideoSystem(osdConfig())) { //DJIcompat is unable to work with scaled values and it only has mAh symbol to work with - tfp_sprintf(buff, "%5d", (int)getMAhDrawn()); // Use 5 digits to allow packs below 100Ah + osdFormatIntUnit(buff, 5, (int)getMAhDrawn(), 0); // Use 5 digits to allow packs below 100Ah buff[5] = SYM_MAH; buff[6] = '\0'; } else @@ -1940,7 +1976,7 @@ static bool osdDrawSingleElement(uint8_t item) #ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it if (isDJICompatibleVideoSystem(osdConfig())) { //DJIcompat is unable to work with scaled values and it only has mAh symbol to work with - tfp_sprintf(buff, "%5d", (int)getBatteryRemainingCapacity()); // Use 5 digits to allow packs below 100Ah + osdFormatIntUnit(buff, 5, (int)getBatteryRemainingCapacity(), 0); // Use 5 digits to allow packs below 100Ah buff[5] = SYM_MAH; buff[6] = '\0'; unitsDrawn = true; @@ -1979,7 +2015,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_POWER_SUPPLY_IMPEDANCE: if (isPowerSupplyImpedanceValid()) - tfp_sprintf(buff, "%3d", getPowerSupplyImpedance()); + osdFormatIntUnit(buff, 3, getPowerSupplyImpedance(), 0); else strcpy(buff, "---"); buff[3] = SYM_MILLIOHM; @@ -1990,7 +2026,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_GPS_SATS: buff[0] = SYM_SAT_L; buff[1] = SYM_SAT_R; - tfp_sprintf(buff + 2, "%2d", gpsSol.numSat); + osdFormatIntUnit(buff + 2, 2, gpsSol.numSat, 0); #ifdef USE_GPS_FIX_ESTIMATION if (STATE(GPS_ESTIMATED_FIX)) { strcpy(buff + 2, "ES"); @@ -2093,7 +2129,7 @@ static bool osdDrawSingleElement(uint8_t item) if (isImuHeadingValid() && navigationPositionEstimateIsHealthy()) { int16_t h = lrintf(CENTIDEGREES_TO_DEGREES((float)wrap_18000(DEGREES_TO_CENTIDEGREES((int32_t)GPS_directionToHome) - (STATE(AIRPLANE) ? posControl.actualState.cog : DECIDEGREES_TO_CENTIDEGREES((int32_t)osdGetHeading()))))); - tfp_sprintf(buff + 2, "%4d", h); + osdFormatIntUnit(buff + 2, 4, h, 0); } else { strcpy(buff + 2, "----"); } @@ -2126,7 +2162,7 @@ static bool osdDrawSingleElement(uint8_t item) #ifdef USE_BLACKBOX if (IS_RC_MODE_ACTIVE(BOXBLACKBOX)) { if (!isBlackboxDeviceWorking()) { - tfp_sprintf(buff, "%c%c", SYM_BLACKBOX, SYM_ALERT); + osdWriteChar2(buff, SYM_BLACKBOX, SYM_ALERT); } else if (isBlackboxDeviceFull()) { tfp_sprintf(buff, "%cFULL", SYM_BLACKBOX); } else { @@ -2134,7 +2170,7 @@ static bool osdDrawSingleElement(uint8_t item) if (logNumber >= 0) { tfp_sprintf(buff, "%c%05" PRId32, SYM_BLACKBOX, logNumber); } else { - tfp_sprintf(buff, "%c", SYM_BLACKBOX); + osdWriteChar(buff, SYM_BLACKBOX); } } } @@ -2178,7 +2214,7 @@ static bool osdDrawSingleElement(uint8_t item) { buff[0] = SYM_GROUND_COURSE; if (osdIsHeadingValid()) { - tfp_sprintf(&buff[1], "%3d", (int16_t)CENTIDEGREES_TO_DEGREES(posControl.actualState.cog)); + osdFormatIntUnit(&buff[1], 3, (int16_t)CENTIDEGREES_TO_DEGREES(posControl.actualState.cog), 0); } else { buff[1] = buff[2] = buff[3] = '-'; } @@ -2203,7 +2239,7 @@ static bool osdDrawSingleElement(uint8_t item) if (ABS(herr) > 99) strcpy(buff + 1, ">99"); else - tfp_sprintf(buff + 1, "%3d", herr); + osdFormatIntUnit(buff + 1, 3, herr, 0); } buff[4] = SYM_DEGREES; @@ -2225,7 +2261,7 @@ static bool osdDrawSingleElement(uint8_t item) if (!ARMING_FLAG(ARMED)) { buff[1] = buff[2] = buff[3] = buff[4] = '-'; } else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { - tfp_sprintf(buff + 1, "%4d", heading_adjust); + osdFormatIntUnit(buff + 1, 4, heading_adjust, 0); } buff[5] = SYM_DEGREES; @@ -2372,7 +2408,7 @@ static bool osdDrawSingleElement(uint8_t item) } else if (!isImuHeadingValid()) { buff[1] = 'H'; } else { - tfp_sprintf(buff + 1, "%1d", getActiveVehiclesCount()); + osdFormatIntUnit(buff + 1, 1, getActiveVehiclesCount(), 0); } break; } @@ -2639,7 +2675,7 @@ static bool osdDrawSingleElement(uint8_t item) tfp_sprintf(buff, "CH:%c%s:", osdInfo.bandLetter, osdInfo.channelName); displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); - tfp_sprintf(buff, "%c", osdInfo.powerIndexLetter); + osdWriteChar(buff, osdInfo.powerIndexLetter); if (isAdjustmentFunctionSelected(ADJUSTMENT_VTX_POWER_LEVEL)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 6, elemPosY, buff, elemAttr); return true; @@ -2651,10 +2687,10 @@ static bool osdDrawSingleElement(uint8_t item) vtxDeviceOsdInfo_t osdInfo; vtxCommonGetOsdInfo(vtxCommonDevice(), &osdInfo); - tfp_sprintf(buff, "%c", SYM_VTX_POWER); + osdWriteChar(buff, SYM_VTX_POWER); displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); - tfp_sprintf(buff, "%c", osdInfo.powerIndexLetter); + osdWriteChar(buff, osdInfo.powerIndexLetter); if (isAdjustmentFunctionSelected(ADJUSTMENT_VTX_POWER_LEVEL)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX+1, elemPosY, buff, elemAttr); return true; @@ -2665,11 +2701,7 @@ static bool osdDrawSingleElement(uint8_t item) { int16_t rssi = rxLinkStatistics.uplinkRSSI; buff[0] = (rxLinkStatistics.activeAntenna == 0) ? SYM_RSSI : SYM_2RSS; // Separate symbols for each antenna - if (rssi <= -100) { - tfp_sprintf(buff + 1, "%4d%c", rssi, SYM_DBM); - } else { - tfp_sprintf(buff + 1, " %3d%c", rssi, SYM_DBM); - } + osdFormatIntUnit(buff + 1, 4, rssi, SYM_DBM); if (!failsafeIsReceivingRxData()){ TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } else if (osdConfig()->rssi_dbm_alarm && rssi < osdConfig()->rssi_dbm_alarm) { @@ -2695,18 +2727,18 @@ static bool osdDrawSingleElement(uint8_t item) break; case OSD_CRSF_LQ_TYPE3: if (!failsafeIsReceivingRxData()) { - tfp_sprintf(buff+1, "%3d", 0); + osdFormatIntUnit(buff+1, 3, 0, 0); } else { int16_t scaledLQ = scaleRange(constrain(rxLinkStatistics.uplinkLQ, 0, 100), 0, 100, 170, 300); - tfp_sprintf(buff+1, "%3d", rxLinkStatistics.rfMode >= 2 ? scaledLQ : rxLinkStatistics.uplinkLQ); + osdFormatIntUnit(buff+1, 3, rxLinkStatistics.rfMode >= 2 ? scaledLQ : rxLinkStatistics.uplinkLQ, 0); } break; case OSD_CRSF_LQ_TYPE1: default: if (!failsafeIsReceivingRxData()) { - tfp_sprintf(buff+1, "%3d", 0); + osdFormatIntUnit(buff+1, 3, 0, 0); } else { - tfp_sprintf(buff+1, "%3d", rxLinkStatistics.uplinkLQ); + osdFormatIntUnit(buff+1, 3, rxLinkStatistics.uplinkLQ, 0); } break; } @@ -2722,9 +2754,9 @@ static bool osdDrawSingleElement(uint8_t item) { buff[0] = SYM_LQ; if (!failsafeIsReceivingRxData()) { - tfp_sprintf(buff+1, "%3d%c", 0, SYM_AH_DECORATION_DOWN); + osdFormatIntUnit(buff+1, 3, 0, SYM_AH_DECORATION_DOWN); } else { - tfp_sprintf(buff+1, "%3d%c", rxLinkStatistics.downlinkLQ, SYM_AH_DECORATION_DOWN); + osdFormatIntUnit(buff+1, 3, rxLinkStatistics.downlinkLQ, SYM_AH_DECORATION_DOWN); } if (!failsafeIsReceivingRxData()) { @@ -2752,7 +2784,7 @@ static bool osdDrawSingleElement(uint8_t item) } else if (snrVal <= osdConfig()->snr_alarm) { buff[0] = SYM_SNR; if (snrVal <= -10) { - tfp_sprintf(buff + 1, "%3d%c", snrVal, SYM_DB); + osdFormatIntUnit(buff + 1, 3, snrVal, SYM_DB); } else { tfp_sprintf(buff + 1, "%2d%c%c", snrVal, SYM_DB, ' '); } @@ -2765,7 +2797,7 @@ static bool osdDrawSingleElement(uint8_t item) if (!failsafeIsReceivingRxData()) tfp_sprintf(buff, "%s%c", " ", SYM_MW); else - tfp_sprintf(buff, "%4d%c", rxLinkStatistics.uplinkTXPower, SYM_MW); + osdFormatIntUnit(buff, 4, rxLinkStatistics.uplinkTXPower, SYM_MW); break; } @@ -2889,7 +2921,7 @@ static bool osdDrawSingleElement(uint8_t item) } altc = ABS(constrain(altc, -999, 999)); - tfp_sprintf(buff, "%3d", altc); + osdFormatIntUnit(buff, 3, altc, 0); displayWrite(osdDisplayPort, elemPosX + 1, elemPosY + 2, buff); return true; @@ -3098,7 +3130,7 @@ static bool osdDrawSingleElement(uint8_t item) // mAh/foot if (efficiencyValid) { osdFormatCentiNumber(buff, (value * METERS_PER_FOOT), 1, 2, 2, 3, false); - tfp_sprintf(buff + strlen(buff), "%c%c", SYM_AH_V_FT_0, SYM_AH_V_FT_1); + osdWriteChar2(buff + strlen(buff), SYM_AH_V_FT_0, SYM_AH_V_FT_1); } else { buff[0] = buff[1] = buff[2] = '-'; buff[3] = SYM_AH_V_FT_0; @@ -3112,7 +3144,7 @@ static bool osdDrawSingleElement(uint8_t item) // mAh/metre if (efficiencyValid) { osdFormatCentiNumber(buff, value, 1, 2, 2, 3, false); - tfp_sprintf(buff + strlen(buff), "%c%c", SYM_AH_V_M_0, SYM_AH_V_M_1); + osdWriteChar2(buff + strlen(buff), SYM_AH_V_M_0, SYM_AH_V_M_1); } else { buff[0] = buff[1] = buff[2] = '-'; buff[3] = SYM_AH_V_M_0; @@ -3135,7 +3167,7 @@ static bool osdDrawSingleElement(uint8_t item) buff[4] = SYM_DECORATION; buff[5] = SYM_DECORATION; } else { - tfp_sprintf(buff + 1, "%02d:%02d", (int)(glideTime / 60), (int)(glideTime % 60)); + osdFormatTime_MMSS(buff + 1, (int)(glideTime / 60), (int)(glideTime % 60)); } } else { tfp_sprintf(buff + 1, "%s", "--:--"); @@ -3191,7 +3223,7 @@ static bool osdDrawSingleElement(uint8_t item) } if (osdConfig()->pan_servo_indicator_show_degrees) { - tfp_sprintf(buff, "%3d%c", -panOffset, SYM_DEGREES); + osdFormatIntUnit(buff, 3, -panOffset, SYM_DEGREES); displayWriteWithAttr(osdDisplayPort, elemPosX+1, elemPosY, buff, elemAttr); } displayWriteCharWithAttr(osdDisplayPort, elemPosX, elemPosY, SYM_SERVO_PAN_IS_OFFSET_R, elemAttr); @@ -3207,7 +3239,7 @@ static bool osdDrawSingleElement(uint8_t item) } if (osdConfig()->pan_servo_indicator_show_degrees) { - tfp_sprintf(buff, "%3d%c", panOffset, SYM_DEGREES); + osdFormatIntUnit(buff, 3, panOffset, SYM_DEGREES); displayWriteWithAttr(osdDisplayPort, elemPosX+1, elemPosY, buff, elemAttr); } displayWriteCharWithAttr(osdDisplayPort, elemPosX, elemPosY, SYM_SERVO_PAN_IS_OFFSET_L, elemAttr); @@ -3215,7 +3247,7 @@ static bool osdDrawSingleElement(uint8_t item) panServoTimeOffCentre = 0; if (osdConfig()->pan_servo_indicator_show_degrees) { - tfp_sprintf(buff, "%3d%c", panOffset, SYM_DEGREES); + osdFormatIntUnit(buff, 3, panOffset, SYM_DEGREES); displayWriteWithAttr(osdDisplayPort, elemPosX+1, elemPosY, buff, elemAttr); } displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_SERVO_PAN_IS_CENTRED); @@ -3294,7 +3326,7 @@ static bool osdDrawSingleElement(uint8_t item) displayWrite(osdDisplayPort, elemPosX, elemPosY, "SPR"); elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", currentControlProfile->stabilized.rates[FD_PITCH]); + osdFormatIntUnit(buff, 3, currentControlProfile->stabilized.rates[FD_PITCH], 0); if (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_RATE)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); @@ -3304,7 +3336,7 @@ static bool osdDrawSingleElement(uint8_t item) displayWrite(osdDisplayPort, elemPosX, elemPosY, "SRR"); elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", currentControlProfile->stabilized.rates[FD_ROLL]); + osdFormatIntUnit(buff, 3, currentControlProfile->stabilized.rates[FD_ROLL], 0); if (isAdjustmentFunctionSelected(ADJUSTMENT_ROLL_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_RATE)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); @@ -3326,7 +3358,7 @@ static bool osdDrawSingleElement(uint8_t item) displayWrite(osdDisplayPort, elemPosX, elemPosY, "MPR"); elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", currentControlProfile->manual.rates[FD_PITCH]); + osdFormatIntUnit(buff, 3, currentControlProfile->manual.rates[FD_PITCH], 0); if (isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_ROLL_RATE)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); @@ -3336,7 +3368,7 @@ static bool osdDrawSingleElement(uint8_t item) displayWrite(osdDisplayPort, elemPosX, elemPosY, "MRR"); elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", currentControlProfile->manual.rates[FD_ROLL]); + osdFormatIntUnit(buff, 3, currentControlProfile->manual.rates[FD_ROLL], 0); if (isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_ROLL_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_ROLL_RATE)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); @@ -3398,11 +3430,11 @@ static bool osdDrawSingleElement(uint8_t item) const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); strcpy(buff, "POSO "); // display requested velocity cm/s - tfp_sprintf(buff + 5, "%4d", (int)lrintf(nav_pids->pos[X].output_constrained * 100)); + osdFormatIntUnit(buff + 5, 4, (int)lrintf(nav_pids->pos[X].output_constrained * 100), 0); buff[9] = ' '; - tfp_sprintf(buff + 10, "%4d", (int)lrintf(nav_pids->pos[Y].output_constrained * 100)); + osdFormatIntUnit(buff + 10, 4, (int)lrintf(nav_pids->pos[Y].output_constrained * 100), 0); buff[14] = ' '; - tfp_sprintf(buff + 15, "%4d", (int)lrintf(nav_pids->pos[Z].output_constrained * 100)); + osdFormatIntUnit(buff + 15, 4, (int)lrintf(nav_pids->pos[Z].output_constrained * 100), 0); buff[19] = '\0'; break; } @@ -3515,7 +3547,7 @@ static bool osdDrawSingleElement(uint8_t item) if (h < 0) { h += 360; } - tfp_sprintf(&buff[1], "%3d", h); + osdFormatIntUnit(&buff[1], 3, h, 0); } else { buff[1] = buff[2] = buff[3] = '-'; } @@ -3575,9 +3607,9 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_UNIT_IMPERIAL: moreThanAh = osdFormatCentiNumber(buff, value * METERS_PER_MILE / 10, 1000, 0, 2, digits, false); if (!moreThanAh) { - tfp_sprintf(buff + strlen(buff), "%c%c", SYM_MAH_MI_0, SYM_MAH_MI_1); + osdWriteChar2(buff + strlen(buff), SYM_MAH_MI_0, SYM_MAH_MI_1); } else { - tfp_sprintf(buff + strlen(buff), "%c", SYM_AH_MI); + osdWriteChar(buff + strlen(buff), SYM_AH_MI); } if (!efficiencyValid) { buff[0] = buff[1] = buff[2] = buff[3] = '-'; @@ -3589,9 +3621,9 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_UNIT_GA: moreThanAh = osdFormatCentiNumber(buff, value * METERS_PER_NAUTICALMILE / 10, 1000, 0, 2, digits, false); if (!moreThanAh) { - tfp_sprintf(buff + strlen(buff), "%c%c", SYM_MAH_NM_0, SYM_MAH_NM_1); + osdWriteChar2(buff + strlen(buff), SYM_MAH_NM_0, SYM_MAH_NM_1); } else { - tfp_sprintf(buff + strlen(buff), "%c", SYM_AH_NM); + osdWriteChar(buff + strlen(buff), SYM_AH_NM); } if (!efficiencyValid) { buff[0] = buff[1] = buff[2] = buff[3] = '-'; @@ -3605,9 +3637,9 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_UNIT_METRIC: moreThanAh = osdFormatCentiNumber(buff, value * 100, 1000, 0, 2, digits, false); if (!moreThanAh) { - tfp_sprintf(buff + strlen(buff), "%c%c", SYM_MAH_KM_0, SYM_MAH_KM_1); + osdWriteChar2(buff + strlen(buff), SYM_MAH_KM_0, SYM_MAH_KM_1); } else { - tfp_sprintf(buff + strlen(buff), "%c", SYM_AH_KM); + osdWriteChar(buff + strlen(buff), SYM_AH_KM); } if (!efficiencyValid) { buff[0] = buff[1] = buff[2] = buff[3] = '-'; @@ -3818,7 +3850,7 @@ static bool osdDrawSingleElement(uint8_t item) else h = h + 180; - tfp_sprintf(&buff[1], "%3d", h); + osdFormatIntUnit(&buff[1], 3, h, 0); } else { buff[1] = buff[2] = buff[3] = '-'; } @@ -3948,7 +3980,7 @@ static bool osdDrawSingleElement(uint8_t item) displayWrite(osdDisplayPort, elemPosX, elemPosY, "TPA"); attr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", currentControlProfile->throttle.dynPID); + osdFormatIntUnit(buff, 3, currentControlProfile->throttle.dynPID, 0); if (isAdjustmentFunctionSelected(ADJUSTMENT_TPA)) { TEXT_ATTRIBUTES_ADD_BLINK(attr); } @@ -3956,7 +3988,7 @@ static bool osdDrawSingleElement(uint8_t item) displayWrite(osdDisplayPort, elemPosX, elemPosY + 1, "BP"); attr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%4d", currentControlProfile->throttle.pa_breakpoint); + osdFormatIntUnit(buff, 4, currentControlProfile->throttle.pa_breakpoint, 0); if (isAdjustmentFunctionSelected(ADJUSTMENT_TPA_BREAKPOINT)) { TEXT_ATTRIBUTES_ADD_BLINK(attr); } @@ -4636,17 +4668,18 @@ uint8_t drawStat_Stats(uint8_t statNameX, uint8_t row, uint8_t statValueX, bool char string_buffer[osdDisplayPort->cols - statValueX]; if (statsConfig()->stats_enabled) { - if (isBootStats) + if (isBootStats) { displayWrite(osdDisplayPort, statNameX, row, "ODOMETER:"); - else + } else { displayWrite(osdDisplayPort, statNameX, row, "ODOMETER"); + } switch (osdConfig()->units) { case OSD_UNIT_UK: FALLTHROUGH; case OSD_UNIT_IMPERIAL: if (isBootStats) { - tfp_sprintf(string_buffer, "%5d", (uint16_t)(statsConfig()->stats_total_dist / METERS_PER_MILE)); + osdFormatIntUnit(string_buffer, 5, (uint16_t)(statsConfig()->stats_total_dist / METERS_PER_MILE), 0); buffLen = 5; } else { uint16_t statTotalDist = (uint16_t)(statsConfig()->stats_total_dist / METERS_PER_MILE); @@ -4659,7 +4692,7 @@ uint8_t drawStat_Stats(uint8_t statNameX, uint8_t row, uint8_t statValueX, bool default: case OSD_UNIT_GA: if (isBootStats) { - tfp_sprintf(string_buffer, "%5d", (uint16_t)(statsConfig()->stats_total_dist / METERS_PER_NAUTICALMILE)); + osdFormatIntUnit(string_buffer, 5, (uint16_t)(statsConfig()->stats_total_dist / METERS_PER_NAUTICALMILE), 0); buffLen = 5; } else { uint16_t statTotalDist = (uint16_t)(statsConfig()->stats_total_dist / METERS_PER_NAUTICALMILE); @@ -4673,7 +4706,7 @@ uint8_t drawStat_Stats(uint8_t statNameX, uint8_t row, uint8_t statValueX, bool FALLTHROUGH; case OSD_UNIT_METRIC: if (isBootStats) { - tfp_sprintf(string_buffer, "%5d", (uint16_t)(statsConfig()->stats_total_dist / METERS_PER_KILOMETER)); + osdFormatIntUnit(string_buffer, 5, (uint16_t)(statsConfig()->stats_total_dist / METERS_PER_KILOMETER), 0); buffLen = 5; } else { uint16_t statTotalDist = (uint16_t)(statsConfig()->stats_total_dist / METERS_PER_KILOMETER); @@ -4687,25 +4720,27 @@ uint8_t drawStat_Stats(uint8_t statNameX, uint8_t row, uint8_t statValueX, bool string_buffer[buffLen] = '\0'; displayWrite(osdDisplayPort, statValueX-(isBootStats ? 5 : 0), row, string_buffer); - if (isBootStats) + if (isBootStats) { displayWrite(osdDisplayPort, statNameX, ++row, "TOTAL TIME:"); - else + } else { displayWrite(osdDisplayPort, statNameX, ++row, "TOTAL TIME"); + } uint32_t tot_mins = statsConfig()->stats_total_time / 60; - if (isBootStats) + if (isBootStats) { tfp_sprintf(string_buffer, "%d:%02dH:M%c", (int)(tot_mins / 60), (int)(tot_mins % 60), '\0'); - else + } else { tfp_sprintf(string_buffer, ": %d:%02d H:M%c", (int)(tot_mins / 60), (int)(tot_mins % 60), '\0'); + } displayWrite(osdDisplayPort, statValueX-(isBootStats ? 7 : 0), row, string_buffer); #ifdef USE_ADC if (feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER) && statsConfig()->stats_total_energy) { uint8_t buffOffset = 0; - if (isBootStats) + if (isBootStats) { displayWrite(osdDisplayPort, statNameX, ++row, "TOTAL ENERGY:"); - else { + } else { displayWrite(osdDisplayPort, statNameX, ++row, "TOTAL ENERGY"); string_buffer[0] = ':'; buffOffset = 2; @@ -4723,9 +4758,9 @@ uint8_t drawStat_Stats(uint8_t statNameX, uint8_t row, uint8_t statValueX, bool } if (statsConfig()->stats_total_dist) { - if (isBootStats) + if (isBootStats) { displayWrite(osdDisplayPort, statNameX, ++row, "AVG EFFICIENCY:"); - else { + } else { displayWrite(osdDisplayPort, statNameX, ++row, "AVG EFFICIENCY"); strcat(avgEffBuff, ": "); } @@ -4751,10 +4786,11 @@ uint8_t drawStat_Stats(uint8_t statNameX, uint8_t row, uint8_t statValueX, bool break; } - if (isBootStats) + if (isBootStats) { strcat(avgEffBuff, string_buffer); - else + } else { strcat(avgEffBuff, osdFormatTrimWhiteSpace(string_buffer)); + } } else { strcat(avgEffBuff, "----"); } @@ -4846,8 +4882,7 @@ static void osdCompleteAsyncInitialization(void) void osdInit(displayPort_t *osdDisplayPortToUse) { - if (!osdDisplayPortToUse) - return; + if (!osdDisplayPortToUse) return; BUILD_BUG_ON(OSD_POS_MAX != OSD_POS(63,63)); @@ -4896,23 +4931,17 @@ static void osdUpdateStats(void) value = osdGet3DSpeed(); const float airspeed_estimate = getAirspeedEstimate(); - if (stats.max_3D_speed < value) - stats.max_3D_speed = value; + if (stats.max_3D_speed < value) stats.max_3D_speed = value; - if (stats.max_speed < gpsSol.groundSpeed) - stats.max_speed = gpsSol.groundSpeed; + if (stats.max_speed < gpsSol.groundSpeed) stats.max_speed = gpsSol.groundSpeed; - if (stats.max_air_speed < airspeed_estimate) - stats.max_air_speed = airspeed_estimate; + if (stats.max_air_speed < airspeed_estimate) stats.max_air_speed = airspeed_estimate; - if (stats.max_distance < GPS_distanceToHome) - stats.max_distance = GPS_distanceToHome; + if (stats.max_distance < GPS_distanceToHome) stats.max_distance = GPS_distanceToHome; - if (stats.min_sats > gpsSol.numSat) - stats.min_sats = gpsSol.numSat; + if (stats.min_sats > gpsSol.numSat) stats.min_sats = gpsSol.numSat; - if (stats.max_sats < gpsSol.numSat) - stats.max_sats = gpsSol.numSat; + if (stats.max_sats < gpsSol.numSat) stats.max_sats = gpsSol.numSat; } #if defined(USE_ESC_SENSOR) if (STATE(ESC_SENSOR_ENABLED)) { @@ -4920,41 +4949,31 @@ static void osdUpdateStats(void) bool escTemperatureValid = escSensor && escSensor->dataAge <= ESC_DATA_MAX_AGE; if (escTemperatureValid) { - if (stats.min_esc_temp > escSensor->temperature) - stats.min_esc_temp = escSensor->temperature; - - if (stats.max_esc_temp < escSensor->temperature) - stats.max_esc_temp = escSensor->temperature; + if (stats.min_esc_temp > escSensor->temperature) stats.min_esc_temp = escSensor->temperature; + if (stats.max_esc_temp < escSensor->temperature) stats.max_esc_temp = escSensor->temperature; } } #endif value = getBatteryVoltage(); - if (stats.min_voltage > value) - stats.min_voltage = value; + if (stats.min_voltage > value) stats.min_voltage = value; value = abs(getAmperage()); - if (stats.max_current < value) - stats.max_current = value; + if (stats.max_current < value) stats.max_current = value; value = labs(getPower()); - if (stats.max_power < value) - stats.max_power = value; + if (stats.max_power < value) stats.max_power = value; value = osdConvertRSSI(); - if (stats.min_rssi > value) - stats.min_rssi = value; + if (stats.min_rssi > value) stats.min_rssi = value; value = osdGetCrsfLQ(); - if (stats.min_lq > value) - stats.min_lq = value; + if (stats.min_lq > value) stats.min_lq = value; - if (!failsafeIsReceivingRxData()) - stats.min_lq = 0; + if (!failsafeIsReceivingRxData()) stats.min_lq = 0; value = osdGetCrsfdBm(); - if (stats.min_rssi_dbm > value) - stats.min_rssi_dbm = value; + if (stats.min_rssi_dbm > value) stats.min_rssi_dbm = value; stats.max_altitude = MAX(stats.max_altitude, osdGetAltitude()); } @@ -4992,10 +5011,10 @@ uint8_t drawStat_MaxDistanceFromHome(uint8_t col, uint8_t row, uint8_t statValX) uint8_t valueXOffset = 0; if (!osdDisplayIsHD()) { displayWrite(osdDisplayPort, col, row, "DISTANCE FROM "); - valueXOffset = 14; + valueXOffset = 14; } else { displayWrite(osdDisplayPort, col, row, "MAX DISTANCE FROM "); - valueXOffset = 18; + valueXOffset = 18; } displayWriteChar(osdDisplayPort, col + valueXOffset, row, SYM_HOME); tfp_sprintf(buff, ": "); @@ -5039,10 +5058,11 @@ uint8_t drawStat_BatteryVoltage(uint8_t col, uint8_t row, uint8_t statValX) { char buff[12]; uint8_t multiValueXOffset = 0; - if (!osdDisplayIsHD()) + if (!osdDisplayIsHD()) { displayWrite(osdDisplayPort, col, row, "MIN VOLTS P/C"); - else + } else { displayWrite(osdDisplayPort, col, row, "MIN VOLTS PACK/CELL"); + } // Pack voltage tfp_sprintf(buff, ": "); @@ -5051,7 +5071,7 @@ uint8_t drawStat_BatteryVoltage(uint8_t col, uint8_t row, uint8_t statValX) multiValueXOffset = strlen(buff); // AverageCell osdFormatCentiNumber(buff + multiValueXOffset, stats.min_voltage / getBatteryCellCount(), 0, 2, 0, 3, false); - tfp_sprintf(buff + strlen(buff), "%c", SYM_VOLT); + osdWriteChar(buff + strlen(buff), SYM_VOLT); displayWrite(osdDisplayPort, statValX, row++, buff); @@ -5070,10 +5090,11 @@ uint8_t drawStat_MaximumPowerAndCurrent(uint8_t col, uint8_t row, uint8_t statVa strcat(outBuff, osdFormatTrimWhiteSpace(buff)); displayWrite(osdDisplayPort, statValX, row, outBuff); - if (kiloWatt) + if (kiloWatt) { displayWrite(osdDisplayPort, col, row, "MAX AMPS/K WATTS"); - else + } else { displayWrite(osdDisplayPort, col, row, "MAX AMPS/WATTS"); + } return ++row; } @@ -5082,10 +5103,12 @@ uint8_t drawStat_UsedEnergy(uint8_t col, uint8_t row, uint8_t statValX) { char buff[12]; - if (osdDisplayIsHD()) + if (osdDisplayIsHD()) { displayWrite(osdDisplayPort, col, row, "USED ENERGY FLT/TOT"); - else + } else { displayWrite(osdDisplayPort, col, row, "USED ENERGY F/T"); + } + tfp_sprintf(buff, ": "); if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { tfp_sprintf(buff + 2, "%d/%d%c", (int)(getMAhDrawn() - stats.flightStartMAh), (int)getMAhDrawn(), SYM_MAH); @@ -5096,7 +5119,7 @@ uint8_t drawStat_UsedEnergy(uint8_t col, uint8_t row, uint8_t statValX) strcat(buff, "/"); osdFormatCentiNumber(preBuff, getMWhDrawn() / 10, 0, 2, 0, 3, false); strcat(buff, osdFormatTrimWhiteSpace(preBuff)); - tfp_sprintf(buff + strlen(buff), "%c", SYM_WH); + osdWriteChar(buff + strlen(buff), SYM_WH); } displayWrite(osdDisplayPort, statValX, row++, buff); @@ -5111,10 +5134,11 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b bool moreThanAh = false; bool efficiencyValid = totalDistance >= 10000; - if (osdDisplayIsHD()) + if (osdDisplayIsHD()) { displayWrite(osdDisplayPort, col, row, "AVG EFFICIENCY FLT/TOT"); - else + } else { displayWrite(osdDisplayPort, col, row, "AV EFFICIENCY F/T"); + } tfp_sprintf(outBuff, ": "); uint8_t digits = 3U; // Total number of digits (including decimal point) @@ -5134,10 +5158,11 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b moreThanAh = osdFormatCentiNumber(buff, (int32_t)((getMAhDrawn() - stats.flightStartMAh) * 10000.0f * METERS_PER_MILE / totalDistance), 1000, 0, 2, digits, false); strcat(outBuff, osdFormatTrimWhiteSpace(buff)); if (osdDisplayIsHD()) { - if (!moreThanAh) - tfp_sprintf(outBuff + strlen(outBuff), "%c%c", SYM_MAH_MI_0, SYM_MAH_MI_1); - else - tfp_sprintf(outBuff + strlen(outBuff), "%c", SYM_AH_MI); + if (!moreThanAh) { + osdWriteChar2(outBuff + strlen(outBuff), SYM_MAH_MI_0, SYM_MAH_MI_1); + } else { + osdWriteChar(outBuff + strlen(outBuff), SYM_AH_MI); + } moreThanAh = false; } @@ -5146,10 +5171,11 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b moreThanAh = moreThanAh || osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_MILE / totalDistance), 1000, 0, 2, digits, false); strcat(outBuff, osdFormatTrimWhiteSpace(buff)); - if (!moreThanAh) - tfp_sprintf(outBuff + strlen(outBuff), "%c%c", SYM_MAH_MI_0, SYM_MAH_MI_1); - else - tfp_sprintf(outBuff + strlen(outBuff), "%c", SYM_AH_MI); + if (!moreThanAh) { + osdWriteChar2(outBuff + strlen(outBuff), SYM_MAH_MI_0, SYM_MAH_MI_1); + } else { + osdWriteChar(outBuff + strlen(outBuff), SYM_AH_MI); + } } else { tfp_sprintf(outBuff + strlen(outBuff), "---/---%c%c", SYM_MAH_MI_0, SYM_MAH_MI_1); } @@ -5163,7 +5189,7 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b } else { strcat(outBuff, "---/---"); } - tfp_sprintf(outBuff + strlen(outBuff), "%c", SYM_WH_MI); + osdWriteChar(outBuff + strlen(outBuff), SYM_WH_MI); } break; case OSD_UNIT_GA: @@ -5172,10 +5198,11 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b moreThanAh = osdFormatCentiNumber(buff, (int32_t)((getMAhDrawn()-stats.flightStartMAh) * 10000.0f * METERS_PER_NAUTICALMILE / totalDistance), 1000, 0, 2, digits, false); strcat(outBuff, osdFormatTrimWhiteSpace(buff)); if (osdDisplayIsHD()) { - if (!moreThanAh) - tfp_sprintf(outBuff + strlen(outBuff), "%c%c", SYM_MAH_NM_0, SYM_MAH_NM_1); - else - tfp_sprintf(outBuff + strlen(outBuff), "%c", SYM_AH_NM); + if (!moreThanAh) { + osdWriteChar2(outBuff + strlen(outBuff), SYM_MAH_NM_0, SYM_MAH_NM_1); + } else { + osdWriteChar(outBuff + strlen(outBuff), SYM_AH_NM); + } moreThanAh = false; } @@ -5184,9 +5211,9 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b moreThanAh = moreThanAh || osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_NAUTICALMILE / totalDistance), 1000, 0, 2, digits, false); strcat(outBuff, osdFormatTrimWhiteSpace(buff)); if (!moreThanAh) { - tfp_sprintf(outBuff + strlen(outBuff), "%c%c", SYM_MAH_NM_0, SYM_MAH_NM_1); + osdWriteChar2(outBuff + strlen(outBuff), SYM_MAH_NM_0, SYM_MAH_NM_1); } else { - tfp_sprintf(outBuff + strlen(outBuff), "%c", SYM_AH_NM); + osdWriteChar(outBuff + strlen(outBuff), SYM_AH_NM); } } else { tfp_sprintf(outBuff + strlen(outBuff), "---/---%c%c", SYM_MAH_NM_0, SYM_MAH_NM_1); @@ -5201,7 +5228,7 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b } else { strcat(outBuff, "---/---"); } - tfp_sprintf(outBuff + strlen(outBuff), "%c", SYM_WH_NM); + osdWriteChar(outBuff + strlen(outBuff), SYM_WH_NM); } break; case OSD_UNIT_METRIC_MPH: @@ -5217,10 +5244,11 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b moreThanAh = osdFormatCentiNumber(buff, (int32_t)((getMAhDrawn() - stats.flightStartMAh) * 10000000.0f / totalDistance), 1000, 0, 2, digits, false); strcat(outBuff, osdFormatTrimWhiteSpace(buff)); if (osdDisplayIsHD()) { - if (!moreThanAh) - tfp_sprintf(outBuff + strlen(outBuff), "%c%c", SYM_MAH_KM_0, SYM_MAH_KM_1); - else - tfp_sprintf(outBuff + strlen(outBuff), "%c", SYM_AH_KM); + if (!moreThanAh) { + osdWriteChar2(outBuff + strlen(outBuff), SYM_MAH_KM_0, SYM_MAH_KM_1); + } else { + osdWriteChar(outBuff + strlen(outBuff), SYM_AH_KM); + } moreThanAh = false; } @@ -5229,9 +5257,9 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b moreThanAh = moreThanAh || osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000000.0f / totalDistance), 1000, 0, 2, digits, false); strcat(outBuff, osdFormatTrimWhiteSpace(buff)); if (!moreThanAh) { - tfp_sprintf(outBuff + strlen(outBuff), "%c%c", SYM_MAH_KM_0, SYM_MAH_KM_1); + osdWriteChar2(outBuff + strlen(outBuff), SYM_MAH_KM_0, SYM_MAH_KM_1); } else { - tfp_sprintf(outBuff + strlen(outBuff), "%c", SYM_AH_KM); + osdWriteChar(outBuff + strlen(outBuff), SYM_AH_KM); } } else { tfp_sprintf(outBuff + strlen(outBuff), "---/---%c%c", SYM_MAH_KM_0, SYM_MAH_KM_1); @@ -5246,11 +5274,11 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b } else { strcat(outBuff, "---/---"); } - tfp_sprintf(outBuff + strlen(outBuff), "%c", SYM_WH_KM); + osdWriteChar(outBuff + strlen(outBuff), SYM_WH_KM); } } + osdWriteChar(outBuff + strlen(outBuff), '\0'); - tfp_sprintf(outBuff + strlen(outBuff), "%c", '\0'); displayWrite(osdDisplayPort, statValX, row++, outBuff); return row; @@ -5259,14 +5287,12 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b uint8_t drawStat_RXStats(uint8_t col, uint8_t row, uint8_t statValX) { char buff[20]; - uint8_t multiValueXOffset = 0; tfp_sprintf(buff, "MIN RSSI"); if (rxConfig()->serialrx_provider == SERIALRX_CRSF) { strcat(buff, "/LQ"); - if (osdDisplayIsHD()) - strcat(buff, "/DBM"); + if (osdDisplayIsHD()) strcat(buff, "/DBM"); } displayWrite(osdDisplayPort, col, row, buff); @@ -5277,14 +5303,13 @@ uint8_t drawStat_RXStats(uint8_t col, uint8_t row, uint8_t statValX) if (rxConfig()->serialrx_provider == SERIALRX_CRSF) { strcat(osdFormatTrimWhiteSpace(buff), "/"); - multiValueXOffset = strlen(buff); - itoa(stats.min_lq, buff + multiValueXOffset, 10); + itoa(stats.min_lq, buff + strlen(buff), 10); strcat(osdFormatTrimWhiteSpace(buff), "%"); if (osdDisplayIsHD()) { strcat(osdFormatTrimWhiteSpace(buff), "/"); - itoa(stats.min_rssi_dbm, buff + 2, 10); - tfp_sprintf(buff + strlen(buff), "%c", SYM_DBM); + itoa(stats.min_rssi_dbm, buff + strlen(buff), 10); + osdWriteChar(buff + strlen(buff), SYM_DBM); displayWrite(osdDisplayPort, statValX, row++, buff); } } @@ -5296,7 +5321,7 @@ uint8_t drawStat_RXStats(uint8_t col, uint8_t row, uint8_t statValX) memset(buff, '\0', strlen(buff)); tfp_sprintf(buff, ": "); itoa(stats.min_rssi_dbm, buff + 2, 10); - tfp_sprintf(buff + strlen(buff), "%c", SYM_DBM); + osdWriteChar(buff + strlen(buff), SYM_DBM); displayWrite(osdDisplayPort, statValX, row++, buff); } @@ -5336,10 +5361,11 @@ uint8_t drawStat_GForce(uint8_t col, uint8_t row, uint8_t statValX) const float acc_extremes_min = acc_extremes[Z].min; const float acc_extremes_max = acc_extremes[Z].max; - if (!osdDisplayIsHD()) + if (!osdDisplayIsHD()) { displayWrite(osdDisplayPort, col, row, "MAX G-FORCE"); - else - displayWrite(osdDisplayPort, col, row, "MAX/MIN Z/MAX Z G-FORCE"); + } else { + displayWrite(osdDisplayPort, col, row, "MAX/MIN Z/MAX Z G-FORCE"); + } tfp_sprintf(outBuff, ": "); osdFormatCentiNumber(buff, max_gforce * 100, 0, 2, 0, 3, false); @@ -5380,23 +5406,18 @@ uint8_t drawStat_DisarmMethod(uint8_t col, uint8_t row, uint8_t statValX) static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) { - const char * statsHeader[2] = {"*** STATS 1/2 -> ***", "*** STATS <- 2/2 ***"}; - uint8_t row = 1; // Start one line down leaving space at the top of the screen. + uint8_t row = 1; // Start one line down leaving space at the top of the screen (Top row = index 0) const uint8_t statNameX = (osdDisplayPort->cols - (osdDisplayIsHD() ? 41 : 28)) / 2; const uint8_t statValuesX = osdDisplayPort->cols - statNameX - (osdDisplayIsHD() ? 15 : 11); - if (page > 1) - page = 0; - displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING); displayClearScreen(osdDisplayPort); if (isSinglePageStatsCompatible) { char buff[25]; tfp_sprintf(buff, "*** STATS "); -#ifdef USE_BLACKBOX -#ifdef USE_SDCARD +#if defined(USE_BLACKBOX) && defined(USE_SDCARD) if (feature(FEATURE_BLACKBOX)) { int32_t logNumber = blackboxGetLogNumber(); if (logNumber >= 0) @@ -5404,16 +5425,19 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) else tfp_sprintf(buff + strlen(buff), " %c ", SYM_BLACKBOX); } -#endif #endif strcat(buff, "***"); displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buff)) / 2, row++, buff); - } else - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(statsHeader[page + 1])) / 2, row++, statsHeader[page]); + } else { + const char * statsHeader[2] = {"*** STATS 1/2 -> ***", "*** STATS <- 2/2 ***"}; + + if (page > 1) page = 0; + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(statsHeader[page])) / 2, row++, statsHeader[page]); + } if (isSinglePageStatsCompatible) { - // Top 15 rows for most important stats. Max 19 rows (WTF) + /* 13 rows used for most important stats */ row = drawStat_FlightTime(statNameX, row, statValuesX); // 1 row row = drawStat_FlightDistance(statNameX, row, statValuesX); // 1 row if (feature(FEATURE_GPS)) row = drawStat_MaxDistanceFromHome(statNameX, row, statValuesX); // 1 row @@ -5424,68 +5448,65 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) if (feature(FEATURE_CURRENT_METER)) row = drawStat_UsedEnergy(statNameX, row, statValuesX); // 1 row if (feature(FEATURE_CURRENT_METER) && feature(FEATURE_GPS)) row = drawStat_AverageEfficiency(statNameX, row, statValuesX, false); // 1 row if (osdConfig()->stats_show_metric_efficiency && osdIsNotMetric() && feature(FEATURE_CURRENT_METER) && feature(FEATURE_GPS)) row = drawStat_AverageEfficiency(statNameX, row, statValuesX, true); // 1 row - row = drawStat_RXStats(statNameX, row, statValuesX); // 1 row if non-CRSF else 2 rows + row = drawStat_RXStats(statNameX, row, statValuesX); // 1 row if (feature(FEATURE_GPS)) row = drawStat_GPS(statNameX, row, statValuesX); // 1 row if (STATE(ESC_SENSOR_ENABLED)) row = drawStat_ESCTemperature(statNameX, row, statValuesX); // 1 row - // Draw these if there is space space - if (row < (osdDisplayPort->cols-3)) row = drawStat_GForce(statNameX, row, statValuesX); // 1 row HD or 2 rows SD + /* Additional stats added if space available whilst leaving 3 rows available at the bottom for + * disarm method (1 row), save settings message (1 row) and an empty last row */ + if (row < (osdDisplayPort->rows - 3)) row = drawStat_GForce(statNameX, row, statValuesX); // 1 row #ifdef USE_STATS - if (row < (osdDisplayPort->cols-7) && statsConfig()->stats_enabled) row = drawStat_Stats(statNameX, row, statValuesX, false); // 4 rows + if (row < (osdDisplayPort->rows - 6) && statsConfig()->stats_enabled) row = drawStat_Stats(statNameX, row, statValuesX, false); // 4 rows #endif } else { - switch (page) { - case 0: - // Max 10 rows - row = drawStat_FlightTime(statNameX, row, statValuesX); // 1 row - row = drawStat_FlightDistance(statNameX, row, statValuesX); // 1 row - if (feature(FEATURE_GPS)) row = drawStat_MaxDistanceFromHome(statNameX, row, statValuesX); // 1 row - if (feature(FEATURE_GPS)) row = drawStat_Speed(statNameX, row, statValuesX); // 1 row - row = drawStat_MaximumAltitude(statNameX, row, statValuesX); // 1 row - row = drawStat_BatteryVoltage(statNameX, row, statValuesX); // 1 row - if (feature(FEATURE_CURRENT_METER)) row = drawStat_MaximumPowerAndCurrent(statNameX, row, statValuesX); // 1 row - if (feature(FEATURE_CURRENT_METER))row = drawStat_UsedEnergy(statNameX, row, statValuesX); // 1 row - if (feature(FEATURE_CURRENT_METER) && feature(FEATURE_GPS)) row = drawStat_AverageEfficiency(statNameX, row, statValuesX, false); // 1 row - if (feature(FEATURE_GPS))row = drawStat_GPS(statNameX, row, statValuesX); // 1 row - break; - case 1: - // Max 10 rows - row = drawStat_RXStats(statNameX, row, statValuesX); // 1 row if non-CRSF else 2 rows - if (STATE(ESC_SENSOR_ENABLED)) row = drawStat_ESCTemperature(statNameX, row, statValuesX); // 1 row - row = drawStat_GForce(statNameX, row, statValuesX); // 1 row HD or 2 rows SD - if (osdConfig()->stats_show_metric_efficiency && osdIsNotMetric() && feature(FEATURE_CURRENT_METER) && feature(FEATURE_GPS)) row = drawStat_AverageEfficiency(statNameX, row, statValuesX, true); // 1 row -#ifdef USE_BLACKBOX -#ifdef USE_SDCARD - if (feature(FEATURE_BLACKBOX)) { - char buff[12]; - displayWrite(osdDisplayPort, statNameX, row, "BLACKBOX FILE"); - - tfp_sprintf(buff, ": %u/%u", stats.min_sats, stats.max_sats); - - int32_t logNumber = blackboxGetLogNumber(); - if (logNumber >= 0) - tfp_sprintf(buff, ": %05ld ", logNumber); - else - strcat(buff, ": INVALID"); - - displayWrite(osdDisplayPort, statValuesX, row++, buff); // 1 row + if (page == 0) { + // Max 10 rows + row = drawStat_FlightTime(statNameX, row, statValuesX); // 1 row + row = drawStat_FlightDistance(statNameX, row, statValuesX); // 1 row + if (feature(FEATURE_GPS)) row = drawStat_MaxDistanceFromHome(statNameX, row, statValuesX); // 1 row + if (feature(FEATURE_GPS)) row = drawStat_Speed(statNameX, row, statValuesX); // 1 row + row = drawStat_MaximumAltitude(statNameX, row, statValuesX); // 1 row + row = drawStat_BatteryVoltage(statNameX, row, statValuesX); // 1 row + if (feature(FEATURE_CURRENT_METER)) row = drawStat_MaximumPowerAndCurrent(statNameX, row, statValuesX); // 1 row + if (feature(FEATURE_CURRENT_METER))row = drawStat_UsedEnergy(statNameX, row, statValuesX); // 1 row + if (feature(FEATURE_CURRENT_METER) && feature(FEATURE_GPS)) row = drawStat_AverageEfficiency(statNameX, row, statValuesX, false); // 1 row + if (feature(FEATURE_GPS))row = drawStat_GPS(statNameX, row, statValuesX); // 1 row + } else { + // Max 10 rows + row = drawStat_RXStats(statNameX, row, statValuesX); // 1 row if non-CRSF else 2 rows + if (STATE(ESC_SENSOR_ENABLED)) row = drawStat_ESCTemperature(statNameX, row, statValuesX); // 1 row + row = drawStat_GForce(statNameX, row, statValuesX); // 1 row HD or 2 rows SD + if (osdConfig()->stats_show_metric_efficiency && osdIsNotMetric() && feature(FEATURE_CURRENT_METER) && feature(FEATURE_GPS)) row = drawStat_AverageEfficiency(statNameX, row, statValuesX, true); // 1 row +#if defined(USE_BLACKBOX) && defined(USE_SDCARD) + if (feature(FEATURE_BLACKBOX)) { + char buff[12]; + displayWrite(osdDisplayPort, statNameX, row, "BLACKBOX FILE"); + + tfp_sprintf(buff, ": %u/%u", stats.min_sats, stats.max_sats); + + int32_t logNumber = blackboxGetLogNumber(); + if (logNumber >= 0) { + tfp_sprintf(buff, ": %05ld ", logNumber); + } else { + strcat(buff, ": INVALID"); } -#endif + + displayWrite(osdDisplayPort, statValuesX, row++, buff); // 1 row + } #endif #ifdef USE_STATS - if (row < (osdDisplayPort->cols-7) && statsConfig()->stats_enabled) row = drawStat_Stats(statNameX, row, statValuesX, false); // 4 rows + if (row < (osdDisplayPort->rows - 6) && statsConfig()->stats_enabled) row = drawStat_Stats(statNameX, row, statValuesX, false); // 4 rows #endif - - break; } } - row = drawStat_DisarmMethod(statNameX, row, statValuesX); + row = drawStat_DisarmMethod(statNameX, row, statValuesX); // 1 row // The following has been commented out as it will be added in #9688 // uint16_t rearmMs = (emergInflightRearmEnabled()) ? emergencyInFlightRearmTimeMS() : 0; - if (savingSettings == true) { + // Adds 1 row + if (savingSettings) { displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(OSD_MESSAGE_STR(OSD_MSG_SAVING_SETTNGS))) / 2, row++, OSD_MESSAGE_STR(OSD_MSG_SAVING_SETTNGS)); /*} else if (rearmMs > 0) { // Show rearming time if settings not actively being saved. Ignore the settings saved message if rearm available. char emReArmMsg[23]; @@ -5494,11 +5515,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) strcat(emReArmMsg, " **\0"); displayWrite(osdDisplayPort, statNameX, top++, OSD_MESSAGE_STR(emReArmMsg));*/ } else if (notify_settings_saved > 0) { - if (millis() > notify_settings_saved) { - notify_settings_saved = 0; - } else { - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(OSD_MESSAGE_STR(OSD_MSG_SETTINGS_SAVED))) / 2, row++, OSD_MESSAGE_STR(OSD_MSG_SETTINGS_SAVED)); - } + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(OSD_MESSAGE_STR(OSD_MSG_SETTINGS_SAVED))) / 2, row++, OSD_MESSAGE_STR(OSD_MSG_SETTINGS_SAVED)); } displayCommitTransaction(osdDisplayPort); @@ -5545,14 +5562,12 @@ static void osdShowHDArmScreen(void) memset(buf, '\0', sizeof(buf)); memset(buf2, '\0', sizeof(buf2)); -#if defined(USE_GPS) -#if defined (USE_SAFE_HOME) +#if defined(USE_GPS) && defined(USE_SAFE_HOME) if (posControl.safehomeState.distance) { safehomeRow = armScreenRow; armScreenRow +=2; } -#endif // USE_SAFE_HOME -#endif // USE_GPS +#endif // USE_GPS && USE_SAFE_HOME if (posControl.waypointListValid && posControl.waypointCount > 0) { #ifdef USE_MULTI_MISSION @@ -5653,15 +5668,12 @@ static void osdShowSDArmScreen(void) strcpy(buf, "! ARMED !"); displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); memset(buf, '\0', sizeof(buf)); -#if defined(USE_GPS) -#if defined (USE_SAFE_HOME) +#if defined(USE_GPS) && defined(USE_SAFE_HOME) if (posControl.safehomeState.distance) { safehomeRow = armScreenRow; armScreenRow += 2; } #endif -#endif - memset(buf2, '\0', sizeof(buf2)); if (strlen(systemConfig()->pilotName) > 0) { osdFormatPilotName(buf2); @@ -5859,35 +5871,37 @@ static void osdRefresh(timeUs_t currentTimeUs) return; } - bool statsSinglePageCompatible = (osdDisplayPort->rows >= OSD_STATS_SINGLE_PAGE_MIN_ROWS); static uint8_t statsCurrentPage = 0; static bool statsDisplayed = false; - static bool statsAutoPagingEnabled = true; + static bool statsAutoPagingEnabled = false; static bool isThrottleHigh = false; + bool statsSinglePageCompatible = osdDisplayPort->rows >= OSD_STATS_SINGLE_PAGE_MIN_ROWS; + bool updateShowStats = false; // Detect arm/disarm if (armState != ARMING_FLAG(ARMED)) { if (ARMING_FLAG(ARMED)) { // Display the "Arming" screen statsDisplayed = false; - if (!STATE(IN_FLIGHT_EMERG_REARM)) - osdResetStats(); + if (!STATE(IN_FLIGHT_EMERG_REARM)) osdResetStats(); osdShowArmed(); uint16_t delay = osdConfig()->arm_screen_display_time; - if (STATE(IN_FLIGHT_EMERG_REARM)) + if (STATE(IN_FLIGHT_EMERG_REARM)) { delay = 500; #if defined(USE_SAFE_HOME) - else if (posControl.safehomeState.distance) + } else if (posControl.safehomeState.distance) { delay += 3000; + #endif + } osdSetNextRefreshIn(delay); } else { - // Display the "Stats" screen + // Setup display of the "Stats" screen statsDisplayed = true; statsCurrentPage = 0; - statsAutoPagingEnabled = osdConfig()->stats_page_auto_swap_time > 0 ? true : false; - osdShowStats(statsSinglePageCompatible, statsCurrentPage); + statsAutoPagingEnabled = !statsSinglePageCompatible && osdConfig()->stats_page_auto_swap_time > 0 ? true : false; + updateShowStats = true; osdSetNextRefreshIn(STATS_SCREEN_DISPLAY_TIME); isThrottleHigh = checkStickPosition(THR_HI); } @@ -5898,27 +5912,15 @@ static void osdRefresh(timeUs_t currentTimeUs) // This block is entered when we're showing the "Splash", "Armed" or "Stats" screens if (resumeRefreshAt) { - // Handle events only when the "Stats" screen is being displayed. + // Handle events only when the "multi-page Stats" screen is being displayed. if (statsDisplayed) { - - // Manual paging stick commands are only applicable to multi-page stats. - // ****************************** - // For single-page stats, this effectively disables the ability to cancel the - // automatic paging/updates with the stick commands. So unless stats_page_auto_swap_time - // is set to 0 or greater than 4 (saved settings display interval is 5 seconds), then - // "Saved Settings" should display if it is active within the refresh interval. - // ****************************** - // With multi-page stats, "Saved Settings" could also be missed if the user - // has canceled automatic paging using the stick commands, because that is only - // updated when osdShowStats() is called. So, in that case, they would only see - // the "Saved Settings" message if they happen to manually change pages using the - // stick commands within the interval the message is displayed. - bool manualPageUpRequested = false; - bool manualPageDownRequested = false; if (!statsSinglePageCompatible) { + bool manualPageUpRequested = false; + bool manualPageDownRequested = false; + // These methods ensure the paging stick commands are held for a brief period // Otherwise it can result in a race condition where the stats are - // updated too quickly and can result in partial blanks, etc. + // updated too quickly and can result in partial blanks etc. if (osdIsPageUpStickCommandHeld()) { manualPageUpRequested = true; statsAutoPagingEnabled = false; @@ -5926,38 +5928,54 @@ static void osdRefresh(timeUs_t currentTimeUs) manualPageDownRequested = true; statsAutoPagingEnabled = false; } - } - if (statsAutoPagingEnabled) { - // Alternate screens for multi-page stats. - // Also, refreshes screen at swap interval for single-page stats. - if (OSD_ALTERNATING_CHOICES((osdConfig()->stats_page_auto_swap_time * 1000), 2)) { - if (statsCurrentPage == 0) { - osdShowStats(statsSinglePageCompatible, statsCurrentPage); - statsCurrentPage = 1; + if (statsAutoPagingEnabled) { + // Auto alternate screens + if (OSD_ALTERNATING_CHOICES((osdConfig()->stats_page_auto_swap_time * 1000), 2)) { + if (statsCurrentPage == 0) { + osdShowStats(statsSinglePageCompatible, statsCurrentPage); + statsCurrentPage = 1; + } + } else { + if (statsCurrentPage == 1) { + osdShowStats(statsSinglePageCompatible, statsCurrentPage); + statsCurrentPage = 0; + } } } else { - if (statsCurrentPage == 1) { - osdShowStats(statsSinglePageCompatible, statsCurrentPage); + // Process manual page change events + if (manualPageUpRequested) { + updateShowStats = true; + statsCurrentPage = 1; + } else if (manualPageDownRequested) { + updateShowStats = true; statsCurrentPage = 0; } } - } else { - // Process manual page change events for multi-page stats. - if (manualPageUpRequested) { - osdShowStats(statsSinglePageCompatible, 1); - statsCurrentPage = 1; - } else if (manualPageDownRequested) { - osdShowStats(statsSinglePageCompatible, 0); - statsCurrentPage = 0; + } + + // Process Save Settings messages + static bool saveSettingsToggle = false; + if (notify_settings_saved) { + if (!saveSettingsToggle) { + updateShowStats = true; + saveSettingsToggle = true; } + if (millis() > notify_settings_saved) notify_settings_saved = 0; + } else if (saveSettingsToggle) { + updateShowStats = true; + saveSettingsToggle = false; + } + + // Update stats page display only when required + if (!statsAutoPagingEnabled && updateShowStats) { + osdShowStats(statsSinglePageCompatible, statsCurrentPage); } } // Handle events when either "Splash", "Armed" or "Stats" screens are displayed. if (currentTimeUs > resumeRefreshAt || (OSD_RESUME_UPDATES_STICK_COMMAND && !isThrottleHigh)) { - // Time elapsed or canceled by stick commands. - // Exit to normal OSD operation. + // Time elapsed or canceled by stick commands. Exit to normal OSD operation. displayClearScreen(osdDisplayPort); resumeRefreshAt = 0; statsDisplayed = false; @@ -6406,192 +6424,6 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter return elemAttr; } -static bool osdCheckWarning(bool condition, uint8_t warningFlag) -{ - static timeMs_t newWarningEndTime = 0; - static uint8_t newWarningFlags = 0; // bitfield - const timeMs_t currentTimeMs = millis(); - - /* New warnings dislayed individually for 10s with blinking after which - * all current warnings displayed without blinking on 1 second cycle */ - if (condition) { // condition required to trigger warning - if (!(multiFunctionWarning.osdWarningsFlags & warningFlag)) { // check for new warnings - multiFunctionWarning.osdWarningsFlags |= warningFlag; - newWarningFlags |= warningFlag; - newWarningEndTime = currentTimeMs + 10000; - multiFunctionWarning.newWarningActive = true; - } -#ifdef USE_DEV_TOOLS - if (systemConfig()->groundTestMode) { - return true; - } -#endif - if (currentTimeMs < newWarningEndTime) { - return (newWarningFlags & warningFlag); // filter out new warnings excluding older warnings - } else { - newWarningFlags = 0; - multiFunctionWarning.newWarningActive = false; - } - return true; - } else if (multiFunctionWarning.osdWarningsFlags & warningFlag) { - multiFunctionWarning.osdWarningsFlags &= ~warningFlag; - } - - return false; -} - -static textAttributes_t osdGetMultiFunctionMessage(char *buff) -{ - /* Message length limit 10 char max */ - - textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE; - const char *message = NULL; - -#ifdef USE_MULTI_FUNCTIONS - /* --- FUNCTIONS --- */ - multi_function_e selectedFunction = multiFunctionSelection(); - - if (selectedFunction) { - multi_function_e activeFunction = selectedFunction; - - switch (selectedFunction) { - case MULTI_FUNC_NONE: - case MULTI_FUNC_1: - message = posControl.flags.manualEmergLandActive ? "ABORT LAND" : "EMERG LAND"; - break; - case MULTI_FUNC_2: -#if defined(USE_SAFE_HOME) - if (navConfig()->general.flags.safehome_usage_mode != SAFEHOME_USAGE_OFF) { - message = MULTI_FUNC_FLAG(MF_SUSPEND_SAFEHOMES) ? "USE SFHOME" : "SUS SFHOME"; - break; - } -#endif - activeFunction++; - FALLTHROUGH; - case MULTI_FUNC_3: - if (navConfig()->general.flags.rth_trackback_mode != RTH_TRACKBACK_OFF) { - message = MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK) ? "USE TKBACK" : "SUS TKBACK"; - break; - } - activeFunction++; - FALLTHROUGH; - case MULTI_FUNC_4: -#ifdef USE_DSHOT - if (STATE(MULTIROTOR)) { - message = MULTI_FUNC_FLAG(MF_TURTLE_MODE) ? "END TURTLE" : "USE TURTLE"; - break; - } -#endif - activeFunction++; - FALLTHROUGH; - case MULTI_FUNC_5: - message = ARMING_FLAG(ARMED) ? "NOW ARMED " : "EMERG ARM "; - break; - case MULTI_FUNC_END: - break; - } - - if (activeFunction != selectedFunction) { - setMultifunctionSelection(activeFunction); - } - - strcpy(buff, message); - - if (isNextMultifunctionItemAvailable()) { - // provides feedback indicating when a new selection command has been received by flight controller - buff[9] = '>'; - } - - return elemAttr; - } -#endif // MULTIFUNCTION - functions only, warnings always defined - - /* --- WARNINGS --- */ - const char *messages[8]; - uint8_t messageCount = 0; - bool warningCondition = false; - uint8_t warningFlagID = 1; - - // Low Battery Voltage - const batteryState_e batteryVoltageState = checkBatteryVoltageState(); - warningCondition = batteryVoltageState == BATTERY_CRITICAL || batteryVoltageState == BATTERY_WARNING; - if (osdCheckWarning(warningCondition, warningFlagID)) { - messages[messageCount++] = batteryVoltageState == BATTERY_CRITICAL ? "VBATT LAND" : "VBATT LOW "; - } - - // Low Battery Capacity - if (batteryUsesCapacityThresholds()) { - const batteryState_e batteryState = getBatteryState(); - warningCondition = batteryState == BATTERY_CRITICAL || batteryState == BATTERY_WARNING; - if (osdCheckWarning(warningCondition, warningFlagID <<= 1)) { - messages[messageCount++] = batteryState == BATTERY_CRITICAL ? "BATT EMPTY" : "BATT DYING"; - } - } -#if defined(USE_GPS) - // GPS Fix and Failure - if (feature(FEATURE_GPS)) { - if (osdCheckWarning(!STATE(GPS_FIX), warningFlagID <<= 1)) { - bool gpsFailed = getHwGPSStatus() == HW_SENSOR_UNAVAILABLE; - messages[messageCount++] = gpsFailed ? "GPS FAILED" : "NO GPS FIX"; - } - } - - // RTH sanity (warning if RTH heads 200m further away from home than closest point) - warningCondition = NAV_Status.state == MW_NAV_STATE_RTH_ENROUTE && !posControl.flags.rthTrackbackActive && - (posControl.homeDistance - posControl.rthSanityChecker.minimalDistanceToHome) > 20000; - if (osdCheckWarning(warningCondition, warningFlagID <<= 1)) { - messages[messageCount++] = "RTH SANITY"; - } - - // Altitude sanity (warning if significant mismatch between estimated and GPS altitude) - if (osdCheckWarning(posControl.flags.gpsCfEstimatedAltitudeMismatch, warningFlagID <<= 1)) { - messages[messageCount++] = "ALT SANITY"; - } -#endif - -#if defined(USE_MAG) - // Magnetometer failure - if (requestedSensors[SENSOR_INDEX_MAG] != MAG_NONE) { - hardwareSensorStatus_e magStatus = getHwCompassStatus(); - if (osdCheckWarning(magStatus == HW_SENSOR_UNAVAILABLE || magStatus == HW_SENSOR_UNHEALTHY, warningFlagID <<= 1)) { - messages[messageCount++] = "MAG FAILED"; - } - } -#endif - -#if defined(USE_PITOT) - // Pitot sensor validation failure (blocked/failed pitot tube) - if (sensors(SENSOR_PITOT) && detectedSensors[SENSOR_INDEX_PITOT] != PITOT_VIRTUAL) { - if (osdCheckWarning(pitotHasFailed(), warningFlagID <<= 1)) { - messages[messageCount++] = "PITOT FAIL"; - } - } -#endif - - // Vibration levels TODO - needs better vibration measurement to be useful - // const float vibrationLevel = accGetVibrationLevel(); - // warningCondition = vibrationLevel > 1.5f; - // if (osdCheckWarning(warningCondition, warningFlagID <<= 1, &warningsCount)) { - // messages[messageCount++] = vibrationLevel > 2.5f ? "BAD VIBRTN" : "VIBRATION!"; - // } - -#ifdef USE_DEV_TOOLS - if (osdCheckWarning(systemConfig()->groundTestMode, warningFlagID <<= 1)) { - messages[messageCount++] = "GRD TEST !"; - } -#endif - - if (messageCount) { - message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)]; // display each warning on 1s cycle - strcpy(buff, message); - if (multiFunctionWarning.newWarningActive) { - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - } - - return elemAttr; -} - void osdDrawCustomItem(uint8_t item){ osdDrawSingleElement(item); } diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 0b893916895..e50115d99ed 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -116,6 +116,8 @@ #define MSP2_INAV_CUSTOM_OSD_ELEMENT 0x2101 #define MSP2_INAV_SET_CUSTOM_OSD_ELEMENTS 0x2102 +#define MSP2_INAV_GET_LINK_STATS 0x2103 + #define MSP2_INAV_SERVO_CONFIG 0x2200 #define MSP2_INAV_SET_SERVO_CONFIG 0x2201 @@ -126,3 +128,8 @@ #define MSP2_INAV_SET_GVAR 0x2214 #define MSP2_INAV_FULL_LOCAL_POSE 0x2220 + +#define MSP2_INAV_SET_WP_INDEX 0x2221 //in message jump to waypoint N during active WP mission; payload: U8 wp_index (0-based, relative to mission start) +#define MSP2_INAV_SET_CRUISE_HEADING 0x2223 //in message set heading while in Cruise/Course Hold mode; payload: I32 heading_centidegrees (0-35999) + +#define MSP2_INAV_SET_AUX_RC 0x2230 \ No newline at end of file diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index e7f0774925e..03d4a3da36b 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -785,6 +785,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP] = NAV_STATE_WAYPOINT_PRE_ACTION, // MSP2_INAV_SET_WP_INDEX } }, @@ -806,6 +807,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP] = NAV_STATE_WAYPOINT_PRE_ACTION, // MSP2_INAV_SET_WP_INDEX } }, @@ -830,6 +832,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP] = NAV_STATE_WAYPOINT_PRE_ACTION, // MSP2_INAV_SET_WP_INDEX } }, @@ -851,6 +854,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP] = NAV_STATE_WAYPOINT_PRE_ACTION, // MSP2_INAV_SET_WP_INDEX } }, @@ -909,6 +913,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP] = NAV_STATE_WAYPOINT_PRE_ACTION, // MSP2_INAV_SET_WP_INDEX } }, @@ -3876,6 +3881,69 @@ int isGCSValid(void) (posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)); } +/* + * navSetActiveWaypointIndex - MSP2_INAV_SET_WP_INDEX handler + * + * Jumps to a specific waypoint during an active WP mission without interrupting + * navigation mode. 'index' is 0-based and relative to the mission start + * (i.e. the first waypoint in the loaded mission is index 0, regardless of + * startWpIndex). + * + * Returns true on success, false when the preconditions are not met (not armed, + * not in WP mode, or index out of range). + */ +bool navSetActiveWaypointIndex(uint8_t index) +{ + // Must be armed and actively executing a WP mission + if (!ARMING_FLAG(ARMED) || !FLIGHT_MODE(NAV_WP_MODE)) { + return false; + } + + // Translate user-visible 0-based index to the internal absolute index + int8_t absoluteIndex = (int8_t)index + posControl.startWpIndex; + if (absoluteIndex < posControl.startWpIndex || + absoluteIndex >= posControl.startWpIndex + posControl.waypointCount) { + return false; + } + + posControl.activeWaypointIndex = absoluteIndex; + posControl.wpMissionRestart = false; + + // Transition immediately to WAYPOINT_PRE_ACTION so the new WP is set up + // on this navigation tick. navProcessFSMEvents is safe to call here as + // everything runs in the same main-loop task context. + navProcessFSMEvents(NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP); + return true; +} + +/* + * navSetCruiseHeading - MSP2_INAV_SET_CRUISE_HEADING handler + * + * Sets the target heading while Cruise or Course Hold mode is active. + * 'headingCd' is in centidegrees (0-35999), matching the internal + * posControl.cruise.course representation. + * + * Returns true on success, false when the preconditions are not met. + */ +bool navSetCruiseHeading(int32_t headingCd) +{ + if (!ARMING_FLAG(ARMED)) { + return false; + } + + // Only valid while Cruise or Course Hold is the active navigation mode + if (!FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { + return false; + } + + // Clamp to valid centidegree range + headingCd = ((headingCd % 36000) + 36000) % 36000; + + posControl.cruise.course = headingCd; + posControl.cruise.previousCourse = headingCd; + return true; +} + void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData) { gpsLocation_t wpLLH; diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index ddc7e23e76d..eb1621e9f8d 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -708,6 +708,8 @@ int isGCSValid(void); void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData); void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData); void resetWaypointList(void); +bool navSetActiveWaypointIndex(uint8_t index); // MSP2_INAV_SET_WP_INDEX: jump to WP during active mission +bool navSetCruiseHeading(int32_t headingCd); // MSP2_INAV_SET_CRUISE_HEADING: set cruise/course-hold heading (centidegrees) bool loadNonVolatileWaypointList(bool clearIfLoaded); bool saveNonVolatileWaypointList(void); #ifdef USE_MULTI_MISSION diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index f6cf9329e98..a1f07e470c0 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -178,6 +178,7 @@ typedef enum { NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME = NAV_FSM_EVENT_STATE_SPECIFIC_1, NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND = NAV_FSM_EVENT_STATE_SPECIFIC_2, NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED = NAV_FSM_EVENT_STATE_SPECIFIC_3, + NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP = NAV_FSM_EVENT_STATE_SPECIFIC_4, // jump to a different WP index mid-mission NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE = NAV_FSM_EVENT_STATE_SPECIFIC_1, NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK = NAV_FSM_EVENT_STATE_SPECIFIC_2, NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME = NAV_FSM_EVENT_STATE_SPECIFIC_3, diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h index 8d84d52b8e5..d5c0219f106 100755 --- a/src/main/rx/crsf.h +++ b/src/main/rx/crsf.h @@ -46,6 +46,7 @@ enum { CRSF_FRAME_VARIO_SENSOR_PAYLOAD_SIZE = 2, CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE = 8, CRSF_FRAME_BAROMETER_ALTITUDE_PAYLOAD_SIZE = 2, + CRSF_FRAME_AIRSPEED_PAYLOAD_SIZE = 2, CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE = 10, CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE = 22, // 11 bits per channel * 16 channels = 22 bytes. CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE = 6, @@ -88,6 +89,9 @@ typedef enum { CRSF_FRAMETYPE_VARIO_SENSOR = 0x07, CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08, CRSF_FRAMETYPE_BAROMETER_ALTITUDE = 0x09, + CRSF_FRAMETYPE_AIRSPEED_SENSOR = 0x0A, + CRSF_FRAMETYPE_RPM = 0x0C, + CRSF_FRAMETYPE_TEMP = 0x0D, CRSF_FRAMETYPE_LINK_STATISTICS = 0x14, CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16, CRSF_FRAMETYPE_ATTITUDE = 0x1E, diff --git a/src/main/rx/msp.c b/src/main/rx/msp.c index b338fbc2d97..e2d310423a6 100644 --- a/src/main/rx/msp.c +++ b/src/main/rx/msp.c @@ -31,6 +31,7 @@ static uint16_t mspFrame[MAX_SUPPORTED_RC_CHANNEL_COUNT]; static bool rxMspFrameDone = false; +static uint8_t mspLastChannelCount = 0; static uint16_t rxMspReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t chan) { @@ -49,9 +50,15 @@ void rxMspFrameReceive(uint16_t *frame, int channelCount) mspFrame[i] = 0; } + mspLastChannelCount = channelCount; rxMspFrameDone = true; } +uint8_t rxMspGetLastChannelCount(void) +{ + return mspLastChannelCount; +} + static uint8_t rxMspFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) { UNUSED(rxRuntimeConfig); diff --git a/src/main/rx/msp.h b/src/main/rx/msp.h index 78a9bfd7132..c99fe44d641 100644 --- a/src/main/rx/msp.h +++ b/src/main/rx/msp.h @@ -21,3 +21,4 @@ void rxMspFrameReceive(uint16_t *frame, int channelCount); void rxMspInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); +uint8_t rxMspGetLastChannelCount(void); diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 0531904d063..b897238ed1d 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -94,6 +94,9 @@ static bool isRxSuspended = false; static rcChannel_t rcChannels[MAX_SUPPORTED_RC_CHANNEL_COUNT]; +// MSP aux channel overlay: non-zero values override rcChannels[].data for CH9-CH32 +static uint16_t mspAuxOverlay[MAX_SUPPORTED_RC_CHANNEL_COUNT]; + rxLinkStatistics_t rxLinkStatistics; rxRuntimeConfig_t rxRuntimeConfig; static uint8_t rcSampleIndex = 0; @@ -512,6 +515,31 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs) } #endif + // Apply MSP aux channel overlay (CH13-CH32) + { + int overlayStart = 12; +#ifdef USE_RX_MSP + // When MSP is the primary RX, skip channels covered by MSP_SET_RAW_RC + if (rxConfig()->receiverType == RX_TYPE_MSP) { + const uint8_t mspChannels = rxMspGetLastChannelCount(); + if (mspChannels > overlayStart) { + overlayStart = mspChannels; + } + } +#endif + for (int i = overlayStart; i < 32; i++) { + if (mspAuxOverlay[i] > 0) { +#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) + // Skip channels controlled by MSP RC Override when active + if (IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) && (rxConfig()->mspOverrideChannels & (1U << i))) { + continue; + } +#endif + rcChannels[i].data = mspAuxOverlay[i]; + } + } + } + // Update failsafe if (rxFlightChannelsValid && rxSignalReceived) { failsafeOnValidDataReceived(); @@ -663,6 +691,13 @@ int16_t rxGetChannelValue(unsigned channelNumber) } } +void rxMspAuxOverlaySet(uint8_t channelIndex, uint16_t value) +{ + if (channelIndex >= 12 && channelIndex < 32) { + mspAuxOverlay[channelIndex] = value; + } +} + void lqTrackerReset(rxLinkQualityTracker_e * lqTracker) { lqTracker->lastUpdatedMs = millis(); diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 3ed6add3e48..5b5bcea50c3 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -232,3 +232,7 @@ void resumeRxSignal(void); // filtering and some extra processing like value holding // during failsafe. int16_t rxGetChannelValue(unsigned channelNumber); + +// MSP aux channel overlay (CH13-CH32). Sets a channel value that persists +// across RX update cycles. value=0 ignores that channel and skips it. +void rxMspAuxOverlaySet(uint8_t channelIndex, uint16_t value); diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c index e04a92a8993..ad21c32359e 100755 --- a/src/main/scheduler/scheduler.c +++ b/src/main/scheduler/scheduler.c @@ -19,6 +19,10 @@ #include #include +#if defined(SITL_BUILD) +#include +#endif + #include "platform.h" #include "scheduler.h" @@ -215,11 +219,22 @@ void FAST_CODE NOINLINE scheduler(void) uint16_t selectedTaskDynamicPriority = 0; bool forcedRealTimeTask = false; +#if defined(SITL_BUILD) + // Track the earliest time at which the next task will become due so we can + // sleep until then instead of busy-waiting. Cap at 1 ms so event-driven + // tasks (checkFunc) are still polled frequently enough. + timeUs_t sitlEarliestNextTaskAt = currentTimeUs + 1000; + bool sitlHasCheckFuncTask = false; +#endif + // Update task dynamic priorities uint16_t waitingTasks = 0; for (cfTask_t *task = queueFirst(); task != NULL; task = queueNext()) { // Task has checkFunc - event driven if (task->checkFunc) { +#if defined(SITL_BUILD) + sitlHasCheckFuncTask = true; +#endif const timeUs_t currentTimeBeforeCheckFuncCallUs = micros(); // Increase priority for event driven tasks @@ -248,7 +263,19 @@ void FAST_CODE NOINLINE scheduler(void) waitingTasks++; forcedRealTimeTask = true; } +#if defined(SITL_BUILD) + const timeUs_t taskNextAt = task->lastExecutedAt + (timeUs_t)task->desiredPeriod; + if (taskNextAt < sitlEarliestNextTaskAt) { + sitlEarliestNextTaskAt = taskNextAt; + } +#endif } else { +#if defined(SITL_BUILD) + const timeUs_t taskNextAt = task->lastExecutedAt + (timeUs_t)task->desiredPeriod; + if (taskNextAt < sitlEarliestNextTaskAt) { + sitlEarliestNextTaskAt = taskNextAt; + } +#endif // Task is time-driven, dynamicPriority is last execution age (measured in desiredPeriods) // Task age is calculated from last execution task->taskAgeCycles = ((timeDelta_t)(currentTimeUs - task->lastExecutedAt)) / task->desiredPeriod; @@ -294,4 +321,27 @@ void FAST_CODE NOINLINE scheduler(void) selectedTask->totalExecutionTime += taskExecutionTime; // time consumed by scheduler + task selectedTask->maxExecutionTime = MAX(selectedTask->maxExecutionTime, taskExecutionTime); } + +#if defined(SITL_BUILD) + { + // Avoid busy-waiting and burning 100% CPU in SITL. After executing the + // current task (or finding nothing to do), sleep until just before the + // next task is due. For event-driven tasks (checkFunc) we limit the + // sleep so the check function is still called frequently. + if (sitlHasCheckFuncTask) { + // Poll event-driven tasks at least every 500 µs + const timeUs_t eventCap = micros() + 500; + if (eventCap < sitlEarliestNextTaskAt) { + sitlEarliestNextTaskAt = eventCap; + } + } + const timeUs_t nowUs = micros(); + if (sitlEarliestNextTaskAt > nowUs + 50) { + const timeDelta_t sleepUs = (timeDelta_t)(sitlEarliestNextTaskAt - nowUs) - 50; + if (sleepUs > 0) { + usleep((useconds_t)sleepUs); + } + } + } +#endif } diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 16d10624c69..cc52be637a9 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -45,6 +45,7 @@ #include "drivers/accgyro/accgyro_bmi270.h" #include "drivers/accgyro/accgyro_icm20689.h" #include "drivers/accgyro/accgyro_icm42605.h" +#include "drivers/accgyro/accgyro_icm45686.h" #include "drivers/accgyro/accgyro_lsm6dxx.h" #include "drivers/accgyro/accgyro_fake.h" #include "drivers/sensor.h" @@ -247,6 +248,18 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse) } FALLTHROUGH; #endif +#ifdef USE_IMU_ICM45686 + case ACC_ICM45686: + if (icm45686AccDetect(dev)) { + accHardware = ACC_ICM45686; + break; + } + /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */ + if (accHardwareToUse != ACC_AUTODETECT) { + break; + } + FALLTHROUGH; +#endif #ifdef USE_IMU_FAKE case ACC_FAKE: if (fakeAccDetect(dev)) { @@ -586,8 +599,8 @@ void accUpdate(void) // calc difference from this sample and 5hz filtered value, square and filter at 2hz const float accDiff = acc.accADCf[axis] - accFloorFilt; acc.accVibeSq[axis] = pt1FilterApply(&accVibeFilter[axis], accDiff * accDiff); - acc.accVibe = fast_fsqrtf(acc.accVibeSq[X] + acc.accVibeSq[Y] + acc.accVibeSq[Z]); } + acc.accVibe = fast_fsqrtf(acc.accVibeSq[X] + acc.accVibeSq[Y] + acc.accVibeSq[Z]); // Filter acceleration for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index 2e2bfa461e0..ea76b63ca75 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -44,6 +44,7 @@ typedef enum { ACC_ICM42605, ACC_BMI270, ACC_LSM6DXX, + ACC_ICM45686, ACC_FAKE, ACC_MAX = ACC_FAKE } accelerationSensor_e; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 9fa0a7c0e4d..8544b45dac1 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -47,6 +47,7 @@ #include "drivers/accgyro/accgyro_bmi270.h" #include "drivers/accgyro/accgyro_icm20689.h" #include "drivers/accgyro/accgyro_icm42605.h" +#include "drivers/accgyro/accgyro_icm45686.h" #include "drivers/accgyro/accgyro_lsm6dxx.h" #include "drivers/accgyro/accgyro_fake.h" #include "drivers/io.h" @@ -228,6 +229,15 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard FALLTHROUGH; #endif +#ifdef USE_IMU_ICM45686 + case GYRO_ICM45686: + if (icm45686GyroDetect(dev)) { + gyroHardware = GYRO_ICM45686; + break; + } + FALLTHROUGH; +#endif + #ifdef USE_IMU_FAKE case GYRO_FAKE: if (fakeGyroDetect(dev)) { diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index de78b2f81c2..18fe5d9e517 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -43,8 +43,8 @@ typedef enum { GYRO_ICM42605, GYRO_BMI270, GYRO_LSM6DXX, + GYRO_ICM45686, GYRO_FAKE - } gyroSensor_e; typedef enum { diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c index b4b61f57669..3618923614a 100755 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -31,6 +31,7 @@ #include "drivers/pitotmeter/pitotmeter.h" #include "drivers/pitotmeter/pitotmeter_ms4525.h" +#include "drivers/pitotmeter/pitotmeter_ms5525.h" #include "drivers/pitotmeter/pitotmeter_dlvr_l10d.h" #include "drivers/pitotmeter/pitotmeter_adc.h" #include "drivers/pitotmeter/pitotmeter_msp.h" @@ -111,6 +112,19 @@ bool pitotDetect(pitotDev_t *dev, uint8_t pitotHardwareToUse) } FALLTHROUGH; + case PITOT_MS5525: +#ifdef USE_PITOT_MS5525 + if (ms5525Detect(dev)) { + pitotHardware = PITOT_MS5525; + break; + } +#endif + /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */ + if (pitotHardwareToUse != PITOT_AUTODETECT) { + break; + } + FALLTHROUGH; + case PITOT_DLVR: // Skip autodetection for DLVR (it is indistinguishable from MS4525) and allow only manual config @@ -134,14 +148,15 @@ bool pitotDetect(pitotDev_t *dev, uint8_t pitotHardwareToUse) FALLTHROUGH; case PITOT_VIRTUAL: -#if defined(USE_WIND_ESTIMATOR) && defined(USE_PITOT_VIRTUAL) - if ((pitotHardwareToUse != PITOT_AUTODETECT) && virtualPitotDetect(dev)) { - pitotHardware = PITOT_VIRTUAL; - break; - } -#endif - /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */ if (pitotHardwareToUse != PITOT_AUTODETECT) { +#if defined(USE_WIND_ESTIMATOR) && defined(USE_PITOT_VIRTUAL) + if (virtualPitotDetect(dev)) { + pitotHardware = PITOT_VIRTUAL; + break; + } +#endif + // set requested to None to prevent hardware failure if GPS not enabled + requestedSensors[SENSOR_INDEX_PITOT] = PITOT_NONE; break; } FALLTHROUGH; @@ -226,9 +241,10 @@ STATIC_PROTOTHREAD(pitotThread) // Init filter pitot.lastMeasurementUs = micros(); - if(pitotmeterConfig()->pitot_lpf_milli_hz >0){ + if (pitotmeterConfig()->pitot_lpf_milli_hz > 0) { pt1FilterInit(&pitot.lpfState, pitotmeterConfig()->pitot_lpf_milli_hz / 1000.0f, 0.0f); } + while(1) { #ifdef USE_SIMULATOR while (SIMULATOR_HAS_OPTION(HITL_AIRSPEED) && SIMULATOR_HAS_OPTION(HITL_PITOT_FAILURE)) @@ -236,33 +252,33 @@ STATIC_PROTOTHREAD(pitotThread) ptDelayUs(10000); } #endif - - if ( pitot.lastSeenHealthyMs == 0 ) { + if (pitot.lastSeenHealthyMs == 0) { if (pitot.dev.start(&pitot.dev)) { pitot.lastSeenHealthyMs = millis(); - } + } } - if ( (millis() - pitot.lastSeenHealthyMs) >= US2MS(pitot.dev.delay)) { - if (pitot.dev.get(&pitot.dev)) // read current data + if ((millis() - pitot.lastSeenHealthyMs) >= US2MS(pitot.dev.delay)) { + if (pitot.dev.get(&pitot.dev)) { // read current data pitot.lastSeenHealthyMs = millis(); + } - if (pitot.dev.start(&pitot.dev)) // init for next read - pitot.lastSeenHealthyMs = millis(); + if (pitot.dev.start(&pitot.dev)) { // init for next read + pitot.lastSeenHealthyMs = millis(); + } } - pitot.dev.calculate(&pitot.dev, &pitotPressureTmp, &pitotTemperatureTmp); #ifdef USE_SIMULATOR if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { - pitotPressureTmp = sq(simulatorData.airSpeed) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE; + pitotPressureTmp = sq(simulatorData.airSpeed) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE; } #endif #if defined(USE_PITOT_FAKE) - if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) { - pitotPressureTmp = sq(fakePitotGetAirspeed()) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE; - } + if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) { + pitotPressureTmp = sq(fakePitotGetAirspeed()) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE; + } #endif ptYield(); @@ -280,9 +296,9 @@ STATIC_PROTOTHREAD(pitotThread) // NOTE ::filter pressure - apply filter when NOT calibrating for zero !!! currentTimeUs = micros(); - if(pitotmeterConfig()->pitot_lpf_milli_hz >0){ + if (pitotmeterConfig()->pitot_lpf_milli_hz > 0) { pitot.pressure = pt1FilterApply3(&pitot.lpfState, pitotPressureTmp, US2S(currentTimeUs - pitot.lastMeasurementUs)); - }else{ + } else { pitot.pressure = pitotPressureTmp; } pitot.lastMeasurementUs = currentTimeUs; @@ -297,7 +313,7 @@ STATIC_PROTOTHREAD(pitotThread) } #if defined(USE_PITOT_FAKE) - if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) { + if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) { pitot.airSpeed = fakePitotGetAirspeed(); } #endif diff --git a/src/main/sensors/pitotmeter.h b/src/main/sensors/pitotmeter.h index 69451098ec8..dc5ac422974 100755 --- a/src/main/sensors/pitotmeter.h +++ b/src/main/sensors/pitotmeter.h @@ -32,6 +32,7 @@ typedef enum { PITOT_FAKE = 5, PITOT_MSP = 6, PITOT_DLVR = 7, + PITOT_MS5525 = 8, } pitotSensor_e; #define PITOT_MAX PITOT_FAKE diff --git a/src/main/target/AIKONF4V2/CMakeLists.txt b/src/main/target/AIKONF4V2/CMakeLists.txt new file mode 100644 index 00000000000..a9393354aea --- /dev/null +++ b/src/main/target/AIKONF4V2/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(AIKONF4V2) diff --git a/src/main/target/AIKONF4V2/config.c b/src/main/target/AIKONF4V2/config.c new file mode 100644 index 00000000000..fcb850b3f53 --- /dev/null +++ b/src/main/target/AIKONF4V2/config.c @@ -0,0 +1,27 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include +#include + +#include + +#include "drivers/pwm_mapping.h" + +void targetConfiguration(void) +{ +} diff --git a/src/main/target/AIKONF4V2/target.c b/src/main/target/AIKONF4V2/target.c new file mode 100644 index 00000000000..54777bc509c --- /dev/null +++ b/src/main/target/AIKONF4V2/target.c @@ -0,0 +1,39 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software 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. + * + * Cleanflight and Betaflight are distributed in the hope that they + * 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 software. + * + * If not, see . + */ + +#include + +#include "platform.h" +#include "drivers/io.h" + +#include "drivers/dma.h" +#include "drivers/timer.h" +#include "drivers/timer_def.h" + +timerHardware_t timerHardware[] = { + // DEF_TIM(TIM2, CH2, PB3, TIM_USE_CAMERA_CONTROL, 0, 0), // There is not camera control in INAV + DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 1), // S1 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), // S3 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // LED +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/AIKONF4V2/target.h b/src/main/target/AIKONF4V2/target.h new file mode 100644 index 00000000000..a93415371bd --- /dev/null +++ b/src/main/target/AIKONF4V2/target.h @@ -0,0 +1,134 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software 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. + * + * Cleanflight and Betaflight are distributed in the hope that they + * 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 software. + * + * If not, see . + */ + +#pragma once + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_BLACKBOX | FEATURE_SOFTSERIAL) + +#define TARGET_BOARD_IDENTIFIER "AIK4" +#define USBD_PRODUCT_STRING "AIKONF4V2" + +// Beeper +#define USE_BEEPER +#define BEEPER PB5 +#define BEEPER_INVERTED +// Leds +#define USE_LED_STRIP +#define WS2811_PIN PB6 +#define LED0 PB4 +// UARTs +#define USB_IO +#define USE_VCP +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 +#define USE_UART2 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 +#define USE_UART4 +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 +#define SERIAL_PORT_COUNT 5 //VCP, UART1, UART2, UART3, UART4 +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART1 +// SPI +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_NSS_PIN PA4 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 +#define USE_SPI_DEVICE_2 +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 +#define USE_SPI_DEVICE_3 +#define SPI3_NSS_PIN PA15 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 +// I2C +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 +#define DEFAULT_I2C_BUS BUS_I2C1 +#define EXTERNAL_I2C_BUS DEFAULT_I2C_BUS +// MAG +#define USE_MAG +#define MAG_I2C_BUS DEFAULT_I2C_BUS +#define USE_MAG_ALL +// ADC +#define USE_ADC +#define ADC_CHANNEL_1_PIN PC1 +#define ADC_CHANNEL_2_PIN PC2 +#define ADC_CHANNEL_3_PIN PC3 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_1 +#define VBAT_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 +#define ADC_INSTANCE ADC1 +// Gyro & ACC +#define USE_IMU_MPU6000 +#define MPU6000_CS_PIN SPI1_NSS_PIN +#define MPU6000_SPI_BUS BUS_SPI1 +#define IMU_MPU6000_ALIGN CW0_DEG +#define USE_IMU_ICM42605 +#define ICM42605_CS_PIN SPI1_NSS_PIN +#define ICM42605_SPI_BUS BUS_SPI1 +#define IMU_ICM42605_ALIGN CW90_DEG +// OSD +#define USE_MAX7456 +#define MAX7456_CS_PIN SPI3_NSS_PIN +#define MAX7456_SPI_BUS BUS_SPI3 +// Baro +#define USE_BARO +#define BARO_I2C_BUS DEFAULT_I2C_BUS +#define USE_BARO_ALL +// Blackbox +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#define M25P16_CS_PIN SPI2_NSS_PIN +#define M25P16_SPI_BUS BUS_SPI2 +#define USE_FLASHFS +#define USE_FLASH_M25P16 +// Others +#define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS +#define PITOT_I2C_BUS DEFAULT_I2C_BUS +#define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS + +#define MAX_PWM_OUTPUT_PORTS 4 +#define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define USE_SERIALSHOT +#define USE_DSHOT +#define USE_ESC_SENSOR +#define VOLTAGE_METER_SCALE 110 +#define CURRENT_METER_SCALE 400 + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff +#define TARGET_IO_PORTF 0xffff + diff --git a/src/main/target/BLUEBERRYF405/target.c b/src/main/target/BLUEBERRYF405/target.c index b712d9f0974..8151b81e77e 100644 --- a/src/main/target/BLUEBERRYF405/target.c +++ b/src/main/target/BLUEBERRYF405/target.c @@ -40,6 +40,7 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S11 D(1,0,2) DEF_TIM(TIM3, CH4, PB1, TIM_USE_LED, 0, 0), // 2812LED D(1,2,5) + DEF_TIM(TIM11, CH1, PB9, TIM_USE_BEEPER, 0, 0), // BEEPER PWM DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), // softserial1_TX (PA2 shared with UART2_TX) }; diff --git a/src/main/target/BLUEBERRYH743/CMakeLists.txt b/src/main/target/BLUEBERRYH743/CMakeLists.txt new file mode 100644 index 00000000000..99d8aeeda11 --- /dev/null +++ b/src/main/target/BLUEBERRYH743/CMakeLists.txt @@ -0,0 +1,2 @@ +target_stm32h743xi(BLUEBERRYH743) +target_stm32h743xi(BLUEBERRYH743HD) diff --git a/src/main/target/BLUEBERRYH743/config.c b/src/main/target/BLUEBERRYH743/config.c new file mode 100644 index 00000000000..0673117c182 --- /dev/null +++ b/src/main/target/BLUEBERRYH743/config.c @@ -0,0 +1,31 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#include + +#include "platform.h" + +#include "fc/fc_msp_box.h" +#include "fc/config.h" +#include "io/serial.h" +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; +} diff --git a/src/main/target/BLUEBERRYH743/target.c b/src/main/target/BLUEBERRYH743/target.c new file mode 100644 index 00000000000..2c924ecac48 --- /dev/null +++ b/src/main/target/BLUEBERRYH743/target.c @@ -0,0 +1,55 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" +#include "drivers/sensor.h" + +BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6000_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_icm42688, DEVHW_ICM42605, MPU6000_SPI_BUS, MPU6000_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6000_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_icm20602, DEVHW_MPU6500, MPU6500_SPI_BUS, MPU6500_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU_MPU6500_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_icm42605, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, NONE, 2, DEVFLAGS_NONE, IMU_ICM42605_ALIGN); + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 + + DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 2), // S3 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 3), // S4 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 4), // S5 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 5), // S6 + + DEF_TIM(TIM4, CH1, PD12, TIM_USE_OUTPUT_AUTO, 0, 6), // S7 + DEF_TIM(TIM4, CH2, PD13, TIM_USE_OUTPUT_AUTO, 0, 7), // S8 + DEF_TIM(TIM4, CH3, PD14, TIM_USE_OUTPUT_AUTO, 0, 0), // S9 + DEF_TIM(TIM4, CH4, PD15, TIM_USE_OUTPUT_AUTO, 0, 0), // S10 DMA_NONE + + DEF_TIM(TIM15, CH1, PE5, TIM_USE_OUTPUT_AUTO, 0, 0), // S11 + DEF_TIM(TIM15, CH2, PE6, TIM_USE_OUTPUT_AUTO, 0, 0), // S12 DMA_NONE + DEF_TIM(TIM2, CH1, PA15, TIM_USE_BEEPER, 0, 0), // BEEPER PWM + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 9), // LED_2812 + +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/BLUEBERRYH743/target.h b/src/main/target/BLUEBERRYH743/target.h new file mode 100644 index 00000000000..b69400d0674 --- /dev/null +++ b/src/main/target/BLUEBERRYH743/target.h @@ -0,0 +1,210 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "BB43" + +#if defined(BLUEBERRYH743HD) + #define USBD_PRODUCT_STRING "BLUEBERRYH743HD" +#else + #define USBD_PRODUCT_STRING "BLUEBERRYH743" +#endif + +#define USE_TARGET_CONFIG + +#define LED0 PE3 +#define LED1 PE4 + +#define BEEPER PA15 +#define BEEPER_INVERTED + +// *************** IMU generic *********************** +#define USE_DUAL_GYRO +#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS + +// *************** SPI1 IMU0 MPU6000 **************** +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PD7 + +#define USE_IMU_MPU6000 + +#define IMU_MPU6000_ALIGN CW0_DEG_FLIP +#define MPU6000_SPI_BUS BUS_SPI1 +#define MPU6000_CS_PIN PC15 + +// *************** SPI4 IMU1 ICM20602 ************** +#define USE_SPI_DEVICE_4 +#define SPI4_SCK_PIN PE12 +#define SPI4_MISO_PIN PE13 +#define SPI4_MOSI_PIN PE14 + +#define USE_IMU_MPU6500 + +#define IMU_MPU6500_ALIGN CW0_DEG_FLIP +#define MPU6500_SPI_BUS BUS_SPI4 +#define MPU6500_CS_PIN PE11 + +// *************** SPI4 IMU2 ICM42605 ************** +#define USE_IMU_ICM42605 + +#define IMU_ICM42605_ALIGN CW90_DEG_FLIP +#define ICM42605_SPI_BUS BUS_SPI4 +#define ICM42605_CS_PIN PC13 + +// *************** SPI2 OSD *********************** + #define USE_SPI_DEVICE_2 + #define SPI2_SCK_PIN PB13 + #define SPI2_MISO_PIN PB14 + #define SPI2_MOSI_PIN PB15 + +#if defined(BLUEBERRYH743) + #define USE_MAX7456 + #define MAX7456_SPI_BUS BUS_SPI2 + #define MAX7456_CS_PIN PB12 +#endif + +// *************** SPI3 SPARE for external RM3100 *********** +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +#define USE_MAG_RM3100 +#define RM3100_CS_PIN PE2 //CS2 pad +// PD4 //CS1 pad +#define RM3100_SPI_BUS BUS_SPI3 + +// *************** I2C /Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB6 +#define I2C1_SDA PB7 + +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C2 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_DPS310 +#define USE_BARO_SPL06 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_ALL + +#define TEMPERATURE_I2C_BUS BUS_I2C2 +#define PITOT_I2C_BUS BUS_I2C2 + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS BUS_I2C1 + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PD5 +#define UART2_RX_PIN PD6 + +#define USE_UART3 +#define UART3_TX_PIN PD8 +#define UART3_RX_PIN PD9 + +#define USE_UART4 +#define UART4_TX_PIN PB9 +#define UART4_RX_PIN PB8 + +#define USE_UART6 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define USE_UART7 +#define UART7_TX_PIN PE8 +#define UART7_RX_PIN PE7 + +#define USE_UART8 +#define UART8_TX_PIN PE1 +#define UART8_RX_PIN PE0 + + +#define SERIAL_PORT_COUNT 8 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART6 + +// *************** SDIO SD BLACKBOX******************* +#define USE_SDCARD +#define USE_SDCARD_SDIO +#define SDCARD_SDIO_DEVICE SDIODEV_1 +#define SDCARD_SDIO_4BIT + +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 + +#define ADC_CHANNEL_1_PIN PC0 //ADC123 VBAT1 +#define ADC_CHANNEL_2_PIN PC1 //ADC123 CURR1 +#define ADC_CHANNEL_3_PIN PC5 //ADC12 RSSI +#define ADC_CHANNEL_4_PIN PC4 //ADC12 AirS +#define ADC_CHANNEL_5_PIN PA4 //ADC12 VB2 +#define ADC_CHANNEL_6_PIN PA7 //ADC12 CU2 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 +#define AIRSPEED_ADC_CHANNEL ADC_CHN_4 + +// *************** PINIO *************************** +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PD10 // VTX power switcher +#define PINIO2_PIN PD11 // 2xCamera switcher + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) +#define CURRENT_METER_SCALE 250 +#define CURRENT_METER_OFFSET 400 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA (0xffff & ~(BIT(14) | BIT(13))) +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff + +#define MAX_PWM_OUTPUT_PORTS 15 +#define USE_DSHOT +#define USE_ESC_SENSOR + diff --git a/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.c b/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.c index d86bbb17379..2962c394a3e 100644 --- a/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.c +++ b/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.c @@ -34,7 +34,7 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM2, CH4, PB11, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 D(1,7,3) UP173 DEF_TIM(TIM2, CH3, PB10, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 D(1,1,3) UP173 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // 2812LED D(1,2,5) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_LED, 0, 0), // 2812LED D(1,2,5) DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), //TX2 softserial1_Tx }; diff --git a/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.h b/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.h index cf98fcaca14..8eecb2c466c 100644 --- a/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.h +++ b/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.h @@ -95,6 +95,10 @@ #define ADC_CHANNEL_1_PIN PC4 #define VBAT_ADC_CHANNEL ADC_CHN_1 +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PB1 + // *************** others ************************ #define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) #define VBAT_SCALE_DEFAULT 2100 diff --git a/src/main/target/FLYWOOF405PRO/config.c b/src/main/target/FLYWOOF405PRO/config.c index dfe6c0ce100..6366994c25c 100644 --- a/src/main/target/FLYWOOF405PRO/config.c +++ b/src/main/target/FLYWOOF405PRO/config.c @@ -31,4 +31,8 @@ void targetConfiguration(void) timerOverridesMutable(timer2id(TIM3))->outputMode = OUTPUT_MODE_MOTORS; timerOverridesMutable(timer2id(TIM2))->outputMode = OUTPUT_MODE_MOTORS; + + // Pinio Box params as per manufacturer specification + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; } diff --git a/src/main/target/FLYWOOF405PRO/target.h b/src/main/target/FLYWOOF405PRO/target.h index a5a02c5fe84..22ddd0a6fd2 100644 --- a/src/main/target/FLYWOOF405PRO/target.h +++ b/src/main/target/FLYWOOF405PRO/target.h @@ -152,6 +152,12 @@ #define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 #define RSSI_ADC_CHANNEL ADC_CHN_3 +// *************** PINIO *********************** +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PD6 +#define PINIO2_PIN PC8 + // *************** LED2812 ************************ #define USE_LED_STRIP #define WS2811_PIN PA9 diff --git a/src/main/target/FRSKYF405/CMakeLists.txt b/src/main/target/FRSKYF405/CMakeLists.txt new file mode 100644 index 00000000000..0e382d8acb7 --- /dev/null +++ b/src/main/target/FRSKYF405/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(FRSKYF405 HSE_MHZ 24) diff --git a/src/main/target/FRSKYF405/target.c b/src/main/target/FRSKYF405/target.c new file mode 100644 index 00000000000..0708a3981e4 --- /dev/null +++ b/src/main/target/FRSKYF405/target.c @@ -0,0 +1,39 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include + +#include "platform.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), // S5 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 + DEF_TIM(TIM12, CH1, PB14, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 - no DShot (TIM12 has no DMA) + DEF_TIM(TIM12, CH2, PB15, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 - no DShot (TIM12 has no DMA) + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S9 + + DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), // LED strip +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/FRSKYF405/target.h b/src/main/target/FRSKYF405/target.h new file mode 100644 index 00000000000..12e61c5f29a --- /dev/null +++ b/src/main/target/FRSKYF405/target.h @@ -0,0 +1,163 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "FR45" + +#define USBD_PRODUCT_STRING "FrSkyF405" + +// *************** LED & BEEPER ********************** +#define LED0 PA14 // shares SWCLK +#define LED1 PA13 // shares SWDIO + +#define BEEPER PC15 +#define BEEPER_INVERTED + +// *************** Gyro & ACC ********************** +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +// IIM-42688P is compatible with ICM42605 driver +#define USE_IMU_ICM42605 +#define ICM42605_CS_PIN PA4 +#define ICM42605_SPI_BUS BUS_SPI1 +#define IMU_ICM42605_ALIGN CW180_DEG_FLIP + + +// *************** OSD ***************************** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PC2 +#define SPI2_MOSI_PIN PC3 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PB12 + +// *************** SD Card ************************* +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +#define USE_SDCARD +#define USE_SDCARD_SPI +#define SDCARD_SPI_BUS BUS_SPI3 +#define SDCARD_CS_PIN PC14 + +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +// UART2 has two pads: standard (for CRSF etc) and hardware-inverted SBUS +#define SERIALRX_UART SERIAL_PORT_USART2 +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL + +#define USE_UART3 +#define UART3_TX_PIN PC10 +#define UART3_RX_PIN PC11 + +#define USE_UART4 +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + + +#define USE_UART5 +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 + +#define USE_UART6 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define SERIAL_PORT_COUNT 7 // VCP + UART1-5, UART6 + +// *************** I2C **************************** +#define USE_I2C + +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 + +#define DEFAULT_I2C_BUS BUS_I2C1 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_SPL06 + +#define USE_MAG +#define MAG_I2C_BUS DEFAULT_I2C_BUS +#define USE_MAG_ALL + +#define PITOT_I2C_BUS DEFAULT_I2C_BUS +#define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 + +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC1 +#define ADC_CHANNEL_3_PIN PC5 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + +#define CURRENT_METER_SCALE 250 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY | FEATURE_BLACKBOX) + +// *************** LED STRIP *********************** +#define USE_LED_STRIP +#define WS2811_PIN PA15 + +// *************** PWM OUTPUTS ********************* +// S7/S8 on TIM12 do not support DShot +#define MAX_PWM_OUTPUT_PORTS 9 + +// *************** Other *************************** +#define USE_DSHOT +#define USE_ESC_SENSOR +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) // PD2 - UART5_RX diff --git a/src/main/target/HUMMINGBIRD_FC305/config.c b/src/main/target/HUMMINGBIRD_FC305/config.c index f024a26d54c..9932306ba02 100644 --- a/src/main/target/HUMMINGBIRD_FC305/config.c +++ b/src/main/target/HUMMINGBIRD_FC305/config.c @@ -27,7 +27,7 @@ void targetConfiguration(void) { - barometerConfigMutable()->baro_hardware = BARO_SPL06; + barometerConfigMutable()->baro_hardware = BARO_AUTODETECT; serialConfigMutable()->portConfigs[3].functionMask = FUNCTION_ESCSERIAL; pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; } diff --git a/src/main/target/HUMMINGBIRD_FC305/target.c b/src/main/target/HUMMINGBIRD_FC305/target.c index 005d5a16990..275d3f27935 100644 --- a/src/main/target/HUMMINGBIRD_FC305/target.c +++ b/src/main/target/HUMMINGBIRD_FC305/target.c @@ -27,7 +27,7 @@ BUSDEV_REGISTER_SPI_TAG(busdev_icm42688, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_ICM42605_ALIGN); BUSDEV_REGISTER_SPI(busdev_sdcard_spi, DEVHW_SDCARD, SDCARD_SPI_BUS, SDCARD_CS_PIN, NONE, DEVFLAGS_SPI_MODE_0, 0); BUSDEV_REGISTER_SPI(busdev_max7456, DEVHW_MAX7456, MAX7456_SPI_BUS, MAX7456_CS_PIN, NONE, DEVFLAGS_USE_RAW_REGISTERS, 0); -BUSDEV_REGISTER_I2C(busdev_spl06, DEVHW_SPL06, SPL06_I2C_BUS, SPL06_I2C_ADDR, NONE, DEVFLAGS_NONE, 0); +BUSDEV_REGISTER_I2C(busdev_dps310, DEVHW_DPS310, BARO_I2C_BUS, DPS310_I2C_ADDR, NONE, DEVFLAGS_NONE, 0); BUSDEV_REGISTER_I2C(busdev_qmc5883, DEVHW_QMC5883, MAG_I2C_BUS, 0x0D, NONE, DEVFLAGS_NONE, 0); BUSDEV_REGISTER_I2C(busdev_hmc5883, DEVHW_HMC5883, MAG_I2C_BUS, 0x1E, NONE, DEVFLAGS_NONE, 0); diff --git a/src/main/target/HUMMINGBIRD_FC305/target.h b/src/main/target/HUMMINGBIRD_FC305/target.h index 6fd5fa705f0..7b121a775b2 100644 --- a/src/main/target/HUMMINGBIRD_FC305/target.h +++ b/src/main/target/HUMMINGBIRD_FC305/target.h @@ -54,9 +54,9 @@ #define I2C1_SDA PB9 #define USE_BARO -#define USE_BARO_SPL06 -#define SPL06_I2C_ADDR (0x76) -#define SPL06_I2C_BUS BUS_I2C1 +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_DPS310 +#define DPS310_I2C_ADDR (0x76) #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 diff --git a/src/main/target/NEXUS/CMakeLists.txt b/src/main/target/NEXUS/CMakeLists.txt new file mode 100644 index 00000000000..a9afac49839 --- /dev/null +++ b/src/main/target/NEXUS/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f722xe(NEXUS) diff --git a/src/main/target/NEXUS/README.md b/src/main/target/NEXUS/README.md new file mode 100644 index 00000000000..0edf84a7ac3 --- /dev/null +++ b/src/main/target/NEXUS/README.md @@ -0,0 +1,149 @@ +# RadioMaster Nexus (Original) + +Flight controller originally designed for helicopters using Rotorflight. +Based on STM32F722RET6. This is the **original** (discontinued) Nexus, +not the Nexus-X or Nexus-XR. + +For the Nexus-X and Nexus-XR, use the `NEXUSX` target instead. + +## Hardware + +| Component | Spec | +|-----------|------| +| MCU | STM32F722RET6 (216MHz, 256KB RAM, 512KB flash) | +| IMU | ICM-42688-P (SPI1), CW90 alignment | +| Barometer | SPL06-001 (I2C1, internal) | +| Blackbox | W25N01G 128MB (SPI2) | +| Input voltage | 5 - 12.6V | +| BEC output | 5V / 2A on Port A, B, C | +| DSM power | 3.3V / 0.5A | +| Dimensions | 41.3 x 25.4 x 13.1mm | +| Weight | 16.8g | + +## Differences from Nexus-X/XR + +| | OG Nexus | Nexus-X/XR | +|---|----------|------------| +| IMU EXTI | PA15 | PB8 | +| IMU alignment | CW90 | CW180 | +| Baro I2C | I2C1 (PB8/PB9) | I2C2 (PB10/PB11) | +| Flash | W25N01G (128MB) | W25N02K (256MB) | +| Internal ELRS RX | None | RP4TD-M on UART5 | +| PINIO1 (RX power) | None | PC8 | +| UART1 pins | PA9/PA10 | PB6/PB7 | +| Voltage sense | Vin ADC only (5-12.6V) | EXT-V input (3.6-70V) | +| Servo outputs | 5 | 7 default (9 max) | +| Rotorflight target | NEXUS_F7 | NEXUSX | + +## Pin Functions + +### Default Output Assignment + +| Output | Pin | Timer | Connector | +|--------|-----|-------|-----------| +| S1 | PB4 | TIM3_CH1 | Servo header | +| S2 | PB5 | TIM3_CH2 | Servo header | +| S3 | PB0 | TIM3_CH3 | Servo header | +| S4 | PB3 | TIM2_CH2 | Servo header (Tail) | +| M1 | PB6 | TIM4_CH1 | ESC header | + +### UART Ports + +| UART | Label | TX | RX | Notes | +|------|-------|----|----|-------| +| UART1 | DSM | PA9 | PA10 | | +| UART2 | SBUS | PA2 | PA3 | Shared with FREQ/PPM | +| UART3 | Port C | PB11 | PB10 | Shared with I2C2 | +| UART4 | Port A | PA1 | PA0 | Primary receiver (CRSF) | +| UART6 | Port B | PC7 | PC6 | | + +### I2C Buses + +| Bus | SCL | SDA | Usage | +|-----|-----|-----|-------| +| I2C1 | PB8 | PB9 | Internal barometer (SPL06) | +| I2C2 | PB10 | PB11 | External sensors via Port C | + +### ADC Channels + +| Channel | Pin | Divider | Usage | +|---------|-----|---------|-------| +| BUS | PC2 | 320 | Vin rail, 5-12.6V (mapped as VBAT) | +| BEC | PC1 | 160 | 5V BEC rail monitoring | + +Note: Unlike the Nexus-X/XR, the OG Nexus has no dedicated EXT-V +high-voltage battery sense input. VBAT monitors the board input power. + +### Connector Pinouts + +**Port A (UART4 - CRSF receiver):** +1. GND +2. 5V +3. RX (PA0, connect to receiver TX) +4. TX (PA1, connect to receiver RX / telemetry) + +**Port B (UART6):** +1. GND +2. 5V +3. TX (PC7) +4. RX (PC6) + +**Port C (UART3 / I2C2):** +1. GND +2. 5V +3. SDA/TX (PB11) +4. SCL/RX (PB10) + +**ESC Header:** +- Signal: PB6 (TIM4_CH1 PWM) + +## Typical Glider Setup (Elevon / Flying Wing) + +For a 5-channel elevon glider like the Kunai: + +1. Flash NEXUS target via DFU +2. Set aircraft type: **Flying Wing** +3. Connect RP3-H receiver to **Port A** (CRSF on UART4) +4. Connect ESC to **ESC header** (M1) +5. Connect left elevon servo to **S1** +6. Connect right elevon servo to **S2** +7. Configure elevon mixing in the Mixer tab +8. Vario: SPL06 baro provides altitude-based vario out of the box +9. GPS (Phase 2): Connect to **Port B** (UART6) + +## Verified on Hardware + +- [x] MCU boots, LEDs active (PC14, PC15) +- [x] USB CDC enumeration and iNAV CLI +- [x] IMU (gyro + accel) detected and responding +- [x] Gyro alignment (CW90) confirmed +- [x] Accelerometer working +- [x] Barometer (SPL06 on I2C1) working +- [x] VBAT ADC (PC2) working (scale 320) +- [x] All UART ports (1-4, 6) verified +- [x] UART4 CRSF receiver working (TX/RX swap confirmed) +- [x] DShot on all motor outputs +- [x] Servo outputs working +- [x] Blackbox logging working +- [x] LEDs working +- [x] I2C2 bus working + +## Building + +Requires NixOS flake (included in repo root) or standard iNAV build deps. + +```sh +nix develop --impure # or set up arm-none-eabi-gcc manually +mkdir -p build && cd build +cmake -GNinja -DCOMPILER_VERSION_CHECK=OFF .. +ninja NEXUS +``` + +## Flashing via DFU + +Hold button while connecting USB, then: + +```sh +arm-none-eabi-objcopy -O binary build/bin/NEXUS.elf build/NEXUS.bin +dfu-util -a 0 -s 0x08000000:leave -D build/NEXUS.bin +``` diff --git a/src/main/target/NEXUS/config.c b/src/main/target/NEXUS/config.c new file mode 100644 index 00000000000..ebbdf786f1c --- /dev/null +++ b/src/main/target/NEXUS/config.c @@ -0,0 +1,19 @@ +/* + * RadioMaster Nexus (Original) - Target configuration defaults + * + * Unlike the Nexus-X/XR, the OG Nexus has no internal ELRS receiver, + * so there is no PINIO / USER1 box configuration needed here. + */ + +#include +#include "platform.h" +#include "drivers/pwm_mapping.h" + +void targetConfiguration(void) +{ + // Force M1/ESC (PB6/TIM4) to motor mode so the PWM auto-allocator + // picks it as the motor output first. Without this, it walks the + // timer table in order, converts TIM3 (S1/S2/S3) to motors via + // pwmClaimTimer(), and leaves no timers available for servos. + timerOverridesMutable(timer2id(TIM4))->outputMode = OUTPUT_MODE_MOTORS; +} diff --git a/src/main/target/NEXUS/target.c b/src/main/target/NEXUS/target.c new file mode 100644 index 00000000000..a903264269e --- /dev/null +++ b/src/main/target/NEXUS/target.c @@ -0,0 +1,29 @@ +/* + * RadioMaster Nexus (Original) - Timer/PWM hardware configuration + * + * Timer allocation (from Rotorflight NEXUS_F7 dump): + * TIM3: S1 (PB4/CH1), S2 (PB5/CH2), S3 (PB0/CH3) + * TIM2: S4 (PB3/CH2) + * TIM4: M1 (PB6/CH1) - ESC output + * + * Note: PA2/PA3 are FREQ/PPM inputs in Rotorflight. In iNAV they + * can be repurposed as servo outputs when UART2 is not assigned. + */ + +#include +#include "platform.h" +#include "drivers/io.h" +#include "drivers/timer.h" +#include "drivers/pwm_mapping.h" + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // M1/ESC + DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), // shared w/ UART2 TX + DEF_TIM(TIM9, CH2, PA3, TIM_USE_OUTPUT_AUTO, 0, 0), // shared w/ UART2 RX +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/NEXUS/target.h b/src/main/target/NEXUS/target.h new file mode 100644 index 00000000000..63c08049931 --- /dev/null +++ b/src/main/target/NEXUS/target.h @@ -0,0 +1,173 @@ +/* + * RadioMaster Nexus (Original) - iNAV target + * + * Based on the NEXUSX target by functionpointer, adapted for the + * original (discontinued) Nexus hardware. + * + * Key differences from Nexus-X/XR: + * - IMU EXTI on PA15 (X/XR uses PB8) + * - IMU alignment CW180 (Rotorflight uses CW90 with a different + * board orientation reference; iNAV uses the arrow on the case) + * - Flash is W25N01G 128MB (X/XR uses W25N02K 256MB) + * - No internal ELRS receiver (X/XR has RP4TD-M on UART5) + * - No PINIO1 receiver power gate + * - Voltage input 5-12.6V (X/XR supports 3.6-70V) + * - 5 servo/motor outputs vs 7-9 on X/XR + * - Baro on I2C1 (PB8/PB9), not I2C2 + * - UART1 on PA9/PA10, not PB6/PB7 + * + * Pin mapping derived from: + * - Rotorflight NEXUS_F7 target (authoritative) + * - groundflight project (joshperry/groundflight) + * - RadioMaster documentation + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "NEXS" +#define USBD_PRODUCT_STRING "RadioMaster Nexus" + +/* ---- LEDs ---- */ +#define LED0 PC14 // active low +#define LED1 PC15 // active low + +/* ---- Beeper ---- */ +// No dedicated beeper pin on Nexus hardware + +/* ---- SPI ---- */ +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +/* ---- IMU: ICM-42688-P ---- */ +// iNAV uses ICM42605 driver which is register-compatible with ICM42688P +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW180_DEG +#define ICM42605_CS_PIN PA4 +#define ICM42605_SPI_BUS BUS_SPI1 +#define ICM42605_EXTI_PIN PA15 + +/* ---- I2C ---- */ +// I2C1: PB8/PB9 - internal, used for barometer +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +// I2C2: PB10/PB11 - external via Port C connector (shared with UART3) +// Available for external sensors (magnetometer, rangefinder, etc.) +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 +#define I2C_DEVICE_2_SHARES_UART3 + +/* ---- Barometer: SPL06-001 ---- */ +// Confirmed on I2C1 per Rotorflight NEXUS_F7 dump +#define USE_BARO +#define USE_BARO_SPL06 +#define BARO_I2C_BUS BUS_I2C1 + +/* ---- Magnetometer (external, optional via Port C / I2C2) ---- */ +#define USE_MAG +#define USE_MAG_ALL +#define MAG_I2C_BUS BUS_I2C2 + +/* ---- Flash: W25N01G (128MB) ---- */ +#define USE_FLASHFS +#define USE_FLASH_W25N01G +#define W25N01G_SPI_BUS BUS_SPI2 +#define W25N01G_CS_PIN PB12 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +/* ---- UARTs ---- */ +// OG Nexus UART layout (confirmed from Rotorflight NEXUS_F7 dump): +// UART1 [DSM port] : PA9 (TX) / PA10 (RX) +// UART2 [SBUS/FREQ] : PA2 (TX) / PA3 (RX) - shared with RPM/TLM pins +// UART3 [Port C] : PB11 (TX) / PB10 (RX) - shared with I2C2 +// UART4 [Port A] : PA1 (TX) / PA0 (RX) - primary CRSF receiver +// UART6 [Port B] : PC7 (TX) / PC6 (RX) +// +// NOTE: No UART5 on OG Nexus (X/XR uses UART5 for internal ELRS) + +#define USE_VCP +#define USE_USB_DETECT +#define USB_DETECT_PIN NONE + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define USE_UART3 +#define UART3_TX_PIN PB11 +#define UART3_RX_PIN PB10 + +#define USE_UART4 +#define USE_UART4_SWAP +#define UART4_TX_PIN PA1 +#define UART4_RX_PIN PA0 + +#define USE_UART6 +#define UART6_TX_PIN PC7 +#define UART6_RX_PIN PC6 + +#define SERIAL_PORT_COUNT 6 // VCP + UART1-4 + UART6 + +/* ---- Default serial receiver ---- */ +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART4 + + +/* ---- ADC ---- */ +// OG Nexus has no EXT-V input (unlike X/XR which has a dedicated +// high-voltage sense on PC0). Only two ADC channels: +// ADC_BUS = PC2, divider 320 (Vin rail, 5-12.6V) +// ADC_BEC = PC1, divider 160 (BEC 5V rail) +// Map Vin as VBAT since it's the primary power input +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC_CHANNEL_1_PIN PC2 // Vin (input power rail) +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define ADC_CHANNEL_2_PIN PC1 // BEC 5V rail +// VBAT scale: hardware-verified value (divider ratio ~320) +#define VBAT_SCALE_DEFAULT 320 + +/* ---- Sensors ---- */ +#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO) + +/* ---- PWM / Servo / Motor outputs ---- */ +// OG Nexus outputs (from Rotorflight dump): +// S1: PB4 (TIM3_CH1) - Servo header +// S2: PB5 (TIM3_CH2) - Servo header +// S3: PB0 (TIM3_CH3) - Servo header +// S4: PB3 (TIM2_CH2) - Servo header (Tail) +// M1: PB6 (TIM4_CH1) - ESC header (motor only, NOT UART1) +// +// Pin multiplexing when UARTs freed: +// PA2 (TIM5_CH3) - shared with UART2 TX / FREQ input +// PA3 (TIM9_CH2) - shared with UART2 RX / PPM input + +#define MAX_PWM_OUTPUT_PORTS 7 + +/* ---- No internal receiver ---- */ +// OG Nexus has no internal ELRS receiver. +// No UART5, no PINIO1 power gate. + +/* ---- Platform ---- */ +#define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff + +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) diff --git a/src/main/target/PAURCF405V2/CMakeLists.txt b/src/main/target/PAURCF405V2/CMakeLists.txt new file mode 100644 index 00000000000..2034128e311 --- /dev/null +++ b/src/main/target/PAURCF405V2/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(PAURCF405V2) diff --git a/src/main/target/PAURCF405V2/config.c b/src/main/target/PAURCF405V2/config.c new file mode 100644 index 00000000000..79a0eb18472 --- /dev/null +++ b/src/main/target/PAURCF405V2/config.c @@ -0,0 +1,28 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include +#include "platform.h" +#include "fc/fc_msp_box.h" +#include "fc/config.h" +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; +} diff --git a/src/main/target/PAURCF405V2/target.c b/src/main/target/PAURCF405V2/target.c new file mode 100644 index 00000000000..efc300259d5 --- /dev/null +++ b/src/main/target/PAURCF405V2/target.c @@ -0,0 +1,48 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include +#include +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/sensor.h" +#include "drivers/timer_def_stm32f4xx.h" + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 D(2,7,7) UP217 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 D(2,2,0) UP217 + DEF_TIM(TIM1, CH3N, PB15, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 D(2,6,0) UP256 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 2), // S4 D(2,3,6) UP256 + + DEF_TIM(TIM2, CH4, PB11, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 D(1,7,3) UP173 + DEF_TIM(TIM2, CH3, PB10, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 D(1,1,3) UP173 + DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 D(1,6,3) UP173 + DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 D(1,5,3) UP173 + + DEF_TIM(TIM12, CH1, PB14, TIM_USE_OUTPUT_AUTO, 0, 0), // S9 DMA NONE + DEF_TIM(TIM13, CH1, PA6, TIM_USE_OUTPUT_AUTO, 0, 0), // S10 DMA NONE + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S11 D(1,0,2) + + DEF_TIM(TIM3, CH4, PB1, TIM_USE_LED, 0, 0), // 2812LED D(1,2,5) + DEF_TIM(TIM11, CH1, PB9, TIM_USE_BEEPER, 0, 0), // BEEPER PWM + + DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), //TX2 softserial1_Tx ISR-driven, does not use DMA1_Stream0 +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/PAURCF405V2/target.h b/src/main/target/PAURCF405V2/target.h new file mode 100644 index 00000000000..658ee136d4f --- /dev/null +++ b/src/main/target/PAURCF405V2/target.h @@ -0,0 +1,166 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#pragma once +#define USE_TARGET_CONFIG + +#define TARGET_BOARD_IDENTIFIER "PA42" +#define USBD_PRODUCT_STRING "PauRcF405v2" + +#define LED0 PA14 //Blue +#define LED1 PA13 //Green + +#define BEEPER PB9 +#define BEEPER_INVERTED +#define BEEPER_PWM_FREQUENCY 2500 + +// *************** SPI1 IMU & OSD ******************* +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PB4 +#define SPI1_MOSI_PIN PA7 + +// Board ships with either ICM42605/ICM42688P or BMI270 - same CS pin (PC14) +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW0_DEG +#define ICM42605_SPI_BUS BUS_SPI1 +#define ICM42605_CS_PIN PC14 + +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW180_DEG +#define BMI270_SPI_BUS BUS_SPI1 +#define BMI270_CS_PIN PC14 + + + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI1 +#define MAX7456_CS_PIN PB12 + +// *************** SPI2 Flash **************** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PC2 +#define SPI2_MOSI_PIN PC3 + +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_SPI_BUS BUS_SPI2 +#define M25P16_CS_PIN PC13 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// *************** I2C /Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB7 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_DPS310 +#define USE_BARO_SPL06 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_ALL + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS BUS_I2C1 +#define PITOT_I2C_BUS BUS_I2C1 +#define TEMPERATURE_I2C_BUS BUS_I2C1 + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define USE_UART3 +#define UART3_TX_PIN PC10 +#define UART3_RX_PIN PC11 + +#define USE_UART4 +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define USE_UART5 +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 + +#define USE_UART6 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define USE_SOFTSERIAL1 +#define SOFTSERIAL_1_TX_PIN PA2 // Shared with UART2_TX - mutually exclusive with UART2 +#define SOFTSERIAL_1_RX_PIN PA2 // Shared with UART2_TX - mutually exclusive with UART2 + +#define SERIAL_PORT_COUNT 8 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART2 + +// *************** ADC *************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream4 +#define ADC_CHANNEL_1_PIN PC4 +#define ADC_CHANNEL_2_PIN PC5 +#define ADC_CHANNEL_3_PIN PB0 +#define ADC_CHANNEL_4_PIN PC0 +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 +#define AIRSPEED_ADC_CHANNEL ADC_CHN_4 + +// *************** PINIO *************************** +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PA4 +#define PINIO2_PIN PB5 + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PB1 + +// *************** others ************************ +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) +#define VBAT_SCALE_DEFAULT 2100 +#define CURRENT_METER_SCALE 150 + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + +#define MAX_PWM_OUTPUT_PORTS 11 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define USE_DSHOT +#define USE_DSHOT_DMAR +#define USE_ESC_SENSOR + diff --git a/src/main/target/SITL/sim/realFlight.c b/src/main/target/SITL/sim/realFlight.c index a88d20a612d..6ff0ff413af 100644 --- a/src/main/target/SITL/sim/realFlight.c +++ b/src/main/target/SITL/sim/realFlight.c @@ -28,14 +28,14 @@ #include #include #include +#include #include #include "platform.h" #include "target.h" #include "target/SITL/sim/realFlight.h" -#include "target/SITL/sim/simple_soap_client.h" -#include "target/SITL/sim/xplane.h" +#include "target/SITL/sim/soap_client.h" #include "target/SITL/sim/simHelper.h" #include "fc/runtime_config.h" #include "drivers/time.h" @@ -55,8 +55,9 @@ #include "flight/imu.h" #include "io/gps.h" #include "rx/sim.h" +#include "realFlight.h" -#define RF_PORT 18083 +#define RF_PORT "18083" #define RF_MAX_CHANNEL_COUNT 12 // "RealFlight Ranch" is located in Sierra Nevada, southern Spain @@ -67,17 +68,14 @@ static uint8_t pwmMapping[RF_MAX_PWM_OUTS]; static uint8_t mappingCount; -static pthread_cond_t sockcond1 = PTHREAD_COND_INITIALIZER; -static pthread_cond_t sockcond2 = PTHREAD_COND_INITIALIZER; -static pthread_mutex_t sockmtx = PTHREAD_MUTEX_INITIALIZER; +static pthread_mutex_t initMutex = PTHREAD_MUTEX_INITIALIZER; +static pthread_cond_t initCond = PTHREAD_COND_INITIALIZER; +static atomic_bool shouldStopSoapThread = false; -static soap_client_t *client = NULL; -static soap_client_t *clientNext = NULL; +static soap_client_t client; +static pthread_t soapThread = 0; -static pthread_t soapThread; -static pthread_t creationThread; - -static bool isInitalised = false; +static atomic_bool isInitalised = false; static bool useImu = false; typedef struct @@ -133,38 +131,6 @@ typedef struct rfValues_t rfValues; -static void deleteClient(soap_client_t *client) -{ - soapClientClose(client); - free(client); - client = NULL; -} - -static void startRequest(char* action, const char* fmt, ...) -{ - pthread_mutex_lock(&sockmtx); - while (clientNext == NULL) { - pthread_cond_wait(&sockcond1, &sockmtx); - } - - client = clientNext; - clientNext = NULL; - - pthread_cond_broadcast(&sockcond2); - pthread_mutex_unlock(&sockmtx); - - va_list va; - va_start(va, fmt); - soapClientSendRequestVa(client, action, fmt, va); - va_end(va); -} - -static char* endRequest(void) -{ - char* ret = soapClientReceive(client); - deleteClient(client); - return ret; -} // Simple, but fast ;) static double getDoubleFromResponse(const char* response, const char* elementName) @@ -261,6 +227,7 @@ static float convertAzimuth(float azimuth) return 360 - fmodf(azimuth + 90, 360.0f); } + static void exchangeData(void) { double servoValues[RF_MAX_PWM_OUTS] = { }; @@ -272,14 +239,32 @@ static void exchangeData(void) } } - startRequest("ExchangeData", "%u" - "%.4f%.4f%.4f%.4f%.4f%.4f%.4f%.4f" - "%.4f%.4f%.4f%.4f", - 0xFFF, - servoValues[0], servoValues[1], servoValues[2], servoValues[3], servoValues[4], servoValues[5], servoValues[6], servoValues[7], - servoValues[8], servoValues[9], servoValues[10], servoValues[11]); - char* response = endRequest(); + char requestBody[1024] = "4095"; + + for (int i = 0; i < RF_MAX_CHANNEL_COUNT; i++) { + char value[32]; + snprintf(value, sizeof(value), "%.4f", servoValues[i]); + strncat(requestBody, value, sizeof(requestBody) - strlen(requestBody) - 1); + } + strncat(requestBody, "", sizeof(requestBody) - strlen(requestBody) - 1); + + char *response = NULL; + int http_status = 0; + int ret = soap_client_call_raw_body( + &client, + "ExchangeData", + requestBody, + &response, + &http_status + ); + + if (ret < 0 || http_status != 200 || !response) { + fprintf(stderr, "[SIM] Data exchange with RealFlight failed.\n"); + free(response); + return; + } + //rfValues.m_currentPhysicsTime_SEC = getDoubleFromResponse(response, "m-currentPhysicsTime-SEC"); //rfValues.m_currentPhysicsSpeedMultiplier = getDoubleFromResponse(response, "m-currentPhysicsSpeedMultiplier"); rfValues.m_airspeed_MPS = getDoubleFromResponse(response, "m-airspeed-MPS"); @@ -410,50 +395,70 @@ static void exchangeData(void) free(response); } -static void* soapWorker(void* arg) +static bool restoreOriginalControllerDevice(void) { - UNUSED(arg); - while(true) - { - if (!isInitalised) { - startRequest("RestoreOriginalControllerDevice", "12"); - free(endRequest()); - startRequest("InjectUAVControllerInterface", "12"); - free(endRequest()); - exchangeData(); - ENABLE_ARMING_FLAG(SIMULATOR_MODE_SITL); - - isInitalised = true; - } + char* response = NULL; + int http_status = 0; + const int ret = soap_client_call_raw_body( + &client, + "RestoreOriginalControllerDevice", + "12", + &response, + &http_status + ); - exchangeData(); - unlockMainPID(); + if (ret < 0 || (http_status != 200 && http_status != 500) || !response) { + free(response); + return false; } - return NULL; + free(response); + return true; } - -static void* creationWorker(void* arg) +static void* soapWorker(void* arg) { - char* ip = (char*)arg; - - while (true) { - pthread_mutex_lock(&sockmtx); - while (clientNext != NULL) { - pthread_cond_wait(&sockcond2, &sockmtx); - } - pthread_mutex_unlock(&sockmtx); - - soap_client_t *cli = malloc(sizeof(soap_client_t)); - if (!soapClientConnect(cli, ip, RF_PORT)) { - continue; + UNUSED(arg); + while(!atomic_load(&shouldStopSoapThread)) { + + if (!atomic_load(&isInitalised)) { + // Initialize RealFlight + + // Alway try to restore the original controller device first to avoid problems with broken connections and the interface being stuck in a half-initialised state. + // RealFlight seems to not properly close the connection on its side if the connection is interrupted, but only after a timeout of about 30 seconds. + // During this time the interface is not usable, but without this step it would be stuck in an unusable state until the next restart of the SITL. + if (!restoreOriginalControllerDevice()) { + delay(1000); + continue; + } + + char* response = NULL; + int http_status = 0; + const int ret = soap_client_call_raw_body( + &client, + "InjectUAVControllerInterface", + "12", + &response, + &http_status + ); + + if (ret < 0 || http_status != 200 || !response) { + free(response); + delay(1000); + continue; + } + + exchangeData(); // Get initial data and set initial state in RealFlight + + ENABLE_ARMING_FLAG(SIMULATOR_MODE_SITL); + atomic_store(&isInitalised, true); + pthread_mutex_lock(&initMutex); + pthread_cond_signal(&initCond); + pthread_mutex_unlock(&initMutex); } - - clientNext = cli; - pthread_mutex_lock(&sockmtx); - pthread_cond_broadcast(&sockcond1); - pthread_mutex_unlock(&sockmtx); + + exchangeData(); + unlockMainPID(); } return NULL; @@ -465,19 +470,46 @@ bool simRealFlightInit(char* ip, uint8_t* mapping, uint8_t mapCount, bool imu) mappingCount = mapCount; useImu = imu; - if (pthread_create(&soapThread, NULL, soapWorker, NULL) < 0) { + if (soap_client_init(&client, ip, RF_PORT, "/", 1000) < 0) { return false; } - if (pthread_create(&creationThread, NULL, creationWorker, (void*)ip) < 0) { + atomic_store(&isInitalised, false); + atomic_store(&shouldStopSoapThread, false); + if (pthread_create(&soapThread, NULL, soapWorker, &client) < 0) { return false; } // Wait until the connection is established, the interface has been initialised - // and the first valid packet has been received to avoid problems with the startup calibration. - while (!isInitalised) { - delay(250); + // and the first valid packet has been received to avoid problems with the startup calibration. + pthread_mutex_lock(&initMutex); + while (!atomic_load(&isInitalised)) { + pthread_cond_wait(&initCond, &initMutex); } + pthread_mutex_unlock(&initMutex); return true; } + +void simRealFlightClose(void) +{ + atomic_store(&shouldStopSoapThread, true); + if (soapThread) { + pthread_join(soapThread, NULL); + } + + if (atomic_load(&isInitalised)) { + + if (!restoreOriginalControllerDevice( )) { + fprintf(stderr, "[SIM] Failed to restore original controller device in RealFlight.\n"); + } else { + fprintf(stderr, "[SIM] Restored original controller device in RealFlight.\n"); + } + } + + DISABLE_ARMING_FLAG(SIMULATOR_MODE_SITL); + atomic_store(&isInitalised, false); + pthread_mutex_destroy(&initMutex); + pthread_cond_destroy(&initCond); + soap_client_destroy(&client); +} diff --git a/src/main/target/SITL/sim/realFlight.h b/src/main/target/SITL/sim/realFlight.h index af34c2c524e..80e27c9087c 100644 --- a/src/main/target/SITL/sim/realFlight.h +++ b/src/main/target/SITL/sim/realFlight.h @@ -27,3 +27,4 @@ #define RF_MAX_PWM_OUTS 12 bool simRealFlightInit(char* ip, uint8_t* mapping, uint8_t mapCount, bool imu); +void simRealFlightClose(void); diff --git a/src/main/target/SITL/sim/simple_soap_client.c b/src/main/target/SITL/sim/simple_soap_client.c deleted file mode 100644 index e6400ae3b14..00000000000 --- a/src/main/target/SITL/sim/simple_soap_client.c +++ /dev/null @@ -1,172 +0,0 @@ -/* - * This file is part of INAV Project. - * - * This Source Code Form is subject to the terms of the Mozilla Public - * License, v. 2.0. If a copy of the MPL was not distributed with this file, - * You can obtain one at http://mozilla.org/MPL/2.0/. - * - * Alternatively, the contents of this file may be used under the terms - * of the GNU General Public License Version 3, as described below: - * - * This file is free software: you may copy, redistribute and/or modify - * it under the terms of the GNU General Public License as published by the - * Free Software Foundation, either version 3 of the License, or (at your - * option) any later version. - * - * This file 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 http://www.gnu.org/licenses/. - */ - -#define _GNU_SOURCE - -#include -#include -#include -#include -#include -#include -#include -# include -# include -#include -#include - -#include "simple_soap_client.h" - -#define REC_BUF_SIZE 6000 -char recBuffer[REC_BUF_SIZE]; - -bool soapClientConnect(soap_client_t *client, const char *address, int port) -{ - client->sockedFd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); - if (client->sockedFd < 0) { - return false; - } - - int one = 1; - if (setsockopt(client->sockedFd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)) < 0) { - return false; - } - - client->socketAddr.sin_family = AF_INET; - client->socketAddr.sin_port = htons(port); - client->socketAddr.sin_addr.s_addr = inet_addr(address); - - if (connect(client->sockedFd, (struct sockaddr*)&client->socketAddr, sizeof(client->socketAddr)) < 0) { - return false; - } - - client->isConnected = true; - client->isInitalised = true; - - return true; -} - -void soapClientClose(soap_client_t *client) -{ - close(client->sockedFd); - memset(client, 0, sizeof(soap_client_t)); - client->isConnected = false; - client->isInitalised = false; -} - -void soapClientSendRequestVa(soap_client_t *client, const char* action, const char *fmt, va_list va) -{ - if (!client->isConnected) { - return; - } - - char* requestBody; - if (vasprintf(&requestBody, fmt, va) < 0) { - return; - } - - char* request; - if (asprintf(&request, "POST / HTTP/1.1\r\nsoapaction: %s\r\ncontent-length: %u\r\ncontent-type: text/xml;charset='UTF-8'\r\nConnection: Keep-Alive\r\n\r\n%s", - action, (unsigned)strlen(requestBody), requestBody) < 0) { - return; - } - - send(client->sockedFd, request, strlen(request), 0); - - free(requestBody); - free(request); -} - -void soapClientSendRequest(soap_client_t *client, const char* action, const char *fmt, ...) -{ - va_list va; - - va_start(va, fmt); - soapClientSendRequestVa(client, action, fmt, va); - va_end(va); -} - -static bool soapClientPoll(soap_client_t *client, uint32_t timeout_ms) -{ - fd_set fds; - struct timeval tv; - - FD_ZERO(&fds); - FD_SET(client->sockedFd, &fds); - - tv.tv_sec = timeout_ms / 1000; - tv.tv_usec = (timeout_ms % 1000) * 1000UL; - - if (select(client->sockedFd + 1, &fds, NULL, NULL, &tv) != 1) { - return false; - } - return true; -} - - -char* soapClientReceive(soap_client_t *client) -{ - if (!client->isInitalised){ - return false; - } - - if (!soapClientPoll(client, 1000)) { - return false; - } - - ssize_t size = recv(client->sockedFd, recBuffer, REC_BUF_SIZE, 0); - - if (size <= 0) { - return NULL; - } - - char* pos = strstr(recBuffer, "Content-Length: "); - if (!pos) { - return NULL; - } - - uint32_t contentLength = strtoul(pos + 16, NULL, 10); - char *body = strstr(pos, "\r\n\r\n"); - if (!body) { - return NULL; - } - - body += 4; - - ssize_t expectedLength = contentLength + body - recBuffer; - if ((unsigned)expectedLength >= sizeof(recBuffer)) { - return NULL; - } - - while (size < expectedLength){ - ssize_t size2 = recv(client->sockedFd, &recBuffer[size], sizeof(recBuffer - size + 1), 0); - if (size2 <= 0) { - return NULL; - } - size += size2; - } - - recBuffer[size] = '\0'; - return strdup(body); -} diff --git a/src/main/target/SITL/sim/soap_client.c b/src/main/target/SITL/sim/soap_client.c new file mode 100644 index 00000000000..6ac5f65826a --- /dev/null +++ b/src/main/target/SITL/sim/soap_client.c @@ -0,0 +1,516 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file 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 http://www.gnu.org/licenses/. + */ + +#define _POSIX_C_SOURCE 200809L + +#include "soap_client.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +static int set_socket_timeout(int fd, int timeout_ms) { + struct timeval tv; + tv.tv_sec = timeout_ms / 1000; + tv.tv_usec = (timeout_ms % 1000) * 1000; + + if (setsockopt(fd, SOL_SOCKET, SO_SNDTIMEO, &tv, sizeof(tv)) != 0) { + return -1; + } + if (setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)) != 0) { + return -1; + } + return 0; +} + +static int connect_with_timeout(const struct addrinfo* ai, int timeout_ms) { + int fd = socket(ai->ai_family, ai->ai_socktype, ai->ai_protocol); + if (fd < 0) { + return -1; + } + + int flags = fcntl(fd, F_GETFL, 0); + if (flags < 0) { + close(fd); + return -1; + } + if (fcntl(fd, F_SETFL, flags | O_NONBLOCK) < 0) { + close(fd); + return -1; + } + + int one = 1; + if (setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)) != 0) { + close(fd); + return -1; + } + + int rc = connect(fd, ai->ai_addr, ai->ai_addrlen); + if (rc == 0) { + (void)fcntl(fd, F_SETFL, flags); + return fd; + } + if (errno != EINPROGRESS) { + close(fd); + return -1; + } + + fd_set wfds; + FD_ZERO(&wfds); + FD_SET(fd, &wfds); + struct timeval tv; + tv.tv_sec = timeout_ms / 1000; + tv.tv_usec = (timeout_ms % 1000) * 1000; + + rc = select(fd + 1, NULL, &wfds, NULL, &tv); + if (rc <= 0) { + close(fd); + errno = (rc == 0) ? ETIMEDOUT : errno; + return -1; + } + + int so_error = 0; + socklen_t so_error_len = sizeof(so_error); + if (getsockopt(fd, SOL_SOCKET, SO_ERROR, &so_error, &so_error_len) != 0 || so_error != 0) { + close(fd); + errno = (so_error != 0) ? so_error : errno; + return -1; + } + + if (fcntl(fd, F_SETFL, flags) < 0) { + close(fd); + return -1; + } + + return fd; +} + +static int open_tcp_connection(const char* host, const char* port, int timeout_ms) { + struct addrinfo hints; + memset(&hints, 0, sizeof(hints)); + hints.ai_family = AF_UNSPEC; + hints.ai_socktype = SOCK_STREAM; + + struct addrinfo* result = NULL; + if (getaddrinfo(host, port, &hints, &result) != 0) { + return -1; + } + + int fd = -1; + for (const struct addrinfo* ai = result; ai != NULL; ai = ai->ai_next) { + fd = connect_with_timeout(ai, timeout_ms); + if (fd >= 0) { + if (set_socket_timeout(fd, timeout_ms) == 0) { + break; + } + close(fd); + fd = -1; + } + } + + freeaddrinfo(result); + return fd; +} + +static int send_all(int fd, const char* data, size_t len) { + size_t total = 0; + while (total < len) { + ssize_t n = send(fd, data + total, len - total, 0); + if (n < 0) { + if (errno == EINTR) { + continue; + } + return -1; + } + if (n == 0) { + return -1; + } + total += (size_t)n; + } + return 0; +} + +static char* recv_all(int fd, size_t* out_len) { + size_t cap = 8192; + size_t len = 0; + char* buf = (char*)malloc(cap); + if (!buf) { + return NULL; + } + + while (1) { + if (len == cap) { + size_t new_cap = cap * 2; + char* next = (char*)realloc(buf, new_cap); + if (!next) { + free(buf); + return NULL; + } + buf = next; + cap = new_cap; + } + + ssize_t n = recv(fd, buf + len, cap - len, 0); + if (n < 0) { + if (errno == EINTR) { + continue; + } + free(buf); + return NULL; + } + if (n == 0) { + break; + } + len += (size_t)n; + } + + char* out = (char*)realloc(buf, len + 1); + if (!out) { + free(buf); + return NULL; + } + out[len] = '\0'; + *out_len = len; + return out; +} + +static int parse_http_status(const char* response) { + int status = 0; + if (sscanf(response, "HTTP/%*d.%*d %d", &status) == 1) { + return status; + } + return 0; +} + +static char* dechunk_http_body(const char* body, size_t body_len, size_t* out_len) { + size_t cap = body_len + 1; + char* out = (char*)malloc(cap); + if (!out) { + return NULL; + } + size_t out_pos = 0; + size_t pos = 0; + + while (pos < body_len) { + const char* line_end = strstr(body + pos, "\r\n"); + if (!line_end) { + free(out); + return NULL; + } + + char size_buf[32]; + size_t size_len = (size_t)(line_end - (body + pos)); + if (size_len == 0 || size_len >= sizeof(size_buf)) { + free(out); + return NULL; + } + memcpy(size_buf, body + pos, size_len); + size_buf[size_len] = '\0'; + + char* endptr = NULL; + unsigned long chunk_size = strtoul(size_buf, &endptr, 16); + if (endptr == size_buf) { + free(out); + return NULL; + } + + pos = (size_t)(line_end - body) + 2; + if (chunk_size == 0) { + break; + } + if (pos + chunk_size + 2 > body_len) { + free(out); + return NULL; + } + + if (out_pos + chunk_size + 1 > cap) { + size_t new_cap = cap; + while (out_pos + chunk_size + 1 > new_cap) { + new_cap *= 2; + } + char* next = (char*)realloc(out, new_cap); + if (!next) { + free(out); + return NULL; + } + out = next; + cap = new_cap; + } + + memcpy(out + out_pos, body + pos, chunk_size); + out_pos += chunk_size; + pos += chunk_size; + + if (!(body[pos] == '\r' && body[pos + 1] == '\n')) { + free(out); + return NULL; + } + pos += 2; + } + + out[out_pos] = '\0'; + *out_len = out_pos; + return out; +} + +static char* extract_soap_body_raw(const char* xml, size_t xml_len) { + const char* p = xml; + const char* end = xml + xml_len; + + const char* body_open = NULL; + const char* body_name_start = NULL; + const char* body_name_end = NULL; + + while (p < end) { + const char* lt = strchr(p, '<'); + if (!lt || lt >= end) { + break; + } + if (lt + 1 < end && (lt[1] == '/' || lt[1] == '?' || lt[1] == '!')) { + p = lt + 1; + continue; + } + const char* gt = strchr(lt, '>'); + if (!gt || gt >= end) { + break; + } + + const char* name_start = lt + 1; + const char* name_end = name_start; + while (name_end < gt && *name_end != ' ' && *name_end != '\t' && *name_end != '\r' && *name_end != '\n' && *name_end != '/') { + name_end++; + } + + size_t name_len = (size_t)(name_end - name_start); + if (name_len >= 4 && strncmp(name_end - 4, "Body", 4) == 0) { + body_open = gt + 1; + body_name_start = name_start; + body_name_end = name_end; + break; + } + p = gt + 1; + } + + if (!body_open || !body_name_start || !body_name_end) { + return NULL; + } + + char close_tag[128]; + size_t tag_len = (size_t)(body_name_end - body_name_start); + if (tag_len + 4 >= sizeof(close_tag)) { + return NULL; + } + close_tag[0] = '<'; + close_tag[1] = '/'; + memcpy(close_tag + 2, body_name_start, tag_len); + close_tag[tag_len + 2] = '>'; + close_tag[tag_len + 3] = '\0'; + + const char* close = strstr(body_open, close_tag); + if (!close) { + return NULL; + } + + size_t inner_len = (size_t)(close - body_open); + char* inner = (char*)malloc(inner_len + 1); + if (!inner) { + return NULL; + } + memcpy(inner, body_open, inner_len); + inner[inner_len] = '\0'; + return inner; +} + +int soap_client_init(soap_client_t* c, const char* host, const char* port, const char* path, int timeout_ms) { + if (!c || !host || !port || !path) { + return -1; + } + memset(c, 0, sizeof(*c)); + if (snprintf(c->host, sizeof(c->host), "%s", host) >= (int)sizeof(c->host)) { + return -1; + } + if (snprintf(c->port, sizeof(c->port), "%s", port) >= (int)sizeof(c->port)) { + return -1; + } + if (snprintf(c->path, sizeof(c->path), "%s", path) >= (int)sizeof(c->path)) { + return -1; + } + c->timeout_ms = timeout_ms; + return pthread_mutex_init(&c->lock, NULL); +} + +void soap_client_destroy(soap_client_t* c) { + if (c) { + pthread_mutex_destroy(&c->lock); + } +} + +int soap_client_call_raw_body(soap_client_t* c, + const char* soap_action, + const char* request_body_xml, + char** response_body_xml, + int* http_status) { + if (!c || !request_body_xml || !response_body_xml || !http_status) { + return -1; + } + + char host[256]; + char port[16]; + char path[256]; + int timeout_ms = 0; + + pthread_mutex_lock(&c->lock); + snprintf(host, sizeof(host), "%s", c->host); + snprintf(port, sizeof(port), "%s", c->port); + snprintf(path, sizeof(path), "%s", c->path); + timeout_ms = c->timeout_ms; + pthread_mutex_unlock(&c->lock); + + const char* envelope_prefix = + "" + "" + ""; + const char* envelope_suffix = ""; + + size_t body_len = strlen(request_body_xml); + size_t envelope_len = strlen(envelope_prefix) + body_len + strlen(envelope_suffix); + char* envelope = (char*)malloc(envelope_len + 1); + if (!envelope) { + return -1; + } + snprintf(envelope, envelope_len + 1, "%s%s%s", envelope_prefix, request_body_xml, envelope_suffix); + + const char* action_header = (soap_action && soap_action[0] != '\0') ? soap_action : ""; + int req_len = snprintf(NULL, + 0, + "POST %s HTTP/1.1\r\n" + "Host: %s:%s\r\n" + "Content-Type: text/xml; charset=utf-8\r\n" + "SOAPAction: \"%s\"\r\n" + "Content-Length: %zu\r\n" + "Connection: close\r\n\r\n%s", + path, + host, + port, + action_header, + envelope_len, + envelope); + if (req_len < 0) { + free(envelope); + return -1; + } + + char* request = (char*)malloc((size_t)req_len + 1); + if (!request) { + free(envelope); + return -1; + } + snprintf(request, + (size_t)req_len + 1, + "POST %s HTTP/1.1\r\n" + "Host: %s:%s\r\n" + "Content-Type: text/xml; charset=utf-8\r\n" + "SOAPAction: \"%s\"\r\n" + "Content-Length: %zu\r\n" + "Connection: close\r\n\r\n%s", + path, + host, + port, + action_header, + envelope_len, + envelope); + + int fd = open_tcp_connection(host, port, timeout_ms); + if (fd < 0) { + free(request); + free(envelope); + return -1; + } + + int rc = 0; + size_t resp_len = 0; + char* response = NULL; + char* body = NULL; + char* soap_body = NULL; + + if (send_all(fd, request, (size_t)req_len) != 0) { + rc = -1; + goto cleanup; + } + + response = recv_all(fd, &resp_len); + if (!response || resp_len == 0) { + rc = -1; + goto cleanup; + } + + *http_status = parse_http_status(response); + + char* headers_end = strstr(response, "\r\n\r\n"); + if (!headers_end) { + rc = -1; + goto cleanup; + } + char* raw_body = headers_end + 4; + size_t raw_body_len = resp_len - (size_t)(raw_body - response); + + if (strstr(response, "Transfer-Encoding: chunked") || strstr(response, "transfer-encoding: chunked")) { + size_t decoded_len = 0; + body = dechunk_http_body(raw_body, raw_body_len, &decoded_len); + if (!body) { + rc = -1; + goto cleanup; + } + soap_body = extract_soap_body_raw(body, decoded_len); + } else { + soap_body = extract_soap_body_raw(raw_body, raw_body_len); + } + + if (!soap_body) { + rc = -1; + goto cleanup; + } + + *response_body_xml = soap_body; + soap_body = NULL; + +cleanup: + if (fd >= 0) { + close(fd); + } + free(request); + free(envelope); + free(response); + free(body); + free(soap_body); + return rc; +} diff --git a/src/main/target/SITL/sim/simple_soap_client.h b/src/main/target/SITL/sim/soap_client.h similarity index 60% rename from src/main/target/SITL/sim/simple_soap_client.h rename to src/main/target/SITL/sim/soap_client.h index 8522b28859a..ae8cd8cedcf 100644 --- a/src/main/target/SITL/sim/simple_soap_client.h +++ b/src/main/target/SITL/sim/soap_client.h @@ -22,27 +22,25 @@ * along with this program. If not, see http://www.gnu.org/licenses/. */ -#include -#include -#include +#pragma once -#define SOAP_REC_BUF_SIZE 256 * 1024 +#include typedef struct { - int sockedFd; - struct sockaddr_in socketAddr; - bool isInitalised; - bool isConnected; + char host[256]; + char port[16]; + char path[256]; + int timeout_ms; + pthread_mutex_t lock; } soap_client_t; -typedef struct { - soap_client_t client; - char* content; -} send_info_t; - +int soap_client_init(soap_client_t* c, const char* host, const char* port, const char* path, int timeout_ms); +void soap_client_destroy(soap_client_t* c); -bool soapClientConnect(soap_client_t *client, const char *address, int port); -void soapClientClose(soap_client_t *client); -void soapClientSendRequestVa(soap_client_t *client, const char* action, const char *fmt, va_list va); -void soapClientSendRequest(soap_client_t *client, const char* action, const char *fmt, ...); -char* soapClientReceive(soap_client_t *client); +// Sends raw XML as content of and returns raw XML content of response . +// Caller must free(*response_body_xml) on success. +int soap_client_call_raw_body(soap_client_t* c, + const char* soap_action, + const char* request_body_xml, + char** response_body_xml, + int* http_status); \ No newline at end of file diff --git a/src/main/target/SITL/sim/xplane.c b/src/main/target/SITL/sim/xplane.c index 612f4a54a0a..bf0349c47d1 100644 --- a/src/main/target/SITL/sim/xplane.c +++ b/src/main/target/SITL/sim/xplane.c @@ -33,8 +33,10 @@ #include #include #include +#include #include #include +#include #include "platform.h" @@ -58,6 +60,7 @@ #include "flight/imu.h" #include "io/gps.h" #include "rx/sim.h" +#include "xplane.h" #define XP_PORT 49000 #define XPLANE_JOYSTICK_AXIS_COUNT 8 @@ -66,11 +69,15 @@ static uint8_t pwmMapping[XP_MAX_PWM_OUTS]; static uint8_t mappingCount; +static pthread_mutex_t initMutex = PTHREAD_MUTEX_INITIALIZER; +static pthread_cond_t initCond = PTHREAD_COND_INITIALIZER; +static atomic_bool shouldStopListenThread = false; + static struct sockaddr_storage serverAddr; static socklen_t serverAddrLen; static int sockFd; -static pthread_t listenThread; -static bool initialized = false; +static pthread_t listenThread = 0; +static atomic_bool initalized = false; static bool useImu = false; static float lattitude = 0; @@ -170,6 +177,40 @@ static void sendDref(char* dref, float value) sendto(sockFd, (void*)buf, sizeof(buf), 0, (struct sockaddr*)&serverAddr, serverAddrLen); } +static void tryRegisterDrefs(void) +{ + registerDref(DREF_LATITUDE, "sim/flightmodel/position/latitude", 100); + registerDref(DREF_LONGITUDE, "sim/flightmodel/position/longitude", 100); + registerDref(DREF_ELEVATION, "sim/flightmodel/position/elevation", 100); + registerDref(DREF_AGL, "sim/flightmodel/position/y_agl", 100); + registerDref(DREF_LOCAL_VX, "sim/flightmodel/position/local_vx", 100); + registerDref(DREF_LOCAL_VY, "sim/flightmodel/position/local_vy", 100); + registerDref(DREF_LOCAL_VZ, "sim/flightmodel/position/local_vz", 100); + registerDref(DREF_GROUNDSPEED, "sim/flightmodel/position/groundspeed", 100); + registerDref(DREF_TRUE_AIRSPEED, "sim/flightmodel/position/true_airspeed", 100); + registerDref(DREF_POS_PHI, "sim/flightmodel/position/phi", 100); + registerDref(DREF_POS_THETA, "sim/flightmodel/position/theta", 100); + registerDref(DREF_POS_PSI, "sim/flightmodel/position/psi", 100); + registerDref(DREF_POS_HPATH, "sim/flightmodel/position/hpath", 100); + registerDref(DREF_FORCE_G_AXI1, "sim/flightmodel/forces/g_axil", 100); + registerDref(DREF_FORCE_G_SIDE, "sim/flightmodel/forces/g_side", 100); + registerDref(DREF_FORCE_G_NRML, "sim/flightmodel/forces/g_nrml", 100); + registerDref(DREF_POS_P, "sim/flightmodel/position/P", 100); + registerDref(DREF_POS_Q, "sim/flightmodel/position/Q", 100); + registerDref(DREF_POS_R, "sim/flightmodel/position/R", 100); + registerDref(DREF_POS_BARO_CURRENT_INHG, "sim/weather/barometer_current_inhg", 100); + registerDref(DREF_HAS_JOYSTICK, "sim/joystick/has_joystick", 100); + registerDref(DREF_JOYSTICK_VALUES_PITCH, "sim/joystick/joy_mapped_axis_value[1]", 100); + registerDref(DREF_JOYSTICK_VALUES_ROll, "sim/joystick/joy_mapped_axis_value[2]", 100); + registerDref(DREF_JOYSTICK_VALUES_YAW, "sim/joystick/joy_mapped_axis_value[3]", 100); + // Abusing cowl flaps for other channels + registerDref(DREF_JOYSTICK_VALUES_THROTTLE, "sim/joystick/joy_mapped_axis_value[57]", 100); + registerDref(DREF_JOYSTICK_VALUES_CH5, "sim/joystick/joy_mapped_axis_value[58]", 100); + registerDref(DREF_JOYSTICK_VALUES_CH6, "sim/joystick/joy_mapped_axis_value[59]", 100); + registerDref(DREF_JOYSTICK_VALUES_CH7, "sim/joystick/joy_mapped_axis_value[60]", 100); + registerDref(DREF_JOYSTICK_VALUES_CH8, "sim/joystick/joy_mapped_axis_value[61]", 100); +} + static void* listenWorker(void* arg) { UNUSED(arg); @@ -179,8 +220,11 @@ static void* listenWorker(void* arg) socklen_t slen = sizeof(remoteAddr); int recvLen; - while (true) + while (!atomic_load(&shouldStopListenThread)) { + if (!atomic_load(&initalized)) { + tryRegisterDrefs(); + } float motorValue = 0; float yokeValues[3] = { 0 }; @@ -210,10 +254,12 @@ static void* listenWorker(void* arg) recvLen = recvfrom(sockFd, buf, sizeof(buf), 0, (struct sockaddr*)&remoteAddr, &slen); if (recvLen < 0 && errno != EWOULDBLOCK) { + delay(250); continue; } if (strncmp((char*)buf, "RREF", 4) != 0) { + delay(250); continue; } @@ -426,11 +472,14 @@ static void* listenWorker(void* arg) constrainToInt16(north.z * 1024.0f) ); - if (!initialized) { + if (!atomic_load(&initalized)) { ENABLE_ARMING_FLAG(SIMULATOR_MODE_SITL); // Aircraft can wobble on the runway and prevents calibration of the accelerometer ENABLE_STATE(ACCELEROMETER_CALIBRATED); - initialized = true; + atomic_store(&initalized, true); + pthread_mutex_lock(&initMutex); + pthread_cond_signal(&initCond); + pthread_mutex_unlock(&initMutex); } unlockMainPID(); @@ -458,7 +507,7 @@ bool simXPlaneInit(char* ip, int port, uint8_t* mapping, uint8_t mapCount, bool if (sockFd < 0) { return false; } else { - char addrbuf[IPADDRESS_PRINT_BUFLEN]; + char addrbuf[IPADDRESS_PRINT_BUFLEN]; char *nptr = prettyPrintAddress((struct sockaddr *)&serverAddr, addrbuf, IPADDRESS_PRINT_BUFLEN ); if (nptr != NULL) { fprintf(stderr, "[SOCKET] xplane address = %s, fd=%d\n", nptr, sockFd); @@ -466,9 +515,9 @@ bool simXPlaneInit(char* ip, int port, uint8_t* mapping, uint8_t mapCount, bool } struct timeval tv; - tv.tv_sec = 1; - tv.tv_usec = 0; - if (setsockopt(sockFd, SOL_SOCKET, SO_RCVTIMEO, (struct timeval *) &tv,sizeof(struct timeval))) { + tv.tv_sec = 1; + tv.tv_usec = 0; + if (setsockopt(sockFd, SOL_SOCKET, SO_RCVTIMEO, (struct timeval *) &tv,sizeof(struct timeval))) { return false; } @@ -476,43 +525,30 @@ bool simXPlaneInit(char* ip, int port, uint8_t* mapping, uint8_t mapCount, bool return false; } + atomic_store(&initalized, false); + atomic_store(&shouldStopListenThread, false); if (pthread_create(&listenThread, NULL, listenWorker, NULL) < 0) { return false; } - while (!initialized) { - registerDref(DREF_LATITUDE, "sim/flightmodel/position/latitude", 100); - registerDref(DREF_LONGITUDE, "sim/flightmodel/position/longitude", 100); - registerDref(DREF_ELEVATION, "sim/flightmodel/position/elevation", 100); - registerDref(DREF_AGL, "sim/flightmodel/position/y_agl", 100); - registerDref(DREF_LOCAL_VX, "sim/flightmodel/position/local_vx", 100); - registerDref(DREF_LOCAL_VY, "sim/flightmodel/position/local_vy", 100); - registerDref(DREF_LOCAL_VZ, "sim/flightmodel/position/local_vz", 100); - registerDref(DREF_GROUNDSPEED, "sim/flightmodel/position/groundspeed", 100); - registerDref(DREF_TRUE_AIRSPEED, "sim/flightmodel/position/true_airspeed", 100); - registerDref(DREF_POS_PHI, "sim/flightmodel/position/phi", 100); - registerDref(DREF_POS_THETA, "sim/flightmodel/position/theta", 100); - registerDref(DREF_POS_PSI, "sim/flightmodel/position/psi", 100); - registerDref(DREF_POS_HPATH, "sim/flightmodel/position/hpath", 100); - registerDref(DREF_FORCE_G_AXI1, "sim/flightmodel/forces/g_axil", 100); - registerDref(DREF_FORCE_G_SIDE, "sim/flightmodel/forces/g_side", 100); - registerDref(DREF_FORCE_G_NRML, "sim/flightmodel/forces/g_nrml", 100); - registerDref(DREF_POS_P, "sim/flightmodel/position/P", 100); - registerDref(DREF_POS_Q, "sim/flightmodel/position/Q", 100); - registerDref(DREF_POS_R, "sim/flightmodel/position/R", 100); - registerDref(DREF_POS_BARO_CURRENT_INHG, "sim/weather/barometer_current_inhg", 100); - registerDref(DREF_HAS_JOYSTICK, "sim/joystick/has_joystick", 100); - registerDref(DREF_JOYSTICK_VALUES_PITCH, "sim/joystick/joy_mapped_axis_value[1]", 100); - registerDref(DREF_JOYSTICK_VALUES_ROll, "sim/joystick/joy_mapped_axis_value[2]", 100); - registerDref(DREF_JOYSTICK_VALUES_YAW, "sim/joystick/joy_mapped_axis_value[3]", 100); - // Abusing cowl flaps for other channels - registerDref(DREF_JOYSTICK_VALUES_THROTTLE, "sim/joystick/joy_mapped_axis_value[57]", 100); - registerDref(DREF_JOYSTICK_VALUES_CH5, "sim/joystick/joy_mapped_axis_value[58]", 100); - registerDref(DREF_JOYSTICK_VALUES_CH6, "sim/joystick/joy_mapped_axis_value[59]", 100); - registerDref(DREF_JOYSTICK_VALUES_CH7, "sim/joystick/joy_mapped_axis_value[60]", 100); - registerDref(DREF_JOYSTICK_VALUES_CH8, "sim/joystick/joy_mapped_axis_value[61]", 100); - delay(250); + pthread_mutex_lock(&initMutex); + while (!atomic_load(&initalized)) { + pthread_cond_wait(&initCond, &initMutex); } + pthread_mutex_unlock(&initMutex); return true; } + +void simXPlaneClose(void) +{ + atomic_store(&shouldStopListenThread, true); + if (listenThread) { + pthread_join(listenThread, NULL); + } + DISABLE_ARMING_FLAG(SIMULATOR_MODE_SITL); + atomic_store(&initalized, false); + pthread_mutex_destroy(&initMutex); + pthread_cond_destroy(&initCond); + close(sockFd); +} diff --git a/src/main/target/SITL/sim/xplane.h b/src/main/target/SITL/sim/xplane.h index 1777a30af2b..619bef615e0 100644 --- a/src/main/target/SITL/sim/xplane.h +++ b/src/main/target/SITL/sim/xplane.h @@ -27,3 +27,4 @@ #define XP_MAX_PWM_OUTS 4 bool simXPlaneInit(char* ip, int port, uint8_t* mapping, uint8_t mapCount, bool imu); +void simXPlaneClose(void); diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c index bb34f2cd665..2542a352bc1 100644 --- a/src/main/target/SITL/target.c +++ b/src/main/target/SITL/target.c @@ -39,6 +39,7 @@ #include #include #include +#include #include #include "target.h" @@ -81,6 +82,25 @@ static void printVersion(void) { fprintf(stderr, "INAV %d.%d.%d SITL (%s)\n", FC_VERSION_MAJOR, FC_VERSION_MINOR, FC_VERSION_PATCH_LEVEL, shortGitRevision); } +static void cleanupAndExit(int code, bool shouldExit) { + if (sitlSim == SITL_SIM_XPLANE) { + simXPlaneClose(); + } else if (sitlSim == SITL_SIM_REALFLIGHT) { + simRealFlightClose(); + } + pthread_mutex_destroy(&mainLoopLock); + + if (shouldExit) { + exit(code); + } +} + +static void on_sigint(int sig) { + UNUSED(sig); + fprintf(stderr, "\n[SYSTEM] Caught SIGINT, exiting...\n"); + cleanupAndExit(0, true); +} + void systemInit(void) { printVersion(); clock_gettime(CLOCK_MONOTONIC, &start_time); @@ -101,6 +121,12 @@ void systemInit(void) { exit(1); } + struct sigaction sa; + sa.sa_handler = on_sigint; + sigemptyset(&sa.sa_mask); + sa.sa_flags = 0; + sigaction(SIGINT, &sa, NULL); + if (sitlSim != SITL_SIM_NONE) { fprintf(stderr, "[SIM] Waiting for connection...\n"); } @@ -390,6 +416,7 @@ void delay(timeMs_t ms) void systemReset(void) { fprintf(stderr, "[SYSTEM] Reset\n"); + cleanupAndExit(0, false); #if defined(__CYGWIN__) || defined(__APPLE__) || GCC_MAJOR < 12 for(int j = 3; j < 1024; j++) { close(j); @@ -404,7 +431,7 @@ void systemReset(void) void systemResetToBootloader(void) { fprintf(stderr, "[SYSTEM] Reset to bootloader\n"); - exit(0); + cleanupAndExit(0, true); } void failureMode(failureMode_e mode) { diff --git a/src/main/target/TBS_LUCID_H7/target.c b/src/main/target/TBS_LUCID_H7/target.c index 88b2df3e3b7..8202f462b0f 100644 --- a/src/main/target/TBS_LUCID_H7/target.c +++ b/src/main/target/TBS_LUCID_H7/target.c @@ -54,7 +54,8 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM15, CH1, PE5, TIM_USE_OUTPUT_AUTO, 0, 0), // S11 DEF_TIM(TIM15, CH2, PE6, TIM_USE_OUTPUT_AUTO, 0, 0), // S12 DMA_NONE - DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 9), // RGB_LED + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO | TIM_USE_LED, 0, 9), // RGB_LED + DEF_TIM(TIM2, CH1, PA15, TIM_USE_BEEPER, 0, 0), // BEEPER }; diff --git a/src/main/target/TBS_LUCID_H7/target.h b/src/main/target/TBS_LUCID_H7/target.h index e9eef8686bf..7cecc767868 100644 --- a/src/main/target/TBS_LUCID_H7/target.h +++ b/src/main/target/TBS_LUCID_H7/target.h @@ -101,8 +101,8 @@ #define GYRO2_CS_PIN PE11 #define USE_IMU_MPU6000 -#define IMU_1_MPU6000_ALIGN CW0_DEG_FLIP -#define IMU_2_MPU6000_ALIGN CW90_DEG_FLIP +#define IMU_1_MPU6000_ALIGN CW90_DEG_FLIP +#define IMU_2_MPU6000_ALIGN CW0_DEG_FLIP #define USE_IMU_ICM42605 #define IMU_1_ICM42605_ALIGN CW90_DEG_FLIP @@ -124,6 +124,8 @@ #define USE_BARO #define USE_BARO_DPS310 +#define USE_BARO_BMP388 +#define USE_BARO_BMP390 #define BARO_I2C_BUS BUS_I2C2 #define USE_MAG diff --git a/src/main/target/TBS_LUCID_H7_WING/target.c b/src/main/target/TBS_LUCID_H7_WING/target.c index 1dd790735d0..f8ecaa5a29b 100644 --- a/src/main/target/TBS_LUCID_H7_WING/target.c +++ b/src/main/target/TBS_LUCID_H7_WING/target.c @@ -55,6 +55,7 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM15, CH2, PE6, TIM_USE_OUTPUT_AUTO, 0, 0), // S12 DMA_NONE DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 9), // S13 + DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO | TIM_USE_LED, 0, 8), // LED_STRIP }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/TBS_LUCID_H7_WING/target.h b/src/main/target/TBS_LUCID_H7_WING/target.h index 8871cee7398..9dd98e1527d 100644 --- a/src/main/target/TBS_LUCID_H7_WING/target.h +++ b/src/main/target/TBS_LUCID_H7_WING/target.h @@ -100,7 +100,7 @@ #define GYRO2_CS_PIN PE11 #define USE_IMU_MPU6000 -#define IMU_1_MPU6000_ALIGN CW0_DEG_FLIP +#define IMU_1_MPU6000_ALIGN CW180_DEG_FLIP #define IMU_2_MPU6000_ALIGN CW90_DEG_FLIP #define USE_IMU_ICM42605 @@ -124,6 +124,8 @@ #define USE_BARO #define USE_BARO_DPS310 +#define USE_BARO_BMP388 +#define USE_BARO_BMP390 #define BARO_I2C_BUS BUS_I2C2 #define USE_MAG @@ -161,6 +163,9 @@ #define PINIO1_PIN PD10 #define PINIO2_PIN PD11 +#define USE_LED_STRIP +#define WS2811_PIN PA15 + #define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) #define CURRENT_METER_SCALE 200 #define VBAT_SCALE_DEFAULT 2100 diff --git a/src/main/target/TBS_LUCID_H7_WING_MINI/target.c b/src/main/target/TBS_LUCID_H7_WING_MINI/target.c index 837f05985a9..1c27e0d6d04 100644 --- a/src/main/target/TBS_LUCID_H7_WING_MINI/target.c +++ b/src/main/target/TBS_LUCID_H7_WING_MINI/target.c @@ -47,6 +47,7 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 5), // S6 DEF_TIM(TIM4, CH1, PD12, TIM_USE_OUTPUT_AUTO, 0, 6), // S7 + DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO | TIM_USE_LED, 0, 8), // LED_STRIP }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/TBS_LUCID_H7_WING_MINI/target.h b/src/main/target/TBS_LUCID_H7_WING_MINI/target.h index f211decb7bd..4f475a46a0f 100644 --- a/src/main/target/TBS_LUCID_H7_WING_MINI/target.h +++ b/src/main/target/TBS_LUCID_H7_WING_MINI/target.h @@ -100,7 +100,7 @@ #define GYRO2_CS_PIN PE11 #define USE_IMU_MPU6000 -#define IMU_1_MPU6000_ALIGN CW0_DEG_FLIP +#define IMU_1_MPU6000_ALIGN CW180_DEG_FLIP #define IMU_2_MPU6000_ALIGN CW90_DEG_FLIP #define USE_IMU_ICM42605 @@ -124,6 +124,8 @@ #define USE_BARO #define USE_BARO_DPS310 +#define USE_BARO_BMP388 +#define USE_BARO_BMP390 #define BARO_I2C_BUS BUS_I2C2 #define USE_MAG @@ -160,6 +162,9 @@ #define USE_PINIOBOX #define PINIO1_PIN PD10 +#define USE_LED_STRIP +#define WS2811_PIN PA15 + #define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) #define CURRENT_METER_SCALE 250 #define VBAT_SCALE_DEFAULT 2100 diff --git a/src/main/target/common.h b/src/main/target/common.h index 45eb12ac4bc..58b572495fe 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -95,6 +95,7 @@ // Allow default airspeed sensors #define USE_PITOT #define USE_PITOT_MS4525 +#define USE_PITOT_MS5525 #define USE_PITOT_MSP #define USE_PITOT_DLVR diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c index aac3db564cc..8314926184f 100755 --- a/src/main/target/common_hardware.c +++ b/src/main/target/common_hardware.c @@ -82,6 +82,10 @@ BUSDEV_REGISTER_SPI(busdev_lsm6dxx, DEVHW_LSM6D, LSM6DXX_SPI_BUS, LSM6DXX_CS_PIN, NONE, DEVFLAGS_NONE, IMU_LSM6DXX_ALIGN); #endif + #if defined(USE_IMU_ICM45686) + BUSDEV_REGISTER_SPI(busdev_icm45686, DEVHW_ICM45686, ICM45686_SPI_BUS, ICM45686_CS_PIN, NONE, DEVFLAGS_NONE, IMU_ICM45686_ALIGN); + #endif + #endif @@ -397,6 +401,14 @@ BUSDEV_REGISTER_I2C(busdev_ms4525, DEVHW_MS4525, MS4525_I2C_BUS, 0x28, NONE, DEVFLAGS_USE_RAW_REGISTERS, 0); // Requires 0xFF to passthrough #endif +#if defined(PITOT_I2C_BUS) && !defined(MS5525_I2C_BUS) + #define MS5525_I2C_BUS PITOT_I2C_BUS +#endif + +#if defined(USE_PITOT_MS5525) && defined(MS5525_I2C_BUS) + BUSDEV_REGISTER_I2C(busdev_ms5525, DEVHW_MS5525, MS5525_I2C_BUS, 0x76, NONE, DEVFLAGS_NONE, 0); +#endif + #if defined(PITOT_I2C_BUS) && !defined(DLVR_I2C_BUS) #define DLVR_I2C_BUS PITOT_I2C_BUS diff --git a/src/main/target/link/stm32_flash_f765xg.ld b/src/main/target/link/stm32_flash_f765xg.ld index a83374e4e41..ea548ed4ff3 100644 --- a/src/main/target/link/stm32_flash_f765xg.ld +++ b/src/main/target/link/stm32_flash_f765xg.ld @@ -1,10 +1,10 @@ /* ***************************************************************************** ** -** File : stm32_flash_f745.ld +** File : stm32_flash_f765xg.ld ** -** Abstract : Linker script for STM32F745VGTx Device with -** 1024KByte FLASH, 320KByte RAM +** Abstract : Linker script for STM32F765xGTx Device with +** 1024KByte FLASH, 512KByte RAM ** ***************************************************************************** */ @@ -29,7 +29,7 @@ MEMORY { ITCM_RAM (rx) : ORIGIN = 0x00000000, LENGTH = 16K ITCM_FLASH (rx) : ORIGIN = 0x00200000, LENGTH = 32K - /* config occupies the entire flash sector 1 for the ease of erasure, 32K on F74x */ + /* config occupies the entire flash sector 1 for the ease of erasure, 32K on F7xx */ ITCM_FLASH_CONFIG (r) : ORIGIN = 0x00208000, LENGTH = 32K ITCM_FLASH1 (rx) : ORIGIN = 0x00210000, LENGTH = 960K @@ -37,12 +37,16 @@ MEMORY FLASH_CONFIG (r) : ORIGIN = 0x08008000, LENGTH = 32K FLASH1 (rx) : ORIGIN = 0x08010000, LENGTH = 960K - TCM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K - RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 256K + DTCM_RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K + SRAM1 (rwx) : ORIGIN = 0x20020000, LENGTH = 368K + SRAM2 (rwx) : ORIGIN = 0x2007C000, LENGTH = 16K MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K } -/* note CCM could be used for stack */ -REGION_ALIAS("STACKRAM", TCM) -REGION_ALIAS("FASTRAM", TCM) +/* STM32F765 DTCM is 128KB (0x20000000-0x2001FFFF); SRAM1 starts at 0x20020000. + * DMA cannot access DTCM on STM32F7 - all DMA buffers must be in SRAM1 or SRAM2. + * NOTE: F745 DTCM is only 64KB, so F745 SRAM1 starts at 0x20010000 (different!). */ +REGION_ALIAS("STACKRAM", DTCM_RAM) +REGION_ALIAS("FASTRAM", DTCM_RAM) +REGION_ALIAS("RAM", SRAM1) INCLUDE "stm32_flash_f7_split.ld" diff --git a/src/main/target/link/stm32_flash_f765xg_bl.ld b/src/main/target/link/stm32_flash_f765xg_bl.ld index d357aad98d8..92f3d36754b 100644 --- a/src/main/target/link/stm32_flash_f765xg_bl.ld +++ b/src/main/target/link/stm32_flash_f765xg_bl.ld @@ -1,10 +1,10 @@ /* ***************************************************************************** ** -** File : stm32_flash_f745.ld +** File : stm32_flash_f765xg_bl.ld ** -** Abstract : Linker script for STM32F745VGTx Device with -** 1024KByte FLASH, 320KByte RAM +** Abstract : Linker script for STM32F765xGTx Device with +** 1024KByte FLASH, 512KByte RAM (bootloader) ** ***************************************************************************** */ @@ -29,7 +29,7 @@ MEMORY { ITCM_RAM (rx) : ORIGIN = 0x00000000, LENGTH = 16K ITCM_FLASH (rx) : ORIGIN = 0x00200000, LENGTH = 32K - /* config occupies the entire flash sector 1 for the ease of erasure, 32K on F74x */ + /* config occupies the entire flash sector 1 for the ease of erasure, 32K on F7xx */ ITCM_FLASH_CONFIG (r) : ORIGIN = 0x00208000, LENGTH = 32K ITCM_FLASH1 (rx) : ORIGIN = 0x00210000, LENGTH = 928K @@ -37,13 +37,17 @@ MEMORY FIRMWARE (rx) : ORIGIN = 0x08008000, LENGTH = 32K FLASH_CONFIG (r) : ORIGIN = 0x08010000, LENGTH = 32K - TCM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K - RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 256K + DTCM_RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K + SRAM1 (rwx) : ORIGIN = 0x20020000, LENGTH = 368K + SRAM2 (rwx) : ORIGIN = 0x2007C000, LENGTH = 16K MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K } -/* note CCM could be used for stack */ -REGION_ALIAS("STACKRAM", TCM) -REGION_ALIAS("FASTRAM", TCM) +/* STM32F765 DTCM is 128KB (0x20000000-0x2001FFFF); SRAM1 starts at 0x20020000. + * DMA cannot access DTCM on STM32F7 - all DMA buffers must be in SRAM1 or SRAM2. + * NOTE: F745 DTCM is only 64KB, so F745 SRAM1 starts at 0x20010000 (different!). */ +REGION_ALIAS("STACKRAM", DTCM_RAM) +REGION_ALIAS("FASTRAM", DTCM_RAM) +REGION_ALIAS("RAM", SRAM1) __firmware_start = ORIGIN(FIRMWARE); diff --git a/src/main/target/link/stm32_flash_f765xg_for_bl.ld b/src/main/target/link/stm32_flash_f765xg_for_bl.ld index c7667d1dc26..b50247b015d 100644 --- a/src/main/target/link/stm32_flash_f765xg_for_bl.ld +++ b/src/main/target/link/stm32_flash_f765xg_for_bl.ld @@ -1,10 +1,10 @@ /* ***************************************************************************** ** -** File : stm32_flash_f745.ld +** File : stm32_flash_f765xg_for_bl.ld ** -** Abstract : Linker script for STM32F745VGTx Device with -** 1024KByte FLASH, 320KByte RAM +** Abstract : Linker script for STM32F765xGTx Device with +** 1024KByte FLASH, 512KByte RAM (bootloader-aware) ** ***************************************************************************** */ @@ -29,7 +29,7 @@ MEMORY { ITCM_RAM (rx) : ORIGIN = 0x00000000, LENGTH = 16K ITCM_FLASH (rx) : ORIGIN = 0x00200000, LENGTH = 32K - /* config occupies the entire flash sector 1 for the ease of erasure, 32K on F74x */ + /* config occupies the entire flash sector 1 for the ease of erasure, 32K on F7xx */ ITCM_FLASH_CONFIG (r) : ORIGIN = 0x00208000, LENGTH = 32K ITCM_FLASH1 (rx) : ORIGIN = 0x00210000, LENGTH = 960K @@ -37,13 +37,17 @@ MEMORY FLASH_CONFIG (r) : ORIGIN = 0x08010000, LENGTH = 32K FLASH1 (rx) : ORIGIN = 0x08018000, LENGTH = 928K - TCM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K - RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 256K + DTCM_RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K + SRAM1 (rwx) : ORIGIN = 0x20020000, LENGTH = 368K + SRAM2 (rwx) : ORIGIN = 0x2007C000, LENGTH = 16K MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K } -/* note CCM could be used for stack */ -REGION_ALIAS("STACKRAM", TCM) -REGION_ALIAS("FASTRAM", TCM) +/* STM32F765 DTCM is 128KB (0x20000000-0x2001FFFF); SRAM1 starts at 0x20020000. + * DMA cannot access DTCM on STM32F7 - all DMA buffers must be in SRAM1 or SRAM2. + * NOTE: F745 DTCM is only 64KB, so F745 SRAM1 starts at 0x20010000 (different!). */ +REGION_ALIAS("STACKRAM", DTCM_RAM) +REGION_ALIAS("FASTRAM", DTCM_RAM) +REGION_ALIAS("RAM", SRAM1) __firmware_start = ORIGIN(FLASH); diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index d86822ada7a..e8214fc8c1f 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -46,6 +46,7 @@ #include "fc/runtime_config.h" #include "flight/imu.h" +#include "flight/mixer.h" #include "io/gps.h" #include "io/serial.h" @@ -56,7 +57,10 @@ #include "rx/rx.h" #include "sensors/battery.h" +#include "sensors/esc_sensor.h" +#include "sensors/pitotmeter.h" #include "sensors/sensors.h" +#include "sensors/temperature.h" #include "telemetry/crsf.h" #include "telemetry/telemetry.h" @@ -162,6 +166,16 @@ static void crsfSerialize16(sbuf_t *dst, uint16_t v) crsfSerialize8(dst, (uint8_t)v); } +#ifdef USE_ESC_SENSOR +static void crsfSerialize24(sbuf_t *dst, uint32_t v) +{ + // Use BigEndian format + crsfSerialize8(dst, (v >> 16)); + crsfSerialize8(dst, (v >> 8)); + crsfSerialize8(dst, (uint8_t)v); +} +#endif + static void crsfSerialize32(sbuf_t *dst, uint32_t v) { // Use BigEndian format @@ -296,6 +310,96 @@ static void crsfBarometerAltitude(sbuf_t *dst) crsfSerialize16(dst, altitude_packed); } +#ifdef USE_PITOT +/* +0x0A Airspeed sensor +Payload: +int16 Air speed ( dm/s ) +*/ +static void crsfFrameAirSpeedSensor(sbuf_t *dst) +{ + // use sbufWrite since CRC does not include frame length + sbufWriteU8(dst, CRSF_FRAME_AIRSPEED_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); + crsfSerialize8(dst, CRSF_FRAMETYPE_AIRSPEED_SENSOR); + crsfSerialize16(dst, (uint16_t)(getAirspeedEstimate() * 36.0f / 100.0f)); +} +#endif + +#ifdef USE_ESC_SENSOR +/* +0x0C RPM +Payload: +uint8_t rpm_source_id; // Identifies the source of the RPM data (e.g., 0 = Motor 1, 1 = Motor 2, etc.) +int24_t rpm_value[]; // 1 - 19 RPM values with negative ones representing the motor spinning in reverse +*/ +static bool crsfRpm(sbuf_t *dst) +{ + const uint8_t MAX_CRSF_RPM_VALUES = 19; // CRSF protocol limit: 1-19 RPM values + uint8_t motorCount = getMotorCount(); + + if (STATE(ESC_SENSOR_ENABLED) && motorCount > 0) { + // Enforce protocol limit + if (motorCount > MAX_CRSF_RPM_VALUES) { + motorCount = MAX_CRSF_RPM_VALUES; + } + + sbufWriteU8(dst, 1 + (motorCount * 3) + CRSF_FRAME_LENGTH_TYPE_CRC); + crsfSerialize8(dst, CRSF_FRAMETYPE_RPM); + // 0 = FC including all ESCs + crsfSerialize8(dst, 0); + + for (uint8_t i = 0; i < motorCount; i++) { + const escSensorData_t *escState = getEscTelemetry(i); + crsfSerialize24(dst, (escState) ? escState->rpm : 0); + } + return true; + } + return false; +} +#endif + +/* +0x0D TEMP +Payload: +uint8_t temp_source_id; // Identifies the source of the temperature data (e.g., 0 = FC including all ESCs, 1 = Ambient, etc.) +int16_t temperature[]; // up to 20 temperature values in deci-degree (tenths of a degree) Celsius (e.g., 250 = 25.0°C, -50 = -5.0°C) +*/ +static bool crsfTemperature(sbuf_t *dst) +{ + const uint8_t MAX_CRSF_TEMPS = 20; // Maximum temperatures per CRSF frame + uint8_t tempCount = 0; + int16_t temperatures[20]; + +#ifdef USE_ESC_SENSOR + uint8_t motorCount = getMotorCount(); + if (STATE(ESC_SENSOR_ENABLED) && motorCount > 0) { + for (uint8_t i = 0; i < motorCount && tempCount < MAX_CRSF_TEMPS; i++) { + const escSensorData_t *escState = getEscTelemetry(i); + temperatures[tempCount++] = (escState) ? escState->temperature * 10 : TEMPERATURE_INVALID_VALUE; + } + } +#endif + +#ifdef USE_TEMPERATURE_SENSOR + for (uint8_t i = 0; i < MAX_TEMP_SENSORS && tempCount < MAX_CRSF_TEMPS; i++) { + int16_t value; + if (getSensorTemperature(i, &value)) + temperatures[tempCount++] = value; + } +#endif + + if (tempCount > 0) { + sbufWriteU8(dst, 1 + (tempCount * 2) + CRSF_FRAME_LENGTH_TYPE_CRC); + crsfSerialize8(dst, CRSF_FRAMETYPE_TEMP); + // 0 = FC including all ESCs + crsfSerialize8(dst, 0); + for (uint8_t i = 0; i < tempCount; i++) + crsfSerialize16(dst, temperatures[i]); + return true; + } + return false; +} + typedef enum { CRSF_ACTIVE_ANTENNA1 = 0, CRSF_ACTIVE_ANTENNA2 = 1 @@ -465,11 +569,14 @@ typedef enum { CRSF_FRAME_GPS_INDEX, CRSF_FRAME_VARIO_SENSOR_INDEX, CRSF_FRAME_BAROMETER_ALTITUDE_INDEX, + CRSF_FRAME_TEMP_INDEX, + CRSF_FRAME_RPM_INDEX, + CRSF_FRAME_AIRSPEED_INDEX, CRSF_SCHEDULE_COUNT_MAX } crsfFrameTypeIndex_e; static uint8_t crsfScheduleCount; -static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX]; +static uint16_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX]; #if defined(USE_MSP_OVER_TELEMETRY) @@ -506,7 +613,7 @@ static void processCrsf(void) } static uint8_t crsfScheduleIndex = 0; - const uint8_t currentSchedule = crsfSchedule[crsfScheduleIndex]; + const uint16_t currentSchedule = crsfSchedule[crsfScheduleIndex]; sbuf_t crsfPayloadBuf; sbuf_t *dst = &crsfPayloadBuf; @@ -526,6 +633,22 @@ static void processCrsf(void) crsfFrameFlightMode(dst); crsfFinalize(dst); } +#ifdef USE_ESC_SENSOR + if (currentSchedule & BV(CRSF_FRAME_RPM_INDEX)) { + crsfInitializeFrame(dst); + if (crsfRpm(dst)) { + crsfFinalize(dst); + } + } +#endif +#if defined(USE_ESC_SENSOR) || defined(USE_TEMPERATURE_SENSOR) + if (currentSchedule & BV(CRSF_FRAME_TEMP_INDEX)) { + crsfInitializeFrame(dst); + if (crsfTemperature(dst)) { + crsfFinalize(dst); + } + } +#endif #ifdef USE_GPS if (currentSchedule & BV(CRSF_FRAME_GPS_INDEX)) { crsfInitializeFrame(dst); @@ -544,6 +667,13 @@ static void processCrsf(void) crsfBarometerAltitude(dst); crsfFinalize(dst); } +#endif +#ifdef USE_PITOT + if (currentSchedule & BV(CRSF_FRAME_AIRSPEED_INDEX)) { + crsfInitializeFrame(dst); + crsfFrameAirSpeedSensor(dst); + crsfFinalize(dst); + } #endif crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount; } @@ -582,6 +712,40 @@ void initCrsfTelemetry(void) if (sensors(SENSOR_BARO)) { crsfSchedule[index++] = BV(CRSF_FRAME_BAROMETER_ALTITUDE_INDEX); } +#endif +#ifdef USE_ESC_SENSOR + if (STATE(ESC_SENSOR_ENABLED) && getMotorCount() > 0) { + crsfSchedule[index++] = BV(CRSF_FRAME_RPM_INDEX); + } +#endif +#if defined(USE_ESC_SENSOR) || defined(USE_TEMPERATURE_SENSOR) + // Only schedule temperature frame if we have temperature sources available + bool hasTemperatureSources = false; +#ifdef USE_ESC_SENSOR + if (STATE(ESC_SENSOR_ENABLED) && getMotorCount() > 0) { + hasTemperatureSources = true; + } +#endif +#ifdef USE_TEMPERATURE_SENSOR + if (!hasTemperatureSources) { + // Check if any temperature sensors are configured + for (uint8_t i = 0; i < MAX_TEMP_SENSORS; i++) { + int16_t value; + if (getSensorTemperature(i, &value)) { + hasTemperatureSources = true; + break; + } + } + } +#endif + if (hasTemperatureSources) { + crsfSchedule[index++] = BV(CRSF_FRAME_TEMP_INDEX); + } +#endif +#ifdef USE_PITOT + if (sensors(SENSOR_PITOT)) { + crsfSchedule[index++] = BV(CRSF_FRAME_AIRSPEED_INDEX); + } #endif crsfScheduleCount = (uint8_t)index; } diff --git a/src/main/telemetry/msp_shared.c b/src/main/telemetry/msp_shared.c index 1589a513596..f6d4c6eafbd 100644 --- a/src/main/telemetry/msp_shared.c +++ b/src/main/telemetry/msp_shared.c @@ -73,10 +73,10 @@ void initSharedMsp(void) mspPackage.responseBuffer = (uint8_t *)&mspTxBuffer; mspPackage.responsePacket = &mspTxPacket; mspPackage.responsePacket->buf.ptr = mspPackage.responseBuffer; - mspPackage.responsePacket->buf.end = mspPackage.responseBuffer + sizeof(mspTxBuffer); + mspPackage.responsePacket->buf.end = mspPackage.responseBuffer; } -static void processMspPacket(void) +static bool processMspPacket(void) { mspPackage.responsePacket->cmd = 0; mspPackage.responsePacket->result = 0; @@ -84,14 +84,20 @@ static void processMspPacket(void) mspPackage.responsePacket->buf.end = mspPackage.responseBuffer + sizeof(mspTxBuffer); mspPostProcessFnPtr mspPostProcessFn = NULL; - if (mspFcProcessCommand(mspPackage.requestPacket, mspPackage.responsePacket, &mspPostProcessFn) == MSP_RESULT_ERROR) { + const mspResult_e status = mspFcProcessCommand(mspPackage.requestPacket, mspPackage.responsePacket, &mspPostProcessFn); + if (status == MSP_RESULT_ERROR) { sbufWriteU8(&mspPackage.responsePacket->buf, TELEMETRY_MSP_ERROR); } if (mspPostProcessFn) { mspPostProcessFn(NULL); } + if (status == MSP_RESULT_NO_REPLY) { + return false; + } + sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer); + return true; } void sendMspErrorResponse(uint8_t error, int16_t cmd) @@ -194,8 +200,7 @@ bool handleMspFrame(uint8_t *const frameStart, const int payloadLength) sbufAdvance(&mspPackage.requestFrame, payloadExpecting); sbufWriteData(&requestPacket->buf, payload, payloadExpecting); sbufSwitchToReader(&requestPacket->buf, mspPackage.requestBuffer); - processMspPacket(); - return true; + return processMspPacket(); } bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn)