diff --git a/CMakeLists.txt b/CMakeLists.txt index 14db3259720..24a0fe83ca0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -52,7 +52,7 @@ else() endif() -project(INAV VERSION 9.0.1) +project(INAV VERSION 9.1.0) enable_language(ASM) diff --git a/cmake/pg_struct_sizes.reference.db b/cmake/pg_struct_sizes.reference.db index a98e89bf21c..49fa79ce34d 100644 --- a/cmake/pg_struct_sizes.reference.db +++ b/cmake/pg_struct_sizes.reference.db @@ -47,3 +47,4 @@ telemetryConfig_t 58 8 timeConfig_t 4 1 vtxConfig_t 55 4 vtxSettingsConfig_t 12 2 +pinioBoxConfig_t 4 1 diff --git a/docs/ADSB.md b/docs/ADSB.md index 2e5bef69dfe..5cbbb52516a 100644 --- a/docs/ADSB.md +++ b/docs/ADSB.md @@ -34,6 +34,7 @@ All ADSB receivers which can send Mavlink [ADSB_VEHICLE](https://mavlink.io/en/m * [PINGRX](https://uavionix.com/product/pingrx-pro/) (not tested) * [TT-SC1](https://www.aerobits.pl/product/aero/) (tested) * [ADSBee1090](https://pantsforbirds.com/adsbee-1090/) (tested) +* [SoftRF](https://github.com/lyusupov/SoftRF/wiki/Nano-Edition) (tested) ## TT-SC1 settings * download software for ADSB TT-SC1 from https://www.aerobits.pl/product/aero/ , file Micro_ADSB_App-vX.XX.X_win_setup.zip and install it @@ -63,3 +64,48 @@ AT+SETTINGS=SAVE * in INAV configurator ports TAB set telemetry MAVLINK, and baudrate 115200 * https://pantsforbirds.com/adsbee-1090/quick-start/ +## SoftRF settings +SoftRF supports only MAVLink version 1. +``` +set mavlink_version = 1 +save +``` +The baud rate for SoftRF is 57600. INAV provides minimal support for SoftRF and supports only +the mandatory MAVLink messages: `MAVLINK_MSG_ID_HEARTBEAT`, `MAVLINK_MSG_ID_SYSTEM_TIME`, and `MAVLINK_MSG_ID_GPS_RAW_INT`. + +The following messages are not supported: `MAVLINK_MSG_ID_SYS_STATUS`, `MAVLINK_MSG_ID_VFR_HUD`, and `MAVLINK_MSG_ID_ATTITUDE`. + +## Alert and Warning +The ADS-B warning/alert system supports two operating modes, controlled by the parameter osd_adsb_calculation_use_cpa (ON or OFF). + +--- + +### ADS-B Warning and Alert Messages (CPA Mode OFF) +The ADS-B warning/alert system supports two operating modes, controlled by the parameter **osd_adsb_calculation_use_cpa** (ON or OFF). + +When **osd_adsb_calculation_use_cpa = OFF**, the system evaluates only the **current distance between the aircraft and the UAV**. The aircraft with the **shortest distance** is always selected for monitoring. + +- If the aircraft enters the **warning zone** (`adsb_distance_warning`), the corresponding **OSD element is displayed**. +- If the aircraft enters the **alert zone** (`adsb_distance_alert`), the **OSD element starts blinking**, indicating a higher-priority alert. + +This mode therefore provides a simple proximity-based warning determined purely by real-time distance. + +--- + +### ADS-B Warning and Alert Messages (CPA Mode ON) + +When **osd_adsb_calculation_use_cpa = ON**, the system evaluates aircraft using the **Closest Point of Approach (CPA)** and predicted trajectories, not only the current distance. + +1. **Aircraft already inside the alert zone** + If one or more aircraft are currently inside the **alert zone** (`adsb_distance_alert`), the **closest aircraft** to the UAV is selected and the **OSD element blinks**. + +2. **Aircraft in the warning zone, none predicted to enter the alert zone** + If aircraft are present in the **warning zone** (`adsb_distance_warning`), but none of them are predicted to enter the **alert zone** (their CPA distance is greater than `adsb_distance_alert`), the **closest aircraft to the UAV** is selected and the **OSD element remains steady** (no blinking). + +3. **Aircraft in the warning zone, one predicted to enter the alert zone** + If at least one aircraft in the **warning zone** is predicted to enter the **alert zone**, that aircraft is selected and the **OSD element blinks**. + +4. **Aircraft in the warning zone, multiple predicted to enter the alert zone** + If multiple aircraft are predicted to enter the **alert zone**, the system selects the aircraft that will **reach the alert zone first**, and the **OSD element blinks**. + +![ADSB CPA_ON](assets/images/adsb-CPA-on.png) diff --git a/docs/Display.md b/docs/Display.md index 952f9b08661..771b701588f 100755 --- a/docs/Display.md +++ b/docs/Display.md @@ -44,6 +44,10 @@ Links to displays: The banggood.com display is the cheapest at the time of writing and will correctly send I2C ACK signals. +### SSD1315 OLED displays + +SSD1315 is newer generation drop in replacement for SSD1306. Tested and working unit is bought from https://www.aliexpress.com/item/1005006901360788.html + #### Crius CO-16 This display is best avoided but will work if you modify it. diff --git a/docs/Mixer.md b/docs/Mixer.md index 1e3cc1369e2..eb2901295a3 100644 --- a/docs/Mixer.md +++ b/docs/Mixer.md @@ -82,7 +82,7 @@ Each servo mixing rule has the following parameters: | 26 | Stabilized PITCH- | Clipped between -1000 and 0 | | 27 | Stabilized YAW+ | Clipped between 0 and 1000 | | 28 | Stabilized YAW- | Clipped between -1000 and 0 | -| 29 | MAX | Constant value of 500 | +| 29 | FIXED-VALUE (MAX) | Fixed percentage of total range | The `smix reset` command removes all the existing motor mixing rules. @@ -103,4 +103,4 @@ Other usages can be: * automatic parachute deployment * VTOL and especially tail-sitters that require change in mixings during flight mode transition * crowbar airbrakes -* any kind of servo mixings that should be changed during flight \ No newline at end of file +* any kind of servo mixings that should be changed during flight diff --git a/docs/Settings.md b/docs/Settings.md index 39e3de90d52..3d7351dc371 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -6052,16 +6052,6 @@ When feature SERIALRX is enabled, this allows connection to several receivers wh --- -### servo_autotrim_iterm_rate_limit - -Maximum I-term rate of change (units/sec) for autotrim to be applied. Prevents trim updates during maneuver transitions when I-term is changing rapidly. Only applies when using `feature FW_AUTOTRIM`. - -| Default | Min | Max | -| --- | --- | --- | -| 2 | 0 | 50 | - ---- - ### servo_autotrim_rotation_limit Servo midpoints are only updated when total aircraft rotation is less than this threshold [deg/s]. Only applies when using `feature FW_AUTOTRIM`. diff --git a/docs/VTOL.md b/docs/VTOL.md index 8341c81086d..403e80d21e7 100644 --- a/docs/VTOL.md +++ b/docs/VTOL.md @@ -133,7 +133,7 @@ save ![Alt text](Screenshots/mixerprofile_fw_mixer.png) -You must also assign the tilting servos values using the MAX values. If you don't do this the motors will point in the direction assigned by the transition mode. +You must also assign the tilting servos values using the Fixed Value values (formerly called "MAX"). If you don't do this the motors will point in the direction assigned by the transition mode. # STEP 2: Configuring as a Multi-Copter in Profile 2 @@ -149,7 +149,7 @@ You must also assign the tilting servos values using the MAX values. If you don 2. **Configure the Multicopter/tricopter:** - Set up your multi-copter/fixed-wing as usual, this time for mixer_profile 2 and control_profile 2. - - Utilize the 'MAX' input in the servo mixer to tilt the motors without altering the servo midpoint. + - Utilize the Fixed Value values (formerly called "MAX") input in the servo mixer to tilt the motors without altering the servo midpoint. - At this stage, focus on configuring profile-specific settings. You can streamline this process by copying and pasting the default PID settings. - you can set -1 in motor mixer throttle as a place holder: this will disable that motor but will load following the motor rules - compass is required to enable navigation modes for multi-rotor profile. @@ -199,7 +199,7 @@ The steps below describe how you can fine-tune the tilting servos to obtian the 2. **Switch to Multicopter/Tricopter:** - Assuming that you have set up your mixer similar to STEP1 and STEP2, you can now switch to the tricopter/multicopter mode and your servos should be tilting the motors upwards. If this is not the case, reverse the servo(s) in the Outputs tab such that the servo(s) is/are pointed upwards. - It is OK for the servos not to point exactly 90 degrees upwards, but they should be as close as possible to that position. - - Also, ensure that your MAX values in the Mixer tab are at 100 and -100, so that your servo will move to the maximum position, as shown in the screenshots in STEP1 and STEP2. + - Also, ensure that your Fixed Value values (formerly called "MAX") values in the Mixer tab are at 100 and -100, so that your servo will move to the maximum position, as shown in the screenshots in STEP1 and STEP2. 3. **Adjust the maximum throws for the Multicopter/Tricopter mode:** - While in tricopter mode, navigate to the Outputs tab and adjust the MIN and MAX endpoint values to position the motors slightly backward. @@ -214,7 +214,7 @@ The steps below describe how you can fine-tune the tilting servos to obtian the 5. **Adjsut the vertival position of the tilt servos:** - Switch back to multicopter/tricopter mode and open the Mixer tab. - - Start adjusting the `MAX` mixer lines from STEP2 such that the servos are pointed exactly upwards. In other words, start reducing the values of 100 and -100 to something like 80 and -80 until the motors are are pointed exaxctly upwards. + - Start adjusting the `Fixed Value` mixer lines from STEP2 such that the servos are pointed exactly upwards. In other words, start reducing the values of 100 and -100 to something like 80 and -80 until the motors are are pointed exaxctly upwards. - You will have to `Save & reboot` for adjustement for the changes to take effect, so be patient, take your time and don't forget to `Save & reboot`. - Move the YAW stick to either extreme position and ensure that the servos are tilting the motors both forwards and backwards. - NOTE: When yawing fully left, the left motor should tilt backwards and the right motor should tilt forwards. diff --git a/docs/boards/FlyingRC F4Wing Mini.md b/docs/boards/FlyingRC F4Wing Mini.md index b51907870dc..237d1c9e33e 100644 --- a/docs/boards/FlyingRC F4Wing Mini.md +++ b/docs/boards/FlyingRC F4Wing Mini.md @@ -1,19 +1,21 @@ -# Board - FLYINGRCF4WINGMINI_NOT_RECOMMENDED +# Board - FLYINGRCF4WINGMINI -This is a cheap flight controller (prices range from $16US to $40US) from an unknown company. Many of the components on this FC are likely to have high tollerances due to the low cost. They sold this FC as compatible with INAV without reaching out to the team or having an official target made. The target only exists thanks to a community contributor (dixi83). +This is a budget-friendly flight controller. Component tolerances and quality control may vary compared with higher-cost hardware, so users should test critical functions carefully before flying. The board has been advertised as INAV-compatible, but it was not originally submitted through the official INAV target process; current support exists thanks to community work, especially the contribution from dixi83. -Hardware issues have been reported on these flight controllers. They are also missing many features. Unlike most other _wing_ flight controllers. This is not an all in one solution. It requires an external power source for servos. So is not as small or light as it first appears. +The manufaturer, FlyingRC, now has a GitHub available: https://github.com/FlyingRC-Official + +Hardware issues have been reported on these flight controllers. They are also missing many features. Unlike most other _wing_ flight controllers, this is not an all-in-one solution. It requires an external power source for servos, so is not as small or light as it first appears. > [!WARNING] -> We recommend you only use this flight controller on very light aircraft that you will keep within line-of-sight distances. Reliability of the hardware is far from guaranteed. So fitting to a larger, heavier aircraft adds unneccesary safety risks. Also, there are essential features missing for other types of flights. Please keep this for small park fliers only, if used at all. +> We recommend you only use this flight controller on very light aircraft that you will keep within line-of-sight distances. Reliability of the hardware is far from guaranteed. So fitting to a larger, heavier aircraft adds unnecessary safety risks. Also, there are essential features missing for other types of flights. Please keep this for small park fliers only. > -> Also, if you insist on buying one of these. Make sure it's from a somewhere selling it at $16US. Spending $40US on this is a waste of money. You can get better FCs for around that money. +> Also, if you choose to buy one of these, compare pricing carefully. At around US$40 there are more capable flight controllers available. ## Specifications | | | |-----|-----| | MCU | STM32F405RTG6 | -| Gyro | ICM-42605 | +| Gyro | ICM-42605 / BMI270 | | Baro | SPL06 | | UARTS | 1, 2 (RX only - SBUS), 4 (DJI), 5 | | PWM | Six + One (S12 used for LED control) | @@ -22,7 +24,7 @@ Hardware issues have been reported on these flight controllers. They are also mi | Weight | 2.8g | > [!NOTE] -> There is conflicting information for the power this FC can handle. There are 2 specs providied: +> There is conflicting information for the power this FC can handle. There are 2 specs provided: > | | | > |---|---| > | Voltmeter | 2.5-30V | @@ -31,12 +33,10 @@ Hardware issues have been reported on these flight controllers. They are also mi > There is no ADC for "voltmeter" input. So potentially this FC can run at 1S to 6S. However there are only 2 LDO regulators on the FC itself. ## Notable missing features -* Current sensor +* Current sensor on earlier hardware revisions (V4 adds a backside ADC current-sensor pad) * Blackbox recording * Analogue OSD * PINIO (for VTX power switching etc) -* ADCs (external current sensor, airspeed sensor, rssi, etc) +* Additional ADCs (airspeed sensor, RSSI, etc) * On-board power rail for servos * Filtered power for video - -All the above can be found on the Matek F405-WMO. Which is a $45US flight controller. Which is 32 x 22 x 12.7 mm and 9g, and has a definite input voltage range of 2S to 6S, and able to handle up to 132A. \ No newline at end of file diff --git a/docs/development/release-create.md b/docs/development/release-create.md index a2cb4994a4c..a75f8f6848a 100644 --- a/docs/development/release-create.md +++ b/docs/development/release-create.md @@ -2,11 +2,11 @@ This document describes the process for creating INAV firmware and configurator releases. -> **Note:** This document is designed to be used with coding assistants (such as Claude Code) that can execute the commands and automate parts of the release process. Update this document with lessons learned after each release. +> **Note:** This document is designed to be used with coding assistants (such as Claude Code) that can execute the commands and automate parts of the release process. Update this document with lessons learned after each release. Sensei has written more detailed guides for his process in the third-party repo https://github.com/sensei-hacker/inav-claude/tree/master/claude/release-manager ## CRITICAL PRINCIPLE: Verify Builds BEFORE Creating Tags -**Never tag a commit that hasn't been proven to build successfully.** +**Never tag a commit that hasn't been fully tested successfully.** Order of operations: 1. Merge all firmware PRs to the release branch @@ -93,6 +93,22 @@ Version numbers are set in: - View: `jq -r .version package.json` (or `node -p "require('./package.json').version"`) - Update: `npm version --no-git-tag-version` +## Version String Format (RC Releases) + +**CRITICAL:** Establish the canonical version string before starting any release work. + +RC version strings must use **lowercase `rc`** joined to the version with a **hyphen**: + +| Correct | Wrong | +|---------|-------| +| `9.1.0-rc1` | `9.1.0-RC1` | +| `9.1.0-rc2` | `9.1.0_RC2` | +| `9.0.0-rc3` | `9.0.0-rc_3` | + +The Configurator firmware flasher uses a case-sensitive regex to parse firmware filenames. Uppercase `RC` or underscore separators cause the target board name to be misread, making the firmware invisible in the flasher even after a successful release upload. + +--- + ## Pre-Release Checklist ### Code Readiness @@ -318,6 +334,20 @@ LAST_TAG=$(git describe --tags --abbrev=0) gh pr list --state merged --search "merged:>=$(git log -1 --format=%ai $LAST_TAG | cut -d' ' -f1)" --limit 100 ``` +### Verify Each PR Is on the Correct Branch + +**Before including a PR in release notes**, confirm it is actually merged into the release branch, not a future branch. `gh pr list` shows PRs by merge date regardless of target branch — a PR merged to `maintenance-10.x` will appear even though it's not in the current release. + +```bash +# Confirm a PR's merge commit exists on the release branch +git log upstream/maintenance-9.x --oneline | grep + +# Or check all recent merge commits on the branch +git log upstream/maintenance-9.x --oneline --merges | head -30 +``` + +If a PR is not in that output, exclude it from the release notes. + ### Using git log ```bash @@ -403,7 +433,7 @@ make MATEKF405 MATEKF722 Remove CI suffix and add RC number for RC releases: ```bash -RC_NUM="RC2" # Empty for final releases +RC_NUM="rc2" # Empty for final releases # Check if any .hex files exist to avoid errors with the glob if compgen -G "*.hex" > /dev/null; then @@ -411,7 +441,7 @@ if compgen -G "*.hex" > /dev/null; then target=$(echo "$f" | sed -E 's/inav_[0-9]+\.[0-9]+\.[0-9]+_(.*)_ci-.*/\1/') version=$(echo "$f" | sed -E 's/inav_([0-9]+\.[0-9]+\.[0-9]+)_.*/\1/') if [ -n "$RC_NUM" ]; then - mv "$f" "inav_${version}_${RC_NUM}_${target}.hex" + mv "$f" "inav_${version}-${RC_NUM}_${target}.hex" else mv "$f" "inav_${version}_${target}.hex" fi @@ -429,29 +459,46 @@ Download from GitHub Actions CI: # List recent workflow runs gh run list --repo iNavFlight/inav-configurator --limit 10 -# Download artifacts +# Download artifacts (creates one subdirectory per platform artifact) gh run download --repo iNavFlight/inav-configurator -# Flatten directory structure -find . -mindepth 2 -type f -exec mv -t . {} + -# Remove the now-empty subdirectories -find . -mindepth 1 -type d -empty -delete +# CRITICAL: Organize by platform — NEVER flatten all files into one directory. +# Flattening can put Windows .exe files inside macOS DMGs (caused a 9.0.0 release incident). +mkdir -p linux/ macos/ windows/ +mv INAV-Configurator_linux_*/* linux/ +mv INAV-Configurator_macOS*/* macos/ +mv INAV-Configurator_win_*/* windows/ +rmdir INAV-Configurator_* ``` ## Creating GitHub Releases ### Create Draft Release -```bash -# Firmware -cd inav -gh release create --draft --title "INAV " --notes-file release-notes.md -gh release upload *.hex +For RC releases, add `--prerelease` so GitHub marks them as pre-release and they don't appear as the latest stable release. Use `--target ` to tag a specific commit (safer than tagging the current HEAD, and works even when the local repo is locked). -# Configurator -cd inav-configurator -gh release create --draft --title "INAV Configurator " --notes-file release-notes.md -gh release upload *.zip *.dmg *.exe *.AppImage *.deb *.rpm *.msi +```bash +# Firmware (RC release) +gh release create 9.1.0-rc1 \ + --repo iNavFlight/inav \ + --target \ + --title "INAV 9.1.0-rc1 release candidate for testing" \ + --notes-file release-notes.md \ + --prerelease \ + --draft +gh release upload 9.1.0-rc1 firmware-dir/*.hex --repo iNavFlight/inav + +# Configurator (RC release) +gh release create 9.1.0-rc1 \ + --repo iNavFlight/inav-configurator \ + --target \ + --title "INAV Configurator 9.1.0-rc1 release candidate for testing" \ + --notes-file release-notes.md \ + --prerelease \ + --draft +gh release upload 9.1.0-rc1 linux/* macos/* windows/* --repo iNavFlight/inav-configurator + +# Final releases: same commands, omit --prerelease ``` ### Managing Release Assets @@ -477,16 +524,32 @@ gh api -X DELETE "repos/iNavFlight/inav/releases/assets/ASSET_ID" ### Publish Release +**Publish firmware first, then verify the Configurator can see it before publishing the Configurator release.** + +```bash +# Step 1: Publish firmware release +gh release edit --repo iNavFlight/inav --draft=false +``` + +**Step 2: Verify firmware appears in Configurator Firmware Flasher (human step)** + +Open INAV Configurator → Firmware Flasher tab → enable "Show unstable releases". The new firmware version must appear in the release list. This confirms the GitHub release is properly formatted and the filename regex parsed correctly. + +Also select a target whose name contains spaces (e.g., `MAMBAH743 2022B GYRO2`) and confirm it displays with spaces, not underscores — this validates that multi-word target names parsed correctly. + +If the firmware does not appear: check that filenames follow `inav_-rc_.hex` exactly (lowercase `rc`, hyphen separator). See [Asset Naming Conventions](#asset-naming-conventions). + ```bash -gh release edit --draft=false +# Step 3: Publish configurator release (only after firmware verified in flasher) +gh release edit --repo iNavFlight/inav-configurator --draft=false ``` ## Asset Naming Conventions -**Firmware (RC releases):** `inav__RC_.hex` +**Firmware (RC releases):** `inav_-rc_.hex` **Firmware (final):** `inav__.hex` -**Configurator (RC releases):** `INAV-Configurator___RC.` +**Configurator (RC releases):** `INAV-Configurator__-rc.` **Configurator (final):** `INAV-Configurator__.` ## Maintenance Branches diff --git a/src/main/drivers/flash_m25p16.c b/src/main/drivers/flash_m25p16.c index 2db61a3a166..c6d5543e448 100644 --- a/src/main/drivers/flash_m25p16.c +++ b/src/main/drivers/flash_m25p16.c @@ -118,6 +118,9 @@ struct { {0x1C3017, 128, 256}, // JEDEC_ID_SPANSION_S25FL116 {0x014015, 32, 256 }, + // JEDEC_ID_XTX_XT25F128F + // Datasheet: https://www.xtxtech.com/Products/info_productModel_XT25F128FSSIGT.html + {0x0B4018, 256, 256 }, // End of list {0x000000, 0, 0}}; diff --git a/src/main/drivers/timer_impl_hal.c b/src/main/drivers/timer_impl_hal.c index 9c4093bafa8..a24875eec08 100644 --- a/src/main/drivers/timer_impl_hal.c +++ b/src/main/drivers/timer_impl_hal.c @@ -330,8 +330,8 @@ static void impl_timerDMA_IRQHandler(DMA_t descriptor) tch->dmaState = TCH_DMA_IDLE; } - LL_DMA_DisableStream(tch->dma->dma, lookupDMALLStreamTable[DMATAG_GET_STREAM(tch->timHw->dmaTag)]); LL_TIM_DisableDMAReq_CCx(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex]); + LL_DMA_DisableStream(tch->dma->dma, lookupDMALLStreamTable[DMATAG_GET_STREAM(tch->timHw->dmaTag)]); DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF); } diff --git a/src/main/drivers/timer_impl_stdperiph.c b/src/main/drivers/timer_impl_stdperiph.c index 5b52cb862a4..13b38cdd2b3 100644 --- a/src/main/drivers/timer_impl_stdperiph.c +++ b/src/main/drivers/timer_impl_stdperiph.c @@ -279,8 +279,8 @@ static void impl_timerDMA_IRQHandler(DMA_t descriptor) tch->dmaState = TCH_DMA_IDLE; - DMA_Cmd(tch->dma->ref, DISABLE); TIM_DMACmd(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex], DISABLE); + DMA_Cmd(tch->dma->ref, DISABLE); DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF); } @@ -518,8 +518,8 @@ void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferElementCount) // Clear the flag as well, so even if DMA transfer finishes while within ATOMIC_BLOCK // the resulting IRQ won't mess up the DMA state ATOMIC_BLOCK(NVIC_PRIO_MAX) { - DMA_Cmd(tch->dma->ref, DISABLE); TIM_DMACmd(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex], DISABLE); + DMA_Cmd(tch->dma->ref, DISABLE); DMA_CLEAR_FLAG(tch->dma, DMA_IT_TCIF); } @@ -561,8 +561,8 @@ void impl_timerPWMStartDMA(TCH_t * tch) void impl_timerPWMStopDMA(TCH_t * tch) { - DMA_Cmd(tch->dma->ref, DISABLE); TIM_DMACmd(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex], DISABLE); + DMA_Cmd(tch->dma->ref, DISABLE); tch->dmaState = TCH_DMA_IDLE; TIM_Cmd(tch->timHw->tim, ENABLE); } diff --git a/src/main/drivers/timer_impl_stdperiph_at32.c b/src/main/drivers/timer_impl_stdperiph_at32.c index 782943ab25d..54c6d257078 100644 --- a/src/main/drivers/timer_impl_stdperiph_at32.c +++ b/src/main/drivers/timer_impl_stdperiph_at32.c @@ -409,8 +409,8 @@ void impl_timerPWMStartDMA(TCH_t * tch) void impl_timerPWMStopDMA(TCH_t * tch) { - dma_channel_enable(tch->dma->ref,FALSE); tmr_dma_request_enable(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex], FALSE); + dma_channel_enable(tch->dma->ref,FALSE); tch->dmaState = TCH_DMA_IDLE; tmr_counter_enable(tch->timHw->tim, TRUE); } diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 0d6e75627a1..9bb2c776883 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -2700,6 +2700,8 @@ static void cliPid(char *cmdline) { programmingPidsMutable(i)->gains.D = args[D_GAIN]; programmingPidsMutable(i)->gains.FF = args[FF_GAIN]; + programmingPidInit(); + cliPid(""); } else { cliShowParseError(); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 75372ff8825..3ec3f89f3f0 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2399,6 +2399,9 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) programmingPidsMutable(tmp_u8)->gains.I = sbufReadU16(src); programmingPidsMutable(tmp_u8)->gains.D = sbufReadU16(src); programmingPidsMutable(tmp_u8)->gains.FF = sbufReadU16(src); + + programmingPidInit(); + } else return MSP_RESULT_ERROR; break; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 6ef471e3bb0..4e8affb0221 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1361,11 +1361,6 @@ groups: default_value: 15 min: 1 max: 60 - - name: servo_autotrim_iterm_rate_limit - description: "Maximum I-term rate of change (units/sec) for autotrim to be applied. Prevents trim updates during maneuver transitions when I-term is changing rapidly. Only applies when using `feature FW_AUTOTRIM`." - default_value: 2 - min: 0 - max: 50 - name: PG_CONTROL_PROFILES type: controlConfig_t diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 379a023ffd8..e5d2e3be75f 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -729,8 +729,17 @@ static void imuCalculateTurnRateacceleration(fpVector3_t *vEstcentrifugalAccelBF //fixed wing only static float lastspeed = -1.0f; float currentspeed = 0; +#ifdef USE_PITOT + if (pitotGetValidForAirspeed()) + { + // first choice is airspeed + currentspeed = getAirspeedEstimate(); + *acc_ignore_slope_multipiler = 4.0f; + } + else +#endif if (isGPSTrustworthy()) { - //first speed choice is gps + // second choice is gps static bool lastGPSHeartbeat; static pt1Filter_t GPS3DspeedFilter; static float GPS3DspeedFiltered = 0.0f; @@ -742,13 +751,6 @@ static void imuCalculateTurnRateacceleration(fpVector3_t *vEstcentrifugalAccelBF currentspeed = GPS3DspeedFiltered; *acc_ignore_slope_multipiler = 4.0f; } -#ifdef USE_PITOT - else if (sensors(SENSOR_PITOT) && pitotIsHealthy()) { - // second choice is pitot - currentspeed = getAirspeedEstimate(); - *acc_ignore_slope_multipiler = 2.0f; - } -#endif else { //third choice is fixedWingReferenceAirspeed diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 354ffeff371..9beda7ae225 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -553,7 +553,7 @@ void updatePIDCoefficients(void) float tpaFactor=1.0f; float iTermFactor=1.0f; // Separate factor for I-term scaling if(usedPidControllerType == PID_TYPE_PIFF){ // Fixed wing TPA calculation - if(currentControlProfile->throttle.apa_pow>0 && pitotValidForAirspeed()){ + if(currentControlProfile->throttle.apa_pow>0 && pitotGetValidForAirspeed()){ tpaFactor = calculateFixedWingAirspeedTPAFactor(); iTermFactor = calculateFixedWingAirspeedITermFactor(); // Less aggressive I-term scaling }else{ diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 9f6eb4851a7..e72dd66ea7d 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -603,11 +603,6 @@ void processServoAutotrimMode(void) } } -#define SERVO_AUTOTRIM_FILTER_CUTOFF 1 // LPF cutoff frequency -#define SERVO_AUTOTRIM_CENTER_MIN 1300 -#define SERVO_AUTOTRIM_CENTER_MAX 1700 -#define SERVO_AUTOTRIM_UPDATE_SIZE 5 -#define SERVO_AUTOTRIM_ATTITUDE_LIMIT 50 // 5 degrees void processContinuousServoAutotrim(const float dT) { @@ -646,7 +641,7 @@ void processContinuousServoAutotrim(const float dT) for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { // For each stabilized axis, add 5 units of I-term to all associated servo midpoints const float axisIterm = getAxisIterm(axis); - const bool itermIsStable = itermRateOfChange[axis] < servoConfig()->servo_autotrim_iterm_rate_limit; + const bool itermIsStable = itermRateOfChange[axis] < SERVO_AUTOTRIM_ITERM_RATE_LIMIT; if (fabsf(axisIterm) > SERVO_AUTOTRIM_UPDATE_SIZE && itermIsStable) { const int8_t ItermUpdate = axisIterm > 0.0f ? SERVO_AUTOTRIM_UPDATE_SIZE : -SERVO_AUTOTRIM_UPDATE_SIZE; diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h index b16dd7ca915..3f8ebf1b4e8 100644 --- a/src/main/flight/servos.h +++ b/src/main/flight/servos.h @@ -161,6 +161,14 @@ typedef struct servoParam_s { PG_DECLARE_ARRAY(servoParam_t, MAX_SUPPORTED_SERVOS, servoParams); + +#define SERVO_AUTOTRIM_FILTER_CUTOFF 1.0f // LPF cutoff frequency +#define SERVO_AUTOTRIM_CENTER_MIN 1300 +#define SERVO_AUTOTRIM_CENTER_MAX 1700 +#define SERVO_AUTOTRIM_UPDATE_SIZE 5 +#define SERVO_AUTOTRIM_ATTITUDE_LIMIT 50 // 5 degrees +#define SERVO_AUTOTRIM_ITERM_RATE_LIMIT 30 // ~90th percentile during stable cruise (blackbox-derived) + typedef struct servoConfig_s { // PWM values, in milliseconds, common range is 1000-2000 (1ms to 2ms) uint16_t servoCenterPulse; // This is the value for servos when they should be in the middle. e.g. 1500. @@ -171,7 +179,6 @@ typedef struct servoConfig_s { uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed uint8_t servo_autotrim_rotation_limit; // Max rotation for servo midpoints to be updated uint8_t servo_autotrim_iterm_threshold; // How much of the Iterm is carried over to the servo midpoints on each update - uint8_t servo_autotrim_iterm_rate_limit; // Max I-term rate of change (units/sec) to apply autotrim } servoConfig_t; PG_DECLARE(servoConfig_t, servoConfig); diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index af0f826c88c..c8cc7769921 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -345,7 +345,9 @@ void beeperUpdate(timeUs_t currentTimeUs) if (!beeperIsOn) { #ifdef USE_DSHOT if (isMotorProtocolDshot() && !areMotorsRunning() && beeperConfig()->dshot_beeper_enabled - && currentTimeUs - lastDshotBeeperCommandTimeUs > getDShotBeaconGuardDelayUs()) + && currentTimeUs - lastDshotBeeperCommandTimeUs > getDShotBeaconGuardDelayUs() + && currentBeeperEntry->sequence[beeperPos] != 0 // added beeper timeout so dshot does not beep on "off" + && !(getBeeperOffMask() & (1 << (currentBeeperEntry->mode - 1)))) // added beeper ignore to dshot beacon { lastDshotBeeperCommandTimeUs = currentTimeUs; sendDShotCommand(beeperConfig()->dshot_beeper_tone); diff --git a/src/main/io/displayport_msp_osd.c b/src/main/io/displayport_msp_osd.c index 4963a09b97b..6b674a300ec 100644 --- a/src/main/io/displayport_msp_osd.c +++ b/src/main/io/displayport_msp_osd.c @@ -119,7 +119,7 @@ static void checkVtxPresent(void) vtxActive = false; } - if (ARMING_FLAG(SIMULATOR_MODE_HITL)) { + if (ARMING_FLAG(SIMULATOR_MODE_HITL)) { vtxActive = true; } } @@ -243,10 +243,11 @@ static int writeString(displayPort_t *displayPort, uint8_t col, uint8_t row, con */ static int drawScreen(displayPort_t *displayPort) // 250Hz { +#ifdef USE_SIMULATOR if (SIMULATOR_HAS_OPTION(HITL_SITL_MODE)) { vtxActive = true; } - +#endif static uint8_t counter = 0; if ((!cmsInMenu && IS_RC_MODE_ACTIVE(BOXOSD)) || (counter++ % DRAW_FREQ_DENOM)) { // 62.5Hz @@ -258,14 +259,14 @@ static int drawScreen(displayPort_t *displayPort) // 250Hz uint8_t refreshSubcmd[1]; refreshSubcmd[0] = MSP_DP_CLEAR_SCREEN; output(displayPort, MSP_DISPLAYPORT, refreshSubcmd, sizeof(refreshSubcmd)); - + // Then dirty the characters that are not blank, to send all data on this draw. for (unsigned int pos = 0; pos < sizeof(screen); pos++) { if (screen[pos] != SYM_BLANK) { bitArraySet(dirty, pos); } } - + sendSubFrameMs = (osdConfig()->msp_displayport_fullframe_interval > 0) ? (millis() + DS2MS(osdConfig()->msp_displayport_fullframe_interval)) : 0; } @@ -570,7 +571,7 @@ static mspResult_e fixDjiBrokenO4ProcessMspCommand(mspPacket_t *cmd, mspPacket_t return processMspCommand(cmd, reply, mspPostProcessFn); } #else -#define fixDjiBrokenO4ProcessMspCommand processMspCommand +#define fixDjiBrokenO4ProcessMspCommand processMspCommand #endif void mspOsdSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index d0196308c1c..0bdb9e3375f 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -6156,10 +6156,10 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter /* WARNING: messageBuf is shared, use accordingly */ char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH + 1)]; - /* WARNING: ensure number of messages returned does not exceed messages array size - * Messages array set 1 larger than maximum expected message count of 7 */ + /* Messages array - use ADD_MSG() for bounds-safe access. */ const char *messages[8]; unsigned messageCount = 0; + #define ADD_MSG(msg) do { if (messageCount < ARRAYLEN(messages)) messages[messageCount++] = (msg); } while(0) const char *failsafeInfoMessage = NULL; const char *invertedInfoMessage = NULL; @@ -6168,16 +6168,16 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { /* ADDS MAXIMUM OF 3 MESSAGES TO TOTAL NORMALLY, 5 MESSAGES DURING FAILSAFE */ if (navGetCurrentStateFlags() & NAV_AUTO_WP_DONE) { - messages[messageCount++] = STATE(LANDING_DETECTED) ? OSD_MESSAGE_STR(OSD_MSG_WP_LANDED) : OSD_MESSAGE_STR(OSD_MSG_WP_FINISHED); + ADD_MSG(STATE(LANDING_DETECTED) ? OSD_MESSAGE_STR(OSD_MSG_WP_LANDED) : OSD_MESSAGE_STR(OSD_MSG_WP_FINISHED)); } else if (NAV_Status.state == MW_NAV_STATE_WP_ENROUTE) { // Countdown display for remaining Waypoints char buf[6]; osdFormatDistanceSymbol(buf, posControl.wpDistance, 0, 3); tfp_sprintf(messageBuf, "TO WP %u/%u (%s)", getGeoWaypointNumber(posControl.activeWaypointIndex), posControl.geoWaypointCount, buf); - messages[messageCount++] = messageBuf; + ADD_MSG(messageBuf); } else if (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED) { if (navConfig()->general.waypoint_enforce_altitude && !posControl.wpAltitudeReached) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ADJUSTING_WP_ALT); + ADD_MSG(OSD_MESSAGE_STR(OSD_MSG_ADJUSTING_WP_ALT)); } else { // WP hold time countdown in seconds timeMs_t currentTime = millis(); @@ -6186,25 +6186,25 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter tfp_sprintf(messageBuf, "HOLDING WP FOR %2u S", holdTimeRemaining); - messages[messageCount++] = messageBuf; + ADD_MSG(messageBuf); } } else { const char *navStateMessage = navigationStateMessage(); if (navStateMessage) { - messages[messageCount++] = navStateMessage; + ADD_MSG(navStateMessage); } } #if defined(USE_SAFE_HOME) const char *safehomeMessage = divertingToSafehomeMessage(); if (safehomeMessage) { - messages[messageCount++] = safehomeMessage; + ADD_MSG(safehomeMessage); } #endif #ifdef USE_GEOZONE if (geozone.avoidInRTHInProgress) { - messages[messageCount++] = OSD_MSG_AVOID_ZONES_RTH; + ADD_MSG(OSD_MSG_AVOID_ZONES_RTH); } #endif if (FLIGHT_MODE(FAILSAFE_MODE)) { // In FS mode while armed @@ -6212,24 +6212,24 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter uint16_t remainingHoldSec = MS2S(posControl.landingDelay - millis()); tfp_sprintf(messageBuf, "LANDING DELAY: %3u SECONDS", remainingHoldSec); - messages[messageCount++] = messageBuf; + ADD_MSG(messageBuf); } const char *failsafePhaseMessage = osdFailsafePhaseMessage(); failsafeInfoMessage = osdFailsafeInfoMessage(); if (failsafePhaseMessage) { - messages[messageCount++] = failsafePhaseMessage; + ADD_MSG(failsafePhaseMessage); } if (failsafeInfoMessage) { - messages[messageCount++] = failsafeInfoMessage; + ADD_MSG(failsafeInfoMessage); } } else if (isWaypointMissionRTHActive()) { // if RTH activated whilst WP mode selected, remind pilot to cancel WP mode to exit RTH - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_WP_RTH_CANCEL); + ADD_MSG(OSD_MESSAGE_STR(OSD_MSG_WP_RTH_CANCEL)); } } else if (STATE(LANDING_DETECTED)) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_LANDED); + ADD_MSG(OSD_MESSAGE_STR(OSD_MSG_LANDED)); } else { /* Messages shown only when Failsafe, WP, RTH or Emergency Landing not active and landed state inactive */ /* ADDS MAXIMUM OF 5 MESSAGES TO TOTAL */ @@ -6237,15 +6237,15 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter char buf[12], buf1[12]; switch (geozone.messageState) { /* ADDS MAXIMUM OF 2 MESSAGES TO TOTAL */ case GEOZONE_MESSAGE_STATE_NFZ: - messages[messageCount++] = OSD_MSG_NFZ; + ADD_MSG(OSD_MSG_NFZ); break; case GEOZONE_MESSAGE_STATE_LEAVING_FZ: osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0, 3); tfp_sprintf(messageBuf, OSD_MSG_LEAVING_FZ, buf); - messages[messageCount++] = messageBuf; + ADD_MSG(messageBuf); break; case GEOZONE_MESSAGE_STATE_OUTSIDE_FZ: - messages[messageCount++] = OSD_MSG_OUTSIDE_FZ; + ADD_MSG(OSD_MSG_OUTSIDE_FZ); break; case GEOZONE_MESSAGE_STATE_ENTERING_NFZ: osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0, 3); @@ -6255,36 +6255,36 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter osdFormatAltitudeSymbol(buf1, geozone.zoneInfo); } tfp_sprintf(messageBuf, OSD_MSG_ENTERING_NFZ, buf, buf1); - messages[messageCount++] = messageBuf; + ADD_MSG(messageBuf); break; case GEOZONE_MESSAGE_STATE_AVOIDING_FB: - messages[messageCount++] = OSD_MSG_AVOIDING_FB; + ADD_MSG(OSD_MSG_AVOIDING_FB); if (!posControl.sendTo.lockSticks) { - messages[messageCount++] = OSD_MSG_MOVE_STICKS; + ADD_MSG(OSD_MSG_MOVE_STICKS); } break; case GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE: - messages[messageCount++] = OSD_MSG_RETURN_TO_ZONE; + ADD_MSG(OSD_MSG_RETURN_TO_ZONE); if (!posControl.sendTo.lockSticks) { - messages[messageCount++] = OSD_MSG_MOVE_STICKS; + ADD_MSG(OSD_MSG_MOVE_STICKS); } break; case GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH: - messages[messageCount++] = OSD_MSG_AVOIDING_ALT_BREACH; + ADD_MSG(OSD_MSG_AVOIDING_ALT_BREACH); if (!posControl.sendTo.lockSticks) { - messages[messageCount++] = OSD_MSG_MOVE_STICKS; + ADD_MSG(OSD_MSG_MOVE_STICKS); } break; case GEOZONE_MESSAGE_STATE_FLYOUT_NFZ: - messages[messageCount++] = OSD_MSG_FLYOUT_NFZ; + ADD_MSG(OSD_MSG_FLYOUT_NFZ); if (!posControl.sendTo.lockSticks) { - messages[messageCount++] = OSD_MSG_MOVE_STICKS; + ADD_MSG(OSD_MSG_MOVE_STICKS); } break; case GEOZONE_MESSAGE_STATE_POS_HOLD: - messages[messageCount++] = OSD_MSG_AVOIDING_FB; + ADD_MSG(OSD_MSG_AVOIDING_FB); if (!geozone.sticksLocked) { - messages[messageCount++] = OSD_MSG_MOVE_STICKS; + ADD_MSG(OSD_MSG_MOVE_STICKS); } break; case GEOZONE_MESSAGE_STATE_NONE: @@ -6294,37 +6294,37 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter if (STATE(AIRPLANE)) { /* ADDS MAXIMUM OF 3 MESSAGES TO TOTAL */ #ifdef USE_FW_AUTOLAND if (canFwLandingBeCancelled()) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_MOVE_STICKS); + ADD_MSG(OSD_MESSAGE_STR(OSD_MSG_MOVE_STICKS)); } else #endif if (navGetCurrentStateFlags() & NAV_CTL_LAUNCH) { - messages[messageCount++] = navConfig()->fw.launch_manual_throttle ? OSD_MESSAGE_STR(OSD_MSG_AUTOLAUNCH_MANUAL) : - OSD_MESSAGE_STR(OSD_MSG_AUTOLAUNCH); + ADD_MSG(navConfig()->fw.launch_manual_throttle ? OSD_MESSAGE_STR(OSD_MSG_AUTOLAUNCH_MANUAL) : + OSD_MESSAGE_STR(OSD_MSG_AUTOLAUNCH)); const char *launchStateMessage = fixedWingLaunchStateMessage(); if (launchStateMessage) { - messages[messageCount++] = launchStateMessage; + ADD_MSG(launchStateMessage); } } else if (FLIGHT_MODE(SOARING_MODE)) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_NAV_SOARING); + ADD_MSG(OSD_MESSAGE_STR(OSD_MSG_NAV_SOARING)); } else if (isFwAutoModeActive(BOXAUTOTUNE)) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTUNE); + ADD_MSG(OSD_MESSAGE_STR(OSD_MSG_AUTOTUNE)); if (FLIGHT_MODE(MANUAL_MODE)) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTUNE_ACRO); + ADD_MSG(OSD_MESSAGE_STR(OSD_MSG_AUTOTUNE_ACRO)); } } else if (isFwAutoModeActive(BOXAUTOTRIM) && !feature(FEATURE_FW_AUTOTRIM)) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTRIM); + ADD_MSG(OSD_MESSAGE_STR(OSD_MSG_AUTOTRIM)); } else if (isFixedWingLevelTrimActive()) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOLEVEL); + ADD_MSG(OSD_MESSAGE_STR(OSD_MSG_AUTOLEVEL)); } if (IS_RC_MODE_ACTIVE(BOXANGLEHOLD)) { int8_t navAngleHoldAxis = navCheckActiveAngleHoldAxis(); if (isAngleHoldLevel()) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_LEVEL); + ADD_MSG(OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_LEVEL)); } else if (navAngleHoldAxis == FD_ROLL) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_ROLL); + ADD_MSG(OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_ROLL)); } else if (navAngleHoldAxis == FD_PITCH) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_PITCH); + ADD_MSG(OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_PITCH)); } } } else if (STATE(MULTIROTOR)) { /* ADDS MAXIMUM OF 2 MESSAGES TO TOTAL */ @@ -6336,25 +6336,26 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter } else { strcpy(messageBuf, "(HOLD)"); } - messages[messageCount++] = messageBuf; + ADD_MSG(messageBuf); } else if (FLIGHT_MODE(HEADFREE_MODE)) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_HEADFREE); + ADD_MSG(OSD_MESSAGE_STR(OSD_MSG_HEADFREE)); } if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) { if (posControl.flags.isTerrainFollowEnabled) { if (posControl.flags.estAglStatus == EST_TRUSTED) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_SURFACE_OK); + ADD_MSG(OSD_MESSAGE_STR(OSD_MSG_SURFACE_OK)); } else { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_SURFACE_BAD); + ADD_MSG(OSD_MESSAGE_STR(OSD_MSG_SURFACE_BAD)); } } else if (!navigationRequiresAngleMode()) { /* If ALTHOLD is separately enabled for multirotor together with ANGL/HORIZON/ACRO modes * then ANGL/HORIZON/ACRO are indicated by the OSD_FLYMODE field. * In this case indicate ALTHOLD is active via a system message */ - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ALTITUDE_HOLD); + ADD_MSG(OSD_MESSAGE_STR(OSD_MSG_ALTITUDE_HOLD)); } + } } } @@ -6369,44 +6370,44 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter messageBuf[ii] = sl_toupper(messageBuf[ii]); } invertedInfoMessage = messageBuf; - messages[messageCount++] = invertedInfoMessage; + ADD_MSG(invertedInfoMessage); invertedInfoMessage = OSD_MESSAGE_STR(OSD_MSG_INVALID_SETTING); - messages[messageCount++] = invertedInfoMessage; + ADD_MSG(invertedInfoMessage); } else { invertedInfoMessage = OSD_MESSAGE_STR(OSD_MSG_UNABLE_ARM); - messages[messageCount++] = invertedInfoMessage; + ADD_MSG(invertedInfoMessage); // Show the reason for not arming - messages[messageCount++] = osdArmingDisabledReasonMessage(); + ADD_MSG(osdArmingDisabledReasonMessage()); } } else if (!ARMING_FLAG(ARMED)) { if (isWaypointListValid()) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_WP_MISSION_LOADED); + ADD_MSG(OSD_MESSAGE_STR(OSD_MSG_WP_MISSION_LOADED)); } } /* Messages that are shown regardless of Arming state */ /* ADDS MAXIMUM OF 2 MESSAGES TO TOTAL NORMALLY, 1 MESSAGE DURING FAILSAFE */ if (posControl.flags.wpMissionPlannerActive && !FLIGHT_MODE(FAILSAFE_MODE)) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_MISSION_PLANNER); + ADD_MSG(OSD_MESSAGE_STR(OSD_MSG_MISSION_PLANNER)); } // The following has been commented out as it will be added in #9688 // uint16_t rearmMs = (emergInflightRearmEnabled()) ? emergencyInFlightRearmTimeMS() : 0; if (savingSettings == true) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_SAVING_SETTNGS); + ADD_MSG(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]; tfp_sprintf(emReArmMsg, "** REARM PERIOD: "); tfp_sprintf(emReArmMsg + strlen(emReArmMsg), "%02d", (uint8_t)MS2S(rearmMs)); strcat(emReArmMsg, " **\0"); - messages[messageCount++] = OSD_MESSAGE_STR(emReArmMsg);*/ + ADD_MSG(OSD_MESSAGE_STR(emReArmMsg));*/ } else if (notify_settings_saved > 0) { if (millis() > notify_settings_saved) { notify_settings_saved = 0; } else { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_SETTINGS_SAVED); + ADD_MSG(OSD_MESSAGE_STR(OSD_MSG_SETTINGS_SAVED)); } } @@ -6429,6 +6430,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter osdFormatMessage(buff, buff_size, message, isCenteredText); } + #undef ADD_MSG return elemAttr; } diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c index 3618923614a..6c6f81aafcc 100755 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -70,8 +70,9 @@ pitot_t pitot = {.lastMeasurementUs = 0, .lastSeenHealthyMs = 0}; static bool pitotHardwareFailed = false; static uint16_t pitotFailureCounter = 0; static uint16_t pitotRecoveryCounter = 0; -#define PITOT_FAILURE_THRESHOLD 20 // 0.2 seconds at 100Hz - fast detection per LOG00002 analysis -#define PITOT_RECOVERY_THRESHOLD 200 // 2 seconds of consecutive good readings to recover +static bool pitotAirspeedValidCached = false; +#define PITOT_FAILURE_THRESHOLD 10 // 0.2 seconds at 50Hz - fast detection per LOG00002 analysis +#define PITOT_RECOVERY_THRESHOLD 100 // 2 seconds of consecutive good readings to recover // Forward declaration for GPS-based airspeed fallback static float getVirtualAirspeedEstimate(void); @@ -231,6 +232,7 @@ static void performPitotCalibrationCycle(void) } } + STATIC_PROTOTHREAD(pitotThread) { ptBegin(pitotThread); @@ -280,6 +282,7 @@ STATIC_PROTOTHREAD(pitotThread) pitotPressureTmp = sq(fakePitotGetAirspeed()) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE; } #endif + pitotAirspeedValidCached = pitotValidateAirspeed(); ptYield(); // Calculate IAS @@ -449,7 +452,7 @@ bool pitotHasFailed(void) return pitotHardwareFailed; } -bool pitotValidForAirspeed(void) +bool pitotValidateAirspeed(void) { bool ret = false; ret = pitotIsHealthy() && pitotIsCalibrationComplete(); @@ -508,4 +511,9 @@ bool pitotValidForAirspeed(void) return ret; } + +bool pitotGetValidForAirspeed(void) +{ + return pitotAirspeedValidCached; +} #endif /* PITOT */ diff --git a/src/main/sensors/pitotmeter.h b/src/main/sensors/pitotmeter.h index dc5ac422974..651059f4636 100755 --- a/src/main/sensors/pitotmeter.h +++ b/src/main/sensors/pitotmeter.h @@ -70,7 +70,8 @@ void pitotStartCalibration(void); void pitotUpdate(void); float getAirspeedEstimate(void); bool pitotIsHealthy(void); -bool pitotValidForAirspeed(void); +bool pitotValidateAirspeed(void); +bool pitotGetValidForAirspeed(void); bool pitotHasFailed(void); #endif diff --git a/src/main/target/AEDROXH7/CMakeLists.txt b/src/main/target/AEDROXH7/CMakeLists.txt new file mode 100644 index 00000000000..7842a1d933a --- /dev/null +++ b/src/main/target/AEDROXH7/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32h743xi(AEDROXH7) diff --git a/src/main/target/AEDROXH7/config.c b/src/main/target/AEDROXH7/config.c new file mode 100644 index 00000000000..de29bf3d716 --- /dev/null +++ b/src/main/target/AEDROXH7/config.c @@ -0,0 +1,52 @@ +/* + * 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 "config/config_master.h" + +#include "fc/fc_msp_box.h" +#include "fc/config.h" + +#include "io/serial.h" +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + // GPS on UART2 + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART2)].functionMask = FUNCTION_GPS; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART2)].gps_baudrateIndex = BAUD_115200; + + // ESC telemetry on UART7 (RX only) + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART7)].functionMask = FUNCTION_ESCSERIAL; + + // HD OSD via MSP DisplayPort on UART8 + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART8)].functionMask = FUNCTION_MSP; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART8)].msp_baudrateIndex = BAUD_115200; + + // PINIO boxes: USER1-4 mapped to switch boxes + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; + pinioBoxConfigMutable()->permanentId[2] = BOX_PERMANENT_ID_USER3; + pinioBoxConfigMutable()->permanentId[3] = BOX_PERMANENT_ID_USER4; + + // Enable PWM drive for passive beeper on PA7 / TIM3_CH2 + beeperConfigMutable()->pwmMode = true; +} diff --git a/src/main/target/AEDROXH7/target.c b/src/main/target/AEDROXH7/target.c new file mode 100644 index 00000000000..250f7ba4bba --- /dev/null +++ b/src/main/target/AEDROXH7/target.c @@ -0,0 +1,52 @@ +/* + * 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" + +// ICM42688P: DEVHW_ICM42605 driver detects both via WHO_AM_I +BUSDEV_REGISTER_SPI_TAG(busdev_icm42688, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, ICM42605_EXTI_PIN, 0, DEVFLAGS_NONE, IMU_ICM42605_ALIGN); + +timerHardware_t timerHardware[] = { + // Motors M1-M4: TIM8 (CH mapping follows board silkscreen, not channel order) + DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // M1 + DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 1), // M2 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 2), // M3 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 3), // M4 + + // Motors M5-M8: TIM1 on PE9/PE11/PE13/PE14 (AF1) + DEF_TIM(TIM1, CH1, PE9, TIM_USE_OUTPUT_AUTO, 0, 4), // M5 + DEF_TIM(TIM1, CH2, PE11, TIM_USE_OUTPUT_AUTO, 0, 5), // M6 + DEF_TIM(TIM1, CH3, PE13, TIM_USE_OUTPUT_AUTO, 0, 6), // M7 + DEF_TIM(TIM1, CH4, PE14, TIM_USE_OUTPUT_AUTO, 0, 7), // M8 + + // LED strip: TIM2_CH1 on PA5 (AF1) — separate timer from motors + DEF_TIM(TIM2, CH1, PA5, TIM_USE_LED, 0, 0), // LED strip + + // Beeper PWM: TIM3_CH2 on PA7 (AF2) + DEF_TIM(TIM3, CH2, PA7, TIM_USE_BEEPER, 0, 0), // Beeper +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/AEDROXH7/target.h b/src/main/target/AEDROXH7/target.h new file mode 100644 index 00000000000..16fa802d48b --- /dev/null +++ b/src/main/target/AEDROXH7/target.h @@ -0,0 +1,179 @@ +/* + * 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 "AEDH" +#define USBD_PRODUCT_STRING "AEDROXH7" + +#define USE_TARGET_CONFIG + +// *************** LEDs *************************** +#define LED0 PE5 +#define LED1 PE2 + +// *************** Beeper ************************* +#define BEEPER PA7 +#define BEEPER_INVERTED +#define BEEPER_PWM_FREQUENCY 2500 + +// *************** SPI1 - OSD (MAX7456) *********** +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PB3 +#define SPI1_MISO_PIN PB4 +#define SPI1_MOSI_PIN PB5 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI1 +#define MAX7456_CS_PIN PE4 + +// *************** SPI2 - Gyro (ICM42688P) ******** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS + +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW90_DEG +#define ICM42605_SPI_BUS BUS_SPI2 +#define ICM42605_CS_PIN PB12 +#define ICM42605_EXTI_PIN PC4 + +// *************** SPI3 - Flash (W25N01G) ********* +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PB2 +#define SPI3_NSS_PIN PA15 +// PB2 requires AF7 for SPI3 MOSI — AF6 is the default and incorrect for this pin +#define SPI3_SCK_AF GPIO_AF6_SPI3 +#define SPI3_MISO_AF GPIO_AF6_SPI3 +#define SPI3_MOSI_AF GPIO_AF7_SPI3 + +#define USE_BLACKBOX +#define USE_FLASHFS +#define USE_FLASH_W25N01G +#define W25N01G_SPI_BUS BUS_SPI3 +#define W25N01G_CS_PIN PA15 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// *************** I2C1 - Magnetometer ************ +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB6 +#define I2C1_SDA PB7 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_ALL + +// *************** I2C2 - Barometer (DPS310) ****** +#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_ALL + +#define TEMPERATURE_I2C_BUS BUS_I2C2 +#define PITOT_I2C_BUS BUS_I2C1 +#define RANGEFINDER_I2C_BUS BUS_I2C1 +#define USE_RANGEFINDER + +// *************** UARTs ************************** +// UART5 not wired; UART6 pins PC6/PC7 used as motor outputs +#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 + +// *************** CAN Bus ************************* +// CAN not yet tested +#define USE_DRONECAN +#define CAN1_RX PD0 +#define CAN1_TX PD1 +#define CAN1_STANDBY PD3 + +#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 6 // VCP + UART1-3 + UART7 + UART8 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART3 + +// *************** ADC **************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 + +#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 + +// *************** LED Strip ********************** +#define USE_LED_STRIP +#define WS2811_PIN PA5 + +// *************** PINIO ************************** +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PA2 +#define PINIO2_PIN PA3 +#define PINIO3_PIN PB1 +#define PINIO3_FLAGS PINIO_FLAGS_INVERTED +#define PINIO4_PIN PD15 + +// *************** Features *********************** +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) +#define CURRENT_METER_SCALE 250 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define USE_ESC_SENSOR + +// *************** IO Port Masks ****************** +#define TARGET_IO_PORTA (0xffff & ~(BIT(13) | BIT(14))) +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff + +// *************** PWM Outputs ******************** +#define MAX_PWM_OUTPUT_PORTS 8 +#define USE_DSHOT diff --git a/src/main/target/BLADE_F4/CMakeLists.txt b/src/main/target/BLADE_F4/CMakeLists.txt new file mode 100644 index 00000000000..4a1557112b5 --- /dev/null +++ b/src/main/target/BLADE_F4/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(BLADE_F4) diff --git a/src/main/target/BLADE_F4/config.c b/src/main/target/BLADE_F4/config.c new file mode 100644 index 00000000000..b963bfbfa59 --- /dev/null +++ b/src/main/target/BLADE_F4/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 + +#include "fc/fc_msp_box.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/BLADE_F4/target.c b/src/main/target/BLADE_F4/target.c new file mode 100644 index 00000000000..a44aabc0142 --- /dev/null +++ b/src/main/target/BLADE_F4/target.c @@ -0,0 +1,37 @@ +/* + * 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/io.h" +#include "drivers/timer.h" + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), // S1_OUT D2_ST4 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S2_OUT D2_ST7 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 1), // S3_OUT D2_ST1 + DEF_TIM(TIM1, CH2, PA9, TIM_USE_OUTPUT_AUTO, 0, 1), // S4_OUT D2_ST2 + DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0, 0), // S5_OUT D2_ST6 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S6_OUT D1_ST4 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S7_OUT D1_ST7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S8_OUT D1_ST2 + + DEF_TIM(TIM2, CH2, PB3, TIM_USE_LED, 0, 0), // D1_ST6 +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/BLADE_F4/target.h b/src/main/target/BLADE_F4/target.h new file mode 100644 index 00000000000..71cf76cc0cf --- /dev/null +++ b/src/main/target/BLADE_F4/target.h @@ -0,0 +1,179 @@ +/* + * 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 "BLF4" +#define USBD_PRODUCT_STRING "BLADE_F4" + +#define USE_TARGET_CONFIG + +/*** Indicators ***/ +#define LED0 PC15 +#define LED1 PC14 +#define BEEPER PC13 +#define BEEPER_INVERTED + +/*** SPI/I2C bus ***/ +#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 SPI2_FLASH_PIN PA15 + + +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PB5 + +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + + +/*** IMU sensors ***/ + +// MPU6000 +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW270_DEG +#define MPU6000_SPI_BUS BUS_SPI1 +#define MPU6000_CS_PIN SPI1_NSS_PIN + + +// ICM42605/ICM42688P +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW270_DEG +#define ICM42605_SPI_BUS BUS_SPI1 +#define ICM42605_CS_PIN SPI1_NSS_PIN + +//BMI270 +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW270_DEG +#define BMI270_SPI_BUS BUS_SPI1 +#define BMI270_CS_PIN SPI1_NSS_PIN + + +/*** OSD ***/ +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN SPI2_NSS_PIN + +/*** Onboard flash ***/ +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_CS_PIN SPI2_FLASH_PIN +#define M25P16_SPI_BUS BUS_SPI2 + +// *** PINIO *** + +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PC2 +#define PINIO2_PIN PC5 + +/*** Serial ports ***/ +#define USE_VCP +#define USE_UART_INVERTER + +#define USE_UART1 +#define UART1_RX_PIN PB7 +#define UART1_TX_PIN PB6 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 +#define INVERTER_PIN_UART2_RX PC0 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART5 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN PC12 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define SERIAL_PORT_COUNT 7 + +/*** BARO & MAG ***/ +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_SPL06 +#define USE_BARO_DPS310 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_ALL + +/*** ADC ***/ +#define USE_ADC +#define ADC_CHANNEL_1_PIN PC1 +#define ADC_CHANNEL_2_PIN PC3 + + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 + + +/*** LED STRIP ***/ +#define USE_LED_STRIP +#define WS2811_PIN PB3 + +/*** Default settings ***/ +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define CURRENT_METER_SCALE 250 + + +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY ) + +/*** Timer/PWM output ***/ +#define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define MAX_PWM_OUTPUT_PORTS 8 +#define USE_DSHOT +#define USE_ESC_SENSOR + +// ********** Optical Flow and Lidar ************** +#define USE_RANGEFINDER +#define USE_RANGEFINDER_MSP +#define USE_OPFLOW +#define USE_OPFLOW_MSP + +/*** Used pins ***/ +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) diff --git a/src/main/target/BLADE_PRO_H7/CMakeLists.txt b/src/main/target/BLADE_PRO_H7/CMakeLists.txt new file mode 100644 index 00000000000..37321c43054 --- /dev/null +++ b/src/main/target/BLADE_PRO_H7/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32h743xi(BLADE_PRO_H7) diff --git a/src/main/target/BLADE_PRO_H7/config.c b/src/main/target/BLADE_PRO_H7/config.c new file mode 100644 index 00000000000..1065971614a --- /dev/null +++ b/src/main/target/BLADE_PRO_H7/config.c @@ -0,0 +1,32 @@ +/* + * 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/piniobox.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; + beeperConfigMutable()->pwmMode = true; +} diff --git a/src/main/target/BLADE_PRO_H7/target.c b/src/main/target/BLADE_PRO_H7/target.c new file mode 100644 index 00000000000..28745e92017 --- /dev/null +++ b/src/main/target/BLADE_PRO_H7/target.c @@ -0,0 +1,60 @@ +/* + * 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_1, DEVHW_MPU6000, MPU6000_SPI_BUS_1, MPU6000_CS_PIN_1, MPU6000_EXTI_PIN_1, DEVFLAGS_NONE, 0, IMU_MPU6000_ALIGN_1); +BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000_2, DEVHW_MPU6000, MPU6000_SPI_BUS_2, MPU6000_CS_PIN_2, MPU6000_EXTI_PIN_2, DEVFLAGS_NONE, 1, IMU_MPU6000_ALIGN_2); + +BUSDEV_REGISTER_SPI_TAG(busdev_icm42688_1, DEVHW_ICM42605, ICM42605_SPI_BUS_1, ICM42605_CS_PIN_1, ICM42605_EXTI_PIN_1, DEVFLAGS_NONE, 0, IMU_ICM42605_ALIGN_1); +BUSDEV_REGISTER_SPI_TAG(busdev_icm42688_2, DEVHW_ICM42605, ICM42605_SPI_BUS_2, ICM42605_CS_PIN_2, ICM42605_EXTI_PIN_2, DEVFLAGS_NONE, 1, IMU_ICM42605_ALIGN_2); + +BUSDEV_REGISTER_SPI_TAG(busdev_bmi270_1, DEVHW_BMI270, BMI270_SPI_BUS_1, BMI270_CS_PIN_1, BMI270_EXTI_PIN_1, DEVFLAGS_NONE, 0, IMU_BMI270_ALIGN_1); +BUSDEV_REGISTER_SPI_TAG(busdev_bmi270_2, DEVHW_BMI270, BMI270_SPI_BUS_2, BMI270_CS_PIN_2, BMI270_EXTI_PIN_2, DEVFLAGS_NONE, 1, IMU_BMI270_ALIGN_2); + +timerHardware_t timerHardware[] = { + + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 1), // S1 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 2), // S4 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 3), // S3 + + 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(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(TIM4, CH4, PD15, TIM_USE_OUTPUT_AUTO, 0, 0), // S10 DMA_NONE + DEF_TIM(TIM4, CH3, PD14, TIM_USE_OUTPUT_AUTO, 0, 0), // S9 + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 9), // LED_2812 + DEF_TIM(TIM2, CH1, PA15, TIM_USE_BEEPER, 0, 0), // BEEPER PWM + +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/BLADE_PRO_H7/target.h b/src/main/target/BLADE_PRO_H7/target.h new file mode 100644 index 00000000000..a341a504863 --- /dev/null +++ b/src/main/target/BLADE_PRO_H7/target.h @@ -0,0 +1,233 @@ +/* + * 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 "BLPH" +#define USBD_PRODUCT_STRING "BLADE_PRO_H7" + +#define USE_TARGET_CONFIG + +#define LED0 PE3 +#define LED1 PE4 + +#define BEEPER PA15 +#define BEEPER_INVERTED +#define BEEPER_PWM_FREQUENCY 2500 + +// *************** IMU generic *********************** +#define USE_DUAL_GYRO +#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS +// *************** SPI1 IMU1 MPU6000 && BMI270 && ICM42688P **************** +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PD7 + +#define SPI1_NSS_PIN PC15 +#define SPI1_EXTI_PIN PB2 + +// MPU6000 +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN_1 CW180_DEG +#define MPU6000_SPI_BUS_1 BUS_SPI4 +#define MPU6000_CS_PIN_1 SPI1_NSS_PIN +#define MPU6000_EXTI_PIN_1 SPI1_EXTI_PIN + +// ICM42605/ICM42688P +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN_1 CW180_DEG +#define ICM42605_SPI_BUS_1 BUS_SPI4 +#define ICM42605_CS_PIN_1 SPI1_NSS_PIN +#define ICM42605_EXTI_PIN_1 SPI1_EXTI_PIN + +//BMI270 +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN_1 CW180_DEG +#define BMI270_SPI_BUS_1 BUS_SPI4 +#define BMI270_CS_PIN_1 SPI1_NSS_PIN +#define BMI270_EXTI_PIN_1 SPI1_EXTI_PIN + +// *************** SPI4 IMU2 BMI270 ************** +#define USE_SPI_DEVICE_4 +#define SPI4_SCK_PIN PE12 +#define SPI4_MISO_PIN PE13 +#define SPI4_MOSI_PIN PE14 + +#define SPI4_NSS_PIN PE11 +#define SPI4_EXTI_PIN PE15 + +// MPU6000 +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN_2 CW270_DEG +#define MPU6000_SPI_BUS_2 BUS_SPI1 +#define MPU6000_CS_PIN_2 SPI4_NSS_PIN +#define MPU6000_EXTI_PIN_2 SPI4_EXTI_PIN + +// ICM42605/ICM42688P +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN_2 CW270_DEG +#define ICM42605_SPI_BUS_2 BUS_SPI1 +#define ICM42605_CS_PIN_2 SPI4_NSS_PIN +#define ICM42605_EXTI_PIN_2 SPI4_EXTI_PIN + +//BMI270 +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN_2 CW270_DEG +#define BMI270_SPI_BUS_2 BUS_SPI1 +#define BMI270_CS_PIN_2 SPI4_NSS_PIN +#define BMI270_EXTI_PIN_2 SPI4_EXTI_PIN + +// *************** SPI2 OSD *********************** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PB12 + +// *************** SPI3 FLASH *********************** +#define USE_SPI_DEVICE_3 + +#define SPI3_NSS_PIN PE2 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +#define SPI3_SCK_AF GPIO_AF6_SPI3 +#define SPI3_MISO_AF GPIO_AF6_SPI3 +#define SPI3_MOSI_AF GPIO_AF7_SPI3 + +#define W25N01G_SPI_BUS BUS_SPI3 +#define W25N01G_CS_PIN SPI3_NSS_PIN + +#define USE_BLACKBOX +#define USE_FLASHFS +#define USE_FLASH_W25N01G +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_CS_PIN SPI3_NSS_PIN +#define M25P16_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_I2C2 + + +// *************** 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_SBUS + +// *************** 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 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 USE_SERIAL_4WAY_BLHELI_INTERFACE + +#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 MAX_PWM_OUTPUT_PORTS 15 +#define USE_DSHOT +#define USE_ESC_SENSOR diff --git a/src/main/target/FLYINGRCF4WINGMINI/CMakeLists.txt b/src/main/target/FLYINGRCF4WINGMINI/CMakeLists.txt new file mode 100644 index 00000000000..ef771cdd4e4 --- /dev/null +++ b/src/main/target/FLYINGRCF4WINGMINI/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(FLYINGRCF4WINGMINI) diff --git a/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/config.c b/src/main/target/FLYINGRCF4WINGMINI/config.c similarity index 100% rename from src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/config.c rename to src/main/target/FLYINGRCF4WINGMINI/config.c diff --git a/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.c b/src/main/target/FLYINGRCF4WINGMINI/target.c similarity index 100% rename from src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.c rename to src/main/target/FLYINGRCF4WINGMINI/target.c diff --git a/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.h b/src/main/target/FLYINGRCF4WINGMINI/target.h similarity index 88% rename from src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.h rename to src/main/target/FLYINGRCF4WINGMINI/target.h index 8eecb2c466c..21b90a853ee 100644 --- a/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.h +++ b/src/main/target/FLYINGRCF4WINGMINI/target.h @@ -19,7 +19,7 @@ #define USE_TARGET_CONFIG #define TARGET_BOARD_IDENTIFIER "FRF4WM" -#define USBD_PRODUCT_STRING "FLYINGRCF4WINGMINI_NOT_RECOMMENDED" +#define USBD_PRODUCT_STRING "FLYINGRCF4WINGMINI" #define LED0 PA14 //Blue #define LED1 PA13 //Green @@ -33,10 +33,15 @@ #define SPI1_MOSI_PIN PA7 #define USE_IMU_ICM42605 -#define IMU_ICM42605_ALIGN CW90_DEG_FLIP +#define IMU_ICM42605_ALIGN CW270_DEG_FLIP #define ICM42605_SPI_BUS BUS_SPI1 #define ICM42605_CS_PIN PC14 +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW180_DEG_FLIP +#define BMI270_SPI_BUS BUS_SPI1 +#define BMI270_CS_PIN PC14 + // *************** I2C /Baro/Mag ********************* #define USE_I2C #define USE_I2C_DEVICE_1 @@ -93,7 +98,10 @@ #define ADC_INSTANCE ADC1 #define ADC1_DMA_STREAM DMA2_Stream4 #define ADC_CHANNEL_1_PIN PC4 +#define ADC_CHANNEL_2_PIN PC5 #define VBAT_ADC_CHANNEL ADC_CHN_1 +// V4 hardware adds a backside current-sensor pad on the same ADC pin used by Matek F405 TE. +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 // *************** LEDSTRIP ************************ #define USE_LED_STRIP diff --git a/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/CMakeLists.txt b/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/CMakeLists.txt deleted file mode 100644 index 9c9a729ef0a..00000000000 --- a/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/CMakeLists.txt +++ /dev/null @@ -1 +0,0 @@ -target_stm32f405xg(FLYINGRCF4WINGMINI_NOT_RECOMMENDED NO_BOOTLOADER) diff --git a/src/main/target/GEPRCF745_BT_HD/target.c b/src/main/target/GEPRCF745_BT_HD/target.c index 6fe88bde011..91f9957fefc 100644 --- a/src/main/target/GEPRCF745_BT_HD/target.c +++ b/src/main/target/GEPRCF745_BT_HD/target.c @@ -29,7 +29,7 @@ BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, IMU1_SPI_BUS, IMU1_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU1_ALIGN); BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000_2, DEVHW_MPU6000, IMU2_SPI_BUS, IMU2_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU2_ALIGN); -BUSDEV_REGISTER_SPI_TAG(busdev_icm42688, DEVHW_MPU6000, IMU1_SPI_BUS, IMU1_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU1_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_icm42688, DEVHW_ICM42605, IMU1_SPI_BUS, IMU1_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU1_ALIGN); BUSDEV_REGISTER_SPI_TAG(busdev_icm42688_2, DEVHW_ICM42605, IMU2_SPI_BUS, IMU2_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU2_ALIGN); diff --git a/src/main/target/GEPRC_TAKER_H743/target.c b/src/main/target/GEPRC_TAKER_H743/target.c index a04e65b2d58..c0ea036db39 100644 --- a/src/main/target/GEPRC_TAKER_H743/target.c +++ b/src/main/target/GEPRC_TAKER_H743/target.c @@ -29,7 +29,7 @@ BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, IMU1_SPI_BUS, IMU1_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU1_ALIGN); BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000_2, DEVHW_MPU6000, IMU2_SPI_BUS, IMU2_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU2_ALIGN); -BUSDEV_REGISTER_SPI_TAG(busdev_icm42688, DEVHW_MPU6000, IMU1_SPI_BUS, IMU1_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU1_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_icm42688, DEVHW_ICM42605, IMU1_SPI_BUS, IMU1_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU1_ALIGN); BUSDEV_REGISTER_SPI_TAG(busdev_icm42688_2, DEVHW_ICM42605, IMU2_SPI_BUS, IMU2_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU2_ALIGN); diff --git a/src/main/target/JHEMCUF435/target.h b/src/main/target/JHEMCUF435/target.h index db165c558b8..edbc3d0bbda 100644 --- a/src/main/target/JHEMCUF435/target.h +++ b/src/main/target/JHEMCUF435/target.h @@ -114,15 +114,11 @@ #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 -// UART2 TX does not work due currently unknown software issue -// Leaving this info here for future work. -/* #define USE_UART2 #define UART2_RX_AF 6 #define UART2_TX_AF 8 #define UART2_RX_PIN PB0 #define UART2_TX_PIN PA8 -*/ #define USE_UART3 #define UART3_RX_PIN PB11 @@ -149,7 +145,7 @@ #define UART8_RX_PIN PC3 #define UART8_TX_PIN PC2 -#define SERIAL_PORT_COUNT 8 +#define SERIAL_PORT_COUNT 9 #define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define SERIALRX_PROVIDER SERIALRX_CRSF diff --git a/src/main/target/system_stm32h7xx.c b/src/main/target/system_stm32h7xx.c index 6ec0d1c4002..56a53cefcfb 100644 --- a/src/main/target/system_stm32h7xx.c +++ b/src/main/target/system_stm32h7xx.c @@ -302,7 +302,7 @@ static void SystemClockHSE_Config(void) RCC_OscInitStruct.PLL.PLLR = pll1Config->r; RCC_OscInitStruct.PLL.PLLVCOSEL = RCC_PLL1VCOWIDE; - RCC_OscInitStruct.PLL.PLLRGE = RCC_PLL1VCIRANGE_2; + RCC_OscInitStruct.PLL.PLLRGE = RCC_PLL1VCIRANGE_1; // VCI = HSE/M = 2 MHz, range is 2-4 MHz HAL_StatusTypeDef status = HAL_RCC_OscConfig(&RCC_OscInitStruct); @@ -498,17 +498,22 @@ void SystemClock_Config(void) HAL_RCCEx_PeriphCLKConfig(&RCC_PeriphClkInit); #ifdef USE_SDCARD_SDIO + // PLL2M = HSE_VALUE / 1600000 pins the VCO input to exactly 1.6 MHz for any HSE. + // With N=500 this gives VCO=800 MHz: PLL2R/4=200 MHz (SDMMC), PLL2P/2=400 MHz. + STATIC_ASSERT(HSE_VALUE % 1600000 == 0, HSE_not_a_multiple_of_1600000); RCC_PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_SDMMC; - RCC_PeriphClkInit.PLL2.PLL2M = 5; + RCC_PeriphClkInit.PLL2.PLL2M = HSE_VALUE / 1600000; RCC_PeriphClkInit.PLL2.PLL2N = 500; - RCC_PeriphClkInit.PLL2.PLL2P = 2; // 500Mhz + RCC_PeriphClkInit.PLL2.PLL2P = 2; // 400Mhz RCC_PeriphClkInit.PLL2.PLL2Q = 3; // 266Mhz - 133Mhz can be derived from this for for QSPI if flash chip supports the speed. RCC_PeriphClkInit.PLL2.PLL2R = 4; // 200Mhz HAL LIBS REQUIRE 200MHZ SDMMC CLOCK, see HAL_SD_ConfigWideBusOperation, SDMMC_HSpeed_CLK_DIV, SDMMC_NSpeed_CLK_DIV RCC_PeriphClkInit.PLL2.PLL2RGE = RCC_PLL2VCIRANGE_0; RCC_PeriphClkInit.PLL2.PLL2VCOSEL = RCC_PLL2VCOWIDE; RCC_PeriphClkInit.PLL2.PLL2FRACN = 0; RCC_PeriphClkInit.SdmmcClockSelection = RCC_SDMMCCLKSOURCE_PLL2; - HAL_RCCEx_PeriphCLKConfig(&RCC_PeriphClkInit); + if (HAL_RCCEx_PeriphCLKConfig(&RCC_PeriphClkInit) != HAL_OK) { + Error_Handler(); + } #endif #ifdef USE_QUADSPI diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index d5084b32a62..6c7e130d06a 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -171,10 +171,19 @@ static uint8_t mavRates[] = { [MAV_DATA_STREAM_RC_CHANNELS] = 1, // 1Hz [MAV_DATA_STREAM_POSITION] = 2, // 2Hz [MAV_DATA_STREAM_EXTRA1] = 3, // 3Hz - [MAV_DATA_STREAM_EXTRA2] = 2, // 2Hz, HEARTBEATs are important - [MAV_DATA_STREAM_EXTRA3] = 1 // 1Hz + [MAV_DATA_STREAM_EXTRA2] = 2, // 2Hz + [MAV_DATA_STREAM_EXTRA3] = 1, // 1Hz }; +/* HEARTBEAT and SYSTEM_TIME are not part of any MAV_DATA_STREAM; scheduled independently. */ +typedef struct mavlinkScheduledMessage_s { + uint8_t rateHz; // desired transmission rate in Hz + uint8_t ticks; // countdown decremented at TELEMETRY_MAVLINK_MAXRATE +} mavlinkScheduledMessage_t; + +static mavlinkScheduledMessage_t mavHeartbeat = { .rateHz = 1, .ticks = 0 }; +static mavlinkScheduledMessage_t mavSystemTime = { .rateHz = 1, .ticks = TELEMETRY_MAVLINK_MAXRATE / 2 }; // Stagger SYSTEM_TIME by half a period so it doesn't ride in the same TX burst as HEARTBEAT. + #define MAXSTREAMS (sizeof(mavRates) / sizeof(mavRates[0])) static timeUs_t lastMavlinkMessage = 0; @@ -286,6 +295,22 @@ static int mavlinkStreamTrigger(enum MAV_DATA_STREAM streamNum) return 0; } +static bool mavlinkScheduledTrigger(mavlinkScheduledMessage_t *msg) +{ + if (msg->rateHz == 0) { + return false; + } + + if (msg->ticks == 0) { + uint8_t rate = msg->rateHz > TELEMETRY_MAVLINK_MAXRATE ? TELEMETRY_MAVLINK_MAXRATE : msg->rateHz; + msg->ticks = TELEMETRY_MAVLINK_MAXRATE / rate; + return true; + } + + msg->ticks--; + return false; +} + void freeMAVLinkTelemetryPort(void) { closeSerialPort(mavlinkPort); @@ -593,6 +618,8 @@ void mavlinkSendRCChannelsAndRSSI(void) void mavlinkSendPosition(timeUs_t currentTimeUs) { uint8_t gpsFixType = 0; + rtcTime_t rtcTime; + uint64_t timeUnixUsec = currentTimeUs; if (!(sensors(SENSOR_GPS) #ifdef USE_GPS_FIX_ESTIMATION @@ -601,16 +628,21 @@ void mavlinkSendPosition(timeUs_t currentTimeUs) )) return; - if (gpsSol.fixType == GPS_NO_FIX) + if (gpsSol.fixType == GPS_NO_FIX) { gpsFixType = 1; - else if (gpsSol.fixType == GPS_FIX_2D) - gpsFixType = 2; - else if (gpsSol.fixType == GPS_FIX_3D) - gpsFixType = 3; + } else if (gpsSol.fixType == GPS_FIX_2D) { + gpsFixType = 2; + } else if (gpsSol.fixType == GPS_FIX_3D) { + gpsFixType = 3; + } + + if (rtcGet(&rtcTime)) { + timeUnixUsec = (uint64_t)rtcTime * 1000ULL; + } mavlink_msg_gps_raw_int_pack(mavSystemId, mavComponentId, &mavSendMsg, // time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - currentTimeUs, + timeUnixUsec, // fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. gpsFixType, // lat Latitude in 1E7 degrees @@ -704,52 +736,22 @@ void mavlinkSendAttitude(void) mavlinkSendMessage(); } -void mavlinkSendHUDAndHeartbeat(void) +void mavlinkSendSystemTime(void) { - float mavAltitude = 0; - float mavGroundSpeed = 0; - float mavAirSpeed = 0; - float mavClimbRate = 0; - -#if defined(USE_GPS) - // use ground speed if source available - if (sensors(SENSOR_GPS) -#ifdef USE_GPS_FIX_ESTIMATION - || STATE(GPS_ESTIMATED_FIX) -#endif - ) { - mavGroundSpeed = gpsSol.groundSpeed / 100.0f; - } -#endif + uint64_t timeUnixUsec = 0; + rtcTime_t rtcTime; -#if defined(USE_PITOT) - if (sensors(SENSOR_PITOT) && pitotIsHealthy()) { - mavAirSpeed = getAirspeedEstimate() / 100.0f; + if (rtcGet(&rtcTime)) { + //timeUnixUsec = (uint64_t)rtcTime * 1000ULL + (uint64_t)(micros() % 1000); // extrapolation to uS + timeUnixUsec = (uint64_t)rtcTime * 1000ULL; // mS resolution } -#endif - - // select best source for altitude - mavAltitude = getEstimatedActualPosition(Z) / 100.0f; - mavClimbRate = getEstimatedActualVelocity(Z) / 100.0f; - - int16_t thr = getThrottlePercent(osdUsingScaledThrottle()); - mavlink_msg_vfr_hud_pack(mavSystemId, mavComponentId, &mavSendMsg, - // airspeed Current airspeed in m/s - mavAirSpeed, - // groundspeed Current ground speed in m/s - mavGroundSpeed, - // heading Current heading in degrees, in compass units (0..360, 0=north) - DECIDEGREES_TO_DEGREES(attitude.values.yaw), - // throttle Current throttle setting in integer percent, 0 to 100 - thr, - // alt Current altitude (MSL), in meters, if we have surface or baro use them, otherwise use GPS (less accurate) - mavAltitude, - // climb Current climb rate in meters/second - mavClimbRate); + mavlink_msg_system_time_pack(mavSystemId, mavComponentId, &mavSendMsg, timeUnixUsec, millis()); mavlinkSendMessage(); +} - +void mavlinkSendHeartbeat(void) +{ uint8_t mavModes = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; if (ARMING_FLAG(ARMED)) mavModes |= MAV_MODE_FLAG_SAFETY_ARMED; @@ -821,16 +823,62 @@ void mavlinkSendHUDAndHeartbeat(void) } mavlink_msg_heartbeat_pack(mavSystemId, mavComponentId, &mavSendMsg, - // type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - mavSystemType, - // autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM - mavType, - // base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h - mavModes, - // custom_mode A bitfield for use for autopilot-specific flags. - mavCustomMode, - // system_status System status flag, see MAV_STATE ENUM - mavSystemState); + // type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + mavSystemType, + // autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM + mavType, + // base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h + mavModes, + // custom_mode A bitfield for use for autopilot-specific flags. + mavCustomMode, + // system_status System status flag, see MAV_STATE ENUM + mavSystemState); + + mavlinkSendMessage(); +} + +void mavlinkSendHUD(void) +{ + float mavAltitude = 0; + float mavGroundSpeed = 0; + float mavAirSpeed = 0; + float mavClimbRate = 0; + +#if defined(USE_GPS) + // use ground speed if source available + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) { + mavGroundSpeed = gpsSol.groundSpeed / 100.0f; + } +#endif + +#if defined(USE_PITOT) + if (sensors(SENSOR_PITOT) && pitotIsHealthy()) { + mavAirSpeed = getAirspeedEstimate() / 100.0f; + } +#endif + + // select best source for altitude + mavAltitude = getEstimatedActualPosition(Z) / 100.0f; + mavClimbRate = getEstimatedActualVelocity(Z) / 100.0f; + + int16_t thr = getThrottlePercent(osdUsingScaledThrottle()); + mavlink_msg_vfr_hud_pack(mavSystemId, mavComponentId, &mavSendMsg, + // airspeed Current airspeed in m/s + mavAirSpeed, + // groundspeed Current ground speed in m/s + mavGroundSpeed, + // heading Current heading in degrees, in compass units (0..360, 0=north) + DECIDEGREES_TO_DEGREES(attitude.values.yaw), + // throttle Current throttle setting in integer percent, 0 to 100 + thr, + // alt Current altitude (MSL), in meters, if we have surface or baro use them, otherwise use GPS (less accurate) + mavAltitude, + // climb Current climb rate in meters/second + mavClimbRate); mavlinkSendMessage(); } @@ -952,13 +1000,20 @@ void processMAVLinkTelemetry(timeUs_t currentTimeUs) } if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA2)) { - mavlinkSendHUDAndHeartbeat(); + mavlinkSendHUD(); } if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA3)) { mavlinkSendBatteryTemperatureStatusText(); } + if (mavlinkScheduledTrigger(&mavHeartbeat)) { + mavlinkSendHeartbeat(); + } + + if (mavlinkScheduledTrigger(&mavSystemTime)) { + mavlinkSendSystemTime(); + } } static bool handleIncoming_MISSION_CLEAR_ALL(void)