Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-19 22:35:19 +03:00
Code Issues Wiki Activity

Merge branch 'master' into OSD-Nav-Message-Update

Browse source
This commit is contained in:
breadoven 2020-11-17 21:47:14 +00:00
commit 2d0c92b605
111 changed files with 4498 additions and 1280 deletions
Download patch file Download diff file Expand all files Collapse all files

4
.github/workflows/ci.yml vendored
View file

@ -27,8 +27,8 @@ jobs:
COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }')
echo "::set-env name=BUILD_SUFFIX::${BUILD_SUFFIX}"
echo "::set-env name=BUILD_NAME::inav-${VERSION}-${BUILD_SUFFIX}"
echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
- uses: actions/cache@v1
with:
path: downloads

23
.github/workflows/docs.yml vendored Normal file
View file

@ -0,0 +1,23 @@
name: Make sure docs are updated
on:
push:
paths:
- src/main/fc/settings.yaml
- docs/Settings.md
jobs:
settings_md:
runs-on: ubuntu-18.04
steps:
- uses: actions/checkout@v2
- name: Install dependencies
run: sudo apt-get update && sudo apt-get -y install python3-yaml
- name: Check that Settings.md is up to date
run: |
cp docs/Settings.md{,.ci}
python3 src/utils/update_cli_docs.py -q
if ! diff -q docs/Settings.md{,.ci} >/dev/null; then
echo "::error ::\"docs/Settings.md\" is not up to date, please run \"src/utils/update_cli_docs.py\""
exit 1
fi

13
docs/Cli.md
View file

@ -65,7 +65,8 @@ After restoring it's always a good idea to `dump` or `diff` the settings once ag
| `1wire <esc>` | passthrough 1wire to the specified esc |
| `adjrange` | show/set adjustment ranges settings |
| `aux` | show/set aux settings |
| `beeper` | show/set beeper (buzzer) usage (see docs/Buzzer.md) |
| `beeper` | show/set beeper (buzzer) [usage](Buzzer.md) |
| `bind_rx` | Initiate binding for RX_SPI or SRXL2 receivers |
| `mmix` | design custom motor mixer |
| `smix` | design custom servo mixer |
| `color` | configure colors |
@ -80,18 +81,19 @@ After restoring it's always a good idea to `dump` or `diff` the settings once ag
| `led` | configure leds |
| `map` | mapping of rc channel order |
| `motor` | get/set motor output value |
| `msc` | Enter USB Mass storage mode. See docs/USB_Mass_Storage_(MSC)_mode.md for usage information.|
| `msc` | Enter USB Mass storage mode. See [USB MSC documentation](USB_Mass_Storage_(MSC)_mode.md) for usage information.|
| `play_sound` | index, or none for next |
| `profile` | index (0 to 2) |
| `rxrange` | configure rx channel ranges (end-points) |
| `safehome` | Define safe home locations. See the [safehome documentation](Safehomes.md) for usage information. |
| `save` | save and reboot |
| `serial` | Configure serial ports |
| `serialpassthrough <id> <baud> <mode>`| where `id` is the zero based port index, `baud` is a standard baud rate, and mode is `rx`, `tx`, or both (`rxtx`) |
| `set` | name=value or blank or * for list |
| `status` | show system status |
| `temp_sensor` | list or configure temperature sensor(s). See docs/Temperature sensors.md |
| `wp` | list or configure waypoints. See more in docs/Navigation.md section NAV WP |
| `version` | |
| `temp_sensor` | list or configure temperature sensor(s). See [temperature sensors documentation](Temperature sensors.md) for more information. |
| `wp` | list or configure waypoints. See more in the [navigation documentation](Navigation.md#cli-command-wp-to-manage-waypoints). |
| `version` | Displays version information, |
### serial
@ -145,5 +147,4 @@ For targets that have a flash data chip, typically used for blackbox logs, the f
| `flash_write <address> <data>` | Writes `data` to `address` |
## CLI Variable Reference
See [Settings.md](Settings.md).

4
docs/Controls.md
View file

@ -2,9 +2,9 @@
## Arming
When armed, the aircraft is ready to fly and the motors will spin when throttle is applied. The motors will spin at a slow speed when armed (this feature may be disabled by setting MOTOR_STOP, but for safety reasons, that is not recommended).
When armed, the aircraft is ready to fly and the motors will spin when throttle is applied. With multirotors, the motors will spin at a slow speed when armed (this feature may be disabled by setting MOTOR_STOP, but for safety reasons, that is not recommended).
By default, arming and disarming is done using stick positions. (NOTE: this feature is disabled when using a switch to arm.)
Arming and disarming is done using a switch, set up on the modes page. (NOTE: Stick arming was removed in iNav 2.2)
## Stick Positions

5
docs/Programming Framework.md
View file

@ -116,6 +116,9 @@ IPF can be edited using INAV Configurator user interface, of via CLI
| 28 | STABILIZED_YAW | Yaw PID controller output `[-500:500]` |
| 29 | ACTIVE_WAYPOINT_INDEX | Indexed from `1`. To verify WP is in progress, use `IS_WP` |
| 30 | ACTIVE_WAYPOINT_ACTION | See ACTIVE_WAYPOINT_ACTION paragraph |
| 31 | 3D HOME_DISTANCE | in `meters`, calculated from HOME_DISTANCE and ALTITUDE using Pythagorean theorem |
| 32 | CROSSFIRE LQ | Crossfire Link quality as returned by the CRSF protocol |
| 33 | CROSSFIRE SNR | Crossfire SNR as returned by the CRSF protocol |
##### ACTIVE_WAYPOINT_ACTION
@ -143,6 +146,8 @@ IPF can be edited using INAV Configurator user interface, of via CLI
| 6 | ANGLE | |
| 7 | HORIZON | |
| 8 | AIR | |
| 9 | USER1 | |
| 10 | USER2 | |
### Flags

75
docs/Rx.md
View file

@ -33,6 +33,8 @@ http://www.frsky-rc.com/product/pro.php?pro_id=21
### Spektrum
This section describes the legacy Spektrum satellite capability; the newer SRXL2 protocol is described [later in this document](#srxl2) .
8 channels via serial currently supported.
These receivers are reported working:
@ -47,10 +49,10 @@ As of iNav 1.6, a pseudo RSSI, based on satellite fade count is supported and re
* Bind the satellite receiver using a physical RX; the bind function provided by the flight controller is not sufficient.
* The CLI variable `rssi_channel` is set to channel 9:
````
```
set rssi_channel = 9
````
This pseudo-RSSI should work on all makes of Spektrum satellite RX; it is tested as working on Lemon RX satellites http://www.lemon-rx.com/index.php?route=product/product&path=72&product_id=109 and http://www.lemon-rx.com/index.php?route=product/product&path=72&product_id=135 (recommended).
```
This pseudo-RSSI should work on all makes of Spektrum satellite RX; it is tested as working on [Lemon RX satellites](http://www.lemon-rx.com/index.php?route=product/product&path=72&product_id=109 and http://www.lemon-rx.com/index.php?route=product/product&path=72&product_id=135) (recommended).
### S.BUS
@ -170,6 +172,46 @@ After flash "10ch Timer Mod i6 Updater", it is passible to get RSSI signal on se
It is possible to use IBUS RX and IBUS telemetry on only one port of the hardware UART. More information in Telemetry.md.obj/inav_2.2.2_SPRACINGF3EVO.hex
### SRXL2
SRXL2 is a newer Spektrum protocol that provides a bidirectional link between the FC and the receiver, allowing the user to get FC telemetry data and basic settings on Spektrum Gen 2 airware TX. SRXL2 is supported in inav 2.6 and later. It offers improved performance and features compared to earlier Spektrum RX.
#### Wiring
Signal pin on receiver (labeled "S") must be wired to a **UART TX** pin on the FC. Voltage can be 3.3V (4.0V for SPM4651T) to 8.4V. On some F4 FCs, the TX pin may have a signal inverter (such as for S.Port). Make sure this isn't the case for the pin you intend to use.
#### Configuration
Selection of SXRL2 is provided in the inav 2.6 and later configurators. It is necessary to complete the configuration via the CLI; the following settings are recommended:
```
feature TELEMETRY
feature -RSSI_ADC
map TAER
set receiver_type = SERIAL
set serialrx_provider = SRXL2
set serialrx_inverted = OFF
set srxl2_unit_id = 1
set srxl2_baud_fast = ON
set rssi_source = PROTOCOL
set rssi_channel = 0
```
#### Notes:
* RSSI_ADC is disabled, as this would override the value provided through SRXL2
* `rssi_channel = 0` is required, unlike earlier Spektrum devices (e.g. SPM4649T).
Setting these values differently may have an adverse effects on RSSI readings.
#### CLI Bind Command
This command will put the receiver into bind mode without the need to reboot the FC as it was required with the older `spektrum_sat_bind` command.
```
bind_rx
```
## MultiWii serial protocol (MSP)
Allows you to use MSP commands as the RC input. Only 8 channel support to maintain compatibility with MSP.
@ -215,21 +257,20 @@ To setup spectrum in the GUI:
2. Move to the "Configuration" page and in the upper lefthand corner choose Serial RX as the receiver type.
3. Below that choose the type of serial receiver that you are using. Save and reboot.
Using CLI:
For Serial RX enable `RX_SERIAL` and set the `serialrx_provider` CLI setting as follows.
#### Using CLI:
| Serial RX Provider | Value |
| ------------------ | ----- |
| SPEKTRUM1024 | 0 |
| SPEKTRUM2048 | 1 |
| SBUS | 2 |
| SUMD | 3 |
| SUMH | 4 |
| XBUS_MODE_B | 5 |
| XBUS_MODE_B_RJ01 | 6 |
| SERIALRX_IBUS | 7 |
| SERIALRX_JETIEXBUS | 8 |
| SERIALRX_CRSF | 9 |
For Serial RX set the `receiver_type` and `serialrx_provider` setting as appropriate for your RX.
```
# get rec
receiver_type = SERIAL
Allowed values: NONE, PPM, SERIAL, MSP, SPI, UIB
# get serialrx
serialrx_provider = SBUS
Allowed values: SPEK1024, SPEK2048, SBUS, SUMD, SUMH, XB-B, XB-B-RJ01, IBUS, JETIEXBUS, CRSF, FPORT, SBUS_FAST, FPORT2, SRXL2
```
### PPM/PWM input filtering.

25
docs/Settings.md
View file

@ -61,6 +61,9 @@
| debug_mode | NONE | Defines debug values exposed in debug variables (developer / debugging setting) |
| disarm_kill_switch | ON | Disarms the motors independently of throttle value. Setting to OFF reverts to the old behaviour of disarming only when the throttle is low. Only applies when arming and disarming with an AUX channel. |
| display_force_sw_blink | OFF | OFF = OSD hardware blink / ON = OSD software blink. If OSD warning text/values are invisible, try setting this to ON |
| dji_esc_temp_source | ESC | Re-purpose the ESC temperature field for IMU/BARO temperature |
| dji_use_name_for_messages | ON | Re-purpose the craft name field for messages. Replace craft name with :WTSED for Warnings|Throttle|Speed|Efficiency|Trip distance |
| dji_workarounds | | Enables workarounds for different versions of MSP protocol used |
| dterm_lpf2_hz | 0 | Cutoff frequency for stage 2 D-term low pass filter |
| dterm_lpf2_type | `BIQUAD` | Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. |
| dterm_lpf_hz | 40 | Dterm low pass filter cutoff frequency. Default setting is very conservative and small multirotors should use higher value between 80 and 100Hz. 80 seems like a gold spot for 7-inch builds while 100 should work best with 5-inch machines. If motors are getting too hot, lower the value |
@ -125,6 +128,7 @@
| fw_p_yaw | 6 | Fixed-wing rate stabilisation P-gain for YAW |
| fw_reference_airspeed | 1000 | Reference airspeed. Set this to airspeed at which PIDs were tuned. Usually should be set to cruise airspeed. Also used for coordinated turn calculation if airspeed sensor is not present. |
| fw_tpa_time_constant | 0 | TPA smoothing and delay time constant to reflect non-instant speed/throttle response of the plane. Planes with low thrust/weight ratio generally need higher time constant. Default is zero for compatibility with old setups |
| fw_turn_assist_pitch_gain | 1 | Gain required to keep constant pitch angle during coordinated turns (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter |
| fw_turn_assist_yaw_gain | 1 | Gain required to keep the yaw rate consistent with the turn rate for a coordinated turn (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter |
| gps_auto_baud | ON | Automatic configuration of GPS baudrate(The specified baudrate in configured in ports will be used) when used with UBLOX GPS. When used with NAZA/DJI it will automatic detect GPS baudrate and change to it, ignoring the selected baudrate set in ports |
| gps_auto_config | ON | Enable automatic configuration of UBlox GPS receivers. |
@ -187,9 +191,9 @@
| mag_declination | 0 | Current location magnetic declination in format. For example, -6deg 37min = -637 for Japan. Leading zero in ddd not required. Get your local magnetic declination here: http://magnetic-declination.com/ . Not in use if inav_auto_mag_decl is turned on and you acquire valid GPS fix. |
| mag_hardware | AUTO | Selection of mag hardware. See Wiki Sensor auto detect and hardware failure detection for more info |
| mag_to_use | | Allow to chose between built-in and external compass sensor if they are connected to separate buses. Currently only for REVO target |
| maggain_x | 0 | Magnetometer calibration X gain. If 0, no calibration or calibration failed |
| maggain_y | 0 | Magnetometer calibration Y gain. If 0, no calibration or calibration failed |
| maggain_z | 0 | Magnetometer calibration Z gain. If 0, no calibration or calibration failed |
| maggain_x | 1024 | Magnetometer calibration X gain. If 1024, no calibration or calibration failed |
| maggain_y | 1024 | Magnetometer calibration Y gain. If 1024, no calibration or calibration failed |
| maggain_z | 1024 | Magnetometer calibration Z gain. If 1024, no calibration or calibration failed |
| magzero_x | 0 | Magnetometer calibration X offset. If its 0 none offset has been applied and calibration is failed. |
| magzero_y | 0 | Magnetometer calibration Y offset. If its 0 none offset has been applied and calibration is failed. |
| magzero_z | 0 | Magnetometer calibration Z offset. If its 0 none offset has been applied and calibration is failed. |
@ -201,6 +205,7 @@
| mavlink_ext_status_rate | | |
| mavlink_extra1_rate | | |
| mavlink_extra2_rate | | |
| mavlink_extra3_rate | | |
| mavlink_pos_rate | | |
| mavlink_rc_chan_rate | | |
| max_angle_inclination_pit | 300 | Maximum inclination in level (angle) mode (PITCH axis). 100=10° |
@ -258,6 +263,7 @@
| nav_fw_launch_accel | 1863 | Forward acceleration threshold for bungee launch of throw launch [cm/s/s], 1G = 981 cm/s/s |
| nav_fw_launch_climb_angle | 18 | Climb angle for launch sequence (degrees), is also restrained by global max_angle_inclination_pit |
| nav_fw_launch_detect_time | 40 | Time for which thresholds have to breached to consider launch happened [ms] |
| nav_fw_launch_end_time | 2000 | Time for the transition of throttle and pitch angle, between the launch state and the subsequent flight mode [ms] |
| nav_fw_launch_idle_thr | 1000 | Launch idle throttle - throttle to be set before launch sequence is initiated. If set below minimum throttle it will force motor stop or at idle throttle (depending if the MOTOR_STOP is enabled). If set above minimum throttle it will force throttle to this value (if MOTOR_STOP is enabled it will be handled according to throttle stick position) |
| nav_fw_launch_max_altitude | 0 | Altitude (centimeters) at which LAUNCH mode will be turned off and regular flight mode will take over [0-60000]. |
| nav_fw_launch_max_angle | 45 | Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg] |
@ -271,6 +277,8 @@
| nav_fw_max_thr | 1700 | Maximum throttle for flying wing in GPS assisted modes |
| nav_fw_min_thr | 1200 | Minimum throttle for flying wing in GPS assisted modes |
| nav_fw_pitch2thr | 10 | Amount of throttle applied related to pitch attitude in GPS assisted modes. Throttle = nav_fw_cruise_throttle - (nav_fw_pitch2thr * pitch_angle). (notice that pitch_angle is in degrees and is negative when climbing and positive when diving, and throttle value is constrained between nav_fw_min_thr and nav_fw_max_thr) |
| nav_fw_pitch2thr_smoothing | 0 | How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh. |
| nav_fw_pitch2thr_threshold | 0 | Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees] |
| nav_fw_pos_hdg_d | 0 | D gain of heading trajectory PID controller. (Fixedwing, rovers, boats) |
| nav_fw_pos_hdg_i | 0 | I gain of heading trajectory PID controller. (Fixedwing, rovers, boats) |
| nav_fw_pos_hdg_p | 60 | P gain of heading PID controller. (Fixedwing, rovers, boats) |
@ -316,7 +324,7 @@
| nav_mc_vel_z_i | 50 | I gain of velocity PID controller |
| nav_mc_vel_z_p | 100 | P gain of velocity PID controller |
| nav_min_rth_distance | 500 | Minimum distance from homepoint when RTH full procedure will be activated [cm]. Below this distance, the mode will activate at the current location and the final phase is executed (loiter / land). Above this distance, the full procedure is activated, which may include initial climb and flying directly to the homepoint before entering the loiter / land phase. |
| nav_overrides_motor_stop | ON | Setting to OFF combined with MOTOR_STOP feature will allow user to stop motor when in autonomous modes. On most places this setting is likely to cause a stall. |
| nav_overrides_motor_stop | ALL_NAV | When set OFF the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV |
| nav_position_timeout | 5 | If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) |
| nav_rth_abort_threshold | 50000 | RTH sanity checking feature will notice if distance to home is increasing during RTH and once amount of increase exceeds the threshold defined by this parameter, instead of continuing RTH machine will enter emergency landing, self-level and go down safely. Default is 500m which is safe enough for both multirotor machines and airplanes. [cm] |
| nav_rth_allow_landing | ALWAYS | If set to ON drone will land as a last phase of RTH. |
@ -348,6 +356,7 @@
| osd_camera_uptilt | 0 | Set the camera uptilt for the FPV camera in degres, positive is up, negative is down, relative to the horizontal |
| osd_coordinate_digits | | |
| osd_crosshairs_style | DEFAULT | To set the visual type for the crosshair |
| osd_crsf_lq_format | TYPE1 | To select LQ format |
| osd_current_alarm | 0 | Value above which the OSD current consumption element will start blinking. Measured in full Amperes. |
| osd_dist_alarm | 1000 | Value above which to make the OSD distance from home indicator blink (meters) |
| osd_esc_temp_alarm_max | 900 | Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade) |
@ -372,15 +381,17 @@
| osd_imu_temp_alarm_min | -200 | Temperature under which the IMU temperature OSD element will start blinking (decidegrees centigrade) |
| osd_left_sidebar_scroll | | |
| osd_left_sidebar_scroll_step | 0 | How many units each sidebar step represents. 0 means the default value for the scroll type. |
| osd_link_quality_alarm | 70 | LQ % indicator blinks below this value. For Crossfire use 70%, for Tracer use 50% |
| osd_main_voltage_decimals | 1 | Number of decimals for the battery voltages displayed in the OSD [1-2]. |
| osd_neg_alt_alarm | 5 | Value bellow which (negative altitude) to make the OSD relative altitude indicator blink (meters) |
| osd_neg_alt_alarm | 5 | Value below which (negative altitude) to make the OSD relative altitude indicator blink (meters) |
| osd_plus_code_digits | | |
| osd_right_sidebar_scroll | | |
| osd_right_sidebar_scroll_step | 0 | Same as left_sidebar_scroll_step, but for the right sidebar |
| osd_row_shiftdown | 0 | Number of rows to shift the OSD display (increase if top rows are cut off) |
| osd_rssi_alarm | 20 | Value bellow which to make the OSD RSSI indicator blink |
| osd_rssi_alarm | 20 | Value below which to make the OSD RSSI indicator blink |
| osd_sidebar_horizontal_offset | 0 | Sidebar horizontal offset from default position. Positive values move the sidebars closer to the edges. |
| osd_sidebar_scroll_arrows | | |
| osd_snr_alarm | 4 | Value below which Crossfire SNR Alarm pops-up. (dB) |
| osd_stats_energy_unit | MAH | Unit used for the drawn energy in the OSD stats [MAH/WH] (milliAmpere hour/ Watt hour) |
| osd_temp_label_align | LEFT | Allows to chose between left and right alignment for the OSD temperature sensor labels. Valid values are `LEFT` and `RIGHT` |
| osd_time_alarm | 10 | Value above which to make the OSD flight time indicator blink (minutes) |
@ -448,6 +459,8 @@
| smartport_master_halfduplex | | |
| smartport_master_inverted | | |
| spektrum_sat_bind | 0 | 0 = disabled. Used to bind the spektrum satellite to RX |
| srxl2_baud_fast | | |
| srxl2_unit_id | | |
| stats | OFF | General switch of the statistics recording feature (a.k.a. odometer) |
| stats_total_dist | 0 | Total flight distance [in meters]. The value is updated on every disarm when "stats" are enabled. |
| stats_total_energy | | |

43
docs/development/Building in Windows 10 with Linux Subsystem.md
View file

@ -41,6 +41,18 @@ Mount MS windows C drive and clone iNav
You are ready!
You now have a folder called inav in the root of C drive that you can edit in windows
### If you get a cloning error
On some installations, you may see the following error:
```
Cloning into 'inav'...
error: chmod on /mnt/c/inav/.git/config.lock failed: Operation not permitted
fatal: could not set 'core.filemode' to 'false'
```
You can fix this with by remounting the drive using the following commands
1. `sudo umount /mnt/c`
2. `sudo mount -t drvfs C: /mnt/c -o metadata`
## Building (example):
@ -84,7 +96,9 @@ make[1]: *** [CMakeFiles/Makefile2:33290: src/main/target/MATEKF722SE/CMakeFiles
make: *** [Makefile:13703: MATEKF722SE] Error 2
```
This error can be triggered by a Windows PATHs included in the Linux Subsystem. The solution is to:
This error can be triggered by a Windows PATHs included in the Linux Subsystem. The solution is:
#### For WSL V1 - Flags set as 7 by default
1. Open Windows RegEdit tool
1. Find `HKEY_CURRENT_USER\Software\Microsoft\Windows\CurrentVersion\Lxss\{GUID}\Flags`
@ -92,4 +106,29 @@ This error can be triggered by a Windows PATHs included in the Linux Subsystem.
1. Restart WSL and Windows preferably
1. `cd build`
1. `cmake ..`
1. `make {TARGET}` should be working again
1. `make {TARGET}` should be working again
#### For WSL V2 - Flags set as 0x0000000f (15) by default
1. Open Windows RegEdit tool
1. Find `HKEY_CURRENT_USER\Software\Microsoft\Windows\CurrentVersion\Lxss\{GUID}\Flags`
1. Change `Flags` from `f` to `d`, it is stored as Base Hexadecimal
1. Restart WSL and Windows preferably
1. `cd build`
1. `cmake ..`
1. `make {TARGET}` should be working again
#### Or, for either version
1. In the Linux Subsystem, `cd /etc/`
2. Create a new file with `sudo nano wsl.conf`
3. Enter the following in to the new file:
```
[Interop]
appendWindowsPath=false
```
4. Save the file by holding `Ctrl` and pressing `o`
5. Press `Enter` to confirm the wsl.conf filename.
6. Hit `Ctrl`+`x` to exit nano
7. Restart WSL and Windows preferably
8. `cd build`
9. `cmake ..`
9. `make {TARGET}` should be working again

6
docs/development/IDE - Visual Studio Code with Windows 10.md
View file

@ -104,13 +104,13 @@ Edit `./.vscode/tasks.json` to enable Building with `Ctrl + Shift + B` keyboard
}
,
{
"label": "CMAKE Update",
"label": "Install/Update CMAKE",
"type": "shell",
"command": "cmake ..",
"command": "mkdir -p build && cd build && cmake ..",
"group": "build",
"problemMatcher": [],
"options": {
"cwd": "${workspaceFolder}/build"
"cwd": "${workspaceFolder}"
}
}
]

25
src/main/CMakeLists.txt
View file

@ -118,6 +118,8 @@ main_sources(COMMON_SRC
drivers/barometer/barometer_ms56xx.h
drivers/barometer/barometer_spl06.c
drivers/barometer/barometer_spl06.h
drivers/barometer/barometer_msp.c
drivers/barometer/barometer_msp.h
drivers/buf_writer.c
drivers/buf_writer.h
@ -148,6 +150,8 @@ main_sources(COMMON_SRC
drivers/compass/compass_mpu9250.h
drivers/compass/compass_qmc5883l.c
drivers/compass/compass_qmc5883l.h
drivers/compass/compass_msp.c
drivers/compass/compass_msp.h
drivers/display.c
drivers/display.h
@ -195,12 +199,14 @@ main_sources(COMMON_SRC
drivers/osd.h
drivers/persistent.c
drivers/persistent.h
drivers/pitotmeter_adc.c
drivers/pitotmeter_adc.h
drivers/pitotmeter_ms4525.c
drivers/pitotmeter_ms4525.h
drivers/pitotmeter_virtual.c
drivers/pitotmeter_virtual.h
drivers/pitotmeter/pitotmeter_adc.c
drivers/pitotmeter/pitotmeter_adc.h
drivers/pitotmeter/pitotmeter_ms4525.c
drivers/pitotmeter/pitotmeter_ms4525.h
drivers/pitotmeter/pitotmeter_msp.c
drivers/pitotmeter/pitotmeter_msp.h
drivers/pitotmeter/pitotmeter_virtual.c
drivers/pitotmeter/pitotmeter_virtual.h
drivers/pwm_esc_detect.c
drivers/pwm_esc_detect.h
drivers/pwm_mapping.c
@ -401,6 +407,8 @@ main_sources(COMMON_SRC
rx/sbus_channels.h
rx/spektrum.c
rx/spektrum.h
rx/srxl2.c
rx/srxl2.h
rx/sumd.c
rx/sumd.h
rx/sumh.c
@ -486,6 +494,8 @@ main_sources(COMMON_SRC
io/displayport_msp.h
io/displayport_oled.c
io/displayport_oled.h
io/displayport_srxl.c
io/displayport_srxl.h
io/displayport_hott.c
io/displayport_hott.h
io/flashfs.c
@ -495,6 +505,7 @@ main_sources(COMMON_SRC
io/gps_ublox.c
io/gps_nmea.c
io/gps_naza.c
io/gps_msp.c
io/gps_private.h
io/ledstrip.c
io/ledstrip.h
@ -547,6 +558,8 @@ main_sources(COMMON_SRC
telemetry/crsf.c
telemetry/crsf.h
telemetry/srxl.c
telemetry/srxl.h
telemetry/frsky.c
telemetry/frsky.h
telemetry/frsky_d.c

65
src/main/cms/cms_menu_navigation.c
View file

@ -96,9 +96,9 @@ static const CMS_Menu cmsx_menuRTH = {
.entries = cmsx_menuRTHEntries
};
static const OSD_Entry cmsx_menuFixedWingEntries[] =
static const OSD_Entry cmsx_menuFWCruiseEntries[] =
{
OSD_LABEL_ENTRY("-- FIXED WING --"),
OSD_LABEL_ENTRY("-- CRUISE --"),
OSD_SETTING_ENTRY("CRUISE THROTTLE", SETTING_NAV_FW_CRUISE_THR),
OSD_SETTING_ENTRY("MIN THROTTLE", SETTING_NAV_FW_MIN_THR),
@ -109,11 +109,66 @@ static const OSD_Entry cmsx_menuFixedWingEntries[] =
OSD_SETTING_ENTRY("PITCH TO THR RATIO", SETTING_NAV_FW_PITCH2THR),
OSD_SETTING_ENTRY("LOITER RADIUS", SETTING_NAV_FW_LOITER_RADIUS),
OSD_SETTING_ENTRY("CONTROL SMOOTHNESS", SETTING_NAV_FW_CONTROL_SMOOTHNESS),
OSD_SETTING_ENTRY("PITCH TO THR SMOOTHING", SETTING_NAV_FW_PITCH2THR_SMOOTHING),
OSD_SETTING_ENTRY("PITCH TO THR THRESHOLD", SETTING_NAV_FW_PITCH2THR_THRESHOLD),
OSD_SETTING_ENTRY("MANUAL THR INCREASE", SETTING_NAV_FW_ALLOW_MANUAL_THR_INCREASE),
OSD_BACK_AND_END_ENTRY,
};
static const CMS_Menu cmsx_menuFixedWing = {
static const CMS_Menu cmsx_menuFWCruise = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "MENUNAVFWCRUISE",
.GUARD_type = OME_MENU,
#endif
.onEnter = NULL,
.onExit = NULL,
.onGlobalExit = NULL,
.entries = cmsx_menuFWCruiseEntries
};
static const OSD_Entry cmsx_menuFWLaunchEntries[] =
{
OSD_LABEL_ENTRY("-- AUTOLAUNCH --"),
OSD_SETTING_ENTRY("LAUNCH THR", SETTING_NAV_FW_LAUNCH_THR),
OSD_SETTING_ENTRY("IDLE THROTTLE", SETTING_NAV_FW_LAUNCH_IDLE_THR),
OSD_SETTING_ENTRY("MOTOR SPINUP TIME", SETTING_NAV_FW_LAUNCH_SPINUP_TIME),
OSD_SETTING_ENTRY("TIMEOUT", SETTING_NAV_FW_LAUNCH_TIMEOUT),
OSD_SETTING_ENTRY("END TRANSITION TIME", SETTING_NAV_FW_LAUNCH_END_TIME),
OSD_SETTING_ENTRY("MAX ALTITUDE", SETTING_NAV_FW_LAUNCH_MAX_ALTITUDE),
OSD_SETTING_ENTRY("CLIMB ANGLE", SETTING_NAV_FW_LAUNCH_CLIMB_ANGLE),
OSD_SETTING_ENTRY("MAX BANK ANGLE", SETTING_NAV_FW_LAUNCH_MAX_ANGLE),
OSD_SETTING_ENTRY("MOTOR DELAY", SETTING_NAV_FW_LAUNCH_MOTOR_DELAY),
OSD_SETTING_ENTRY("VELOCITY", SETTING_NAV_FW_LAUNCH_VELOCITY),
OSD_SETTING_ENTRY("ACCELERATION", SETTING_NAV_FW_LAUNCH_ACCEL),
OSD_SETTING_ENTRY("DETECT TIME", SETTING_NAV_FW_LAUNCH_DETECT_TIME),
OSD_BACK_AND_END_ENTRY,
};
static const CMS_Menu cmsx_menuFWLaunch = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "MENUNAVFWLAUNCH",
.GUARD_type = OME_MENU,
#endif
.onEnter = NULL,
.onExit = NULL,
.onGlobalExit = NULL,
.entries = cmsx_menuFWLaunchEntries
};
static const OSD_Entry cmsx_menuFWSettingsEntries[] =
{
OSD_LABEL_ENTRY("-- FIXED WING --"),
OSD_SUBMENU_ENTRY("AUTOLAUNCH", &cmsx_menuFWLaunch),
OSD_SUBMENU_ENTRY("CRUISE", &cmsx_menuFWCruise),
OSD_BACK_AND_END_ENTRY,
};
static const CMS_Menu cmsx_menuFWSettings = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "MENUNAVFW",
.GUARD_type = OME_MENU,
@ -121,7 +176,7 @@ static const CMS_Menu cmsx_menuFixedWing = {
.onEnter = NULL,
.onExit = NULL,
.onGlobalExit = NULL,
.entries = cmsx_menuFixedWingEntries
.entries = cmsx_menuFWSettingsEntries
};
static const OSD_Entry cmsx_menuNavigationEntries[] =
@ -130,7 +185,7 @@ static const OSD_Entry cmsx_menuNavigationEntries[] =
OSD_SUBMENU_ENTRY("BASIC SETTINGS", &cmsx_menuNavSettings),
OSD_SUBMENU_ENTRY("RTH", &cmsx_menuRTH),
OSD_SUBMENU_ENTRY("FIXED WING", &cmsx_menuFixedWing),
OSD_SUBMENU_ENTRY("FIXED WING", &cmsx_menuFWSettings),
OSD_BACK_AND_END_ENTRY,
};

5
src/main/cms/cms_menu_osd.c
View file

@ -143,8 +143,9 @@ static const OSD_Entry menuCrsfRxEntries[]=
{
OSD_LABEL_ENTRY("-- CRSF RX --"),
OSD_SETTING_ENTRY("CRSF SNR LEVEL", SETTING_OSD_SNR_ALARM),
OSD_SETTING_ENTRY("LQ ALARM LEVEL", SETTING_OSD_RSSI_ALARM),
OSD_SETTING_ENTRY("LQ FORMAT", SETTING_OSD_CRSF_LQ_FORMAT),
OSD_SETTING_ENTRY("LQ ALARM LEVEL", SETTING_OSD_LINK_QUALITY_ALARM),
OSD_SETTING_ENTRY("SNR ALARM LEVEL", SETTING_OSD_SNR_ALARM),
OSD_ELEMENT_ENTRY("RX RSSI DBM", OSD_CRSF_RSSI_DBM),
OSD_ELEMENT_ENTRY("RX LQ", OSD_CRSF_LQ),
OSD_ELEMENT_ENTRY("RX SNR ALARM", OSD_CRSF_SNR_DB),

46
src/main/common/filter.c
View file

@ -135,6 +135,17 @@ void biquadFilterInitLPF(biquadFilter_t *filter, uint16_t filterFreq, uint32_t s
biquadFilterInit(filter, filterFreq, samplingIntervalUs, BIQUAD_Q, FILTER_LPF);
}
static void biquadFilterSetupPassthrough(biquadFilter_t *filter)
{
// By default set as passthrough
filter->b0 = 1.0f;
filter->b1 = 0.0f;
filter->b2 = 0.0f;
filter->a1 = 0.0f;
filter->a2 = 0.0f;
}
void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samplingIntervalUs, float Q, biquadFilterType_e filterType)
{
// Check for Nyquist frequency and if it's not possible to initialize filter as requested - set to no filtering at all
@ -148,16 +159,19 @@ void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samp
float b0, b1, b2;
switch (filterType) {
case FILTER_LPF:
b0 = (1 - cs) / 2;
b1 = 1 - cs;
b2 = (1 - cs) / 2;
break;
case FILTER_NOTCH:
b0 = 1;
b1 = -2 * cs;
b2 = 1;
break;
case FILTER_LPF:
b0 = (1 - cs) / 2;
b1 = 1 - cs;
b2 = (1 - cs) / 2;
break;
case FILTER_NOTCH:
b0 = 1;
b1 = -2 * cs;
b2 = 1;
break;
default:
biquadFilterSetupPassthrough(filter);
return;
}
const float a0 = 1 + alpha;
const float a1 = -2 * cs;
@ -169,14 +183,8 @@ void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samp
filter->b2 = b2 / a0;
filter->a1 = a1 / a0;
filter->a2 = a2 / a0;
}
else {
// Not possible to filter frequencies above Nyquist frequency - passthrough
filter->b0 = 1.0f;
filter->b1 = 0.0f;
filter->b2 = 0.0f;
filter->a1 = 0.0f;
filter->a2 = 0.0f;
} else {
biquadFilterSetupPassthrough(filter);
}
// zero initial samples
@ -231,4 +239,4 @@ FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint
filter->x2 = x2;
filter->y1 = y1;
filter->y2 = y2;
}
}

1
src/main/common/log.h
View file

@ -30,6 +30,7 @@ typedef enum {
LOG_TOPIC_POS_ESTIMATOR, // 8, mask = 256
LOG_TOPIC_VTX, // 9, mask = 512
LOG_TOPIC_OSD, // 10, mask = 1024
LOG_TOPIC_GPS, // 11, mask = 2048
LOG_TOPIC_COUNT,
} logTopic_e;

3
src/main/config/parameter_group_ids.h
View file

@ -114,7 +114,8 @@
#define PG_SMARTPORT_MASTER_CONFIG 1024
#define PG_OSD_LAYOUTS_CONFIG 1025
#define PG_SAFE_HOME_CONFIG 1026
#define PG_INAV_END 1026
#define PG_DJI_OSD_CONFIG 1027
#define PG_INAV_END 1027
// OSD configuration (subject to change)
//#define PG_OSD_FONT_CONFIG 2047

102
src/main/drivers/barometer/barometer_msp.c Normal file
View file

@ -0,0 +1,102 @@
/*
* This file is part of INAV Project.
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
* You can obtain one at http://mozilla.org/MPL/2.0/.
*
* Alternatively, the contents of this file may be used under the terms
* of the GNU General Public License Version 3, as described below:
*
* This file is free software: you may copy, redistribute and/or modify
* it under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or (at your
* option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
* Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*/
#include <stdbool.h>
#include <stdint.h>
#include "platform.h"
#if defined(USE_BARO_MSP)
#include "build/build_config.h"
#include "build/debug.h"
#include "common/utils.h"
#include "common/time.h"
#include "drivers/time.h"
#include "drivers/barometer/barometer.h"
#include "drivers/barometer/barometer_msp.h"
#include "msp/msp_protocol_v2_sensor_msg.h"
#define MSP_BARO_TIMEOUT_MS 250 // Less than 4Hz updates is considered a failure
static int32_t mspBaroPressure;
static int32_t mspBaroTemperature;
static timeMs_t mspBaroLastUpdateMs;
static bool mspBaroStartGet(baroDev_t * baro)
{
UNUSED(baro);
return true;
}
static bool mspBaroCalculate(baroDev_t * baro, int32_t *pressure, int32_t *temperature)
{
UNUSED(baro);
if ((millis() - mspBaroLastUpdateMs) > MSP_BARO_TIMEOUT_MS) {
return false;
}
if (pressure)
*pressure = mspBaroPressure;
if (temperature)
*temperature = mspBaroTemperature;
return true;
}
void mspBaroReceiveNewData(uint8_t * bufferPtr)
{
const mspSensorBaroDataMessage_t * pkt = (const mspSensorBaroDataMessage_t *)bufferPtr;
mspBaroPressure = pkt->pressurePa;
mspBaroTemperature = pkt->temp;
mspBaroLastUpdateMs = millis();
}
bool mspBaroDetect(baroDev_t *baro)
{
mspBaroPressure = 101325; // pressure in Pa (0m MSL)
mspBaroTemperature = 2500; // temperature in 0.01 C = 25 deg
// these are dummy as temperature is measured as part of pressure
baro->ut_delay = 10000;
baro->get_ut = mspBaroStartGet;
baro->start_ut = mspBaroStartGet;
// only _up part is executed, and gets both temperature and pressure
baro->up_delay = 10000;
baro->start_up = mspBaroStartGet;
baro->get_up = mspBaroStartGet;
baro->calculate = mspBaroCalculate;
return true;
}
#endif

29
src/main/drivers/barometer/barometer_msp.h Normal file
View file

@ -0,0 +1,29 @@
/*
* This file is part of INAV Project.
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
* You can obtain one at http://mozilla.org/MPL/2.0/.
*
* Alternatively, the contents of this file may be used under the terms
* of the GNU General Public License Version 3, as described below:
*
* This file is free software: you may copy, redistribute and/or modify
* it under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or (at your
* option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
* Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*/
#pragma once
struct baroDev_s;
bool mspBaroDetect(struct baroDev_s *baro);
void mspBaroReceiveNewData(uint8_t * bufferPtr);

96
src/main/drivers/compass/compass_msp.c Normal file
View file

@ -0,0 +1,96 @@
/*
* This file is part of INAV Project.
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
* You can obtain one at http://mozilla.org/MPL/2.0/.
*
* Alternatively, the contents of this file may be used under the terms
* of the GNU General Public License Version 3, as described below:
*
* This file is free software: you may copy, redistribute and/or modify
* it under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or (at your
* option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
* Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*/
#include <stdbool.h>
#include <stdint.h>
#include "platform.h"
#if defined(USE_MAG_MSP)
#include "build/build_config.h"
#include "common/axis.h"
#include "common/utils.h"
#include "common/time.h"
#include "drivers/time.h"
#include "drivers/compass/compass.h"
#include "drivers/compass/compass_msp.h"
#include "sensors/boardalignment.h"
#include "msp/msp_protocol_v2_sensor_msg.h"
#define MSP_MAG_TIMEOUT_MS 250 // Less than 4Hz updates is considered a failure
static int32_t mspMagData[XYZ_AXIS_COUNT];
static timeMs_t mspMagLastUpdateMs;
static bool mspMagInit(magDev_t *magDev)
{
UNUSED(magDev);
mspMagData[X] = 0;
mspMagData[Y] = 0;
mspMagData[Z] = 0;
mspMagLastUpdateMs = 0;
return true;
}
void mspMagReceiveNewData(uint8_t * bufferPtr)
{
const mspSensorCompassDataMessage_t * pkt = (const mspSensorCompassDataMessage_t *)bufferPtr;
mspMagData[X] = pkt->magX;
mspMagData[Y] = pkt->magY;
mspMagData[Z] = pkt->magZ;
applySensorAlignment(mspMagData, mspMagData, CW90_DEG_FLIP);
mspMagLastUpdateMs = millis();
}
static bool mspMagRead(magDev_t *magDev)
{
UNUSED(magDev);
if ((millis() - mspMagLastUpdateMs) > MSP_MAG_TIMEOUT_MS) {
return false;
}
magDev->magADCRaw[X] = mspMagData[X];
magDev->magADCRaw[Y] = mspMagData[Y];
magDev->magADCRaw[Z] = mspMagData[Z];
return true;
}
bool mspMagDetect(magDev_t *mag)
{
mag->init = mspMagInit;
mag->read = mspMagRead;
return true;
}
#endif

28
src/main/drivers/compass/compass_msp.h Normal file
View file

@ -0,0 +1,28 @@
/*
* This file is part of INAV Project.
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
* You can obtain one at http://mozilla.org/MPL/2.0/.
*
* Alternatively, the contents of this file may be used under the terms
* of the GNU General Public License Version 3, as described below:
*
* This file is free software: you may copy, redistribute and/or modify
* it under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or (at your
* option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
* Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*/
#pragma once
bool mspMagDetect(struct magDev_s *mag);
void mspMagReceiveNewData(uint8_t * bufferPtr);

0
src/main/drivers/pitotmeter.h → src/main/drivers/pitotmeter/pitotmeter.h
View file

6
src/main/drivers/pitotmeter_adc.c → src/main/drivers/pitotmeter/pitotmeter_adc.c Executable file → Normal file
View file

@ -24,9 +24,9 @@
#include "common/utils.h"
#include "pitotmeter.h"
#include "pitotmeter_adc.h"
#include "adc.h"
#include "drivers/pitotmeter/pitotmeter.h"
#include "drivers/pitotmeter/pitotmeter_adc.h"
#include "drivers/adc.h"
#if defined(USE_ADC) && defined(USE_PITOT_ADC)

0
src/main/drivers/pitotmeter_adc.h → src/main/drivers/pitotmeter/pitotmeter_adc.h Executable file → Normal file
View file

0
src/main/drivers/pitotmeter_fake.c → src/main/drivers/pitotmeter/pitotmeter_fake.c Executable file → Normal file
View file

0
src/main/drivers/pitotmeter_fake.h → src/main/drivers/pitotmeter/pitotmeter_fake.h Executable file → Normal file
View file

2
src/main/drivers/pitotmeter_ms4525.c → src/main/drivers/pitotmeter/pitotmeter_ms4525.c
View file

@ -24,8 +24,8 @@
#include "common/utils.h"
#include "common/maths.h"
#include "drivers/bus_i2c.h"
#include "drivers/pitotmeter.h"
#include "drivers/time.h"
#include "drivers/pitotmeter/pitotmeter.h"
// MS4525, Standard address 0x28
#define MS4525_ADDR 0x28

0
src/main/drivers/pitotmeter_ms4525.h → src/main/drivers/pitotmeter/pitotmeter_ms4525.h
View file

97
src/main/drivers/pitotmeter/pitotmeter_msp.c Normal file
View file

@ -0,0 +1,97 @@
/*
* This file is part of INAV Project.
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
* You can obtain one at http://mozilla.org/MPL/2.0/.
*
* Alternatively, the contents of this file may be used under the terms
* of the GNU General Public License Version 3, as described below:
*
* This file is free software: you may copy, redistribute and/or modify
* it under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or (at your
* option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
* Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*/
#include <stdbool.h>
#include <stdint.h>
#include "platform.h"
#if defined(USE_PITOT_MSP)
#include "build/build_config.h"
#include "build/debug.h"
#include "common/utils.h"
#include "common/time.h"
#include "drivers/time.h"
#include "drivers/pitotmeter/pitotmeter.h"
#include "drivers/pitotmeter/pitotmeter_msp.h"
#include "msp/msp_protocol_v2_sensor_msg.h"
#define MSP_PITOT_TIMEOUT_MS 500 // Less than 2Hz updates is considered a failure
static int32_t mspPitotPressure;
static int32_t mspPitotTemperature;
static timeMs_t mspPitotLastUpdateMs;
static bool mspPitotStart(pitotDev_t *pitot)
{
UNUSED(pitot);
return true;
}
static bool mspPitotRead(pitotDev_t *pitot)
{
UNUSED(pitot);
return true;
}
static void mspPitotCalculate(pitotDev_t *pitot, float *pressure, float *temperature)
{
UNUSED(pitot);
if (pressure) {
*pressure = mspPitotPressure;
}
if (temperature) {
*temperature = (mspPitotTemperature - 27315) / 100.0f; // Pitot expects temp in Kelvin
}
}
void mspPitotmeterReceiveNewData(uint8_t * bufferPtr)
{
const mspSensorAirspeedDataMessage_t * pkt = (const mspSensorAirspeedDataMessage_t *)bufferPtr;
mspPitotPressure = pkt->diffPressurePa;
mspPitotTemperature = pkt->temp;
mspPitotLastUpdateMs = millis();
}
bool mspPitotmeterDetect(pitotDev_t *pitot)
{
mspPitotPressure = 0;
mspPitotTemperature = 27315; // 0 deg/c
pitot->delay = 10000;
pitot->start = mspPitotStart;
pitot->get = mspPitotRead;
pitot->calculate = mspPitotCalculate;
return true;
}
#endif

29
src/main/drivers/pitotmeter/pitotmeter_msp.h Normal file
View file

@ -0,0 +1,29 @@
/*
* This file is part of INAV Project.
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
* You can obtain one at http://mozilla.org/MPL/2.0/.
*
* Alternatively, the contents of this file may be used under the terms
* of the GNU General Public License Version 3, as described below:
*
* This file is free software: you may copy, redistribute and/or modify
* it under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or (at your
* option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
* Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*/
#pragma once
struct pitotDev_s;
bool mspPitotmeterDetect(struct pitotDev_s *pitot);
void mspPitotmeterReceiveNewData(uint8_t * bufferPtr);

4
src/main/drivers/pitotmeter_virtual.c → src/main/drivers/pitotmeter/pitotmeter_virtual.c
View file

@ -42,8 +42,8 @@
#include "sensors/pitotmeter.h"
#include "pitotmeter.h"
#include "pitotmeter_virtual.h"
#include "drivers/pitotmeter/pitotmeter.h"
#include "drivers/pitotmeter/pitotmeter_virtual.h"
#if defined(USE_WIND_ESTIMATOR) && defined(USE_PITOT_VIRTUAL)

0
src/main/drivers/pitotmeter_virtual.h → src/main/drivers/pitotmeter/pitotmeter_virtual.h
View file

4
src/main/drivers/sdcard/sdmmc_sdio_f7xx.c
View file

@ -485,7 +485,7 @@ static SD_Error_t SD_InitializeCard(void)
SD_GetResponse(SD_Handle.CID);
}
if ((SD_CardType == SD_STD_CAPACITY_V1_1) || (SD_CardType == SD_STD_CAPACITY_V2_0) ||
if ((SD_CardType == SD_STD_CAPACITY_V1_1) || (SD_CardType == SD_STD_CAPACITY_V2_0) ||
(SD_CardType == SD_SECURE_DIGITAL_IO_COMBO) || (SD_CardType == SD_HIGH_CAPACITY)) {
// Send CMD3 SET_REL_ADDR with argument 0
// SD Card publishes its RCA.
@ -1000,7 +1000,7 @@ SD_Error_t SD_GetStatus(void)
}
}
else {
ErrorState = SD_CARD_ERROR;
ErrorState = SD_ERROR;
}
return ErrorState;

33
src/main/fc/cli.c
View file

@ -103,6 +103,7 @@ extern uint8_t __config_end;
#include "rx/rx.h"
#include "rx/spektrum.h"
#include "rx/eleres.h"
#include "rx/srxl2.h"
#include "scheduler/scheduler.h"
@ -2578,6 +2579,35 @@ static void cliEleresBind(char *cmdline)
}
#endif // USE_RX_ELERES
#if defined(USE_RX_SPI) || defined (USE_SERIALRX_SRXL2)
void cliRxBind(char *cmdline){
UNUSED(cmdline);
if (rxConfig()->receiverType == RX_TYPE_SERIAL) {
switch (rxConfig()->serialrx_provider) {
default:
cliPrint("Not supported.");
break;
#if defined(USE_SERIALRX_SRXL2)
case SERIALRX_SRXL2:
srxl2Bind();
cliPrint("Binding SRXL2 receiver...");
break;
#endif
}
}
#if defined(USE_RX_SPI)
else if (rxConfig()->receiverType == RX_TYPE_SPI) {
switch (rxConfig()->rx_spi_protocol) {
default:
cliPrint("Not supported.");
break;
}
}
#endif
}
#endif
static void cliExit(char *cmdline)
{
UNUSED(cmdline);
@ -3465,6 +3495,9 @@ const clicmd_t cmdTable[] = {
CLI_COMMAND_DEF("beeper", "turn on/off beeper", "list\r\n"
"\t<+|->[name]", cliBeeper),
#endif
#if defined(USE_RX_SPI) || defined (USE_SERIALRX_SRXL2)
CLI_COMMAND_DEF("bind_rx", "initiate binding for RX SPI or SRXL2", NULL, cliRxBind),
#endif
#if defined(USE_BOOTLOG)
CLI_COMMAND_DEF("bootlog", "show boot events", NULL, cliBootlog),
#endif

6
src/main/fc/fc_init.c
View file

@ -107,6 +107,7 @@
#include "io/displayport_frsky_osd.h"
#include "io/displayport_msp.h"
#include "io/displayport_max7456.h"
#include "io/displayport_srxl.h"
#include "io/flashfs.h"
#include "io/gps.h"
#include "io/ledstrip.h"
@ -573,6 +574,11 @@ void init(void)
uavInterconnectBusInit();
#endif
#if defined(USE_CMS) && defined(USE_SPEKTRUM_CMS_TELEMETRY) && defined(USE_TELEMETRY_SRXL)
// Register the srxl Textgen telemetry sensor as a displayport device
cmsDisplayPortRegister(displayPortSrxlInit());
#endif
#ifdef USE_GPS
if (feature(FEATURE_GPS)) {
gpsInit();

29
src/main/fc/fc_msp.c
View file

@ -40,8 +40,11 @@
#include "config/parameter_group_ids.h"
#include "drivers/accgyro/accgyro.h"
#include "drivers/bus_i2c.h"
#include "drivers/compass/compass.h"
#include "drivers/compass/compass_msp.h"
#include "drivers/barometer/barometer_msp.h"
#include "drivers/pitotmeter/pitotmeter_msp.h"
#include "drivers/bus_i2c.h"
#include "drivers/display.h"
#include "drivers/flash.h"
#include "drivers/osd.h"
@ -3215,6 +3218,30 @@ static mspResult_e mspProcessSensorCommand(uint16_t cmdMSP, sbuf_t *src)
mspOpflowReceiveNewData(sbufPtr(src));
break;
#endif
#if defined(USE_GPS_PROTO_MSP)
case MSP2_SENSOR_GPS:
mspGPSReceiveNewData(sbufPtr(src));
break;
#endif
#if defined(USE_MAG_MSP)
case MSP2_SENSOR_COMPASS:
mspMagReceiveNewData(sbufPtr(src));
break;
#endif
#if defined(USE_BARO_MSP)
case MSP2_SENSOR_BAROMETER:
mspBaroReceiveNewData(sbufPtr(src));
break;
#endif
#if defined(USE_PITOT_MSP)
case MSP2_SENSOR_AIRSPEED:
mspPitotmeterReceiveNewData(sbufPtr(src));
break;
#endif
}
return MSP_RESULT_NO_REPLY;

18
src/main/fc/rc_adjustments.c
View file

@ -268,12 +268,14 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU
.adjustmentFunction = ADJUSTMENT_VTX_POWER_LEVEL,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
#ifdef USE_INFLIGHT_PROFILE_ADJUSTMENT
}, {
.adjustmentFunction = ADJUSTMENT_PROFILE,
.mode = ADJUSTMENT_MODE_SELECT,
.data = { .selectConfig = { .switchPositions = 3 }}
#endif
.adjustmentFunction = ADJUSTMENT_TPA,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_TPA_BREAKPOINT,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 5 }}
}
};
@ -558,6 +560,12 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
}
break;
#endif
case ADJUSTMENT_TPA:
applyAdjustmentU8(ADJUSTMENT_TPA, &controlRateConfig->throttle.dynPID, delta, 0, CONTROL_RATE_CONFIG_TPA_MAX);
break;
case ADJUSTMENT_TPA_BREAKPOINT:
applyAdjustmentU16(ADJUSTMENT_TPA_BREAKPOINT, &controlRateConfig->throttle.pa_breakpoint, delta, PWM_RANGE_MIN, PWM_RANGE_MAX);
break;
default:
break;
};

5
src/main/fc/rc_adjustments.h
View file

@ -75,9 +75,8 @@ typedef enum {
ADJUSTMENT_VEL_Z_D = 47,
ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE = 48,
ADJUSTMENT_VTX_POWER_LEVEL = 49,
#ifdef USE_INFLIGHT_PROFILE_ADJUSTMENT
ADJUSTMENT_PROFILE = 50,
#endif
ADJUSTMENT_TPA = 50,
ADJUSTMENT_TPA_BREAKPOINT = 51,
ADJUSTMENT_FUNCTION_COUNT // must be last
} adjustmentFunction_e;

101
src/main/fc/settings.yaml
View file

@ -10,22 +10,22 @@ tables:
values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UIB", "BENEWAKE"]
enum: rangefinderType_e
- name: mag_hardware
values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "FAKE"]
values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "FAKE"]
enum: magSensor_e
- name: opflow_hardware
values: ["NONE", "PMW3901", "CXOF", "MSP", "FAKE"]
enum: opticalFlowSensor_e
- name: baro_hardware
values: ["NONE", "AUTO", "BMP085", "MS5611", "BMP280", "MS5607", "LPS25H", "SPL06", "BMP388", "DPS310", "FAKE"]
values: ["NONE", "AUTO", "BMP085", "MS5611", "BMP280", "MS5607", "LPS25H", "SPL06", "BMP388", "DPS310", "MSP", "FAKE"]
enum: baroSensor_e
- name: pitot_hardware
values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE"]
values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE", "MSP"]
enum: pitotSensor_e
- name: receiver_type
values: ["NONE", "PPM", "SERIAL", "MSP", "SPI", "UIB"]
enum: rxReceiverType_e
- name: serial_rx
values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "SUMH", "XB-B", "XB-B-RJ01", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST", "FPORT2"]
values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "SUMH", "XB-B", "XB-B-RJ01", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST", "FPORT2", "SRXL2"]
- name: rx_spi_protocol
values: ["V202_250K", "V202_1M", "SYMA_X", "SYMA_X5C", "CX10", "CX10A", "H8_3D", "INAV", "ELERES"]
enum: rx_spi_protocol_e
@ -44,7 +44,7 @@ tables:
values: ["NONE", "ADC", "ESC"]
enum: voltageSensor_e
- name: gps_provider
values: ["NMEA", "UBLOX", "UNUSED", "NAZA", "UBLOX7", "MTK"]
values: ["NMEA", "UBLOX", "UNUSED", "NAZA", "UBLOX7", "MTK", "MSP"]
enum: gpsProvider_e
- name: gps_sbas_mode
values: ["AUTO", "EGNOS", "WAAS", "MSAS", "GAGAN", "NONE"]
@ -144,6 +144,17 @@ tables:
- name: tristate
enum: tristate_e
values: ["AUTO", "ON", "OFF"]
- name: osd_crsf_lq_format
enum: osd_crsf_lq_format_e
values: ["TYPE1", "TYPE2"]
- name: off_on
values: ["OFF", "ON"]
- name: djiOsdTempSource
values: ["ESC", "IMU", "BARO"]
enum: djiOsdTempSource_e
- name: nav_overrides_motor_stop
enum: navOverridesMotorStop_e
values: ["OFF", "AUTO_ONLY", "ALL_NAV"]
groups:
- name: PG_GYRO_CONFIG
@ -567,6 +578,13 @@ groups:
condition: USE_SPEKTRUM_BIND
min: SPEKTRUM_SAT_BIND_DISABLED
max: SPEKTRUM_SAT_BIND_MAX
- name: srxl2_unit_id
condition: USE_SERIALRX_SRXL2
min: 0
max: 15
- name: srxl2_baud_fast
condition: USE_SERIALRX_SRXL2
table: off_on
- name: rx_min_usec
description: "Defines the shortest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value lower than this value then the channel will be marked as bad and will default to the value of mid_rc."
default_value: "885"
@ -1539,6 +1557,12 @@ groups:
field: fixedWingCoordinatedYawGain
min: 0
max: 2
- name: fw_turn_assist_pitch_gain
description: "Gain required to keep constant pitch angle during coordinated turns (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter"
default_value: "1"
field: fixedWingCoordinatedPitchGain
min: 0
max: 2
- name: fw_iterm_limit_stick_position
description: "Iterm is not allowed to grow when stick position is above threshold. This solves the problem of bounceback or followthrough when full stick deflection is applied on poorely tuned fixed wings. In other words, stabilization is partialy disabled when pilot is actively controlling the aircraft and active when sticks are not touched. `0` mean stick is in center position, `1` means it is fully deflected to either side"
default_value: "0.5"
@ -2083,10 +2107,10 @@ groups:
min: 0
max: 5000
- name: nav_overrides_motor_stop
description: "Setting to OFF combined with MOTOR_STOP feature will allow user to stop motor when in autonomous modes. On most places this setting is likely to cause a stall."
default_value: "ON"
field: general.flags.auto_overrides_motor_stop
type: bool
description: "When set OFF the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV"
default_value: "ALL_NAV"
field: general.flags.nav_overrides_motor_stop
table: nav_overrides_motor_stop
- name: nav_rth_climb_first
description: "If set to ON drone will climb to nav_rth_altitude first and head home afterwards. If set to OFF drone will head home instantly and climb on the way."
default_value: "ON"
@ -2260,6 +2284,18 @@ groups:
field: fw.pitch_to_throttle
min: 0
max: 100
- name: nav_fw_pitch2thr_smoothing
description: "How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh."
default_value: "0"
field: fw.pitch_to_throttle_smooth
min: 0
max: 9
- name: nav_fw_pitch2thr_threshold
description: "Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees]"
default_value: "0"
field: fw.pitch_to_throttle_thresh
min: 0
max: 900
- name: nav_fw_loiter_radius
description: "PosHold radius. 3000 to 7500 is a good value (30-75m) [cm]"
default_value: "5000"
@ -2335,6 +2371,12 @@ groups:
field: fw.launch_motor_spinup_time
min: 0
max: 1000
- name: nav_fw_launch_end_time
description: "Time for the transition of throttle and pitch angle, between the launch state and the subsequent flight mode [ms]"
default_value: "2000"
field: fw.launch_end_time
min: 0
max: 5000
- name: nav_fw_launch_min_time
description: "Allow launch mode to execute at least this time (ms) and ignore stick movements [0-60000]."
default_value: "0"
@ -2624,7 +2666,7 @@ groups:
type: uint8_t
- name: osd_rssi_alarm
description: "Value bellow which to make the OSD RSSI indicator blink"
description: "Value below which to make the OSD RSSI indicator blink"
default_value: "20"
field: rssi_alarm
min: 0
@ -2648,7 +2690,7 @@ groups:
min: 0
max: 50000
- name: osd_neg_alt_alarm
description: "Value bellow which (negative altitude) to make the OSD relative altitude indicator blink (meters)"
description: "Value below which (negative altitude) to make the OSD relative altitude indicator blink (meters)"
default_value: "5"
field: neg_alt_alarm
min: 0
@ -2717,10 +2759,16 @@ groups:
max: 1250
- name: osd_snr_alarm
description: "Value below which Crossfire SNR Alarm pops-up. (dB)"
default_value: "5"
default_value: "4"
field: snr_alarm
min: -12
max: 8
- name: osd_link_quality_alarm
description: "LQ % indicator blinks below this value. For Crossfire use 70%, for Tracer use 50%"
default_value: "70"
field: link_quality_alarm
min: 0
max: 100
- name: osd_temp_label_align
description: "Allows to chose between left and right alignment for the OSD temperature sensor labels. Valid values are `LEFT` and `RIGHT`"
default_value: "LEFT"
@ -2744,6 +2792,12 @@ groups:
field: crosshairs_style
table: osd_crosshairs_style
type: uint8_t
- name: osd_crsf_lq_format
description: "To select LQ format"
default_value: "TYPE1"
field: crsf_lq_format
table: osd_crsf_lq_format
type: uint8_t
- name: osd_horizon_offset
description: "To vertically adjust the whole OSD and AHI and scrolling bars"
default_value: "0"
@ -3096,6 +3150,7 @@ groups:
max: UINT32_MAX
description: "Defines serial debugging log topic. See `docs/development/serial_printf_debugging.md` for usage."
default_value: "0"
- name: PG_ESC_SENSOR_CONFIG
type: escSensorConfig_t
headers: ["sensors/esc_sensor.h"]
@ -3116,3 +3171,25 @@ groups:
- name: smartport_master_inverted
field: inverted
type: bool
- name: PG_DJI_OSD_CONFIG
type: djiOsdConfig_t
headers: ["io/osd_dji_hd.h"]
condition: USE_DJI_HD_OSD
members:
- name: dji_workarounds
description: "Enables workarounds for different versions of MSP protocol used"
field: proto_workarounds
min: 0
max: UINT8_MAX
- name: dji_use_name_for_messages
description: "Re-purpose the craft name field for messages. Replace craft name with :WTSED for Warnings|Throttle|Speed|Efficiency|Trip distance"
default_value: "ON"
field: use_name_for_messages
type: bool
- name: dji_esc_temp_source
description: "Re-purpose the ESC temperature field for IMU/BARO temperature"
default_value: "ESC"
field: esc_temperature_source
table: djiOsdTempSource
type: uint8_t

8
src/main/flight/imu.c
View file

@ -77,7 +77,7 @@ FILE_COMPILE_FOR_SPEED
// http://gentlenav.googlecode.com/files/fastRotations.pdf
#define SPIN_RATE_LIMIT 20
#define MAX_ACC_SQ_NEARNESS 25 // 25% or G^2, accepted acceleration of (0.87 - 1.12G)
#define MAX_ACC_NEARNESS 0.33 // 33% or G error soft-accepted (0.67-1.33G)
#define IMU_CENTRIFUGAL_LPF 1 // Hz
FASTRAM fpVector3_t imuMeasuredAccelBF;
@ -472,16 +472,12 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
static float imuCalculateAccelerometerWeight(const float dT)
{
// If centrifugal test passed - do the usual "nearness" style check
float accMagnitudeSq = 0;
for (int axis = 0; axis < 3; axis++) {
accMagnitudeSq += acc.accADCf[axis] * acc.accADCf[axis];
}
// Magnitude^2 in percent of G^2
const float nearness = ABS(100 - (accMagnitudeSq * 100));
const float accWeight_Nearness = (nearness > MAX_ACC_SQ_NEARNESS) ? 0.0f : 1.0f;
const float accWeight_Nearness = bellCurve(sqrtf(accMagnitudeSq) - 1.0f, MAX_ACC_NEARNESS);
// Experiment: if rotation rate on a FIXED_WING_LEGACY is higher than a threshold - centrifugal force messes up too much and we
// should not use measured accel for AHRS comp

27
src/main/flight/mixer.c
View file

@ -574,13 +574,32 @@ void FAST_CODE mixTable(const float dT)
motorStatus_e getMotorStatus(void)
{
if (failsafeRequiresMotorStop() || (!failsafeIsActive() && STATE(NAV_MOTOR_STOP_OR_IDLE))) {
if (failsafeRequiresMotorStop()) {
return MOTOR_STOPPED_AUTO;
}
if (calculateThrottleStatus(feature(FEATURE_REVERSIBLE_MOTORS) ? THROTTLE_STATUS_TYPE_COMMAND : THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW) {
if ((STATE(FIXED_WING_LEGACY) || !STATE(AIRMODE_ACTIVE)) && (!(navigationIsFlyingAutonomousMode() && navConfig()->general.flags.auto_overrides_motor_stop)) && (!failsafeIsActive())) {
return MOTOR_STOPPED_USER;
if (!failsafeIsActive() && STATE(NAV_MOTOR_STOP_OR_IDLE)) {
return MOTOR_STOPPED_AUTO;
}
const bool fixedWingOrAirmodeNotActive = STATE(FIXED_WING_LEGACY) || !STATE(AIRMODE_ACTIVE);
const bool throttleStickLow =
(calculateThrottleStatus(feature(FEATURE_REVERSIBLE_MOTORS) ? THROTTLE_STATUS_TYPE_COMMAND : THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW);
if (throttleStickLow && fixedWingOrAirmodeNotActive && !failsafeIsActive()) {
// If user is holding stick low, we are not in failsafe and either on a plane or on a quad with inactive
// airmode - we need to check if we are allowing navigation to override MOTOR_STOP
switch (navConfig()->general.flags.nav_overrides_motor_stop) {
case NOMS_ALL_NAV:
return navigationInAutomaticThrottleMode() ? MOTOR_RUNNING : MOTOR_STOPPED_USER;
case NOMS_AUTO_ONLY:
return navigationIsFlyingAutonomousMode() ? MOTOR_RUNNING : MOTOR_STOPPED_USER;
case NOMS_OFF:
default:
return MOTOR_STOPPED_USER;
}
}

5
src/main/flight/pid.c
View file

@ -154,7 +154,7 @@ static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn;
static EXTENDED_FASTRAM filterApplyFnPtr dTermLpf2FilterApplyFn;
static EXTENDED_FASTRAM bool levelingEnabled = false;
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 15);
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 0);
PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.bank_mc = {
@ -256,6 +256,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.fixedWingItermThrowLimit = FW_ITERM_THROW_LIMIT_DEFAULT,
.fixedWingReferenceAirspeed = 1000,
.fixedWingCoordinatedYawGain = 1.0f,
.fixedWingCoordinatedPitchGain = 1.0f,
.fixedWingItermLimitOnStickPosition = 0.5f,
.loiter_direction = NAV_LOITER_RIGHT,
@ -889,7 +890,7 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState)
// Add in roll and pitch
pidState[ROLL].rateTarget = constrainf(pidState[ROLL].rateTarget + targetRates.x, -currentControlRateProfile->stabilized.rates[ROLL] * 10.0f, currentControlRateProfile->stabilized.rates[ROLL] * 10.0f);
pidState[PITCH].rateTarget = constrainf(pidState[PITCH].rateTarget + targetRates.y, -currentControlRateProfile->stabilized.rates[PITCH] * 10.0f, currentControlRateProfile->stabilized.rates[PITCH] * 10.0f);
pidState[PITCH].rateTarget = constrainf(pidState[PITCH].rateTarget + targetRates.y * pidProfile()->fixedWingCoordinatedPitchGain, -currentControlRateProfile->stabilized.rates[PITCH] * 10.0f, currentControlRateProfile->stabilized.rates[PITCH] * 10.0f);
// Replace YAW on quads - add it in on airplanes
if (STATE(AIRPLANE)) {

1
src/main/flight/pid.h
View file

@ -126,6 +126,7 @@ typedef struct pidProfile_s {
uint16_t fixedWingItermThrowLimit;
float fixedWingReferenceAirspeed; // Reference tuning airspeed for the airplane - the speed for which PID gains are tuned
float fixedWingCoordinatedYawGain; // This is the gain of the yaw rate required to keep the yaw rate consistent with the turn rate for a coordinated turn.
float fixedWingCoordinatedPitchGain; // This is the gain of the pitch rate to keep the pitch angle constant during coordinated turns.
float fixedWingItermLimitOnStickPosition; //Do not allow Iterm to grow when stick position is above this point
uint8_t loiter_direction; // Direction of loitering center point on right wing (clockwise - as before), or center point on left wing (counterclockwise)

2
src/main/flight/rth_estimator.c
View file

@ -77,7 +77,7 @@ static int8_t RTHInitialAltitudeChangePitchAngle(float altitudeChange) {
// pitch in degrees
// output in Watt
static float estimatePitchPower(float pitch) {
int16_t altitudeChangeThrottle = fixedWingPitchToThrottleCorrection(DEGREES_TO_DECIDEGREES(pitch));
int16_t altitudeChangeThrottle = (int16_t)pitch * navConfig()->fw.pitch_to_throttle;
altitudeChangeThrottle = constrain(altitudeChangeThrottle, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle);
const float altitudeChangeThrToCruiseThrRatio = (float)(altitudeChangeThrottle - getThrottleIdleValue()) / (navConfig()->fw.cruise_throttle - getThrottleIdleValue());
return (float)heatLossesCompensatedPower(batteryMetersConfig()->idle_power + batteryMetersConfig()->cruise_power * altitudeChangeThrToCruiseThrRatio) / 100;

4
src/main/io/asyncfatfs/asyncfatfs.c
View file

@ -1694,6 +1694,8 @@ static afatfsOperationStatus_e afatfs_appendSuperclusterContinue(afatfsFile_t *f
// Update the fileSize/firstCluster in the directory entry for the file
status = afatfs_saveDirectoryEntry(file, AFATFS_SAVE_DIRECTORY_NORMAL);
break;
default:
status = AFATFS_OPERATION_FAILURE;
}
if ((status == AFATFS_OPERATION_FAILURE || status == AFATFS_OPERATION_SUCCESS) && file->operation.operation == AFATFS_FILE_OPERATION_APPEND_SUPERCLUSTER) {
@ -2487,6 +2489,8 @@ static afatfsOperationStatus_e afatfs_ftruncateContinue(afatfsFilePtr_t file, bo
return AFATFS_OPERATION_SUCCESS;
break;
default:
status = AFATFS_OPERATION_FAILURE;
}
if (status == AFATFS_OPERATION_FAILURE && file->operation.operation == AFATFS_FILE_OPERATION_TRUNCATE) {

53
src/main/io/beeper.c
View file

@ -121,6 +121,10 @@ static const uint8_t beep_runtimeCalibrationDone[] = {
static const uint8_t beep_launchModeBeep[] = {
5, 5, 5, 100, BEEPER_COMMAND_STOP
};
// Two short beeps, then one shorter beep. Beeps to show the throttle needs to be raised. Will repeat every second while the throttle is low.
static const uint8_t beep_launchModeLowThrottleBeep[] = {
5, 5, 5, 5, 3, 100, BEEPER_COMMAND_STOP
};
// short beeps
static const uint8_t beep_hardwareFailure[] = {
10, 10, BEEPER_COMMAND_STOP
@ -164,31 +168,32 @@ typedef struct beeperTableEntry_s {
#define BEEPER_ENTRY(a,b,c,d) a,b,c,d
/*static*/ const beeperTableEntry_t beeperTable[] = {
{ BEEPER_ENTRY(BEEPER_RUNTIME_CALIBRATION_DONE, 0, beep_runtimeCalibrationDone, "RUNTIME_CALIBRATION") },
{ BEEPER_ENTRY(BEEPER_HARDWARE_FAILURE , 1, beep_hardwareFailure, "HW_FAILURE") },
{ BEEPER_ENTRY(BEEPER_RX_LOST, 2, beep_txLostBeep, "RX_LOST") },
{ BEEPER_ENTRY(BEEPER_RX_LOST_LANDING, 3, beep_sos, "RX_LOST_LANDING") },
{ BEEPER_ENTRY(BEEPER_DISARMING, 4, beep_disarmBeep, "DISARMING") },
{ BEEPER_ENTRY(BEEPER_ARMING, 5, beep_armingBeep, "ARMING") },
{ BEEPER_ENTRY(BEEPER_ARMING_GPS_FIX, 6, beep_armedGpsFix, "ARMING_GPS_FIX") },
{ BEEPER_ENTRY(BEEPER_BAT_CRIT_LOW, 7, beep_critBatteryBeep, "BAT_CRIT_LOW") },
{ BEEPER_ENTRY(BEEPER_BAT_LOW, 8, beep_lowBatteryBeep, "BAT_LOW") },
{ BEEPER_ENTRY(BEEPER_GPS_STATUS, 9, beep_multiBeeps, "GPS_STATUS") },
{ BEEPER_ENTRY(BEEPER_RX_SET, 10, beep_shortBeep, "RX_SET") },
{ BEEPER_ENTRY(BEEPER_ACTION_SUCCESS, 11, beep_2shortBeeps, "ACTION_SUCCESS") },
{ BEEPER_ENTRY(BEEPER_ACTION_FAIL, 12, beep_2longerBeeps, "ACTION_FAIL") },
{ BEEPER_ENTRY(BEEPER_READY_BEEP, 13, beep_readyBeep, "READY_BEEP") },
{ BEEPER_ENTRY(BEEPER_MULTI_BEEPS, 14, beep_multiBeeps, "MULTI_BEEPS") }, // FIXME having this listed makes no sense since the beep array will not be initialised.
{ BEEPER_ENTRY(BEEPER_DISARM_REPEAT, 15, beep_disarmRepeatBeep, "DISARM_REPEAT") },
{ BEEPER_ENTRY(BEEPER_ARMED, 16, beep_armedBeep, "ARMED") },
{ BEEPER_ENTRY(BEEPER_SYSTEM_INIT, 17, NULL, "SYSTEM_INIT") },
{ BEEPER_ENTRY(BEEPER_USB, 18, NULL, "ON_USB") },
{ BEEPER_ENTRY(BEEPER_LAUNCH_MODE_ENABLED, 19, beep_launchModeBeep, "LAUNCH_MODE") },
{ BEEPER_ENTRY(BEEPER_CAM_CONNECTION_OPEN, 20, beep_camOpenBeep, "CAM_CONNECTION_OPEN") },
{ BEEPER_ENTRY(BEEPER_CAM_CONNECTION_CLOSE, 21, beep_camCloseBeep, "CAM_CONNECTION_CLOSED") },
{ BEEPER_ENTRY(BEEPER_RUNTIME_CALIBRATION_DONE, 0, beep_runtimeCalibrationDone, "RUNTIME_CALIBRATION") },
{ BEEPER_ENTRY(BEEPER_HARDWARE_FAILURE , 1, beep_hardwareFailure, "HW_FAILURE") },
{ BEEPER_ENTRY(BEEPER_RX_LOST, 2, beep_txLostBeep, "RX_LOST") },
{ BEEPER_ENTRY(BEEPER_RX_LOST_LANDING, 3, beep_sos, "RX_LOST_LANDING") },
{ BEEPER_ENTRY(BEEPER_DISARMING, 4, beep_disarmBeep, "DISARMING") },
{ BEEPER_ENTRY(BEEPER_ARMING, 5, beep_armingBeep, "ARMING") },
{ BEEPER_ENTRY(BEEPER_ARMING_GPS_FIX, 6, beep_armedGpsFix, "ARMING_GPS_FIX") },
{ BEEPER_ENTRY(BEEPER_BAT_CRIT_LOW, 7, beep_critBatteryBeep, "BAT_CRIT_LOW") },
{ BEEPER_ENTRY(BEEPER_BAT_LOW, 8, beep_lowBatteryBeep, "BAT_LOW") },
{ BEEPER_ENTRY(BEEPER_GPS_STATUS, 9, beep_multiBeeps, "GPS_STATUS") },
{ BEEPER_ENTRY(BEEPER_RX_SET, 10, beep_shortBeep, "RX_SET") },
{ BEEPER_ENTRY(BEEPER_ACTION_SUCCESS, 11, beep_2shortBeeps, "ACTION_SUCCESS") },
{ BEEPER_ENTRY(BEEPER_ACTION_FAIL, 12, beep_2longerBeeps, "ACTION_FAIL") },
{ BEEPER_ENTRY(BEEPER_READY_BEEP, 13, beep_readyBeep, "READY_BEEP") },
{ BEEPER_ENTRY(BEEPER_MULTI_BEEPS, 14, beep_multiBeeps, "MULTI_BEEPS") }, // FIXME having this listed makes no sense since the beep array will not be initialised.
{ BEEPER_ENTRY(BEEPER_DISARM_REPEAT, 15, beep_disarmRepeatBeep, "DISARM_REPEAT") },
{ BEEPER_ENTRY(BEEPER_ARMED, 16, beep_armedBeep, "ARMED") },
{ BEEPER_ENTRY(BEEPER_SYSTEM_INIT, 17, NULL, "SYSTEM_INIT") },
{ BEEPER_ENTRY(BEEPER_USB, 18, NULL, "ON_USB") },
{ BEEPER_ENTRY(BEEPER_LAUNCH_MODE_ENABLED, 19, beep_launchModeBeep, "LAUNCH_MODE") },
{ BEEPER_ENTRY(BEEPER_LAUNCH_MODE_LOW_THROTTLE, 20, beep_launchModeLowThrottleBeep, "LAUNCH_MODE_LOW_THROTTLE") },
{ BEEPER_ENTRY(BEEPER_CAM_CONNECTION_OPEN, 21, beep_camOpenBeep, "CAM_CONNECTION_OPEN") },
{ BEEPER_ENTRY(BEEPER_CAM_CONNECTION_CLOSE, 22, beep_camCloseBeep, "CAM_CONNECTION_CLOSED") },
{ BEEPER_ENTRY(BEEPER_ALL, 22, NULL, "ALL") },
{ BEEPER_ENTRY(BEEPER_PREFERENCE, 23, NULL, "PREFERED") },
{ BEEPER_ENTRY(BEEPER_ALL, 23, NULL, "ALL") },
{ BEEPER_ENTRY(BEEPER_PREFERENCE, 24, NULL, "PREFERED") },
};
static const beeperTableEntry_t *currentBeeperEntry = NULL;

49
src/main/io/beeper.h
View file

@ -21,33 +21,34 @@
typedef enum {
// IMPORTANT: these are in priority order, 0 = Highest
BEEPER_SILENCE = 0, // Silence, see beeperSilence()
BEEPER_SILENCE = 0, // Silence, see beeperSilence()
BEEPER_RUNTIME_CALIBRATION_DONE,
BEEPER_HARDWARE_FAILURE, // HW failure
BEEPER_RX_LOST, // Beeps when TX is turned off or signal lost (repeat until TX is okay)
BEEPER_RX_LOST_LANDING, // Beeps SOS when armed and TX is turned off or signal lost (autolanding/autodisarm)
BEEPER_DISARMING, // Beep when disarming the board
BEEPER_ARMING, // Beep when arming the board
BEEPER_ARMING_GPS_FIX, // Beep a special tone when arming the board and GPS has fix
BEEPER_BAT_CRIT_LOW, // Longer warning beeps when battery is critically low (repeats)
BEEPER_BAT_LOW, // Warning beeps when battery is getting low (repeats)
BEEPER_GPS_STATUS, // FIXME **** Disable beeper when connected to USB ****
BEEPER_RX_SET, // Beeps when aux channel is set for beep or beep sequence how many satellites has found if GPS enabled
BEEPER_ACTION_SUCCESS, // Action success (various actions)
BEEPER_ACTION_FAIL, // Action fail (varions actions)
BEEPER_READY_BEEP, // Ring a tone when GPS is locked and ready
BEEPER_MULTI_BEEPS, // Internal value used by 'beeperConfirmationBeeps()'.
BEEPER_DISARM_REPEAT, // Beeps sounded while stick held in disarm position
BEEPER_ARMED, // Warning beeps when board is armed (repeats until board is disarmed or throttle is increased)
BEEPER_SYSTEM_INIT, // Initialisation beeps when board is powered on
BEEPER_USB, // Some boards have beeper powered USB connected
BEEPER_LAUNCH_MODE_ENABLED, // Fixed-wing launch mode enabled
BEEPER_CAM_CONNECTION_OPEN, // When the 5 key simulation stated
BEEPER_CAM_CONNECTION_CLOSE, // When the 5 key simulation stop
BEEPER_HARDWARE_FAILURE, // HW failure
BEEPER_RX_LOST, // Beeps when TX is turned off or signal lost (repeat until TX is okay)
BEEPER_RX_LOST_LANDING, // Beeps SOS when armed and TX is turned off or signal lost (autolanding/autodisarm)
BEEPER_DISARMING, // Beep when disarming the board
BEEPER_ARMING, // Beep when arming the board
BEEPER_ARMING_GPS_FIX, // Beep a special tone when arming the board and GPS has fix
BEEPER_BAT_CRIT_LOW, // Longer warning beeps when battery is critically low (repeats)
BEEPER_BAT_LOW, // Warning beeps when battery is getting low (repeats)
BEEPER_GPS_STATUS, // FIXME **** Disable beeper when connected to USB ****
BEEPER_RX_SET, // Beeps when aux channel is set for beep or beep sequence how many satellites has found if GPS enabled
BEEPER_ACTION_SUCCESS, // Action success (various actions)
BEEPER_ACTION_FAIL, // Action fail (varions actions)
BEEPER_READY_BEEP, // Ring a tone when GPS is locked and ready
BEEPER_MULTI_BEEPS, // Internal value used by 'beeperConfirmationBeeps()'.
BEEPER_DISARM_REPEAT, // Beeps sounded while stick held in disarm position
BEEPER_ARMED, // Warning beeps when board is armed (repeats until board is disarmed or throttle is increased)
BEEPER_SYSTEM_INIT, // Initialisation beeps when board is powered on
BEEPER_USB, // Some boards have beeper powered USB connected
BEEPER_LAUNCH_MODE_ENABLED, // Fixed-wing launch mode enabled
BEEPER_LAUNCH_MODE_LOW_THROTTLE, // Fixed-wing launch mode enabled, but throttle is low
BEEPER_CAM_CONNECTION_OPEN, // When the 5 key simulation stated
BEEPER_CAM_CONNECTION_CLOSE, // When the 5 key simulation stop
BEEPER_ALL, // Turn ON or OFF all beeper conditions
BEEPER_PREFERENCE, // Save preferred beeper configuration
BEEPER_ALL, // Turn ON or OFF all beeper conditions
BEEPER_PREFERENCE, // Save preferred beeper configuration
// BEEPER_ALL and BEEPER_PREFERENCE must remain at the bottom of this enum
} beeperMode_e;

152
src/main/io/displayport_srxl.c Normal file
View file

@ -0,0 +1,152 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include "platform.h"
#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS) && defined(USE_TELEMETRY_SRXL)
#include "common/utils.h"
#include "drivers/display.h"
#include "cms/cms.h"
#include "telemetry/srxl.h"
displayPort_t srxlDisplayPort;
static int srxlDrawScreen(displayPort_t *displayPort)
{
UNUSED(displayPort);
return 0;
}
static int srxlScreenSize(const displayPort_t *displayPort)
{
return displayPort->rows * displayPort->cols;
}
static int srxlWriteChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint16_t c, textAttributes_t attr)
{
return (spektrumTmTextGenPutChar(col, row, c));
UNUSED(displayPort);
UNUSED(attr);
}
static int srxlWriteString(displayPort_t *displayPort, uint8_t col, uint8_t row, const char *s, textAttributes_t attr)
{
while (*s) {
srxlWriteChar(displayPort, col++, row, *(s++), attr);
}
return 0;
}
static int srxlClearScreen(displayPort_t *displayPort)
{
textAttributes_t attr = 0;
for (int row = 0; row < SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS; row++) {
for (int col= 0; col < SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS; col++) {
srxlWriteChar(displayPort, col, row, ' ', attr);
}
}
srxlWriteString(displayPort, 1, 0, "INAV", attr);
if (displayPort->grabCount == 0) {
srxlWriteString(displayPort, 0, 2, CMS_STARTUP_HELP_TEXT1, attr);
srxlWriteString(displayPort, 2, 3, CMS_STARTUP_HELP_TEXT2, attr);
srxlWriteString(displayPort, 2, 4, CMS_STARTUP_HELP_TEXT3, attr);
}
return 0;
}
static bool srxlIsTransferInProgress(const displayPort_t *displayPort)
{
UNUSED(displayPort);
return false;
}
/*
static bool srxlIsSynced(const displayPort_t *displayPort)
{
UNUSED(displayPort);
return true;
}
*/
static int srxlHeartbeat(displayPort_t *displayPort)
{
UNUSED(displayPort);
return 0;
}
static void srxlResync(displayPort_t *displayPort)
{
UNUSED(displayPort);
}
static uint32_t srxlTxBytesFree(const displayPort_t *displayPort)
{
UNUSED(displayPort);
return UINT32_MAX;
}
static int srxlGrab(displayPort_t *displayPort)
{
return displayPort->grabCount = 1;
}
static int srxlRelease(displayPort_t *displayPort)
{
int cnt = displayPort->grabCount = 0;
srxlClearScreen(displayPort);
return cnt;
}
static const displayPortVTable_t srxlVTable = {
.grab = srxlGrab,
.release = srxlRelease,
.clearScreen = srxlClearScreen,
.drawScreen = srxlDrawScreen,
.screenSize = srxlScreenSize,
.writeString = srxlWriteString,
.writeChar = srxlWriteChar,
.isTransferInProgress = srxlIsTransferInProgress,
.heartbeat = srxlHeartbeat,
.resync = srxlResync,
//.isSynced = srxlIsSynced,
.txBytesFree = srxlTxBytesFree,
//.layerSupported = NULL,
//.layerSelect = NULL,
//.layerCopy = NULL,
};
displayPort_t *displayPortSrxlInit(void)
{
srxlDisplayPort.device = NULL;
displayInit(&srxlDisplayPort, &srxlVTable);
srxlDisplayPort.rows = SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS;
srxlDisplayPort.cols = SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS;
return &srxlDisplayPort;
}
#endif

25
src/main/io/displayport_srxl.h Normal file
View file

@ -0,0 +1,25 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
displayPort_t *displayPortSrxlInit(void);
extern displayPort_t srxlDisplayPort;

37
src/main/io/gps.c
View file

@ -60,6 +60,7 @@
#include "fc/runtime_config.h"
typedef struct {
bool isDriverBased;
portMode_t portMode; // Port mode RX/TX (only for serial based)
bool hasCompass; // Has a compass (NAZA)
void (*restart)(void); // Restart protocol driver thread
@ -77,42 +78,48 @@ baudRate_e gpsToSerialBaudRate[GPS_BAUDRATE_COUNT] = { BAUD_115200, BAUD_57600,
static gpsProviderDescriptor_t gpsProviders[GPS_PROVIDER_COUNT] = {
/* NMEA GPS */
#ifdef USE_GPS_PROTO_NMEA
{ MODE_RX, false, &gpsRestartNMEA_MTK, &gpsHandleNMEA },
{ false, MODE_RX, false, &gpsRestartNMEA_MTK, &gpsHandleNMEA },
#else
{ 0, false, NULL, NULL },
{ false, 0, false, NULL, NULL },
#endif
/* UBLOX binary */
#ifdef USE_GPS_PROTO_UBLOX
{ MODE_RXTX, false, &gpsRestartUBLOX, &gpsHandleUBLOX },
{ false, MODE_RXTX, false, &gpsRestartUBLOX, &gpsHandleUBLOX },
#else
{ 0, false, NULL, NULL },
{ false, 0, false, NULL, NULL },
#endif
/* Stub */
{ 0, false, NULL, NULL },
{ false, 0, false, NULL, NULL },
/* NAZA GPS module */
#ifdef USE_GPS_PROTO_NAZA
{ MODE_RX, true, &gpsRestartNAZA, &gpsHandleNAZA },
{ false, MODE_RX, true, &gpsRestartNAZA, &gpsHandleNAZA },
#else
{ 0, false, NULL, NULL },
{ false, 0, false, NULL, NULL },
#endif
/* UBLOX7PLUS binary */
#ifdef USE_GPS_PROTO_UBLOX
{ MODE_RXTX, false, &gpsRestartUBLOX, &gpsHandleUBLOX },
{ false, MODE_RXTX, false, &gpsRestartUBLOX, &gpsHandleUBLOX },
#else
{ 0, false, NULL, NULL },
{ false, 0, false, NULL, NULL },
#endif
/* MTK GPS */
#ifdef USE_GPS_PROTO_MTK
{ MODE_RXTX, false, &gpsRestartNMEA_MTK, &gpsHandleMTK },
{ false, MODE_RXTX, false, &gpsRestartNMEA_MTK, &gpsHandleMTK },
#else
{ 0, false, NULL, NULL },
{ false, 0, false, NULL, NULL },
#endif
/* MSP GPS */
#ifdef USE_GPS_PROTO_MSP
{ true, 0, false, &gpsRestartMSP, &gpsHandleMSP },
#else
{ false, 0, false, NULL, NULL },
#endif
};
PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0);
@ -135,7 +142,7 @@ void gpsSetState(gpsState_e state)
static void gpsUpdateTime(void)
{
if (!rtcHasTime() && gpsSol.flags.validTime) {
if (!rtcHasTime() && gpsSol.flags.validTime && gpsSol.time.year != 0) {
rtcSetDateTime(&gpsSol.time);
}
}
@ -220,6 +227,12 @@ void gpsInit(void)
return;
}
// Shortcut for driver-based GPS (MSP)
if (gpsProviders[gpsState.gpsConfig->provider].isDriverBased) {
gpsSetState(GPS_INITIALIZING);
return;
}
serialPortConfig_t * gpsPortConfig = findSerialPortConfig(FUNCTION_GPS);
if (!gpsPortConfig) {
featureClear(FEATURE_GPS);

2
src/main/io/gps.h
View file

@ -38,6 +38,7 @@ typedef enum {
GPS_NAZA,
GPS_UBLOX7PLUS,
GPS_MTK,
GPS_MSP,
GPS_PROVIDER_COUNT
} gpsProvider_e;
@ -164,3 +165,4 @@ bool isGPSHealthy(void);
bool isGPSHeadingValid(void);
struct serialPort_s;
void gpsEnablePassthrough(struct serialPort_s *gpsPassthroughPort);
void mspGPSReceiveNewData(const uint8_t * bufferPtr);

113
src/main/io/gps_msp.c Normal file
View file

@ -0,0 +1,113 @@
/*
* This file is part of INAV Project.
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
* You can obtain one at http://mozilla.org/MPL/2.0/.
*
* Alternatively, the contents of this file may be used under the terms
* of the GNU General Public License Version 3, as described below:
*
* This file is free software: you may copy, redistribute and/or modify
* it under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or (at your
* option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
* Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*/
#include <stdbool.h>
#include <stdint.h>
#include <ctype.h>
#include <string.h>
#include <math.h>
#include "platform.h"
#include "build/build_config.h"
#if defined(USE_GPS_PROTO_MSP)
#include "build/debug.h"
#include "common/axis.h"
#include "common/gps_conversion.h"
#include "common/maths.h"
#include "common/utils.h"
#include "drivers/serial.h"
#include "drivers/time.h"
#include "fc/config.h"
#include "fc/runtime_config.h"
#include "io/gps.h"
#include "io/gps_private.h"
#include "io/serial.h"
#include "msp/msp_protocol_v2_sensor_msg.h"
static bool newDataReady;
void gpsRestartMSP(void)
{
// NOP
}
void gpsHandleMSP(void)
{
if (newDataReady) {
gpsProcessNewSolutionData();
newDataReady = false;
}
}
static uint8_t gpsMapFixType(uint8_t mspFixType)
{
if (mspFixType == 2)
return GPS_FIX_2D;
if (mspFixType >= 3)
return GPS_FIX_3D;
return GPS_NO_FIX;
}
void mspGPSReceiveNewData(const uint8_t * bufferPtr)
{
const mspSensorGpsDataMessage_t * pkt = (const mspSensorGpsDataMessage_t *)bufferPtr;
gpsSol.fixType = gpsMapFixType(pkt->fixType);
gpsSol.numSat = pkt->satellitesInView;
gpsSol.llh.lon = pkt->longitude;
gpsSol.llh.lat = pkt->latitude;
gpsSol.llh.alt = pkt->mslAltitude;
gpsSol.velNED[X] = pkt->nedVelNorth;
gpsSol.velNED[Y] = pkt->nedVelEast;
gpsSol.velNED[Z] = pkt->nedVelDown;
gpsSol.groundSpeed = sqrtf(sq((float)pkt->nedVelNorth) + sq((float)pkt->nedVelEast));
gpsSol.groundCourse = pkt->groundCourse / 10; // in deg * 10
gpsSol.eph = gpsConstrainEPE(pkt->horizontalPosAccuracy / 10);
gpsSol.epv = gpsConstrainEPE(pkt->verticalPosAccuracy / 10);
gpsSol.hdop = gpsConstrainHDOP(pkt->hdop);
gpsSol.flags.validVelNE = 1;
gpsSol.flags.validVelD = 1;
gpsSol.flags.validEPE = 1;
gpsSol.time.year = pkt->year;
gpsSol.time.month = pkt->month;
gpsSol.time.day = pkt->day;
gpsSol.time.hours = pkt->hour;
gpsSol.time.minutes = pkt->min;
gpsSol.time.seconds = pkt->sec;
gpsSol.time.millis = 0;
gpsSol.flags.validTime = (pkt->fixType >= 3);
newDataReady = true;
}
#endif

3
src/main/io/gps_private.h
View file

@ -78,4 +78,7 @@ extern void gpsHandleMTK(void);
extern void gpsRestartNAZA(void);
extern void gpsHandleNAZA(void);
extern void gpsRestartMSP(void);
extern void gpsHandleMSP(void);
#endif

123
src/main/io/gps_ublox.c
View file

@ -35,6 +35,7 @@
#include "common/gps_conversion.h"
#include "common/maths.h"
#include "common/utils.h"
#include "common/log.h"
#include "drivers/serial.h"
#include "drivers/time.h"
@ -274,6 +275,8 @@ enum {
MSG_VELNED = 0x12,
MSG_TIMEUTC = 0x21,
MSG_SVINFO = 0x30,
MSG_NAV_SAT = 0x35,
MSG_NAV_SIG = 0x43,
MSG_CFG_PRT = 0x00,
MSG_CFG_RATE = 0x08,
MSG_CFG_SET_RATE = 0x01,
@ -701,6 +704,7 @@ STATIC_PROTOTHREAD(gpsConfigure)
gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT);
// Set dynamic model
LOG_D(GPS, "UBLOX: Configuring NAV5");
switch (gpsState.gpsConfig->dynModel) {
case GPS_DYNMODEL_PEDESTRIAN:
configureNAV5(UBX_DYNMODEL_PEDESTRIAN, UBX_FIXMODE_AUTO);
@ -718,6 +722,8 @@ STATIC_PROTOTHREAD(gpsConfigure)
// Disable NMEA messages
gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT);
LOG_D(GPS, "UBLOX: Disable NMEA messages");
configureMSG(MSG_CLASS_NMEA, MSG_NMEA_GGA, 0);
ptWait(_ack_state == UBX_ACK_GOT_ACK);
@ -739,36 +745,17 @@ STATIC_PROTOTHREAD(gpsConfigure)
// Configure UBX binary messages
gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT);
if ((gpsState.gpsConfig->provider == GPS_UBLOX) || (gpsState.hwVersion < 70000)) {
configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 1);
ptWait(_ack_state == UBX_ACK_GOT_ACK);
// u-Blox 9
// M9N does not support some of the UBX 6/7/8 messages, so we have to configure it using special sequence
if (gpsState.hwVersion >= 190000) {
LOG_D(GPS, "UBLOX: Configuring UBX-9 messages");
configureMSG(MSG_CLASS_UBX, MSG_STATUS, 1);
ptWait(_ack_state == UBX_ACK_GOT_ACK);
configureMSG(MSG_CLASS_UBX, MSG_SOL, 1);
ptWait(_ack_state == UBX_ACK_GOT_ACK);
configureMSG(MSG_CLASS_UBX, MSG_VELNED, 1);
ptWait(_ack_state == UBX_ACK_GOT_ACK);
configureMSG(MSG_CLASS_UBX, MSG_TIMEUTC, 10);
ptWait(_ack_state == UBX_ACK_GOT_ACK);
// This may fail on old UBLOX units, advance forward on both ACK and NAK
configureMSG(MSG_CLASS_UBX, MSG_PVT, 0);
ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK);
}
else {
configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 0);
ptWait(_ack_state == UBX_ACK_GOT_ACK);
configureMSG(MSG_CLASS_UBX, MSG_STATUS, 0);
ptWait(_ack_state == UBX_ACK_GOT_ACK);
configureMSG(MSG_CLASS_UBX, MSG_SOL, 0);
ptWait(_ack_state == UBX_ACK_GOT_ACK);
configureMSG(MSG_CLASS_UBX, MSG_VELNED, 0);
ptWait(_ack_state == UBX_ACK_GOT_ACK);
@ -777,20 +764,84 @@ STATIC_PROTOTHREAD(gpsConfigure)
configureMSG(MSG_CLASS_UBX, MSG_PVT, 1);
ptWait(_ack_state == UBX_ACK_GOT_ACK);
}
configureMSG(MSG_CLASS_UBX, MSG_SVINFO, 0);
ptWait(_ack_state == UBX_ACK_GOT_ACK);
configureMSG(MSG_CLASS_UBX, MSG_NAV_SAT, 0);
ptWait(_ack_state == UBX_ACK_GOT_ACK);
// Configure data rate
gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT);
if ((gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) && (gpsState.hwVersion >= 70000)) {
configureRATE(100); // 10Hz
configureMSG(MSG_CLASS_UBX, MSG_NAV_SIG, 0);
ptWait(_ack_state == UBX_ACK_GOT_ACK);
// u-Blox 9 receivers such as M9N can do 10Hz as well, but the number of used satellites will be restricted to 16.
// Not mentioned in the datasheet
configureRATE(200);
ptWait(_ack_state == UBX_ACK_GOT_ACK);
}
else {
configureRATE(200); // 5Hz
// u-Blox 6/7/8
// u-Blox 6 doesn't support PVT, use legacy config
if ((gpsState.gpsConfig->provider == GPS_UBLOX) || (gpsState.hwVersion < 70000)) {
LOG_D(GPS, "UBLOX: Configuring UBX-6 messages");
configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 1);
ptWait(_ack_state == UBX_ACK_GOT_ACK);
configureMSG(MSG_CLASS_UBX, MSG_STATUS, 1);
ptWait(_ack_state == UBX_ACK_GOT_ACK);
configureMSG(MSG_CLASS_UBX, MSG_SOL, 1);
ptWait(_ack_state == UBX_ACK_GOT_ACK);
configureMSG(MSG_CLASS_UBX, MSG_VELNED, 1);
ptWait(_ack_state == UBX_ACK_GOT_ACK);
configureMSG(MSG_CLASS_UBX, MSG_TIMEUTC, 10);
ptWait(_ack_state == UBX_ACK_GOT_ACK);
// This may fail on old UBLOX units, advance forward on both ACK and NAK
configureMSG(MSG_CLASS_UBX, MSG_PVT, 0);
ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK);
configureMSG(MSG_CLASS_UBX, MSG_SVINFO, 0);
ptWait(_ack_state == UBX_ACK_GOT_ACK);
// Configure data rate to 5HZ
configureRATE(200);
ptWait(_ack_state == UBX_ACK_GOT_ACK);
}
// u-Blox 7-8 support PVT
else {
LOG_D(GPS, "UBLOX: Configuring UBX-7 messages");
configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 0);
ptWait(_ack_state == UBX_ACK_GOT_ACK);
configureMSG(MSG_CLASS_UBX, MSG_STATUS, 0);
ptWait(_ack_state == UBX_ACK_GOT_ACK);
configureMSG(MSG_CLASS_UBX, MSG_SOL, 1);
ptWait(_ack_state == UBX_ACK_GOT_ACK);
configureMSG(MSG_CLASS_UBX, MSG_VELNED, 0);
ptWait(_ack_state == UBX_ACK_GOT_ACK);
configureMSG(MSG_CLASS_UBX, MSG_TIMEUTC, 0);
ptWait(_ack_state == UBX_ACK_GOT_ACK);
configureMSG(MSG_CLASS_UBX, MSG_PVT, 1);
ptWait(_ack_state == UBX_ACK_GOT_ACK);
configureMSG(MSG_CLASS_UBX, MSG_SVINFO, 0);
ptWait(_ack_state == UBX_ACK_GOT_ACK);
if ((gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) && (gpsState.hwVersion >= 70000)) {
configureRATE(100); // 10Hz
}
else {
configureRATE(200); // 5Hz
}
ptWait(_ack_state == UBX_ACK_GOT_ACK);
}
}
ptWait(_ack_state == UBX_ACK_GOT_ACK);
// Configure SBAS
// If particular SBAS setting is not supported by the hardware we'll get a NAK,
@ -905,6 +956,14 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread)
ptWaitTimeout((gpsState.hwVersion != 0), GPS_CFG_CMD_TIMEOUT_MS);
} while(gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && gpsState.hwVersion == 0);
if (gpsState.hwVersion == 0) {
LOG_E(GPS, "UBLOX: version detection timeout");
}
else {
LOG_I(GPS, "UBLOX: detected HW: %u", (unsigned)gpsState.hwVersion);
}
// Configure GPS
ptSpawn(gpsConfigure);
}

10
src/main/io/opflow_msp.c
View file

@ -37,15 +37,13 @@
#include "io/serial.h"
#if defined(USE_OPFLOW_MSP)
#include "drivers/opflow/opflow_virtual.h"
#include "drivers/time.h"
#include "io/opflow.h"
typedef struct __attribute__((packed)) {
uint8_t quality; // [0;255]
int32_t motionX;
int32_t motionY;
} mspOpflowSensor_t;
#include "msp/msp_protocol_v2_sensor_msg.h"
static bool hasNewData = false;
static timeUs_t updatedTimeUs = 0;
@ -76,7 +74,7 @@ static bool mspOpflowUpdate(opflowData_t * data)
void mspOpflowReceiveNewData(uint8_t * bufferPtr)
{
const timeUs_t currentTimeUs = micros();
const mspOpflowSensor_t * pkt = (const mspOpflowSensor_t *)bufferPtr;
const mspSensorOpflowDataMessage_t * pkt = (const mspSensorOpflowDataMessage_t *)bufferPtr;
sensorData.deltaTime = currentTimeUs - updatedTimeUs;
sensorData.flowRateRaw[0] = pkt->motionX;

92
src/main/io/osd.c
View file

@ -417,6 +417,7 @@ static void osdFormatWindSpeedStr(char *buff, int32_t ws, bool isValid)
centivalue = (ws * 224) / 100;
suffix = SYM_MPH;
break;
default:
case OSD_UNIT_METRIC:
centivalue = (ws * 36) / 10;
suffix = SYM_KMH;
@ -863,24 +864,23 @@ void osdCrosshairPosition(uint8_t *x, uint8_t *y)
}
/**
* Formats throttle position prefixed by its symbol. If autoThr
* is true and the navigation system is controlling THR, it
* uses the THR value applied by the system rather than the
* input value received by the sticks.
* Formats throttle position prefixed by its symbol.
* Shows output to motor, not stick position
**/
static void osdFormatThrottlePosition(char *buff, bool autoThr, textAttributes_t *elemAttr)
{
const int minThrottle = getThrottleIdleValue();
buff[0] = SYM_BLANK;
buff[1] = SYM_THR;
int16_t thr = rxGetChannelValue(THROTTLE);
int16_t thr = (constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX ) - minThrottle) * 100 / (motorConfig()->maxthrottle - minThrottle);
if (autoThr && navigationIsControllingThrottle()) {
buff[0] = SYM_AUTO_THR0;
buff[1] = SYM_AUTO_THR1;
thr = rcCommand[THROTTLE];
thr = (constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN);
if (isFixedWingAutoThrottleManuallyIncreased())
TEXT_ATTRIBUTES_ADD_BLINK(*elemAttr);
}
tfp_sprintf(buff + 2, "%3d", (constrain(thr, PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN));
tfp_sprintf(buff + 2, "%3d", thr);
}
/**
@ -1011,6 +1011,7 @@ static void osdDrawMap(int referenceHeading, uint8_t referenceSym, uint8_t cente
break;
case OSD_UNIT_UK:
FALLTHROUGH;
default:
case OSD_UNIT_METRIC:
initialScale = 10; // 10m as initial scale
break;
@ -1660,17 +1661,32 @@ static bool osdDrawSingleElement(uint8_t item)
}
break;
case OSD_CRSF_LQ:
buff[0] = SYM_BLANK;
tfp_sprintf(buff, "%d:%3d%s", rxLinkStatistics.rfMode, rxLinkStatistics.uplinkLQ, "%");
if (!failsafeIsReceivingRxData()){
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
} else if (rxLinkStatistics.uplinkLQ < osdConfig()->rssi_alarm) {
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
}
break;
#if defined(USE_SERIALRX_CRSF)
case OSD_CRSF_LQ: {
buff[0] = SYM_BLANK;
int16_t statsLQ = rxLinkStatistics.uplinkLQ;
int16_t scaledLQ = scaleRange(constrain(statsLQ, 0, 100), 0, 100, 170, 300);
if (rxLinkStatistics.rfMode == 2) {
if (osdConfig()->crsf_lq_format == OSD_CRSF_LQ_TYPE1) {
tfp_sprintf(buff, "%5d%s", scaledLQ, "%");
} else {
tfp_sprintf(buff, "%d:%3d%s", rxLinkStatistics.rfMode, rxLinkStatistics.uplinkLQ, "%");
}
} else {
if (osdConfig()->crsf_lq_format == OSD_CRSF_LQ_TYPE1) {
tfp_sprintf(buff, "%5d%s", rxLinkStatistics.uplinkLQ, "%");
} else {
tfp_sprintf(buff, "%d:%3d%s", rxLinkStatistics.rfMode, rxLinkStatistics.uplinkLQ, "%");
}
}
if (!failsafeIsReceivingRxData()){
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
} else if (rxLinkStatistics.uplinkLQ < osdConfig()->link_quality_alarm) {
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
}
break;
}
case OSD_CRSF_SNR_DB: {
const char* showsnr = "-12";
const char* hidesnr = " ";
@ -1829,6 +1845,7 @@ static bool osdDrawSingleElement(uint8_t item)
value = CENTIMETERS_TO_CENTIFEET(value);
sym = SYM_FTS;
break;
default:
case OSD_UNIT_METRIC:
// Already in cm/s
sym = SYM_MS;
@ -2310,6 +2327,7 @@ static bool osdDrawSingleElement(uint8_t item)
break;
case OSD_UNIT_UK:
FALLTHROUGH;
default:
case OSD_UNIT_METRIC:
scaleToUnit = 100; // scale to cm for osdFormatCentiNumber()
scaleUnitDivisor = 1000; // Convert to km when scale gets bigger than 999m
@ -2344,7 +2362,7 @@ static bool osdDrawSingleElement(uint8_t item)
displayWriteChar(osdDisplayPort, elemPosX, elemPosY + 1, referenceSymbol);
return true;
}
case OSD_GVAR_0:
{
osdFormatGVar(buff, 0);
@ -2396,7 +2414,29 @@ static bool osdDrawSingleElement(uint8_t item)
return true;
}
#endif
case OSD_TPA:
{
char buff[4];
textAttributes_t attr;
displayWrite(osdDisplayPort, elemPosX, elemPosY, "TPA BP");
attr = TEXT_ATTRIBUTES_NONE;
tfp_sprintf(buff, "TPA %3d", currentControlRateProfile->throttle.dynPID);
if (isAdjustmentFunctionSelected(ADJUSTMENT_TPA)) {
TEXT_ATTRIBUTES_ADD_BLINK(attr);
}
displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, attr);
attr = TEXT_ATTRIBUTES_NONE;
tfp_sprintf(buff, "BP %4d", currentControlRateProfile->throttle.pa_breakpoint);
if (isAdjustmentFunctionSelected(ADJUSTMENT_TPA_BREAKPOINT)) {
TEXT_ATTRIBUTES_ADD_BLINK(attr);
}
displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY + 1, buff, attr);
return true;
}
default:
return false;
}
@ -2509,7 +2549,9 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
.baro_temp_alarm_max = 600,
#endif
#ifdef USE_SERIALRX_CRSF
.snr_alarm = 5,
.snr_alarm = 4,
.crsf_lq_format = OSD_CRSF_LQ_TYPE1,
.link_quality_alarm = 70,
#endif
#ifdef USE_TEMPERATURE_SENSOR
.temp_label_align = OSD_ALIGN_LEFT,
@ -2598,10 +2640,10 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
osdLayoutsConfig->item_pos[0][OSD_VTX_CHANNEL] = OSD_POS(8, 6);
#ifdef USE_SERIALRX_CRSF
osdLayoutsConfig->item_pos[0][OSD_CRSF_RSSI_DBM] = OSD_POS(23, 12);
osdLayoutsConfig->item_pos[0][OSD_CRSF_LQ] = OSD_POS(22, 11);
osdLayoutsConfig->item_pos[0][OSD_CRSF_SNR_DB] = OSD_POS(23, 9);
osdLayoutsConfig->item_pos[0][OSD_CRSF_TX_POWER] = OSD_POS(24, 10);
osdLayoutsConfig->item_pos[0][OSD_CRSF_RSSI_DBM] = OSD_POS(24, 12);
osdLayoutsConfig->item_pos[0][OSD_CRSF_LQ] = OSD_POS(24, 11);
osdLayoutsConfig->item_pos[0][OSD_CRSF_SNR_DB] = OSD_POS(25, 9);
osdLayoutsConfig->item_pos[0][OSD_CRSF_TX_POWER] = OSD_POS(25, 10);
#endif
osdLayoutsConfig->item_pos[0][OSD_ONTIME] = OSD_POS(23, 8);
@ -3321,6 +3363,10 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
}
} else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) {
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOLAUNCH);
const char *launchStateMessage = fixedWingLaunchStateMessage();
if (launchStateMessage) {
messages[messageCount++] = launchStateMessage;
}
} else {
if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) {
// ALTHOLD might be enabled alongside ANGLE/HORIZON/ACRO

9
src/main/io/osd.h
View file

@ -216,6 +216,7 @@ typedef enum {
OSD_GVAR_1,
OSD_GVAR_2,
OSD_GVAR_3,
OSD_TPA,
OSD_ITEM_COUNT // MUST BE LAST
} osd_items_e;
@ -261,6 +262,11 @@ typedef enum {
OSD_AHI_STYLE_LINE,
} osd_ahi_style_e;
typedef enum {
OSD_CRSF_LQ_TYPE1,
OSD_CRSF_LQ_TYPE2,
} osd_crsf_lq_format_e;
typedef struct osdLayoutsConfig_s {
// Layouts
uint16_t item_pos[OSD_LAYOUT_COUNT][OSD_ITEM_COUNT];
@ -285,6 +291,7 @@ typedef struct osdConfig_s {
float gforce_axis_alarm_max;
#ifdef USE_SERIALRX_CRSF
int16_t snr_alarm; //CRSF SNR alarm in dB
int8_t link_quality_alarm;
#endif
#ifdef USE_BARO
int16_t baro_temp_alarm_min;
@ -338,6 +345,8 @@ typedef struct osdConfig_s {
uint8_t left_sidebar_scroll_step; // How many units each sidebar step represents. 0 means the default value for the scroll type.
uint8_t right_sidebar_scroll_step; // Same as left_sidebar_scroll_step, but for the right sidebar.
uint8_t crsf_lq_format;
} osdConfig_t;
PG_DECLARE(osdConfig_t, osdConfig);

637
src/main/io/osd_dji_hd.c
View file

@ -39,12 +39,16 @@
#include "common/time.h"
#include "common/crc.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "fc/fc_core.h"
#include "fc/config.h"
#include "fc/controlrate_profile.h"
#include "fc/fc_msp.h"
#include "fc/fc_msp_box.h"
#include "fc/runtime_config.h"
#include "fc/settings.h"
#include "flight/imu.h"
#include "flight/pid.h"
@ -63,14 +67,21 @@
#include "sensors/rangefinder.h"
#include "sensors/acceleration.h"
#include "sensors/esc_sensor.h"
#include "sensors/temperature.h"
#include "msp/msp.h"
#include "msp/msp_protocol.h"
#include "msp/msp_serial.h"
#include "common/string_light.h"
#include "navigation/navigation.h"
#include "navigation/navigation_private.h"
#include "common/constants.h"
#include "scheduler/scheduler.h"
#include "common/printf.h"
#include <stdlib.h>
#include "rx/rx.h"
#include "fc/rc_controls.h"
#if defined(USE_DJI_HD_OSD)
@ -80,6 +91,24 @@
#define DJI_OSD_WARNING_COUNT 16
#define DJI_OSD_TIMER_COUNT 2
#define DJI_OSD_FLAGS_OSD_FEATURE (1 << 0)
#define EFFICIENCY_UPDATE_INTERVAL (5 * 1000)
#define RC_RX_LINK_LOST_MSG "!RC RX LINK LOST!"
// Adjust OSD_MESSAGE's default position when
// changing OSD_MESSAGE_LENGTH
#define OSD_MESSAGE_LENGTH 28
#define OSD_ALTERNATING_CHOICES(ms, num_choices) ((millis() / ms) % num_choices)
#define _CONST_STR_SIZE(s) ((sizeof(s)/sizeof(s[0]))-1) // -1 to avoid counting final '\0'
// Wrap all string constants intenteded for display as messages with
// this macro to ensure compile time length validation.
#define OSD_MESSAGE_STR(x) ({ \
STATIC_ASSERT(_CONST_STR_SIZE(x) <= OSD_MESSAGE_LENGTH, message_string_ ## __COUNTER__ ## _too_long); \
x; \
})
/*
* DJI HD goggles use MSPv1 compatible with Betaflight 4.1.0
@ -89,6 +118,13 @@
* but reuse the packet decoder to minimize code duplication
*/
PG_REGISTER_WITH_RESET_TEMPLATE(djiOsdConfig_t, djiOsdConfig, PG_DJI_OSD_CONFIG, 1);
PG_RESET_TEMPLATE(djiOsdConfig_t, djiOsdConfig,
.use_name_for_messages = true,
.esc_temperature_source = DJI_OSD_TEMP_ESC,
.proto_workarounds = DJI_OSD_USE_NON_STANDARD_MSP_ESC_SENSOR_DATA,
);
// External dependency on looptime
extern timeDelta_t cycleTime;
@ -145,7 +181,7 @@ const djiOsdMapping_t djiOSDItemIndexMap[] = {
{ OSD_HEADING, 0 }, // DJI: OSD_NUMERICAL_HEADING
{ OSD_VARIO_NUM, 0 }, // DJI: OSD_NUMERICAL_VARIO
{ -1, 0 }, // DJI: OSD_COMPASS_BAR
{ -1, 0 }, // DJI: OSD_ESC_TMP
{ OSD_ESC_TEMPERATURE, 0 }, // DJI: OSD_ESC_TEMPERATURE
{ OSD_ESC_RPM, 0 }, // DJI: OSD_ESC_RPM
{ OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH, FEATURE_CURRENT_METER }, // DJI: OSD_REMAINING_TIME_ESTIMATE
{ OSD_RTC_TIME, 0 }, // DJI: OSD_RTC_DATETIME
@ -374,8 +410,493 @@ static void djiSerializeOSDConfigReply(sbuf_t *dst)
//sbufWriteU8(dst, DJI_OSD_SCREEN_WIDTH); // osdConfig()->camera_frame_width
//sbufWriteU8(dst, DJI_OSD_SCREEN_HEIGHT); // osdConfig()->camera_frame_height
}
static const char * osdArmingDisabledReasonMessage(void)
{
switch (isArmingDisabledReason()) {
case ARMING_DISABLED_FAILSAFE_SYSTEM:
// See handling of FAILSAFE_RX_LOSS_MONITORING in failsafe.c
if (failsafePhase() == FAILSAFE_RX_LOSS_MONITORING) {
if (failsafeIsReceivingRxData()) {
// If we're not using sticks, it means the ARM switch
// hasn't been off since entering FAILSAFE_RX_LOSS_MONITORING
// yet
return OSD_MESSAGE_STR("DISARM!");
}
// Not receiving RX data
return OSD_MESSAGE_STR(RC_RX_LINK_LOST_MSG);
}
return OSD_MESSAGE_STR("FAILSAFE");
case ARMING_DISABLED_NOT_LEVEL:
return OSD_MESSAGE_STR("!LEVEL");
case ARMING_DISABLED_SENSORS_CALIBRATING:
return OSD_MESSAGE_STR("CALIBRATING");
case ARMING_DISABLED_SYSTEM_OVERLOADED:
return OSD_MESSAGE_STR("OVERLOAD");
case ARMING_DISABLED_NAVIGATION_UNSAFE:
// Check the exact reason
switch (navigationIsBlockingArming(NULL)) {
case NAV_ARMING_BLOCKER_NONE:
break;
case NAV_ARMING_BLOCKER_MISSING_GPS_FIX:
return OSD_MESSAGE_STR("NO GPS FIX");
case NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE:
return OSD_MESSAGE_STR("DISABLE NAV");
case NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR:
return OSD_MESSAGE_STR("1ST WYP TOO FAR");
case NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR:
return OSD_MESSAGE_STR("WYP MISCONFIGURED");
}
break;
case ARMING_DISABLED_COMPASS_NOT_CALIBRATED:
return OSD_MESSAGE_STR("COMPS CALIB");
case ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED:
return OSD_MESSAGE_STR("ACC CALIB");
case ARMING_DISABLED_ARM_SWITCH:
return OSD_MESSAGE_STR("DISARM!");
case ARMING_DISABLED_HARDWARE_FAILURE:
return OSD_MESSAGE_STR("ERR HW!");
// {
// if (!HW_SENSOR_IS_HEALTHY(getHwGyroStatus())) {
// return OSD_MESSAGE_STR("GYRO FAILURE");
// }
// if (!HW_SENSOR_IS_HEALTHY(getHwAccelerometerStatus())) {
// return OSD_MESSAGE_STR("ACCELEROMETER FAILURE");
// }
// if (!HW_SENSOR_IS_HEALTHY(getHwCompassStatus())) {
// return OSD_MESSAGE_STR("COMPASS FAILURE");
// }
// if (!HW_SENSOR_IS_HEALTHY(getHwBarometerStatus())) {
// return OSD_MESSAGE_STR("BAROMETER FAILURE");
// }
// if (!HW_SENSOR_IS_HEALTHY(getHwGPSStatus())) {
// return OSD_MESSAGE_STR("GPS FAILURE");
// }
// if (!HW_SENSOR_IS_HEALTHY(getHwRangefinderStatus())) {
// return OSD_MESSAGE_STR("RANGE FINDER FAILURE");
// }
// if (!HW_SENSOR_IS_HEALTHY(getHwPitotmeterStatus())) {
// return OSD_MESSAGE_STR("PITOT METER FAILURE");
// }
// }
// return OSD_MESSAGE_STR("HARDWARE FAILURE");
case ARMING_DISABLED_BOXFAILSAFE:
return OSD_MESSAGE_STR("FAILSAFE ENABLED");
case ARMING_DISABLED_BOXKILLSWITCH:
return OSD_MESSAGE_STR("KILLSWITCH ENABLED");
case ARMING_DISABLED_RC_LINK:
return OSD_MESSAGE_STR("NO RC LINK");
case ARMING_DISABLED_THROTTLE:
return OSD_MESSAGE_STR("THROTTLE!");
case ARMING_DISABLED_ROLLPITCH_NOT_CENTERED:
return OSD_MESSAGE_STR("ROLLPITCH!");
case ARMING_DISABLED_SERVO_AUTOTRIM:
return OSD_MESSAGE_STR("AUTOTRIM!");
case ARMING_DISABLED_OOM:
return OSD_MESSAGE_STR("MEM LOW");
case ARMING_DISABLED_INVALID_SETTING:
return OSD_MESSAGE_STR("ERR SETTING");
case ARMING_DISABLED_CLI:
return OSD_MESSAGE_STR("CLI");
case ARMING_DISABLED_PWM_OUTPUT_ERROR:
return OSD_MESSAGE_STR("PWM ERR");
// Cases without message
case ARMING_DISABLED_CMS_MENU:
FALLTHROUGH;
case ARMING_DISABLED_OSD_MENU:
FALLTHROUGH;
case ARMING_DISABLED_ALL_FLAGS:
FALLTHROUGH;
case ARMED:
FALLTHROUGH;
case WAS_EVER_ARMED:
break;
}
return NULL;
}
static const char * osdFailsafePhaseMessage(void)
{
// See failsafe.h for each phase explanation
switch (failsafePhase()) {
case FAILSAFE_RETURN_TO_HOME:
// XXX: Keep this in sync with OSD_FLYMODE.
return OSD_MESSAGE_STR("(RTH)");
case FAILSAFE_LANDING:
// This should be considered an emergengy landing
return OSD_MESSAGE_STR("(EMRGY LANDING)");
case FAILSAFE_RX_LOSS_MONITORING:
// Only reachable from FAILSAFE_LANDED, which performs
// a disarm. Since aircraft has been disarmed, we no
// longer show failsafe details.
FALLTHROUGH;
case FAILSAFE_LANDED:
// Very brief, disarms and transitions into
// FAILSAFE_RX_LOSS_MONITORING. Note that it prevents
// further rearming via ARMING_DISABLED_FAILSAFE_SYSTEM,
// so we'll show the user how to re-arm in when
// that flag is the reason to prevent arming.
FALLTHROUGH;
case FAILSAFE_RX_LOSS_IDLE:
// This only happens when user has chosen NONE as FS
// procedure. The recovery messages should be enough.
FALLTHROUGH;
case FAILSAFE_IDLE:
// Failsafe not active
FALLTHROUGH;
case FAILSAFE_RX_LOSS_DETECTED:
// Very brief, changes to FAILSAFE_RX_LOSS_RECOVERED
// or the FS procedure immediately.
FALLTHROUGH;
case FAILSAFE_RX_LOSS_RECOVERED:
// Exiting failsafe
break;
}
return NULL;
}
static const char * osdFailsafeInfoMessage(void)
{
if (failsafeIsReceivingRxData()) {
// User must move sticks to exit FS mode
return OSD_MESSAGE_STR("!MOVE STICKS TO EXIT FS!");
}
return OSD_MESSAGE_STR(RC_RX_LINK_LOST_MSG);
}
static const char * navigationStateMessage(void)
{
switch (NAV_Status.state) {
case MW_NAV_STATE_NONE:
break;
case MW_NAV_STATE_RTH_START:
return OSD_MESSAGE_STR("STARTING RTH");
case MW_NAV_STATE_RTH_ENROUTE:
// TODO: Break this up between climb and head home
return OSD_MESSAGE_STR("EN ROUTE TO HOME");
case MW_NAV_STATE_HOLD_INFINIT:
// Used by HOLD flight modes. No information to add.
break;
case MW_NAV_STATE_HOLD_TIMED:
// TODO: Maybe we can display a count down
return OSD_MESSAGE_STR("HOLDING WAYPOINT");
break;
case MW_NAV_STATE_WP_ENROUTE:
// TODO: Show WP number
return OSD_MESSAGE_STR("TO WP");
case MW_NAV_STATE_PROCESS_NEXT:
return OSD_MESSAGE_STR("PREPARING FOR NEXT WAYPOINT");
case MW_NAV_STATE_DO_JUMP:
// Not used
break;
case MW_NAV_STATE_LAND_START:
// Not used
break;
case MW_NAV_STATE_EMERGENCY_LANDING:
return OSD_MESSAGE_STR("EMRGY LANDING");
case MW_NAV_STATE_LAND_IN_PROGRESS:
return OSD_MESSAGE_STR("LANDING");
case MW_NAV_STATE_HOVER_ABOVE_HOME:
if (STATE(FIXED_WING_LEGACY)) {
return OSD_MESSAGE_STR("LOITERING AROUND HOME");
}
return OSD_MESSAGE_STR("HOVERING");
case MW_NAV_STATE_LANDED:
return OSD_MESSAGE_STR("LANDED");
case MW_NAV_STATE_LAND_SETTLE:
return OSD_MESSAGE_STR("PREPARING TO LAND");
case MW_NAV_STATE_LAND_START_DESCENT:
// Not used
break;
}
return NULL;
}
/**
* Converts velocity based on the current unit system (kmh or mph).
* @param alt Raw velocity (i.e. as taken from gpsSol.groundSpeed in centimeters/second)
*/
static int32_t osdConvertVelocityToUnit(int32_t vel)
{
switch (osdConfig()->units) {
case OSD_UNIT_UK:
FALLTHROUGH;
case OSD_UNIT_IMPERIAL:
return (vel * 224) / 10000; // Convert to mph
case OSD_UNIT_METRIC:
return (vel * 36) / 1000; // Convert to kmh
}
// Unreachable
return -1;
}
static int16_t osdDJIGet3DSpeed(void)
{
int16_t vert_speed = getEstimatedActualVelocity(Z);
int16_t hor_speed = gpsSol.groundSpeed;
return (int16_t)sqrtf(sq(hor_speed) + sq(vert_speed));
}
/**
* Converts velocity into a string based on the current unit system.
* @param alt Raw velocity (i.e. as taken from gpsSol.groundSpeed in centimeters/seconds)
*/
void osdDJIFormatVelocityStr(char* buff, int32_t vel )
{
switch (osdConfig()->units) {
case OSD_UNIT_UK:
FALLTHROUGH;
case OSD_UNIT_IMPERIAL:
tfp_sprintf(buff, "%3d%s", (int)osdConvertVelocityToUnit(vel), "MPH");
break;
case OSD_UNIT_METRIC:
tfp_sprintf(buff, "%3d%s", (int)osdConvertVelocityToUnit(vel), "KMH");
break;
}
}
static void osdDJIFormatThrottlePosition(char *buff, bool autoThr )
{
int16_t thr = rxGetChannelValue(THROTTLE);
if (autoThr && navigationIsControllingThrottle()) {
thr = rcCommand[THROTTLE];
}
tfp_sprintf(buff, "%3d%s", (constrain(thr, PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN), "%THR");
}
/**
* Converts distance into a string based on the current unit system.
* @param dist Distance in centimeters
*/
static void osdDJIFormatDistanceStr(char *buff, int32_t dist)
{
int32_t centifeet;
switch (osdConfig()->units) {
case OSD_UNIT_IMPERIAL:
centifeet = CENTIMETERS_TO_CENTIFEET(dist);
if (abs(centifeet) < FEET_PER_MILE * 100 / 2) {
// Show feet when dist < 0.5mi
tfp_sprintf(buff, "%d%s", (int)(centifeet / 100), "FT");
}
else {
// Show miles when dist >= 0.5mi
tfp_sprintf(buff, "%d.%02d%s", (int)(centifeet / (100*FEET_PER_MILE)),
(abs(centifeet) % (100 * FEET_PER_MILE)) / FEET_PER_MILE, "Mi");
}
break;
case OSD_UNIT_UK:
FALLTHROUGH;
case OSD_UNIT_METRIC:
if (abs(dist) < METERS_PER_KILOMETER * 100) {
// Show meters when dist < 1km
tfp_sprintf(buff, "%d%s", (int)(dist / 100), "M");
}
else {
// Show kilometers when dist >= 1km
tfp_sprintf(buff, "%d.%02d%s", (int)(dist / (100*METERS_PER_KILOMETER)),
(abs(dist) % (100 * METERS_PER_KILOMETER)) / METERS_PER_KILOMETER, "KM");
}
break;
}
}
static void osdDJIEfficiencyMahPerKM(char *buff)
{
// amperage is in centi amps, speed is in cms/s. We want
// mah/km. Values over 999 are considered useless and
// displayed as "---""
static pt1Filter_t eFilterState;
static timeUs_t efficiencyUpdated = 0;
int32_t value = 0;
timeUs_t currentTimeUs = micros();
timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated);
if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) {
if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f,
1, efficiencyTimeDelta * 1e-6f);
efficiencyUpdated = currentTimeUs;
}
else {
value = eFilterState.state;
}
}
if (value > 0 && value <= 999) {
tfp_sprintf(buff, "%3d%s", (int)value, "mAhKM");
}
else {
tfp_sprintf(buff, "%s", "---mAhKM");
}
}
static void djiSerializeCraftNameOverride(sbuf_t *dst, const char * name)
{
// :W T S E D
// | | | | Distance Trip
// | | | Efficiency mA/KM
// | | S 3dSpeed
// | Throttle
// Warnings
const char *message = " ";
const char *enabledElements = name + 1;
char djibuf[24];
// clear name from chars : and leading W
if (enabledElements[0] == 'W') {
enabledElements += 1;
}
int elemLen = strlen(enabledElements);
if (elemLen > 0) {
switch (enabledElements[OSD_ALTERNATING_CHOICES(3000, elemLen)]){
case 'T':
osdDJIFormatThrottlePosition(djibuf,true);
break;
case 'S':
osdDJIFormatVelocityStr(djibuf, osdDJIGet3DSpeed());
break;
case 'E':
osdDJIEfficiencyMahPerKM(djibuf);
break;
case 'D':
osdDJIFormatDistanceStr(djibuf, getTotalTravelDistance());
break;
case 'W':
tfp_sprintf(djibuf, "%s", "MAKE_W_FIRST");
break;
default:
tfp_sprintf(djibuf, "%s", "UNKOWN_ELEM");
break;
}
if (djibuf[0] != '\0') {
message = djibuf;
}
}
if (name[1] == 'W') {
char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)];
if (ARMING_FLAG(ARMED)) {
// Aircraft is armed. We might have up to 5
// messages to show.
const char *messages[5];
unsigned messageCount = 0;
if (FLIGHT_MODE(FAILSAFE_MODE)) {
// In FS mode while being armed too
const char *failsafePhaseMessage = osdFailsafePhaseMessage();
const char *failsafeInfoMessage = osdFailsafeInfoMessage();
const char *navStateFSMessage = navigationStateMessage();
if (failsafePhaseMessage) {
messages[messageCount++] = failsafePhaseMessage;
}
if (failsafeInfoMessage) {
messages[messageCount++] = failsafeInfoMessage;
}
if (navStateFSMessage) {
messages[messageCount++] = navStateFSMessage;
}
if (messageCount > 0) {
message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)];
if (message == failsafeInfoMessage) {
// failsafeInfoMessage is not useful for recovering
// a lost model, but might help avoiding a crash.
// Blink to grab user attention.
//doesnt work TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
}
// We're shoing either failsafePhaseMessage or
// navStateFSMessage. Don't BLINK here since
// having this text available might be crucial
// during a lost aircraft recovery and blinking
// will cause it to be missing from some frames.
}
}
else {
if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
const char *navStateMessage = navigationStateMessage();
if (navStateMessage) {
messages[messageCount++] = navStateMessage;
}
}
else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) {
messages[messageCount++] = "AUTOLAUNCH";
}
else {
if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) {
// ALTHOLD might be enabled alongside ANGLE/HORIZON/ACRO
// when it doesn't require ANGLE mode (required only in FW
// right now). If if requires ANGLE, its display is handled
// by OSD_FLYMODE.
messages[messageCount++] = "(ALT HOLD)";
}
if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM)) {
messages[messageCount++] = "(AUTOTRIM)";
}
if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) {
messages[messageCount++] = "(AUTOTUNE)";
}
if (FLIGHT_MODE(HEADFREE_MODE)) {
messages[messageCount++] = "(HEADFREE)";
}
}
// Pick one of the available messages. Each message lasts
// a second.
if (messageCount > 0) {
message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)];
}
}
}
else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) {
unsigned invalidIndex;
// Check if we're unable to arm for some reason
if (ARMING_FLAG(ARMING_DISABLED_INVALID_SETTING) && !settingsValidate(&invalidIndex)) {
if (OSD_ALTERNATING_CHOICES(1000, 2) == 0) {
const setting_t *setting = settingGet(invalidIndex);
settingGetName(setting, messageBuf);
for (int ii = 0; messageBuf[ii]; ii++) {
messageBuf[ii] = sl_toupper(messageBuf[ii]);
}
message = messageBuf;
}
else {
message = "ERR SETTING";
// TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr);
}
}
else {
if (OSD_ALTERNATING_CHOICES(1000, 2) == 0) {
message = "CANT ARM";
// TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr);
} else {
// Show the reason for not arming
message = osdArmingDisabledReasonMessage();
}
}
}
}
if (message[0] != '\0') {
sbufWriteData(dst, message, strlen(message));
}
}
#endif
static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn)
{
UNUSED(mspPostProcessFn);
@ -410,10 +931,26 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms
case DJI_MSP_NAME:
{
const char * name = systemConfig()->name;
int len = strlen(name);
if (len > 12) len = 12;
sbufWriteData(dst, name, len);
#if defined(USE_OSD)
if (djiOsdConfig()->use_name_for_messages) {
if (name[0] == ':') {
// If craft name starts with a semicolon - use it as a template for what we want to show
djiSerializeCraftNameOverride(dst, name);
}
else {
// Otherwise fall back to just warnings
djiSerializeCraftNameOverride(dst, ":W");
}
}
else
#endif
{
int len = strlen(name);
sbufWriteData(dst, name, MAX(len, 12));
break;
}
}
break;
case DJI_MSP_STATUS:
@ -537,21 +1074,93 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms
}
break;
#if defined(USE_ESC_SENSOR)
case DJI_MSP_ESC_SENSOR_DATA:
if (STATE(ESC_SENSOR_ENABLED)) {
sbufWriteU8(dst, getMotorCount());
for (int i = 0; i < getMotorCount(); i++) {
const escSensorData_t * escSensor = getEscTelemetry(i);
sbufWriteU8(dst, escSensor->temperature);
sbufWriteU16(dst, escSensor->rpm);
if (djiOsdConfig()->proto_workarounds & DJI_OSD_USE_NON_STANDARD_MSP_ESC_SENSOR_DATA) {
// Version 1.00.06 of DJI firmware is not using the standard MSP_ESC_SENSOR_DATA
uint16_t protoRpm = 0;
int16_t protoTemp = 0;
#if defined(USE_ESC_SENSOR)
if (STATE(ESC_SENSOR_ENABLED) && getMotorCount() > 0) {
uint32_t motorRpmAcc = 0;
int32_t motorTempAcc = 0;
for (int i = 0; i < getMotorCount(); i++) {
const escSensorData_t * escSensor = getEscTelemetry(i);
motorRpmAcc += escSensor->rpm;
motorTempAcc += escSensor->temperature;
}
protoRpm = motorRpmAcc / getMotorCount();
protoTemp = motorTempAcc / getMotorCount();
}
#endif
switch (djiOsdConfig()->esc_temperature_source) {
// This is ESC temperature (as intended)
case DJI_OSD_TEMP_ESC:
// No-op, temperature is already set to ESC
break;
// Re-purpose the field for core temperature
case DJI_OSD_TEMP_CORE:
getIMUTemperature(&protoTemp);
protoTemp = protoTemp / 10;
break;
// Re-purpose the field for baro temperature
case DJI_OSD_TEMP_BARO:
getBaroTemperature(&protoTemp);
protoTemp = protoTemp / 10;
break;
}
// No motor count, just raw temp and RPM data
sbufWriteU8(dst, protoTemp);
sbufWriteU16(dst, protoRpm);
}
else {
reply->result = MSP_RESULT_ERROR;
// Use standard MSP_ESC_SENSOR_DATA message
sbufWriteU8(dst, getMotorCount());
for (int i = 0; i < getMotorCount(); i++) {
uint16_t motorRpm = 0;
int16_t motorTemp = 0;
// If ESC_SENSOR is enabled, pull the telemetry data and get motor RPM
#if defined(USE_ESC_SENSOR)
if (STATE(ESC_SENSOR_ENABLED)) {
const escSensorData_t * escSensor = getEscTelemetry(i);
motorRpm = escSensor->rpm;
motorTemp = escSensor->temperature;
}
#endif
// Now populate temperature field (which we may override for different purposes)
switch (djiOsdConfig()->esc_temperature_source) {
// This is ESC temperature (as intended)
case DJI_OSD_TEMP_ESC:
// No-op, temperature is already set to ESC
break;
// Re-purpose the field for core temperature
case DJI_OSD_TEMP_CORE:
getIMUTemperature(&motorTemp);
motorTemp = motorTemp / 10;
break;
// Re-purpose the field for baro temperature
case DJI_OSD_TEMP_BARO:
getBaroTemperature(&motorTemp);
motorTemp = motorTemp / 10;
break;
}
// Add data for this motor to the packet
sbufWriteU8(dst, motorTemp);
sbufWriteU16(dst, motorRpm);
}
}
break;
#endif
case DJI_MSP_OSD_CONFIG:
#if defined(USE_OSD)

20
src/main/io/osd_dji_hd.h
View file

@ -29,6 +29,8 @@
#include "msp/msp.h"
#include "msp/msp_serial.h"
#include "config/parameter_group.h"
#if defined(USE_DJI_HD_OSD)
#define DJI_API_VERSION_MAJOR 1
@ -60,6 +62,24 @@
#define DJI_MSP_SET_PID 202
#define DJI_MSP_SET_RC_TUNING 204
enum djiOsdTempSource_e {
DJI_OSD_TEMP_ESC = 0,
DJI_OSD_TEMP_CORE = 1,
DJI_OSD_TEMP_BARO = 2
};
enum djiOsdProtoWorkarounds_e {
DJI_OSD_USE_NON_STANDARD_MSP_ESC_SENSOR_DATA = 1 << 0,
};
typedef struct djiOsdConfig_s {
uint8_t use_name_for_messages;
uint8_t esc_temperature_source;
uint8_t proto_workarounds;
} djiOsdConfig_t;
PG_DECLARE(djiOsdConfig_t, djiOsdConfig);
void djiOsdSerialInit(void);
void djiOsdSerialProcess(void);

6
src/main/io/osd_grid.c
View file

@ -225,7 +225,7 @@ static uint8_t osdUpdateSidebar(osd_sidebar_scroll_e scroll, osd_sidebar_t *side
// Scroll between SYM_AH_DECORATION_MIN and SYM_AH_DECORATION_MAX.
// Zero scrolling should draw SYM_AH_DECORATION.
uint8_t decoration = SYM_AH_DECORATION;
int offset;
int offset = 0;
int steps;
switch (scroll) {
case OSD_SIDEBAR_SCROLL_NONE:
@ -240,8 +240,6 @@ static uint8_t osdUpdateSidebar(osd_sidebar_scroll_e scroll, osd_sidebar_t *side
case OSD_SIDEBAR_SCROLL_GROUND_SPEED:
#if defined(USE_GPS)
offset = gpsSol.groundSpeed;
#else
offset = 0;
#endif
// Move 1 char for every 20 cm/s
steps = offset / 20;
@ -249,8 +247,6 @@ static uint8_t osdUpdateSidebar(osd_sidebar_scroll_e scroll, osd_sidebar_t *side
case OSD_SIDEBAR_SCROLL_HOME_DISTANCE:
#if defined(USE_GPS)
offset = GPS_distanceToHome;
#else
offset = 0;
#endif
// Move 1 char for every 5m
steps = offset / 5;

19
src/main/io/rangefinder_benewake.c
View file

@ -55,15 +55,20 @@ typedef struct __attribute__((packed)) {
#define BENEWAKE_PACKET_SIZE sizeof(tfminiPacket_t)
#define BENEWAKE_MIN_QUALITY 0
#define BENEWAKE_TIMEOUT_MS 200 // 200ms
static serialPort_t * serialPort = NULL;
static serialPortConfig_t * portConfig;
static uint8_t buffer[BENEWAKE_PACKET_SIZE];
static unsigned bufferPtr;
static timeMs_t lastProtocolActivityMs;
static bool hasNewData = false;
static int32_t sensorData = RANGEFINDER_NO_NEW_DATA;
// TFmini command to initiate 100Hz sampling
static const uint8_t initCmd100Hz[] = { 0x42, 0x57, 0x02, 0x00, 0x00, 0x00, 0x01, 0x06 };
static bool benewakeRangefinderDetect(void)
{
portConfig = findSerialPortConfig(FUNCTION_RANGEFINDER);
@ -80,16 +85,27 @@ static void benewakeRangefinderInit(void)
return;
}
serialPort = openSerialPort(portConfig->identifier, FUNCTION_RANGEFINDER, NULL, NULL, baudRates[BAUD_115200], MODE_RX, SERIAL_NOT_INVERTED);
serialPort = openSerialPort(portConfig->identifier, FUNCTION_RANGEFINDER, NULL, NULL, 115200, MODE_RXTX, SERIAL_NOT_INVERTED);
if (!serialPort) {
return;
}
lastProtocolActivityMs = 0;
bufferPtr = 0;
}
static void benewakeRangefinderUpdate(void)
{
// Initialize the sensor
if (lastProtocolActivityMs == 0 || (millis() - lastProtocolActivityMs) > BENEWAKE_TIMEOUT_MS) {
// Initialize the sensor to do 100Hz sampling to make sure we get the most recent data always
serialWriteBuf(serialPort, initCmd100Hz, sizeof(initCmd100Hz));
// Send the init command every BENEWAKE_TIMEOUT_MS
lastProtocolActivityMs = millis();
}
// Process incoming bytes
tfminiPacket_t *tfminiPacket = (tfminiPacket_t *)buffer;
while (serialRxBytesWaiting(serialPort) > 0) {
uint8_t c = serialRead(serialPort);
@ -117,6 +133,7 @@ static void benewakeRangefinderUpdate(void)
// Valid packet
hasNewData = true;
sensorData = (tfminiPacket->distL << 0) | (tfminiPacket->distH << 8);
lastProtocolActivityMs = millis();
uint16_t qual = (tfminiPacket->strengthL << 0) | (tfminiPacket->strengthH << 8);

8
src/main/io/rangefinder_msp.c
View file

@ -37,14 +37,12 @@
#include "io/serial.h"
#if defined(USE_RANGEFINDER_MSP)
#include "drivers/rangefinder/rangefinder_virtual.h"
#include "drivers/time.h"
#include "io/rangefinder.h"
#include "msp/msp_protocol_v2_sensor_msg.h"
typedef struct __attribute__((packed)) {
uint8_t quality; // [0;255]
int32_t distanceMm; // Negative value for out of range
} mspRangefinderSensor_t;
static bool hasNewData = false;
static int32_t sensorData = RANGEFINDER_NO_NEW_DATA;
@ -76,7 +74,7 @@ static int32_t mspRangefinderGetDistance(void)
void mspRangefinderReceiveNewData(uint8_t * bufferPtr)
{
const mspRangefinderSensor_t * pkt = (const mspRangefinderSensor_t *)bufferPtr;
const mspSensorRangefinderDataMessage_t * pkt = (const mspSensorRangefinderDataMessage_t *)bufferPtr;
sensorData = pkt->distanceMm / 10;
hasNewData = true;

9
src/main/io/serial_4way.c
View file

@ -72,7 +72,7 @@
// *** change to adapt Revision
#define SERIAL_4WAY_VER_MAIN 20
#define SERIAL_4WAY_VER_SUB_1 (uint8_t) 0
#define SERIAL_4WAY_VER_SUB_2 (uint8_t) 03
#define SERIAL_4WAY_VER_SUB_2 (uint8_t) 05
#define SERIAL_4WAY_PROTOCOL_VER 107
// *** end
@ -326,14 +326,13 @@ uint16_t _crc_xmodem_update (uint16_t crc, uint8_t data) {
#define ATMEL_DEVICE_MATCH ((pDeviceInfo->words[0] == 0x9307) || (pDeviceInfo->words[0] == 0x930A) || \
(pDeviceInfo->words[0] == 0x930F) || (pDeviceInfo->words[0] == 0x940B))
#define SILABS_DEVICE_MATCH ((pDeviceInfo->words[0] == 0xF310)||(pDeviceInfo->words[0] ==0xF330) || \
#define SILABS_DEVICE_MATCH ((pDeviceInfo->words[0] == 0xF310)||(pDeviceInfo->words[0] == 0xF330) || \
(pDeviceInfo->words[0] == 0xF410) || (pDeviceInfo->words[0] == 0xF390) || \
(pDeviceInfo->words[0] == 0xF850) || (pDeviceInfo->words[0] == 0xE8B1) || \
(pDeviceInfo->words[0] == 0xE8B2))
#define ARM_DEVICE_MATCH ((pDeviceInfo->words[0] == 0x1F06) || \
(pDeviceInfo->words[0] == 0x3306) || (pDeviceInfo->words[0] == 0x3406) || (pDeviceInfo->words[0] == 0x3506) || \
(pDeviceInfo->words[0] == 0x2B06) || (pDeviceInfo->words[0] == 0x4706))
// BLHeli_32 MCU ID hi > 0x00 and < 0x90 / lo always = 0x06
#define ARM_DEVICE_MATCH ((pDeviceInfo->bytes[1] > 0x00) && (pDeviceInfo->bytes[1] < 0x90) && (pDeviceInfo->bytes[0] == 0x06))
static uint8_t CurrentInterfaceMode;

3
src/main/io/smartport_master.c
View file

@ -300,6 +300,9 @@ static void smartportMasterPoll(void)
break;
}
default: // should not happen
return;
}
}

6
src/main/msp/msp_protocol_v2_sensor.h
View file

@ -18,4 +18,8 @@
#define MSP2_IS_SENSOR_MESSAGE(x) ((x) >= 0x1F00 && (x) <= 0x1FFF)
#define MSP2_SENSOR_RANGEFINDER 0x1F01
#define MSP2_SENSOR_OPTIC_FLOW 0x1F02
#define MSP2_SENSOR_OPTIC_FLOW 0x1F02
#define MSP2_SENSOR_GPS 0x1F03
#define MSP2_SENSOR_COMPASS 0x1F04
#define MSP2_SENSOR_BAROMETER 0x1F05
#define MSP2_SENSOR_AIRSPEED 0x1F06

84
src/main/msp/msp_protocol_v2_sensor_msg.h Normal file
View file

@ -0,0 +1,84 @@
/*
* This file is part of INAV Project.
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
* You can obtain one at http://mozilla.org/MPL/2.0/.
*
* Alternatively, the contents of this file may be used under the terms
* of the GNU General Public License Version 3, as described below:
*
* This file is free software: you may copy, redistribute and/or modify
* it under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or (at your
* option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
* Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*/
#pragma once
typedef struct __attribute__((packed)) {
uint8_t quality; // [0;255]
int32_t motionX;
int32_t motionY;
} mspSensorOpflowDataMessage_t;
typedef struct __attribute__((packed)) {
uint8_t quality; // [0;255]
int32_t distanceMm; // Negative value for out of range
} mspSensorRangefinderDataMessage_t;
typedef struct __attribute__((packed)) {
uint8_t instance; // sensor instance number to support multi-sensor setups
uint16_t gpsWeek; // GPS week, 0xFFFF if not available
uint32_t msTOW;
uint8_t fixType;
uint8_t satellitesInView;
uint16_t horizontalPosAccuracy; // [cm]
uint16_t verticalPosAccuracy; // [cm]
uint16_t horizontalVelAccuracy; // [cm/s]
uint16_t hdop;
int32_t longitude;
int32_t latitude;
int32_t mslAltitude; // cm
int32_t nedVelNorth; // cm/s
int32_t nedVelEast;
int32_t nedVelDown;
uint16_t groundCourse; // deg * 100, 0..36000
uint16_t trueYaw; // deg * 100, values of 0..36000 are valid. 65535 = no data available
uint16_t year;
uint8_t month;
uint8_t day;
uint8_t hour;
uint8_t min;
uint8_t sec;
} mspSensorGpsDataMessage_t;
typedef struct __attribute__((packed)) {
uint8_t instance;
uint32_t timeMs;
float pressurePa;
int16_t temp; // centi-degrees C
} mspSensorBaroDataMessage_t;
typedef struct __attribute__((packed)) {
uint8_t instance;
uint32_t timeMs;
float diffPressurePa;
int16_t temp; // centi-degrees C
} mspSensorAirspeedDataMessage_t;
typedef struct __attribute__((packed)) {
uint8_t instance;
uint32_t timeMs;
int16_t magX; // mGauss, front
int16_t magY; // mGauss, right
int16_t magZ; // mGauss, down
} mspSensorCompassDataMessage_t;

59
src/main/navigation/navigation.c
View file

@ -83,11 +83,15 @@ PG_REGISTER_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig, PG_SAFE_HOME_CO
#endif
#if defined(USE_NAV)
// waypoint 254, 255 are special waypoints
STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_range);
#if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE)
PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0);
#endif
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 7);
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 8);
PG_RESET_TEMPLATE(navConfig_t, navConfig,
.general = {
@ -102,7 +106,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.rth_tail_first = 0,
.disarm_on_landing = 0,
.rth_allow_landing = NAV_RTH_ALLOW_LANDING_ALWAYS,
.auto_overrides_motor_stop = 1,
.nav_overrides_motor_stop = NOMS_ALL_NAV,
},
// General navigation parameters
@ -152,6 +156,8 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.max_throttle = 1700,
.min_throttle = 1200,
.pitch_to_throttle = 10, // pwm units per degree of pitch (10pwm units ~ 1% throttle)
.pitch_to_throttle_smooth = 0,
.pitch_to_throttle_thresh = 0,
.loiter_radius = 5000, // 50m
//Fixed wing landing
@ -165,6 +171,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.launch_idle_throttle = 1000, // Motor idle or MOTOR_STOP
.launch_motor_timer = 500, // ms
.launch_motor_spinup_time = 100, // ms, time to gredually increase throttle from idle to launch
.launch_end_time = 3000, // ms, time to gradually decrease/increase throttle and decrease pitch angle from launch to the current flight mode
.launch_min_time = 0, // ms, min time in launch mode
.launch_timeout = 5000, // ms, timeout for launch procedure
.launch_max_altitude = 0, // cm, altitude where to consider launch ended
@ -1881,13 +1888,13 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode)
// Control System Design, Lecture Notes for ME 155A by Karl Johan Åström (p.228)
// http://www.cds.caltech.edu/~murray/courses/cds101/fa02/caltech/astrom-ch6.pdf
float navPidApply3(
pidController_t *pid,
const float setpoint,
const float measurement,
const float dt,
const float outMin,
const float outMax,
const pidControllerFlags_e pidFlags,
pidController_t *pid,
const float setpoint,
const float measurement,
const float dt,
const float outMin,
const float outMax,
const pidControllerFlags_e pidFlags,
const float gainScaler,
const float dTermScaler
) {
@ -2413,7 +2420,7 @@ static navigationHomeFlags_t navigationActualStateHomeValidity(void)
/*******************************************************
* Is a safehome being used instead of the arming point?
*******************************************************/
bool isSafeHomeInUse(void)
bool isSafeHomeInUse(void)
{
return (safehome_used > -1 && safehome_used < MAX_SAFE_HOMES);
}
@ -2424,7 +2431,7 @@ bool isSafeHomeInUse(void)
**********************************************************/
bool foundNearbySafeHome(void)
{
safehome_used = -1;
safehome_used = -1;
fpVector3_t safeHome;
gpsLocation_t shLLH;
shLLH.alt = 0;
@ -2432,7 +2439,7 @@ bool foundNearbySafeHome(void)
shLLH.lat = safeHomeConfig(i)->lat;
shLLH.lon = safeHomeConfig(i)->lon;
geoConvertGeodeticToLocal(&safeHome, &posControl.gpsOrigin, &shLLH, GEO_ALT_RELATIVE);
safehome_distance = calculateDistanceToDestination(&safeHome);
safehome_distance = calculateDistanceToDestination(&safeHome);
if (safehome_distance < 20000) { // 200 m
safehome_used = i;
setHomePosition(&safeHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
@ -2792,6 +2799,20 @@ void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData)
wpData->lon = wpLLH.lon;
wpData->alt = wpLLH.alt;
}
// WP #254 - special waypoint - get desiredPosition that was set by ground control station if in 3D-guided mode
else if (wpNumber == 254) {
navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
if ((posControl.gpsOrigin.valid) && (navStateFlags & NAV_CTL_ALT) && (navStateFlags & NAV_CTL_POS)) {
gpsLocation_t wpLLH;
geoConvertLocalToGeodetic(&wpLLH, &posControl.gpsOrigin, &posControl.desiredState.pos);
wpData->lat = wpLLH.lat;
wpData->lon = wpLLH.lon;
wpData->alt = wpLLH.alt;
}
}
// WP #1 - #60 - common waypoints - pre-programmed mission
else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS)) {
if (wpNumber <= posControl.waypointCount) {
@ -2918,7 +2939,7 @@ bool saveNonVolatileWaypointList(void)
#if defined(USE_SAFE_HOME)
void resetSafeHomes(void)
void resetSafeHomes(void)
{
memset(safeHomeConfigMutable(0), 0, sizeof(navSafeHome_t) * MAX_SAFE_HOMES);
}
@ -3218,7 +3239,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
}
// Pilot-activated waypoint mission. Fall-back to RTH in case of no mission loaded
// Pilot-activated waypoint mission. Fall-back to RTH in case of no mission loaded
if (IS_RC_MODE_ACTIVE(BOXNAVWP)) {
if (FLIGHT_MODE(NAV_WP_MODE) || (canActivateWaypoint && canActivatePosHold && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME)))
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT;
@ -3628,10 +3649,16 @@ bool navigationIsExecutingAnEmergencyLanding(void)
return navGetCurrentStateFlags() & NAV_CTL_EMERG;
}
bool navigationIsControllingThrottle(void)
bool navigationInAutomaticThrottleMode(void)
{
navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
return ((stateFlags & (NAV_CTL_ALT | NAV_CTL_EMERG | NAV_CTL_LAUNCH | NAV_CTL_LAND)) && (getMotorStatus() != MOTOR_STOPPED_USER));
return (stateFlags & (NAV_CTL_ALT | NAV_CTL_EMERG | NAV_CTL_LAUNCH | NAV_CTL_LAND));
}
bool navigationIsControllingThrottle(void)
{
// Note that this makes a detour into mixer code to evaluate actual motor status
return navigationInAutomaticThrottleMode() && (getMotorStatus() != MOTOR_STOPPED_USER);
}
bool navigationIsFlyingAutonomousMode(void)

17
src/main/navigation/navigation.h
View file

@ -124,6 +124,12 @@ typedef enum {
NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR = 4,
} navArmingBlocker_e;
typedef enum {
NOMS_OFF,
NOMS_AUTO_ONLY,
NOMS_ALL_NAV
} navOverridesMotorStop_e;
typedef struct positionEstimationConfig_s {
uint8_t automatic_mag_declination;
uint8_t reset_altitude_type; // from nav_reset_type_e
@ -175,7 +181,7 @@ typedef struct navConfig_s {
uint8_t disarm_on_landing; //
uint8_t rth_allow_landing; // Enable landing as last stage of RTH. Use constants in navRTHAllowLanding_e.
uint8_t rth_climb_ignore_emerg; // Option to ignore GPS loss on initial climb stage of RTH
uint8_t auto_overrides_motor_stop; // Autonomous modes override motor_stop setting and user command to stop motor
uint8_t nav_overrides_motor_stop; // Autonomous modes override motor_stop setting and user command to stop motor
} flags;
uint8_t pos_failure_timeout; // Time to wait before switching to emergency landing (0 - disable)
@ -222,6 +228,8 @@ typedef struct navConfig_s {
uint16_t min_throttle; // Minimum allowed throttle in auto mode
uint16_t max_throttle; // Maximum allowed throttle in auto mode
uint8_t pitch_to_throttle; // Pitch angle (in deg) to throttle gain (in 1/1000's of throttle) (*10)
uint16_t pitch_to_throttle_smooth; // How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh.
uint8_t pitch_to_throttle_thresh; // Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees]
uint16_t loiter_radius; // Loiter radius when executing PH on a fixed wing
int8_t land_dive_angle;
uint16_t launch_velocity_thresh; // Velocity threshold for swing launch detection
@ -231,7 +239,8 @@ typedef struct navConfig_s {
uint16_t launch_throttle; // Launch throttle
uint16_t launch_motor_timer; // Time to wait before setting launch_throttle (ms)
uint16_t launch_motor_spinup_time; // Time to speed-up motors from idle to launch_throttle (ESC desync prevention)
uint16_t launch_min_time; // Minimum time in launch mode to prevent possible bump of the sticks from leaving launch mode early
uint16_t launch_end_time; // Time to make the transition from launch angle to leveled and throttle transition from launch throttle to the stick position
uint16_t launch_min_time; // Minimum time in launch mode to prevent possible bump of the sticks from leaving launch mode early
uint16_t launch_timeout; // Launch timeout to disable launch mode and swith to normal flight (ms)
uint16_t launch_max_altitude; // cm, altitude where to consider launch ended
uint8_t launch_climb_angle; // Target climb angle for launch (deg)
@ -462,7 +471,7 @@ bool loadNonVolatileWaypointList(void);
bool saveNonVolatileWaypointList(void);
float getFinalRTHAltitude(void);
int16_t fixedWingPitchToThrottleCorrection(int16_t pitch);
int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs);
/* Geodetic functions */
typedef enum {
@ -505,6 +514,7 @@ void abortForcedRTH(void);
rthState_e getStateOfForcedRTH(void);
/* Getter functions which return data about the state of the navigation system */
bool navigationInAutomaticThrottleMode(void);
bool navigationIsControllingThrottle(void);
bool isFixedWingAutoThrottleManuallyIncreased(void);
bool navigationIsFlyingAutonomousMode(void);
@ -517,6 +527,7 @@ bool navigationRTHAllowsLanding(void);
bool isNavLaunchEnabled(void);
bool isFixedWingLaunchDetected(void);
bool isFixedWingLaunchFinishedOrAborted(void);
const char * fixedWingLaunchStateMessage(void);
float calculateAverageSpeed(void);

47
src/main/navigation/navigation_fixedwing.c
View file

@ -51,8 +51,8 @@
#include "rx/rx.h"
// Base frequencies for smoothing pitch and roll
#define NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ 2.0f
// Base frequencies for smoothing pitch and roll
#define NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ 2.0f
#define NAV_FW_BASE_ROLL_CUTOFF_FREQUENCY_HZ 10.0f
// If we are going slower than NAV_FW_MIN_VEL_SPEED_BOOST - boost throttle to fight against the wind
@ -70,15 +70,24 @@ static bool isAutoThrottleManuallyIncreased = false;
static int32_t navHeadingError;
static int8_t loiterDirYaw = 1;
// Calculates the cutoff frequency for smoothing out roll/pitch commands
// Calculates the cutoff frequency for smoothing out roll/pitch commands
// control_smoothness valid range from 0 to 9
// resulting cutoff_freq ranging from baseFreq downwards to ~0.11Hz
// resulting cutoff_freq ranging from baseFreq downwards to ~0.11Hz
static float getSmoothnessCutoffFreq(float baseFreq)
{
uint16_t smoothness = 10 - navConfig()->fw.control_smoothness;
return 0.001f * baseFreq * (float)(smoothness*smoothness*smoothness) + 0.1f;
}
// Calculates the cutoff frequency for smoothing out pitchToThrottleCorrection
// pitch_to_throttle_smooth valid range from 0 to 9
// resulting cutoff_freq ranging from baseFreq downwards to ~0.01Hz
static float getPitchToThrottleSmoothnessCutoffFreq(float baseFreq)
{
uint16_t smoothness = 10 - navConfig()->fw.pitch_to_throttle_smooth;
return 0.001f * baseFreq * (float)(smoothness*smoothness*smoothness) + 0.01f;
}
/*-----------------------------------------------------------
* Altitude controller
*-----------------------------------------------------------*/
@ -205,7 +214,7 @@ static fpVector3_t virtualDesiredPosition;
static pt1Filter_t fwPosControllerCorrectionFilterState;
/*
* TODO Currently this function resets both FixedWing and Rover & Boat position controller
* TODO Currently this function resets both FixedWing and Rover & Boat position controller
*/
void resetFixedWingPositionController(void)
{
@ -300,9 +309,9 @@ float processHeadingYawController(timeDelta_t deltaMicros, int32_t navHeadingErr
const pidControllerFlags_e yawPidFlags = errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0;
const float yawAdjustment = navPidApply2(
&posControl.pids.fw_heading,
0,
applyDeadband(navHeadingError, navConfig()->fw.yawControlDeadband * 100),
&posControl.pids.fw_heading,
0,
applyDeadband(navHeadingError, navConfig()->fw.yawControlDeadband * 100),
US2S(deltaMicros),
-limit,
limit,
@ -461,9 +470,25 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs)
return throttleSpeedAdjustment;
}
int16_t fixedWingPitchToThrottleCorrection(int16_t pitch)
int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs)
{
return DECIDEGREES_TO_DEGREES(pitch) * navConfig()->fw.pitch_to_throttle;
static timeUs_t previousTimePitchToThrCorr = 0;
const timeDelta_t deltaMicrosPitchToThrCorr = currentTimeUs - previousTimePitchToThrCorr;
previousTimePitchToThrCorr = currentTimeUs;
static pt1Filter_t pitchToThrFilterState;
// Apply low-pass filter to pitch angle to smooth throttle correction
int16_t filteredPitch = (int16_t)pt1FilterApply4(&pitchToThrFilterState, pitch, getPitchToThrottleSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicrosPitchToThrCorr));
if (ABS(pitch - filteredPitch) > navConfig()->fw.pitch_to_throttle_thresh) {
// Unfiltered throttle correction outside of pitch deadband
return DECIDEGREES_TO_DEGREES(pitch) * navConfig()->fw.pitch_to_throttle;
}
else {
// Filtered throttle correction inside of pitch deadband
return DECIDEGREES_TO_DEGREES(filteredPitch) * navConfig()->fw.pitch_to_throttle;
}
}
void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs)
@ -485,7 +510,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
// PITCH >0 dive, <0 climb
int16_t pitchCorrection = constrain(posControl.rcAdjustment[PITCH], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle));
rcCommand[PITCH] = -pidAngleToRcCommand(pitchCorrection, pidProfile()->max_angle_inclination[FD_PITCH]);
int16_t throttleCorrection = fixedWingPitchToThrottleCorrection(pitchCorrection);
int16_t throttleCorrection = fixedWingPitchToThrottleCorrection(pitchCorrection, currentTimeUs);
#ifdef NAV_FIXED_WING_LANDING
if (navStateFlags & NAV_CTL_LAND) {

507
src/main/navigation/navigation_fw_launch.c
View file

@ -33,15 +33,6 @@
#include "config/feature.h"
#include "drivers/time.h"
#include "drivers/sensor.h"
#include "drivers/accgyro/accgyro.h"
#include "sensors/sensors.h"
#include "sensors/rangefinder.h"
#include "sensors/barometer.h"
#include "sensors/acceleration.h"
#include "sensors/boardalignment.h"
#include "sensors/compass.h"
#include "io/gps.h"
#include "io/beeper.h"
@ -57,25 +48,238 @@
#include "navigation/navigation.h"
#include "navigation/navigation_private.h"
typedef struct FixedWingLaunchState_s {
/* Launch detection */
timeUs_t launchDetectorPreviousUpdate;
timeUs_t launchDetectionTimeAccum;
bool launchDetected;
/* Launch progress */
timeUs_t launchStartedTime;
bool launchFinished;
bool motorControlAllowed;
} FixedWingLaunchState_t;
static EXTENDED_FASTRAM FixedWingLaunchState_t launchState;
#define COS_MAX_LAUNCH_ANGLE 0.70710678f // cos(45), just to be safe
#define SWING_LAUNCH_MIN_ROTATION_RATE DEGREES_TO_RADIANS(100) // expect minimum 100dps rotation rate
static void updateFixedWingLaunchDetector(timeUs_t currentTimeUs)
{
#define LAUNCH_MOTOR_IDLE_SPINUP_TIME 1500 // ms
#define UNUSED(x) ((void)(x))
#define FW_LAUNCH_MESSAGE_TEXT_WAIT_THROTTLE "RAISE THE THROTTLE"
#define FW_LAUNCH_MESSAGE_TEXT_WAIT_DETECTION "READY"
#define FW_LAUNCH_MESSAGE_TEXT_IN_PROGRESS "MOVE THE STICKS TO ABORT"
#define FW_LAUNCH_MESSAGE_TEXT_FINISHING "FINISHING"
typedef enum {
FW_LAUNCH_MESSAGE_TYPE_NONE = 0,
FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE,
FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION,
FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS,
FW_LAUNCH_MESSAGE_TYPE_FINISHING
} fixedWingLaunchMessage_t;
typedef enum {
FW_LAUNCH_EVENT_NONE = 0,
FW_LAUNCH_EVENT_SUCCESS,
FW_LAUNCH_EVENT_GOTO_DETECTION,
FW_LAUNCH_EVENT_ABORT,
FW_LAUNCH_EVENT_THROTTLE_LOW,
FW_LAUNCH_EVENT_COUNT
} fixedWingLaunchEvent_t;
typedef enum {
FW_LAUNCH_STATE_IDLE = 0,
FW_LAUNCH_STATE_WAIT_THROTTLE,
FW_LAUNCH_STATE_MOTOR_IDLE,
FW_LAUNCH_STATE_WAIT_DETECTION,
FW_LAUNCH_STATE_DETECTED,
FW_LAUNCH_STATE_MOTOR_DELAY,
FW_LAUNCH_STATE_MOTOR_SPINUP,
FW_LAUNCH_STATE_IN_PROGRESS,
FW_LAUNCH_STATE_FINISH,
FW_LAUNCH_STATE_COUNT
} fixedWingLaunchState_t;
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE(timeUs_t currentTimeUs);
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs_t currentTimeUs);
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs);
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeUs_t currentTimeUs);
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_DETECTED(timeUs_t currentTimeUs);
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t currentTimeUs);
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_t currentTimeUs);
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs);
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t currentTimeUs);
typedef struct fixedWingLaunchStateDescriptor_s {
fixedWingLaunchEvent_t (*onEntry)(timeUs_t currentTimeUs);
fixedWingLaunchState_t onEvent[FW_LAUNCH_EVENT_COUNT];
fixedWingLaunchMessage_t messageType;
} fixedWingLaunchStateDescriptor_t;
typedef struct fixedWingLaunchData_s {
timeUs_t currentStateTimeUs;
fixedWingLaunchState_t currentState;
uint8_t pitchAngle; // used to smooth the transition of the pitch angle
} fixedWingLaunchData_t;
static EXTENDED_FASTRAM fixedWingLaunchData_t fwLaunch;
static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE_COUNT] = {
[FW_LAUNCH_STATE_IDLE] = {
.onEntry = fwLaunchState_FW_LAUNCH_STATE_IDLE,
.onEvent = {
},
.messageType = FW_LAUNCH_MESSAGE_TYPE_NONE
},
[FW_LAUNCH_STATE_WAIT_THROTTLE] = {
.onEntry = fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE,
.onEvent = {
[FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_MOTOR_IDLE,
[FW_LAUNCH_EVENT_GOTO_DETECTION] = FW_LAUNCH_STATE_WAIT_DETECTION
},
.messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE
},
[FW_LAUNCH_STATE_MOTOR_IDLE] = {
.onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE,
.onEvent = {
[FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_WAIT_DETECTION,
[FW_LAUNCH_EVENT_THROTTLE_LOW] = FW_LAUNCH_STATE_WAIT_THROTTLE
},
.messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE
},
[FW_LAUNCH_STATE_WAIT_DETECTION] = {
.onEntry = fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION,
.onEvent = {
[FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_DETECTED,
[FW_LAUNCH_EVENT_THROTTLE_LOW] = FW_LAUNCH_STATE_WAIT_THROTTLE
},
.messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION
},
[FW_LAUNCH_STATE_DETECTED] = {
.onEntry = fwLaunchState_FW_LAUNCH_STATE_DETECTED,
.onEvent = {
// waiting for the navigation to move on the next state FW_LAUNCH_STATE_MOTOR_DELAY
},
.messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION
},
[FW_LAUNCH_STATE_MOTOR_DELAY] = {
.onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY,
.onEvent = {
[FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_MOTOR_SPINUP,
[FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_IDLE
},
.messageType = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS
},
[FW_LAUNCH_STATE_MOTOR_SPINUP] = {
.onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP,
.onEvent = {
[FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_IN_PROGRESS,
[FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_IDLE
},
.messageType = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS
},
[FW_LAUNCH_STATE_IN_PROGRESS] = {
.onEntry = fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS,
.onEvent = {
[FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_FINISH,
},
.messageType = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS
},
[FW_LAUNCH_STATE_FINISH] = {
.onEntry = fwLaunchState_FW_LAUNCH_STATE_FINISH,
.onEvent = {
[FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_IDLE
},
.messageType = FW_LAUNCH_MESSAGE_TYPE_FINISHING
}
};
/* Current State Handlers */
static timeMs_t currentStateElapsedMs(timeUs_t currentTimeUs) {
return US2MS(currentTimeUs - fwLaunch.currentStateTimeUs);
}
static void setCurrentState(fixedWingLaunchState_t nextState, timeUs_t currentTimeUs) {
fwLaunch.currentState = nextState;
fwLaunch.currentStateTimeUs = currentTimeUs;
}
/* Wing control Helpers */
static bool isThrottleIdleEnabled(void) {
return navConfig()->fw.launch_idle_throttle > getThrottleIdleValue();
}
static void applyThrottleIdle(void) {
if (isThrottleIdleEnabled()) {
rcCommand[THROTTLE] = navConfig()->fw.launch_idle_throttle;
} else {
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped
rcCommand[THROTTLE] = getThrottleIdleValue(); // If MOTOR_STOP is disabled, motors will spin at minthrottle
}
}
static inline bool isThrottleLow(void) {
return calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW;
}
static inline bool isLaunchMaxAltitudeReached(void) {
return (navConfig()->fw.launch_max_altitude > 0) && (getEstimatedActualPosition(Z) >= navConfig()->fw.launch_max_altitude);
}
static inline bool areSticksMoved(timeMs_t initialTime, timeUs_t currentTimeUs) {
return (initialTime + currentStateElapsedMs(currentTimeUs)) > navConfig()->fw.launch_min_time && areSticksDeflectedMoreThanPosHoldDeadband();
}
static void resetPidsIfNeeded(void) {
// Until motors are started don't use PID I-term and reset TPA filter
if (fwLaunch.currentState < FW_LAUNCH_STATE_MOTOR_SPINUP) {
pidResetErrorAccumulators();
pidResetTPAFilter();
}
}
static void updateRcCommand(void) {
// lock roll and yaw and apply needed pitch angle
rcCommand[ROLL] = 0;
rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(fwLaunch.pitchAngle), pidProfile()->max_angle_inclination[FD_PITCH]);
rcCommand[YAW] = 0;
}
/* onEntry state handlers */
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE(timeUs_t currentTimeUs) {
UNUSED(currentTimeUs);
return FW_LAUNCH_EVENT_NONE;
}
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs_t currentTimeUs) {
UNUSED(currentTimeUs);
if (!isThrottleLow()) {
if (isThrottleIdleEnabled()) {
return FW_LAUNCH_EVENT_SUCCESS;
} else {
fwLaunch.pitchAngle = navConfig()->fw.launch_climb_angle;
return FW_LAUNCH_EVENT_GOTO_DETECTION;
}
}
fwLaunch.pitchAngle = 0;
return FW_LAUNCH_EVENT_NONE;
}
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs) {
if (isThrottleLow()) {
return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE
}
const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs);
if (elapsedTimeMs > LAUNCH_MOTOR_IDLE_SPINUP_TIME) {
applyThrottleIdle();
return FW_LAUNCH_EVENT_SUCCESS;
} else {
rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle);
fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, 0, navConfig()->fw.launch_climb_angle);
}
return FW_LAUNCH_EVENT_NONE;
}
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeUs_t currentTimeUs) {
if (isThrottleLow()) {
return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE
}
const float swingVelocity = (fabsf(imuMeasuredRotationBF.z) > SWING_LAUNCH_MIN_ROTATION_RATE) ? (imuMeasuredAccelBF.y / imuMeasuredRotationBF.z) : 0;
const bool isForwardAccelerationHigh = (imuMeasuredAccelBF.x > navConfig()->fw.launch_accel_thresh);
const bool isAircraftAlmostLevel = (calculateCosTiltAngle() >= cos_approx(DEGREES_TO_RADIANS(navConfig()->fw.launch_max_angle)));
@ -83,156 +287,149 @@ static void updateFixedWingLaunchDetector(timeUs_t currentTimeUs)
const bool isBungeeLaunched = isForwardAccelerationHigh && isAircraftAlmostLevel;
const bool isSwingLaunched = (swingVelocity > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0);
applyThrottleIdle();
if (isBungeeLaunched || isSwingLaunched) {
launchState.launchDetectionTimeAccum += (currentTimeUs - launchState.launchDetectorPreviousUpdate);
launchState.launchDetectorPreviousUpdate = currentTimeUs;
if (launchState.launchDetectionTimeAccum >= MS2US((uint32_t)navConfig()->fw.launch_time_thresh)) {
launchState.launchDetected = true;
if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_time_thresh) {
return FW_LAUNCH_EVENT_SUCCESS; // the launch is detected now, go to FW_LAUNCH_STATE_DETECTED
}
} else {
fwLaunch.currentStateTimeUs = currentTimeUs; // reset the state counter
}
else {
launchState.launchDetectorPreviousUpdate = currentTimeUs;
launchState.launchDetectionTimeAccum = 0;
return FW_LAUNCH_EVENT_NONE;
}
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_DETECTED(timeUs_t currentTimeUs) {
UNUSED(currentTimeUs);
// waiting for the navigation to move it to next step FW_LAUNCH_STATE_MOTOR_DELAY
applyThrottleIdle();
return FW_LAUNCH_EVENT_NONE;
}
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t currentTimeUs) {
applyThrottleIdle();
if (areSticksMoved(0, currentTimeUs)) {
return FW_LAUNCH_EVENT_ABORT; // jump to FW_LAUNCH_STATE_IDLE
}
}
void resetFixedWingLaunchController(timeUs_t currentTimeUs)
{
launchState.launchDetectorPreviousUpdate = currentTimeUs;
launchState.launchDetectionTimeAccum = 0;
launchState.launchStartedTime = 0;
launchState.launchDetected = false;
launchState.launchFinished = false;
launchState.motorControlAllowed = false;
}
bool isFixedWingLaunchDetected(void)
{
return launchState.launchDetected;
}
void enableFixedWingLaunchController(timeUs_t currentTimeUs)
{
launchState.launchStartedTime = currentTimeUs;
launchState.motorControlAllowed = true;
}
bool isFixedWingLaunchFinishedOrAborted(void)
{
return launchState.launchFinished;
}
void abortFixedWingLaunch(void)
{
launchState.launchFinished = true;
}
#define LAUNCH_MOTOR_IDLE_SPINUP_TIME 1500 //ms
static void applyFixedWingLaunchIdleLogic(void)
{
// Until motors are started don't use PID I-term
pidResetErrorAccumulators();
// We're not flying yet, reset TPA filter
pidResetTPAFilter();
// Throttle control logic
if (navConfig()->fw.launch_idle_throttle <= getThrottleIdleValue())
{
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped
rcCommand[THROTTLE] = getThrottleIdleValue(); // If MOTOR_STOP is disabled, motors will spin at minthrottle
if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_motor_timer) {
return FW_LAUNCH_EVENT_SUCCESS;
}
else
{
static float timeThrottleRaisedMs;
if (calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW)
{
timeThrottleRaisedMs = millis();
}
else
{
const float timeSinceMotorStartMs = MIN(millis() - timeThrottleRaisedMs, LAUNCH_MOTOR_IDLE_SPINUP_TIME);
rcCommand[THROTTLE] = scaleRangef(timeSinceMotorStartMs,
0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME,
getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle);
}
return FW_LAUNCH_EVENT_NONE;
}
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_t currentTimeUs) {
if (areSticksMoved(navConfig()->fw.launch_motor_timer, currentTimeUs)) {
return FW_LAUNCH_EVENT_ABORT; // jump to FW_LAUNCH_STATE_IDLE
}
const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs);
const uint16_t motorSpinUpMs = navConfig()->fw.launch_motor_spinup_time;
const uint16_t launchThrottle = navConfig()->fw.launch_throttle;
if (elapsedTimeMs > motorSpinUpMs) {
rcCommand[THROTTLE] = launchThrottle;
return FW_LAUNCH_EVENT_SUCCESS;
} else {
const uint16_t minIdleThrottle = MAX(getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle);
rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, motorSpinUpMs, minIdleThrottle, launchThrottle);
}
return FW_LAUNCH_EVENT_NONE;
}
static inline bool isFixedWingLaunchMaxAltitudeReached(void)
{
return (navConfig()->fw.launch_max_altitude > 0) && (getEstimatedActualPosition(Z) >= navConfig()->fw.launch_max_altitude);
}
static inline bool isLaunchModeMinTimeElapsed(float timeSinceLaunchMs)
{
return timeSinceLaunchMs > navConfig()->fw.launch_min_time;
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs) {
rcCommand[THROTTLE] = navConfig()->fw.launch_throttle;
if (isLaunchMaxAltitudeReached()) {
return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISH state
}
if (areSticksMoved(navConfig()->fw.launch_motor_timer + navConfig()->fw.launch_motor_spinup_time, currentTimeUs)) {
return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISH state
}
if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_timeout) {
return FW_LAUNCH_EVENT_SUCCESS; // launch timeout. go to FW_LAUNCH_STATE_FINISH
}
return FW_LAUNCH_EVENT_NONE;
}
static inline bool isLaunchModeMaxTimeElapsed(float timeSinceLaunchMs)
{
return timeSinceLaunchMs >= navConfig()->fw.launch_timeout;
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t currentTimeUs) {
const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs);
const timeMs_t endTimeMs = navConfig()->fw.launch_end_time;
if (elapsedTimeMs > endTimeMs) {
return FW_LAUNCH_EVENT_SUCCESS;
} else {
// make a smooth transition from the launch state to the current state for throttle and the pitch angle
rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_throttle, rcCommand[THROTTLE]);
fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_climb_angle, rcCommand[PITCH]);
}
return FW_LAUNCH_EVENT_NONE;
}
static inline bool isFixedWingLaunchCompleted(float timeSinceLaunchMs)
{
return (isLaunchModeMaxTimeElapsed(timeSinceLaunchMs)) || ((isLaunchModeMinTimeElapsed(timeSinceLaunchMs)) && (areSticksDeflectedMoreThanPosHoldDeadband())) || isFixedWingLaunchMaxAltitudeReached();
}
// Public methods ---------------------------------------------------------------
void applyFixedWingLaunchController(timeUs_t currentTimeUs)
{
void applyFixedWingLaunchController(timeUs_t currentTimeUs) {
// Called at PID rate
if (launchState.launchDetected) {
bool applyLaunchIdleLogic = true;
if (launchState.motorControlAllowed) {
// If launch detected we are in launch procedure - control airplane
const float timeElapsedSinceLaunchMs = US2MS(currentTimeUs - launchState.launchStartedTime);
launchState.launchFinished = isFixedWingLaunchCompleted(timeElapsedSinceLaunchMs);
// Motor control enabled
if (timeElapsedSinceLaunchMs >= navConfig()->fw.launch_motor_timer) {
// Don't apply idle logic anymore
applyLaunchIdleLogic = false;
// Increase throttle gradually over `launch_motor_spinup_time` milliseconds
if (navConfig()->fw.launch_motor_spinup_time > 0) {
const float timeSinceMotorStartMs = constrainf(timeElapsedSinceLaunchMs - navConfig()->fw.launch_motor_timer, 0.0f, navConfig()->fw.launch_motor_spinup_time);
const uint16_t minIdleThrottle = MAX(getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle);
rcCommand[THROTTLE] = scaleRangef(timeSinceMotorStartMs,
0.0f, navConfig()->fw.launch_motor_spinup_time,
minIdleThrottle, navConfig()->fw.launch_throttle);
}
else {
rcCommand[THROTTLE] = navConfig()->fw.launch_throttle;
}
}
}
if (applyLaunchIdleLogic) {
// Launch idle logic - low throttle and zero out PIDs
applyFixedWingLaunchIdleLogic();
// process the current state, set the next state or exit if FW_LAUNCH_EVENT_NONE
while (launchStateMachine[fwLaunch.currentState].onEntry) {
fixedWingLaunchEvent_t newEvent = launchStateMachine[fwLaunch.currentState].onEntry(currentTimeUs);
if (newEvent == FW_LAUNCH_EVENT_NONE) {
break;
}
setCurrentState(launchStateMachine[fwLaunch.currentState].onEvent[newEvent], currentTimeUs);
}
else {
// We are waiting for launch - update launch detector
updateFixedWingLaunchDetector(currentTimeUs);
// Launch idle logic - low throttle and zero out PIDs
applyFixedWingLaunchIdleLogic();
}
resetPidsIfNeeded();
updateRcCommand();
// Control beeper
if (!launchState.launchFinished) {
if (fwLaunch.currentState == FW_LAUNCH_STATE_WAIT_THROTTLE) {
beeper(BEEPER_LAUNCH_MODE_LOW_THROTTLE);
} else {
beeper(BEEPER_LAUNCH_MODE_ENABLED);
}
}
// Lock out controls
rcCommand[ROLL] = 0;
rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(navConfig()->fw.launch_climb_angle), pidProfile()->max_angle_inclination[FD_PITCH]);
rcCommand[YAW] = 0;
void resetFixedWingLaunchController(timeUs_t currentTimeUs) {
setCurrentState(FW_LAUNCH_STATE_WAIT_THROTTLE, currentTimeUs);
}
bool isFixedWingLaunchDetected(void) {
return fwLaunch.currentState == FW_LAUNCH_STATE_DETECTED;
}
void enableFixedWingLaunchController(timeUs_t currentTimeUs) {
setCurrentState(FW_LAUNCH_STATE_MOTOR_DELAY, currentTimeUs);
}
bool isFixedWingLaunchFinishedOrAborted(void) {
return fwLaunch.currentState == FW_LAUNCH_STATE_IDLE;
}
void abortFixedWingLaunch(void) {
setCurrentState(FW_LAUNCH_STATE_IDLE, 0);
}
const char * fixedWingLaunchStateMessage(void) {
switch (launchStateMachine[fwLaunch.currentState].messageType) {
case FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE:
return FW_LAUNCH_MESSAGE_TEXT_WAIT_THROTTLE;
case FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION:
return FW_LAUNCH_MESSAGE_TEXT_WAIT_DETECTION;
case FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS:
return FW_LAUNCH_MESSAGE_TEXT_IN_PROGRESS;
case FW_LAUNCH_MESSAGE_TYPE_FINISHING:
return FW_LAUNCH_MESSAGE_TEXT_FINISHING;
default:
return NULL;
}
}
#endif

29
src/main/programming/logic_condition.c
View file

@ -36,6 +36,7 @@
#include "fc/fc_core.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "fc/rc_modes.h"
#include "navigation/navigation.h"
#include "sensors/battery.h"
#include "sensors/pitotmeter.h"
@ -478,6 +479,26 @@ static int logicConditionGetFlightOperandValue(int operand) {
return NAV_Status.activeWpAction;
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE: //in m
return constrain(sqrtf(sq(GPS_distanceToHome) + sq(getEstimatedActualPosition(Z)/100)), 0, INT16_MAX);
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ:
#ifdef USE_SERIALRX_CRSF
return rxLinkStatistics.uplinkLQ;
#else
return 0;
#endif
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_SNR:
#ifdef USE_SERIALRX_CRSF
return rxLinkStatistics.uplinkSNR;
#else
return 0;
#endif
break;
default:
return 0;
break;
@ -524,6 +545,14 @@ static int logicConditionGetFlightModeOperandValue(int operand) {
return (bool) FLIGHT_MODE(AIRMODE_ACTIVE);
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER1:
return IS_RC_MODE_ACTIVE(BOXUSER1);
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER2:
return IS_RC_MODE_ACTIVE(BOXUSER2);
break;
default:
return 0;
break;

24
src/main/programming/logic_condition.h
View file

@ -112,18 +112,24 @@ typedef enum {
LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW, // 28
LOGIC_CONDITION_OPERAND_FLIGHT_WAYPOINT_INDEX, // 29
LOGIC_CONDITION_OPERAND_FLIGHT_WAYPOINT_ACTION, // 30
LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE, // 31
LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ, // 32
LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_SNR, // 33
} logicFlightOperands_e;
typedef enum {
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_FAILSAFE,
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_MANUAL,
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_RTH,
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_POSHOLD,
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_CRUISE,
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ALTHOLD,
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ANGLE,
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_HORIZON,
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_AIR,
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_FAILSAFE, // 0
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_MANUAL, // 1
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_RTH, // 2
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_POSHOLD, // 3
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_CRUISE, // 4
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ALTHOLD, // 5
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ANGLE, // 6
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_HORIZON, // 7
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_AIR, // 8
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER1, // 9
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER2, // 10
} logicFlightModeOperands_e;
typedef enum {

17
src/main/rx/rx.c
View file

@ -61,6 +61,7 @@
#include "rx/rx_spi.h"
#include "rx/sbus.h"
#include "rx/spektrum.h"
#include "rx/srxl2.h"
#include "rx/sumd.h"
#include "rx/sumh.h"
#include "rx/uib_rx.h"
@ -91,6 +92,7 @@ static bool mspOverrideDataProcessingRequired = false;
static bool rxSignalReceived = false;
static bool rxFlightChannelsValid = false;
static bool rxIsInFailsafeMode = true;
static uint8_t rxChannelCount;
static timeUs_t rxNextUpdateAtUs = 0;
static timeUs_t needRxSignalBefore = 0;
@ -141,6 +143,8 @@ PG_RESET_TEMPLATE(rxConfig_t, rxConfig,
.mspOverrideChannels = 15,
#endif
.rssi_source = RSSI_SOURCE_AUTO,
.srxl2_unit_id = 1,
.srxl2_baud_fast = 1,
);
void resetAllRxChannelRangeConfigurations(void)
@ -187,6 +191,11 @@ bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
{
bool enabled = false;
switch (rxConfig->serialrx_provider) {
#ifdef USE_SERIALRX_SRXL2
case SERIALRX_SRXL2:
enabled = srxl2RxInit(rxConfig, rxRuntimeConfig);
break;
#endif
#ifdef USE_SERIALRX_SPEKTRUM
case SERIALRX_SPEKTRUM1024:
case SERIALRX_SPEKTRUM2048:
@ -347,6 +356,8 @@ void rxInit(void)
mspOverrideInit();
}
#endif
rxChannelCount = MIN(MAX_SUPPORTED_RC_CHANNEL_COUNT, rxRuntimeConfig.channelCount);
}
void rxUpdateRSSISource(void)
@ -516,7 +527,7 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
rxFlightChannelsValid = true;
// Read and process channel data
for (int channel = 0; channel < rxRuntimeConfig.channelCount; channel++) {
for (int channel = 0; channel < rxChannelCount; channel++) {
const uint8_t rawChannel = calculateChannelRemapping(rxConfig()->rcmap, REMAPPABLE_CHANNEL_COUNT, channel);
// sample the channel
@ -549,11 +560,11 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
// If receiver is in failsafe (not receiving signal or sending invalid channel values) - last good input values are retained
if (rxFlightChannelsValid && rxSignalReceived) {
if (rxRuntimeConfig.requireFiltering) {
for (int channel = 0; channel < rxRuntimeConfig.channelCount; channel++) {
for (int channel = 0; channel < rxChannelCount; channel++) {
rcChannels[channel].data = applyChannelFiltering(channel, rcStaging[channel]);
}
} else {
for (int channel = 0; channel < rxRuntimeConfig.channelCount; channel++) {
for (int channel = 0; channel < rxChannelCount; channel++) {
rcChannels[channel].data = rcStaging[channel];
}
}

3
src/main/rx/rx.h
View file

@ -82,6 +82,7 @@ typedef enum {
SERIALRX_FPORT = 10,
SERIALRX_SBUS_FAST = 11,
SERIALRX_FPORT2 = 12,
SERIALRX_SRXL2 = 13,
} rxSerialReceiverType_e;
#define MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT 16
@ -128,6 +129,8 @@ typedef struct rxConfig_s {
uint8_t rcFilterFrequency; // RC filter cutoff frequency (smoothness vs response sharpness)
uint16_t mspOverrideChannels; // Channels to override with MSP RC when BOXMSPRCOVERRIDE is active
uint8_t rssi_source;
uint8_t srxl2_unit_id;
uint8_t srxl2_baud_fast;
} rxConfig_t;
PG_DECLARE(rxConfig_t, rxConfig);

129
src/main/rx/spektrum.c
View file

@ -15,10 +15,13 @@
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <string.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include "common/maths.h"
#include "platform.h"
#ifdef USE_SERIALRX_SPEKTRUM
@ -44,19 +47,13 @@
#include "rx/spektrum.h"
// driver for spektrum satellite receiver / sbus
#define SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT 12
#define SPEKTRUM_2048_CHANNEL_COUNT 12
#define SPEKTRUM_1024_CHANNEL_COUNT 7
#define SPEK_FRAME_SIZE 16
#define SPEKTRUM_NEEDED_FRAME_INTERVAL 5000
#define SPEKTRUM_BAUDRATE 115200
#define SPEKTRUM_TELEMETRY_FRAME_DELAY_US 1000 // Gap between received Rc frame and transmited TM frame
#define SPEKTRUM_MAX_FADE_PER_SEC 40
#define SPEKTRUM_FADE_REPORTS_PER_SEC 2
bool srxlEnabled = false;
static uint8_t spek_chan_shift;
static uint8_t spek_chan_mask;
static bool rcFrameComplete = false;
@ -81,6 +78,10 @@ static IO_t BindPin = DEFIO_IO(NONE);
static IO_t BindPlug = DEFIO_IO(NONE);
#endif
#if defined(USE_TELEMETRY_SRXL)
static uint8_t telemetryBuf[SRXL_FRAME_SIZE_MAX];
static uint8_t telemetryBufLen = 0;
#endif
// Receive ISR callback
static void spektrumDataReceive(uint16_t c, void *rxCallbackData)
@ -116,46 +117,67 @@ static uint8_t spektrumFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
{
UNUSED(rxRuntimeConfig);
if (!rcFrameComplete) {
return RX_FRAME_PENDING;
}
#if defined(USE_TELEMETRY_SRXL)
static timeUs_t telemetryFrameRequestedUs = 0;
rcFrameComplete = false;
timeUs_t currentTimeUs = micros();
#endif
// Fetch the fade count
const uint16_t fade = (spekFrame[0] << 8) + spekFrame[1];
const uint32_t current_secs = millis() / (1000 / SPEKTRUM_FADE_REPORTS_PER_SEC);
uint8_t result = RX_FRAME_PENDING;
if (spek_fade_last_sec == 0) {
// This is the first frame status received.
spek_fade_last_sec_count = fade;
spek_fade_last_sec = current_secs;
} else if (spek_fade_last_sec != current_secs) {
// If the difference is > 1, then we missed several seconds worth of frames and
// should just throw out the fade calc (as it's likely a full signal loss).
if ((current_secs - spek_fade_last_sec) == 1) {
if (rssi_channel != 0) {
if (spekHiRes)
spekChannelData[rssi_channel] = 2048 - ((fade - spek_fade_last_sec_count) * 2048 / (SPEKTRUM_MAX_FADE_PER_SEC / SPEKTRUM_FADE_REPORTS_PER_SEC));
else
spekChannelData[rssi_channel] = 1024 - ((fade - spek_fade_last_sec_count) * 1024 / (SPEKTRUM_MAX_FADE_PER_SEC / SPEKTRUM_FADE_REPORTS_PER_SEC));
if (rcFrameComplete) {
rcFrameComplete = false;
// Fetch the fade count
const uint16_t fade = (spekFrame[0] << 8) + spekFrame[1];
const uint32_t current_secs = millis() / (1000 / SPEKTRUM_FADE_REPORTS_PER_SEC);
if (spek_fade_last_sec == 0) {
// This is the first frame status received.
spek_fade_last_sec_count = fade;
spek_fade_last_sec = current_secs;
} else if (spek_fade_last_sec != current_secs) {
// If the difference is > 1, then we missed several seconds worth of frames and
// should just throw out the fade calc (as it's likely a full signal loss).
if ((current_secs - spek_fade_last_sec) == 1) {
if (rssi_channel != 0) {
if (spekHiRes)
spekChannelData[rssi_channel] = 2048 - ((fade - spek_fade_last_sec_count) * 2048 / (SPEKTRUM_MAX_FADE_PER_SEC / SPEKTRUM_FADE_REPORTS_PER_SEC));
else
spekChannelData[rssi_channel] = 1024 - ((fade - spek_fade_last_sec_count) * 1024 / (SPEKTRUM_MAX_FADE_PER_SEC / SPEKTRUM_FADE_REPORTS_PER_SEC));
}
}
spek_fade_last_sec_count = fade;
spek_fade_last_sec = current_secs;
}
for (int b = 3; b < SPEK_FRAME_SIZE; b += 2) {
const uint8_t spekChannel = 0x0F & (spekFrame[b - 1] >> spek_chan_shift);
if (spekChannel < rxRuntimeConfigPtr->channelCount && spekChannel < SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT) {
if (rssi_channel == 0 || spekChannel != rssi_channel) {
spekChannelData[spekChannel] = ((uint32_t)(spekFrame[b - 1] & spek_chan_mask) << 8) + spekFrame[b];
}
}
}
spek_fade_last_sec_count = fade;
spek_fade_last_sec = current_secs;
}
for (int b = 3; b < SPEK_FRAME_SIZE; b += 2) {
const uint8_t spekChannel = 0x0F & (spekFrame[b - 1] >> spek_chan_shift);
if (spekChannel < rxRuntimeConfigPtr->channelCount && spekChannel < SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT) {
if (rssi_channel == 0 || spekChannel != rssi_channel) {
spekChannelData[spekChannel] = ((uint32_t)(spekFrame[b - 1] & spek_chan_mask) << 8) + spekFrame[b];
}
#if defined(USE_TELEMETRY_SRXL)
if (srxlEnabled && (spekFrame[2] & 0x80) == 0) {
telemetryFrameRequestedUs = currentTimeUs;
}
}
#endif
return RX_FRAME_COMPLETE;
result = RX_FRAME_COMPLETE;
}
#if defined(USE_TELEMETRY_SRXL)
if (telemetryBufLen && telemetryFrameRequestedUs && cmpTimeUs(currentTimeUs, telemetryFrameRequestedUs) >= SPEKTRUM_TELEMETRY_FRAME_DELAY_US) {
telemetryFrameRequestedUs = 0;
result = (result & ~RX_FRAME_PENDING) | RX_FRAME_PROCESSING_REQUIRED;
}
#endif
return result;
}
static uint16_t spektrumReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
@ -251,6 +273,25 @@ void spektrumBind(rxConfig_t *rxConfig)
}
#endif // SPEKTRUM_BIND
#if defined(USE_TELEMETRY_SRXL)
bool srxlTelemetryBufferEmpty(void)
{
if (telemetryBufLen == 0) {
return true;
} else {
return false;
}
}
void srxlRxWriteTelemetryData(const void *data, int len)
{
len = MIN(len, (int)sizeof(telemetryBuf));
memcpy(telemetryBuf, data, len);
telemetryBufLen = len;
}
#endif
bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{
rxRuntimeConfigPtr = rxRuntimeConfig;
@ -310,4 +351,10 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
return serialPort != NULL;
}
bool srxlRxIsActive(void)
{
return serialPort != NULL;
}
#endif // USE_SERIALRX_SPEKTRUM

22
src/main/rx/spektrum.h
View file

@ -17,8 +17,26 @@
#pragma once
#define SPEKTRUM_SAT_BIND_DISABLED 0
#define SPEKTRUM_SAT_BIND_MAX 10
#define SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT 12
#define SPEKTRUM_2048_CHANNEL_COUNT 12
#define SPEKTRUM_1024_CHANNEL_COUNT 7
#define SPEKTRUM_SAT_BIND_DISABLED 0
#define SPEKTRUM_SAT_BIND_MAX 10
#define SPEK_FRAME_SIZE 16
#define SRXL_FRAME_OVERHEAD 5
#define SRXL_FRAME_SIZE_MAX (SPEK_FRAME_SIZE + SRXL_FRAME_OVERHEAD)
#define SPEKTRUM_NEEDED_FRAME_INTERVAL 5000
#define SPEKTRUM_BAUDRATE 115200
extern bool srxlEnabled;
void spektrumBind(rxConfig_t *rxConfig);
bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
bool srxlTelemetryBufferEmpty(void);
void srxlRxWriteTelemetryData(const void *data, int len);
bool srxlRxIsActive(void);

580
src/main/rx/srxl2.c Normal file
View file

@ -0,0 +1,580 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#include <string.h>
#include "platform.h"
#ifdef USE_SERIALRX_SRXL2
#include "common/crc.h"
#include "common/maths.h"
#include "common/streambuf.h"
#include "drivers/nvic.h"
#include "drivers/time.h"
#include "drivers/serial.h"
#include "drivers/serial_uart.h"
#include "io/serial.h"
#include "rx/srxl2.h"
#include "rx/srxl2_types.h"
#ifndef SRXL2_DEBUG
#define SRXL2_DEBUG 0
#endif
#if SRXL2_DEBUG
//void cliPrintf(const char *format, ...);
//#define DEBUG_PRINTF(format, ...) cliPrintf(format, __VA_ARGS__)
#define DEBUG_PRINTF(...) //Temporary until a better debug printf can be included
#else
#define DEBUG_PRINTF(...)
#endif
#define SRXL2_MAX_CHANNELS 32
#define SRXL2_FRAME_PERIOD_US 11000 // 5500 for DSMR
#define SRXL2_CHANNEL_SHIFT 5
#define SRXL2_CHANNEL_CENTER 0x8000
#define SRXL2_PORT_BAUDRATE_DEFAULT 115200
#define SRXL2_PORT_BAUDRATE_HIGH 400000
#define SRXL2_PORT_OPTIONS (SERIAL_STOPBITS_1 | SERIAL_PARITY_NO | SERIAL_BIDIR)
#define SRXL2_PORT_MODE MODE_RXTX
#define SRXL2_REPLY_QUIESCENCE (2 * 10 * 1000000 / SRXL2_PORT_BAUDRATE_DEFAULT) // 2 * (lastIdleTimestamp - lastReceiveTimestamp). Time taken to send 2 bytes
#define SRXL2_ID 0xA6
#define SRXL2_MAX_PACKET_LENGTH 80
#define SRXL2_DEVICE_ID_BROADCAST 0xFF
#define SRXL2_FRAME_TIMEOUT_US 50000
#define SRXL2_LISTEN_FOR_ACTIVITY_TIMEOUT_US 50000
#define SRXL2_SEND_HANDSHAKE_TIMEOUT_US 50000
#define SRXL2_LISTEN_FOR_HANDSHAKE_TIMEOUT_US 200000
#define SPEKTRUM_PULSE_OFFSET 988 // Offset value to convert digital data into RC pulse
typedef union {
uint8_t raw[SRXL2_MAX_PACKET_LENGTH];
Srxl2Header header;
} Srxl2Frame;
struct rxBuf {
volatile unsigned len;
Srxl2Frame packet;
};
static uint8_t unitId = 0;
static uint8_t baudRate = 0;
static Srxl2State state = Disabled;
static uint32_t timeoutTimestamp = 0;
static uint32_t fullTimeoutTimestamp = 0;
static uint32_t lastValidPacketTimestamp = 0;
static volatile uint32_t lastReceiveTimestamp = 0;
static volatile uint32_t lastIdleTimestamp = 0;
struct rxBuf readBuffer[2];
struct rxBuf* readBufferPtr = &readBuffer[0];
struct rxBuf* processBufferPtr = &readBuffer[1];
static volatile unsigned readBufferIdx = 0;
static volatile bool transmittingTelemetry = false;
static uint8_t writeBuffer[SRXL2_MAX_PACKET_LENGTH];
static unsigned writeBufferIdx = 0;
static serialPort_t *serialPort;
static uint8_t busMasterDeviceId = 0xFF;
static bool telemetryRequested = false;
static uint8_t telemetryFrame[22];
uint8_t globalResult = 0;
/* handshake protocol
1. listen for 50ms for serial activity and go to State::Running if found, autobaud may be necessary
2. if srxl2_unitId = 0:
send a Handshake with destinationDeviceId = 0 every 50ms for at least 200ms
else:
listen for Handshake for at least 200ms
3. respond to Handshake as currently implemented in process if rePst received
4. respond to broadcast Handshake
*/
// if 50ms with not activity, go to default baudrate and to step 1
bool srxl2ProcessHandshake(const Srxl2Header* header)
{
const Srxl2HandshakeSubHeader* handshake = (Srxl2HandshakeSubHeader*)(header + 1);
if (handshake->destinationDeviceId == Broadcast) {
DEBUG_PRINTF("broadcast handshake from %x\r\n", handshake->sourceDeviceId);
busMasterDeviceId = handshake->sourceDeviceId;
if (handshake->baudSupported == 1) {
serialSetBaudRate(serialPort, SRXL2_PORT_BAUDRATE_HIGH);
DEBUG_PRINTF("switching to %d baud\r\n", SRXL2_PORT_BAUDRATE_HIGH);
}
state = Running;
return true;
}
if (handshake->destinationDeviceId != ((FlightController << 4) | unitId)) {
return true;
}
DEBUG_PRINTF("FC handshake from %x\r\n", handshake->sourceDeviceId);
Srxl2HandshakeFrame response = {
.header = *header,
.payload = {
handshake->destinationDeviceId,
handshake->sourceDeviceId,
/* priority */ 10,
/* baudSupported*/ baudRate,
/* info */ 0,
// U_ID_2
}
};
srxl2RxWriteData(&response, sizeof(response));
return true;
}
void srxl2ProcessChannelData(const Srxl2ChannelDataHeader* channelData, rxRuntimeConfig_t *rxRuntimeConfig) {
globalResult = RX_FRAME_COMPLETE;
if (channelData->rssi >= 0) {
const int rssiPercent = channelData->rssi;
lqTrackerSet(rxRuntimeConfig->lqTracker, scaleRange(constrain(rssiPercent, 0, 100), 0, 100, 0, RSSI_MAX_VALUE));
//setRssi(scaleRange(rssiPercent, 0, 100, 0, RSSI_MAX_VALUE), RSSI_SOURCE_RX_PROTOCOL);
}
//If receiver is in a connected state, and a packet is missed, the channel mask will be 0.
if (!channelData->channelMask.u32) {
globalResult |= RX_FRAME_FAILSAFE;
return;
}
const uint16_t *frameChannels = (const uint16_t *) (channelData + 1);
uint32_t channelMask = channelData->channelMask.u32;
while (channelMask) {
unsigned idx = __builtin_ctz (channelMask);
uint32_t mask = 1 << idx;
rxRuntimeConfig->channelData[idx] = *frameChannels++;
channelMask &= ~mask;
}
DEBUG_PRINTF("channel data: %d %d %x\r\n", channelData_header->rssi, channelData_header->frameLosses, channelData_header->channelMask.u32);
}
bool srxl2ProcessControlData(const Srxl2Header* header, rxRuntimeConfig_t *rxRuntimeConfig)
{
const Srxl2ControlDataSubHeader* controlData = (Srxl2ControlDataSubHeader*)(header + 1);
const uint8_t ownId = (FlightController << 4) | unitId;
if (controlData->replyId == ownId) {
telemetryRequested = true;
DEBUG_PRINTF("command: %x replyId: %x ownId: %x\r\n", controlData->command, controlData->replyId, ownId);
}
switch (controlData->command) {
case ChannelData:
srxl2ProcessChannelData((const Srxl2ChannelDataHeader *) (controlData + 1), rxRuntimeConfig);
break;
case FailsafeChannelData: {
globalResult |= RX_FRAME_FAILSAFE;
//setRssiDirect(0, RSSI_SOURCE_RX_PROTOCOL);
lqTrackerSet(rxRuntimeConfig->lqTracker, 0);
// DEBUG_PRINTF("fs channel data\r\n");
} break;
case VTXData: {
#if defined(USE_SPEKTRUM_VTX_CONTROL) && defined(USE_VTX_COMMON)
Srxl2VtxData *vtxData = (Srxl2VtxData*)(controlData + 1);
DEBUG_PRINTF("vtx data\r\n");
DEBUG_PRINTF("vtx band: %x\r\n", vtxData->band);
DEBUG_PRINTF("vtx channel: %x\r\n", vtxData->channel);
DEBUG_PRINTF("vtx pit: %x\r\n", vtxData->pit);
DEBUG_PRINTF("vtx power: %x\r\n", vtxData->power);
DEBUG_PRINTF("vtx powerDec: %x\r\n", vtxData->powerDec);
DEBUG_PRINTF("vtx region: %x\r\n", vtxData->region);
// Pack data as it was used before srxl2 to use existing functions.
// Get the VTX control bytes in a frame
uint32_t vtxControl = (0xE0 << 24) | (0xE0 << 8) |
((vtxData->band & 0x07) << 21) |
((vtxData->channel & 0x0F) << 16) |
((vtxData->pit & 0x01) << 4) |
((vtxData->region & 0x01) << 3) |
((vtxData->power & 0x07));
spektrumHandleVtxControl(vtxControl);
#endif
} break;
}
return true;
}
bool srxl2ProcessPacket(const Srxl2Header* header, rxRuntimeConfig_t *rxRuntimeConfig)
{
switch (header->packetType) {
case Handshake:
return srxl2ProcessHandshake(header);
case ControlData:
return srxl2ProcessControlData(header, rxRuntimeConfig);
default:
DEBUG_PRINTF("Other packet type, ID: %x \r\n", header->packetType);
break;
}
return false;
}
// @note assumes packet is fully there
void srxl2Process(rxRuntimeConfig_t *rxRuntimeConfig)
{
if (processBufferPtr->packet.header.id != SRXL2_ID || processBufferPtr->len != processBufferPtr->packet.header.length) {
DEBUG_PRINTF("invalid header id: %x, or length: %x received vs %x expected \r\n", processBufferPtr->packet.header.id, processBufferPtr->len, processBufferPtr->packet.header.length);
globalResult = RX_FRAME_DROPPED;
return;
}
const uint16_t calculatedCrc = crc16_ccitt_update(0, processBufferPtr->packet.raw, processBufferPtr->packet.header.length);
//Invalid if crc non-zero
if (calculatedCrc) {
globalResult = RX_FRAME_DROPPED;
DEBUG_PRINTF("crc mismatch %x\r\n", calculatedCrc);
return;
}
//Packet is valid only after ID and CRC check out
lastValidPacketTimestamp = micros();
if (srxl2ProcessPacket(&processBufferPtr->packet.header, rxRuntimeConfig)) {
return;
}
DEBUG_PRINTF("could not parse packet: %x\r\n", processBufferPtr->packet.header.packetType);
globalResult = RX_FRAME_DROPPED;
}
static void srxl2DataReceive(uint16_t character, void *data)
{
UNUSED(data);
lastReceiveTimestamp = microsISR();
//If the buffer len is not reset for whatever reason, disable reception
if (readBufferPtr->len > 0 || readBufferIdx >= SRXL2_MAX_PACKET_LENGTH) {
readBufferIdx = 0;
globalResult = RX_FRAME_DROPPED;
}
else {
readBufferPtr->packet.raw[readBufferIdx] = character;
readBufferIdx++;
}
}
static void srxl2Idle(void)
{
if(transmittingTelemetry) { // Transmitting telemetry triggers idle interrupt as well. We dont want to change buffers then
transmittingTelemetry = false;
}
else if(readBufferIdx == 0) { // Packet was invalid
readBufferPtr->len = 0;
}
else {
lastIdleTimestamp = microsISR();
//Swap read and process buffer pointers
if(processBufferPtr == &readBuffer[0]) {
processBufferPtr = &readBuffer[1];
readBufferPtr = &readBuffer[0];
} else {
processBufferPtr = &readBuffer[0];
readBufferPtr = &readBuffer[1];
}
processBufferPtr->len = readBufferIdx;
}
readBufferIdx = 0;
}
static uint8_t srxl2FrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
{
UNUSED(rxRuntimeConfig);
if(serialIsIdle(serialPort))
{
srxl2Idle();
}
globalResult = RX_FRAME_PENDING;
// len should only be set after an idle interrupt (packet reception complete)
if (processBufferPtr != NULL && processBufferPtr->len) {
srxl2Process(rxRuntimeConfig);
processBufferPtr->len = 0;
}
uint8_t result = globalResult;
const uint32_t now = micros();
switch (state) {
case Disabled: break;
case ListenForActivity: {
// activity detected
if (lastValidPacketTimestamp != 0) {
// as ListenForActivity is done at default baud-rate, we don't need to change anything
// @todo if there were non-handshake packets - go to running,
// if there were - go to either Send Handshake or Listen For Handshake
state = Running;
} else if (cmpTimeUs(lastIdleTimestamp, lastReceiveTimestamp) > 0) {
if (baudRate != 0) {
uint32_t currentBaud = serialGetBaudRate(serialPort);
if(currentBaud == SRXL2_PORT_BAUDRATE_DEFAULT)
serialSetBaudRate(serialPort, SRXL2_PORT_BAUDRATE_HIGH);
else
serialSetBaudRate(serialPort, SRXL2_PORT_BAUDRATE_DEFAULT);
}
} else if (cmpTimeUs(now, timeoutTimestamp) >= 0) {
// @todo if there was activity - detect baudrate and ListenForHandshake
if (unitId == 0) {
state = SendHandshake;
timeoutTimestamp = now + SRXL2_SEND_HANDSHAKE_TIMEOUT_US;
fullTimeoutTimestamp = now + SRXL2_LISTEN_FOR_HANDSHAKE_TIMEOUT_US;
} else {
state = ListenForHandshake;
timeoutTimestamp = now + SRXL2_LISTEN_FOR_HANDSHAKE_TIMEOUT_US;
}
}
} break;
case SendHandshake: {
if (cmpTimeUs(now, timeoutTimestamp) >= 0) {
// @todo set another timeout for 50ms tries
// fill write buffer with handshake frame
result |= RX_FRAME_PROCESSING_REQUIRED;
}
if (cmpTimeUs(now, fullTimeoutTimestamp) >= 0) {
serialSetBaudRate(serialPort, SRXL2_PORT_BAUDRATE_DEFAULT);
DEBUG_PRINTF("case SendHandshake: switching to %d baud\r\n", SRXL2_PORT_BAUDRATE_DEFAULT);
timeoutTimestamp = now + SRXL2_LISTEN_FOR_ACTIVITY_TIMEOUT_US;
result = (result & ~RX_FRAME_PENDING) | RX_FRAME_FAILSAFE;
state = ListenForActivity;
lastReceiveTimestamp = 0;
}
} break;
case ListenForHandshake: {
if (cmpTimeUs(now, timeoutTimestamp) >= 0) {
serialSetBaudRate(serialPort, SRXL2_PORT_BAUDRATE_DEFAULT);
DEBUG_PRINTF("case ListenForHandshake: switching to %d baud\r\n", SRXL2_PORT_BAUDRATE_DEFAULT);
timeoutTimestamp = now + SRXL2_LISTEN_FOR_ACTIVITY_TIMEOUT_US;
result = (result & ~RX_FRAME_PENDING) | RX_FRAME_FAILSAFE;
state = ListenForActivity;
lastReceiveTimestamp = 0;
}
} break;
case Running: {
// frame timed out, reset state
if (cmpTimeUs(now, lastValidPacketTimestamp) >= SRXL2_FRAME_TIMEOUT_US) {
serialSetBaudRate(serialPort, SRXL2_PORT_BAUDRATE_DEFAULT);
DEBUG_PRINTF("case Running: switching to %d baud: %d %d\r\n", SRXL2_PORT_BAUDRATE_DEFAULT, now, lastValidPacketTimestamp);
timeoutTimestamp = now + SRXL2_LISTEN_FOR_ACTIVITY_TIMEOUT_US;
result = (result & ~RX_FRAME_PENDING) | RX_FRAME_FAILSAFE;
state = ListenForActivity;
lastReceiveTimestamp = 0;
lastValidPacketTimestamp = 0;
}
} break;
};
if (writeBufferIdx) {
result |= RX_FRAME_PROCESSING_REQUIRED;
}
return result;
}
static bool srxl2ProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig)
{
UNUSED(rxRuntimeConfig);
if (writeBufferIdx == 0) {
return true;
}
const uint32_t now = micros();
if (cmpTimeUs(lastIdleTimestamp, lastReceiveTimestamp) > 0) {
// time sufficient for at least 2 characters has passed
if (cmpTimeUs(now, lastReceiveTimestamp) > SRXL2_REPLY_QUIESCENCE) {
transmittingTelemetry = true;
serialWriteBuf(serialPort, writeBuffer, writeBufferIdx);
writeBufferIdx = 0;
} else {
DEBUG_PRINTF("not enough time to send 2 characters passed yet, %d us since last receive, %d required\r\n", now - lastReceiveTimestamp, SRXL2_REPLY_QUIESCENCE);
}
} else {
DEBUG_PRINTF("still receiving a frame, %d %d\r\n", lastIdleTimestamp, lastReceiveTimestamp);
}
return true;
}
static uint16_t srxl2ReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channelIdx)
{
if (channelIdx >= rxRuntimeConfig->channelCount) {
return 0;
}
return SPEKTRUM_PULSE_OFFSET + ((rxRuntimeConfig->channelData[channelIdx] >> SRXL2_CHANNEL_SHIFT) >> 1);
}
void srxl2RxWriteData(const void *data, int len)
{
const uint16_t crc = crc16_ccitt_update(0, (uint8_t*)data, len - 2);
((uint8_t*)data)[len-2] = ((uint8_t *) &crc)[1] & 0xFF;
((uint8_t*)data)[len-1] = ((uint8_t *) &crc)[0] & 0xFF;
len = MIN(len, (int)sizeof(writeBuffer));
memcpy(writeBuffer, data, len);
writeBufferIdx = len;
}
bool srxl2RxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{
static uint16_t channelData[SRXL2_MAX_CHANNELS];
for (size_t i = 0; i < SRXL2_MAX_CHANNELS; ++i) {
channelData[i] = SRXL2_CHANNEL_CENTER;
}
unitId = rxConfig->srxl2_unit_id;
baudRate = rxConfig->srxl2_baud_fast;
rxRuntimeConfig->channelData = channelData;
rxRuntimeConfig->channelCount = SRXL2_MAX_CHANNELS;
rxRuntimeConfig->rxRefreshRate = SRXL2_FRAME_PERIOD_US;
rxRuntimeConfig->rcReadRawFn = srxl2ReadRawRC;
rxRuntimeConfig->rcFrameStatusFn = srxl2FrameStatus;
rxRuntimeConfig->rcProcessFrameFn = srxl2ProcessFrame;
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
if (!portConfig) {
return false;
}
portOptions_t options = SRXL2_PORT_OPTIONS;
if (rxConfig->serialrx_inverted) {
options |= SERIAL_INVERTED;
}
if (rxConfig->halfDuplex) {
options |= SERIAL_BIDIR;
}
serialPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, srxl2DataReceive,
NULL, SRXL2_PORT_BAUDRATE_DEFAULT, SRXL2_PORT_MODE, options);
if (!serialPort) {
return false;
}
state = ListenForActivity;
timeoutTimestamp = micros() + SRXL2_LISTEN_FOR_ACTIVITY_TIMEOUT_US;
//Looks like this needs to be set in cli config
//if (rssiSource == RSSI_SOURCE_NONE) {
// rssiSource = RSSI_SOURCE_RX_PROTOCOL;
//}
return (bool)serialPort;
}
bool srxl2RxIsActive(void)
{
return serialPort;
}
bool srxl2TelemetryRequested(void)
{
return telemetryRequested;
}
void srxl2InitializeFrame(sbuf_t *dst)
{
dst->ptr = telemetryFrame;
dst->end = ARRAYEND(telemetryFrame);
sbufWriteU8(dst, SRXL2_ID);
sbufWriteU8(dst, TelemetrySensorData);
sbufWriteU8(dst, ARRAYLEN(telemetryFrame));
sbufWriteU8(dst, busMasterDeviceId);
}
void srxl2FinalizeFrame(sbuf_t *dst)
{
sbufSwitchToReader(dst, telemetryFrame);
// Include 2 additional bytes of length since we're letting the srxl2RxWriteData function add the CRC in
srxl2RxWriteData(sbufPtr(dst), sbufBytesRemaining(dst) + 2);
telemetryRequested = false;
}
void srxl2Bind(void)
{
const size_t length = sizeof(Srxl2BindInfoFrame);
Srxl2BindInfoFrame bind = {
.header = {
.id = SRXL2_ID,
.packetType = BindInfo,
.length = length
},
.payload = {
.request = EnterBindMode,
.deviceId = busMasterDeviceId,
.bindType = DMSX_11ms,
.options = SRXL_BIND_OPT_TELEM_TX_ENABLE | SRXL_BIND_OPT_BIND_TX_ENABLE,
}
};
srxl2RxWriteData(&bind, length);
}
#endif

15
src/main/rx/srxl2.h Normal file
View file

@ -0,0 +1,15 @@
#pragma once
#include <stdint.h>
#include <stdbool.h>
#include "rx/rx.h"
struct sbuf_s;
bool srxl2RxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
bool srxl2RxIsActive(void);
void srxl2RxWriteData(const void *data, int len);
bool srxl2TelemetryRequested(void);
void srxl2InitializeFrame(struct sbuf_s *dst);
void srxl2FinalizeFrame(struct sbuf_s *dst);
void srxl2Bind(void);

138
src/main/rx/srxl2_types.h Normal file
View file

@ -0,0 +1,138 @@
#pragma once
#define PACKED __attribute__((packed))
typedef enum {
Disabled,
ListenForActivity,
SendHandshake,
ListenForHandshake,
Running
} Srxl2State;
typedef enum {
Handshake = 0x21,
BindInfo = 0x41,
ParameterConfiguration = 0x50,
SignalQuality = 0x55,
TelemetrySensorData = 0x80,
ControlData = 0xCD,
} Srxl2PacketType;
typedef struct {
uint8_t id;
uint8_t packetType;
uint8_t length;
} PACKED Srxl2Header;
typedef struct {
uint8_t sourceDeviceId;
uint8_t destinationDeviceId;
uint8_t priority;
uint8_t baudSupported;
uint8_t info;
uint32_t uniqueId;
} PACKED Srxl2HandshakeSubHeader;
typedef struct {
uint8_t command;
uint8_t replyId;
} PACKED Srxl2ControlDataSubHeader;
typedef enum {
ChannelData = 0x00,
FailsafeChannelData = 0x01,
VTXData = 0x02,
} Srxl2ControlDataCommand;
typedef struct {
int8_t rssi;
uint16_t frameLosses;
union {
//struct {
// uint8_t channels_0_7;
// uint8_t channels_8_15;
// uint8_t channels_16_23;
// uint8_t channels_24_31;
//} u8;
uint8_t u8[4];
uint32_t u32;
} channelMask;
} PACKED Srxl2ChannelDataHeader;
typedef enum {
NoDevice = 0,
RemoteReceiver = 1,
Receiver = 2,
FlightController = 3,
ESC = 4,
Reserved = 5,
SRXLServo = 6,
SRXLServo_2 = 7,
VTX = 8,
} Srxl2DeviceType;
typedef enum {
FlightControllerDefault = 0x30,
FlightControllerMax = 0x3F,
Broadcast = 0xFF,
} Srxl2DeviceId;
typedef struct {
Srxl2Header header;
Srxl2HandshakeSubHeader payload;
uint8_t crcHigh;
uint8_t crcLow;
} PACKED Srxl2HandshakeFrame;
typedef enum {
EnterBindMode = 0xEB,
RequestBindStatus = 0xB5,
BoundDataReport = 0xDB,
SetBindInfo = 0x5B,
} Srxl2BindRequest;
typedef enum {
NotBound = 0x0,
DSM2_1024_22ms = 0x01,
DSM2_1024_MC24 = 0x02,
DMS2_2048_11ms = 0x12,
DMSX_22ms = 0xA2,
DMSX_11ms = 0xB2,
Surface_DSM2_16_5ms = 0x63,
DSMR_11ms_22ms = 0xE2,
DSMR_5_5ms = 0xE4,
} Srxl2BindType;
// Bit masks for Options byte
#define SRXL_BIND_OPT_NONE (0x00)
#define SRXL_BIND_OPT_TELEM_TX_ENABLE (0x01) // Set if this device should be enabled as the current telemetry device to tx over RF
#define SRXL_BIND_OPT_BIND_TX_ENABLE (0x02) // Set if this device should reply to a bind request with a Discover packet over RF
typedef struct {
uint8_t request;
uint8_t deviceId;
uint8_t bindType;
uint8_t options;
uint64_t guid;
uint32_t uid;
} PACKED Srxl2BindInfoPayload;
typedef struct {
Srxl2Header header;
Srxl2BindInfoPayload payload;
uint8_t crcHigh;
uint8_t crcLow;
} PACKED Srxl2BindInfoFrame;
// VTX Data
typedef struct {
uint8_t band; // VTX Band (0 = Fatshark, 1 = Raceband, 2 = E, 3 = B, 4 = A)
uint8_t channel; // VTX Channel (0-7)
uint8_t pit; // Pit/Race mode (0 = Race, 1 = Pit). Race = (normal operating) mode. Pit = (reduced power) mode.
uint8_t power; // VTX Power (0 = Off, 1 = 1mw to 14mW, 2 = 15mW to 25mW, 3 = 26mW to 99mW, 4 = 100mW to 299mW, 5 = 300mW to 600mW, 6 = 601mW+, 7 = manual control)
uint16_t powerDec; // VTX Power as a decimal 1mw/unit
uint8_t region; // Region (0 = USA, 1 = EU)
} PACKED Srxl2VtxData;
#undef PACKED

21
src/main/sensors/barometer.c
View file

@ -40,6 +40,7 @@
#include "drivers/barometer/barometer_ms56xx.h"
#include "drivers/barometer/barometer_spl06.h"
#include "drivers/barometer/barometer_dps310.h"
#include "drivers/barometer/barometer_msp.h"
#include "drivers/time.h"
#include "fc/runtime_config.h"
@ -55,7 +56,7 @@
baro_t baro; // barometer access functions
PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 1);
PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 2);
#ifdef USE_BARO
#define BARO_HARDWARE_DEFAULT BARO_AUTODETECT
@ -64,7 +65,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER
#endif
PG_RESET_TEMPLATE(barometerConfig_t, barometerConfig,
.baro_hardware = BARO_HARDWARE_DEFAULT,
.use_median_filtering = 1,
.use_median_filtering = 0,
.baro_calibration_tolerance = 150
);
@ -187,6 +188,20 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse)
}
FALLTHROUGH;
case BARO_MSP:
#ifdef USE_BARO_MSP
// Skip autodetection for MSP baro, only allow manual config
if (baroHardwareToUse != BARO_AUTODETECT && mspBaroDetect(dev)) {
baroHardware = BARO_MSP;
break;
}
#endif
/* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
if (baroHardwareToUse != BARO_AUTODETECT) {
break;
}
FALLTHROUGH;
case BARO_FAKE:
#ifdef USE_FAKE_BARO
if (fakeBaroDetect(dev)) {
@ -350,7 +365,7 @@ int32_t baroCalculateAltitude(void)
#endif
// calculates height from ground via baro readings
baro.BaroAlt = pressureToAltitude(baro.baroPressure) - baroGroundAltitude;
}
}
return baro.BaroAlt;
}

3
src/main/sensors/barometer.h
View file

@ -32,7 +32,8 @@ typedef enum {
BARO_SPL06 = 7,
BARO_BMP388 = 8,
BARO_DPS310 = 9,
BARO_FAKE = 10,
BARO_MSP = 10,
BARO_FAKE = 11,
BARO_MAX = BARO_FAKE
} baroSensor_e;

15
src/main/sensors/compass.c
View file

@ -40,6 +40,7 @@
#include "drivers/compass/compass_qmc5883l.h"
#include "drivers/compass/compass_mpu9250.h"
#include "drivers/compass/compass_lis3mdl.h"
#include "drivers/compass/compass_msp.h"
#include "drivers/io.h"
#include "drivers/light_led.h"
#include "drivers/time.h"
@ -249,6 +250,20 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse)
}
FALLTHROUGH;
case MAG_MSP:
#ifdef USE_MAG_MSP
// Skip autodetection for MSP mag
if (magHardwareToUse != MAG_AUTODETECT && mspMagDetect(dev)) {
magHardware = MAG_MSP;
break;
}
#endif
/* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
if (magHardwareToUse != MAG_AUTODETECT) {
break;
}
FALLTHROUGH;
case MAG_FAKE:
#ifdef USE_FAKE_MAG
if (fakeMagDetect(dev)) {

3
src/main/sensors/compass.h
View file

@ -41,7 +41,8 @@ typedef enum {
MAG_MPU9250 = 9,
MAG_IST8308 = 10,
MAG_LIS3MDL = 11,
MAG_FAKE = 12,
MAG_MSP = 12,
MAG_FAKE = 13,
MAG_MAX = MAG_FAKE
} magSensor_e;

23
src/main/sensors/pitotmeter.c
View file

@ -29,10 +29,11 @@
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/pitotmeter.h"
#include "drivers/pitotmeter_ms4525.h"
#include "drivers/pitotmeter_adc.h"
#include "drivers/pitotmeter_virtual.h"
#include "drivers/pitotmeter/pitotmeter.h"
#include "drivers/pitotmeter/pitotmeter_ms4525.h"
#include "drivers/pitotmeter/pitotmeter_adc.h"
#include "drivers/pitotmeter/pitotmeter_msp.h"
#include "drivers/pitotmeter/pitotmeter_virtual.h"
#include "drivers/time.h"
#include "fc/config.h"
@ -108,6 +109,20 @@ bool pitotDetect(pitotDev_t *dev, uint8_t pitotHardwareToUse)
}
FALLTHROUGH;
case PITOT_MSP:
#ifdef USE_PITOT_MSP
// Skip autodetection for MSP baro, only allow manual config
if (pitotHardwareToUse != PITOT_AUTODETECT && mspPitotmeterDetect(dev)) {
pitotHardware = PITOT_MSP;
break;
}
#endif
/* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
if (pitotHardwareToUse != PITOT_AUTODETECT) {
break;
}
FALLTHROUGH;
case PITOT_FAKE:
#ifdef USE_PITOT_FAKE
if (fakePitotDetect(dev)) {

3
src/main/sensors/pitotmeter.h
View file

@ -21,7 +21,7 @@
#include "common/filter.h"
#include "common/calibration.h"
#include "drivers/pitotmeter.h"
#include "drivers/pitotmeter/pitotmeter.h"
typedef enum {
PITOT_NONE = 0,
@ -30,6 +30,7 @@ typedef enum {
PITOT_ADC = 3,
PITOT_VIRTUAL = 4,
PITOT_FAKE = 5,
PITOT_MSP = 6,
} pitotSensor_e;
#define PITOT_MAX PITOT_FAKE

4
src/main/target/AIRHEROF3/target.h
View file

@ -76,6 +76,10 @@
#define VBAT_ADC_CHANNEL ADC_CHN_1
#define AIRSPEED_ADC_CHANNEL ADC_CHN_2
#define USE_DJI_HD_OSD
#define USE_OSD
#undef USE_CMS
#undef CMS_MENU_OSD
/*
#define USE_LED_STRIP

8
src/main/target/MAMBAF722/target.c
View file

@ -27,10 +27,10 @@
const timerHardware_t timerHardware[] = {
DEF_TIM(TIM11, CH1, PB9, TIM_USE_PPM, 0, 0 ), // PPM IN
DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR, 0, 0 ), // S1_OUT – D(2, 4, 7)
DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR, 0, 0 ), // S2_OUT – D(2, 7, 7)
DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR, 0, 1 ), // S3_OUT – D(2, 1, 6)
DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR, 0, 1 ), // S4_OUT – D(2, 2, 6)
DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S1_OUT – D(2, 4, 7)
DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S2_OUT – D(2, 7, 7)
DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // S3_OUT – D(2, 1, 6)
DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // S4_OUT – D(2, 2, 6)
DEF_TIM(TIM2, CH2, PB3, TIM_USE_LED, 0, 0 ), // LED_STRIP – D(1, 6, 3)
};

1
src/main/target/MATEKF722PX/CMakeLists.txt
View file

@ -1 +1,2 @@
target_stm32f722xe(MATEKF722PX)
target_stm32f722xe(MATEKF722WPX)

25
src/main/target/MATEKF722PX/target.h
View file

@ -21,7 +21,7 @@
#define TARGET_BOARD_IDENTIFIER "MF7P"
#define USBD_PRODUCT_STRING "MATEKF722PX"
#define LED0 PA14 //Blue SWCLK
#define LED0 PA14 //Blue SWCLK
#define LED1 PA13 //Green SWDIO
#define BEEPER PC13
@ -78,11 +78,21 @@
#define SPI2_MISO_PIN PB14
#define SPI2_MOSI_PIN PC3
#define USE_FLASHFS
#define USE_FLASH_M25P16
#define M25P16_SPI_BUS BUS_SPI2
#define M25P16_CS_PIN PB12
#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
//F722-PX,F722-HD
#if defined(MATEKF722PX)
#define USE_FLASHFS
#define USE_FLASH_M25P16
#define M25P16_SPI_BUS BUS_SPI2
#define M25P16_CS_PIN PB12
#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
#else
//F722-WPX
#define USE_SDCARD
#define USE_SDCARD_SPI
#define SDCARD_SPI_BUS BUS_SPI2
#define SDCARD_CS_PIN PC15
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
#endif
// *************** UART *****************************
#define USE_VCP
@ -112,7 +122,7 @@
#define USE_UART6
#define UART6_TX_PIN PC6
#define UART6_RX_PIN PC7
#define USE_SOFTSERIAL1
#define SOFTSERIAL_1_TX_PIN PA2 //TX2 pad
#define SOFTSERIAL_1_RX_PIN NONE
@ -163,4 +173,3 @@
#define USE_DSHOT
#define USE_SERIALSHOT
#define USE_ESC_SENSOR

14
src/main/target/MATEKF765/target.h
View file

@ -144,13 +144,13 @@
#define UART8_TX_PIN PE1
#define UART8_RX_PIN PE0
/*
#define USE_SOFTSERIAL1
#define SOFTSERIAL_1_TX_PIN PA2
#define SOFTSERIAL_1_RX_PIN PA2
*/
#define SERIAL_PORT_COUNT 8
#define USE_SOFTSERIAL1
#define SOFTSERIAL_1_TX_PIN PC6 //TX6 pad
#define SOFTSERIAL_1_RX_PIN PC6 //TX6 pad
#define SERIAL_PORT_COUNT 9
#define DEFAULT_RX_TYPE RX_TYPE_SERIAL
#define SERIALRX_PROVIDER SERIALRX_SBUS
@ -171,6 +171,8 @@
#define ADC_CHANNEL_2_PIN PC3
#define ADC_CHANNEL_3_PIN PC1
#define ADC_CHANNEL_4_PIN PC0
#define ADC_CHANNEL_5_PIN PA4 //VBAT2
#define ADC_CHANNEL_6_PIN PC5 //CURR2
#define VBAT_ADC_CHANNEL ADC_CHN_1
#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2

5
src/main/target/SPRACINGF3/target.h
View file

@ -93,6 +93,11 @@
#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2
#define RSSI_ADC_CHANNEL ADC_CHN_3
#define USE_DJI_HD_OSD
#define USE_OSD
#undef USE_CMS
#undef CMS_MENU_OSD
#define USE_LED_STRIP
#define WS2811_PIN PA8

11
src/main/target/common.h
View file

@ -54,6 +54,7 @@
#define USE_BLACKBOX
#define USE_GPS
#define USE_GPS_PROTO_UBLOX
#define USE_GPS_PROTO_MSP
#define USE_NAV
#define USE_TELEMETRY
#define USE_TELEMETRY_LTM
@ -93,8 +94,10 @@
#define USE_OPFLOW_CXOF
#define USE_OPFLOW_MSP
// Allow default airspeed sensors
#define USE_PITOT
#define USE_PITOT_MS4525
#define USE_PITOT_MSP
#define USE_1WIRE
#define USE_1WIRE_DS2482
@ -124,6 +127,14 @@
#define USE_I2C_IO_EXPANDER
#define USE_SERIALRX_SRXL2 // Spektrum SRXL2 protocol
#define USE_TELEMETRY_SRXL
#define USE_SPEKTRUM_CMS_TELEMETRY
//#define USE_SPEKTRUM_VTX_CONTROL //Some functions from betaflight still not implemented
#define USE_SPEKTRUM_VTX_TELEMETRY
#define USE_VTX_COMMON
#else // FLASH_SIZE < 256
#define LOG_LEVEL_MAXIMUM LOG_LEVEL_ERROR
#endif

9
src/main/target/common_post.h
View file

@ -35,6 +35,15 @@
#define USE_CANVAS
#endif
// Enable MSP BARO & MAG drivers if BARO and MAG sensors are compiled in
#if defined(USE_MAG)
#define USE_MAG_MSP
#endif
#if defined(USE_BARO)
#define USE_BARO_MSP
#endif
#ifdef USE_ESC_SENSOR
#define USE_RPM_FILTER
#endif

30
src/main/target/link/stm32_flash_F303_128k.ld
View file

@ -1,30 +0,0 @@
/*
*****************************************************************************
**
** File : stm32_flash.ld
**
** Abstract : Linker script for STM32F30x Device with
** 128KByte FLASH and 40KByte RAM
**
*****************************************************************************
*/
/* Stack & Heap sizes */
_Min_Heap_Size = 0;
_Min_Stack_Size = 0x1800;
/* Specify the memory areas. */
MEMORY
{
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 126K
FLASH_CONFIG (r) : ORIGIN = 0x0801F800, LENGTH = 2K
RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 40K
CCM (xrw) : ORIGIN = 0x10000000, LENGTH = 8K
MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K
}
REGION_ALIAS("STACKRAM", CCM)
REGION_ALIAS("FASTRAM", CCM)
INCLUDE "stm32_flash.ld"

30
src/main/target/link/stm32_flash_F303_256k.ld
View file

@ -1,30 +0,0 @@
/*
*****************************************************************************
**
** File : stm32_flash.ld
**
** Abstract : Linker script for STM32F30x Device with
** 256KByte FLASH and 40KByte RAM
**
*****************************************************************************
*/
/* Stack & Heap sizes */
_Min_Heap_Size = 0;
_Min_Stack_Size = 0x1800;
/* Specify the memory areas. */
MEMORY
{
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 250K
FLASH_CONFIG (r) : ORIGIN = 0x0803E800, LENGTH = 6K
RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 40K
CCM (xrw) : ORIGIN = 0x10000000, LENGTH = 8K
MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K
}
REGION_ALIAS("STACKRAM", CCM)
REGION_ALIAS("FASTRAM", CCM)
INCLUDE "stm32_flash.ld"

40
src/main/target/link/stm32_flash_F405.ld
View file

@ -1,40 +0,0 @@
/*
*****************************************************************************
**
** File : stm32_flash_f405.ld
**
** Abstract : Linker script for STM32F405RG Device with
** 1024KByte FLASH, 128KByte RAM 64KByte CCM (RAM)
**
*****************************************************************************
*/
/* Stack & Heap sizes */
_Min_Heap_Size = 0;
_Min_Stack_Size = 0x1800;
/* Entry Point */
ENTRY(Reset_Handler)
/*
0x08000000 to 0x08100000 1024K full flash,
0x08000000 to 0x080DFFFF 896K firmware,
0x080E0000 to 0x08100000 128K config, // FLASH_Sector_11
*/
/* Specify the memory areas */
MEMORY
{
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 896K
FLASH_CONFIG (r) : ORIGIN = 0x080E0000, LENGTH = 128K
RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K
CCM (rwx) : ORIGIN = 0x10000000, LENGTH = 64K
BACKUP_SRAM (rwx) : ORIGIN = 0x40024000, LENGTH = 4K
MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K
}
REGION_ALIAS("STACKRAM", CCM)
REGION_ALIAS("FASTRAM", CCM)
INCLUDE "stm32_flash.ld"

43
src/main/target/link/stm32_flash_F405_bl.ld
View file

@ -1,43 +0,0 @@
/*
*****************************************************************************
**
** File : stm32_flash_f405.ld
**
** Abstract : Linker script for STM32F405RG Device with
** 1024KByte FLASH, 128KByte RAM 64KByte CCM (RAM)
**
*****************************************************************************
*/
/* Stack & Heap sizes */
_Min_Heap_Size = 0;
_Min_Stack_Size = 0x1800;
/* Entry Point */
ENTRY(Reset_Handler)
/*
0x08000000 to 0x08100000 1024K full flash,
0x08000000 to 0x080DFFFF 896K firmware,
0x080E0000 to 0x08100000 128K config, // FLASH_Sector_11
*/
/* Specify the memory areas */
MEMORY
{
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 32K
FIRMWARE (rx) : ORIGIN = 0x08008000, LENGTH = 864K
FLASH_CONFIG (r) : ORIGIN = 0x080E0000, LENGTH = 128K
RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K
CCM (rwx) : ORIGIN = 0x10000000, LENGTH = 64K
BACKUP_SRAM (rwx) : ORIGIN = 0x40024000, LENGTH = 4K
MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K
}
REGION_ALIAS("STACKRAM", CCM)
REGION_ALIAS("FASTRAM", CCM)
__firmware_start = ORIGIN(FIRMWARE);
INCLUDE "stm32_flash.ld"

42
src/main/target/link/stm32_flash_F405_for_bl.ld
View file

@ -1,42 +0,0 @@
/*
*****************************************************************************
**
** File : stm32_flash_f405.ld
**
** Abstract : Linker script for STM32F405RG Device with
** 1024KByte FLASH, 128KByte RAM 64KByte CCM (RAM)
**
*****************************************************************************
*/
/* Stack & Heap sizes */
_Min_Heap_Size = 0;
_Min_Stack_Size = 0x1800;
/* Entry Point */
ENTRY(Reset_Handler)
/*
0x08000000 to 0x08100000 1024K full flash,
0x08000000 to 0x080DFFFF 896K firmware,
0x080E0000 to 0x08100000 128K config, // FLASH_Sector_11
*/
/* Specify the memory areas */
MEMORY
{
FLASH (rx) : ORIGIN = 0x08008000, LENGTH = 864K
FLASH_CONFIG (r) : ORIGIN = 0x080E0000, LENGTH = 128K
RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K
CCM (rwx) : ORIGIN = 0x10000000, LENGTH = 64K
BACKUP_SRAM (rwx) : ORIGIN = 0x40024000, LENGTH = 4K
MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K
}
REGION_ALIAS("STACKRAM", CCM)
REGION_ALIAS("FASTRAM", CCM)
__firmware_start = ORIGIN(FLASH);
INCLUDE "stm32_flash.ld"

40
src/main/target/link/stm32_flash_F405_opbl.ld
View file

@ -1,40 +0,0 @@
/*
*****************************************************************************
**
** File : stm32_flash_f405.ld
**
** Abstract : Linker script for STM32F405RG Device with
** 1024KByte FLASH, 128KByte RAM 64KByte CCM (RAM)
**
*****************************************************************************
*/
/* Stack & Heap sizes */
_Min_Heap_Size = 0;
_Min_Stack_Size = 0x1800;
/* Entry Point */
ENTRY(Reset_Handler)
/*
0x08000000 to 0x08100000 1024K full flash,
0x08000000 to 0x08004000 16K OPBL,
0x08004000 to 0x080DFFFF 880K firmware,
0x080E0000 to 0x08100000 128K config, // FLASH_Sector_11
*/
/* Specify the memory areas */
MEMORY
{
FLASH (rx) : ORIGIN = 0x08004000, LENGTH = 880K
FLASH_CONFIG (r): ORIGIN = 0x080E0000, LENGTH = 128K
RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K
CCM (rwx) : ORIGIN = 0x10000000, LENGTH = 64K
MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K
}
REGION_ALIAS("STACKRAM", CCM)
REGION_ALIAS("FASTRAM", CCM)
INCLUDE "stm32_flash.ld"

40
src/main/target/link/stm32_flash_F411.ld
View file

@ -1,40 +0,0 @@
/*
*****************************************************************************
**
** File : stm32_flash_f411.ld
**
** Abstract : Linker script for STM32F11 Device with
** 512KByte FLASH, 128KByte RAM
**
*****************************************************************************
*/
/* Stack & Heap sizes */
_Min_Heap_Size = 0;
_Min_Stack_Size = 0x1800;
/* Entry Point */
ENTRY(Reset_Handler)
/*
0x08000000 to 0x0807FFFF 512K full flash,
0x08000000 to 0x08003FFF 16K isr vector, startup code,
0x08004000 to 0x08007FFF 16K config, // FLASH_Sector_1
0x08008000 to 0x0807FFFF 480K firmware,
*/
/* Specify the memory areas */
MEMORY
{
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 16K
FLASH_CONFIG (r) : ORIGIN = 0x08004000, LENGTH = 16K
FLASH1 (rx) : ORIGIN = 0x08008000, LENGTH = 480K
RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K
MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K
}
REGION_ALIAS("STACKRAM", RAM)
REGION_ALIAS("FASTRAM", RAM)
INCLUDE "stm32_flash_split.ld"

40
src/main/target/link/stm32_flash_F411_opbl.ld
View file

@ -1,40 +0,0 @@
/*
*****************************************************************************
**
** File : stm32_flash_f411.ld
**
** Abstract : Linker script for STM32F411 Device with
** 512KByte FLASH, 128KByte RAM
**
*****************************************************************************
*/
/* Stack & Heap sizes */
_Min_Heap_Size = 0;
_Min_Stack_Size = 0x1800;
/* Entry Point */
ENTRY(Reset_Handler)
/*
0x08000000 to 0x08080000 512K full flash,
0x08000000 to 0x08004000 16K OPBL,
0x08004000 to 0x0805FFFF 368K firmware,
0x08060000 to 0x08080000 128K config,
*/
/* Specify the memory areas */
MEMORY
{
FLASH (rx) : ORIGIN = 0x08004000, LENGTH = 368K
FLASH_CONFIG (r) : ORIGIN = 0x08060000, LENGTH = 128K
RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K
CCM (xrw) : ORIGIN = 0x10000000, LENGTH = 64K
MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K
}
REGION_ALIAS("STACKRAM", CCM)
REGION_ALIAS("FASTRAM", CCM)
INCLUDE "stm32_flash.ld"

59
src/main/target/link/stm32_flash_F427.ld
View file

@ -1,59 +0,0 @@
/*
*****************************************************************************
**
** File : stm32_flash.ld
**
** Abstract : Linker script for STM32F427 Device with
** 2048 KByte FLASH, 192KByte RAM
**
** Set heap size, stack size and stack location according
** to application requirements.
**
** Set memory bank area and size if external memory is used.
**
** Target : STMicroelectronics STM32
**
** Environment : Atollic TrueSTUDIO(R)
**
** Distribution: The file is distributed as is, without any warranty
** of any kind.
**
** (c)Copyright Atollic AB.
** You may use this file as-is or modify it according to the needs of your
** project. Distribution of this file (unmodified or modified) is not
** permitted. Atollic AB permit registered Atollic TrueSTUDIO(R) users the
** rights to distribute the assembled, compiled & linked contents of this
** file as part of an application binary file, provided that it is built
** using the Atollic TrueSTUDIO(R) toolchain.
**
*****************************************************************************
*/
/* Stack & Heap sizes */
_Min_Heap_Size = 0;
_Min_Stack_Size = 0x1800;
/* Entry Point */
ENTRY(Reset_Handler)
/*
0x08000000 to 0x08100000 1024K full flash,
0x08000000 to 0x080DFFFF 896K firmware,
0x080E0000 to 0x08100000 128K config, // FLASH_Sector_11
*/
/* Specify the memory areas */
MEMORY
{
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 896K
FLASH_CONFIG (r) : ORIGIN = 0x080E0000, LENGTH = 128K
RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K
CCM (rwx) : ORIGIN = 0x10000000, LENGTH = 64K
MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K
}
REGION_ALIAS("STACKRAM", CCM)
REGION_ALIAS("FASTRAM", CCM)
INCLUDE "stm32_flash.ld"

Some files were not shown because too many files have changed in this diff Show more