From bc80dde6c485b1bb26ee99bcb64e857fc3920c38 Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Tue, 5 Mar 2024 03:43:56 +0000 Subject: [PATCH 01/24] Add 'mixer reset' to mixer CLI help --- src/main/cli/cli.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index ac95cf6129..c8f60ca06c 100644 --- a/src/main/cli/cli.c +++ b/src/main/cli/cli.c @@ -6450,6 +6450,7 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("mcu_id", "id of the microcontroller", NULL, cliMcuId), CLI_COMMAND_DEF("mixer", "configure mixer", "status\r\n\t" + "reset\r\n\t" "input\r\n\t" "input reset\r\n\t" "input \r\n\t" From 0f6c1eebdd8248d30161d49fe6050398e9a18d18 Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Sat, 2 Mar 2024 12:28:11 +0000 Subject: [PATCH 02/24] Change default motor protocol to PWM --- src/main/pg/motor.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/pg/motor.c b/src/main/pg/motor.c index 05b0d9c79c..de850a01bd 100644 --- a/src/main/pg/motor.c +++ b/src/main/pg/motor.c @@ -36,7 +36,7 @@ PG_REGISTER_WITH_RESET_FN(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 1); void pgResetFn_motorConfig(motorConfig_t *motorConfig) { - motorConfig->dev.motorPwmProtocol = PWM_TYPE_DISABLED; + motorConfig->dev.motorPwmProtocol = PWM_TYPE_STANDARD; motorConfig->dev.motorPwmRate = 250; motorConfig->dev.useUnsyncedPwm = false; motorConfig->minthrottle = 1070; From 66c58a3f8ce73736752c9e31f117619154474cd3 Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Wed, 6 Mar 2024 13:17:27 +0000 Subject: [PATCH 03/24] Change default PID D-gains --- src/main/pg/pid.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/pg/pid.c b/src/main/pg/pid.c index 9598b2d652..9d75589f34 100644 --- a/src/main/pg/pid.c +++ b/src/main/pg/pid.c @@ -45,9 +45,9 @@ void resetPidProfile(pidProfile_t *pidProfile) RESET_CONFIG(pidProfile_t, pidProfile, .profileName = "", .pid = { - [PID_ROLL] = { .P = 50, .I = 100, .D = 0, .F = 100, .B = 0, .O = 25, }, - [PID_PITCH] = { .P = 50, .I = 100, .D = 0, .F = 100, .B = 0, .O = 25, }, - [PID_YAW] = { .P = 50, .I = 50, .D = 0, .F = 0, .B = 0, .O = 0, }, + [PID_ROLL] = { .P = 50, .I = 100, .D = 10, .F = 100, .B = 0, .O = 25, }, + [PID_PITCH] = { .P = 50, .I = 100, .D = 20, .F = 100, .B = 0, .O = 25, }, + [PID_YAW] = { .P = 50, .I = 50, .D = 10, .F = 0, .B = 0, .O = 0, }, }, .pid_mode = 3, .dterm_mode = 0, From 9ea32ee41e0f164040f82114f560d686f97bb5cd Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Sun, 10 Mar 2024 20:51:45 +0000 Subject: [PATCH 04/24] Simplify RC frame rate calculation --- src/main/fc/rc.c | 30 ++++++++++-------------------- 1 file changed, 10 insertions(+), 20 deletions(-) diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index 1a3367b399..98e1c9b5ac 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -51,10 +51,10 @@ #include "rc.h" -#define RX_REFRESH_RATE_MIN_US 950 -#define RX_REFRESH_RATE_MAX_US 65000 +#define RX_REFRESH_RATE_MIN_US 490 +#define RX_REFRESH_RATE_MAX_US 40000 -#define RX_REFRESH_RATE_AVERAGING 100 +#define RX_REFRESH_RATE_AVERAGING 64 #define RX_RANGE_COUNT 4 @@ -66,7 +66,6 @@ static FAST_DATA_ZERO_INIT float rcDeadband[4]; static FAST_DATA_ZERO_INIT uint16_t currentRxRefreshRate; static FAST_DATA_ZERO_INIT float averageRxRefreshRate; -static FAST_DATA_ZERO_INIT uint16_t averagingLength; static FAST_DATA_ZERO_INIT timeUs_t lastRxTimeUs; static FAST_DATA_ZERO_INIT uint32_t changeCount; @@ -152,6 +151,8 @@ void updateRcRefreshRate(timeUs_t currentTimeUs) timeDelta_t frameDeltaUs = rxGetFrameDelta(&frameAgeUs); timeDelta_t localDeltaUs = cmpTimeUs(currentTimeUs, lastRxTimeUs); + lastRxTimeUs = currentTimeUs; + DEBUG(RX_TIMING, 4, frameDeltaUs); DEBUG(RX_TIMING, 5, localDeltaUs); DEBUG(RX_TIMING, 6, frameAgeUs); @@ -161,24 +162,11 @@ void updateRcRefreshRate(timeUs_t currentTimeUs) } currentRxRefreshRate = frameDeltaUs; - lastRxTimeUs = currentTimeUs; - float currentRateUs = frameDeltaUs; - - if (averagingLength >= RX_REFRESH_RATE_AVERAGING) { - if (rxIsReceivingSignal() && currentRxRefreshRate > RX_REFRESH_RATE_MIN_US && currentRxRefreshRate < RX_REFRESH_RATE_MAX_US) { - currentRateUs = constrainf(currentRateUs, 0.75f * averageRxRefreshRate, 1.25f * averageRxRefreshRate); - } else { - currentRateUs = averageRxRefreshRate; - } + if (rxIsReceivingSignal() && currentRxRefreshRate > RX_REFRESH_RATE_MIN_US && currentRxRefreshRate < RX_REFRESH_RATE_MAX_US) { + averageRxRefreshRate += (frameDeltaUs - averageRxRefreshRate) / RX_REFRESH_RATE_AVERAGING; + updateRcChange(); } - else { - averagingLength++; - } - - averageRxRefreshRate += (currentRateUs - averageRxRefreshRate) / averagingLength; - - updateRcChange(); DEBUG(RX_TIMING, 0, averageRxRefreshRate); DEBUG(RX_TIMING, 1, averageRxRefreshRate * currentMult); @@ -233,6 +221,8 @@ INIT_CODE void initRcProcessing(void) rcDeadband[2] = rcControlsConfig()->rc_yaw_deadband; rcDeadband[3] = 0; + averageRxRefreshRate = 10000; + currentMult = 1; } From 301b89b657bcdb56969b649d2b97f361d6a1ba08 Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Thu, 7 Mar 2024 17:31:31 +0000 Subject: [PATCH 05/24] Reduce default smoothing level --- src/main/flight/setpoint.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/flight/setpoint.c b/src/main/flight/setpoint.c index 8be412660f..c17856ef0e 100644 --- a/src/main/flight/setpoint.c +++ b/src/main/flight/setpoint.c @@ -40,7 +40,7 @@ #include "setpoint.h" -#define SP_SMOOTHING_FILTER_MIN_HZ 1 +#define SP_SMOOTHING_FILTER_MIN_HZ 10 #define SP_SMOOTHING_FILTER_MAX_HZ 1000 #define SP_MAX_UP_CUTOFF 20.0f @@ -118,14 +118,14 @@ INIT_CODE void setpointInitProfile(void) for (int i = 0; i < 4; i++) { sp.accelLimit[i] = 10.0f * currentControlRateProfile->accel_limit[i] * pidGetDT(); sp.responseCutoff[i] = constrain( - 2500 / (10 * currentControlRateProfile->response_time[i] + 1), + 2500 / constrain(10 * currentControlRateProfile->response_time[i], 1, 1000), SP_SMOOTHING_FILTER_MIN_HZ, SP_SMOOTHING_FILTER_MAX_HZ); } } INIT_CODE void setpointInit(void) { - sp.smoothingFactor = 15e6f / (10 + rcControlsConfig()->rc_smoothness); + sp.smoothingFactor = 25e6f / constrain(rcControlsConfig()->rc_smoothness, 1, 250); sp.maxGainUp = pt1FilterGain(SP_MAX_UP_CUTOFF, pidGetPidFrequency()); sp.maxGainDown = pt1FilterGain(SP_MAX_DN_CUTOFF, pidGetPidFrequency()); From cfe17e8c8e0a5d6f0c089d0b498679bb5f290d1e Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Mon, 11 Mar 2024 17:59:32 +0000 Subject: [PATCH 06/24] Update github workflows --- .github/scripts/extract-release-notes.py | 21 +++++++++++++++++++++ .github/workflows/history.yml | 8 -------- .github/workflows/release.yml | 11 ++--------- .github/workflows/snapshot.yml | 11 ++--------- .github/workflows/testing.yml | 13 +++++-------- 5 files changed, 30 insertions(+), 34 deletions(-) create mode 100755 .github/scripts/extract-release-notes.py diff --git a/.github/scripts/extract-release-notes.py b/.github/scripts/extract-release-notes.py new file mode 100755 index 0000000000..f917905dbf --- /dev/null +++ b/.github/scripts/extract-release-notes.py @@ -0,0 +1,21 @@ +#!/usr/bin/python3 + +import sys + +def main(args): + release = args[1] + inputfile = args[2] + + found = False + + with open(inputfile) as input: + for line in input: + if line.startswith('# '): + found = line.strip() == '# ' + release + elif line.startswith('***'): + found = False + elif found: + sys.stdout.write(line) + + +if __name__ == '__main__': main(sys.argv) diff --git a/.github/workflows/history.yml b/.github/workflows/history.yml index 6b0a73f0d5..bf58118a31 100644 --- a/.github/workflows/history.yml +++ b/.github/workflows/history.yml @@ -20,14 +20,6 @@ jobs: - name: Delete tag run: git push origin :${GITHUB_REF} - - name: Cache ARM tools - uses: actions/cache@v4 - env: - cache-name: cache-arm-tools - with: - path: downloads/gcc-arm-none-eabi-9-2020-q2-update-x86_64-linux.tar.bz2 - key: gcc-arm-none-eabi-9-2020-q2-update-x86_64-linux - - name: Install ARM tools run: make arm_sdk_install diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml index 2f4a8ba123..2a970af36a 100644 --- a/.github/workflows/release.yml +++ b/.github/workflows/release.yml @@ -15,14 +15,6 @@ jobs: - name: Check out repository uses: actions/checkout@v4 - - name: Cache ARM tools - uses: actions/cache@v4 - env: - cache-name: cache-arm-tools - with: - path: downloads/gcc-arm-none-eabi-9-2020-q2-update-x86_64-linux.tar.bz2 - key: gcc-arm-none-eabi-9-2020-q2-update-x86_64-linux - - name: Install ARM tools run: make arm_sdk_install @@ -49,7 +41,8 @@ jobs: - name: Create Release run: | - gh release create ${{ env.GIT_TAG }} --notes-file "Release.md" --title "Rotorflight ${{ env.GH_TYPE }} ${{ env.GIT_VER }}" obj/*.hex + .github/scripts/extract-release-notes.py "${{ env.GIT_VER }}" Releases.md > Notes.md + gh release create ${{ env.GIT_TAG }} --notes-file Notes.md --title "Rotorflight ${{ env.GH_TYPE }} ${{ env.GIT_VER }}" obj/*.hex env: GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/snapshot.yml b/.github/workflows/snapshot.yml index 52f4ff2d79..48772cdbaa 100644 --- a/.github/workflows/snapshot.yml +++ b/.github/workflows/snapshot.yml @@ -15,14 +15,6 @@ jobs: - name: Check out repository uses: actions/checkout@v4 - - name: Cache ARM tools - uses: actions/cache@v4 - env: - cache-name: cache-arm-tools - with: - path: downloads/gcc-arm-none-eabi-9-2020-q2-update-x86_64-linux.tar.bz2 - key: gcc-arm-none-eabi-9-2020-q2-update-x86_64-linux - - name: Install ARM tools run: make arm_sdk_install @@ -46,7 +38,8 @@ jobs: - name: Create Snapshot run: | - gh release create ${{ env.GIT_TAG }} --prerelease --notes-file "Snapshot.md" --title "Rotorflight Snapshot ${{ env.GIT_VER }}" obj/*.hex + .github/scripts/extract-release-notes.py "${{ env.GIT_VER }}" Releases.md > Notes.md + gh release create ${{ env.GIT_TAG }} --prerelease --notes-file Notes.md --title "Rotorflight Snapshot ${{ env.GIT_VER }}" obj/*.hex env: GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/testing.yml b/.github/workflows/testing.yml index a637bf7c15..b3bd920049 100644 --- a/.github/workflows/testing.yml +++ b/.github/workflows/testing.yml @@ -15,14 +15,6 @@ jobs: - name: Check out repository uses: actions/checkout@v4 - - name: Cache ARM tools - uses: actions/cache@v4 - env: - cache-name: cache-arm-tools - with: - path: downloads/gcc-arm-none-eabi-9-2020-q2-update-x86_64-linux.tar.bz2 - key: gcc-arm-none-eabi-9-2020-q2-update-x86_64-linux - - name: Install ARM tools run: make arm_sdk_install @@ -32,6 +24,11 @@ jobs: echo "GIT_TAG=${GITHUB_REF##refs/tags/}" >> ${GITHUB_ENV} cat ${GITHUB_ENV} + - name: Machine details + run: | + free + cat /proc/cpuinfo | grep -E '^(processor)|(vendor_id)|(model name)|(cpu MHz)|(cache size)' + - name: Build HEX files for unified targets run: make unified FC_VER_SUFFIX="${{ env.GIT_VER }}" FLASH_CONFIG_ERASE=yes From cc2561feebca6fc3a729d8a9bc4bce93ede8f710 Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Mon, 11 Mar 2024 17:59:55 +0000 Subject: [PATCH 07/24] Add Releases.md --- Releases.md | 57 +++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 57 insertions(+) create mode 100644 Releases.md diff --git a/Releases.md b/Releases.md new file mode 100644 index 0000000000..4e7499f5e0 --- /dev/null +++ b/Releases.md @@ -0,0 +1,57 @@ +# 4.3.0-RC1 + +This is the first Release Candidate of the Rotorflight 2 Firmware. + +## Downloads + +The official download locations for Rotorflight 2.0.0-RC1 are: + +- [Rotorflight Configurator](https://github.com/rotorflight/rotorflight-configurator/releases/tag/release/2.0.0-RC1) +- [Rotorflight Blackbox](https://github.com/rotorflight/rotorflight-blackbox/releases/tag/release/2.0.0-RC1) +- [LUA Scripts for EdgeTx](https://github.com/rotorflight/rotorflight-lua-scripts/releases/tag/release/2.0.0-RC1) +- [LUA Scripts for Ethos](https://github.com/rotorflight/rotorflight-lua-ethos/releases/tag/release/2.0.0-RC1) + + +## Notes + +1. There is a new website [www.rotorflight.org](https://www.rotorflight.org/) for Rotorflight 2. + The old Wiki in github is deprecated, and is for Rotorflight-1 only. + Big thanks to the documentation team for setting this up! + +1. Please download and install [Rotorflight Configurator](https://github.com/rotorflight/rotorflight-configurator/releases/tag/release/2.0.0-RC1) before attempting to flash this firmware. + +1. Rotorflight 2 is **NOT** backward compatible with RF1. You **MUST NOT** load your configuration dump from RF1 into RF2. + +1. If coming from RF1, please setup your helicopter from scratch for RF2. Follow the instructions on the website! + +1. As always, please double check your configuration on the bench before flying! + + +## Support + +The main source of Rotorflight information and instructions is now the [website](https://www.rotorflight.org/). + +Rotorflight has a strong presence on the Discord platform - you can join us [here](https://discord.gg/FyfMF4RwSA/). +Discord is the primary location for support, questions and discussions. The developers are all active there, +and so are the manufacturers of RF Flight Controllers. Many pro pilots are also there. +This is a great place to ask for advice or discuss any complicated problems or even new ideas. + +There is also a [Rotorflight Facebook Group](https://www.facebook.com/groups/876445460825093) for hanging out with other Rotorflight pilots. + + +## Changes + +RF2 is based on Betaflight 4.3.x, and is rewritten from ground up, with the experience learned from Rotorflight-1. + +Lots of things have changed in the two years of development. A full changelog can be found online later. + +### Changes since 2.0.0-20240218 + +- Refactor MSP_SERVO_CONFIGURATIONS +- Use internal pull-up on FREQ input +- Use falling edge trigger on FREQ input +- Change default yaw precomp parameters +- Change default governor master gain +- Change default rates response time to 0 (no limit) +- Change default dynamic notch count to 4 + From d2331b7178537394b7978ff85fd121e6f5f59d29 Mon Sep 17 00:00:00 2001 From: egonl <34315684+egonl@users.noreply.github.com> Date: Thu, 7 Mar 2024 14:02:51 +0100 Subject: [PATCH 08/24] chore: fixed scale lights terminology (#85) * Aligned firmware with Configurator * Updated documentation --- docs/LedStrip.md | 123 +++++++++++++++++++--------------------- src/main/cli/settings.c | 4 +- src/main/io/ledstrip.c | 14 ++--- src/main/io/ledstrip.h | 6 +- src/main/msp/msp.c | 4 +- 5 files changed, 73 insertions(+), 78 deletions(-) diff --git a/docs/LedStrip.md b/docs/LedStrip.md index fba5c4180c..e780319ee8 100644 --- a/docs/LedStrip.md +++ b/docs/LedStrip.md @@ -1,33 +1,32 @@ # LED Strip -Betaflight supports the use of addressable LED strips. Addressable LED strips allow each LED in the strip to be programmed with a unique and independent color. This is far more advanced than the normal RGB strips which require that all the LEDs in the strip show the same color. - +Rotorflight supports the use of addressable LED strips. Addressable LED strips allow each LED in the strip to be programmed with a unique and independent color. This is far more advanced than the normal RGB strips which require that all the LEDs in the strip show the same color. ## LED Strip Profiles -The LED strip feature supports 4 LED strip profiles, STATUS, STATUS_DIMMED, RACE and BEACON. The selected profile can be changed from the CLI, OSD LED strip menu or from an adjustment channel, i.e. switch on your radio. Take note that the adjustment channel from your radio overrides all other LED strip profile selection options. +The LED strip feature supports 4 LED strip profiles: RACE, BEACON, STATUS and STATUS_ALT. The selected profile can be changed from the CLI, OSD LED strip menu or from an adjustment channel, i.e. switch on your radio. Take note that the adjustment channel from your radio overrides all other LED strip profile selection options. ### STATUS Profile -The STATUS profile is used to display all the information mentioned below, i.e. warning indications, Larson scanner etc. +The STATUS profile is used to display all the information mentioned below, i.e. warning indications, Larson scanner etc, positional lights, ACL. -Addressable LED strips can be used to show information from the flight controller system, the current implementation supports the following: +Addressable LED strips can be used to show information from the flight controller system and supports: -* Up to 32 LEDs. (Support for more than 32 LEDs is possible, it just requires additional development.) +* Up to 32 LEDs. +* User definable blink patterns. +* AUX operated switching between LED profiles. * Indicators showing pitch/roll stick positions. * Heading/Orientation lights. * Flight mode specific color schemes. * Low battery warning. -* AUX operated on/off switch. * GPS state. * RSSI level. * Battery level. -### STATUS_DIMMED Profile - -The STATUS_DIMMED profile equals the STATUS_PROFILE, except for LEDs with the Dimmed overlay, which are dimmed in this profile. +### STATUS_ALT Profile +The STATUS_ALT profile works just like the STATUS_PROFILE, except for LEDs with the Fade overlay. LEDs with the Fade overlay will use their alternate color when the STATUS_ALT profile is active. ### RACE Profile @@ -36,33 +35,33 @@ The RACE profile is used to set ALL strip LEDs to the selected color for racing, ### BEACON Profile -The BEACON profile is used to find a lost quad, it flashes all LEDs white once per second. Again in this profile no other information is displayed on the LEDs. +The BEACON profile is used to find a lost heli, it flashes all LEDs white once per second. Again in this profile no other information is displayed on the LEDs. + +### LED Profile Selection -### LED Profile Configuration +###### OPTION 1: Use the Configurator +1. Go to the *LED Strip* tab +2. Select the profile under *LED Strip Global Settings* -###### OPTION 1: Configure an adjustment range to change the LED strip profile from your radio -1. Turn on Expert mode - see top right of configurator screen "Enable Expert Mode". -2. The LED strip profile selection is performed using an adjustment configured via the Adjustments tab. - - Enable an adjustment. ("If enabled") - - Select the AUX channel to be used to change the LED strip profile. ("when channel") - - Set the range to cover the entire range of the selected AUX channel. ("is in ranges") - - For the action select "RC Rate Adjustment". ("then apply") This will be configured in the CLI since LED strip profiles is not supported by Configurator 10.4.0 and earlier. "RC Rate Adjustment" is only selected to make the configuration in the CLI a little easier below. - - Select the "via channel" to match the selected AUX channel of above. ("when channel"). +###### OPTION 2: Configure an adjustment range to change the LED strip profile from your radio +1. The LED strip profile selection is performed using an adjustment configured via the *Adjustments* tab. + - Enable an adjustment by selecting *Mapped*. + - Select the AUX channel to be used to change the LED strip profile under *Enable channel* and set the range to 900-2100us + - Set the same AUX channel and range under *Value channel* + - For the adjustment select *LED Profile Selection* and set the range to 1-4. - Save -3. Open the CLI and type ```adjrange``` followed by enter. -4. Copy the adjrange configured in step 2. above and paste it in the command window. Change the '1' following the range of the channel to '48' and press enter. Type ```save``` and press enter. The configured adjrange will now be saved and the FC will reboot. -5. Configure the AUX channel on your radio. When this channel is changed the selected LED strip profile will change between STATUS, RACE and BEACON, you should see the LED function change as you do this. +2. Configure the AUX channel on your radio. When this channel is changed the selected LED strip profile will change between RACE, BEACON, STATUS and STATUS_ALT. You should see the LED function change as you do this. -###### OPTION 2: Use the CLI to select the LED strip profile (i.e. not selecting the LED strip profile with your radio) +###### OPTION 3: Use the CLI to select the LED strip profile (i.e. not selecting the LED strip profile with your radio) 1. Open the CLI. 2. Type ```get ledstrip_profile``` followed by enter to display the currently selected LED strip profile. -3. Type ```set ledstrip_profile=x``` where x is the profile STATUS, RACE or BEACON and press enter. +3. Type ```set ledstrip_profile=x``` where x is the profile STATUS, STATUS_ALT, RACE or BEACON and press enter. 4. Type ```save``` followed by enter to save the selected LED strip profile. -###### OPTION 3: By using the OSD +###### OPTION 4: By using the OSD 1. Open the OSD menu by yawing left and pitching forward on your radio. 2. Using the pitch stick, move down to the LED Strip menu and roll right to enter the menu. 3. The profile and race color can be configured using the left stick to go back and the right stick to navigate up/down and to change the selected value. @@ -76,7 +75,7 @@ The BEACON profile is used to find a lost quad, it flashes all LEDs white once p 4. Type ```save``` followed by enter to save the race color to be used. -###### BRIGHTNESS: The brightness can be configured using the CLI: +###### BRIGHTNESS: The overall brightness can be configured using the CLI: 1. Open the CLI. 2. Type ```get ledstrip_brightness``` followed by enter to display the current brightness. 3. Type ```set ledstrip_brightness=x``` where x is the brightness in percentage between 5 and 100. @@ -85,14 +84,12 @@ The BEACON profile is used to find a lost quad, it flashes all LEDs white once p ## Supported hardware -Only strips of 32 WS2811/WS2812 LEDs are supported currently. If the strip is longer than 32 LEDs it does not matter, but only the first 32 are used. +Up to 32 WS2811/WS2812 LEDs are currently supported. WS2812 LEDs require an 800khz signal and precise timings and thus requires the use of a dedicated hardware timer. Note: Not all WS2812 ICs use the same timings, some batches use different timings. -It could be possible to be able to specify the timings required via CLI if users request it. - ### Tested Hardware * [Adafruit NeoPixel Jewel 7](https://www.adafruit.com/products/2226) (preliminary testing) @@ -143,34 +140,32 @@ set ledstrip_inverted_format = 9 WS2812 LED strips generally require a single data line, 5V and GND. -WS2812 LEDs on full brightness can consume quite a bit of current. It is recommended to verify the current draw and ensure your supply can cope with the load. On a multirotor that uses multiple BEC ESC's you can try use a different BEC to the one the FC uses. e.g. ESC1/BEC1 -> FC, ESC2/BEC2 -> LED strip. It's also possible to power one half of the strip from one BEC and the other half from another BEC. Just ensure that the GROUND is the same for all BEC outputs and LEDs. - -| Target | Pin | LED Strip | Signal | -| --------------------- | ---- | --------- | -------| -| Naze | RC5 | Data In | PA6 | -| CC3D | RCO5 | Data In | PB4 | -| ChebuzzF3/F3Discovery | PB8 | Data In | PB8 | -| Sparky | PWM5 | Data In | PA6 | +WS2812 LEDs on full brightness can consume quite a bit of current. It is recommended to verify the current draw and ensure your supply can cope with the load. On a multirotor that uses multiple BEC ESC's you can try use a different BEC to the one the FC uses. e.g. ESC1/BEC1 -> FC, ESC2/BEC2 -> LED strip. It's also possible to power one half of the strip from one BEC and the other half from another BEC. Just ensure that the GROUND is the same for all BEC outputs and LEDs. -Since RC5 is also used for SoftSerial on the Naze it means that you cannot use SoftSerial and led strips at the same time. Additionally, since RC5 is also used for Parallel PWM RC input on both the Naze, Chebuzz and STM32F3Discovery targets, led strips can not be used at the same time at Parallel PWM. +LED Strip pin should be: +- on a separate timer +- with DMA enabled -If you have LEDs that are intermittent, flicker or show the wrong colors then drop the VIN to less than 4.7v, e.g. by using an inline diode on the VIN to the LED strip. The problem occurs because of the difference in voltage between the data signal and the power signal. The WS2811 LED's require the data signal (Din) to be between 0.3 * Vin (Max) and 0.7 * VIN (Min) to register valid logic low/high signals. The LED pin on the CPU will always be between 0v to ~3.3v, so the Vin should be 4.7v (3.3v / 0.7 = 4.71v). Some LEDs are more tolerant of this than others. +If you have LEDs that are intermittent, flicker or show the wrong colors then drop the VIN to less than 4.7v, e.g. by using an inline +diode on the VIN to the LED strip. The problem occurs because of the difference in voltage between the data signal and the power +signal. The WS2811 LED's require the data signal (Din) to be between 0.3 * Vin (Max) and 0.7 * VIN (Min) to register valid logic +low/high signals. The LED pin on the CPU will always be between 0v to ~3.3v, so the Vin should be 4.7v (3.3v / 0.7 = 4.71v). +Some LEDs are more tolerant of this than others. -The datasheet can be found here: http://www.adafruit.com/datasheets/WS2812.pdf +The datasheet can be found here: [WS2812](http://www.adafruit.com/datasheets/WS2812.pdf) ## Configuration -The led strip feature can be configured via the GUI. +The LED strip feature can be configured via the Configurator or the CLI. -GUI: -Enable the Led Strip feature via the GUI under setup. +### Configurator +First enable the *LED_STRIP* feature in the *Configuration* tab. The *LED Strip* tab should now become visible. -Configure the LEDs from the Led Strip tab in the Betaflight GUI. -First setup how the LEDs are laid out so that you can visualize it later as you configure and so the flight controller knows how many LEDs there are available. +Now go to the *LED Strip* tab and configure the LEDs. First setup how the LEDs are laid out so that you can visualize it later as you configure and so the flight controller knows how many LEDs there are available. -There is a step by step guide on how to use the GUI to configure the Led Strip feature using the GUI http://blog.oscarliang.net/setup-rgb-led-cleanflight/ which was published early 2015 by Oscar Liang which may or may not be up-to-date by the time you read this. +There is a step by step guide on how to use the Configurator to [configure the Led Strip feature](http://blog.oscarliang.net/setup-rgb-led-cleanflight/) which was published early 2015 by Oscar Liang. -CLI: +### CLI Enable the `LED_STRIP` feature via the cli: ``` @@ -181,14 +176,14 @@ If you enable LED_STRIP feature and the feature is turned off again after a rebo Configure the LEDs using the `led` command. -The `led` command takes either zero or two arguments - an zero-based led number and a sequence which indicates pair of coordinates, direction flags and mode flags and a color. +The `led` command takes either zero or two arguments - an zero-based LED number and a sequence which indicates pair of coordinates, direction flags and mode flags and a color. -If used with zero arguments it prints out the led configuration which can be copied for future reference. +If used with zero arguments it prints out the LED configuration which can be copied for future reference. -Each led is configured using the following template: `x,y:direction:mode:color:blinkpattern:blinkpause:alternatecolor` +Each LED is configured using the following template: `x,y:direction:mode:color:blinkpattern:blinkpause:alternatecolor` `x` and `y` are grid coordinates of a 0 based 16x16 grid, north west is 0,0, south east is 15,15 -`direction` specifies the directions, since an led can face in any direction it can have multiple directions. Directions are: +`direction` specifies the directions, since an LED can face in any direction it can have multiple directions. Directions are: `N` - North `E` - East @@ -222,15 +217,15 @@ And each LED can have one ore more overlays: * `O` - Lars`O`n Scanner (Cylon Effect). * `V` - `V`TX Frequency. * `K` - Flic`K`er. -* `D` - `D`immed. +* `D` - Fa`D`e. -`color` specifies the color number (0 based index). +`color` specifies the color number (0 based index). This color has a black border in the Configurator. -`blinkpattern` specifies a 16 bit bitmask which indicates when a LED should blink. Example: two fast blinks would be 5 (binary 101), two slow blinks 3855 (binary 111100001111). +`blinkpattern` specifies a 16 bit bitmask which indicates when a LED should blink. Example: two fast blinks would be 5 (binary 101), two slow blinks 3855 (binary 111100001111). The `alternatecolor` is used for blinking. `blinkpause` specifies whether blinking should pause after the blink pattern has been finished. A value of `0` doesn't pause blinking, a value of `3` pauses the blinking for three subsequent rounds. -`alternatecolor` specifies the color for: a. blinking; b. dimming (usually black). +`alternatecolor` specifies the color for: a. blinking; b. fading between `color` and `alternatecolor` if the LED profile is switched from STATUS to STATUS_ALT. The `alternatecolor` can be black if you want to turn a light off in in the STATUS_ALT profile. The alternate color has a red border in the Configurator. Example: @@ -244,7 +239,7 @@ led 5 8,8::C:2:0:0:0 led 6 8,9::CB:1:3855:0:2 ``` -To erase an led, and to mark the end of the chain, use `0,0::` as the second argument, like this: +To erase an LED, and to mark the end of the chain, use `0,0::` as the second argument, like this: ``` led 4 0,0::: @@ -315,9 +310,9 @@ This overlay makes the LED blink from the current active color to the alternate This overlay makes the LED flicker, a bit like a candle. Set the flicker rate using `ledstrip_flicker_rate`. -### Dimmed +#### Fade to alt color -A led with this overlay will fade to the alternate color in the profile STATUS_DIMMED. Specify Black as the alternate color to turn LEDs of (e.g. landing lights). You can set the dimmer rate with `ledstrip_dimmer_rate`. +A LED with this overlay will fade to the alternate color in the profile STATUS_ALT. Specify Black as the alternate color to turn LEDs of (e.g. for switching off landing lights). You can set the fade rate with `ledstrip_fade_rate`. #### Larson Scanner (Cylon Effect) @@ -355,7 +350,7 @@ LEDs are set in a specific order: That is, south facing LEDs have priority. -The mapping between modes led placement and colors is currently fixed and cannot be changed. +The mapping between modes LED placement and colors is currently fixed and cannot be changed. #### Indicator @@ -417,7 +412,7 @@ This mode fades the current LED color to the previous/next color in the HSB colo #### Thrust ring state -This mode is allows you to use one or multiple led rings (e.g. NeoPixel ring) for an afterburner effect. LEDs with this mode will light up with their assigned color in a repeating sequence. Assigning the color black to an LED with the ring mode will prevent the LED from lighting up. +This mode is allows you to use one or multiple LED rings (e.g. NeoPixel ring) for an afterburner effect. LEDs with this mode will light up with their assigned color in a repeating sequence. Assigning the color black to an LED with the ring mode will prevent the LED from lighting up. A better effect is achieved when LEDs configured for thrust ring have no other functions. @@ -425,7 +420,7 @@ LED direction and X/Y positions are irrelevant for thrust ring LED state. The o Each LED of the ring can be a different color. The color can be selected between the 16 colors available. -For example, led 0 is set as a `R`ing thrust state led in color 13 as follow. +For example, LED 0 is set as a `R`ing thrust state LED in color 13 as follow. ``` led 0 2,2::R:13 @@ -441,7 +436,7 @@ x,y position and directions are ignored when using this mode. Other modes will override or combine with the color mode. -For example, to set led 0 to always use color 10 you would issue this command. +For example, to set LED 0 to always use color 10 you would issue this command. ``` led 0 0,0::C:10 @@ -453,7 +448,7 @@ Colors can be configured using the cli `color` command. The `color` command takes either zero or two arguments - an zero-based color number and a sequence which indicates pair of hue, saturation and value (HSV). -See http://en.wikipedia.org/wiki/HSL_and_HSV +See [HSL and HSV](http://en.wikipedia.org/wiki/HSL_and_HSV) If used with zero arguments it prints out the color configuration which can be copied for future reference. diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index cf3215b63d..104b38d7b9 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -378,7 +378,7 @@ static const char * const lookupTableSdcardMode[] = { #ifdef USE_LED_STRIP #ifdef USE_LED_STRIP_STATUS_MODE static const char * const lookupTableLEDProfile[] = { - "RACE", "BEACON", "STATUS", "STATUS_DIMMED" + "RACE", "BEACON", "STATUS", "STATUS_ALT" }; #else static const char * const lookupTableLEDProfile[] = { @@ -1238,7 +1238,7 @@ const clivalue_t valueTable[] = { { "ledstrip_brightness", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 5, 100 }, PG_LED_STRIP_CONFIG, offsetof(ledStripConfig_t, ledstrip_brightness) }, { "ledstrip_blink_period_ms", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 500 }, PG_LED_STRIP_CONFIG, offsetof(ledStripConfig_t, ledstrip_blink_period_ms) }, { "ledstrip_flicker_rate", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_LED_STRIP_CONFIG, offsetof(ledStripConfig_t, ledstrip_flicker_rate) }, - { "ledstrip_dimmer_rate", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_LED_STRIP_CONFIG, offsetof(ledStripConfig_t, ledstrip_dimmer_rate) }, + { "ledstrip_fade_rate", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_LED_STRIP_CONFIG, offsetof(ledStripConfig_t, ledstrip_fade_rate) }, { "ledstrip_inverted_format", VAR_UINT32 | MASTER_VALUE, .config.u32Max = UINT32_MAX, PG_LED_STRIP_CONFIG, offsetof(ledStripConfig_t, ledstrip_inverted_format) }, #endif diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 2beba80cb3..adb0c9d817 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -135,7 +135,7 @@ void pgResetFn_ledStripConfig(ledStripConfig_t *ledStripConfig) ledStripConfig->ledstrip_brightness = 100; ledStripConfig->ledstrip_blink_period_ms = 100; ledStripConfig->ledstrip_flicker_rate = 50; - ledStripConfig->ledstrip_dimmer_rate = 50; + ledStripConfig->ledstrip_fade_rate = 50; ledStripConfig->ledstrip_inverted_format = 0; #ifndef UNIT_TEST ledStripConfig->ioTag = timerioTagGetByUsage(TIM_USE_LED, 0); @@ -1007,12 +1007,12 @@ static void applyLedDimmerLayer(bool updateNow, timeUs_t *timer) } if (dimmerParameters.previousLedProfile != ledStripConfig()->ledstrip_profile) { - if (ledStripConfig()->ledstrip_profile == LED_PROFILE_STATUS_DIMMED) { - // Start dimming + if (ledStripConfig()->ledstrip_profile == LED_PROFILE_STATUS_ALT) { + // Transition to alternate color dimmerParameters.direction = -1; dimmerParameters.currentBrightness = maxBrightness; } else { - // Start brightening + // Transition to normal color dimmerParameters.direction = 1; dimmerParameters.currentBrightness = 0; } @@ -1020,10 +1020,10 @@ static void applyLedDimmerLayer(bool updateNow, timeUs_t *timer) } if (dimmerParameters.direction != 0) { - int8_t step = ledStripConfig()->ledstrip_dimmer_rate; + int8_t step = ledStripConfig()->ledstrip_fade_rate; if (dimmerParameters.direction == -1) { - if (ledStripConfig()->ledstrip_dimmer_rate <= 20 && dimmerParameters.currentBrightness < 50) { + if (ledStripConfig()->ledstrip_fade_rate <= 20 && dimmerParameters.currentBrightness < 50) { // Slow down dimming even more when dimmer rate is low. step = -MIN(1 + dimmerParameters.currentBrightness/5, step); } else { @@ -1391,7 +1391,7 @@ void ledStripUpdate(timeUs_t currentTimeUs) switch (ledStripConfig()->ledstrip_profile) { #ifdef USE_LED_STRIP_STATUS_MODE case LED_PROFILE_STATUS: - case LED_PROFILE_STATUS_DIMMED: { + case LED_PROFILE_STATUS_ALT: { applyStatusProfile(currentTimeUs); break; } diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index af46a003d3..8960455ed1 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -164,7 +164,7 @@ typedef enum { LED_PROFILE_BEACON, #ifdef USE_LED_STRIP_STATUS_MODE LED_PROFILE_STATUS, - LED_PROFILE_STATUS_DIMMED, + LED_PROFILE_STATUS_ALT, #endif LED_PROFILE_COUNT } ledProfile_e; @@ -198,10 +198,10 @@ typedef struct ledStripConfig_s { uint8_t ledstrip_beacon_percent; uint8_t ledstrip_beacon_armed_only; colorId_e ledstrip_visual_beeper_color; - uint8_t ledstrip_brightness; + uint8_t ledstrip_brightness; uint16_t ledstrip_blink_period_ms; uint8_t ledstrip_flicker_rate; - uint8_t ledstrip_dimmer_rate; + uint8_t ledstrip_fade_rate; uint32_t ledstrip_inverted_format; } ledStripConfig_t; diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index ccdd2695d1..7f56979979 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1647,7 +1647,7 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst) sbufWriteU16(dst, ledStripConfigMutable()->ledstrip_beacon_period_ms); sbufWriteU16(dst, ledStripConfigMutable()->ledstrip_blink_period_ms); sbufWriteU8(dst, ledStripConfigMutable()->ledstrip_brightness); - sbufWriteU8(dst, ledStripConfigMutable()->ledstrip_dimmer_rate); + sbufWriteU8(dst, ledStripConfigMutable()->ledstrip_fade_rate); sbufWriteU8(dst, ledStripConfigMutable()->ledstrip_flicker_rate); sbufWriteU8(dst, ledStripConfigMutable()->ledstrip_grb_rgb); sbufWriteU8(dst, ledStripConfigMutable()->ledstrip_profile); @@ -3132,7 +3132,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, ledStripConfigMutable()->ledstrip_beacon_period_ms = sbufReadU16(src); ledStripConfigMutable()->ledstrip_blink_period_ms = sbufReadU16(src); ledStripConfigMutable()->ledstrip_brightness = sbufReadU8(src); - ledStripConfigMutable()->ledstrip_dimmer_rate = sbufReadU8(src); + ledStripConfigMutable()->ledstrip_fade_rate = sbufReadU8(src); ledStripConfigMutable()->ledstrip_flicker_rate = sbufReadU8(src); ledStripConfigMutable()->ledstrip_grb_rgb = sbufReadU8(src); ledStripConfigMutable()->ledstrip_profile = sbufReadU8(src); From a1d7b164ca43bd765eca77beacd5e2e915c1d2e8 Mon Sep 17 00:00:00 2001 From: Rob Gayle Date: Fri, 15 Mar 2024 11:52:55 -0400 Subject: [PATCH 09/24] OpenYGE - support v3 frame header extension - documentation (cherry picked from commit 00fc315409dd568015c3bb77abaf3bc157c9b503) --- src/main/sensors/esc_sensor.c | 40 ++++++++++++++++++----------------- 1 file changed, 21 insertions(+), 19 deletions(-) diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index c58c4e5e70..c7d648b919 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -1545,28 +1545,30 @@ static void apdSensorProcess(timeUs_t currentTimeUs) * 1: version; // frame version * 2: frame_type // telemetry data = 0 * 3: frame_length; // frame length including header and CRC + * 4: reserved // reserved + * 5: reserved // reserved * * Payload... - * 4: reserved // reserved - * 5: temperature; // C degrees (0-> -40°C, 255->215°C) - * 6,7: voltage; // 0.01V Little endian! - * 8,9: current; // 0.01A Little endian! - * 10,11: consumption; // mAh Little endian! - * 12,13: rpm; // 0.1rpm Little endian! - * 14: pwm; // % - * 15: throttle; // % - * 16,17: bec_voltage; // 0.01V Little endian! - * 18,19: bec_current; // 0.01A Little endian! - * 20: bec_temp; // C degrees (0-> -40°C, 255->215°C) - * 21: status1; // see documentation - * 22: cap_temp; // C degrees (0-> -40°C, 255->215°C) - * 23: aux_temp; // C degrees (0-> -40°C, 255->215°C) - * 24: status2; // reserved - * 25: reserved1; // reserved - * 26,27: idx; // maybe future use - * 28,29: idx_data; // maybe future use + * +0: reserved // reserved + * 1: temperature; // C degrees (0-> -40°C, 255->215°C) + * 2,3: voltage; // 0.01V Little endian! + * 4,5: current; // 0.01A Little endian! + * 6,7: consumption; // mAh Little endian! + * 8,9: rpm; // 0.1rpm Little endian! + * 10: pwm; // % + * 11: throttle; // % + * 12,13: bec_voltage; // 0.01V Little endian! + * 14,15: bec_current; // 0.01A Little endian! + * 16: bec_temp; // C degrees (0-> -40°C, 255->215°C) + * 17: status1; // see documentation + * 18: cap_temp; // C degrees (0-> -40°C, 255->215°C) + * 19: aux_temp; // C degrees (0-> -40°C, 255->215°C) + * 20: status2; // reserved + * 21: reserved1; // reserved + * 22,23: idx; // maybe future use + * 24,25: idx_data; // maybe future use * - * 30,31: crc16; // CCITT, poly: 0x1021 + * 32,33: crc16; // CCITT, poly: 0x1021 * */ From fb83f371f8706c354340f0b1c36563e8d13fcebc Mon Sep 17 00:00:00 2001 From: Rob G Date: Sun, 17 Mar 2024 04:02:25 -0400 Subject: [PATCH 10/24] OpenYGE - fix to support v3 frame header extension (#87) * OpenYGE - support v3 frame header extension (cherry picked from commit 7ed080053db66add602356bcf52d24507293c2f5) * OpenYGE - support v3 frame header extension - documentation --- src/main/sensors/esc_sensor.c | 58 +++++++++++++++++++---------------- 1 file changed, 31 insertions(+), 27 deletions(-) diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index 8f7aa6c8b9..62f00865c8 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -1524,28 +1524,30 @@ static void apdSensorProcess(timeUs_t currentTimeUs) * 1: version; // frame version * 2: frame_type // telemetry data = 0 * 3: frame_length; // frame length including header and CRC + * 4: reserved // reserved + * 5: reserved // reserved * * Payload... - * 4: reserved // reserved - * 5: temperature; // C degrees (0-> -40°C, 255->215°C) - * 6,7: voltage; // 0.01V Little endian! - * 8,9: current; // 0.01A Little endian! - * 10,11: consumption; // mAh Little endian! - * 12,13: rpm; // 0.1rpm Little endian! - * 14: pwm; // % - * 15: throttle; // % - * 16,17: bec_voltage; // 0.01V Little endian! - * 18,19: bec_current; // 0.01A Little endian! - * 20: bec_temp; // C degrees (0-> -40°C, 255->215°C) - * 21: status1; // see documentation - * 22: cap_temp; // C degrees (0-> -40°C, 255->215°C) - * 23: aux_temp; // C degrees (0-> -40°C, 255->215°C) - * 24: status2; // reserved - * 25: reserved1; // reserved - * 26,27: idx; // maybe future use - * 28,29: idx_data; // maybe future use + * +0: reserved // reserved + * 1: temperature; // C degrees (0-> -40°C, 255->215°C) + * 2,3: voltage; // 0.01V Little endian! + * 4,5: current; // 0.01A Little endian! + * 6,7: consumption; // mAh Little endian! + * 8,9: rpm; // 0.1rpm Little endian! + * 10: pwm; // % + * 11: throttle; // % + * 12,13: bec_voltage; // 0.01V Little endian! + * 14,15: bec_current; // 0.01A Little endian! + * 16: bec_temp; // C degrees (0-> -40°C, 255->215°C) + * 17: status1; // see documentation + * 18: cap_temp; // C degrees (0-> -40°C, 255->215°C) + * 19: aux_temp; // C degrees (0-> -40°C, 255->215°C) + * 20: status2; // reserved + * 21: reserved1; // reserved + * 22,23: idx; // maybe future use + * 24,25: idx_data; // maybe future use * - * 30,31: crc16; // CCITT, poly: 0x1021 + * 32,33: crc16; // CCITT, poly: 0x1021 * */ @@ -1650,14 +1652,16 @@ static uint8_t oygeDecodeTelemetryFrame(void) } uint8_t version = buffer[1]; - int16_t temp = buffer[5]; - uint16_t volt = buffer[7] << 8 | buffer[6]; - uint16_t curr = buffer[9] << 8 | buffer[8]; - uint16_t capa = buffer[11] << 8 | buffer[10]; - uint16_t erpm = buffer[13] << 8 | buffer[12]; - uint8_t pwm = buffer[14]; - uint16_t voltBEC = buffer[17] << 8 | buffer[16]; - int16_t tempBEC = buffer[20]; + uint8_t hl = (version >= 3) ? 6 : 4; + + int16_t temp = buffer[hl+1]; + uint16_t volt = buffer[hl+3] << 8 | buffer[hl+2]; + uint16_t curr = buffer[hl+5] << 8 | buffer[hl+4]; + uint16_t capa = buffer[hl+7] << 8 | buffer[hl+6]; + uint16_t erpm = buffer[hl+9] << 8 | buffer[hl+8]; + uint8_t pwm = buffer[hl+10]; + uint16_t voltBEC = buffer[hl+13] << 8 | buffer[hl+12]; + int16_t tempBEC = buffer[hl+16]; if (version >= 2) { // apply temperature mapping offsets From 58373c04f309a802e519cbe69b914b333318ef5d Mon Sep 17 00:00:00 2001 From: Rob Gayle Date: Fri, 22 Mar 2024 01:00:33 -0400 Subject: [PATCH 11/24] OpenYGE - telemetry - BEC voltage and current fix --- src/main/sensors/esc_sensor.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index c7d648b919..cd6fdf7b9f 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -1557,8 +1557,8 @@ static void apdSensorProcess(timeUs_t currentTimeUs) * 8,9: rpm; // 0.1rpm Little endian! * 10: pwm; // % * 11: throttle; // % - * 12,13: bec_voltage; // 0.01V Little endian! - * 14,15: bec_current; // 0.01A Little endian! + * 12,13: bec_voltage; // 0.001V Little endian! + * 14,15: bec_current; // 0.001A Little endian! * 16: bec_temp; // C degrees (0-> -40°C, 255->215°C) * 17: status1; // see documentation * 18: cap_temp; // C degrees (0-> -40°C, 255->215°C) @@ -1700,8 +1700,8 @@ static uint8_t oygeDecodeTelemetryFrame(void) escSensorData[0].consumption = capa; escSensorData[0].temperature = temp * 10; escSensorData[0].temperature2 = tempBEC * 10; - escSensorData[0].bec_voltage = voltBEC * 10; - escSensorData[0].bec_current = currBEC * 10; + escSensorData[0].bec_voltage = voltBEC; + escSensorData[0].bec_current = currBEC; escSensorData[0].status = status; totalFrameCount++; From aaa8e19bffa23301b9219e457b3761a4bb9ee476 Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Fri, 22 Mar 2024 18:43:15 +0000 Subject: [PATCH 12/24] Add github workflows for CI builds --- .github/workflows/pr.yml | 46 +++++++++++++++++++++++++++++++++++ .github/workflows/push.yml | 42 ++++++++++++++++++++++++++++++++ .github/workflows/testing.yml | 2 +- 3 files changed, 89 insertions(+), 1 deletion(-) create mode 100644 .github/workflows/pr.yml create mode 100644 .github/workflows/push.yml diff --git a/.github/workflows/pr.yml b/.github/workflows/pr.yml new file mode 100644 index 0000000000..4c362db076 --- /dev/null +++ b/.github/workflows/pr.yml @@ -0,0 +1,46 @@ +name: PR-CI + +on: + pull_request: + branches: + - 'master' + - 'RF-*' + +env: + + TARGETS: unified MATEKF405 MATEKF411 MATEKF722 MATEKH743 + +jobs: + + ci-build: + + runs-on: ubuntu-latest + + steps: + - name: Check out repository + uses: actions/checkout@v4 + + - name: Install ARM tools + run: make arm_sdk_install + + - name: Set build variables + run: | + PR=${GITHUB_REF} + PR=${PR%%/merge} + PR=${PR##*/} + echo "GIT_PR=${PR}" >> ${GITHUB_ENV} + echo "GIT_VER=${GITHUB_SHA:0:7}" >> ${GITHUB_ENV} + cat ${GITHUB_ENV} + + - name: Build HEX files for multiple targets + run: make unified FC_VER_SUFFIX="${{ env.GIT_VER }}" FLASH_CONFIG_ERASE=yes + + - name: Move HEX + run: mv -v obj/*.hex . + + - name: Upload Artifacts + uses: actions/upload-artifact@v4 + with: + name: rotorflight-PR${{ env.GIT_PR }}-${{ env.GIT_VER }} + path: rotorflight*.hex + diff --git a/.github/workflows/push.yml b/.github/workflows/push.yml new file mode 100644 index 0000000000..fc1db360cf --- /dev/null +++ b/.github/workflows/push.yml @@ -0,0 +1,42 @@ +name: Push-CI + +on: + push: + branches: + - 'master' + - 'RF-*' + +env: + + TARGETS: unified MATEKF405 MATEKF411 MATEKF722 MATEKH743 + +jobs: + + ci-build: + + runs-on: ubuntu-latest + + steps: + - name: Check out repository + uses: actions/checkout@v4 + + - name: Install ARM tools + run: make arm_sdk_install + + - name: Set build variables + run: | + echo "GIT_VER=${GITHUB_SHA:0:7}" >> ${GITHUB_ENV} + cat ${GITHUB_ENV} + + - name: Build HEX files for multiple targets + run: make ${{ env.TARGETS }} FC_VER_SUFFIX="${{ env.GIT_VER }}" FLASH_CONFIG_ERASE=yes + + - name: Move HEX + run: mv -v obj/*.hex . + + - name: Upload Artifacts + uses: actions/upload-artifact@v4 + with: + name: rotorflight-CI-${{ env.GIT_VER }} + path: rotorflight*.hex + diff --git a/.github/workflows/testing.yml b/.github/workflows/testing.yml index b3bd920049..a5c2af6e82 100644 --- a/.github/workflows/testing.yml +++ b/.github/workflows/testing.yml @@ -33,7 +33,7 @@ jobs: run: make unified FC_VER_SUFFIX="${{ env.GIT_VER }}" FLASH_CONFIG_ERASE=yes - name: Move HEX - run: mv obj/*.hex . + run: mv -v obj/*.hex . - name: Upload Artifacts uses: actions/upload-artifact@v4 From c0e649613bd4fb977fc3c7e3d54353af7ec628ea Mon Sep 17 00:00:00 2001 From: Rob Thomson Date: Wed, 27 Mar 2024 22:28:55 +0000 Subject: [PATCH 13/24] Add GOV_MODE FrSky telemetry sensor (#89) Added sensor 0x5450 to send governor status to frsky as 0 - OFF 1 - IDLE 2 - SPOOLUP 3 - RECOVERY 4 - ACTIVE 5 - THR-OFF 6 - LOST-HS 7 - AUTOROT 8 - BAILOUT 100 - DISABLED 101 - DISARMED Enabled via: set telemetry_enable_gov_mode = ON --- src/main/cli/settings.c | 1 + src/main/telemetry/smartport.c | 32 +++++++++++++++++++++++++++++++- src/main/telemetry/telemetry.h | 3 ++- 3 files changed, 34 insertions(+), 2 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 104b38d7b9..ca25c215de 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1219,6 +1219,7 @@ const clivalue_t valueTable[] = { { "telemetry_enable_temperature", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = LOG2(SENSOR_TEMPERATURE), PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, enableSensors)}, { "telemetry_enable_cap_used", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = LOG2(SENSOR_CAP_USED), PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, enableSensors)}, { "telemetry_enable_adjustment", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = LOG2(SENSOR_ADJUSTMENT), PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, enableSensors)}, + { "telemetry_enable_gov_mode", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = LOG2(SENSOR_GOV_MODE), PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, enableSensors)}, #else { "telemetry_enable_sensors", VAR_UINT32 | MASTER_VALUE, .config.u32Max = SENSOR_ALL, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, enableSensors)}, #endif diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 1a068b24ac..81ca26e40e 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -57,6 +57,8 @@ #include "flight/mixer.h" #include "flight/pid.h" #include "flight/position.h" +#include "flight/motors.h" +#include "flight/governor.h" #include "io/beeper.h" #include "io/gps.h" @@ -130,6 +132,7 @@ enum FSSP_DATAID_ADJFUNC = 0x5110 , // custom FSSP_DATAID_ADJVALUE = 0x5111 , // custom FSSP_DATAID_CAP_USED = 0x5250 , + FSSP_DATAID_GOV_MODE = 0x5450 , //custom #if defined(USE_ACC) FSSP_DATAID_PITCH = 0x5230 , // custom FSSP_DATAID_ROLL = 0x5240 , // custom @@ -157,7 +160,7 @@ enum }; // if adding more sensors then increase this value (should be equal to the maximum number of ADD_SENSOR calls) -#define MAX_DATAIDS 24 +#define MAX_DATAIDS 25 static uint16_t frSkyDataIdTable[MAX_DATAIDS]; @@ -331,6 +334,11 @@ static void initSmartPortSensors(void) { frSkyDataIdTableInfo.index = 0; + //prob need configurator option for these? + if (telemetryIsSensorEnabled(SENSOR_GOV_MODE)) { + ADD_SENSOR(FSSP_DATAID_GOV_MODE); + } + if (telemetryIsSensorEnabled(SENSOR_MODE)) { ADD_SENSOR(FSSP_DATAID_T1); ADD_SENSOR(FSSP_DATAID_T2); @@ -630,6 +638,28 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear #endif switch (id) { + case FSSP_DATAID_GOV_MODE : + if (!ARMING_FLAG(ARMED)) { + if (isArmingDisabled()) + smartPortSendPackage(id, 100); //DISABLED + else + smartPortSendPackage(id, 101); //DISAMED + } else { + /* + 0, //"OFF", + 1, //"IDLE", + 2, // "SPOOLUP", + 3, //"RECOVERY", + 4, //"ACTIVE", + 5, //"THR-OFF", + 6, //"LOST-HS", + 7, //"AUTOROT", + 8, //"BAILOUT", + */ + smartPortSendPackage(id, getGovernorState()); + } + *clearToSend = false; + break; case FSSP_DATAID_VFAS : vfasVoltage = telemetryConfig()->report_cell_voltage ? getBatteryAverageCellVoltage() : getBatteryVoltage(); smartPortSendPackage(id, vfasVoltage); // in 0.01V according to SmartPort spec diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index 709245872b..13e1b1739b 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -69,7 +69,8 @@ typedef enum { SENSOR_TEMPERATURE = 1 << 19, SENSOR_CAP_USED = 1 << 20, SENSOR_ADJUSTMENT = 1 << 21, - SENSOR_ALL = (1 << 22) - 1, + SENSOR_GOV_MODE = 1 << 22, + SENSOR_ALL = (1 << 23) - 1, } sensor_e; typedef struct telemetryConfig_s { From f29df162853db7ff5f42c3bec75e84a359dfb8c8 Mon Sep 17 00:00:00 2001 From: Rotorflight Date: Thu, 28 Mar 2024 18:39:18 +0000 Subject: [PATCH 14/24] Fix TTA headroom calculation in Gov Passthrough (#92) Co-authored-by: Petri Mattila --- src/main/flight/governor.c | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/main/flight/governor.c b/src/main/flight/governor.c index 3c040c2631..e21d964104 100644 --- a/src/main/flight/governor.c +++ b/src/main/flight/governor.c @@ -465,7 +465,13 @@ static void govUpdateData(void) if (mixerMotorizedTail() && gov.TTAGain != 0) { float YAW = mixerGetInput(MIXER_IN_STABILIZED_YAW); float TTA = filterApply(&gov.TTAFilter, YAW) * getSpoolUpRatio() * gov.TTAGain; - float headroom = 2 * fmaxf(gov.TTALimit - gov.fullHeadSpeedRatio, 0); + float headroom = 0; + + if (gov.mode > GM_PASSTHROUGH) + headroom = 2 * fmaxf(1.0f + gov.TTALimit - gov.fullHeadSpeedRatio, 0); + else + headroom = gov.TTALimit; + gov.TTAAdd = constrainf(TTA, 0, headroom); DEBUG(TTA, 0, YAW * 1000); @@ -1009,7 +1015,7 @@ void governorInitProfile(const pidProfile_t *pidProfile) gov.Kf = pidProfile->governor.f_gain / 100.0f; gov.TTAGain = mixerRotationSign() * pidProfile->governor.tta_gain / -125.0f; - gov.TTALimit = pidProfile->governor.tta_limit / 100.0f + 1.0f; + gov.TTALimit = pidProfile->governor.tta_limit / 100.0f; if (gov.mode >= GM_STANDARD) gov.TTAGain /= gov.K * gov.Kp; From 2a2c1d1548a6f50043c53a8419b743e5e799b5c7 Mon Sep 17 00:00:00 2001 From: Rotorflight Date: Thu, 28 Mar 2024 19:09:02 +0000 Subject: [PATCH 15/24] Fix RPM filter error check with Direct Drive motors (#94) Don't trigger RPMFILTER error if a motor notch is enabled with a Direct Drive motor. Co-authored-by: Petri Mattila --- src/main/flight/rpm_filter.c | 36 ++++++++++++++++++++---------------- 1 file changed, 20 insertions(+), 16 deletions(-) diff --git a/src/main/flight/rpm_filter.c b/src/main/flight/rpm_filter.c index 7afc795a2b..9fb857626d 100644 --- a/src/main/flight/rpm_filter.c +++ b/src/main/flight/rpm_filter.c @@ -120,14 +120,16 @@ INIT_CODE void rpmFilterInit(void) bankNumber++; } // Main Motor (M1) - else if (source == 10 && enable10) { - CHECK_SOURCE(mainMotorIndex); - bank->motor = mainMotorIndex; - bank->ratio = ratio; - bank->minHz = constrainf((config->filter_bank_rpm_limit[index] / mainGearRatio) * ratio, 10, minHzLimit); - bank->maxHz = maxHzLimit; - bank->notchQ = notchQ; - bankNumber++; + else if (source == 10) { + if (enable10) { + CHECK_SOURCE(mainMotorIndex); + bank->motor = mainMotorIndex; + bank->ratio = ratio; + bank->minHz = constrainf((config->filter_bank_rpm_limit[index] / mainGearRatio) * ratio, 10, minHzLimit); + bank->maxHz = maxHzLimit; + bank->notchQ = notchQ; + bankNumber++; + } } // Main Rotor harmonics else if (source >= 11 && source <= 18) { @@ -141,14 +143,16 @@ INIT_CODE void rpmFilterInit(void) bankNumber++; } // Tail Motor (M2) - else if (source == 20 && enable20) { - CHECK_SOURCE(tailMotorIndex); - bank->motor = tailMotorIndex; - bank->ratio = ratio; - bank->minHz = constrainf((config->filter_bank_rpm_limit[index] / tailGearRatio) * ratio, 10, minHzLimit); - bank->maxHz = maxHzLimit; - bank->notchQ = notchQ; - bankNumber++; + else if (source == 20) { + if (enable20) { + CHECK_SOURCE(tailMotorIndex); + bank->motor = tailMotorIndex; + bank->ratio = ratio; + bank->minHz = constrainf((config->filter_bank_rpm_limit[index] / tailGearRatio) * ratio, 10, minHzLimit); + bank->maxHz = maxHzLimit; + bank->notchQ = notchQ; + bankNumber++; + } } // Tail Rotor harmonics else if (source >= 21 && source <= 28) { From bc69ec8b3cfc934d650bbece54e207374994690e Mon Sep 17 00:00:00 2001 From: Rob G Date: Sat, 6 Apr 2024 09:34:46 -0400 Subject: [PATCH 16/24] OpenYGE - fix: address possible buffer underflow condition, add 2nd paranoid frame length validation outside ISR/callback (#96) --- src/main/sensors/esc_sensor.c | 22 +++++++++++++++------- 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index 62f00865c8..c97cdd582b 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -1569,7 +1569,7 @@ enum { static timeMs_t oygeRampTimer = 0; static uint32_t oygeFrameTimestamp = 0; static uint16_t oygeFramePeriod = OPENYGE_FRAME_PERIOD_INITIAL; -static volatile uint8_t oygeFrameLength = OPENYGE_FRAME_MIN_LENGTH; +static volatile uint8_t oygeFrameLength = 0; static uint16_t oygeCalculateCRC16_CCITT(const uint8_t *ptr, size_t len) @@ -1622,11 +1622,13 @@ static FAST_CODE void oygeDataReceive(uint16_t c, void *data) else if (readBytes == 4) { // frame length // protect against buffer overflow - if (c > TELEMETRY_BUFFER_SIZE) + if (c < OPENYGE_FRAME_MIN_LENGTH || c > TELEMETRY_BUFFER_SIZE) { oygeFrameSyncError(); - else + } + else { + oygeFrameLength = c; syncCount++; - oygeFrameLength = c; + } } } } @@ -1634,7 +1636,7 @@ static FAST_CODE void oygeDataReceive(uint16_t c, void *data) static void oygeStartTelemetryFrame(timeMs_t currentTimeMs) { readBytes = 0; - + oygeFrameLength = OPENYGE_FRAME_MIN_LENGTH; oygeFrameTimestamp = currentTimeMs; } @@ -1644,9 +1646,15 @@ static uint8_t oygeDecodeTelemetryFrame(void) if (readBytes < oygeFrameLength) return OPENYGE_FRAME_PENDING; + // paranoid length check + uint8_t len = buffer[3]; + if (len < OPENYGE_FRAME_MIN_LENGTH || len > TELEMETRY_BUFFER_SIZE) { + totalCrcErrorCount++; + return OPENYGE_FRAME_FAILED; + } // verify CRC16 checksum - uint16_t crc = buffer[oygeFrameLength - 1] << 8 | buffer[oygeFrameLength - 2]; - if (oygeCalculateCRC16_CCITT(buffer, oygeFrameLength - 2) != crc) { + uint16_t crc = buffer[len - 1] << 8 | buffer[len - 2]; + if (oygeCalculateCRC16_CCITT(buffer, len - 2) != crc) { totalCrcErrorCount++; return OPENYGE_FRAME_FAILED; } From 73245ca5ca1584845c8da23b451aa865686727fa Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Wed, 10 Apr 2024 13:03:35 +0100 Subject: [PATCH 17/24] Add CI or PR to the firmware extra version string --- .github/workflows/pr.yml | 4 ++-- .github/workflows/push.yml | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/pr.yml b/.github/workflows/pr.yml index 4c362db076..82d3a81331 100644 --- a/.github/workflows/pr.yml +++ b/.github/workflows/pr.yml @@ -29,7 +29,7 @@ jobs: PR=${PR%%/merge} PR=${PR##*/} echo "GIT_PR=${PR}" >> ${GITHUB_ENV} - echo "GIT_VER=${GITHUB_SHA:0:7}" >> ${GITHUB_ENV} + echo "GIT_VER=PR${PR}-${GITHUB_SHA:0:7}" >> ${GITHUB_ENV} cat ${GITHUB_ENV} - name: Build HEX files for multiple targets @@ -41,6 +41,6 @@ jobs: - name: Upload Artifacts uses: actions/upload-artifact@v4 with: - name: rotorflight-PR${{ env.GIT_PR }}-${{ env.GIT_VER }} + name: rotorflight-${{ env.GIT_VER }} path: rotorflight*.hex diff --git a/.github/workflows/push.yml b/.github/workflows/push.yml index fc1db360cf..c54a200406 100644 --- a/.github/workflows/push.yml +++ b/.github/workflows/push.yml @@ -25,7 +25,7 @@ jobs: - name: Set build variables run: | - echo "GIT_VER=${GITHUB_SHA:0:7}" >> ${GITHUB_ENV} + echo "GIT_VER=CI-${GITHUB_SHA:0:7}" >> ${GITHUB_ENV} cat ${GITHUB_ENV} - name: Build HEX files for multiple targets @@ -37,6 +37,6 @@ jobs: - name: Upload Artifacts uses: actions/upload-artifact@v4 with: - name: rotorflight-CI-${{ env.GIT_VER }} + name: rotorflight-${{ env.GIT_VER }} path: rotorflight*.hex From ae2ba81e8d8075e184e031d83a5681a62948403e Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Thu, 11 Apr 2024 18:32:26 +0100 Subject: [PATCH 18/24] Add Horizon mode inverted self-leveling Co-authored-by: Ken Imhof --- src/main/common/maths.h | 3 +++ src/main/flight/imu.c | 9 +++++++++ src/main/flight/imu.h | 1 + src/main/flight/leveling.c | 15 ++++++++++++++- 4 files changed, 27 insertions(+), 1 deletion(-) diff --git a/src/main/common/maths.h b/src/main/common/maths.h index ae26dbd86d..2117512e85 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -128,6 +128,9 @@ static inline float tan_approx(float x) __typeof__ (x) _x = (x); \ _x > 0 ? _x : -_x; }) +#define SIGN(x) \ + __extension__ ({ __typeof__ (x) _x = (x); \ + (_x > 0) - (_x < 0); }) /* * Basic math operations diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 02bf139f2b..01ad72df95 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -634,3 +634,12 @@ bool isUpright(void) return true; #endif } + +bool isUpsidedown(void) +{ +#ifdef USE_ACC + return sensors(SENSOR_ACC) && attitudeIsEstablished && getCosTiltAngle() < 0; +#else + return false; +#endif +} diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 195aef04fb..38e01e7721 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -83,3 +83,4 @@ void imuSetHasNewData(uint32_t dt); bool shouldInitializeGPSHeading(void); bool isUpright(void); +bool isUpsidedown(void); diff --git a/src/main/flight/leveling.c b/src/main/flight/leveling.c index 28b929e6e9..70b857acd4 100644 --- a/src/main/flight/leveling.c +++ b/src/main/flight/leveling.c @@ -100,7 +100,20 @@ static float calcLevelErrorAngle(int axis) #endif angle = constrainf(angle, -level.AngleLimit, level.AngleLimit); - float error = angle - ((attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f); + float currentAngle = ((attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f); + + if (isUpsidedown() && FLIGHT_MODE(HORIZON_MODE)) { + switch (axis) { + case FD_PITCH: + currentAngle = -currentAngle; + break; + case FD_ROLL: + currentAngle -= SIGN(currentAngle) * 180; + break; + } + } + + float error = angle - currentAngle; return error; } From ea1ecc87e6004d0eed8b065e4dc03e11957d9c6e Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Fri, 12 Apr 2024 10:32:37 +0100 Subject: [PATCH 19/24] Update README.md --- README.md | 148 ++++++++++++++++++++++++------------------------------ 1 file changed, 66 insertions(+), 82 deletions(-) diff --git a/README.md b/README.md index 3dc12c0ad6..b2dab056bc 100644 --- a/README.md +++ b/README.md @@ -1,131 +1,115 @@ - # Rotorflight -Rotorflight is a Flight Control software suite for single-rotor helicopters. +[Rotorflight](https://github.com/rotorflight) is a Flight Control software suite designed for +single-rotor helicopters. It consists of: + +- Rotorflight Flight Controller Firmware (this repository) +- Rotorflight Configurator, for flashing and configuring the flight controller +- Rotorflight Blackbox Explorer, for analyzing blackbox flight logs +- Rotorflight LUA Scripts, for configuring the flight controller using a transmitter -It is based on Betaflight 4.3, with many advanced features added for helicopters. -Rotorflight does **NOT** support multi-rotor crafts, nor airplanes; it is only for traditinal RC helicopters. +Built on Betaflight 4.3, Rotorflight incorporates numerous advanced features specifically +tailored for helicopters. It's important to note that Rotorflight does _not_ support multi-rotor +crafts or airplanes; it's exclusively designed for RC helicopters. -**WARNING!** Rotorflight is **WORK-IN-PROGRESS**. Please talk to the authors if you want to participate in the -development effort! +This version of Rotorflight is also known as **Rotorflight 2** or **RF2**. ## Information -For latest information, please see [Rotorflight Wiki](https://github.com/rotorflight/rotorflight/wiki) +Tutorials, documentation, and flight videos can be found on the [Rotorflight website](https://www.rotorflight.org/). ## Features Rotorflight has many features: -* PID control tuned for helicopter use -* Fully customisable feedforward between collective/cyclic/yaw +* Many receiver protocols: CRSF, S.BUS, F.Port, DSM, IBUS, XBUS, EXBUS, GHOST, CPPM +* Support for various telemetry protocols: CSRF, S.Port, HoTT, etc. +* ESC telemetry protocols: BLHeli32, Hobbywing, Scorpion, Kontronik, OMP Hobby, ZTW, APD, YGE +* Advanced PID control tuned for helicopters +* Stabilisation modes (6D) * Rotor speed governor -* Advanced gyro filtering for helicopters - - Advanced dynamic LPF - - Dynamic RPM based notch filter banks +* Motorised tail support with Tail Torque Assist (TTA, also known as TALY) +* Remote configuration and tuning with the transmitter + - With knobs / switches assigned to functions + - With LUA scripts on EdgeTX, OpenTX and Ethos +* Extra servo/motor outputs for AUX functions * Fully customisable servo/motor mixer -* Flexible servo configuration -* Irrelevant (multi-rotor) features removed +* Sensors for battery voltage, current, BEC, etc. +* Advanced gyro filtering + - Dynamic RPM based notch filters + - Dynamic notch filters based on FFT + - Dynamic LPF +* High-speed Blackbox logging Plus lots of features inherited from Betaflight: * Configuration profiles for changing various tuning parameters -* Multi-color RGB LEDs +* Rates profiles for changing the stick feel and agility +* Multiple ESC protocols: PWM, DSHOT, Multishot, etc. * Configurable buzzer sounds -* Multiple ESC protocols: DShot (150,300,600), Multishot, Oneshot, and traditional PWM -* Multiple ESC telemetry protocols: KISS, HW -* Multiple RX telemetry protocols: CSRF, FrSky, HoTT, MSP, etc. -* Voltage inputs for RSSI, battery voltage, current sensors, etc. -* Fully integrated OSD -* Fully integrated video TX control (Unify Pro, IRC Tramp, etc.) +* Multi-color RGB LEDs +* GPS support And many more... ## Hardware support -Generally speaking, Rotorflight supports all flight controller hardware that is supported by Betaflight. -With a caveat that the flight controller must have enough suitable I/O pins for connecting all the servos -and motors required by a collective pitch helicopter. +The best hardware for Rotorflight is any Flight Controller especially designed for it. +See [What board is suitable?](https://www.rotorflight.org/docs/Tutorial-Quickstart/What-Board) -Also, the FC boards are typically labeled for multi-rotor use - thus the user needs to understand how these -functions can be used for a different purpose with helicopters. Usually this is just about using some -of the motor outputs for servos, but in some cases a more advanced remapping may be needed. +Otherwise, Rotorflight supports all flight controller boards that are supported by Betaflight 4.3, +assuming that the board has enough suitable I/O pins for connecting all the servos and motors required. -It is highly recommended to use a STM32F7 based flight controller, as Rotorflight greatly benefits from -the latest filtering algorithms and other new features that are all CPU intensive. +Also, the Betaflight boards are labeled for multi-rotor use - thus the user needs to understand how +these functions can be used for a different purpose with helicopters. Usually this is just about using +the motor outputs for servos, but in some cases a more advanced remapping may be needed. -An absolute minimum is a STM32F4 based board, but it probably won't be able to run all the new features. +Rotorflight supports STM32G4, STM32F4, STM32F7 and STM32H7 MCUs from ST. +It's highly recommended to use an STM32F7 based flight controller for Rotorflight. +It's the way to go since it can take full advantage of the latest control and filtering +algorithms, plus other cool features that really put the CPU to work. -## Installation +An absolute minimum is an STM32G4 based board, but it probably won't be able to run all +the new features later on. The older STM32F411 should be avoided if possible. -Please see the "Releases" page on the Wiki to download the latest official release: -* [Click here to go to Releases](https://github.com/rotorflight/rotorflight/wiki/releases) +## Installation -## Contributing - -Contributions are welcome and encouraged. You can contribute in many ways: +Download and flash the Rotorflight firmware with the +[Rotorflight Configurator](https://github.com/rotorflight/rotorflight-configurator/releases). -* testing Rotorflight with different types of helicopters -* documentation updates and corrections -* writing How-To guides -* bug reports -* new ideas & suggestions -* provide a new translation for configurator -* implement a new feature or a fix in the firmware +Flashing the Rotorflight firmware with any other flashing tools is not recommended. +Visit the [website](https://www.rotorflight.org/) for more details on setting up and using Rotorflight. -## Open Source / Contributors -Rotorflight is software that is **open source** and is available free of charge without warranty to all users. +## Contributing -Rotorflight is forked from Betaflight, which in turn is forked from Cleanflight. -Rotorflight borrows ideas and code also from Heliflight-3D, another Betaflight fork for helis. +Rotorflight is an open-source community project. Anybody can join in and help to make it better by: -So thanks goes to all those whom have contributed along the way. +* helping other users on Rotorflight Discord or other online forums +* [reporting](https://github.com/rotorflight?tab=repositories) bugs and issues, and suggesting improvements +* testing new software versions, new features and fixes; and providing feedback +* participating in discussions on new features +* create or update content on the [Website](https://www.rotorflight.org) +* [contributing](https://www.rotorflight.org/docs/Contributing/intro) to the software development - fixing bugs, implementing new features and improvements +* [translating](https://www.rotorflight.org/docs/Contributing/intro#translations) Rotorflight Configurator into a new language, or helping to maintain an existing translation -Origins for Rotorflight: -* **Dr.Rudder** (author, maintainer) -Origins for Heliflight-3D: -* **James-T1** (author) -* **Dr.Rudder** -* **Westie** +## Origins -Origins for Betaflight: -* **Alexinparis** (for MultiWii), -* **timecop** (for Baseflight), -* **Dominic Clifton** (for Cleanflight), -* **borisbstyle** (for Betaflight), and -* **Sambas** (for the original STM32F4 port). +Rotorflight is software that is **open source** and is available free of charge without warranty. -The Betaflight Configurator is forked from Cleanflight Configurator and its origins. +Rotorflight is forked from [Betaflight](https://github.com/betaflight), which in turn is forked from [Cleanflight](https://github.com/cleanflight). +Rotorflight borrows ideas and code also from [HeliFlight3D](https://github.com/heliflight3d/), another Betaflight fork for helicopters. -Origins for Betaflight Configurator: -* **Dominic Clifton** (for Cleanflight configurator), and -* **ctn** (for the original Configurator). +Big thanks to everyone who has contributed along the journey! -Big thanks to current and past contributors: -* Budden, Martin (martinbudden) -* Bardwell, Joshua (joshuabardwell) -* Blackman, Jason (blckmn) -* ctzsnooze -* Höglund, Anders (andershoglund) -* Ledvina, Petr (ledvinap) - **IO code awesomeness!** -* kc10kevin -* Keeble, Gary (MadmanK) -* Keller, Michael (mikeller) - **Configurator brilliance** -* Kravcov, Albert (skaman82) - **Configurator brilliance** -* MJ666 -* Nathan (nathantsoi) -* ravnav -* sambas - **bringing us the F4** -* savaga -* Stålheim, Anton (KiteAnton) -And many many others who haven't been mentioned.... +## Contact +Team Rotorflight can be contacted by email at rotorflightfc@gmail.com. From ef7d769938d855b0824bdfd81ccf455a66016259 Mon Sep 17 00:00:00 2001 From: Rob G Date: Mon, 15 Apr 2024 04:42:35 -0400 Subject: [PATCH 20/24] Fix Kontronik telemetry protocol (#98) Fix the telemetry protocol to work with Kolibri v3.0/v3.5 and Kosmik 200 HV v4.17 --- src/main/sensors/esc_sensor.c | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index c97cdd582b..4d6319e41b 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -1008,7 +1008,8 @@ static void uncSensorProcess(timeUs_t currentTimeUs) * 28-31: Error Flags * 32: Operational condition * 33: Timing 0..30 - * 34-37: CRC32 + * 34-35: Reserved + * 36-39: CRC32 * */ @@ -1049,7 +1050,7 @@ static bool processKontronikTelemetryStream(uint8_t dataByte) else syncCount++; } - else if (readBytes == 38) { + else if (readBytes == 40) { readBytes = 0; return true; } @@ -1062,9 +1063,9 @@ static void kontronikSensorProcess(timeUs_t currentTimeUs) // check for any available bytes in the rx buffer while (serialRxBytesWaiting(escSensorPort)) { if (processKontronikTelemetryStream(serialRead(escSensorPort))) { - uint32_t crc = buffer[37] << 24 | buffer[36] << 16 | buffer[35] << 8 | buffer[34]; + uint32_t crc = buffer[39] << 24 | buffer[38] << 16 | buffer[37] << 8 | buffer[36]; - if (calculateCRC32(buffer, 34) == crc) { + if (calculateCRC32(buffer, 36) == crc) { uint32_t rpm = buffer[7] << 24 | buffer[6] << 16 | buffer[5] << 8 | buffer[4]; uint16_t pwm = buffer[23] << 8 | buffer[22]; uint16_t voltage = buffer[9] << 8 | buffer[8]; From 9140133c8ef5fb908192a078292fc0bc697798b0 Mon Sep 17 00:00:00 2001 From: Rob Gayle Date: Mon, 1 Apr 2024 21:33:57 -0400 Subject: [PATCH 21/24] RFF-78 - escSensorData - added .throttle to. capture input setpoint as reported by ESC --- src/main/cli/settings.c | 2 +- src/main/sensors/esc_sensor.c | 14 ++++++++++++++ src/main/sensors/esc_sensor.h | 1 + src/main/telemetry/crsf.c | 3 +++ src/main/telemetry/crsf.h | 1 + 5 files changed, 20 insertions(+), 1 deletion(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index e5030ea169..2146efb978 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -493,7 +493,7 @@ const char * const lookupTableCrsfAttReuse[] = { }; const char * const lookupTableCrsfGpsReuse[] = { - "NONE", "HEADSPEED", "THROTTLE", "ESC_TEMP", "ESC_PWM", "ESC_BEC_VOLTAGE", "ESC_BEC_CURRENT", "ESC_BEC_TEMP", "ESC_STATUS", "ESC_STATUS2", "MCU_TEMP", "MCU_LOAD", "SYS_LOAD", "RT_LOAD", + "NONE", "HEADSPEED", "THROTTLE", "ESC_TEMP", "ESC_PWM", "ESC_THROTTLE", "ESC_BEC_VOLTAGE", "ESC_BEC_CURRENT", "ESC_BEC_TEMP", "ESC_STATUS", "ESC_STATUS2", "MCU_TEMP", "MCU_LOAD", "SYS_LOAD", "RT_LOAD", }; const char * const lookupTableCrsfGpsSatsReuse[] = { diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index 854f7f3b2a..b7aecbe7ff 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -603,6 +603,7 @@ static void hw4SensorProcess(timeUs_t currentTimeUs) escSensorData[0].age = 0; escSensorData[0].erpm = rpm; + escSensorData[0].throttle = thr; escSensorData[0].pwm = pwm; escSensorData[0].voltage = lrintf(voltage * 1000); escSensorData[0].current = lrintf(current * 1000); @@ -790,6 +791,7 @@ static void hw5SensorProcess(timeUs_t currentTimeUs) escSensorData[0].age = 0; escSensorData[0].erpm = rpm * 10; + escSensorData[0].throttle = power * 10; escSensorData[0].pwm = power * 10; escSensorData[0].voltage = voltage * 100; escSensorData[0].current = current * 100; @@ -919,6 +921,7 @@ static void uncSensorProcess(timeUs_t currentTimeUs) if (calculateCRC16_CCITT(buffer, 20) == crc) { uint16_t rpm = buffer[18] << 8 | buffer[17]; uint16_t temp = buffer[14]; + uint16_t throttle = buffer[7]; uint16_t power = buffer[15]; uint16_t voltage = buffer[11] << 8 | buffer[10]; uint16_t current = buffer[9] << 8 | buffer[8]; @@ -928,6 +931,7 @@ static void uncSensorProcess(timeUs_t currentTimeUs) escSensorData[0].age = 0; escSensorData[0].erpm = rpm * 5; + escSensorData[0].throttle = throttle * 5; escSensorData[0].pwm = power * 5; escSensorData[0].voltage = voltage * 100; escSensorData[0].current = current * 100; @@ -1076,6 +1080,7 @@ static void kontronikSensorProcess(timeUs_t currentTimeUs) if (calculateCRC32(buffer, 36) == crc) { uint32_t rpm = buffer[7] << 24 | buffer[6] << 16 | buffer[5] << 8 | buffer[4]; + int16_t throttle = (int8_t)buffer[24]; uint16_t pwm = buffer[23] << 8 | buffer[22]; uint16_t voltage = buffer[9] << 8 | buffer[8]; uint16_t current = buffer[11] << 8 | buffer[10]; @@ -1089,6 +1094,7 @@ static void kontronikSensorProcess(timeUs_t currentTimeUs) escSensorData[0].age = 0; escSensorData[0].erpm = rpm; + escSensorData[0].throttle = (throttle + 100) * 5; escSensorData[0].pwm = pwm * 10; escSensorData[0].voltage = voltage * 10; escSensorData[0].current = current * 100; @@ -1193,6 +1199,7 @@ static void ompSensorProcess(timeUs_t currentTimeUs) // Make sure this is OMP M4 ESC if (buffer[1] == 0x01 && buffer[2] == 0x20 && buffer[11] == 0 && buffer[18] == 0 && buffer[20] == 0) { uint16_t rpm = buffer[8] << 8 | buffer[9]; + uint16_t throttle = buffer[7]; uint16_t pwm = buffer[12]; uint16_t temp = buffer[10]; uint16_t voltage = buffer[3] << 8 | buffer[4]; @@ -1202,6 +1209,7 @@ static void ompSensorProcess(timeUs_t currentTimeUs) escSensorData[0].age = 0; escSensorData[0].erpm = rpm * 10; + escSensorData[0].throttle = throttle * 10; escSensorData[0].pwm = pwm * 10; escSensorData[0].voltage = voltage * 100; escSensorData[0].current = current * 100; @@ -1312,6 +1320,7 @@ static void ztwSensorProcess(timeUs_t currentTimeUs) if (buffer[1] == 0x01 && buffer[2] == 0x20) { uint16_t rpm = buffer[8] << 8 | buffer[9]; uint16_t temp = buffer[10]; + uint16_t throttle = buffer[7]; uint16_t power = buffer[12]; uint16_t voltage = buffer[3] << 8 | buffer[4]; uint16_t current = buffer[5] << 8 | buffer[6]; @@ -1321,6 +1330,7 @@ static void ztwSensorProcess(timeUs_t currentTimeUs) escSensorData[0].age = 0; escSensorData[0].erpm = rpm * 10; + escSensorData[0].throttle = throttle * 10; escSensorData[0].pwm = power * 10; escSensorData[0].voltage = voltage * 100; escSensorData[0].current = current * 100; @@ -1453,6 +1463,7 @@ static void apdSensorProcess(timeUs_t currentTimeUs) if (calculateFletcher16(buffer + 2, 18) == crc) { uint16_t rpm = buffer[13] << 24 | buffer[12] << 16 | buffer[11] << 8 | buffer[10]; uint16_t tadc = buffer[3] << 8 | buffer[2]; + uint16_t throttle = buffer[15] << 8 | buffer[14]; uint16_t power = buffer[17] << 8 | buffer[16]; uint16_t voltage = buffer[1] << 8 | buffer[0]; uint16_t current = buffer[5] << 8 | buffer[4]; @@ -1464,6 +1475,7 @@ static void apdSensorProcess(timeUs_t currentTimeUs) escSensorData[0].age = 0; escSensorData[0].erpm = rpm; + escSensorData[0].throttle = throttle; escSensorData[0].pwm = power; escSensorData[0].voltage = voltage * 10; escSensorData[0].current = current * 80; @@ -1690,6 +1702,7 @@ static uint8_t oygeDecodeTelemetryFrame(void) uint16_t capa = buffer[hl+7] << 8 | buffer[hl+6]; uint16_t erpm = buffer[hl+9] << 8 | buffer[hl+8]; uint8_t pwm = buffer[hl+10]; + uint8_t throttle = buffer[hl+11]; uint16_t voltBEC = buffer[hl+13] << 8 | buffer[hl+12]; uint16_t currBEC = buffer[hl+15] << 8 | buffer[hl+14]; int16_t tempBEC = buffer[hl+16]; @@ -1704,6 +1717,7 @@ static uint8_t oygeDecodeTelemetryFrame(void) escSensorData[0].age = 0; escSensorData[0].erpm = erpm * 10; escSensorData[0].pwm = pwm * 10; + escSensorData[0].throttle = throttle * 10; escSensorData[0].voltage = volt * 10; escSensorData[0].current = curr * 10; escSensorData[0].consumption = capa; diff --git a/src/main/sensors/esc_sensor.h b/src/main/sensors/esc_sensor.h index be87c59b73..7023ae7864 100644 --- a/src/main/sensors/esc_sensor.h +++ b/src/main/sensors/esc_sensor.h @@ -58,6 +58,7 @@ PG_DECLARE(escSensorConfig_t, escSensorConfig); typedef struct { uint8_t age; // Data age uint16_t pwm; // Output duty cycle 0.1% + uint16_t throttle; // Input setpoint 0.1% uint32_t erpm; // eRPM uint32_t voltage; // mV uint32_t current; // mA diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index c585e8cd84..11e6f265d3 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -255,6 +255,9 @@ static int16_t crsfGpsReuse(uint8_t reuse, int16_t value) case CRSF_GPS_REUSE_ESC_PWM: escData = getEscSensorData(ESC_SENSOR_COMBINED); return (escData) ? escData->pwm : 0; + case CRSF_GPS_REUSE_ESC_THROTTLE: + escData = getEscSensorData(ESC_SENSOR_COMBINED); + return (escData) ? escData->throttle : 0; case CRSF_GPS_REUSE_ESC_BEC_VOLTAGE: escData = getEscSensorData(ESC_SENSOR_COMBINED); return (escData) ? escData->bec_voltage : 0; diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h index 4f7ab27a1d..f89c9d513f 100644 --- a/src/main/telemetry/crsf.h +++ b/src/main/telemetry/crsf.h @@ -64,6 +64,7 @@ enum { CRSF_GPS_REUSE_THROTTLE, CRSF_GPS_REUSE_ESC_TEMP, CRSF_GPS_REUSE_ESC_PWM, + CRSF_GPS_REUSE_ESC_THROTTLE, CRSF_GPS_REUSE_ESC_BEC_VOLTAGE, CRSF_GPS_REUSE_ESC_BEC_CURRENT, CRSF_GPS_REUSE_ESC_BEC_TEMP, From a0c64521ae82431b647a55936c4a969a1a458edb Mon Sep 17 00:00:00 2001 From: Rob Gayle Date: Mon, 15 Apr 2024 11:58:45 -0400 Subject: [PATCH 22/24] REUSE - added BEC, BUS, MPU voltage sources to GPS sensor options --- src/main/cli/settings.c | 2 +- src/main/telemetry/crsf.c | 28 +++++++++++++++++----------- src/main/telemetry/crsf.h | 3 +++ 3 files changed, 21 insertions(+), 12 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 2146efb978..7ae5c4d1df 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -493,7 +493,7 @@ const char * const lookupTableCrsfAttReuse[] = { }; const char * const lookupTableCrsfGpsReuse[] = { - "NONE", "HEADSPEED", "THROTTLE", "ESC_TEMP", "ESC_PWM", "ESC_THROTTLE", "ESC_BEC_VOLTAGE", "ESC_BEC_CURRENT", "ESC_BEC_TEMP", "ESC_STATUS", "ESC_STATUS2", "MCU_TEMP", "MCU_LOAD", "SYS_LOAD", "RT_LOAD", + "NONE", "HEADSPEED", "THROTTLE", "ESC_TEMP", "ESC_PWM", "ESC_THROTTLE", "ESC_BEC_VOLTAGE", "ESC_BEC_CURRENT", "ESC_BEC_TEMP", "ESC_STATUS", "ESC_STATUS2", "MCU_TEMP", "MCU_LOAD", "SYS_LOAD", "RT_LOAD", "BEC_VOLTAGE", "BUS_VOLTAGE", "MCU_VOLTAGE" }; const char * const lookupTableCrsfGpsSatsReuse[] = { diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 11e6f265d3..4284e14b9e 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -238,6 +238,17 @@ uint16 Altitude ( meter ­1000m offset ) uint8_t Satellites in use ( counter ) */ +static int getVoltageMeter(voltageMeterId_e id) +{ + voltageMeter_t meter; + + voltageMeterRead(id, &meter); + + // Use ratio 200 in EdgeTx 2.9.3 and 20 in earlier versions + // Max voltage 25.5V + return meter.voltage * 255 / 200; +} + static int16_t crsfGpsReuse(uint8_t reuse, int16_t value) { escSensorData_t *escData; @@ -281,6 +292,12 @@ static int16_t crsfGpsReuse(uint8_t reuse, int16_t value) return getAverageSystemLoad(); case CRSF_GPS_REUSE_RT_LOAD: return getMaxRealTimeLoad(); + case CRSF_GPS_REUSE_BEC_VOLTAGE: + return getVoltageMeter(VOLTAGE_METER_ID_BEC); + case CRSF_GPS_REUSE_BUS_VOLTAGE: + return getVoltageMeter(VOLTAGE_METER_ID_BUS); + case CRSF_GPS_REUSE_MCU_VOLTAGE: + return getVoltageMeter(VOLTAGE_METER_ID_MCU); } return 0; @@ -438,17 +455,6 @@ static int16_t decidegrees2Radians10000(int16_t angle_decidegree) return (int16_t)(RAD * 1000.0f * angle_decidegree); } -static int getVoltageMeter(voltageMeterId_e id) -{ - voltageMeter_t meter; - - voltageMeterRead(id, &meter); - - // Use ratio 200 in EdgeTx 2.9.3 and 20 in earlier versions - // Max voltage 25.5V - return meter.voltage * 255 / 200; -} - static int16_t crsfAttitudeReuse(uint8_t reuse, int attitude) { escSensorData_t *escData; diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h index f89c9d513f..55b6f85729 100644 --- a/src/main/telemetry/crsf.h +++ b/src/main/telemetry/crsf.h @@ -74,6 +74,9 @@ enum { CRSF_GPS_REUSE_MCU_LOAD, CRSF_GPS_REUSE_SYS_LOAD, CRSF_GPS_REUSE_RT_LOAD, + CRSF_GPS_REUSE_BEC_VOLTAGE, + CRSF_GPS_REUSE_BUS_VOLTAGE, + CRSF_GPS_REUSE_MCU_VOLTAGE, }; enum { From d7d437c8e5cde1d14890b6bc803fd18752636084 Mon Sep 17 00:00:00 2001 From: Rob Gayle Date: Sun, 24 Mar 2024 21:02:56 -0400 Subject: [PATCH 23/24] CRSF - reuse - expose additional ESC sources on ATT 16-bit sensors - not enough room on GPS sensors --- src/main/cli/settings.c | 2 +- src/main/telemetry/crsf.c | 18 ++++++++++++++++++ src/main/telemetry/crsf.h | 6 ++++++ 3 files changed, 25 insertions(+), 1 deletion(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 7ae5c4d1df..3d4751074c 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -489,7 +489,7 @@ const char * const lookupTableCrsfFmReuse[] = { }; const char * const lookupTableCrsfAttReuse[] = { - "NONE", "THROTTLE", "ESC_TEMP", "MCU_TEMP", "MCU_LOAD", "SYS_LOAD", "RT_LOAD", "BEC_VOLTAGE", "BUS_VOLTAGE", "MCU_VOLTAGE", + "NONE", "THROTTLE", "ESC_TEMP", "ESC_PWM", "ESC_BEC_VOLTAGE", "ESC_BEC_CURRENT", "ESC_BEC_TEMP", "ESC_STATUS", "ESC_STATUS2", "MCU_TEMP", "MCU_LOAD", "SYS_LOAD", "RT_LOAD", "BEC_VOLTAGE", "BUS_VOLTAGE", "MCU_VOLTAGE", }; const char * const lookupTableCrsfGpsReuse[] = { diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 4284e14b9e..52360d1503 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -467,6 +467,24 @@ static int16_t crsfAttitudeReuse(uint8_t reuse, int attitude) case CRSF_ATT_REUSE_ESC_TEMP: escData = getEscSensorData(ESC_SENSOR_COMBINED); return (escData) ? escData->temperature * 10 : 0; + case CRSF_ATT_REUSE_ESC_PWM: + escData = getEscSensorData(ESC_SENSOR_COMBINED); + return (escData) ? escData->pwm : 0; + case CRSF_ATT_REUSE_ESC_BEC_VOLTAGE: + escData = getEscSensorData(ESC_SENSOR_COMBINED); + return (escData) ? escData->bec_voltage : 0; + case CRSF_ATT_REUSE_ESC_BEC_CURRENT: + escData = getEscSensorData(ESC_SENSOR_COMBINED); + return (escData) ? escData->bec_current : 0; + case CRSF_ATT_REUSE_ESC_BEC_TEMP: + escData = getEscSensorData(ESC_SENSOR_COMBINED); + return (escData) ? escData->temperature2 * 10 : 0; + case CRSF_ATT_REUSE_ESC_STATUS: + escData = getEscSensorData(ESC_SENSOR_COMBINED); + return (escData) ? (escData->status & 0xFFFF) : 0; + case CRSF_ATT_REUSE_ESC_STATUS2: + escData = getEscSensorData(ESC_SENSOR_COMBINED); + return (escData) ? (escData->status >> 16) : 0; case CRSF_ATT_REUSE_MCU_TEMP: return getCoreTemperatureCelsius() * 100; case CRSF_ATT_REUSE_MCU_LOAD: diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h index 55b6f85729..ceaab554ff 100644 --- a/src/main/telemetry/crsf.h +++ b/src/main/telemetry/crsf.h @@ -49,6 +49,12 @@ enum { CRSF_ATT_REUSE_NONE = 0, CRSF_ATT_REUSE_THROTTLE, CRSF_ATT_REUSE_ESC_TEMP, + CRSF_ATT_REUSE_ESC_PWM, + CRSF_ATT_REUSE_ESC_BEC_VOLTAGE, + CRSF_ATT_REUSE_ESC_BEC_CURRENT, + CRSF_ATT_REUSE_ESC_BEC_TEMP, + CRSF_ATT_REUSE_ESC_STATUS, + CRSF_ATT_REUSE_ESC_STATUS2, CRSF_ATT_REUSE_MCU_TEMP, CRSF_ATT_REUSE_MCU_LOAD, CRSF_ATT_REUSE_SYS_LOAD, From a3086bf4a2a8e4470d8f8b28f5399e16591b2197 Mon Sep 17 00:00:00 2001 From: Rob Gayle Date: Sat, 30 Mar 2024 22:01:40 -0400 Subject: [PATCH 24/24] CRSF - reuse - minor code consistency - missing comma --- src/main/cli/settings.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 3d4751074c..d1c6b3175f 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -493,7 +493,7 @@ const char * const lookupTableCrsfAttReuse[] = { }; const char * const lookupTableCrsfGpsReuse[] = { - "NONE", "HEADSPEED", "THROTTLE", "ESC_TEMP", "ESC_PWM", "ESC_THROTTLE", "ESC_BEC_VOLTAGE", "ESC_BEC_CURRENT", "ESC_BEC_TEMP", "ESC_STATUS", "ESC_STATUS2", "MCU_TEMP", "MCU_LOAD", "SYS_LOAD", "RT_LOAD", "BEC_VOLTAGE", "BUS_VOLTAGE", "MCU_VOLTAGE" + "NONE", "HEADSPEED", "THROTTLE", "ESC_TEMP", "ESC_PWM", "ESC_THROTTLE", "ESC_BEC_VOLTAGE", "ESC_BEC_CURRENT", "ESC_BEC_TEMP", "ESC_STATUS", "ESC_STATUS2", "MCU_TEMP", "MCU_LOAD", "SYS_LOAD", "RT_LOAD", "BEC_VOLTAGE", "BUS_VOLTAGE", "MCU_VOLTAGE", }; const char * const lookupTableCrsfGpsSatsReuse[] = {