Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 16:55:29 +03:00
Code Issues Wiki Activity

Merge pull request #5 from iNavFlight/master

Browse source 
Merge with master fork
This commit is contained in:
Darren Lines 2020-11-17 08:27:38 +00:00 • committed by GitHub
commit db8e8f5b6d
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
27 changed files with 1437 additions and 328 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).

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 | | |

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

@ -109,6 +109,9 @@ static const OSD_Entry cmsx_menuFWCruiseEntries[] =
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,
};
@ -132,6 +135,7 @@ static const OSD_Entry cmsx_menuFWLaunchEntries[] =
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),

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;
}
}

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

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;

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

@ -149,6 +149,12 @@ tables:
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
@ -2101,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"
@ -2278,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"
@ -2353,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"
@ -3126,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"]
@ -3146,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

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;
}
}

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;

43
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;
@ -1844,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;
@ -2325,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
@ -2411,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;
}
@ -3324,6 +3349,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

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

@ -215,6 +215,7 @@ typedef enum {
OSD_GVAR_1,
OSD_GVAR_2,
OSD_GVAR_3,
OSD_TPA,
OSD_ITEM_COUNT // MUST BE LAST
} osd_items_e;

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);

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

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

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)
@ -461,7 +470,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 {
@ -504,6 +513,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);
@ -516,6 +526,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