mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-19 06:15:14 +03:00
Merge branch 'development' into dzikuvx-nav-yaw-adjustments
This commit is contained in:
Pawel Spychalski (DzikuVx)
commit
e3b0327657
92 changed files with 1437 additions and 455 deletions
4
Makefile
4
Makefile
|
|
@ -185,9 +185,11 @@ endif
|
|||
ifneq ($(SEMIHOSTING),)
|
||||
SEMIHOSTING_CFLAGS = -DSEMIHOSTING
|
||||
SEMIHOSTING_LDFLAGS = --specs=rdimon.specs -lc -lrdimon
|
||||
SYSLIB :=
|
||||
else
|
||||
SEMIHOSTING_CFLAGS =
|
||||
SEMIHOSTING_LDFLAGS =
|
||||
SYSLIB := -lnosys
|
||||
endif
|
||||
|
||||
DEBUG_FLAGS = -ggdb3 -DDEBUG
|
||||
|
|
@ -224,7 +226,7 @@ LDFLAGS = -lm \
|
|||
-nostartfiles \
|
||||
--specs=nano.specs \
|
||||
-lc \
|
||||
-lnosys \
|
||||
$(SYSLIB) \
|
||||
$(ARCH_FLAGS) \
|
||||
$(LTO_FLAGS) \
|
||||
$(DEBUG_FLAGS) \
|
||||
|
|
|
|||
24
docs/Cli.md
24
docs/Cli.md
|
|
@ -138,8 +138,8 @@ A shorter form is also supported to enable and disable functions using `serial <
|
|||
| rssi_max | 100 | The maximum RSSI value sent by the receiver, in %. For example, if your receiver's maximum RSSI value shows as 83% in the configurator/OSD set this parameter to 83. See also rssi_min. |
|
||||
| rc_filter_frequency | 50 | RC data biquad filter cutoff frequency. Lower cutoff frequencies result in smoother response at expense of command control delay. Practical values are 20-50. Set to zero to disable entirely and use unsmoothed RC stick values |
|
||||
| input_filtering_mode | OFF | Filter out noise from OpenLRS Telemetry RX |
|
||||
| min_throttle | 1150 | These are min/max values (in us) that are sent to esc when armed. Defaults of 1150/1850 are OK for everyone, for use with AfroESC, they could be set to 1064/1864. |
|
||||
| max_throttle | 1850 | These are min/max values (in us) that are sent to esc when armed. Defaults of 1150/1850 are OK for everyone, for use with AfroESC, they could be set to 1064/1864. If you have brushed motors, the value should be set to 2000. |
|
||||
| throttle_idle | 15 | The percentage of the throttle range (`max_throttle` - `min_command`) above `min_command` used for minimum / idle throttle. |
|
||||
| max_throttle | 1850 | This is the maximum value (in us) sent to esc when armed. Default of 1850 are OK for everyone (legacy). For modern ESCs, higher values (c. 2000) may be more appropriate. If you have brushed motors, the value should be set to 2000. |
|
||||
| min_command | 1000 | This is the PWM value sent to ESCs when they are not armed. If ESCs beep slowly when powered up, try decreasing this value. It can also be used for calibrating all ESCs at once. |
|
||||
| 3d_deadband_low | 1406 | Low value of throttle deadband for 3D mode (when stick is in the 3d_deadband_throttle range, the fixed values of 3d_deadband_low / _high are used instead) |
|
||||
| 3d_deadband_high | 1514 | High value of throttle deadband for 3D mode (when stick is in the deadband range, the value in 3d_neutral is used instead) |
|
||||
|
|
@ -182,7 +182,7 @@ A shorter form is also supported to enable and disable functions using `serial <
|
|||
| nav_user_control_mode | ATTI | Defines how Pitch/Roll input from RC receiver affects flight in POSHOLD mode: ATTI - pitch/roll controls attitude like in ANGLE mode; CRUISE - pitch/roll controls velocity in forward and right direction. |
|
||||
| nav_position_timeout | 5 | If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) |
|
||||
| nav_wp_radius | 100 | Waypoint radius [cm]. Waypoint would be considered reached if machine is within this radius |
|
||||
| nav_wp_safe_distance | 10000 | First waypoint in the mission should be closer than this value [cm] |
|
||||
| nav_wp_safe_distance | 10000 | First waypoint in the mission should be closer than this value [cm]. A value of 0 disables this check. |
|
||||
| nav_auto_speed | 300 | Maximum velocity firmware is allowed in full auto modes (RTH, WP) [cm/s] [Multirotor only] |
|
||||
| nav_auto_climb_rate | 500 | Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s] |
|
||||
| nav_manual_speed | 500 | Maximum velocity firmware is allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only] |
|
||||
|
|
@ -221,9 +221,9 @@ A shorter form is also supported to enable and disable functions using `serial <
|
|||
| nav_fw_launch_max_angle | 45 | Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg] |
|
||||
| nav_fw_launch_detect_time | 40 | Time for which thresholds have to breached to consider launch happened [ms] |
|
||||
| nav_fw_launch_thr | 1700 | Launch throttle - throttle to be set during launch sequence (pwm units) |
|
||||
| nav_fw_launch_idle_thr | 1000 | Launch idle throttle - throttle to be set before launch sequence is initiated. If set below min_throttle it will force motor stop or at idle throttle (depending if the MOTOR_STOP is enabled). If set above min_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_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_motor_delay | 500 | Delay between detected launch and launch sequence start and throttling up (ms) |
|
||||
| nav_fw_launch_spinup_time | 100 | Time to bring power from min_throttle to nav_fw_launch_thr - to avoid big stress on ESC and large torque from propeller |
|
||||
| nav_fw_launch_spinup_time | 100 | Time to bring power from minimum throttle to nav_fw_launch_thr - to avoid big stress on ESC and large torque from propeller |
|
||||
| nav_fw_launch_timeout | 5000 | Maximum time for launch sequence to be executed. After this time LAUNCH mode will be turned off and regular flight mode will take over (ms) |
|
||||
| nav_fw_launch_max_altitude | 0 | Altitude at which LAUNCH mode will be turned off and regular flight mode will take over. [cm] |
|
||||
| nav_fw_launch_climb_angle | 18 | Climb angle for launch sequence (degrees), is also restrained by global max_angle_inclination_pit |
|
||||
|
|
@ -290,7 +290,6 @@ A shorter form is also supported to enable and disable functions using `serial <
|
|||
| pos_hold_deadband | 20 | Stick deadband in [r/c points], applied after r/c deadband and expo |
|
||||
| alt_hold_deadband | 50 | Defines the deadband of throttle during alt_hold [r/c points] |
|
||||
| yaw_motor_direction | 1 | Use if you need to inverse yaw motor direction. |
|
||||
| yaw_jump_prevention_limit | 200 | Prevent yaw jumps during yaw stops and rapid YAW input. To disable set to 500. Adjust this if your aircraft 'skids out'. Higher values increases YAW authority but can cause roll/pitch instability in case of underpowered UAVs. Lower values makes yaw adjustments more gentle but can cause UAV unable to keep heading |
|
||||
| tri_unarmed_servo | ON | On tricopter mix only, if this is set to ON, servo will always be correcting regardless of armed state. to disable this, set it to OFF. |
|
||||
| servo_lpf_hz | 20 | Selects the servo PWM output cutoff frequency. Value is in [Hz] |
|
||||
| servo_center_pulse | 1500 | Servo midpoint |
|
||||
|
|
@ -364,8 +363,6 @@ A shorter form is also supported to enable and disable functions using `serial <
|
|||
| nav_mc_vel_z_i | 50 | I gain of velocity PID controller |
|
||||
| nav_mc_vel_z_d | 10 | D gain of velocity PID controller |
|
||||
| nav_mc_pos_xy_p | 65 | Controls how fast the drone will fly towards the target position. This is a multiplier to convert displacement to target velocity |
|
||||
| nav_mc_pos_xy_i | 120 | Controls deceleration time. Measured in 1/100 sec. Expected hold position is placed at a distance calculated as decelerationTime * currentVelocity |
|
||||
| nav_mc_pos_xy_d | 10 | |
|
||||
| nav_mc_vel_xy_p | 40 | P gain of Position-Rate (Velocity to Acceleration) PID controller. Higher P means stronger response when position error occurs. Too much P might cause "nervous" behavior and oscillations |
|
||||
| nav_mc_vel_xy_i | 15 | I gain of Position-Rate (Velocity to Acceleration) PID controller. Used for drift compensation (caused by wind for example). Higher I means stronger response to drift. Too much I gain might cause target overshot |
|
||||
| nav_mc_vel_xy_d | 100 | D gain of Position-Rate (Velocity to Acceleration) PID controller. It can damp P and I. Increasing D might help when drone overshoots target. |
|
||||
|
|
@ -420,14 +417,23 @@ A shorter form is also supported to enable and disable functions using `serial <
|
|||
| acc_lpf_hz | 15 | Software-based filter to remove mechanical vibrations from the accelerometer measurements. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value. |
|
||||
| acc_lpf_type | BIQUAD | Specifies the type of the software LPF of the acc signals. BIQUAD gives better filtering and more delay, PT1 less filtering and less delay, so use only on clean builds. |
|
||||
| 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 |
|
||||
| dterm_lpf_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_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. |
|
||||
| yaw_lpf_hz | 30 | Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below) |
|
||||
| dyn_notch_width_percent | 8 | Distance in % of the attenuated frequency for double dynamic filter notched. When set to `0` single dynamic notch filter is used |
|
||||
| dyn_notch_range | MEDIUM | Dynamic gyro filter range. Possible values `LOW` `MEDIUM` `HIGH`. `MEDIUM` should work best for 5-6" multirotors. `LOW` should work best with 7" and bigger. `HIGH` should work with everything below 4" |
|
||||
| dyn_notch_q | 120 | Q factor for dynamic notches |
|
||||
| dyn_notch_min_hz | 150 | Minimum frequency for dynamic notches. Default value of `150` works best with 5" multirors. Should be lowered with increased size of propellers. Values around `100` work fine on 7" drones. 10" can go down to `60` - `70` |
|
||||
| gyro_stage2_lowpass_hz | 0 | Software based second stage lowpass filter for gyro. Value is cutoff frequency (Hz). Currently experimental |
|
||||
| gyro_stage2_lowpass_hz | 0 | Software based second stage lowpass filter for gyro. Value is cutoff frequency (Hz) |
|
||||
| gyro_stage2_lowpass_type | `BIQUAD` | Defines the type of stage 2 gyro LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. |
|
||||
| rpm_gyro_filter_enabled | `OFF` | Enables gyro RPM filtere. Set to `ON` only when ESC telemetry is working and rotation speed of the motors is correctly reported to INAV |
|
||||
| rpm_gyro_harmonics | 1 | Number of harmonic frequences to be covered by gyro RPM filter. Default value of `1` usually works just fine |
|
||||
| rpm_gyro_min_hz | 150 | The lowest frequency for gyro RPM filtere. Default `150` is fine for 5" mini-quads. On 7-inch drones you can lower even down to `60`-`70` |
|
||||
| rpm_gyro_q | 500 | Q factor for gyro RPM filter. Lower values give softer, wider attenuation. Usually there is no need to change this setting |
|
||||
| pidsum_limit | 500 | A limitation to overall amount of correction Flight PID can request on each axis (Roll/Pitch). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help |
|
||||
| pidsum_limit_yaw | 400 | A limitation to overall amount of correction Flight PID can request on each axis (Yaw). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help |
|
||||
| `pid_type` | Allows to set type of PID controller used in control loop. Possible values: `NONE`, `PID`, `PIFF`, `AUTO`. Change only in case of experimental platforms like VTOL, tailsitters, rovers, boats, etc. Airplanes should always use `PIFF` and multirotors `PID` |
|
||||
| iterm_windup | 50 | Used to prevent Iterm accumulation on during maneuvers. Iterm will be dampened when motors are reaching it's limit (when requested motor correction range is above percentage specified by this parameter) |
|
||||
| rate_accel_limit_roll_pitch | 0 | Limits acceleration of ROLL/PITCH rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 5000 dps^2 and even > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 360 dps^2). When set correctly, it greatly improves stopping performance. Value of 0 disables limiting. |
|
||||
| rate_accel_limit_yaw | 10000 | Limits acceleration of YAW rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 180 dps^2). When set correctly, it greatly improves stopping performance and general stability during yaw turns. Value of 0 disables limiting. |
|
||||
|
|
|
|||
|
|
@ -2,12 +2,9 @@
|
|||
|
||||
## Arming
|
||||
|
||||
When armed, the aircraft is ready to fly and the motors will spin when throttle is applied. The motors will
|
||||
spin at a slow speed when armed (this feature may be disabled by setting MOTOR_STOP, but for safety reasons,
|
||||
that is not recommended).
|
||||
When armed, the aircraft is ready to fly and the motors will spin when throttle is applied. The motors will spin at a slow speed when armed (this feature may be disabled by setting MOTOR_STOP, but for safety reasons, that is not recommended).
|
||||
|
||||
By default, arming and disarming is done using stick positions. (NOTE: this feature is disabled when using a
|
||||
switch to arm.)
|
||||
By default, arming and disarming is done using stick positions. (NOTE: this feature is disabled when using a switch to arm.)
|
||||
|
||||
## Stick Positions
|
||||
|
||||
|
|
@ -45,14 +42,9 @@ The stick positions are combined to activate different functions:
|
|||
|
||||
## Yaw control
|
||||
|
||||
While arming/disarming with sticks, your yaw stick will be moving to extreme values. In order to prevent your craft
|
||||
from trying to yaw during arming/disarming while on the ground, your yaw input will not cause the craft to yaw when the
|
||||
throttle is LOW (i.e. below the `min_check` setting).
|
||||
While arming/disarming with sticks, your yaw stick will be moving to extreme values. In order to prevent your craft from trying to yaw during arming/disarming while on the ground, your yaw input will not cause the craft to yaw when the throttle is LOW (i.e. below the `min_check` setting).
|
||||
|
||||
For tricopters, you may want to retain the ability to yaw while on the ground, so that you can verify that your tail
|
||||
servo is working correctly before takeoff. You can do this by setting `tri_unarmed_servo` to `1` on the CLI (this is the
|
||||
default). If you are having issues with your tail rotor contacting the ground during arm/disarm, you can set this to
|
||||
`0` instead. Check this table to decide which setting will suit you:
|
||||
For tricopters, you may want to retain the ability to yaw while on the ground, so that you can verify that your tail servo is working correctly before takeoff. You can do this by setting `tri_unarmed_servo` to `1` on the CLI (this is the default). If you are having issues with your tail rotor contacting the ground during arm/disarm, you can set this to `0` instead. Check this table to decide which setting will suit you:
|
||||
|
||||
<table>
|
||||
<tr>
|
||||
|
|
@ -78,23 +70,24 @@ default). If you are having issues with your tail rotor contacting the ground du
|
|||
</tr>
|
||||
</table>
|
||||
|
||||
|
||||
## Throttle settings and their interaction
|
||||
|
||||
*Terminology. After iNav 2.3, the setting `min_throttle` was replaced with `throttle_idle` which is more appropriate to modern hardware. In this document `min_throttle` may be taken as either the older `min_throttle` value, or the throttle value calculated from the modern `throttle_idle` setting. The way that `throttle_idle` generates a throttle value is described in `Cli.md`.*
|
||||
|
||||
`min_command` -
|
||||
With motor stop enabled this is the command sent to the esc's when the throttle is below min_check or disarmed. With motor stop disabled, this is the command sent only when the copter is disarmed. This must be set well below motors spinning for safety.
|
||||
|
||||
`min_check` -
|
||||
With switch arming mode is in use, lowering your throttle below min_check will result in motors spinning at min_throttle. When using the default stick arming, lowering your throttle below min_check will result in motors spinning at min_throttle and yaw being disabled so that you may arm/disarm. With motor stop enabled, lowering your throttle below min_check will also result in motors off and the esc's being sent min_command. Min_check must be set to a level that is 100% reliably met by the throttle throw. A setting too low may result in a dangerous condition where the copter can’t be disarmed. It is ok to set this below min_throttle because the FC will automaticly scale the output to the esc's
|
||||
With switch arming mode is in use, lowering your throttle below min_check will result in motors spinning at `throttle_idle` (min_throttle). When using the default stick arming, lowering your throttle below min_check will result in motors spinning at min_throttle and yaw being disabled so that you may arm/disarm. With motor stop enabled, lowering your throttle below min_check will also result in motors off and the esc's being sent min_command. Min_check must be set to a level that is 100% reliably met by the throttle throw. A setting too low may result in a dangerous condition where the copter can’t be disarmed. It is ok to set this below `throttle_idle` (min_throttle) because the FC will automaticly scale the output to the ESCs
|
||||
|
||||
`min_throttle` -
|
||||
`throttle_idle` (previously `min_throttle)` -
|
||||
Typically set to just above reliable spin up of all motors. Sometimes this is set slightly higher for prop stall prevention during advanced maneuvers or sometimes considerably higher to produce a desired result. When armed with motor stop off, your motors will spin at this command so keep that in mind from a safety stand point.
|
||||
|
||||
`max_check` -
|
||||
Throttle positions above this level will send max_command to the esc's.
|
||||
Throttle positions above this level will send max_command to the ESCs.
|
||||
|
||||
`max_throttle` -
|
||||
This is the max command to the esc's from the flight controller.
|
||||
This is the max command to the ESCs from the flight controller.
|
||||
|
||||
In depth videos explaining these terms are available from Joshua Bardwell here:
|
||||
|
||||
|
|
|
|||
127
docs/development/serial_printf_debugging.md
Normal file
127
docs/development/serial_printf_debugging.md
Normal file
|
|
@ -0,0 +1,127 @@
|
|||
# Serial printf style debugging
|
||||
|
||||
## Overview
|
||||
|
||||
inav offers a function to use serial `printf` style debugging.
|
||||
|
||||
This provides a simple and intuitive debugging facility. This facility is only available after the serial sub-system has been initialised, which should be adequate for all but the most hard-core debugging requirements.
|
||||
|
||||
In order to use this feature, the source file must include `common/log.h`.
|
||||
|
||||
## CLI settings
|
||||
|
||||
It is necessary to set a serial port for serial logging using the function mask `FUNCTION_LOG`, 32768. For convenience this may be shared with MSP (mask 1), but no other function.
|
||||
For example, on a VCP port.
|
||||
|
||||
```
|
||||
serial 20 32769 115200 115200 0 115200
|
||||
```
|
||||
|
||||
If the port is shared, it will be resused with extant settings; if the port is not shared it is opened at 921600 baud.
|
||||
|
||||
There are two run time settings that control the verbosity, the most verbose settings being:
|
||||
|
||||
```
|
||||
log_level = DEBUG
|
||||
Allowed values: ERROR, WARNING, INFO, VERBOSE, DEBUG
|
||||
|
||||
log_topics = 0
|
||||
Allowed range: 0 - 4294967295
|
||||
|
||||
```
|
||||
|
||||
The use of level and topics is described in the following sections.
|
||||
|
||||
## LOG LEVELS
|
||||
|
||||
Log levels are defined in `src/main/common/log.h`, at the time of writing these include (in ascending order):
|
||||
|
||||
* ERROR
|
||||
* WARNING
|
||||
* INFO
|
||||
* VERBOSE
|
||||
* DEBUG
|
||||
|
||||
These are used at both compile time and run time.
|
||||
|
||||
At compile time, a maximum level may be defined. As of inav 2.3, for F3 targets the maximum level is ERROR, for F4/F7 the maximum level is DEBUG.
|
||||
|
||||
At run time, the level defines the level that will be displayed, so for a F4 or F7 target that has compile time suport for all log levels, if the CLI sets
|
||||
```
|
||||
log_level = INFO
|
||||
```
|
||||
then only `ERROR`, `WARNING` and `INFO` levels will be output.
|
||||
|
||||
## Log Topic
|
||||
|
||||
Log topics are defined in `src/main/common/log.h`, at the time of writing:
|
||||
|
||||
* SYSTEM
|
||||
* GYRO
|
||||
* BARO
|
||||
* PITOT
|
||||
* PWM
|
||||
* TIMER
|
||||
* IMU
|
||||
* TEMPERATURE
|
||||
* POS_ESTIMATOR
|
||||
* VTX
|
||||
* OSD
|
||||
|
||||
Topics are stored as masks (SYSTEM=1 ... OSD=1024) and may be used to unconditionally display log messages.
|
||||
|
||||
If the CLI `log_topics` is non-zero, then all topics matching the mask will be displayed regardless of `log_level`. Setting `log_topics` to 4294967295 (all bits set) will display all log messages regardless of run time level (but still constrained by compile time settings), so F3 will still only display ERROR level messages.
|
||||
|
||||
## Code usage
|
||||
|
||||
A set of macros `LOG_S()` (log system) through `LOG_D()` (log debug) may be used, subject to compile time log level constraints. These provide `printf` style logging for a given topic.
|
||||
|
||||
```
|
||||
// LOG_D(topic, fmt, ...)
|
||||
LOG_D(SYSTEM, "This is %s topic debug message, value %d", "system", 42);
|
||||
```
|
||||
|
||||
It is also possible to dump a hex representation of arbitrary data:
|
||||
|
||||
```
|
||||
// LOG_BUF_D(topic, buf, size)
|
||||
|
||||
struct {...} tstruct;
|
||||
...
|
||||
LOG_BUF_D(TEMPERATURE, &tstruct, sizeof(tstruct));
|
||||
|
||||
```
|
||||
|
||||
## Output Support
|
||||
|
||||
Log messages are transmitted through the `FUNCTION_LOG` serial port as MSP messages (`MSP_DEBUGMSG`). It is possible to use any serial terminal to display these messages, however it is advisable to use an application that understands `MSP_DEBUGMSG` in order to maintain readability (in a raw serial terminal the MSP message envelope may result in the display of strange characters). `MSP_DEBUGMSG` aware applications include:
|
||||
|
||||
* msp-tool https://github.com/fiam/msp-tool
|
||||
* mwp https://github.com/stronnag/mwptools
|
||||
* inav Configurator
|
||||
|
||||
For example, with the final lines of `src/main/fc/fc_init.c` set to:
|
||||
|
||||
```
|
||||
LOG_E(SYSTEM, "Init is complete");
|
||||
systemState |= SYSTEM_STATE_READY;
|
||||
```
|
||||
|
||||
and the following CLI settings:
|
||||
|
||||
```
|
||||
serial 20 32769 115200 115200 0 115200
|
||||
set log_level = DEBUG
|
||||
set log_topics = 4294967295
|
||||
```
|
||||
|
||||
The output will be formatted as follows:
|
||||
|
||||
```
|
||||
# msp-tool
|
||||
[DEBUG] [ 3.967] Init is complete
|
||||
# mwp (stderr log file)
|
||||
2020-02-02T19:09:02+0000 DEBUG:[ 3.968] Init is complete
|
||||
```
|
||||
|
||||
The numeric value in square brackets is the FC uptime in seconds.
|
||||
|
|
@ -25,7 +25,7 @@ RELEASE_TARGETS += NOX WINGFC
|
|||
|
||||
RELEASE_TARGETS += OMNIBUSF4V6
|
||||
|
||||
RELEASE_TARGETS += MAMBAF405
|
||||
RELEASE_TARGETS += MAMBAF405 MAMBAF405US MAMBAF722
|
||||
|
||||
RELEASE_TARGETS += IFLIGHTF7_TWING IFLIGHTF4_TWING
|
||||
|
||||
|
|
|
|||
|
|
@ -101,6 +101,7 @@ COMMON_SRC = \
|
|||
io/beeper.c \
|
||||
io/esc_serialshot.c \
|
||||
io/frsky_osd.c \
|
||||
io/osd_dji_hd.c \
|
||||
io/lights.c \
|
||||
io/piniobox.c \
|
||||
io/pwmdriver_i2c.c \
|
||||
|
|
|
|||
|
|
@ -7,8 +7,9 @@
|
|||
#
|
||||
###############################################################
|
||||
|
||||
GCC_REQUIRED_VERSION ?= 8.2.1
|
||||
ARM_SDK_DIR ?= $(TOOLS_DIR)/gcc-arm-none-eabi-8-2018-q4-major
|
||||
GCC_REQUIRED_VERSION ?= 9.2.1
|
||||
ARM_SDK_DIR ?= $(TOOLS_DIR)/gcc-arm-none-eabi-9-2019-q4-major
|
||||
ARM_SDK_URL_BASE := https://armkeil.blob.core.windows.net/developer/Files/downloads/gnu-rm/9-2019q4/gcc-arm-none-eabi-9-2019-q4-major
|
||||
|
||||
.PHONY: arm_sdk_version
|
||||
|
||||
|
|
@ -17,11 +18,9 @@ arm_sdk_version:
|
|||
|
||||
.PHONY: arm_sdk_install
|
||||
|
||||
ARM_SDK_URL_BASE := https://developer.arm.com/-/media/Files/downloads/gnu-rm/8-2018q4/gcc-arm-none-eabi-8-2018-q4-major
|
||||
|
||||
# source: https://developer.arm.com/open-source/gnu-toolchain/gnu-rm/downloads
|
||||
ifdef LINUX
|
||||
ARM_SDK_URL := $(ARM_SDK_URL_BASE)-linux.tar.bz2
|
||||
ARM_SDK_URL := $(ARM_SDK_URL_BASE)-x86_64-linux.tar.bz2
|
||||
endif
|
||||
|
||||
ifdef MACOSX
|
||||
|
|
|
|||
|
|
@ -624,14 +624,14 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
|
|||
|
||||
case FLIGHT_LOG_FIELD_CONDITION_FIXED_WING_NAV:
|
||||
#ifdef USE_NAV
|
||||
return STATE(FIXED_WING);
|
||||
return STATE(FIXED_WING_LEGACY);
|
||||
#else
|
||||
return false;
|
||||
#endif
|
||||
|
||||
case FLIGHT_LOG_FIELD_CONDITION_MC_NAV:
|
||||
#ifdef USE_NAV
|
||||
return !STATE(FIXED_WING);
|
||||
return !STATE(FIXED_WING_LEGACY);
|
||||
#else
|
||||
return false;
|
||||
#endif
|
||||
|
|
@ -755,7 +755,7 @@ static void writeIntraframe(void)
|
|||
* Write the throttle separately from the rest of the RC data so we can apply a predictor to it.
|
||||
* Throttle lies in range [minthrottle..maxthrottle]:
|
||||
*/
|
||||
blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[THROTTLE] - motorConfig()->minthrottle);
|
||||
blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[THROTTLE] - getThrottleIdleValue());
|
||||
|
||||
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
|
||||
/*
|
||||
|
|
@ -809,7 +809,7 @@ static void writeIntraframe(void)
|
|||
}
|
||||
|
||||
//Motors can be below minthrottle when disarmed, but that doesn't happen much
|
||||
blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - motorConfig()->minthrottle);
|
||||
blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - getThrottleIdleValue());
|
||||
|
||||
//Motors tend to be similar to each other so use the first motor's value as a predictor of the others
|
||||
const int motorCount = getMotorCount();
|
||||
|
|
@ -1369,7 +1369,7 @@ static void loadMainState(timeUs_t currentTimeUs)
|
|||
blackboxCurrent->magADC[i] = mag.magADC[i];
|
||||
#endif
|
||||
#ifdef USE_NAV
|
||||
if (!STATE(FIXED_WING)) {
|
||||
if (!STATE(FIXED_WING_LEGACY)) {
|
||||
// log requested velocity in cm/s
|
||||
blackboxCurrent->mcPosAxisP[i] = lrintf(nav_pids->pos[i].output_constrained);
|
||||
|
||||
|
|
@ -1384,7 +1384,7 @@ static void loadMainState(timeUs_t currentTimeUs)
|
|||
}
|
||||
|
||||
#ifdef USE_NAV
|
||||
if (STATE(FIXED_WING)) {
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
|
||||
// log requested pitch in decidegrees
|
||||
blackboxCurrent->fwAltPID[0] = lrintf(nav_pids->fw_alt.proportional);
|
||||
|
|
@ -1618,10 +1618,10 @@ static bool blackboxWriteSysinfo(void)
|
|||
BLACKBOX_PRINT_HEADER_LINE("Log start datetime", "%s", blackboxGetStartDateTime(buf));
|
||||
BLACKBOX_PRINT_HEADER_LINE("Craft name", "%s", systemConfig()->name);
|
||||
BLACKBOX_PRINT_HEADER_LINE("P interval", "%u/%u", blackboxConfig()->rate_num, blackboxConfig()->rate_denom);
|
||||
BLACKBOX_PRINT_HEADER_LINE("minthrottle", "%d", motorConfig()->minthrottle);
|
||||
BLACKBOX_PRINT_HEADER_LINE("minthrottle", "%d", getThrottleIdleValue());
|
||||
BLACKBOX_PRINT_HEADER_LINE("maxthrottle", "%d", motorConfig()->maxthrottle);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_scale", "0x%x", castFloatBytesToInt(1.0f));
|
||||
BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d", motorConfig()->minthrottle,motorConfig()->maxthrottle);
|
||||
BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d", getThrottleIdleValue(),motorConfig()->maxthrottle);
|
||||
BLACKBOX_PRINT_HEADER_LINE("acc_1G", "%u", acc.dev.acc_1G);
|
||||
|
||||
BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
|
||||
|
|
@ -1682,12 +1682,21 @@ static bool blackboxWriteSysinfo(void)
|
|||
pidBank()->pid[PID_VEL_Z].D);
|
||||
BLACKBOX_PRINT_HEADER_LINE("yaw_lpf_hz", "%d", pidProfile()->yaw_lpf_hz);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_hz", "%d", pidProfile()->dterm_lpf_hz);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_type", "%d", pidProfile()->dterm_lpf_type);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dterm_lpf2_hz", "%d", pidProfile()->dterm_lpf2_hz);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dterm_lpf2_type", "%d", pidProfile()->dterm_lpf2_type);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dterm_notch_hz", "%d", pidProfile()->dterm_soft_notch_hz);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dterm_notch_cutoff", "%d", pidProfile()->dterm_soft_notch_cutoff);
|
||||
BLACKBOX_PRINT_HEADER_LINE("deadband", "%d", rcControlsConfig()->deadband);
|
||||
BLACKBOX_PRINT_HEADER_LINE("yaw_deadband", "%d", rcControlsConfig()->yaw_deadband);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf", "%d", gyroConfig()->gyro_lpf);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_hz", "%d", gyroConfig()->gyro_soft_lpf_hz);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_type", "%d", gyroConfig()->gyro_soft_lpf_type);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf2_hz", "%d", gyroConfig()->gyro_stage2_lowpass_hz);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_width_percent", "%d", gyroConfig()->dyn_notch_width_percent);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_range", "%d", gyroConfig()->dyn_notch_range);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_q", "%d", gyroConfig()->dyn_notch_q);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_min_hz", "%d", gyroConfig()->dyn_notch_min_hz);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz", "%d,%d", gyroConfig()->gyro_soft_notch_hz_1,
|
||||
gyroConfig()->gyro_soft_notch_hz_2);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1,
|
||||
|
|
@ -1717,10 +1726,6 @@ static bool blackboxWriteSysinfo(void)
|
|||
BLACKBOX_PRINT_HEADER_LINE("rpm_gyro_harmonics", "%d", rpmFilterConfig()->gyro_harmonics);
|
||||
BLACKBOX_PRINT_HEADER_LINE("rpm_gyro_min_hz", "%d", rpmFilterConfig()->gyro_min_hz);
|
||||
BLACKBOX_PRINT_HEADER_LINE("rpm_gyro_q", "%d", rpmFilterConfig()->gyro_q);
|
||||
BLACKBOX_PRINT_HEADER_LINE("rpm_dterm_filter_enabled", "%d", rpmFilterConfig()->dterm_filter_enabled);
|
||||
BLACKBOX_PRINT_HEADER_LINE("rpm_dterm_harmonics", "%d", rpmFilterConfig()->dterm_harmonics);
|
||||
BLACKBOX_PRINT_HEADER_LINE("rpm_dterm_min_hz", "%d", rpmFilterConfig()->dterm_min_hz);
|
||||
BLACKBOX_PRINT_HEADER_LINE("rpm_dterm_q", "%d", rpmFilterConfig()->dterm_q);
|
||||
#endif
|
||||
default:
|
||||
return true;
|
||||
|
|
|
|||
|
|
@ -16,7 +16,7 @@
|
|||
*/
|
||||
|
||||
#define FC_VERSION_MAJOR 2 // increment when a major release is made (big new feature, etc)
|
||||
#define FC_VERSION_MINOR 4 // increment when a minor release is made (small new feature, change etc)
|
||||
#define FC_VERSION_MINOR 5 // increment when a minor release is made (small new feature, change etc)
|
||||
#define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed
|
||||
|
||||
#define STR_HELPER(x) #x
|
||||
|
|
|
|||
|
|
@ -63,6 +63,7 @@
|
|||
#include "fc/settings.h"
|
||||
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/servos.h"
|
||||
|
||||
// For VISIBLE*
|
||||
#include "io/osd.h"
|
||||
|
|
@ -765,6 +766,7 @@ void cmsMenuOpen(void)
|
|||
{
|
||||
if (!cmsInMenu) {
|
||||
// New open
|
||||
setServoOutputEnabled(false);
|
||||
pCurrentDisplay = cmsDisplayPortSelectCurrent();
|
||||
if (!pCurrentDisplay)
|
||||
return;
|
||||
|
|
@ -860,6 +862,8 @@ long cmsMenuExit(displayPort_t *pDisplay, const void *ptr)
|
|||
displayRelease(pDisplay);
|
||||
currentCtx.menu = NULL;
|
||||
|
||||
setServoOutputEnabled(true);
|
||||
|
||||
if ((exitType == CMS_EXIT_SAVEREBOOT) || (exitType == CMS_POPUP_SAVEREBOOT)) {
|
||||
displayClearScreen(pDisplay);
|
||||
displayWrite(pDisplay, 5, 3, "REBOOTING...");
|
||||
|
|
|
|||
|
|
@ -42,7 +42,7 @@ static const OSD_Entry menuMiscEntries[]=
|
|||
{
|
||||
OSD_LABEL_ENTRY("-- MISC --"),
|
||||
|
||||
OSD_SETTING_ENTRY("MIN THR", SETTING_MIN_THROTTLE),
|
||||
OSD_SETTING_ENTRY("THR IDLE", SETTING_THROTTLE_IDLE),
|
||||
#if defined(USE_OSD) && defined(USE_ADC)
|
||||
OSD_SETTING_ENTRY("OSD VOLT DECIMALS", SETTING_OSD_MAIN_VOLTAGE_DECIMALS),
|
||||
OSD_SETTING_ENTRY("STATS ENERGY UNIT", SETTING_OSD_STATS_ENERGY_UNIT),
|
||||
|
|
|
|||
|
|
@ -271,6 +271,10 @@ static const OSD_Entry menuOsdElemsEntries[] =
|
|||
OSD_ELEMENT_ENTRY("SENSOR 7 TEMP", OSD_TEMP_SENSOR_7_TEMPERATURE),
|
||||
#endif
|
||||
|
||||
#ifdef USE_ESC_SENSOR
|
||||
OSD_ELEMENT_ENTRY("ESC RPM", OSD_ESC_RPM),
|
||||
#endif
|
||||
|
||||
OSD_BACK_AND_END_ENTRY,
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -38,7 +38,7 @@
|
|||
|
||||
#include "common/axis.h"
|
||||
|
||||
PG_REGISTER_ARRAY(globalFunction_t, MAX_GLOBAL_FUNCTIONS, globalFunctions, PG_GLOBAL_FUNCTIONS, 0);
|
||||
PG_REGISTER_ARRAY_WITH_RESET_FN(globalFunction_t, MAX_GLOBAL_FUNCTIONS, globalFunctions, PG_GLOBAL_FUNCTIONS, 0);
|
||||
|
||||
EXTENDED_FASTRAM uint64_t globalFunctionsFlags = 0;
|
||||
EXTENDED_FASTRAM globalFunctionState_t globalFunctionsStates[MAX_GLOBAL_FUNCTIONS];
|
||||
|
|
|
|||
|
|
@ -42,25 +42,6 @@
|
|||
|
||||
PG_REGISTER_ARRAY(logicCondition_t, MAX_LOGIC_CONDITIONS, logicConditions, PG_LOGIC_CONDITIONS, 0);
|
||||
|
||||
void pgResetFn_logicConditions(logicCondition_t *instance)
|
||||
{
|
||||
for (int i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
|
||||
RESET_CONFIG(logicCondition_t, &instance[i],
|
||||
.enabled = 0,
|
||||
.operation = LOGIC_CONDITION_TRUE,
|
||||
.operandA = {
|
||||
.type = LOGIC_CONDITION_OPERAND_TYPE_VALUE,
|
||||
.value = 0
|
||||
},
|
||||
.operandB = {
|
||||
.type = LOGIC_CONDITION_OPERAND_TYPE_VALUE,
|
||||
.value = 0
|
||||
},
|
||||
.flags = 0
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
logicConditionState_t logicConditionStates[MAX_LOGIC_CONDITIONS];
|
||||
|
||||
static int logicConditionCompute(
|
||||
|
|
|
|||
|
|
@ -42,7 +42,7 @@
|
|||
//#define PG_PROFILE_SELECTION 23
|
||||
#define PG_RX_CONFIG 24
|
||||
#define PG_RC_CONTROLS_CONFIG 25
|
||||
#define PG_MOTOR_3D_CONFIG 26
|
||||
#define PG_REVERSIBLE_MOTORS_CONFIG 26
|
||||
#define PG_LED_STRIP_CONFIG 27
|
||||
//#define PG_COLOR_CONFIG 28
|
||||
//#define PG_AIRPLANE_ALT_HOLD_CONFIG 29
|
||||
|
|
|
|||
|
|
@ -127,6 +127,7 @@ static bool mpu6500DeviceDetect(busDevice_t * dev)
|
|||
switch (tmp) {
|
||||
case MPU6500_WHO_AM_I_CONST:
|
||||
case ICM20608G_WHO_AM_I_CONST:
|
||||
case ICM20601_WHO_AM_I_CONST:
|
||||
case ICM20602_WHO_AM_I_CONST:
|
||||
case ICM20689_WHO_AM_I_CONST:
|
||||
// Compatible chip detected
|
||||
|
|
|
|||
|
|
@ -97,7 +97,7 @@ static bool mag3110Read(magDev_t * mag)
|
|||
|
||||
mag->magADCRaw[X] = (int16_t)(buf[0] << 8 | buf[1]);
|
||||
mag->magADCRaw[Y] = (int16_t)(buf[2] << 8 | buf[3]);
|
||||
mag->magADCRaw[Z] = (int16_t)(buf[4] << 8 | buf[5]);
|
||||
mag->magADCRaw[Z] = -(int16_t)(buf[4] << 8 | buf[5]); // Z is down, not up in this sensor
|
||||
|
||||
return true;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -146,7 +146,7 @@ static bool commandBatchError = false;
|
|||
static const char * const featureNames[] = {
|
||||
"THR_VBAT_COMP", "VBAT", "TX_PROF_SEL", "BAT_PROF_AUTOSWITCH", "MOTOR_STOP",
|
||||
"DYNAMIC_FILTERS", "SOFTSERIAL", "GPS", "RPM_FILTERS",
|
||||
"", "TELEMETRY", "CURRENT_METER", "3D", "",
|
||||
"", "TELEMETRY", "CURRENT_METER", "REVERSIBLE_MOTORS", "",
|
||||
"", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "",
|
||||
"BLACKBOX", "", "TRANSPONDER", "AIRMODE",
|
||||
"SUPEREXPO", "VTX", "", "", "PWM_SERVO_DRIVER", "PWM_OUTPUT_ENABLE",
|
||||
|
|
|
|||
|
|
@ -48,7 +48,7 @@ typedef enum {
|
|||
FEATURE_UNUSED_4 = 1 << 9, // was FEATURE_SONAR
|
||||
FEATURE_TELEMETRY = 1 << 10,
|
||||
FEATURE_CURRENT_METER = 1 << 11,
|
||||
FEATURE_3D = 1 << 12,
|
||||
FEATURE_REVERSIBLE_MOTORS = 1 << 12,
|
||||
FEATURE_UNUSED_5 = 1 << 13, // RX_PARALLEL_PWM
|
||||
FEATURE_UNUSED_6 = 1 << 14, // RX_MSP
|
||||
FEATURE_RSSI_ADC = 1 << 15,
|
||||
|
|
|
|||
|
|
@ -515,7 +515,7 @@ void processRx(timeUs_t currentTimeUs)
|
|||
calculateRxChannelsAndUpdateFailsafe(currentTimeUs);
|
||||
|
||||
// in 3D mode, we need to be able to disarm by switch at any time
|
||||
if (feature(FEATURE_3D)) {
|
||||
if (feature(FEATURE_REVERSIBLE_MOTORS)) {
|
||||
if (!IS_RC_MODE_ACTIVE(BOXARM))
|
||||
disarm(DISARM_SWITCH_3D);
|
||||
}
|
||||
|
|
@ -532,7 +532,7 @@ void processRx(timeUs_t currentTimeUs)
|
|||
const throttleStatus_e throttleStatus = calculateThrottleStatus();
|
||||
|
||||
// When armed and motors aren't spinning, do beeps periodically
|
||||
if (ARMING_FLAG(ARMED) && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING)) {
|
||||
if (ARMING_FLAG(ARMED) && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING_LEGACY)) {
|
||||
static bool armedBeeperOn = false;
|
||||
|
||||
if (throttleStatus == THROTTLE_LOW) {
|
||||
|
|
@ -633,7 +633,7 @@ void processRx(timeUs_t currentTimeUs)
|
|||
#endif
|
||||
|
||||
// Handle passthrough mode
|
||||
if (STATE(FIXED_WING)) {
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
if ((IS_RC_MODE_ACTIVE(BOXMANUAL) && !navigationRequiresAngleMode() && !failsafeRequiresAngleMode()) || // Normal activation of passthrough
|
||||
(!ARMING_FLAG(ARMED) && isCalibrating())){ // Backup - if we are not armed - enforce passthrough while calibrating
|
||||
ENABLE_FLIGHT_MODE(MANUAL_MODE);
|
||||
|
|
@ -649,13 +649,13 @@ void processRx(timeUs_t currentTimeUs)
|
|||
/* In MANUAL mode we reset integrators prevent I-term wind-up (PID output is not used in MANUAL) */
|
||||
pidResetErrorAccumulators();
|
||||
}
|
||||
else if (STATE(FIXED_WING) || rcControlsConfig()->airmodeHandlingType == STICK_CENTER) {
|
||||
else if (STATE(FIXED_WING_LEGACY) || rcControlsConfig()->airmodeHandlingType == STICK_CENTER) {
|
||||
if (throttleStatus == THROTTLE_LOW) {
|
||||
if (STATE(AIRMODE_ACTIVE) && !failsafeIsActive() && ARMING_FLAG(ARMED)) {
|
||||
rollPitchStatus_e rollPitchStatus = calculateRollPitchCenterStatus();
|
||||
|
||||
// ANTI_WINDUP at centred stick with MOTOR_STOP is needed on MRs and not needed on FWs
|
||||
if ((rollPitchStatus == CENTERED) || (feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING))) {
|
||||
if ((rollPitchStatus == CENTERED) || (feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING_LEGACY))) {
|
||||
ENABLE_STATE(ANTI_WINDUP);
|
||||
}
|
||||
else {
|
||||
|
|
@ -753,7 +753,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
|
|||
cycleTime = getTaskDeltaTime(TASK_SELF);
|
||||
dT = (float)cycleTime * 0.000001f;
|
||||
|
||||
if (ARMING_FLAG(ARMED) && (!STATE(FIXED_WING) || !isNavLaunchEnabled() || (isNavLaunchEnabled() && (isFixedWingLaunchDetected() || isFixedWingLaunchFinishedOrAborted())))) {
|
||||
if (ARMING_FLAG(ARMED) && (!STATE(FIXED_WING_LEGACY) || !isNavLaunchEnabled() || (isNavLaunchEnabled() && (isFixedWingLaunchDetected() || isFixedWingLaunchFinishedOrAborted())))) {
|
||||
flightTime += cycleTime;
|
||||
updateAccExtremes();
|
||||
}
|
||||
|
|
@ -782,7 +782,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
|
|||
#endif
|
||||
|
||||
// Apply throttle tilt compensation
|
||||
if (!STATE(FIXED_WING)) {
|
||||
if (!STATE(FIXED_WING_LEGACY)) {
|
||||
int16_t thrTiltCompStrength = 0;
|
||||
|
||||
if (navigationRequiresThrottleTiltCompensation()) {
|
||||
|
|
@ -793,9 +793,9 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
|
|||
}
|
||||
|
||||
if (thrTiltCompStrength) {
|
||||
rcCommand[THROTTLE] = constrain(motorConfig()->minthrottle
|
||||
+ (rcCommand[THROTTLE] - motorConfig()->minthrottle) * calculateThrottleTiltCompensationFactor(thrTiltCompStrength),
|
||||
motorConfig()->minthrottle,
|
||||
rcCommand[THROTTLE] = constrain(getThrottleIdleValue()
|
||||
+ (rcCommand[THROTTLE] - getThrottleIdleValue()) * calculateThrottleTiltCompensationFactor(thrTiltCompStrength),
|
||||
getThrottleIdleValue(),
|
||||
motorConfig()->maxthrottle);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -108,6 +108,7 @@
|
|||
#include "io/ledstrip.h"
|
||||
#include "io/pwmdriver_i2c.h"
|
||||
#include "io/osd.h"
|
||||
#include "io/osd_dji_hd.h"
|
||||
#include "io/rcdevice_cam.h"
|
||||
#include "io/serial.h"
|
||||
#include "io/displayport_msp.h"
|
||||
|
|
@ -265,6 +266,11 @@ void init(void)
|
|||
// to run after the sensors have been detected.
|
||||
mspSerialInit();
|
||||
|
||||
#if defined(USE_DJI_HD_OSD)
|
||||
// DJI OSD uses a special flavour of MSP (subset of Betaflight 4.1.1 MSP) - process as part of serial task
|
||||
djiOsdSerialInit();
|
||||
#endif
|
||||
|
||||
#if defined(USE_LOG)
|
||||
// LOG might use serial output, so we only can init it after serial port is ready
|
||||
// From this point on we can use LOG_*() to produce real-time debugging information
|
||||
|
|
@ -279,7 +285,7 @@ void init(void)
|
|||
|
||||
// Some sanity checking
|
||||
if (motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) {
|
||||
featureClear(FEATURE_3D);
|
||||
featureClear(FEATURE_REVERSIBLE_MOTORS);
|
||||
}
|
||||
|
||||
// Initialize motor and servo outpus
|
||||
|
|
|
|||
|
|
@ -153,26 +153,74 @@ typedef enum {
|
|||
MSP_FLASHFS_BIT_SUPPORTED = 2
|
||||
} mspFlashfsFlags_e;
|
||||
|
||||
#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||
#define ESC_4WAY 0xff
|
||||
typedef enum {
|
||||
MSP_PASSTHROUGH_SERIAL_ID = 0xFD,
|
||||
MSP_PASSTHROUGH_SERIAL_FUNCTION_ID = 0xFE,
|
||||
|
||||
static uint8_t escMode;
|
||||
static uint8_t escPortIndex;
|
||||
MSP_PASSTHROUGH_ESC_4WAY = 0xFF,
|
||||
} mspPassthroughType_e;
|
||||
|
||||
static void mspFc4waySerialCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn)
|
||||
static uint8_t mspPassthroughMode;
|
||||
static uint8_t mspPassthroughArgument;
|
||||
|
||||
static serialPort_t *mspFindPassthroughSerialPort(void)
|
||||
{
|
||||
serialPortUsage_t *portUsage = NULL;
|
||||
|
||||
switch (mspPassthroughMode) {
|
||||
case MSP_PASSTHROUGH_SERIAL_ID:
|
||||
{
|
||||
portUsage = findSerialPortUsageByIdentifier(mspPassthroughArgument);
|
||||
break;
|
||||
}
|
||||
case MSP_PASSTHROUGH_SERIAL_FUNCTION_ID:
|
||||
{
|
||||
const serialPortConfig_t *portConfig = findSerialPortConfig(1 << mspPassthroughArgument);
|
||||
if (portConfig) {
|
||||
portUsage = findSerialPortUsageByIdentifier(portConfig->identifier);
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
return portUsage ? portUsage->serialPort : NULL;
|
||||
}
|
||||
|
||||
static void mspSerialPassthroughFn(serialPort_t *serialPort)
|
||||
{
|
||||
serialPort_t *passthroughPort = mspFindPassthroughSerialPort();
|
||||
if (passthroughPort && serialPort) {
|
||||
serialPassthrough(passthroughPort, serialPort, NULL, NULL);
|
||||
}
|
||||
}
|
||||
|
||||
static void mspFcSetPassthroughCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn)
|
||||
{
|
||||
const unsigned int dataSize = sbufBytesRemaining(src);
|
||||
|
||||
if (dataSize == 0) {
|
||||
// Legacy format
|
||||
escMode = ESC_4WAY;
|
||||
mspPassthroughMode = MSP_PASSTHROUGH_ESC_4WAY;
|
||||
} else {
|
||||
escMode = sbufReadU8(src);
|
||||
escPortIndex = sbufReadU8(src);
|
||||
mspPassthroughMode = sbufReadU8(src);
|
||||
if (!sbufReadU8Safe(&mspPassthroughArgument, src)) {
|
||||
mspPassthroughArgument = 0;
|
||||
}
|
||||
}
|
||||
|
||||
switch (escMode) {
|
||||
case ESC_4WAY:
|
||||
switch (mspPassthroughMode) {
|
||||
case MSP_PASSTHROUGH_SERIAL_ID:
|
||||
case MSP_PASSTHROUGH_SERIAL_FUNCTION_ID:
|
||||
if (mspFindPassthroughSerialPort()) {
|
||||
if (mspPostProcessFn) {
|
||||
*mspPostProcessFn = mspSerialPassthroughFn;
|
||||
}
|
||||
sbufWriteU8(dst, 1);
|
||||
} else {
|
||||
sbufWriteU8(dst, 0);
|
||||
}
|
||||
break;
|
||||
#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||
case MSP_PASSTHROUGH_ESC_4WAY:
|
||||
// get channel number
|
||||
// switch all motor lines HI
|
||||
// reply with the count of ESC found
|
||||
|
|
@ -182,14 +230,12 @@ static void mspFc4waySerialCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr
|
|||
*mspPostProcessFn = esc4wayProcess;
|
||||
}
|
||||
break;
|
||||
|
||||
#endif
|
||||
default:
|
||||
sbufWriteU8(dst, 0);
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
static void mspRebootFn(serialPort_t *serialPort)
|
||||
{
|
||||
UNUSED(serialPort);
|
||||
|
|
@ -702,7 +748,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
|
|||
case MSP_MISC:
|
||||
sbufWriteU16(dst, PWM_RANGE_MIDDLE);
|
||||
|
||||
sbufWriteU16(dst, motorConfig()->minthrottle);
|
||||
sbufWriteU16(dst, 0); // Was min_throttle
|
||||
sbufWriteU16(dst, motorConfig()->maxthrottle);
|
||||
sbufWriteU16(dst, motorConfig()->mincommand);
|
||||
|
||||
|
|
@ -732,7 +778,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
|
|||
case MSP2_INAV_MISC:
|
||||
sbufWriteU16(dst, PWM_RANGE_MIDDLE);
|
||||
|
||||
sbufWriteU16(dst, motorConfig()->minthrottle);
|
||||
sbufWriteU16(dst, 0); //Was min_throttle
|
||||
sbufWriteU16(dst, motorConfig()->maxthrottle);
|
||||
sbufWriteU16(dst, motorConfig()->mincommand);
|
||||
|
||||
|
|
@ -1064,16 +1110,16 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
|
|||
break;
|
||||
|
||||
case MSP_3D:
|
||||
sbufWriteU16(dst, flight3DConfig()->deadband3d_low);
|
||||
sbufWriteU16(dst, flight3DConfig()->deadband3d_high);
|
||||
sbufWriteU16(dst, flight3DConfig()->neutral3d);
|
||||
sbufWriteU16(dst, reversibleMotorsConfig()->deadband_low);
|
||||
sbufWriteU16(dst, reversibleMotorsConfig()->deadband_high);
|
||||
sbufWriteU16(dst, reversibleMotorsConfig()->neutral);
|
||||
break;
|
||||
|
||||
case MSP_RC_DEADBAND:
|
||||
sbufWriteU8(dst, rcControlsConfig()->deadband);
|
||||
sbufWriteU8(dst, rcControlsConfig()->yaw_deadband);
|
||||
sbufWriteU8(dst, rcControlsConfig()->alt_hold_deadband);
|
||||
sbufWriteU16(dst, rcControlsConfig()->deadband3d_throttle);
|
||||
sbufWriteU16(dst, rcControlsConfig()->mid_throttle_deadband);
|
||||
break;
|
||||
|
||||
case MSP_SENSOR_ALIGNMENT:
|
||||
|
|
@ -1712,7 +1758,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
|
|||
if (dataSize >= 22) {
|
||||
sbufReadU16(src); // midrc
|
||||
|
||||
motorConfigMutable()->minthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX);
|
||||
sbufReadU16(src); //Was min_throttle
|
||||
motorConfigMutable()->maxthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX);
|
||||
motorConfigMutable()->mincommand = constrain(sbufReadU16(src), 0, PWM_RANGE_MAX);
|
||||
|
||||
|
|
@ -1753,7 +1799,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
|
|||
if (dataSize == 41) {
|
||||
sbufReadU16(src); // midrc
|
||||
|
||||
motorConfigMutable()->minthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX);
|
||||
sbufReadU16(src); // was min_throttle
|
||||
motorConfigMutable()->maxthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX);
|
||||
motorConfigMutable()->mincommand = constrain(sbufReadU16(src), 0, PWM_RANGE_MAX);
|
||||
|
||||
|
|
@ -1936,9 +1982,9 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
|
|||
|
||||
case MSP_SET_3D:
|
||||
if (dataSize >= 6) {
|
||||
flight3DConfigMutable()->deadband3d_low = sbufReadU16(src);
|
||||
flight3DConfigMutable()->deadband3d_high = sbufReadU16(src);
|
||||
flight3DConfigMutable()->neutral3d = sbufReadU16(src);
|
||||
reversibleMotorsConfigMutable()->deadband_low = sbufReadU16(src);
|
||||
reversibleMotorsConfigMutable()->deadband_high = sbufReadU16(src);
|
||||
reversibleMotorsConfigMutable()->neutral = sbufReadU16(src);
|
||||
} else
|
||||
return MSP_RESULT_ERROR;
|
||||
break;
|
||||
|
|
@ -1948,7 +1994,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
|
|||
rcControlsConfigMutable()->deadband = sbufReadU8(src);
|
||||
rcControlsConfigMutable()->yaw_deadband = sbufReadU8(src);
|
||||
rcControlsConfigMutable()->alt_hold_deadband = sbufReadU8(src);
|
||||
rcControlsConfigMutable()->deadband3d_throttle = sbufReadU16(src);
|
||||
rcControlsConfigMutable()->mid_throttle_deadband = sbufReadU16(src);
|
||||
} else
|
||||
return MSP_RESULT_ERROR;
|
||||
break;
|
||||
|
|
@ -1991,7 +2037,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
|
|||
case MSP_SET_FILTER_CONFIG :
|
||||
if (dataSize >= 5) {
|
||||
gyroConfigMutable()->gyro_soft_lpf_hz = sbufReadU8(src);
|
||||
pidProfileMutable()->dterm_lpf_hz = constrain(sbufReadU16(src), 0, 255);
|
||||
pidProfileMutable()->dterm_lpf_hz = constrain(sbufReadU16(src), 0, 500);
|
||||
pidProfileMutable()->yaw_lpf_hz = constrain(sbufReadU16(src), 0, 255);
|
||||
if (dataSize >= 9) {
|
||||
gyroConfigMutable()->gyro_soft_notch_hz_1 = constrain(sbufReadU16(src), 0, 500);
|
||||
|
|
@ -3177,11 +3223,9 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro
|
|||
ret = mspProcessSensorCommand(cmdMSP, src);
|
||||
} else if (mspFcProcessOutCommand(cmdMSP, dst, mspPostProcessFn)) {
|
||||
ret = MSP_RESULT_ACK;
|
||||
#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||
} else if (cmdMSP == MSP_SET_4WAY_IF) {
|
||||
mspFc4waySerialCommand(dst, src, mspPostProcessFn);
|
||||
} else if (cmdMSP == MSP_SET_PASSTHROUGH) {
|
||||
mspFcSetPassthroughCommand(dst, src, mspPostProcessFn);
|
||||
ret = MSP_RESULT_ACK;
|
||||
#endif
|
||||
} else {
|
||||
if (!mspFCProcessInOutCommand(cmdMSP, dst, src, &ret)) {
|
||||
ret = mspFcProcessInCommand(cmdMSP, src);
|
||||
|
|
|
|||
|
|
@ -187,17 +187,17 @@ void initActiveBoxIds(void)
|
|||
activeBoxIds[activeBoxIdCount++] = BOXCAMSTAB;
|
||||
|
||||
#ifdef USE_GPS
|
||||
if (sensors(SENSOR_BARO) || (STATE(FIXED_WING) && feature(FEATURE_GPS))) {
|
||||
if (sensors(SENSOR_BARO) || (STATE(FIXED_WING_LEGACY) && feature(FEATURE_GPS))) {
|
||||
activeBoxIds[activeBoxIdCount++] = BOXNAVALTHOLD;
|
||||
activeBoxIds[activeBoxIdCount++] = BOXSURFACE;
|
||||
}
|
||||
|
||||
const bool navReadyQuads = !STATE(FIXED_WING) && (getHwCompassStatus() != HW_SENSOR_NONE) && sensors(SENSOR_ACC) && feature(FEATURE_GPS);
|
||||
const bool navReadyPlanes = STATE(FIXED_WING) && sensors(SENSOR_ACC) && feature(FEATURE_GPS);
|
||||
const bool navReadyQuads = !STATE(FIXED_WING_LEGACY) && (getHwCompassStatus() != HW_SENSOR_NONE) && sensors(SENSOR_ACC) && feature(FEATURE_GPS);
|
||||
const bool navReadyPlanes = STATE(FIXED_WING_LEGACY) && sensors(SENSOR_ACC) && feature(FEATURE_GPS);
|
||||
const bool navFlowDeadReckoning = sensors(SENSOR_OPFLOW) && sensors(SENSOR_ACC) && positionEstimationConfig()->allow_dead_reckoning;
|
||||
if (navFlowDeadReckoning || navReadyQuads || navReadyPlanes) {
|
||||
activeBoxIds[activeBoxIdCount++] = BOXNAVPOSHOLD;
|
||||
if (STATE(FIXED_WING)) {
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
activeBoxIds[activeBoxIdCount++] = BOXLOITERDIRCHN;
|
||||
}
|
||||
}
|
||||
|
|
@ -209,7 +209,7 @@ void initActiveBoxIds(void)
|
|||
|
||||
if (feature(FEATURE_GPS)) {
|
||||
activeBoxIds[activeBoxIdCount++] = BOXGCSNAV;
|
||||
if (STATE(FIXED_WING)) {
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
activeBoxIds[activeBoxIdCount++] = BOXNAVCRUISE;
|
||||
}
|
||||
}
|
||||
|
|
@ -223,7 +223,7 @@ void initActiveBoxIds(void)
|
|||
|
||||
#endif
|
||||
|
||||
if (STATE(FIXED_WING)) {
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
activeBoxIds[activeBoxIdCount++] = BOXMANUAL;
|
||||
if (!feature(FEATURE_FW_LAUNCH)) {
|
||||
activeBoxIds[activeBoxIdCount++] = BOXNAVLAUNCH;
|
||||
|
|
|
|||
|
|
@ -61,6 +61,7 @@
|
|||
#include "io/serial.h"
|
||||
#include "io/rcdevice_cam.h"
|
||||
#include "io/vtx.h"
|
||||
#include "io/osd_dji_hd.h"
|
||||
|
||||
#include "msp/msp_serial.h"
|
||||
|
||||
|
|
@ -97,6 +98,11 @@ void taskHandleSerial(timeUs_t currentTimeUs)
|
|||
|
||||
// Allow MSP processing even if in CLI mode
|
||||
mspSerialProcess(ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA, mspFcProcessCommand);
|
||||
|
||||
#if defined(USE_DJI_HD_OSD)
|
||||
// DJI OSD uses a special flavour of MSP (subset of Betaflight 4.1.1 MSP) - process as part of serial task
|
||||
djiOsdSerialProcess();
|
||||
#endif
|
||||
}
|
||||
|
||||
void taskUpdateBattery(timeUs_t currentTimeUs)
|
||||
|
|
|
|||
|
|
@ -76,7 +76,7 @@ PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig,
|
|||
.yaw_deadband = 5,
|
||||
.pos_hold_deadband = 20,
|
||||
.alt_hold_deadband = 50,
|
||||
.deadband3d_throttle = 50,
|
||||
.mid_throttle_deadband = 50,
|
||||
.airmodeHandlingType = STICK_CENTER,
|
||||
.airmodeThrottleThreshold = AIRMODE_THROTTLE_THRESHOLD,
|
||||
);
|
||||
|
|
@ -101,10 +101,10 @@ bool areSticksDeflectedMoreThanPosHoldDeadband(void)
|
|||
|
||||
throttleStatus_e calculateThrottleStatus(void)
|
||||
{
|
||||
const uint16_t deadband3d_throttle = rcControlsConfig()->deadband3d_throttle;
|
||||
if (feature(FEATURE_3D) && (rxGetChannelValue(THROTTLE) > (PWM_RANGE_MIDDLE - deadband3d_throttle) && rxGetChannelValue(THROTTLE) < (PWM_RANGE_MIDDLE + deadband3d_throttle)))
|
||||
const uint16_t mid_throttle_deadband = rcControlsConfig()->mid_throttle_deadband;
|
||||
if (feature(FEATURE_REVERSIBLE_MOTORS) && (rxGetChannelValue(THROTTLE) > (PWM_RANGE_MIDDLE - mid_throttle_deadband) && rxGetChannelValue(THROTTLE) < (PWM_RANGE_MIDDLE + mid_throttle_deadband)))
|
||||
return THROTTLE_LOW;
|
||||
else if (!feature(FEATURE_3D) && (rxGetChannelValue(THROTTLE) < rxConfig()->mincheck))
|
||||
else if (!feature(FEATURE_REVERSIBLE_MOTORS) && (rxGetChannelValue(THROTTLE) < rxConfig()->mincheck))
|
||||
return THROTTLE_LOW;
|
||||
|
||||
return THROTTLE_HIGH;
|
||||
|
|
@ -183,7 +183,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
|
|||
bool armingSwitchIsActive = IS_RC_MODE_ACTIVE(BOXARM);
|
||||
emergencyArmingUpdate(armingSwitchIsActive);
|
||||
|
||||
if (STATE(FIXED_WING) && feature(FEATURE_MOTOR_STOP) && armingConfig()->fixed_wing_auto_arm) {
|
||||
if (STATE(FIXED_WING_LEGACY) && feature(FEATURE_MOTOR_STOP) && armingConfig()->fixed_wing_auto_arm) {
|
||||
// Auto arm on throttle when using fixedwing and motorstop
|
||||
if (throttleStatus != THROTTLE_LOW) {
|
||||
tryArm();
|
||||
|
|
|
|||
|
|
@ -80,7 +80,7 @@ typedef struct rcControlsConfig_s {
|
|||
uint8_t yaw_deadband; // introduce a deadband around the stick center for yaw axis. Must be greater than zero.
|
||||
uint8_t pos_hold_deadband; // Adds ability to adjust the Hold-position when moving the sticks (assisted mode)
|
||||
uint8_t alt_hold_deadband; // Defines the neutral zone of throttle stick during altitude hold
|
||||
uint16_t deadband3d_throttle; // default throttle deadband from MIDRC
|
||||
uint16_t mid_throttle_deadband; // default throttle deadband from MIDRC
|
||||
uint8_t airmodeHandlingType; // Defaults to ANTI_WINDUP triggered at sticks centered
|
||||
uint16_t airmodeThrottleThreshold; // Throttle threshold for airmode initial activation
|
||||
} rcControlsConfig_t;
|
||||
|
|
|
|||
|
|
@ -36,12 +36,13 @@
|
|||
#define YAW_LOOKUP_LENGTH 7
|
||||
#define THROTTLE_LOOKUP_LENGTH 11
|
||||
|
||||
static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE
|
||||
static EXTENDED_FASTRAM int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE
|
||||
int16_t lookupThrottleRCMid; // THROTTLE curve mid point
|
||||
|
||||
void generateThrottleCurve(const controlRateConfig_t *controlRateConfig)
|
||||
{
|
||||
lookupThrottleRCMid = motorConfig()->minthrottle + (int32_t)(motorConfig()->maxthrottle - motorConfig()->minthrottle) * controlRateConfig->throttle.rcMid8 / 100; // [MINTHROTTLE;MAXTHROTTLE]
|
||||
const int minThrottle = getThrottleIdleValue();
|
||||
lookupThrottleRCMid = minThrottle + (int32_t)(motorConfig()->maxthrottle - minThrottle) * controlRateConfig->throttle.rcMid8 / 100; // [MINTHROTTLE;MAXTHROTTLE]
|
||||
|
||||
for (int i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) {
|
||||
const int16_t tmp = 10 * i - controlRateConfig->throttle.rcMid8;
|
||||
|
|
@ -51,7 +52,7 @@ void generateThrottleCurve(const controlRateConfig_t *controlRateConfig)
|
|||
if (tmp < 0)
|
||||
y = controlRateConfig->throttle.rcMid8;
|
||||
lookupThrottleRC[i] = 10 * controlRateConfig->throttle.rcMid8 + tmp * (100 - controlRateConfig->throttle.rcExpo8 + (int32_t) controlRateConfig->throttle.rcExpo8 * (tmp * tmp) / (y * y)) / 10;
|
||||
lookupThrottleRC[i] = motorConfig()->minthrottle + (int32_t) (motorConfig()->maxthrottle - motorConfig()->minthrottle) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE]
|
||||
lookupThrottleRC[i] = minThrottle + (int32_t) (motorConfig()->maxthrottle - minThrottle) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE]
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -104,7 +104,7 @@ static void processAirmodeMultirotor(void) {
|
|||
|
||||
void processAirmode(void) {
|
||||
|
||||
if (STATE(FIXED_WING)) {
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
processAirmodeAirplane();
|
||||
} else {
|
||||
processAirmodeMultirotor();
|
||||
|
|
|
|||
|
|
@ -102,18 +102,21 @@ uint32_t sensorsMask(void)
|
|||
|
||||
flightModeForTelemetry_e getFlightModeForTelemetry(void)
|
||||
{
|
||||
if (FLIGHT_MODE(MANUAL_MODE))
|
||||
return FLM_MANUAL;
|
||||
|
||||
if (FLIGHT_MODE(FAILSAFE_MODE))
|
||||
return FLM_FAILSAFE;
|
||||
|
||||
if (FLIGHT_MODE(MANUAL_MODE))
|
||||
return FLM_MANUAL;
|
||||
|
||||
if (FLIGHT_MODE(NAV_RTH_MODE))
|
||||
return FLM_RTH;
|
||||
|
||||
if (FLIGHT_MODE(NAV_POSHOLD_MODE))
|
||||
return FLM_POSITION_HOLD;
|
||||
|
||||
if (FLIGHT_MODE(NAV_CRUISE_MODE))
|
||||
return FLM_CRUISE;
|
||||
|
||||
if (FLIGHT_MODE(NAV_WP_MODE))
|
||||
return FLM_MISSION;
|
||||
|
||||
|
|
@ -129,5 +132,5 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void)
|
|||
if (FLIGHT_MODE(NAV_LAUNCH_MODE))
|
||||
return FLM_LAUNCH;
|
||||
|
||||
return FLM_ACRO;
|
||||
return STATE(AIRMODE_ACTIVE) ? FLM_ACRO_AIR : FLM_ACRO;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -110,7 +110,7 @@ typedef enum {
|
|||
GPS_FIX = (1 << 1),
|
||||
CALIBRATE_MAG = (1 << 2),
|
||||
SMALL_ANGLE = (1 << 3),
|
||||
FIXED_WING = (1 << 4), // set when in flying_wing or airplane mode. currently used by althold selection code
|
||||
FIXED_WING_LEGACY = (1 << 4), // No new code should use this state. Use AIRPLANE, MULTIROTOR, ROVER, BOAT, ALTITUDE_CONTROL and MOVE_FORWARD_ONLY states
|
||||
ANTI_WINDUP = (1 << 5),
|
||||
FLAPERON_AVAILABLE = (1 << 6),
|
||||
NAV_MOTOR_STOP_OR_IDLE = (1 << 7), // navigation requests MOTOR_STOP or motor idle regardless of throttle stick, will only activate if MOTOR_STOP feature is available
|
||||
|
|
@ -123,7 +123,13 @@ typedef enum {
|
|||
NAV_EXTRA_ARMING_SAFETY_BYPASSED = (1 << 14), // nav_extra_arming_safey was bypassed. Keep it until power cycle.
|
||||
AIRMODE_ACTIVE = (1 << 15),
|
||||
ESC_SENSOR_ENABLED = (1 << 16),
|
||||
FW_HEADING_USE_YAW = (1 << 17),
|
||||
AIRPLANE = (1 << 17),
|
||||
MULTIROTOR = (1 << 18),
|
||||
ROVER = (1 << 19),
|
||||
BOAT = (1 << 20),
|
||||
ALTITUDE_CONTROL = (1 << 21),
|
||||
MOVE_FORWARD_ONLY = (1 << 22),
|
||||
FW_HEADING_USE_YAW = (1 << 23),
|
||||
} stateFlags_t;
|
||||
|
||||
#define DISABLE_STATE(mask) (stateFlags &= ~(mask))
|
||||
|
|
@ -135,12 +141,14 @@ extern uint32_t stateFlags;
|
|||
typedef enum {
|
||||
FLM_MANUAL,
|
||||
FLM_ACRO,
|
||||
FLM_ACRO_AIR,
|
||||
FLM_ANGLE,
|
||||
FLM_HORIZON,
|
||||
FLM_ALTITUDE_HOLD,
|
||||
FLM_POSITION_HOLD,
|
||||
FLM_RTH,
|
||||
FLM_MISSION,
|
||||
FLM_CRUISE,
|
||||
FLM_LAUNCH,
|
||||
FLM_FAILSAFE,
|
||||
FLM_COUNT
|
||||
|
|
|
|||
|
|
@ -25,7 +25,7 @@ tables:
|
|||
values: ["NONE", "PWM", "PPM", "SERIAL", "MSP", "SPI", "UIB"]
|
||||
enum: rxReceiverType_e
|
||||
- name: serial_rx
|
||||
values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "SUMH", "XB-B", "XB-B-RJ01", "IBUS", "JETIEXBUS", "CRSF", "FPORT"]
|
||||
values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "SUMH", "XB-B", "XB-B-RJ01", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST"]
|
||||
- name: rx_spi_protocol
|
||||
values: ["V202_250K", "V202_1M", "SYMA_X", "SYMA_X5C", "CX10", "CX10A", "H8_3D", "INAV", "ELERES"]
|
||||
enum: rx_spi_protocol_e
|
||||
|
|
@ -478,10 +478,6 @@ groups:
|
|||
type: motorConfig_t
|
||||
headers: ["flight/mixer.h"]
|
||||
members:
|
||||
- name: min_throttle
|
||||
field: minthrottle
|
||||
min: PWM_RANGE_MIN
|
||||
max: PWM_RANGE_MAX
|
||||
- name: max_throttle
|
||||
field: maxthrottle
|
||||
min: PWM_RANGE_MIN
|
||||
|
|
@ -509,6 +505,10 @@ groups:
|
|||
field: throttleScale
|
||||
min: 0
|
||||
max: 1
|
||||
- name: throttle_idle
|
||||
field: throttleIdle
|
||||
min: 4
|
||||
max: 30
|
||||
- name: motor_poles
|
||||
field: motorPoleCount
|
||||
min: 4
|
||||
|
|
@ -704,19 +704,19 @@ groups:
|
|||
min: 0
|
||||
max: 450
|
||||
|
||||
- name: PG_MOTOR_3D_CONFIG
|
||||
type: flight3DConfig_t
|
||||
- name: PG_REVERSIBLE_MOTORS_CONFIG
|
||||
type: reversibleMotorsConfig_t
|
||||
members:
|
||||
- name: 3d_deadband_low
|
||||
field: deadband3d_low
|
||||
field: deadband_low
|
||||
min: PWM_RANGE_MIN
|
||||
max: PWM_RANGE_MAX
|
||||
- name: 3d_deadband_high
|
||||
field: deadband3d_high
|
||||
field: deadband_high
|
||||
min: PWM_RANGE_MIN
|
||||
max: PWM_RANGE_MAX
|
||||
- name: 3d_neutral
|
||||
field: neutral3d
|
||||
field: neutral
|
||||
min: PWM_RANGE_MIN
|
||||
max: PWM_RANGE_MAX
|
||||
|
||||
|
|
@ -878,9 +878,6 @@ groups:
|
|||
- name: rpm_gyro_filter_enabled
|
||||
field: gyro_filter_enabled
|
||||
type: bool
|
||||
- name: rpm_dterm_filter_enabled
|
||||
field: dterm_filter_enabled
|
||||
type: bool
|
||||
- name: rpm_gyro_harmonics
|
||||
field: gyro_harmonics
|
||||
type: uint8_t
|
||||
|
|
@ -896,21 +893,6 @@ groups:
|
|||
type: uint16_t
|
||||
min: 1
|
||||
max: 3000
|
||||
- name: dterm_gyro_harmonics
|
||||
field: dterm_harmonics
|
||||
type: uint8_t
|
||||
min: 1
|
||||
max: 3
|
||||
- name: rpm_dterm_min_hz
|
||||
field: dterm_min_hz
|
||||
type: uint8_t
|
||||
min: 50
|
||||
max: 200
|
||||
- name: rpm_dterm_q
|
||||
field: dterm_q
|
||||
type: uint16_t
|
||||
min: 1
|
||||
max: 3000
|
||||
- name: PG_GPS_CONFIG
|
||||
type: gpsConfig_t
|
||||
condition: USE_GPS
|
||||
|
|
@ -958,7 +940,7 @@ groups:
|
|||
min: 10
|
||||
max: 250
|
||||
- name: 3d_deadband_throttle
|
||||
field: deadband3d_throttle
|
||||
field: mid_throttle_deadband
|
||||
min: 0
|
||||
max: 200
|
||||
- name: mc_airmode_type
|
||||
|
|
@ -1080,7 +1062,16 @@ groups:
|
|||
max: 900
|
||||
- name: dterm_lpf_hz
|
||||
min: 0
|
||||
max: 200
|
||||
max: 500
|
||||
- name: dterm_lpf_type
|
||||
field: dterm_lpf_type
|
||||
table: filter_type
|
||||
- name: dterm_lpf2_hz
|
||||
min: 0
|
||||
max: 500
|
||||
- name: dterm_lpf2_type
|
||||
field: dterm_lpf2_type
|
||||
table: filter_type
|
||||
- name: use_dterm_fir_filter
|
||||
field: use_dterm_fir_filter
|
||||
type: bool
|
||||
|
|
|
|||
|
|
@ -216,7 +216,7 @@ bool failsafeRequiresMotorStop(void)
|
|||
{
|
||||
return failsafeState.active &&
|
||||
failsafeState.activeProcedure == FAILSAFE_PROCEDURE_AUTO_LANDING &&
|
||||
failsafeConfig()->failsafe_throttle < motorConfig()->minthrottle;
|
||||
failsafeConfig()->failsafe_throttle < getThrottleIdleValue();
|
||||
}
|
||||
|
||||
void failsafeStartMonitoring(void)
|
||||
|
|
@ -258,7 +258,7 @@ void failsafeApplyControlInput(void)
|
|||
{
|
||||
// Prepare FAILSAFE_CHANNEL_AUTO values for rcCommand
|
||||
int16_t autoRcCommand[4];
|
||||
if (STATE(FIXED_WING)) {
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
autoRcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]);
|
||||
autoRcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]);
|
||||
autoRcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]);
|
||||
|
|
@ -287,7 +287,7 @@ void failsafeApplyControlInput(void)
|
|||
break;
|
||||
|
||||
case THROTTLE:
|
||||
rcCommand[idx] = feature(FEATURE_3D) ? PWM_RANGE_MIDDLE : motorConfig()->minthrottle;
|
||||
rcCommand[idx] = feature(FEATURE_REVERSIBLE_MOTORS) ? PWM_RANGE_MIDDLE : getThrottleIdleValue();
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
|
|
|||
|
|
@ -57,7 +57,7 @@ void hilUpdateControlState(void)
|
|||
{
|
||||
// FIXME: There must be a cleaner way to to this
|
||||
// If HIL active, store PID outout into hilState and disable motor control
|
||||
if (FLIGHT_MODE(MANUAL_MODE) || !STATE(FIXED_WING)) {
|
||||
if (FLIGHT_MODE(MANUAL_MODE) || !STATE(FIXED_WING_LEGACY)) {
|
||||
hilToSIM.pidCommand[ROLL] = rcCommand[ROLL];
|
||||
hilToSIM.pidCommand[PITCH] = rcCommand[PITCH];
|
||||
hilToSIM.pidCommand[YAW] = rcCommand[YAW];
|
||||
|
|
|
|||
|
|
@ -481,7 +481,7 @@ static float imuCalculateAccelerometerWeight(const float dT)
|
|||
const float nearness = ABS(100 - (accMagnitudeSq * 100));
|
||||
const float accWeight_Nearness = (nearness > MAX_ACC_SQ_NEARNESS) ? 0.0f : 1.0f;
|
||||
|
||||
// Experiment: if rotation rate on a FIXED_WING is higher than a threshold - centrifugal force messes up too much and we
|
||||
// Experiment: if rotation rate on a FIXED_WING_LEGACY is higher than a threshold - centrifugal force messes up too much and we
|
||||
// should not use measured accel for AHRS comp
|
||||
// Centrifugal acceleration AccelC = Omega^2 * R = Speed^2 / R
|
||||
// Omega = Speed / R
|
||||
|
|
@ -499,7 +499,7 @@ static float imuCalculateAccelerometerWeight(const float dT)
|
|||
// Default - don't apply rate/ignore scaling
|
||||
float accWeight_RateIgnore = 1.0f;
|
||||
|
||||
if (ARMING_FLAG(ARMED) && STATE(FIXED_WING) && imuConfig()->acc_ignore_rate) {
|
||||
if (ARMING_FLAG(ARMED) && STATE(FIXED_WING_LEGACY) && imuConfig()->acc_ignore_rate) {
|
||||
const float rotRateMagnitude = sqrtf(vectorNormSquared(&imuMeasuredRotationBF));
|
||||
const float rotRateMagnitudeFiltered = pt1FilterApply4(&rotRateFilter, rotRateMagnitude, IMU_CENTRIFUGAL_LPF, dT);
|
||||
|
||||
|
|
@ -532,7 +532,7 @@ static void imuCalculateEstimatedAttitude(float dT)
|
|||
bool useCOG = false;
|
||||
|
||||
#if defined(USE_GPS)
|
||||
if (STATE(FIXED_WING)) {
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
bool canUseCOG = isGPSHeadingValid();
|
||||
|
||||
// Prefer compass (if available)
|
||||
|
|
@ -670,7 +670,7 @@ bool isImuReady(void)
|
|||
|
||||
bool isImuHeadingValid(void)
|
||||
{
|
||||
return (sensors(SENSOR_MAG) && STATE(COMPASS_CALIBRATED)) || (STATE(FIXED_WING) && gpsHeadingInitialized);
|
||||
return (sensors(SENSOR_MAG) && STATE(COMPASS_CALIBRATED)) || (STATE(FIXED_WING_LEGACY) && gpsHeadingInitialized);
|
||||
}
|
||||
|
||||
float calculateCosTiltAngle(void)
|
||||
|
|
|
|||
|
|
@ -61,13 +61,14 @@ static float mixerScale = 1.0f;
|
|||
static EXTENDED_FASTRAM motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
|
||||
static EXTENDED_FASTRAM uint8_t motorCount = 0;
|
||||
EXTENDED_FASTRAM int mixerThrottleCommand;
|
||||
static EXTENDED_FASTRAM int throttleIdleValue = 0;
|
||||
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(flight3DConfig_t, flight3DConfig, PG_MOTOR_3D_CONFIG, 0);
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig, PG_REVERSIBLE_MOTORS_CONFIG, 0);
|
||||
|
||||
PG_RESET_TEMPLATE(flight3DConfig_t, flight3DConfig,
|
||||
.deadband3d_low = 1406,
|
||||
.deadband3d_high = 1514,
|
||||
.neutral3d = 1460
|
||||
PG_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig,
|
||||
.deadband_low = 1406,
|
||||
.deadband_high = 1514,
|
||||
.neutral = 1460
|
||||
);
|
||||
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 2);
|
||||
|
|
@ -83,26 +84,23 @@ PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
|
|||
#ifdef BRUSHED_MOTORS
|
||||
#define DEFAULT_PWM_PROTOCOL PWM_TYPE_BRUSHED
|
||||
#define DEFAULT_PWM_RATE 16000
|
||||
#define DEFAULT_MIN_THROTTLE 1000
|
||||
#else
|
||||
#define DEFAULT_PWM_PROTOCOL PWM_TYPE_STANDARD
|
||||
#define DEFAULT_PWM_RATE 400
|
||||
#define DEFAULT_MIN_THROTTLE 1150
|
||||
#endif
|
||||
|
||||
#define DEFAULT_MAX_THROTTLE 1850
|
||||
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 4);
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 5);
|
||||
|
||||
PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
|
||||
.minthrottle = DEFAULT_MIN_THROTTLE,
|
||||
.motorPwmProtocol = DEFAULT_PWM_PROTOCOL,
|
||||
.motorPwmRate = DEFAULT_PWM_RATE,
|
||||
.maxthrottle = DEFAULT_MAX_THROTTLE,
|
||||
.mincommand = 1000,
|
||||
.motorAccelTimeMs = 0,
|
||||
.motorDecelTimeMs = 0,
|
||||
.digitalIdleOffsetValue = 450, // Same scale as in Betaflight
|
||||
.throttleIdle = 15.0f,
|
||||
.throttleScale = 1.0f,
|
||||
.motorPoleCount = 14 // Most brushless motors that we use are 14 poles
|
||||
);
|
||||
|
|
@ -112,6 +110,15 @@ PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTO
|
|||
typedef void (*motorRateLimitingApplyFnPtr)(const float dT);
|
||||
static EXTENDED_FASTRAM motorRateLimitingApplyFnPtr motorRateLimitingApplyFn;
|
||||
|
||||
int getThrottleIdleValue(void)
|
||||
{
|
||||
if (!throttleIdleValue) {
|
||||
throttleIdleValue = motorConfig()->mincommand + (((motorConfig()->maxthrottle - motorConfig()->mincommand) / 100.0f) * motorConfig()->throttleIdle);
|
||||
}
|
||||
|
||||
return throttleIdleValue;
|
||||
}
|
||||
|
||||
static void computeMotorCount(void)
|
||||
{
|
||||
motorCount = 0;
|
||||
|
|
@ -140,16 +147,35 @@ bool mixerIsOutputSaturated(void)
|
|||
|
||||
void mixerUpdateStateFlags(void)
|
||||
{
|
||||
// set flag that we're on something with wings or a land/water vehicle
|
||||
if (
|
||||
mixerConfig()->platformType == PLATFORM_AIRPLANE ||
|
||||
mixerConfig()->platformType == PLATFORM_ROVER ||
|
||||
mixerConfig()->platformType == PLATFORM_BOAT ||
|
||||
mixerConfig()->platformType == PLATFORM_OTHER
|
||||
) {
|
||||
ENABLE_STATE(FIXED_WING); //This is not entirely true, but much closer to reality than not using FIXED_WING mode
|
||||
} else {
|
||||
DISABLE_STATE(FIXED_WING);
|
||||
DISABLE_STATE(FIXED_WING_LEGACY);
|
||||
DISABLE_STATE(MULTIROTOR);
|
||||
DISABLE_STATE(ROVER);
|
||||
DISABLE_STATE(BOAT);
|
||||
DISABLE_STATE(AIRPLANE);
|
||||
DISABLE_STATE(MOVE_FORWARD_ONLY);
|
||||
|
||||
if (mixerConfig()->platformType == PLATFORM_AIRPLANE) {
|
||||
ENABLE_STATE(FIXED_WING_LEGACY);
|
||||
ENABLE_STATE(AIRPLANE);
|
||||
ENABLE_STATE(ALTITUDE_CONTROL);
|
||||
ENABLE_STATE(MOVE_FORWARD_ONLY);
|
||||
} if (mixerConfig()->platformType == PLATFORM_ROVER) {
|
||||
ENABLE_STATE(ROVER);
|
||||
ENABLE_STATE(FIXED_WING_LEGACY);
|
||||
ENABLE_STATE(MOVE_FORWARD_ONLY);
|
||||
} if (mixerConfig()->platformType == PLATFORM_BOAT) {
|
||||
ENABLE_STATE(BOAT);
|
||||
ENABLE_STATE(FIXED_WING_LEGACY);
|
||||
ENABLE_STATE(MOVE_FORWARD_ONLY);
|
||||
} else if (mixerConfig()->platformType == PLATFORM_MULTIROTOR) {
|
||||
ENABLE_STATE(MULTIROTOR);
|
||||
ENABLE_STATE(ALTITUDE_CONTROL);
|
||||
} else if (mixerConfig()->platformType == PLATFORM_TRICOPTER) {
|
||||
ENABLE_STATE(MULTIROTOR);
|
||||
ENABLE_STATE(ALTITUDE_CONTROL);
|
||||
} else if (mixerConfig()->platformType == PLATFORM_HELICOPTER) {
|
||||
ENABLE_STATE(MULTIROTOR);
|
||||
ENABLE_STATE(ALTITUDE_CONTROL);
|
||||
}
|
||||
|
||||
if (mixerConfig()->hasFlaps) {
|
||||
|
|
@ -168,7 +194,7 @@ void applyMotorRateLimiting(const float dT)
|
|||
{
|
||||
static float motorPrevious[MAX_SUPPORTED_MOTORS] = { 0 };
|
||||
|
||||
if (feature(FEATURE_3D)) {
|
||||
if (feature(FEATURE_REVERSIBLE_MOTORS)) {
|
||||
// FIXME: Don't apply rate limiting in 3D mode
|
||||
for (int i = 0; i < motorCount; i++) {
|
||||
motorPrevious[i] = motor[i];
|
||||
|
|
@ -176,7 +202,7 @@ void applyMotorRateLimiting(const float dT)
|
|||
}
|
||||
else {
|
||||
// Calculate max motor step
|
||||
const uint16_t motorRange = motorConfig()->maxthrottle - motorConfig()->minthrottle;
|
||||
const uint16_t motorRange = motorConfig()->maxthrottle - throttleIdleValue;
|
||||
const float motorMaxInc = (motorConfig()->motorAccelTimeMs == 0) ? 2000 : motorRange * dT / (motorConfig()->motorAccelTimeMs * 1e-3f);
|
||||
const float motorMaxDec = (motorConfig()->motorDecelTimeMs == 0) ? 2000 : motorRange * dT / (motorConfig()->motorDecelTimeMs * 1e-3f);
|
||||
|
||||
|
|
@ -185,12 +211,12 @@ void applyMotorRateLimiting(const float dT)
|
|||
motorPrevious[i] = constrainf(motor[i], motorPrevious[i] - motorMaxDec, motorPrevious[i] + motorMaxInc);
|
||||
|
||||
// Handle throttle below min_throttle (motor start/stop)
|
||||
if (motorPrevious[i] < motorConfig()->minthrottle) {
|
||||
if (motor[i] < motorConfig()->minthrottle) {
|
||||
if (motorPrevious[i] < throttleIdleValue) {
|
||||
if (motor[i] < throttleIdleValue) {
|
||||
motorPrevious[i] = motor[i];
|
||||
}
|
||||
else {
|
||||
motorPrevious[i] = motorConfig()->minthrottle;
|
||||
motorPrevious[i] = throttleIdleValue;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -207,7 +233,7 @@ void mixerInit(void)
|
|||
computeMotorCount();
|
||||
loadPrimaryMotorMixer();
|
||||
// in 3D mode, mixer gain has to be halved
|
||||
if (feature(FEATURE_3D)) {
|
||||
if (feature(FEATURE_REVERSIBLE_MOTORS)) {
|
||||
mixerScale = 0.5f;
|
||||
}
|
||||
|
||||
|
|
@ -224,7 +250,7 @@ void mixerResetDisarmedMotors(void)
|
|||
{
|
||||
// set disarmed motor values
|
||||
for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
|
||||
motor_disarmed[i] = feature(FEATURE_3D) ? flight3DConfig()->neutral3d : motorConfig()->mincommand;
|
||||
motor_disarmed[i] = feature(FEATURE_REVERSIBLE_MOTORS) ? reversibleMotorsConfig()->neutral : motorConfig()->mincommand;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -236,15 +262,14 @@ void FAST_CODE NOINLINE writeMotors(void)
|
|||
#ifdef USE_DSHOT
|
||||
// If we use DSHOT we need to convert motorValue to DSHOT ranges
|
||||
if (isMotorProtocolDigital()) {
|
||||
const float dshotMinThrottleOffset = (DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 10000.0f * motorConfig()->digitalIdleOffsetValue;
|
||||
|
||||
if (feature(FEATURE_3D)) {
|
||||
if (motor[i] >= motorConfig()->minthrottle && motor[i] <= flight3DConfig()->deadband3d_low) {
|
||||
motorValue = scaleRangef(motor[i], motorConfig()->minthrottle, flight3DConfig()->deadband3d_low, DSHOT_3D_DEADBAND_LOW, dshotMinThrottleOffset + DSHOT_MIN_THROTTLE);
|
||||
if (feature(FEATURE_REVERSIBLE_MOTORS)) {
|
||||
if (motor[i] >= throttleIdleValue && motor[i] <= reversibleMotorsConfig()->deadband_low) {
|
||||
motorValue = scaleRangef(motor[i], motorConfig()->mincommand, reversibleMotorsConfig()->deadband_low, DSHOT_3D_DEADBAND_LOW, DSHOT_MIN_THROTTLE);
|
||||
motorValue = constrain(motorValue, DSHOT_MIN_THROTTLE, DSHOT_3D_DEADBAND_LOW);
|
||||
}
|
||||
else if (motor[i] >= flight3DConfig()->deadband3d_high && motor[i] <= motorConfig()->maxthrottle) {
|
||||
motorValue = scaleRangef(motor[i], flight3DConfig()->deadband3d_high, motorConfig()->maxthrottle, dshotMinThrottleOffset + DSHOT_3D_DEADBAND_HIGH, DSHOT_MAX_THROTTLE);
|
||||
else if (motor[i] >= reversibleMotorsConfig()->deadband_high && motor[i] <= motorConfig()->maxthrottle) {
|
||||
motorValue = scaleRangef(motor[i], reversibleMotorsConfig()->deadband_high, motorConfig()->maxthrottle, DSHOT_3D_DEADBAND_HIGH, DSHOT_MAX_THROTTLE);
|
||||
motorValue = constrain(motorValue, DSHOT_3D_DEADBAND_HIGH, DSHOT_MAX_THROTTLE);
|
||||
}
|
||||
else {
|
||||
|
|
@ -252,12 +277,12 @@ void FAST_CODE NOINLINE writeMotors(void)
|
|||
}
|
||||
}
|
||||
else {
|
||||
if (motor[i] < motorConfig()->minthrottle) { // motor disarmed
|
||||
if (motor[i] < throttleIdleValue) { // motor disarmed
|
||||
motorValue = DSHOT_DISARM_COMMAND;
|
||||
}
|
||||
else {
|
||||
motorValue = scaleRangef(motor[i], motorConfig()->minthrottle, motorConfig()->maxthrottle, (dshotMinThrottleOffset + DSHOT_MIN_THROTTLE), DSHOT_MAX_THROTTLE);
|
||||
motorValue = constrain(motorValue, (dshotMinThrottleOffset + DSHOT_MIN_THROTTLE), DSHOT_MAX_THROTTLE);
|
||||
motorValue = scaleRangef(motor[i], motorConfig()->mincommand, motorConfig()->maxthrottle, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE);
|
||||
motorValue = constrain(motorValue, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -284,7 +309,7 @@ void writeAllMotors(int16_t mc)
|
|||
|
||||
void stopMotors(void)
|
||||
{
|
||||
writeAllMotors(feature(FEATURE_3D) ? flight3DConfig()->neutral3d : motorConfig()->mincommand);
|
||||
writeAllMotors(feature(FEATURE_REVERSIBLE_MOTORS) ? reversibleMotorsConfig()->neutral : motorConfig()->mincommand);
|
||||
|
||||
delay(50); // give the timers and ESCs a chance to react.
|
||||
}
|
||||
|
|
@ -298,7 +323,7 @@ void FAST_CODE NOINLINE mixTable(const float dT)
|
|||
{
|
||||
int16_t input[3]; // RPY, range [-500:+500]
|
||||
// Allow direct stick input to motors in passthrough mode on airplanes
|
||||
if (STATE(FIXED_WING) && FLIGHT_MODE(MANUAL_MODE)) {
|
||||
if (STATE(FIXED_WING_LEGACY) && FLIGHT_MODE(MANUAL_MODE)) {
|
||||
// Direct passthru from RX
|
||||
input[ROLL] = rcCommand[ROLL];
|
||||
input[PITCH] = rcCommand[PITCH];
|
||||
|
|
@ -334,32 +359,32 @@ void FAST_CODE NOINLINE mixTable(const float dT)
|
|||
// Find min and max throttle based on condition.
|
||||
#ifdef USE_GLOBAL_FUNCTIONS
|
||||
if (GLOBAL_FUNCTION_FLAG(GLOBAL_FUNCTION_FLAG_OVERRIDE_THROTTLE)) {
|
||||
throttleMin = motorConfig()->minthrottle;
|
||||
throttleMin = throttleIdleValue;
|
||||
throttleMax = motorConfig()->maxthrottle;
|
||||
mixerThrottleCommand = constrain(globalFunctionValues[GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE], throttleMin, throttleMax);
|
||||
} else
|
||||
#endif
|
||||
if (feature(FEATURE_3D)) {
|
||||
if (feature(FEATURE_REVERSIBLE_MOTORS)) {
|
||||
if (!ARMING_FLAG(ARMED)) throttlePrevious = PWM_RANGE_MIDDLE; // When disarmed set to mid_rc. It always results in positive direction after arming.
|
||||
|
||||
if ((rcCommand[THROTTLE] <= (PWM_RANGE_MIDDLE - rcControlsConfig()->deadband3d_throttle))) { // Out of band handling
|
||||
throttleMax = flight3DConfig()->deadband3d_low;
|
||||
throttleMin = motorConfig()->minthrottle;
|
||||
if ((rcCommand[THROTTLE] <= (PWM_RANGE_MIDDLE - rcControlsConfig()->mid_throttle_deadband))) { // Out of band handling
|
||||
throttleMax = reversibleMotorsConfig()->deadband_low;
|
||||
throttleMin = throttleIdleValue;
|
||||
throttlePrevious = mixerThrottleCommand = rcCommand[THROTTLE];
|
||||
} else if (rcCommand[THROTTLE] >= (PWM_RANGE_MIDDLE + rcControlsConfig()->deadband3d_throttle)) { // Positive handling
|
||||
} else if (rcCommand[THROTTLE] >= (PWM_RANGE_MIDDLE + rcControlsConfig()->mid_throttle_deadband)) { // Positive handling
|
||||
throttleMax = motorConfig()->maxthrottle;
|
||||
throttleMin = flight3DConfig()->deadband3d_high;
|
||||
throttleMin = reversibleMotorsConfig()->deadband_high;
|
||||
throttlePrevious = mixerThrottleCommand = rcCommand[THROTTLE];
|
||||
} else if ((throttlePrevious <= (PWM_RANGE_MIDDLE - rcControlsConfig()->deadband3d_throttle))) { // Deadband handling from negative to positive
|
||||
mixerThrottleCommand = throttleMax = flight3DConfig()->deadband3d_low;
|
||||
throttleMin = motorConfig()->minthrottle;
|
||||
} else if ((throttlePrevious <= (PWM_RANGE_MIDDLE - rcControlsConfig()->mid_throttle_deadband))) { // Deadband handling from negative to positive
|
||||
mixerThrottleCommand = throttleMax = reversibleMotorsConfig()->deadband_low;
|
||||
throttleMin = throttleIdleValue;
|
||||
} else { // Deadband handling from positive to negative
|
||||
throttleMax = motorConfig()->maxthrottle;
|
||||
mixerThrottleCommand = throttleMin = flight3DConfig()->deadband3d_high;
|
||||
mixerThrottleCommand = throttleMin = reversibleMotorsConfig()->deadband_high;
|
||||
}
|
||||
} else {
|
||||
mixerThrottleCommand = rcCommand[THROTTLE];
|
||||
throttleMin = motorConfig()->minthrottle;
|
||||
throttleMin = throttleIdleValue;
|
||||
throttleMax = motorConfig()->maxthrottle;
|
||||
|
||||
// Throttle scaling to limit max throttle when battery is full
|
||||
|
|
@ -399,22 +424,22 @@ void FAST_CODE NOINLINE mixTable(const float dT)
|
|||
|
||||
if (failsafeIsActive()) {
|
||||
motor[i] = constrain(motor[i], motorConfig()->mincommand, motorConfig()->maxthrottle);
|
||||
} else if (feature(FEATURE_3D)) {
|
||||
if (throttlePrevious <= (PWM_RANGE_MIDDLE - rcControlsConfig()->deadband3d_throttle)) {
|
||||
motor[i] = constrain(motor[i], motorConfig()->minthrottle, flight3DConfig()->deadband3d_low);
|
||||
} else if (feature(FEATURE_REVERSIBLE_MOTORS)) {
|
||||
if (throttlePrevious <= (PWM_RANGE_MIDDLE - rcControlsConfig()->mid_throttle_deadband)) {
|
||||
motor[i] = constrain(motor[i], throttleIdleValue, reversibleMotorsConfig()->deadband_low);
|
||||
} else {
|
||||
motor[i] = constrain(motor[i], flight3DConfig()->deadband3d_high, motorConfig()->maxthrottle);
|
||||
motor[i] = constrain(motor[i], reversibleMotorsConfig()->deadband_high, motorConfig()->maxthrottle);
|
||||
}
|
||||
} else {
|
||||
motor[i] = constrain(motor[i], motorConfig()->minthrottle, motorConfig()->maxthrottle);
|
||||
motor[i] = constrain(motor[i], throttleIdleValue, motorConfig()->maxthrottle);
|
||||
}
|
||||
|
||||
// Motor stop handling
|
||||
if (ARMING_FLAG(ARMED) && (getMotorStatus() != MOTOR_RUNNING)) {
|
||||
if (feature(FEATURE_MOTOR_STOP)) {
|
||||
motor[i] = (feature(FEATURE_3D) ? PWM_RANGE_MIDDLE : motorConfig()->mincommand);
|
||||
motor[i] = (feature(FEATURE_REVERSIBLE_MOTORS) ? PWM_RANGE_MIDDLE : motorConfig()->mincommand);
|
||||
} else {
|
||||
motor[i] = motorConfig()->minthrottle;
|
||||
motor[i] = throttleIdleValue;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -435,7 +460,7 @@ motorStatus_e getMotorStatus(void)
|
|||
}
|
||||
|
||||
if (rxGetChannelValue(THROTTLE) < rxConfig()->mincheck) {
|
||||
if ((STATE(FIXED_WING) || !STATE(AIRMODE_ACTIVE)) && (!(navigationIsFlyingAutonomousMode() && navConfig()->general.flags.auto_overrides_motor_stop)) && (!failsafeIsActive())) {
|
||||
if ((STATE(FIXED_WING_LEGACY) || !STATE(AIRMODE_ACTIVE)) && (!(navigationIsFlyingAutonomousMode() && navConfig()->general.flags.auto_overrides_motor_stop)) && (!failsafeIsActive())) {
|
||||
return MOTOR_STOPPED_USER;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -72,17 +72,16 @@ typedef struct mixerConfig_s {
|
|||
|
||||
PG_DECLARE(mixerConfig_t, mixerConfig);
|
||||
|
||||
typedef struct flight3DConfig_s {
|
||||
uint16_t deadband3d_low; // min 3d value
|
||||
uint16_t deadband3d_high; // max 3d value
|
||||
uint16_t neutral3d; // center 3d value
|
||||
} flight3DConfig_t;
|
||||
typedef struct reversibleMotorsConfig_s {
|
||||
uint16_t deadband_low; // min 3d value
|
||||
uint16_t deadband_high; // max 3d value
|
||||
uint16_t neutral; // center 3d value
|
||||
} reversibleMotorsConfig_t;
|
||||
|
||||
PG_DECLARE(flight3DConfig_t, flight3DConfig);
|
||||
PG_DECLARE(reversibleMotorsConfig_t, reversibleMotorsConfig);
|
||||
|
||||
typedef struct motorConfig_s {
|
||||
// PWM values, in milliseconds, common range is 1000-2000 (1ms to 2ms)
|
||||
uint16_t minthrottle; // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed.
|
||||
uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000
|
||||
uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs
|
||||
uint16_t motorPwmRate; // The update rate of motor outputs (50-498Hz)
|
||||
|
|
@ -90,6 +89,7 @@ typedef struct motorConfig_s {
|
|||
uint16_t motorAccelTimeMs; // Time limit for motor to accelerate from 0 to 100% throttle [ms]
|
||||
uint16_t motorDecelTimeMs; // Time limit for motor to decelerate from 0 to 100% throttle [ms]
|
||||
uint16_t digitalIdleOffsetValue;
|
||||
float throttleIdle; // Throttle IDLE value based on min_command, max_throttle, in percent
|
||||
float throttleScale; // Scaling factor for throttle.
|
||||
uint8_t motorPoleCount; // Magnetic poles in the motors for calculating actual RPM from eRPM provided by ESC telemetry
|
||||
} motorConfig_t;
|
||||
|
|
@ -106,6 +106,7 @@ extern int16_t motor[MAX_SUPPORTED_MOTORS];
|
|||
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
|
||||
extern int mixerThrottleCommand;
|
||||
|
||||
int getThrottleIdleValue(void);
|
||||
uint8_t getMotorCount(void);
|
||||
float getMotorMixRange(void);
|
||||
bool mixerIsOutputSaturated(void);
|
||||
|
|
|
|||
|
|
@ -81,7 +81,8 @@ typedef struct {
|
|||
// Rate filtering
|
||||
rateLimitFilter_t axisAccelFilter;
|
||||
pt1Filter_t ptermLpfState;
|
||||
biquadFilter_t deltaLpfState;
|
||||
filter_t dtermLpfState;
|
||||
filter_t dtermLpf2State;
|
||||
// Dterm notch filtering
|
||||
biquadFilter_t deltaNotchFilter;
|
||||
float stickPosition;
|
||||
|
|
@ -145,6 +146,7 @@ static EXTENDED_FASTRAM uint8_t usedPidControllerType;
|
|||
typedef void (*pidControllerFnPtr)(pidState_t *pidState, flight_dynamics_index_t axis, float dT);
|
||||
static EXTENDED_FASTRAM pidControllerFnPtr pidControllerApplyFn;
|
||||
static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn;
|
||||
static EXTENDED_FASTRAM filterApplyFnPtr dTermLpf2FilterApplyFn;
|
||||
|
||||
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 12);
|
||||
|
||||
|
|
@ -229,7 +231,10 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
|
|||
|
||||
.dterm_soft_notch_hz = 0,
|
||||
.dterm_soft_notch_cutoff = 1,
|
||||
.dterm_lpf_type = 1, //Default to BIQUAD
|
||||
.dterm_lpf_hz = 40,
|
||||
.dterm_lpf2_type = 1, //Default to BIQUAD
|
||||
.dterm_lpf2_hz = 0, // Off by default
|
||||
.yaw_lpf_hz = 0,
|
||||
.dterm_setpoint_weight = 1.0f,
|
||||
.use_dterm_fir_filter = 1,
|
||||
|
|
@ -307,8 +312,25 @@ bool pidInitFilters(void)
|
|||
}
|
||||
|
||||
// Init other filters
|
||||
if (pidProfile()->dterm_lpf_hz) {
|
||||
for (int axis = 0; axis < 3; ++ axis) {
|
||||
biquadFilterInitLPF(&pidState[axis].deltaLpfState, pidProfile()->dterm_lpf_hz, refreshRate);
|
||||
if (pidProfile()->dterm_lpf_type == FILTER_PT1) {
|
||||
pt1FilterInit(&pidState[axis].dtermLpfState.pt1, pidProfile()->dterm_lpf_hz, refreshRate * 1e-6f);
|
||||
} else {
|
||||
biquadFilterInitLPF(&pidState[axis].dtermLpfState.biquad, pidProfile()->dterm_lpf_hz, refreshRate);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Init other filters
|
||||
if (pidProfile()->dterm_lpf2_hz) {
|
||||
for (int axis = 0; axis < 3; ++ axis) {
|
||||
if (pidProfile()->dterm_lpf2_type == FILTER_PT1) {
|
||||
pt1FilterInit(&pidState[axis].dtermLpf2State.pt1, pidProfile()->dterm_lpf2_hz, refreshRate * 1e-6f);
|
||||
} else {
|
||||
biquadFilterInitLPF(&pidState[axis].dtermLpf2State.biquad, pidProfile()->dterm_lpf2_hz, refreshRate);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
|
||||
|
|
@ -334,7 +356,7 @@ void pidResetTPAFilter(void)
|
|||
{
|
||||
if (usedPidControllerType == PID_TYPE_PIFF && currentControlRateProfile->throttle.fixedWingTauMs > 0) {
|
||||
pt1FilterInitRC(&fixedWingTpaFilter, currentControlRateProfile->throttle.fixedWingTauMs * 1e-3f, getLooptime() * 1e-6f);
|
||||
pt1FilterReset(&fixedWingTpaFilter, motorConfig()->minthrottle);
|
||||
pt1FilterReset(&fixedWingTpaFilter, getThrottleIdleValue());
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -382,10 +404,10 @@ static float calculateFixedWingTPAFactor(uint16_t throttle)
|
|||
|
||||
// tpa_rate is amount of curve TPA applied to PIDs
|
||||
// tpa_breakpoint for fixed wing is cruise throttle value (value at which PIDs were tuned)
|
||||
if (currentControlRateProfile->throttle.dynPID != 0 && currentControlRateProfile->throttle.pa_breakpoint > motorConfig()->minthrottle) {
|
||||
if (throttle > motorConfig()->minthrottle) {
|
||||
if (currentControlRateProfile->throttle.dynPID != 0 && currentControlRateProfile->throttle.pa_breakpoint > getThrottleIdleValue()) {
|
||||
if (throttle > getThrottleIdleValue()) {
|
||||
// Calculate TPA according to throttle
|
||||
tpaFactor = 0.5f + ((float)(currentControlRateProfile->throttle.pa_breakpoint - motorConfig()->minthrottle) / (throttle - motorConfig()->minthrottle) / 2.0f);
|
||||
tpaFactor = 0.5f + ((float)(currentControlRateProfile->throttle.pa_breakpoint - getThrottleIdleValue()) / (throttle - getThrottleIdleValue()) / 2.0f);
|
||||
|
||||
// Limit to [0.5; 2] range
|
||||
tpaFactor = constrainf(tpaFactor, 0.5f, 2.0f);
|
||||
|
|
@ -521,7 +543,7 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h
|
|||
float angleTarget = pidRcCommandToAngle(rcCommand[axis], pidProfile()->max_angle_inclination[axis]);
|
||||
|
||||
// Automatically pitch down if the throttle is manually controlled and reduced bellow cruise throttle
|
||||
if ((axis == FD_PITCH) && STATE(FIXED_WING) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle())
|
||||
if ((axis == FD_PITCH) && STATE(FIXED_WING_LEGACY) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle())
|
||||
angleTarget += scaleRange(MAX(0, navConfig()->fw.cruise_throttle - rcCommand[THROTTLE]), 0, navConfig()->fw.cruise_throttle - PWM_RANGE_MIN, 0, mixerConfig()->fwMinThrottleDownPitchAngle);
|
||||
|
||||
const float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - attitude.raw[axis]);
|
||||
|
|
@ -698,12 +720,9 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid
|
|||
// Apply D-term notch
|
||||
deltaFiltered = notchFilterApplyFn(&pidState->deltaNotchFilter, deltaFiltered);
|
||||
|
||||
#ifdef USE_RPM_FILTER
|
||||
deltaFiltered = rpmFilterDtermApply((uint8_t)axis, deltaFiltered);
|
||||
#endif
|
||||
|
||||
// Apply additional lowpass
|
||||
deltaFiltered = dTermLpfFilterApplyFn(&pidState->deltaLpfState, deltaFiltered);
|
||||
deltaFiltered = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, deltaFiltered);
|
||||
deltaFiltered = dTermLpf2FilterApplyFn((filter_t *) &pidState->dtermLpf2State, deltaFiltered);
|
||||
|
||||
firFilterUpdate(&pidState->gyroRateFilter, deltaFiltered);
|
||||
newDTerm = firFilterApply(&pidState->gyroRateFilter);
|
||||
|
|
@ -851,7 +870,7 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState)
|
|||
targetRates.x = 0.0f;
|
||||
targetRates.y = 0.0f;
|
||||
|
||||
if (STATE(FIXED_WING)) {
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
if (calculateCosTiltAngle() >= 0.173648f) {
|
||||
// Ideal banked turn follow the equations:
|
||||
// forward_vel^2 / radius = Gravity * tan(roll_angle)
|
||||
|
|
@ -895,7 +914,7 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState)
|
|||
pidState[PITCH].rateTarget = constrainf(pidState[PITCH].rateTarget + targetRates.y, -currentControlRateProfile->stabilized.rates[PITCH] * 10.0f, currentControlRateProfile->stabilized.rates[PITCH] * 10.0f);
|
||||
|
||||
// Replace YAW on quads - add it in on airplanes
|
||||
if (STATE(FIXED_WING)) {
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
pidState[YAW].rateTarget = constrainf(pidState[YAW].rateTarget + targetRates.z * pidProfile()->fixedWingCoordinatedYawGain, -currentControlRateProfile->stabilized.rates[YAW] * 10.0f, currentControlRateProfile->stabilized.rates[YAW] * 10.0f);
|
||||
}
|
||||
else {
|
||||
|
|
@ -1008,7 +1027,7 @@ pidType_e pidIndexGetType(pidIndex_e pidIndex)
|
|||
if (pidIndex == PID_ROLL || pidIndex == PID_PITCH || pidIndex == PID_YAW) {
|
||||
return usedPidControllerType;
|
||||
}
|
||||
if (STATE(FIXED_WING)) {
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
if (pidIndex == PID_VEL_XY || pidIndex == PID_VEL_Z) {
|
||||
return PID_TYPE_NONE;
|
||||
}
|
||||
|
|
@ -1062,21 +1081,35 @@ void pidInit(void)
|
|||
}
|
||||
|
||||
if (pidProfile()->pidControllerType == PID_TYPE_AUTO) {
|
||||
if (mixerConfig()->platformType == PLATFORM_MULTIROTOR ||
|
||||
mixerConfig()->platformType == PLATFORM_TRICOPTER ||
|
||||
mixerConfig()->platformType == PLATFORM_HELICOPTER) {
|
||||
usedPidControllerType = PID_TYPE_PID;
|
||||
} else {
|
||||
if (
|
||||
mixerConfig()->platformType == PLATFORM_AIRPLANE ||
|
||||
mixerConfig()->platformType == PLATFORM_BOAT ||
|
||||
mixerConfig()->platformType == PLATFORM_ROVER
|
||||
) {
|
||||
usedPidControllerType = PID_TYPE_PIFF;
|
||||
} else {
|
||||
usedPidControllerType = PID_TYPE_PID;
|
||||
}
|
||||
} else {
|
||||
usedPidControllerType = pidProfile()->pidControllerType;
|
||||
}
|
||||
|
||||
if (pidProfile()->dterm_lpf_hz) {
|
||||
dTermLpfFilterApplyFn = (filterApplyFnPtr) biquadFilterApply;
|
||||
} else {
|
||||
dTermLpfFilterApplyFn = nullFilterApply;
|
||||
if (pidProfile()->dterm_lpf_hz) {
|
||||
if (pidProfile()->dterm_lpf_type == FILTER_PT1) {
|
||||
dTermLpfFilterApplyFn = (filterApplyFnPtr) pt1FilterApply;
|
||||
} else {
|
||||
dTermLpfFilterApplyFn = (filterApplyFnPtr) biquadFilterApply;
|
||||
}
|
||||
}
|
||||
|
||||
dTermLpf2FilterApplyFn = nullFilterApply;
|
||||
if (pidProfile()->dterm_lpf2_hz) {
|
||||
if (pidProfile()->dterm_lpf2_type == FILTER_PT1) {
|
||||
dTermLpf2FilterApplyFn = (filterApplyFnPtr) pt1FilterApply;
|
||||
} else {
|
||||
dTermLpf2FilterApplyFn = (filterApplyFnPtr) biquadFilterApply;
|
||||
}
|
||||
}
|
||||
|
||||
if (usedPidControllerType == PID_TYPE_PIFF) {
|
||||
|
|
|
|||
|
|
@ -106,7 +106,13 @@ typedef struct pidProfile_s {
|
|||
|
||||
uint16_t dterm_soft_notch_hz; // Dterm Notch frequency
|
||||
uint16_t dterm_soft_notch_cutoff; // Dterm Notch Cutoff frequency
|
||||
uint8_t dterm_lpf_hz; // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5
|
||||
|
||||
uint8_t dterm_lpf_type; // Dterm LPF type: PT1, BIQUAD
|
||||
uint16_t dterm_lpf_hz;
|
||||
|
||||
uint8_t dterm_lpf2_type; // Dterm LPF type: PT1, BIQUAD
|
||||
uint16_t dterm_lpf2_hz;
|
||||
|
||||
uint8_t use_dterm_fir_filter; // Use classical INAV FIR differentiator. Very noise robust, can be quite slowish
|
||||
|
||||
uint8_t yaw_lpf_hz;
|
||||
|
|
|
|||
|
|
@ -43,17 +43,13 @@
|
|||
#define RPM_FILTER_RPM_LPF_HZ 150
|
||||
#define RPM_FILTER_HARMONICS 3
|
||||
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 0);
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 1);
|
||||
|
||||
PG_RESET_TEMPLATE(rpmFilterConfig_t, rpmFilterConfig,
|
||||
.gyro_filter_enabled = 0,
|
||||
.dterm_filter_enabled = 0,
|
||||
.gyro_harmonics = 1,
|
||||
.gyro_min_hz = 100,
|
||||
.gyro_q = 500,
|
||||
.dterm_harmonics = 1,
|
||||
.dterm_min_hz = 100,
|
||||
.dterm_q = 500, );
|
||||
.gyro_q = 500, );
|
||||
|
||||
typedef struct
|
||||
{
|
||||
|
|
@ -70,11 +66,8 @@ typedef void (*rpmFilterUpdateFnPtr)(rpmFilterBank_t *filterBank, uint8_t motor,
|
|||
static EXTENDED_FASTRAM pt1Filter_t motorFrequencyFilter[MAX_SUPPORTED_MOTORS];
|
||||
static EXTENDED_FASTRAM float erpmToHz;
|
||||
static EXTENDED_FASTRAM rpmFilterBank_t gyroRpmFilters;
|
||||
static EXTENDED_FASTRAM rpmFilterBank_t dtermRpmFilters;
|
||||
static EXTENDED_FASTRAM rpmFilterApplyFnPtr rpmGyroApplyFn;
|
||||
static EXTENDED_FASTRAM rpmFilterApplyFnPtr rpmDtermApplyFn;
|
||||
static EXTENDED_FASTRAM rpmFilterUpdateFnPtr rpmGyroUpdateFn;
|
||||
static EXTENDED_FASTRAM rpmFilterUpdateFnPtr rpmDtermUpdateFn;
|
||||
|
||||
float nullRpmFilterApply(rpmFilterBank_t *filter, uint8_t axis, float input)
|
||||
{
|
||||
|
|
@ -141,7 +134,6 @@ static void rpmFilterInit(rpmFilterBank_t *filter, uint16_t q, uint8_t minHz, ui
|
|||
|
||||
void disableRpmFilters(void) {
|
||||
rpmGyroApplyFn = (rpmFilterApplyFnPtr)nullRpmFilterApply;
|
||||
rpmDtermApplyFn = (rpmFilterApplyFnPtr)nullRpmFilterApply;
|
||||
}
|
||||
|
||||
void FAST_CODE NOINLINE rpmFilterUpdate(rpmFilterBank_t *filterBank, uint8_t motor, float baseFrequency)
|
||||
|
|
@ -172,7 +164,6 @@ void rpmFiltersInit(void)
|
|||
erpmToHz = ERPM_PER_LSB / (motorConfig()->motorPoleCount / 2) / RPM_TO_HZ;
|
||||
|
||||
rpmGyroUpdateFn = (rpmFilterUpdateFnPtr)nullRpmFilterUpdate;
|
||||
rpmDtermUpdateFn = (rpmFilterUpdateFnPtr)nullRpmFilterUpdate;
|
||||
|
||||
if (rpmFilterConfig()->gyro_filter_enabled)
|
||||
{
|
||||
|
|
@ -184,17 +175,6 @@ void rpmFiltersInit(void)
|
|||
rpmGyroApplyFn = (rpmFilterApplyFnPtr)rpmFilterApply;
|
||||
rpmGyroUpdateFn = (rpmFilterUpdateFnPtr)rpmFilterUpdate;
|
||||
}
|
||||
|
||||
if (rpmFilterConfig()->dterm_filter_enabled)
|
||||
{
|
||||
rpmFilterInit(
|
||||
&dtermRpmFilters,
|
||||
rpmFilterConfig()->dterm_q,
|
||||
rpmFilterConfig()->dterm_min_hz,
|
||||
rpmFilterConfig()->dterm_harmonics);
|
||||
rpmDtermApplyFn = (rpmFilterApplyFnPtr)rpmFilterApply;
|
||||
rpmDtermUpdateFn = (rpmFilterUpdateFnPtr)rpmFilterUpdate;
|
||||
}
|
||||
}
|
||||
|
||||
void FAST_CODE NOINLINE rpmFilterUpdateTask(timeUs_t currentTimeUs)
|
||||
|
|
@ -215,7 +195,6 @@ void FAST_CODE NOINLINE rpmFilterUpdateTask(timeUs_t currentTimeUs)
|
|||
}
|
||||
|
||||
rpmGyroUpdateFn(&gyroRpmFilters, i, baseFrequency);
|
||||
rpmDtermUpdateFn(&dtermRpmFilters, i, baseFrequency);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -224,9 +203,4 @@ float FAST_CODE rpmFilterGyroApply(uint8_t axis, float input)
|
|||
return rpmGyroApplyFn(&gyroRpmFilters, axis, input);
|
||||
}
|
||||
|
||||
float FAST_CODE rpmFilterDtermApply(uint8_t axis, float input)
|
||||
{
|
||||
return rpmDtermApplyFn(&dtermRpmFilters, axis, input);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -50,4 +50,3 @@ void disableRpmFilters(void);
|
|||
void rpmFiltersInit(void);
|
||||
void rpmFilterUpdateTask(timeUs_t currentTimeUs);
|
||||
float rpmFilterGyroApply(uint8_t axis, float input);
|
||||
float rpmFilterDtermApply(uint8_t axis, float input);
|
||||
|
|
@ -79,7 +79,7 @@ static int8_t RTHInitialAltitudeChangePitchAngle(float altitudeChange) {
|
|||
static float estimatePitchPower(float pitch) {
|
||||
int16_t altitudeChangeThrottle = fixedWingPitchToThrottleCorrection(DEGREES_TO_DECIDEGREES(pitch));
|
||||
altitudeChangeThrottle = constrain(altitudeChangeThrottle, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle);
|
||||
const float altitudeChangeThrToCruiseThrRatio = (float)(altitudeChangeThrottle - motorConfig()->minthrottle) / (navConfig()->fw.cruise_throttle - motorConfig()->minthrottle);
|
||||
const float altitudeChangeThrToCruiseThrRatio = (float)(altitudeChangeThrottle - getThrottleIdleValue()) / (navConfig()->fw.cruise_throttle - getThrottleIdleValue());
|
||||
return (float)heatLossesCompensatedPower(batteryMetersConfig()->idle_power + batteryMetersConfig()->cruise_power * altitudeChangeThrToCruiseThrRatio) / 100;
|
||||
}
|
||||
|
||||
|
|
@ -146,10 +146,10 @@ static float estimateRTHEnergyAfterInitialClimb(float distanceToHome, float spee
|
|||
// returns Wh
|
||||
static float calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) {
|
||||
// Fixed wing only for now
|
||||
if (!STATE(FIXED_WING))
|
||||
if (!STATE(FIXED_WING_LEGACY))
|
||||
return -1;
|
||||
|
||||
if (!(feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER) && navigationPositionEstimateIsHealthy() && (batteryMetersConfig()->cruise_power > 0) && (ARMING_FLAG(ARMED)) && ((!STATE(FIXED_WING)) || (isNavLaunchEnabled() && isFixedWingLaunchDetected())) && (navConfig()->fw.cruise_speed > 0) && (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH) && (currentBatteryProfile->capacity.value > 0) && batteryWasFullWhenPluggedIn() && isImuHeadingValid()
|
||||
if (!(feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER) && navigationPositionEstimateIsHealthy() && (batteryMetersConfig()->cruise_power > 0) && (ARMING_FLAG(ARMED)) && ((!STATE(FIXED_WING_LEGACY)) || (isNavLaunchEnabled() && isFixedWingLaunchDetected())) && (navConfig()->fw.cruise_speed > 0) && (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH) && (currentBatteryProfile->capacity.value > 0) && batteryWasFullWhenPluggedIn() && isImuHeadingValid()
|
||||
#ifdef USE_WIND_ESTIMATOR
|
||||
&& isEstimatedWindSpeedValid()
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -247,7 +247,7 @@ void servoMixer(float dT)
|
|||
input[INPUT_STABILIZED_YAW] = axisPID[YAW];
|
||||
|
||||
// Reverse yaw servo when inverted in 3D mode only for multirotor and tricopter
|
||||
if (feature(FEATURE_3D) && (rxGetChannelValue(THROTTLE) < PWM_RANGE_MIDDLE) &&
|
||||
if (feature(FEATURE_REVERSIBLE_MOTORS) && (rxGetChannelValue(THROTTLE) < PWM_RANGE_MIDDLE) &&
|
||||
(mixerConfig()->platformType == PLATFORM_MULTIROTOR || mixerConfig()->platformType == PLATFORM_TRICOPTER)) {
|
||||
input[INPUT_STABILIZED_YAW] *= -1;
|
||||
}
|
||||
|
|
@ -449,6 +449,11 @@ bool FAST_CODE NOINLINE isServoOutputEnabled(void)
|
|||
return servoOutputEnabled;
|
||||
}
|
||||
|
||||
void NOINLINE setServoOutputEnabled(bool flag)
|
||||
{
|
||||
servoOutputEnabled = flag;
|
||||
}
|
||||
|
||||
bool FAST_CODE NOINLINE isMixerUsingServos(void)
|
||||
{
|
||||
return mixerUsesServos;
|
||||
|
|
|
|||
|
|
@ -142,6 +142,7 @@ typedef struct servoMetadata_s {
|
|||
extern int16_t servo[MAX_SUPPORTED_SERVOS];
|
||||
|
||||
bool isServoOutputEnabled(void);
|
||||
void setServoOutputEnabled(bool flag);
|
||||
bool isMixerUsingServos(void);
|
||||
void writeServos(void);
|
||||
void loadCustomServoMixer(void);
|
||||
|
|
|
|||
|
|
@ -79,7 +79,7 @@ void updateWindEstimator(timeUs_t currentTimeUs)
|
|||
{
|
||||
static timeUs_t lastUpdateUs = 0;
|
||||
|
||||
if (!STATE(FIXED_WING) ||
|
||||
if (!STATE(FIXED_WING_LEGACY) ||
|
||||
!isGPSHeadingValid() ||
|
||||
!gpsSol.flags.validVelNE ||
|
||||
!gpsSol.flags.validVelD) {
|
||||
|
|
|
|||
|
|
@ -811,7 +811,7 @@ static const char * navigationStateMessage(void)
|
|||
case MW_NAV_STATE_LAND_IN_PROGRESS:
|
||||
return OSD_MESSAGE_STR("LANDING");
|
||||
case MW_NAV_STATE_HOVER_ABOVE_HOME:
|
||||
if (STATE(FIXED_WING)) {
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
return OSD_MESSAGE_STR("LOITERING AROUND HOME");
|
||||
}
|
||||
return OSD_MESSAGE_STR("HOVERING");
|
||||
|
|
@ -2113,7 +2113,7 @@ static bool osdDrawSingleElement(uint8_t item)
|
|||
if (navStateMessage) {
|
||||
messages[messageCount++] = navStateMessage;
|
||||
}
|
||||
} else if (STATE(FIXED_WING) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) {
|
||||
} else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) {
|
||||
messages[messageCount++] = "AUTOLAUNCH";
|
||||
} else {
|
||||
if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) {
|
||||
|
|
|
|||
|
|
@ -116,7 +116,7 @@ void osdCanvasDrawDirArrow(displayPort_t *display, displayCanvas_t *canvas, cons
|
|||
displayCanvasSetFillColor(canvas, DISPLAY_CANVAS_COLOR_WHITE);
|
||||
displayCanvasSetStrokeColor(canvas, DISPLAY_CANVAS_COLOR_BLACK);
|
||||
|
||||
displayCanvasCtmRotate(canvas, -DEGREES_TO_RADIANS(degrees));
|
||||
displayCanvasCtmRotate(canvas, -DEGREES_TO_RADIANS(180 + degrees));
|
||||
displayCanvasCtmTranslate(canvas, px + canvas->gridElementWidth / 2, py + canvas->gridElementHeight / 2);
|
||||
displayCanvasFillStrokeTriangle(canvas, 0, 6, 5, -6, -5, -6);
|
||||
displayCanvasSetFillColor(canvas, DISPLAY_CANVAS_COLOR_TRANSPARENT);
|
||||
|
|
|
|||
653
src/main/io/osd_dji_hd.c
Normal file
653
src/main/io/osd_dji_hd.c
Normal file
|
|
@ -0,0 +1,653 @@
|
|||
/*
|
||||
* This file is part of INAV.
|
||||
*
|
||||
* This Source Code Form is subject to the terms of the Mozilla Public
|
||||
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
|
||||
* You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
*
|
||||
* Alternatively, the contents of this file may be used under the terms
|
||||
* of the GNU General Public License Version 3, as described below:
|
||||
*
|
||||
* This file is free software: you may copy, redistribute and/or modify
|
||||
* it under the terms of the GNU General Public License as published by the
|
||||
* Free Software Foundation, either version 3 of the License, or (at your
|
||||
* option) any later version.
|
||||
*
|
||||
* This file is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
|
||||
* Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see http://www.gnu.org/licenses/.
|
||||
*
|
||||
* @author Konstantin Sharlaimov (ksharlaimov@inavflight.com)
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#include "build/debug.h"
|
||||
#include "build/version.h"
|
||||
|
||||
#include "common/streambuf.h"
|
||||
#include "common/utils.h"
|
||||
#include "common/maths.h"
|
||||
#include "common/time.h"
|
||||
#include "common/crc.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 "flight/imu.h"
|
||||
#include "flight/pid.h"
|
||||
#include "flight/mixer.h"
|
||||
|
||||
#include "io/serial.h"
|
||||
#include "io/gps.h"
|
||||
#include "io/osd.h"
|
||||
#include "io/osd_dji_hd.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
|
||||
#include "sensors/sensors.h"
|
||||
#include "sensors/gyro.h"
|
||||
#include "sensors/battery.h"
|
||||
#include "sensors/rangefinder.h"
|
||||
#include "sensors/acceleration.h"
|
||||
#include "sensors/esc_sensor.h"
|
||||
|
||||
#include "msp/msp.h"
|
||||
#include "msp/msp_protocol.h"
|
||||
#include "msp/msp_serial.h"
|
||||
|
||||
#include "navigation/navigation.h"
|
||||
|
||||
#include "scheduler/scheduler.h"
|
||||
|
||||
#if defined(USE_DJI_HD_OSD)
|
||||
|
||||
#define DJI_MSP_BAUDRATE 115200
|
||||
|
||||
#define DJI_ARMING_DISABLE_FLAGS_COUNT 25
|
||||
#define DJI_OSD_WARNING_COUNT 16
|
||||
#define DJI_OSD_TIMER_COUNT 2
|
||||
#define DJI_OSD_FLAGS_OSD_FEATURE (1 << 0)
|
||||
|
||||
/*
|
||||
* DJI HD goggles use MSPv1 compatible with Betaflight 4.1.0
|
||||
* DJI uses a subset of messages and assume fixed bit positions for flight modes
|
||||
*
|
||||
* To avoid compatibility issues we maintain a separate MSP command processor
|
||||
* but reuse the packet decoder to minimize code duplication
|
||||
*/
|
||||
|
||||
// External dependency on looptime
|
||||
extern timeDelta_t cycleTime;
|
||||
|
||||
// MSP packet decoder state structure
|
||||
static mspPort_t djiMspPort;
|
||||
|
||||
// Mapping table between DJI PID and INAV PID (order is different)
|
||||
const uint8_t djiPidIndexMap[] = {
|
||||
PID_ROLL, // DJI: PID_ROLL
|
||||
PID_PITCH, // DJI: PID_PITCH
|
||||
PID_YAW, // DJI: PID_YAW
|
||||
PID_LEVEL, // DJI: PID_LEVEL
|
||||
PID_HEADING // DJI: PID_MAG
|
||||
};
|
||||
|
||||
const int djiOSDItemIndexMap[] = {
|
||||
OSD_RSSI_VALUE, // DJI: OSD_RSSI_VALUE
|
||||
OSD_MAIN_BATT_VOLTAGE, // DJI: OSD_MAIN_BATT_VOLTAGE
|
||||
OSD_CROSSHAIRS, // DJI: OSD_CROSSHAIRS
|
||||
OSD_ARTIFICIAL_HORIZON, // DJI: OSD_ARTIFICIAL_HORIZON
|
||||
OSD_HORIZON_SIDEBARS, // DJI: OSD_HORIZON_SIDEBARS
|
||||
OSD_ONTIME, // DJI: OSD_ITEM_TIMER_1
|
||||
OSD_FLYTIME, // DJI: OSD_ITEM_TIMER_2
|
||||
OSD_FLYMODE, // DJI: OSD_FLYMODE
|
||||
OSD_CRAFT_NAME, // DJI: OSD_CRAFT_NAME
|
||||
OSD_THROTTLE_POS, // DJI: OSD_THROTTLE_POS
|
||||
OSD_VTX_CHANNEL, // DJI: OSD_VTX_CHANNEL
|
||||
OSD_CURRENT_DRAW, // DJI: OSD_CURRENT_DRAW
|
||||
OSD_MAH_DRAWN, // DJI: OSD_MAH_DRAWN
|
||||
OSD_GPS_SPEED, // DJI: OSD_GPS_SPEED
|
||||
OSD_GPS_SATS, // DJI: OSD_GPS_SATS
|
||||
OSD_ALTITUDE, // DJI: OSD_ALTITUDE
|
||||
OSD_ROLL_PIDS, // DJI: OSD_ROLL_PIDS
|
||||
OSD_PITCH_PIDS, // DJI: OSD_PITCH_PIDS
|
||||
OSD_YAW_PIDS, // DJI: OSD_YAW_PIDS
|
||||
OSD_POWER, // DJI: OSD_POWER
|
||||
-1, // DJI: OSD_PIDRATE_PROFILE
|
||||
-1, // DJI: OSD_WARNINGS
|
||||
OSD_MAIN_BATT_CELL_VOLTAGE, // DJI: OSD_AVG_CELL_VOLTAGE
|
||||
OSD_GPS_LON, // DJI: OSD_GPS_LON
|
||||
OSD_GPS_LAT, // DJI: OSD_GPS_LAT
|
||||
OSD_DEBUG, // DJI: OSD_DEBUG
|
||||
OSD_ATTITUDE_PITCH, // DJI: OSD_PITCH_ANGLE
|
||||
OSD_ATTITUDE_ROLL, // DJI: OSD_ROLL_ANGLE
|
||||
-1, // DJI: OSD_MAIN_BATT_USAGE
|
||||
-1, // DJI: OSD_DISARMED
|
||||
OSD_HOME_DIR, // DJI: OSD_HOME_DIR
|
||||
OSD_HOME_DIST, // DJI: OSD_HOME_DIST
|
||||
OSD_HEADING, // DJI: OSD_NUMERICAL_HEADING
|
||||
OSD_VARIO_NUM, // DJI: OSD_NUMERICAL_VARIO
|
||||
-1, // DJI: OSD_COMPASS_BAR
|
||||
-1, // DJI: OSD_ESC_TMP
|
||||
OSD_ESC_RPM, // DJI: OSD_ESC_RPM
|
||||
OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH, // DJI: OSD_REMAINING_TIME_ESTIMATE
|
||||
OSD_RTC_TIME, // DJI: OSD_RTC_DATETIME
|
||||
-1, // DJI: OSD_ADJUSTMENT_RANGE
|
||||
-1, // DJI: OSD_CORE_TEMPERATURE
|
||||
-1, // DJI: OSD_ANTI_GRAVITY
|
||||
-1, // DJI: OSD_G_FORCE
|
||||
-1, // DJI: OSD_MOTOR_DIAG
|
||||
-1, // DJI: OSD_LOG_STATUS
|
||||
-1, // DJI: OSD_FLIP_ARROW
|
||||
-1, // DJI: OSD_LINK_QUALITY
|
||||
OSD_TRIP_DIST, // DJI: OSD_FLIGHT_DIST
|
||||
-1, // DJI: OSD_STICK_OVERLAY_LEFT
|
||||
-1, // DJI: OSD_STICK_OVERLAY_RIGHT
|
||||
-1, // DJI: OSD_DISPLAY_NAME
|
||||
-1, // DJI: OSD_ESC_RPM_FREQ
|
||||
-1, // DJI: OSD_RATE_PROFILE_NAME
|
||||
-1, // DJI: OSD_PID_PROFILE_NAME
|
||||
-1, // DJI: OSD_PROFILE_NAME
|
||||
-1, // DJI: OSD_RSSI_DBM_VALUE
|
||||
-1, // DJI: OSD_RC_CHANNELS
|
||||
};
|
||||
|
||||
const int djiOSDStatisticsMap[] = {
|
||||
-1, // DJI: OSD_STAT_RTC_DATE_TIME
|
||||
-1, // DJI: OSD_STAT_TIMER_1
|
||||
-1, // DJI: OSD_STAT_TIMER_2
|
||||
-1, // DJI: OSD_STAT_MAX_SPEED
|
||||
-1, // DJI: OSD_STAT_MAX_DISTANCE
|
||||
-1, // DJI: OSD_STAT_MIN_BATTERY
|
||||
-1, // DJI: OSD_STAT_END_BATTERY
|
||||
-1, // DJI: OSD_STAT_BATTERY
|
||||
-1, // DJI: OSD_STAT_MIN_RSSI
|
||||
-1, // DJI: OSD_STAT_MAX_CURRENT
|
||||
-1, // DJI: OSD_STAT_USED_MAH
|
||||
-1, // DJI: OSD_STAT_MAX_ALTITUDE
|
||||
-1, // DJI: OSD_STAT_BLACKBOX
|
||||
-1, // DJI: OSD_STAT_BLACKBOX_NUMBER
|
||||
-1, // DJI: OSD_STAT_MAX_G_FORCE
|
||||
-1, // DJI: OSD_STAT_MAX_ESC_TEMP
|
||||
-1, // DJI: OSD_STAT_MAX_ESC_RPM
|
||||
-1, // DJI: OSD_STAT_MIN_LINK_QUALITY
|
||||
-1, // DJI: OSD_STAT_FLIGHT_DISTANCE
|
||||
-1, // DJI: OSD_STAT_MAX_FFT
|
||||
-1, // DJI: OSD_STAT_TOTAL_FLIGHTS
|
||||
-1, // DJI: OSD_STAT_TOTAL_TIME
|
||||
-1, // DJI: OSD_STAT_TOTAL_DIST
|
||||
-1, // DJI: OSD_STAT_MIN_RSSI_DBM
|
||||
};
|
||||
|
||||
void djiOsdSerialInit(void)
|
||||
{
|
||||
memset(&djiMspPort, 0, sizeof(mspPort_t));
|
||||
|
||||
serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_DJI_HD_OSD);
|
||||
|
||||
if (!portConfig) {
|
||||
return;
|
||||
}
|
||||
|
||||
serialPort_t *serialPort = openSerialPort(portConfig->identifier, FUNCTION_DJI_HD_OSD, NULL, NULL, DJI_MSP_BAUDRATE, MODE_RXTX, SERIAL_NOT_INVERTED);
|
||||
|
||||
if (serialPort) {
|
||||
resetMspPort(&djiMspPort, serialPort);
|
||||
}
|
||||
}
|
||||
|
||||
static void djiPackBoxModeBitmask(boxBitmask_t * flightModeBitmask)
|
||||
{
|
||||
memset(flightModeBitmask, 0, sizeof(boxBitmask_t));
|
||||
|
||||
// Map flight modes to DJI-supported bits
|
||||
switch(getFlightModeForTelemetry()) {
|
||||
case FLM_MANUAL:
|
||||
case FLM_ACRO:
|
||||
case FLM_ACRO_AIR:
|
||||
// DJI: No bits set = ACRO
|
||||
break;
|
||||
case FLM_ANGLE:
|
||||
bitArraySet(flightModeBitmask->bits, 1); // DJI: 1 << 1 : ANGLE
|
||||
break;
|
||||
case FLM_HORIZON:
|
||||
bitArraySet(flightModeBitmask->bits, 2); // DJI: 1 << 2
|
||||
break;
|
||||
case FLM_RTH:
|
||||
bitArraySet(flightModeBitmask->bits, 5); // DJI: 1 << 5 : GPS_RESQUE
|
||||
break;
|
||||
case FLM_CRUISE:
|
||||
bitArraySet(flightModeBitmask->bits, 3); // DJI: 1 << 3 : technically HEADFREE
|
||||
break;
|
||||
case FLM_FAILSAFE:
|
||||
bitArraySet(flightModeBitmask->bits, 4); // DJI: 1 << 4
|
||||
break;
|
||||
case FLM_LAUNCH:
|
||||
case FLM_ALTITUDE_HOLD:
|
||||
case FLM_POSITION_HOLD:
|
||||
case FLM_MISSION:
|
||||
default:
|
||||
// Unsupported ATM, keep at ANGLE
|
||||
bitArraySet(flightModeBitmask->bits, 1); // DJI: 1 << 1 : ANGLE
|
||||
}
|
||||
|
||||
// Set ARMED mode
|
||||
if (ARMING_FLAG(ARMED)) {
|
||||
bitArraySet(flightModeBitmask->bits, 0); // DJI: 1 << 0 : ARMED
|
||||
}
|
||||
}
|
||||
|
||||
static uint32_t djiPackArmingDisabledFlags(void)
|
||||
{
|
||||
// TODO: Map INAV arming disabled flags to DJI/BF ones
|
||||
// https://github.com/betaflight/betaflight/blob/c6e5882dd91fa20d246b8f8af10cf6c92876bc3d/src/main/fc/runtime_config.h#L42
|
||||
// For now hide everything in ARMING_DISABLED_ARM_SWITCH (bit 24)
|
||||
|
||||
return isArmingDisabled() ? (1 << 24) : 0;
|
||||
}
|
||||
|
||||
static uint32_t djiEncodeOSDEnabledWarnings(void)
|
||||
{
|
||||
// TODO:
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void djiSerializeOSDConfigReply(sbuf_t *dst)
|
||||
{
|
||||
// Only send supported flag - always
|
||||
sbufWriteU8(dst, DJI_OSD_FLAGS_OSD_FEATURE);
|
||||
|
||||
// 7456 video system (AUTO/PAL/NTSC)
|
||||
sbufWriteU8(dst, osdConfig()->video_system);
|
||||
|
||||
// Configuration
|
||||
sbufWriteU8(dst, osdConfig()->units);
|
||||
|
||||
// Alarms
|
||||
sbufWriteU8(dst, osdConfig()->rssi_alarm);
|
||||
sbufWriteU16(dst, currentBatteryProfile->capacity.warning);
|
||||
|
||||
// OSD_ITEM_COUNT (previously was timer alarm)
|
||||
sbufWriteU8(dst, 0);
|
||||
sbufWriteU8(dst, ARRAYLEN(djiOSDItemIndexMap));
|
||||
|
||||
// Altitude alarm
|
||||
sbufWriteU16(dst, osdConfig()->alt_alarm);
|
||||
|
||||
// OSD element position and visibility
|
||||
for (unsigned i = 0; i < ARRAYLEN(djiOSDItemIndexMap); i++) {
|
||||
int inavOSDIdx = djiOSDItemIndexMap[i];
|
||||
if (inavOSDIdx >= 0) {
|
||||
// Position & visibility encoded in 16 bits. Position encoding is the same between BF/DJI and INAV
|
||||
// However visibility is different. INAV has 3 layouts, while BF only has visibility profiles
|
||||
// Here we use only one OSD layout mapped to first OSD BF profile
|
||||
uint16_t itemPos = osdConfig()->item_pos[0][inavOSDIdx];
|
||||
|
||||
// Workarounds for certain OSD element positions
|
||||
// INAV calculates these dynamically, while DJI expects the config to have defined coordinates
|
||||
switch(inavOSDIdx) {
|
||||
case OSD_CROSSHAIRS:
|
||||
itemPos = (itemPos & (~OSD_POS_MAX)) | OSD_POS(13, 6);
|
||||
break;
|
||||
|
||||
case OSD_ARTIFICIAL_HORIZON:
|
||||
itemPos = (itemPos & (~OSD_POS_MAX)) | OSD_POS(14, 2);
|
||||
break;
|
||||
|
||||
case OSD_HORIZON_SIDEBARS:
|
||||
itemPos = (itemPos & (~OSD_POS_MAX)) | OSD_POS(14, 5);
|
||||
break;
|
||||
}
|
||||
|
||||
// Enforce visibility in 3 BF OSD profiles
|
||||
if (OSD_VISIBLE(itemPos)) {
|
||||
itemPos |= 0x3000;
|
||||
}
|
||||
|
||||
sbufWriteU16(dst, itemPos);
|
||||
}
|
||||
else {
|
||||
// Hide OSD elements unsupported by INAV
|
||||
sbufWriteU16(dst, 0);
|
||||
}
|
||||
}
|
||||
|
||||
// Post flight statistics
|
||||
sbufWriteU8(dst, ARRAYLEN(djiOSDStatisticsMap));
|
||||
for (unsigned i = 0; i < ARRAYLEN(djiOSDStatisticsMap); i++ ) {
|
||||
if (djiOSDStatisticsMap[i] >= 0) {
|
||||
// FIXME: Map post-flight statistics from INAV to BF/DJI
|
||||
sbufWriteU8(dst, 0);
|
||||
}
|
||||
else {
|
||||
sbufWriteU8(dst, 0);
|
||||
}
|
||||
}
|
||||
|
||||
// Timers
|
||||
sbufWriteU8(dst, DJI_OSD_TIMER_COUNT);
|
||||
for (int i = 0; i < DJI_OSD_TIMER_COUNT; i++) {
|
||||
// STUB: We don't support BF's OSD timers
|
||||
sbufWriteU16(dst, 0);
|
||||
}
|
||||
|
||||
// Enabled warnings
|
||||
// API < 1.41 stub, kept for compatibility
|
||||
sbufWriteU16(dst, djiEncodeOSDEnabledWarnings() & 0xFFFF);
|
||||
|
||||
// API >= 1.41
|
||||
// Send the warnings count and 32bit enabled warnings flags.
|
||||
sbufWriteU8(dst, DJI_OSD_WARNING_COUNT);
|
||||
sbufWriteU32(dst, djiEncodeOSDEnabledWarnings());
|
||||
|
||||
// DJI OSD expects 1 OSD profile
|
||||
sbufWriteU8(dst, 1);
|
||||
sbufWriteU8(dst, 1);
|
||||
|
||||
// No OSD stick overlay
|
||||
sbufWriteU8(dst, 0);
|
||||
|
||||
// API >= 1.43
|
||||
// Camera frame element width/height - magic numbers taken from Betaflight source
|
||||
//sbufWriteU8(dst, DJI_OSD_SCREEN_WIDTH); // osdConfig()->camera_frame_width
|
||||
//sbufWriteU8(dst, DJI_OSD_SCREEN_HEIGHT); // osdConfig()->camera_frame_height
|
||||
}
|
||||
|
||||
static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn)
|
||||
{
|
||||
UNUSED(mspPostProcessFn);
|
||||
|
||||
sbuf_t *dst = &reply->buf;
|
||||
sbuf_t *src = &cmd->buf;
|
||||
|
||||
// Start initializing the reply message
|
||||
reply->cmd = cmd->cmd;
|
||||
reply->result = MSP_RESULT_ACK;
|
||||
|
||||
switch (cmd->cmd) {
|
||||
case DJI_MSP_API_VERSION:
|
||||
sbufWriteU8(dst, MSP_PROTOCOL_VERSION);
|
||||
sbufWriteU8(dst, DJI_API_VERSION_MAJOR);
|
||||
sbufWriteU8(dst, DJI_API_VERSION_MINOR);
|
||||
break;
|
||||
|
||||
case DJI_MSP_FC_VARIANT:
|
||||
{
|
||||
const char * const flightControllerIdentifier = INAV_IDENTIFIER;
|
||||
sbufWriteData(dst, flightControllerIdentifier, FLIGHT_CONTROLLER_IDENTIFIER_LENGTH);
|
||||
}
|
||||
break;
|
||||
|
||||
case DJI_MSP_FC_VERSION:
|
||||
sbufWriteU8(dst, 4);
|
||||
sbufWriteU8(dst, 1);
|
||||
sbufWriteU8(dst, 0);
|
||||
break;
|
||||
|
||||
case DJI_MSP_NAME:
|
||||
{
|
||||
const char * name = systemConfig()->name;
|
||||
int len = strlen(name);
|
||||
if (len > 12) len = 12;
|
||||
sbufWriteData(dst, name, len);
|
||||
}
|
||||
break;
|
||||
|
||||
case DJI_MSP_STATUS:
|
||||
case DJI_MSP_STATUS_EX:
|
||||
{
|
||||
// DJI OSD relies on a statically defined bit order and doesn't use MSP_BOXIDS
|
||||
// to get actual BOX order. We need a special packBoxModeFlags()
|
||||
boxBitmask_t flightModeBitmask;
|
||||
djiPackBoxModeBitmask(&flightModeBitmask);
|
||||
|
||||
sbufWriteU16(dst, (uint16_t)cycleTime);
|
||||
sbufWriteU16(dst, 0);
|
||||
sbufWriteU16(dst, packSensorStatus());
|
||||
sbufWriteData(dst, &flightModeBitmask, 4); // unconditional part of flags, first 32 bits
|
||||
sbufWriteU8(dst, getConfigProfile());
|
||||
|
||||
sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100));
|
||||
if (cmd->cmd == MSP_STATUS_EX) {
|
||||
sbufWriteU8(dst, 3); // PID_PROFILE_COUNT
|
||||
sbufWriteU8(dst, 1); // getCurrentControlRateProfileIndex()
|
||||
} else {
|
||||
sbufWriteU16(dst, cycleTime); // gyro cycle time
|
||||
}
|
||||
|
||||
// Cap BoxModeFlags to 32 bits
|
||||
// write flightModeFlags header. Lowest 4 bits contain number of bytes that follow
|
||||
sbufWriteU8(dst, 0);
|
||||
// sbufWriteData(dst, ((uint8_t*)&flightModeBitmask) + 4, byteCount);
|
||||
|
||||
// Write arming disable flags
|
||||
sbufWriteU8(dst, DJI_ARMING_DISABLE_FLAGS_COUNT);
|
||||
sbufWriteU32(dst, djiPackArmingDisabledFlags());
|
||||
|
||||
// Extra flags
|
||||
sbufWriteU8(dst, 0);
|
||||
}
|
||||
break;
|
||||
|
||||
case DJI_MSP_RC:
|
||||
// Only send sticks (first 4 channels)
|
||||
for (int i = 0; i < STICK_CHANNEL_COUNT; i++) {
|
||||
sbufWriteU16(dst, rxGetRawChannelValue(i));
|
||||
}
|
||||
break;
|
||||
|
||||
case DJI_MSP_RAW_GPS:
|
||||
sbufWriteU8(dst, gpsSol.fixType);
|
||||
sbufWriteU8(dst, gpsSol.numSat);
|
||||
sbufWriteU32(dst, gpsSol.llh.lat);
|
||||
sbufWriteU32(dst, gpsSol.llh.lon);
|
||||
sbufWriteU16(dst, gpsSol.llh.alt / 100);
|
||||
sbufWriteU16(dst, gpsSol.groundSpeed);
|
||||
sbufWriteU16(dst, gpsSol.groundCourse);
|
||||
break;
|
||||
|
||||
case DJI_MSP_COMP_GPS:
|
||||
sbufWriteU16(dst, GPS_distanceToHome);
|
||||
sbufWriteU16(dst, GPS_directionToHome);
|
||||
sbufWriteU8(dst, gpsSol.flags.gpsHeartbeat ? 1 : 0);
|
||||
break;
|
||||
|
||||
case DJI_MSP_ATTITUDE:
|
||||
sbufWriteU16(dst, attitude.values.roll);
|
||||
sbufWriteU16(dst, attitude.values.pitch);
|
||||
sbufWriteU16(dst, DECIDEGREES_TO_DEGREES(attitude.values.yaw));
|
||||
break;
|
||||
|
||||
case DJI_MSP_ALTITUDE:
|
||||
sbufWriteU32(dst, lrintf(getEstimatedActualPosition(Z)));
|
||||
sbufWriteU16(dst, lrintf(getEstimatedActualVelocity(Z)));
|
||||
break;
|
||||
|
||||
case DJI_MSP_ANALOG:
|
||||
sbufWriteU8(dst, constrain(getBatteryVoltage() / 10, 0, 255));
|
||||
sbufWriteU16(dst, constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery
|
||||
sbufWriteU16(dst, getRSSI());
|
||||
sbufWriteU16(dst, constrain(getAmperage(), -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A
|
||||
sbufWriteU16(dst, getBatteryVoltage());
|
||||
break;
|
||||
|
||||
case DJI_MSP_PID:
|
||||
for (unsigned i = 0; i < ARRAYLEN(djiPidIndexMap); i++) {
|
||||
sbufWriteU8(dst, pidBank()->pid[djiPidIndexMap[i]].P);
|
||||
sbufWriteU8(dst, pidBank()->pid[djiPidIndexMap[i]].I);
|
||||
sbufWriteU8(dst, pidBank()->pid[djiPidIndexMap[i]].D);
|
||||
}
|
||||
break;
|
||||
|
||||
case DJI_MSP_BATTERY_STATE:
|
||||
// Battery characteristics
|
||||
sbufWriteU8(dst, constrain(getBatteryCellCount(), 0, 255));
|
||||
sbufWriteU16(dst, currentBatteryProfile->capacity.value);
|
||||
|
||||
// Battery state
|
||||
sbufWriteU8(dst, constrain(getBatteryVoltage() / 10, 0, 255)); // in 0.1V steps
|
||||
sbufWriteU16(dst, constrain(getMAhDrawn(), 0, 0xFFFF));
|
||||
sbufWriteU16(dst, constrain(getAmperage(), -0x8000, 0x7FFF));
|
||||
|
||||
// Battery alerts - used values match Betaflight's/DJI's
|
||||
sbufWriteU8(dst, getBatteryState());
|
||||
|
||||
// Additional battery voltage field (in 0.01V steps)
|
||||
sbufWriteU16(dst, getBatteryVoltage());
|
||||
break;
|
||||
|
||||
case DJI_MSP_RTC:
|
||||
{
|
||||
dateTime_t datetime;
|
||||
|
||||
// We don't care about validity here - dt will be always set to a sane value
|
||||
// rtcGetDateTime() will call rtcGetDefaultDateTime() internally
|
||||
rtcGetDateTime(&datetime);
|
||||
|
||||
sbufWriteU16(dst, datetime.year);
|
||||
sbufWriteU8(dst, datetime.month);
|
||||
sbufWriteU8(dst, datetime.day);
|
||||
sbufWriteU8(dst, datetime.hours);
|
||||
sbufWriteU8(dst, datetime.minutes);
|
||||
sbufWriteU8(dst, datetime.seconds);
|
||||
sbufWriteU16(dst, datetime.millis);
|
||||
}
|
||||
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);
|
||||
}
|
||||
}
|
||||
else {
|
||||
reply->result = MSP_RESULT_ERROR;
|
||||
}
|
||||
break;
|
||||
#endif
|
||||
|
||||
case DJI_MSP_OSD_CONFIG:
|
||||
#if defined(USE_OSD)
|
||||
// This involved some serious magic, better contain in a separate function for readability
|
||||
djiSerializeOSDConfigReply(dst);
|
||||
#else
|
||||
sbufWriteU8(dst, 0);
|
||||
#endif
|
||||
break;
|
||||
|
||||
case DJI_MSP_FILTER_CONFIG:
|
||||
sbufWriteU8(dst, gyroConfig()->gyro_soft_lpf_hz); // BF: gyroConfig()->gyro_lowpass_hz
|
||||
sbufWriteU16(dst, pidProfile()->dterm_lpf_hz); // BF: currentPidProfile->dterm_lowpass_hz
|
||||
sbufWriteU16(dst, pidProfile()->yaw_lpf_hz); // BF: currentPidProfile->yaw_lowpass_hz
|
||||
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_1); // BF: gyroConfig()->gyro_soft_notch_hz_1
|
||||
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_1); // BF: gyroConfig()->gyro_soft_notch_cutoff_1
|
||||
sbufWriteU16(dst, pidProfile()->dterm_soft_notch_hz); // BF: currentPidProfile->dterm_notch_hz
|
||||
sbufWriteU16(dst, pidProfile()->dterm_soft_notch_cutoff); // BF: currentPidProfile->dterm_notch_cutoff
|
||||
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_2); // BF: gyroConfig()->gyro_soft_notch_hz_2
|
||||
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_2); // BF: gyroConfig()->gyro_soft_notch_cutoff_2
|
||||
sbufWriteU8(dst, 0); // BF: currentPidProfile->dterm_filter_type
|
||||
sbufWriteU8(dst, gyroConfig()->gyro_lpf); // BF: gyroConfig()->gyro_hardware_lpf);
|
||||
sbufWriteU8(dst, 0); // BF: DEPRECATED: gyro_32khz_hardware_lpf
|
||||
sbufWriteU16(dst, gyroConfig()->gyro_soft_lpf_hz); // BF: gyroConfig()->gyro_lowpass_hz);
|
||||
sbufWriteU16(dst, gyroConfig()->gyro_stage2_lowpass_hz); // BF: gyroConfig()->gyro_lowpass2_hz);
|
||||
sbufWriteU8(dst, 0); // BF: gyroConfig()->gyro_lowpass_type);
|
||||
sbufWriteU8(dst, 0); // BF: gyroConfig()->gyro_lowpass2_type);
|
||||
sbufWriteU16(dst, 0); // BF: currentPidProfile->dterm_lowpass2_hz);
|
||||
sbufWriteU8(dst, 0); // BF: currentPidProfile->dterm_filter2_type);
|
||||
break;
|
||||
|
||||
case DJI_MSP_RC_TUNING:
|
||||
sbufWriteU8(dst, 100); // INAV doesn't use rcRate
|
||||
sbufWriteU8(dst, currentControlRateProfile->stabilized.rcExpo8);
|
||||
for (int i = 0 ; i < 3; i++) {
|
||||
// R,P,Y rates see flight_dynamics_index_t
|
||||
sbufWriteU8(dst, currentControlRateProfile->stabilized.rates[i]);
|
||||
}
|
||||
sbufWriteU8(dst, currentControlRateProfile->throttle.dynPID);
|
||||
sbufWriteU8(dst, currentControlRateProfile->throttle.rcMid8);
|
||||
sbufWriteU8(dst, currentControlRateProfile->throttle.rcExpo8);
|
||||
sbufWriteU16(dst, currentControlRateProfile->throttle.pa_breakpoint);
|
||||
sbufWriteU8(dst, currentControlRateProfile->stabilized.rcYawExpo8);
|
||||
sbufWriteU8(dst, 100); // INAV doesn't use rcRate
|
||||
sbufWriteU8(dst, 100); // INAV doesn't use rcRate
|
||||
sbufWriteU8(dst, currentControlRateProfile->stabilized.rcExpo8);
|
||||
|
||||
// added in 1.41
|
||||
sbufWriteU8(dst, 0);
|
||||
sbufWriteU8(dst, currentControlRateProfile->throttle.dynPID);
|
||||
break;
|
||||
|
||||
case DJI_MSP_SET_PID:
|
||||
// Check if we have enough data for all PID coefficients
|
||||
if ((unsigned)sbufBytesRemaining(src) >= ARRAYLEN(djiPidIndexMap) * 3) {
|
||||
for (unsigned i = 0; i < ARRAYLEN(djiPidIndexMap); i++) {
|
||||
pidBankMutable()->pid[djiPidIndexMap[i]].P = sbufReadU8(src);
|
||||
pidBankMutable()->pid[djiPidIndexMap[i]].I = sbufReadU8(src);
|
||||
pidBankMutable()->pid[djiPidIndexMap[i]].D = sbufReadU8(src);
|
||||
}
|
||||
schedulePidGainsUpdate();
|
||||
#if defined(USE_NAV)
|
||||
// This is currently unnecessary, DJI HD doesn't set any NAV PIDs
|
||||
//navigationUsePIDs();
|
||||
#endif
|
||||
}
|
||||
else {
|
||||
reply->result = MSP_RESULT_ERROR;
|
||||
}
|
||||
break;
|
||||
|
||||
case DJI_MSP_PID_ADVANCED:
|
||||
// TODO
|
||||
reply->result = MSP_RESULT_ERROR;
|
||||
break;
|
||||
|
||||
case DJI_MSP_SET_FILTER_CONFIG:
|
||||
case DJI_MSP_SET_PID_ADVANCED:
|
||||
case DJI_MSP_SET_RC_TUNING:
|
||||
// TODO
|
||||
reply->result = MSP_RESULT_ERROR;
|
||||
break;
|
||||
|
||||
default:
|
||||
// debug[1]++;
|
||||
// debug[2] = cmd->cmd;
|
||||
reply->result = MSP_RESULT_ERROR;
|
||||
break;
|
||||
}
|
||||
|
||||
// Process DONT_REPLY flag
|
||||
if (cmd->flags & MSP_FLAG_DONT_REPLY) {
|
||||
reply->result = MSP_RESULT_NO_REPLY;
|
||||
}
|
||||
|
||||
return reply->result;
|
||||
}
|
||||
|
||||
void djiOsdSerialProcess(void)
|
||||
{
|
||||
// Check if DJI OSD is configured
|
||||
if (!djiMspPort.port) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Piggyback on existing MSP protocol stack, but pass our special command processing function
|
||||
mspSerialProcessOnePort(&djiMspPort, MSP_SKIP_NON_MSP_DATA, djiProcessMspCommand);
|
||||
}
|
||||
|
||||
#endif
|
||||
66
src/main/io/osd_dji_hd.h
Normal file
66
src/main/io/osd_dji_hd.h
Normal file
|
|
@ -0,0 +1,66 @@
|
|||
/*
|
||||
* This file is part of INAV.
|
||||
*
|
||||
* This Source Code Form is subject to the terms of the Mozilla Public
|
||||
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
|
||||
* You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
*
|
||||
* Alternatively, the contents of this file may be used under the terms
|
||||
* of the GNU General Public License Version 3, as described below:
|
||||
*
|
||||
* This file is free software: you may copy, redistribute and/or modify
|
||||
* it under the terms of the GNU General Public License as published by the
|
||||
* Free Software Foundation, either version 3 of the License, or (at your
|
||||
* option) any later version.
|
||||
*
|
||||
* This file is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
|
||||
* Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see http://www.gnu.org/licenses/.
|
||||
*
|
||||
* @author Konstantin Sharlaimov (ksharlaimov@inavflight.com)
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "msp/msp.h"
|
||||
#include "msp/msp_serial.h"
|
||||
|
||||
#if defined(USE_DJI_HD_OSD)
|
||||
|
||||
#define DJI_API_VERSION_MAJOR 1
|
||||
#define DJI_API_VERSION_MINOR 42
|
||||
|
||||
#define DJI_MSP_API_VERSION 1 // INAV: Implemented | DSI: ??? |
|
||||
#define DJI_MSP_FC_VARIANT 2 // INAV: Implemented | DSI: ??? |
|
||||
#define DJI_MSP_FC_VERSION 3 // INAV: Implemented | DSI: ??? |
|
||||
#define DJI_MSP_NAME 10 // INAV: Implemented | DSI: Implemented | For OSD 'Craft Name'
|
||||
#define DJI_MSP_OSD_CONFIG 84 // INAV: Implemented | DSI: Implemented | OSD item count + positions
|
||||
#define DJI_MSP_FILTER_CONFIG 92 // INAV: Not implemented | DSI: Implemented |
|
||||
#define DJI_MSP_PID_ADVANCED 94 // INAV: Not implemented | DSI: Implemented |
|
||||
#define DJI_MSP_STATUS 101 // INAV: Implemented | DSI: Implemented | For OSD ‘armingTime’, Flight controller arming status
|
||||
#define DJI_MSP_RC 105 // INAV: Implemented | DSI: Implemented |
|
||||
#define DJI_MSP_RAW_GPS 106 // INAV: Implemented | DSI: Implemented | For OSD ‘GPS Sats’ + coordinates
|
||||
#define DJI_MSP_COMP_GPS 107 // INAV: Implemented | DSI: Not implemented | GPS direction to home & distance to home
|
||||
#define DJI_MSP_ATTITUDE 108 // INAV: Implemented | DSI: Implemented | For OSD ‘Angle: roll & pitch’
|
||||
#define DJI_MSP_ALTITUDE 109 // INAV: Implemented | DSI: Implemented | For OSD ‘Numerical Vario’
|
||||
#define DJI_MSP_ANALOG 110 // INAV: Implemented | DSI: Implemented | For OSD ‘RSSI Value’, For OSD ‘Battery voltage’ etc
|
||||
#define DJI_MSP_RC_TUNING 111 // INAV: Not implemented | DSI: Implemented |
|
||||
#define DJI_MSP_PID 112 // INAV: Implemented | DSI: Implemented | For OSD ‘PID roll, yaw, pitch'
|
||||
#define DJI_MSP_BATTERY_STATE 130 // INAV: Implemented | DSI: Implemented | For OSD ‘Battery current mAh drawn’ etc
|
||||
#define DJI_MSP_ESC_SENSOR_DATA 134 // INAV: Implemented | DSI: Implemented | For OSD ‘ESC temperature’
|
||||
#define DJI_MSP_STATUS_EX 150 // INAV: Implemented | DSI: Implemented | For OSD ‘Fly mode', For OSD ‘Disarmed’
|
||||
#define DJI_MSP_RTC 247 // INAV: Implemented | DSI: Implemented | For OSD ‘RTC date time’
|
||||
|
||||
#define DJI_MSP_SET_FILTER_CONFIG 93
|
||||
#define DJI_MSP_SET_PID_ADVANCED 95
|
||||
#define DJI_MSP_SET_PID 202
|
||||
#define DJI_MSP_SET_RC_TUNING 204
|
||||
|
||||
void djiOsdSerialInit(void);
|
||||
void djiOsdSerialProcess(void);
|
||||
|
||||
#endif
|
||||
|
|
@ -52,6 +52,7 @@ typedef enum {
|
|||
FUNCTION_ESCSERIAL = (1 << 18), // 262144: this is used for both SERIALSHOT and ESC_SENSOR telemetry
|
||||
FUNCTION_TELEMETRY_SIM = (1 << 19), // 524288
|
||||
FUNCTION_FRSKY_OSD = (1 << 20), // 1048576
|
||||
FUNCTION_DJI_HD_OSD = (1 << 21), // 2097152
|
||||
} serialPortFunction_e;
|
||||
|
||||
typedef enum {
|
||||
|
|
|
|||
|
|
@ -321,7 +321,7 @@
|
|||
#define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values
|
||||
#define MSP_SERVO_MIX_RULES 241 //out message Returns servo mixer configuration
|
||||
#define MSP_SET_SERVO_MIX_RULE 242 //in message Sets servo mixer configuration
|
||||
#define MSP_SET_4WAY_IF 245 //in message Sets 4way interface
|
||||
#define MSP_SET_PASSTHROUGH 245 //in message Sets up passthrough to different peripherals (4way interface, uart, etc...)
|
||||
#define MSP_RTC 246 //out message Gets the RTC clock (returns: secs(i32) millis(u16) - (0,0) if time is not known)
|
||||
#define MSP_SET_RTC 247 //in message Sets the RTC clock (args: secs(i32) millis(u16))
|
||||
|
||||
|
|
|
|||
|
|
@ -40,7 +40,7 @@
|
|||
static mspPort_t mspPorts[MAX_MSP_PORT_COUNT];
|
||||
|
||||
|
||||
static void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort)
|
||||
void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort)
|
||||
{
|
||||
memset(mspPortToReset, 0, sizeof(mspPort_t));
|
||||
|
||||
|
|
@ -452,19 +452,8 @@ static void mspProcessPendingRequest(mspPort_t * mspPort)
|
|||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Process MSP commands from serial ports configured as MSP ports.
|
||||
*
|
||||
* Called periodically by the scheduler.
|
||||
*/
|
||||
void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn)
|
||||
void mspSerialProcessOnePort(mspPort_t * const mspPort, mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn)
|
||||
{
|
||||
for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
|
||||
mspPort_t * const mspPort = &mspPorts[portIndex];
|
||||
if (!mspPort->port) {
|
||||
continue;
|
||||
}
|
||||
|
||||
mspPostProcessFnPtr mspPostProcessFn = NULL;
|
||||
|
||||
if (serialRxBytesWaiting(mspPort->port)) {
|
||||
|
|
@ -496,6 +485,20 @@ void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessComm
|
|||
mspProcessPendingRequest(mspPort);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Process MSP commands from serial ports configured as MSP ports.
|
||||
*
|
||||
* Called periodically by the scheduler.
|
||||
*/
|
||||
void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn)
|
||||
{
|
||||
for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
|
||||
mspPort_t * const mspPort = &mspPorts[portIndex];
|
||||
if (mspPort->port) {
|
||||
mspSerialProcessOnePort(mspPort, evaluateNonMspData, mspProcessCommandFn);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void mspSerialInit(void)
|
||||
|
|
|
|||
|
|
@ -18,6 +18,7 @@
|
|||
#pragma once
|
||||
|
||||
#include "drivers/time.h"
|
||||
#include "io/serial.h"
|
||||
#include "msp/msp.h"
|
||||
|
||||
// Each MSP port requires state and a receive buffer, revisit this default if someone needs more than 3 MSP ports.
|
||||
|
|
@ -99,7 +100,9 @@ typedef struct mspPort_s {
|
|||
|
||||
|
||||
void mspSerialInit(void);
|
||||
void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort);
|
||||
void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn);
|
||||
void mspSerialProcessOnePort(mspPort_t * const mspPort, mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn);
|
||||
void mspSerialAllocatePorts(void);
|
||||
void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort);
|
||||
int mspSerialPushPort(uint16_t cmd, const uint8_t *data, int datalen, mspPort_t *mspPort, mspVersion_e version);
|
||||
|
|
|
|||
|
|
@ -843,7 +843,7 @@ navigationFSMStateFlags_t navGetCurrentStateFlags(void)
|
|||
static bool navTerrainFollowingRequested(void)
|
||||
{
|
||||
// Terrain following not supported on FIXED WING aircraft yet
|
||||
return !STATE(FIXED_WING) && IS_RC_MODE_ACTIVE(BOXSURFACE);
|
||||
return !STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXSURFACE);
|
||||
}
|
||||
|
||||
/*************************************************************************************************/
|
||||
|
|
@ -941,7 +941,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE(na
|
|||
{
|
||||
const navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
|
||||
|
||||
if (!STATE(FIXED_WING)) { return NAV_FSM_EVENT_ERROR; } // Only on FW for now
|
||||
if (!STATE(FIXED_WING_LEGACY)) { return NAV_FSM_EVENT_ERROR; } // Only on FW for now
|
||||
|
||||
DEBUG_SET(DEBUG_CRUISE, 0, 1);
|
||||
if (checkForPositionSensorTimeout()) { return NAV_FSM_EVENT_SWITCH_TO_IDLE; } // Switch to IDLE if we do not have an healty position. Try the next iteration.
|
||||
|
|
@ -1020,7 +1020,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_ADJUSTING(nav
|
|||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_INITIALIZE(navigationFSMState_t previousState)
|
||||
{
|
||||
if (!STATE(FIXED_WING)) { return NAV_FSM_EVENT_ERROR; } // only on FW for now
|
||||
if (!STATE(FIXED_WING_LEGACY)) { return NAV_FSM_EVENT_ERROR; } // only on FW for now
|
||||
|
||||
navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(previousState);
|
||||
|
||||
|
|
@ -1051,7 +1051,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
|
|||
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
|
||||
}
|
||||
|
||||
if (STATE(FIXED_WING) && (posControl.homeDistance < navConfig()->general.min_rth_distance) && !posControl.flags.forcedRTHActivated) {
|
||||
if (STATE(FIXED_WING_LEGACY) && (posControl.homeDistance < navConfig()->general.min_rth_distance) && !posControl.flags.forcedRTHActivated) {
|
||||
// Prevent RTH from activating on airplanes if too close to home unless it's a failsafe RTH
|
||||
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
|
||||
}
|
||||
|
|
@ -1080,7 +1080,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
|
|||
else {
|
||||
fpVector3_t targetHoldPos;
|
||||
|
||||
if (STATE(FIXED_WING)) {
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
// Airplane - climbout before turning around
|
||||
calculateFarAwayTarget(&targetHoldPos, posControl.actualState.yaw, 100000.0f); // 1km away
|
||||
} else {
|
||||
|
|
@ -1117,14 +1117,14 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
|
|||
|
||||
// If we have valid pos sensor OR we are configured to ignore GPS loss
|
||||
if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout() || navConfig()->general.flags.rth_climb_ignore_emerg) {
|
||||
const uint8_t rthClimbMarginPercent = STATE(FIXED_WING) ? FW_RTH_CLIMB_MARGIN_PERCENT : MR_RTH_CLIMB_MARGIN_PERCENT;
|
||||
const uint8_t rthClimbMarginPercent = STATE(FIXED_WING_LEGACY) ? FW_RTH_CLIMB_MARGIN_PERCENT : MR_RTH_CLIMB_MARGIN_PERCENT;
|
||||
const float rthAltitudeMargin = MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (rthClimbMarginPercent/100.0) * fabsf(posControl.rthState.rthInitialAltitude - posControl.rthState.homePosition.pos.z));
|
||||
|
||||
// If we reached desired initial RTH altitude or we don't want to climb first
|
||||
if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (!navConfig()->general.flags.rth_climb_first)) {
|
||||
|
||||
// Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance
|
||||
if (STATE(FIXED_WING)) {
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos);
|
||||
}
|
||||
|
||||
|
|
@ -1132,7 +1132,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
|
|||
posControl.rthState.rthInitialDistance = posControl.homeDistance;
|
||||
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL);
|
||||
|
||||
if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING)) {
|
||||
if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING_LEGACY)) {
|
||||
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
|
||||
}
|
||||
else {
|
||||
|
|
@ -1146,12 +1146,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
|
|||
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL);
|
||||
|
||||
/* For multi-rotors execute sanity check during initial ascent as well */
|
||||
if (!STATE(FIXED_WING) && !validateRTHSanityChecker()) {
|
||||
if (!STATE(FIXED_WING_LEGACY) && !validateRTHSanityChecker()) {
|
||||
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
|
||||
}
|
||||
|
||||
// Climb to safe altitude and turn to correct direction
|
||||
if (STATE(FIXED_WING)) {
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM;
|
||||
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
|
||||
} else {
|
||||
|
|
@ -1224,7 +1224,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
|
|||
if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout()) {
|
||||
|
||||
// Wait until target heading is reached (with 15 deg margin for error)
|
||||
if (STATE(FIXED_WING)) {
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
resetLandingDetector();
|
||||
updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
|
||||
return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME;
|
||||
|
|
@ -1401,8 +1401,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
|
|||
fpVector3_t tmpWaypoint;
|
||||
tmpWaypoint.x = posControl.activeWaypoint.pos.x;
|
||||
tmpWaypoint.y = posControl.activeWaypoint.pos.y;
|
||||
tmpWaypoint.z = scaleRange(constrainf(posControl.wpDistance, posControl.wpInitialDistance / 10, posControl.wpInitialDistance),
|
||||
posControl.wpInitialDistance, posControl.wpInitialDistance / 10,
|
||||
tmpWaypoint.z = scaleRangef(constrainf(posControl.wpDistance, posControl.wpInitialDistance / 10.0f, posControl.wpInitialDistance),
|
||||
posControl.wpInitialDistance, posControl.wpInitialDistance / 10.0f,
|
||||
posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z);
|
||||
setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
|
||||
return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
|
||||
|
|
@ -1675,7 +1675,7 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode)
|
|||
|
||||
case RTH_HOME_ENROUTE_PROPORTIONAL:
|
||||
{
|
||||
float rthTotalDistanceToTravel = posControl.rthState.rthInitialDistance - (STATE(FIXED_WING) ? navConfig()->fw.loiter_radius : 0);
|
||||
float rthTotalDistanceToTravel = posControl.rthState.rthInitialDistance - (STATE(FIXED_WING_LEGACY) ? navConfig()->fw.loiter_radius : 0);
|
||||
if (rthTotalDistanceToTravel >= 100) {
|
||||
float ratioNotTravelled = constrainf(posControl.homeDistance / rthTotalDistanceToTravel, 0.0f, 1.0f);
|
||||
posControl.rthState.homeTmpWaypoint.z = (posControl.rthState.rthInitialAltitude * ratioNotTravelled) + (posControl.rthState.rthFinalAltitude * (1.0f - ratioNotTravelled));
|
||||
|
|
@ -2059,7 +2059,7 @@ static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWayp
|
|||
// We consider waypoint reached if within specified radius
|
||||
posControl.wpDistance = calculateDistanceToDestination(pos);
|
||||
|
||||
if (STATE(FIXED_WING) && isWaypointHome) {
|
||||
if (STATE(FIXED_WING_LEGACY) && isWaypointHome) {
|
||||
// Airplane will do a circular loiter over home and might never approach it closer than waypoint_radius - need extra check
|
||||
return (posControl.wpDistance <= navConfig()->general.waypoint_radius)
|
||||
|| (posControl.wpDistance <= (navConfig()->fw.loiter_radius * 1.10f)); // 10% margin of desired circular loiter radius
|
||||
|
|
@ -2318,7 +2318,7 @@ uint32_t getTotalTravelDistance(void)
|
|||
*-----------------------------------------------------------*/
|
||||
void calculateInitialHoldPosition(fpVector3_t * pos)
|
||||
{
|
||||
if (STATE(FIXED_WING)) { // FIXED_WING
|
||||
if (STATE(FIXED_WING_LEGACY)) { // FIXED_WING_LEGACY
|
||||
calculateFixedWingInitialHoldPosition(pos);
|
||||
}
|
||||
else {
|
||||
|
|
@ -2375,7 +2375,7 @@ void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distanc
|
|||
*-----------------------------------------------------------*/
|
||||
void resetLandingDetector(void)
|
||||
{
|
||||
if (STATE(FIXED_WING)) { // FIXED_WING
|
||||
if (STATE(FIXED_WING_LEGACY)) { // FIXED_WING_LEGACY
|
||||
resetFixedWingLandingDetector();
|
||||
}
|
||||
else {
|
||||
|
|
@ -2387,7 +2387,7 @@ bool isLandingDetected(void)
|
|||
{
|
||||
bool landingDetected;
|
||||
|
||||
if (STATE(FIXED_WING)) { // FIXED_WING
|
||||
if (STATE(FIXED_WING_LEGACY)) { // FIXED_WING_LEGACY
|
||||
landingDetected = isFixedWingLandingDetected();
|
||||
}
|
||||
else {
|
||||
|
|
@ -2413,7 +2413,7 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
|
|||
posControl.desiredState.pos.z = altitudeToUse;
|
||||
}
|
||||
else {
|
||||
if (STATE(FIXED_WING)) {
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
// Fixed wing climb rate controller is open-loop. We simply move the known altitude target
|
||||
float timeDelta = US2S(currentTimeUs - lastUpdateTimeUs);
|
||||
|
||||
|
|
@ -2436,7 +2436,7 @@ static void resetAltitudeController(bool useTerrainFollowing)
|
|||
// Set terrain following flag
|
||||
posControl.flags.isTerrainFollowEnabled = useTerrainFollowing;
|
||||
|
||||
if (STATE(FIXED_WING)) {
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
resetFixedWingAltitudeController();
|
||||
}
|
||||
else {
|
||||
|
|
@ -2446,7 +2446,7 @@ static void resetAltitudeController(bool useTerrainFollowing)
|
|||
|
||||
static void setupAltitudeController(void)
|
||||
{
|
||||
if (STATE(FIXED_WING)) {
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
setupFixedWingAltitudeController();
|
||||
}
|
||||
else {
|
||||
|
|
@ -2456,7 +2456,7 @@ static void setupAltitudeController(void)
|
|||
|
||||
static bool adjustAltitudeFromRCInput(void)
|
||||
{
|
||||
if (STATE(FIXED_WING)) {
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
return adjustFixedWingAltitudeFromRCInput();
|
||||
}
|
||||
else {
|
||||
|
|
@ -2469,7 +2469,7 @@ static bool adjustAltitudeFromRCInput(void)
|
|||
*-----------------------------------------------------------*/
|
||||
static void resetHeadingController(void)
|
||||
{
|
||||
if (STATE(FIXED_WING)) {
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
resetFixedWingHeadingController();
|
||||
}
|
||||
else {
|
||||
|
|
@ -2479,7 +2479,7 @@ static void resetHeadingController(void)
|
|||
|
||||
static bool adjustHeadingFromRCInput(void)
|
||||
{
|
||||
if (STATE(FIXED_WING)) {
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
return adjustFixedWingHeadingFromRCInput();
|
||||
}
|
||||
else {
|
||||
|
|
@ -2492,7 +2492,7 @@ static bool adjustHeadingFromRCInput(void)
|
|||
*-----------------------------------------------------------*/
|
||||
static void resetPositionController(void)
|
||||
{
|
||||
if (STATE(FIXED_WING)) {
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
resetFixedWingPositionController();
|
||||
}
|
||||
else {
|
||||
|
|
@ -2505,7 +2505,7 @@ static bool adjustPositionFromRCInput(void)
|
|||
{
|
||||
bool retValue;
|
||||
|
||||
if (STATE(FIXED_WING)) {
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
retValue = adjustFixedWingPositionFromRCInput();
|
||||
}
|
||||
else {
|
||||
|
|
@ -2773,7 +2773,7 @@ static void processNavigationRCAdjustments(void)
|
|||
posControl.flags.isAdjustingPosition = adjustPositionFromRCInput();
|
||||
}
|
||||
else {
|
||||
if (!STATE(FIXED_WING)) {
|
||||
if (!STATE(FIXED_WING_LEGACY)) {
|
||||
resetMulticopterBrakingMode();
|
||||
}
|
||||
}
|
||||
|
|
@ -2825,7 +2825,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
|
|||
|
||||
/* Process controllers */
|
||||
navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
|
||||
if (STATE(FIXED_WING)) {
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
applyFixedWingNavigationController(navStateFlags, currentTimeUs);
|
||||
}
|
||||
else {
|
||||
|
|
@ -2898,7 +2898,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
|
|||
bool canActivateNavigation = canActivateNavigationModes();
|
||||
|
||||
// LAUNCH mode has priority over any other NAV mode
|
||||
if (STATE(FIXED_WING)) {
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
if (isNavLaunchEnabled()) { // FIXME: Only available for fixed wing aircrafts now
|
||||
if (canActivateLaunchMode) {
|
||||
canActivateLaunchMode = false;
|
||||
|
|
@ -2981,7 +2981,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
|
|||
*-----------------------------------------------------------*/
|
||||
bool navigationRequiresThrottleTiltCompensation(void)
|
||||
{
|
||||
return !STATE(FIXED_WING) && (navGetStateFlags(posControl.navState) & NAV_REQUIRE_THRTILT);
|
||||
return !STATE(FIXED_WING_LEGACY) && (navGetStateFlags(posControl.navState) & NAV_REQUIRE_THRTILT);
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
|
|
@ -2990,7 +2990,7 @@ bool navigationRequiresThrottleTiltCompensation(void)
|
|||
bool navigationRequiresAngleMode(void)
|
||||
{
|
||||
const navigationFSMStateFlags_t currentState = navGetStateFlags(posControl.navState);
|
||||
return (currentState & NAV_REQUIRE_ANGLE) || ((currentState & NAV_REQUIRE_ANGLE_FW) && STATE(FIXED_WING));
|
||||
return (currentState & NAV_REQUIRE_ANGLE) || ((currentState & NAV_REQUIRE_ANGLE_FW) && STATE(FIXED_WING_LEGACY));
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
|
|
@ -2999,7 +2999,7 @@ bool navigationRequiresAngleMode(void)
|
|||
bool navigationRequiresTurnAssistance(void)
|
||||
{
|
||||
const navigationFSMStateFlags_t currentState = navGetStateFlags(posControl.navState);
|
||||
if (STATE(FIXED_WING)) {
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
// For airplanes turn assistant is always required when controlling position
|
||||
return (currentState & (NAV_CTL_POS | NAV_CTL_ALT));
|
||||
}
|
||||
|
|
@ -3014,7 +3014,7 @@ bool navigationRequiresTurnAssistance(void)
|
|||
int8_t navigationGetHeadingControlState(void)
|
||||
{
|
||||
// For airplanes report as manual heading control
|
||||
if (STATE(FIXED_WING)) {
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
return NAV_HEADING_CONTROL_MANUAL;
|
||||
}
|
||||
|
||||
|
|
@ -3039,7 +3039,7 @@ bool navigationTerrainFollowingEnabled(void)
|
|||
|
||||
navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
|
||||
{
|
||||
const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING) && IS_RC_MODE_ACTIVE(BOXNAVCRUISE));
|
||||
const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVCRUISE));
|
||||
const bool navLaunchComboModesEnabled = isNavLaunchEnabled() && (IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE));
|
||||
|
||||
if (usedBypass) {
|
||||
|
|
|
|||
|
|
@ -533,7 +533,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
|
|||
isAutoThrottleManuallyIncreased = false;
|
||||
}
|
||||
|
||||
rcCommand[THROTTLE] = constrain(correctedThrottleValue, motorConfig()->minthrottle, motorConfig()->maxthrottle);
|
||||
rcCommand[THROTTLE] = constrain(correctedThrottleValue, getThrottleIdleValue(), motorConfig()->maxthrottle);
|
||||
}
|
||||
|
||||
#ifdef NAV_FIXED_WING_LANDING
|
||||
|
|
@ -546,7 +546,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
|
|||
((posControl.flags.estAglStatus == EST_TRUSTED) && (posControl.actualState.agl.pos.z <= navConfig()->general.land_slowdown_minalt)) ) {
|
||||
|
||||
// Set motor to min. throttle and stop it when MOTOR_STOP feature is enabled
|
||||
rcCommand[THROTTLE] = motorConfig()->minthrottle;
|
||||
rcCommand[THROTTLE] = getThrottleIdleValue();
|
||||
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
|
||||
|
||||
// Stabilize ROLL axis on 0 degrees banking regardless of loiter radius and position
|
||||
|
|
|
|||
|
|
@ -138,10 +138,10 @@ static void applyFixedWingLaunchIdleLogic(void)
|
|||
pidResetTPAFilter();
|
||||
|
||||
// Throttle control logic
|
||||
if (navConfig()->fw.launch_idle_throttle <= motorConfig()->minthrottle)
|
||||
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] = motorConfig()->minthrottle; // If MOTOR_STOP is disabled, motors will spin at minthrottle
|
||||
rcCommand[THROTTLE] = getThrottleIdleValue(); // If MOTOR_STOP is disabled, motors will spin at minthrottle
|
||||
}
|
||||
else
|
||||
{
|
||||
|
|
@ -155,7 +155,7 @@ static void applyFixedWingLaunchIdleLogic(void)
|
|||
const float timeSinceMotorStartMs = MIN(millis() - timeThrottleRaisedMs, LAUNCH_MOTOR_IDLE_SPINUP_TIME);
|
||||
rcCommand[THROTTLE] = scaleRangef(timeSinceMotorStartMs,
|
||||
0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME,
|
||||
motorConfig()->minthrottle, navConfig()->fw.launch_idle_throttle);
|
||||
getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -200,7 +200,7 @@ void applyFixedWingLaunchController(timeUs_t currentTimeUs)
|
|||
// 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(motorConfig()->minthrottle, navConfig()->fw.launch_idle_throttle);
|
||||
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);
|
||||
|
|
|
|||
|
|
@ -104,7 +104,7 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
|
|||
static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros)
|
||||
{
|
||||
// Calculate min and max throttle boundaries (to compensate for integral windup)
|
||||
const int16_t thrAdjustmentMin = (int16_t)motorConfig()->minthrottle - (int16_t)navConfig()->mc.hover_throttle;
|
||||
const int16_t thrAdjustmentMin = (int16_t)getThrottleIdleValue() - (int16_t)navConfig()->mc.hover_throttle;
|
||||
const int16_t thrAdjustmentMax = (int16_t)motorConfig()->maxthrottle - (int16_t)navConfig()->mc.hover_throttle;
|
||||
|
||||
posControl.rcAdjustment[THROTTLE] = navPidApply2(&posControl.pids.vel[Z], posControl.desiredState.vel.z, navGetCurrentActualPositionAndVelocity()->vel.z, US2S(deltaMicros), thrAdjustmentMin, thrAdjustmentMax, 0);
|
||||
|
|
@ -116,7 +116,7 @@ static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros)
|
|||
bool adjustMulticopterAltitudeFromRCInput(void)
|
||||
{
|
||||
if (posControl.flags.isTerrainFollowEnabled) {
|
||||
const float altTarget = scaleRangef(rcCommand[THROTTLE], motorConfig()->minthrottle, motorConfig()->maxthrottle, 0, navConfig()->general.max_terrain_follow_altitude);
|
||||
const float altTarget = scaleRangef(rcCommand[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle, 0, navConfig()->general.max_terrain_follow_altitude);
|
||||
|
||||
// In terrain follow mode we apply different logic for terrain control
|
||||
if (posControl.flags.estAglStatus == EST_TRUSTED && altTarget > 10.0f) {
|
||||
|
|
@ -144,7 +144,7 @@ bool adjustMulticopterAltitudeFromRCInput(void)
|
|||
}
|
||||
else {
|
||||
// Scaling from minthrottle to altHoldThrottleRCZero
|
||||
rcClimbRate = rcThrottleAdjustment * navConfig()->general.max_manual_climb_rate / (float)(altHoldThrottleRCZero - motorConfig()->minthrottle - rcControlsConfig()->alt_hold_deadband);
|
||||
rcClimbRate = rcThrottleAdjustment * navConfig()->general.max_manual_climb_rate / (float)(altHoldThrottleRCZero - getThrottleIdleValue() - rcControlsConfig()->alt_hold_deadband);
|
||||
}
|
||||
|
||||
updateClimbRateToAltitudeController(rcClimbRate, ROC_TO_ALT_NORMAL);
|
||||
|
|
@ -181,7 +181,7 @@ void setupMulticopterAltitudeController(void)
|
|||
|
||||
// Make sure we are able to satisfy the deadband
|
||||
altHoldThrottleRCZero = constrain(altHoldThrottleRCZero,
|
||||
motorConfig()->minthrottle + rcControlsConfig()->alt_hold_deadband + 10,
|
||||
getThrottleIdleValue() + rcControlsConfig()->alt_hold_deadband + 10,
|
||||
motorConfig()->maxthrottle - rcControlsConfig()->alt_hold_deadband - 10);
|
||||
|
||||
// Force AH controller to initialize althold integral for pending takeoff on reset
|
||||
|
|
@ -249,7 +249,7 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs)
|
|||
}
|
||||
|
||||
// Update throttle controller
|
||||
rcCommand[THROTTLE] = constrain((int16_t)navConfig()->mc.hover_throttle + posControl.rcAdjustment[THROTTLE], motorConfig()->minthrottle, motorConfig()->maxthrottle);
|
||||
rcCommand[THROTTLE] = constrain((int16_t)navConfig()->mc.hover_throttle + posControl.rcAdjustment[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle);
|
||||
|
||||
// Save processed throttle for future use
|
||||
rcCommandAdjustedThrottle = rcCommand[THROTTLE];
|
||||
|
|
@ -721,12 +721,12 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
|
|||
}
|
||||
|
||||
// Update throttle controller
|
||||
rcCommand[THROTTLE] = constrain((int16_t)navConfig()->mc.hover_throttle + posControl.rcAdjustment[THROTTLE], motorConfig()->minthrottle, motorConfig()->maxthrottle);
|
||||
rcCommand[THROTTLE] = constrain((int16_t)navConfig()->mc.hover_throttle + posControl.rcAdjustment[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle);
|
||||
}
|
||||
else {
|
||||
/* Sensors has gone haywire, attempt to land regardless */
|
||||
if (failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_DROP_IT) {
|
||||
rcCommand[THROTTLE] = motorConfig()->minthrottle;
|
||||
rcCommand[THROTTLE] = getThrottleIdleValue();
|
||||
}
|
||||
else {
|
||||
rcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle;
|
||||
|
|
|
|||
|
|
@ -581,7 +581,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
|
|||
|
||||
return true;
|
||||
}
|
||||
else if (STATE(FIXED_WING) && (ctx->newFlags & EST_GPS_Z_VALID)) {
|
||||
else if (STATE(FIXED_WING_LEGACY) && (ctx->newFlags & EST_GPS_Z_VALID)) {
|
||||
// If baro is not available - use GPS Z for correction on a plane
|
||||
// Reset current estimate to GPS altitude if estimate not valid
|
||||
if (!(ctx->newFlags & EST_Z_VALID)) {
|
||||
|
|
|
|||
|
|
@ -75,7 +75,7 @@ void mspOverrideInit(void)
|
|||
mspRcChannels[i].expiresAt = nowMs + MAX_INVALID_RX_PULSE_TIME;
|
||||
}
|
||||
|
||||
mspRcChannels[THROTTLE].raw = (feature(FEATURE_3D)) ? PWM_RANGE_MIDDLE : rxConfig()->rx_min_usec;
|
||||
mspRcChannels[THROTTLE].raw = (feature(FEATURE_REVERSIBLE_MOTORS)) ? PWM_RANGE_MIDDLE : rxConfig()->rx_min_usec;
|
||||
mspRcChannels[THROTTLE].data = mspRcChannels[THROTTLE].raw;
|
||||
|
||||
// Initialize ARM switch to OFF position when arming via switch is defined
|
||||
|
|
|
|||
|
|
@ -195,6 +195,9 @@ bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
|
|||
case SERIALRX_SBUS:
|
||||
enabled = sbusInit(rxConfig, rxRuntimeConfig);
|
||||
break;
|
||||
case SERIALRX_SBUS_FAST:
|
||||
enabled = sbusInitFast(rxConfig, rxRuntimeConfig);
|
||||
break;
|
||||
#endif
|
||||
#ifdef USE_SERIALRX_SUMD
|
||||
case SERIALRX_SUMD:
|
||||
|
|
@ -259,7 +262,7 @@ void rxInit(void)
|
|||
rcChannels[i].expiresAt = nowMs + MAX_INVALID_RX_PULSE_TIME;
|
||||
}
|
||||
|
||||
rcChannels[THROTTLE].raw = (feature(FEATURE_3D)) ? PWM_RANGE_MIDDLE : rxConfig()->rx_min_usec;
|
||||
rcChannels[THROTTLE].raw = (feature(FEATURE_REVERSIBLE_MOTORS)) ? PWM_RANGE_MIDDLE : rxConfig()->rx_min_usec;
|
||||
rcChannels[THROTTLE].data = rcChannels[THROTTLE].raw;
|
||||
|
||||
// Initialize ARM switch to OFF position when arming via switch is defined
|
||||
|
|
|
|||
|
|
@ -79,6 +79,7 @@ typedef enum {
|
|||
SERIALRX_JETIEXBUS = 8,
|
||||
SERIALRX_CRSF = 9,
|
||||
SERIALRX_FPORT = 10,
|
||||
SERIALRX_SBUS_FAST = 11,
|
||||
} rxSerialReceiverType_e;
|
||||
|
||||
#define MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT 16
|
||||
|
|
|
|||
|
|
@ -57,6 +57,7 @@
|
|||
#define SBUS_FRAME_BEGIN_BYTE 0x0F
|
||||
|
||||
#define SBUS_BAUDRATE 100000
|
||||
#define SBUS_BAUDRATE_FAST 200000
|
||||
|
||||
#if !defined(SBUS_PORT_OPTIONS)
|
||||
#define SBUS_PORT_OPTIONS (SERIAL_STOPBITS_2 | SERIAL_PARITY_EVEN)
|
||||
|
|
@ -191,7 +192,7 @@ static uint8_t sbusFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
|
|||
return retValue;
|
||||
}
|
||||
|
||||
bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
|
||||
static bool sbusInitEx(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, uint32_t sbusBaudRate)
|
||||
{
|
||||
static uint16_t sbusChannelData[SBUS_MAX_CHANNEL];
|
||||
static sbusFrameData_t sbusFrameData;
|
||||
|
|
@ -221,7 +222,7 @@ bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
|
|||
FUNCTION_RX_SERIAL,
|
||||
sbusDataReceive,
|
||||
&sbusFrameData,
|
||||
SBUS_BAUDRATE,
|
||||
sbusBaudRate,
|
||||
portShared ? MODE_RXTX : MODE_RX,
|
||||
SBUS_PORT_OPTIONS | (rxConfig->serialrx_inverted ? 0 : SERIAL_INVERTED) | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)
|
||||
);
|
||||
|
|
@ -234,4 +235,14 @@ bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
|
|||
|
||||
return sBusPort != NULL;
|
||||
}
|
||||
|
||||
bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
|
||||
{
|
||||
return sbusInitEx(rxConfig, rxRuntimeConfig, SBUS_BAUDRATE);
|
||||
}
|
||||
|
||||
bool sbusInitFast(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
|
||||
{
|
||||
return sbusInitEx(rxConfig, rxRuntimeConfig, SBUS_BAUDRATE_FAST);
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -20,3 +20,4 @@
|
|||
#define SBUS_DEFAULT_INTERFRAME_DELAY_US 3000 // According to FrSky interframe is 6.67ms, we go smaller just in case
|
||||
|
||||
bool sbusInit(const rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
|
||||
bool sbusInitFast(const rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
|
||||
|
|
|
|||
|
|
@ -78,11 +78,11 @@ STATIC_FASTRAM filter_t accFilter[XYZ_AXIS_COUNT];
|
|||
STATIC_FASTRAM filterApplyFnPtr accSoftLpfFilterApplyFn;
|
||||
STATIC_FASTRAM void *accSoftLpfFilter[XYZ_AXIS_COUNT];
|
||||
|
||||
STATIC_FASTRAM pt1Filter_t accVibeFloorFilter[XYZ_AXIS_COUNT];
|
||||
STATIC_FASTRAM pt1Filter_t accVibeFilter[XYZ_AXIS_COUNT];
|
||||
static EXTENDED_FASTRAM pt1Filter_t accVibeFloorFilter[XYZ_AXIS_COUNT];
|
||||
static EXTENDED_FASTRAM pt1Filter_t accVibeFilter[XYZ_AXIS_COUNT];
|
||||
|
||||
STATIC_FASTRAM filterApplyFnPtr accNotchFilterApplyFn;
|
||||
STATIC_FASTRAM void *accNotchFilter[XYZ_AXIS_COUNT];
|
||||
static EXTENDED_FASTRAM filterApplyFnPtr accNotchFilterApplyFn;
|
||||
static EXTENDED_FASTRAM void *accNotchFilter[XYZ_AXIS_COUNT];
|
||||
|
||||
PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 3);
|
||||
|
||||
|
|
|
|||
|
|
@ -112,7 +112,7 @@ static bool escSensorDecodeFrame(void)
|
|||
escSensorData[escSensorMotor].temperature = telemetryBuffer[0];
|
||||
escSensorData[escSensorMotor].voltage = ((uint16_t)telemetryBuffer[1]) << 8 | telemetryBuffer[2];
|
||||
escSensorData[escSensorMotor].current = ((uint16_t)telemetryBuffer[3]) << 8 | telemetryBuffer[4];
|
||||
escSensorData[escSensorMotor].rpm = ((uint16_t)telemetryBuffer[7]) << 8 | telemetryBuffer[8];
|
||||
escSensorData[escSensorMotor].rpm = computeRpm(((uint16_t)telemetryBuffer[7]) << 8 | telemetryBuffer[8]);
|
||||
escSensorDataNeedsUpdate = true;
|
||||
|
||||
if (escSensorMotor < 4) {
|
||||
|
|
@ -169,7 +169,7 @@ escSensorData_t * escSensorGetData(void)
|
|||
if (usedEscSensorCount) {
|
||||
escSensorDataCombined.current = (uint32_t)escSensorDataCombined.current * getMotorCount() / usedEscSensorCount + escSensorConfig()->currentOffset;
|
||||
escSensorDataCombined.voltage = (uint32_t)escSensorDataCombined.voltage / usedEscSensorCount;
|
||||
escSensorDataCombined.rpm = computeRpm((float)escSensorDataCombined.rpm / usedEscSensorCount);
|
||||
escSensorDataCombined.rpm = (float)escSensorDataCombined.rpm / usedEscSensorCount;
|
||||
}
|
||||
else {
|
||||
escSensorDataCombined.dataAge = ESC_DATA_INVALID;
|
||||
|
|
|
|||
|
|
@ -29,7 +29,7 @@ typedef struct {
|
|||
int8_t temperature;
|
||||
int16_t voltage;
|
||||
int32_t current;
|
||||
int16_t rpm;
|
||||
uint32_t rpm;
|
||||
} escSensorData_t;
|
||||
|
||||
typedef struct escSensorConfig_s {
|
||||
|
|
|
|||
|
|
@ -57,7 +57,6 @@ void targetConfiguration(void)
|
|||
if (hardwareMotorType == MOTOR_BRUSHED) {
|
||||
motorConfigMutable()->motorPwmProtocol = PWM_TYPE_BRUSHED;
|
||||
motorConfigMutable()->motorPwmRate = BRUSHED_MOTORS_PWM_RATE;
|
||||
motorConfigMutable()->minthrottle = 1000;
|
||||
}
|
||||
|
||||
pidProfileMutable()->bank_mc.pid[ROLL].P = 36;
|
||||
|
|
|
|||
|
|
@ -63,7 +63,6 @@ void targetConfiguration(void)
|
|||
if (hardwareMotorType == MOTOR_BRUSHED) {
|
||||
motorConfigMutable()->motorPwmProtocol = PWM_TYPE_BRUSHED;
|
||||
motorConfigMutable()->motorPwmRate = BRUSHED_MOTORS_PWM_RATE;
|
||||
motorConfigMutable()->minthrottle = 1000;
|
||||
}
|
||||
if (hardwareRevision == AFF4_REV_1) {
|
||||
rxConfigMutable()->serialrx_provider = SERIALRX_SPEKTRUM2048;
|
||||
|
|
|
|||
|
|
@ -64,7 +64,6 @@ void targetConfiguration(void)
|
|||
|
||||
if (hardwareMotorType == MOTOR_BRUSHED) {
|
||||
motorConfigMutable()->motorPwmRate = BRUSHED_MOTORS_PWM_RATE;
|
||||
motorConfigMutable()->minthrottle = 1000;
|
||||
}
|
||||
|
||||
if (hardwareRevision == AFF7_REV_1) {
|
||||
|
|
|
|||
|
|
@ -37,6 +37,5 @@ void targetConfiguration(void)
|
|||
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_SMARTPORT;
|
||||
motorConfigMutable()->motorPwmProtocol = PWM_TYPE_MULTISHOT;
|
||||
motorConfigMutable()->motorPwmRate = 4000;
|
||||
motorConfigMutable()->minthrottle = 1075;
|
||||
motorConfigMutable()->maxthrottle = 1950;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -37,6 +37,5 @@ void targetConfiguration(void)
|
|||
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_SMARTPORT;
|
||||
motorConfigMutable()->motorPwmProtocol = PWM_TYPE_MULTISHOT;
|
||||
motorConfigMutable()->motorPwmRate = 4000;
|
||||
motorConfigMutable()->minthrottle = 1075;
|
||||
motorConfigMutable()->maxthrottle = 1950;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -35,6 +35,9 @@
|
|||
|
||||
#define MPU6000_CS_PIN PA4
|
||||
#define MPU6000_SPI_BUS BUS_SPI1
|
||||
#define ICM20689_CS_PIN PA4
|
||||
#define ICM20689_SPI_BUS BUS_SPI1
|
||||
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_MPU6000
|
||||
|
|
@ -44,6 +47,12 @@
|
|||
#define USE_GYRO_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW90_DEG
|
||||
|
||||
#define USE_ACC_ICM20689
|
||||
#define ACC_ICM20689_ALIGN CW90_DEG
|
||||
|
||||
#define USE_GYRO_ICM20689
|
||||
#define GYRO_ICM20689_ALIGN CW90_DEG
|
||||
|
||||
#define USE_MAG
|
||||
#define MAG_I2C_BUS BUS_I2C2
|
||||
#define MAG_HMC5883_ALIGN CW90_DEG
|
||||
|
|
|
|||
|
|
@ -3,6 +3,7 @@ FEATURES += SDCARD VCP MSC
|
|||
|
||||
TARGET_SRC = \
|
||||
drivers/accgyro/accgyro_mpu6000.c \
|
||||
drivers/accgyro/accgyro_icm20689.c \
|
||||
drivers/barometer/barometer_ms56xx.c \
|
||||
drivers/compass/compass_hmc5883l.c \
|
||||
drivers/compass/compass_qmc5883l.c \
|
||||
|
|
|
|||
|
|
@ -82,7 +82,6 @@ void targetConfiguration(void)
|
|||
blackboxConfigMutable()->rate_num = 1;
|
||||
blackboxConfigMutable()->rate_denom = 4;
|
||||
|
||||
motorConfigMutable()->minthrottle = 1015;
|
||||
motorConfigMutable()->maxthrottle = 2000;
|
||||
motorConfigMutable()->mincommand = 980;
|
||||
motorConfigMutable()->motorPwmRate = 2000;
|
||||
|
|
|
|||
|
|
@ -24,10 +24,10 @@
|
|||
const timerHardware_t timerHardware[] = {
|
||||
|
||||
DEF_TIM(TIM8, CH4, PC9, TIM_USE_PPM, 0, 0 ), // PPM IN
|
||||
DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1 ), // S1_OUT - DMA1_ST6_CH3
|
||||
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S2_OUT - DMA1_ST7_CH5
|
||||
DEF_TIM(TIM1, CH3N, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S3_OUT - DMA2_ST6_CH0
|
||||
DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S4_OUT - DMA1_ST1_CH3
|
||||
DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1 ), // S1_OUT - D(1, 6, 3)
|
||||
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S2_OUT - D(1, 7, 5)
|
||||
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S3_OUT - D(1, 2, 5)
|
||||
DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S4_OUT - D(1, 1, 3)
|
||||
DEF_TIM(TIM2, CH3, PB10, TIM_USE_ANY, 0, 0),
|
||||
DEF_TIM(TIM2, CH4, PB11, TIM_USE_ANY, 0, 0),
|
||||
DEF_TIM(TIM5, CH1, PA0, TIM_USE_LED, 0, 0 ), // LED_STRIP - DMA1_ST2_CH6
|
||||
|
|
|
|||
|
|
@ -38,7 +38,7 @@ const timerHardware_t timerHardware[] = {
|
|||
DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), // M5 , DMA2_ST7
|
||||
DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), // M6 , DMA1_ST1
|
||||
|
||||
DEF_TIM(TIM4, CH1, PD12, TIM_USE_LED, 0, 0), // LED_TRIP, DMA1_ST0
|
||||
DEF_TIM(TIM4, CH1, PD12, TIM_USE_LED, 0, 0), // LED_STRIP, DMA1_ST0
|
||||
};
|
||||
|
||||
const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
|
||||
|
|
|
|||
|
|
@ -172,6 +172,9 @@
|
|||
#define SERIALRX_UART SERIAL_PORT_USART6
|
||||
#define SERIALRX_PROVIDER SERIALRX_SBUS
|
||||
|
||||
#define USE_LED_STRIP
|
||||
#define WS2811_PIN PD12 //TIM4_CH1
|
||||
|
||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||
|
||||
#define TARGET_IO_PORTA 0xffff
|
||||
|
|
|
|||
|
|
@ -27,7 +27,7 @@
|
|||
const timerHardware_t timerHardware[] = {
|
||||
DEF_TIM(TIM11, CH1, PB9, TIM_USE_PPM, 0, 0 ), // PPM IN
|
||||
|
||||
DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR, 0, 0 ), // S4_OUT – D(2, 6, 0)
|
||||
DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR, 0, 1 ), // S4_OUT – D(2, 2, 6)
|
||||
DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR, 0, 1 ), // S3_OUT – D(2, 1, 6)
|
||||
DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR, 0, 0 ), // S2_OUT – D(2, 7, 7)
|
||||
DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR, 0, 0 ), // S1_OUT – D(2, 4, 7)
|
||||
|
|
|
|||
|
|
@ -30,7 +30,7 @@ const timerHardware_t timerHardware[] = {
|
|||
DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR, 0, 0 ), // S1_OUT – D(2, 4, 7)
|
||||
DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR, 0, 0 ), // S2_OUT – D(2, 7, 7)
|
||||
DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR, 0, 1 ), // S3_OUT – D(2, 1, 6)
|
||||
DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR, 0, 0 ), // S4_OUT – D(2, 6, 0)
|
||||
DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR, 0, 1 ), // S4_OUT – D(2, 2, 6)
|
||||
|
||||
DEF_TIM(TIM2, CH2, PB3, TIM_USE_LED, 0, 0 ), // LED_STRIP – D(1, 6, 3)
|
||||
};
|
||||
|
|
|
|||
|
|
@ -46,7 +46,7 @@
|
|||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_MPU6000
|
||||
#define ACC_MPU6500_ALIGN CW180_DEG
|
||||
#define ACC_MPU6000_ALIGN CW180_DEG
|
||||
|
||||
// *************** Baro **************************
|
||||
#define USE_I2C
|
||||
|
|
|
|||
|
|
@ -24,7 +24,7 @@
|
|||
#include "drivers/bus.h"
|
||||
|
||||
const timerHardware_t timerHardware[] = {
|
||||
#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4PRO_LEDSTRIPM5) || defined(OMNIBUSF4V3) || defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS)
|
||||
#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)
|
||||
DEF_TIM(TIM10, CH1, PB8, TIM_USE_PPM, 0, 0), // PPM
|
||||
DEF_TIM(TIM4, CH4, PB9, TIM_USE_ANY, 0, 0), // S2_IN
|
||||
#else
|
||||
|
|
@ -42,7 +42,7 @@ const timerHardware_t timerHardware[] = {
|
|||
DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4_OUT D1_ST1
|
||||
|
||||
// { TIM9, IO_TAG(PA3), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // MOTOR_3
|
||||
#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)
|
||||
#if (defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)) && !(defined(OMNIBUSF4PRO_LEDSTRIPM5) || defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS))
|
||||
DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S5_OUT
|
||||
DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6_OUT
|
||||
#elif defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS)
|
||||
|
|
@ -59,7 +59,7 @@ const timerHardware_t timerHardware[] = {
|
|||
DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6_OUT
|
||||
#endif
|
||||
|
||||
#if (defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) || defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS))
|
||||
#if (defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)) && !defined(OMNIBUSF4PRO_LEDSTRIPM5)
|
||||
DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // LED strip for F4 V2 / F4-Pro-0X and later (RCD_CS for F4)
|
||||
#endif
|
||||
};
|
||||
|
|
|
|||
|
|
@ -17,12 +17,18 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
//Same target as OMNIBUSF4PRO with LED strip in M5
|
||||
#ifdef OMNIBUSF4PRO_LEDSTRIPM5
|
||||
#define OMNIBUSF4PRO
|
||||
#endif
|
||||
//Same target as OMNIBUSF4V3 with softserial in M5 and M6
|
||||
#if defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS)
|
||||
#define OMNIBUSF4V3
|
||||
#endif
|
||||
|
||||
#ifdef OMNIBUSF4PRO
|
||||
#define TARGET_BOARD_IDENTIFIER "OBSD"
|
||||
#elif defined(OMNIBUSF4V3) || defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS)
|
||||
#elif defined(OMNIBUSF4V3)
|
||||
#define TARGET_BOARD_IDENTIFIER "OB43"
|
||||
#elif defined(DYSF4PRO)
|
||||
#define TARGET_BOARD_IDENTIFIER "DYS4"
|
||||
|
|
@ -32,7 +38,7 @@
|
|||
#define TARGET_BOARD_IDENTIFIER "OBF4"
|
||||
#endif
|
||||
|
||||
#if defined(DYSF4PRO)
|
||||
#if defined(DYSF4PRO) || defined(DYSF4PROV2)
|
||||
#define USBD_PRODUCT_STRING "DysF4Pro"
|
||||
#else
|
||||
#define USBD_PRODUCT_STRING "Omnibus F4"
|
||||
|
|
@ -69,8 +75,7 @@
|
|||
#define MPU6000_CS_PIN PA4
|
||||
#define MPU6000_SPI_BUS BUS_SPI1
|
||||
|
||||
// Long sentence, OMNIBUSF4 always defined
|
||||
#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4PRO_LEDSTRIPM5) || defined(OMNIBUSF4V3) || defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS)
|
||||
#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)
|
||||
#define USE_GYRO_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW270_DEG
|
||||
|
||||
|
|
@ -85,7 +90,7 @@
|
|||
#endif
|
||||
|
||||
// Support for OMNIBUS F4 PRO CORNER - it has ICM20608 instead of MPU6000
|
||||
#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4PRO_LEDSTRIPM5) || defined(OMNIBUSF4V3) || defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS)
|
||||
#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)
|
||||
#define MPU6500_CS_PIN MPU6000_CS_PIN
|
||||
#define MPU6500_SPI_BUS MPU6000_SPI_BUS
|
||||
|
||||
|
|
@ -111,7 +116,7 @@
|
|||
|
||||
#define USE_BARO
|
||||
|
||||
#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4PRO_LEDSTRIPM5) || defined(OMNIBUSF4V3) || defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS)
|
||||
#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)
|
||||
#define USE_BARO_BMP280
|
||||
#define BMP280_SPI_BUS BUS_SPI3
|
||||
#define BMP280_CS_PIN PB3 // v1
|
||||
|
|
@ -141,7 +146,7 @@
|
|||
#define UART1_RX_PIN PA10
|
||||
#define UART1_TX_PIN PA9
|
||||
#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2
|
||||
#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4PRO_LEDSTRIPM5)
|
||||
#if defined(OMNIBUSF4PRO)
|
||||
#define INVERTER_PIN_UART1_RX PC0 // PC0 has never been used as inverter control on genuine OMNIBUS F4 variants, but leave it as is since some clones actually implement it.
|
||||
#endif
|
||||
|
||||
|
|
@ -152,12 +157,12 @@
|
|||
#define USE_UART6
|
||||
#define UART6_RX_PIN PC7
|
||||
#define UART6_TX_PIN PC6
|
||||
#if defined(OMNIBUSF4V3) || defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS)
|
||||
#if defined(OMNIBUSF4V3)
|
||||
#define INVERTER_PIN_UART6_RX PC8
|
||||
#define INVERTER_PIN_UART6_TX PC9
|
||||
#endif
|
||||
|
||||
#if defined(OMNIBUSF4V3)
|
||||
#if defined(OMNIBUSF4V3) && !(defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS))
|
||||
#define USE_SOFTSERIAL1
|
||||
#define SOFTSERIAL_1_RX_PIN PC6 // shared with UART6 TX
|
||||
#define SOFTSERIAL_1_TX_PIN PC6 // shared with UART6 TX
|
||||
|
|
@ -201,7 +206,7 @@
|
|||
|
||||
#define USE_SPI_DEVICE_1
|
||||
|
||||
#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4PRO_LEDSTRIPM5) || defined(OMNIBUSF4V3) || defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS)
|
||||
#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)
|
||||
#define USE_SPI_DEVICE_2
|
||||
#define SPI2_NSS_PIN PB12
|
||||
#define SPI2_SCK_PIN PB13
|
||||
|
|
@ -210,7 +215,7 @@
|
|||
#endif
|
||||
|
||||
#define USE_SPI_DEVICE_3
|
||||
#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4PRO_LEDSTRIPM5) || defined(OMNIBUSF4V3) || defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS)
|
||||
#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)
|
||||
#define SPI3_NSS_PIN PA15
|
||||
#else
|
||||
#define SPI3_NSS_PIN PB3
|
||||
|
|
@ -224,7 +229,7 @@
|
|||
#define MAX7456_SPI_BUS BUS_SPI3
|
||||
#define MAX7456_CS_PIN PA15
|
||||
|
||||
#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4PRO_LEDSTRIPM5) || defined(OMNIBUSF4V3) || defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS)
|
||||
#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)
|
||||
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
|
||||
#define USE_SDCARD
|
||||
#define USE_SDCARD_SPI
|
||||
|
|
@ -259,7 +264,7 @@
|
|||
#define SENSORS_SET (SENSOR_ACC|SENSOR_MAG|SENSOR_BARO)
|
||||
|
||||
#define USE_LED_STRIP
|
||||
#if (defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) || defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS))
|
||||
#if (defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)) && !defined(OMNIBUSF4PRO_LEDSTRIPM5)
|
||||
#define WS2811_PIN PB6
|
||||
#else
|
||||
#define WS2811_PIN PA1
|
||||
|
|
|
|||
|
|
@ -28,12 +28,13 @@ const timerHardware_t timerHardware[] = {
|
|||
|
||||
DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 UP(1,2)
|
||||
DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S2 UP(2,1)
|
||||
DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3 UP(2,1)
|
||||
DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 UP(2,1)
|
||||
DEF_TIM(TIM3, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 UP(2,1)
|
||||
DEF_TIM(TIM3, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 UP(2,1)
|
||||
|
||||
DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S5 UP(1,7)
|
||||
DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6 UP(2,5)
|
||||
|
||||
DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S7 D(1,7)!S5 UP(2,6)
|
||||
|
||||
DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // LED D(1,0) UP(2,6)
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -150,8 +150,8 @@
|
|||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||
|
||||
// Number of available PWM outputs
|
||||
#define MAX_PWM_OUTPUT_PORTS 6
|
||||
#define TARGET_MOTOR_COUNT 6
|
||||
#define MAX_PWM_OUTPUT_PORTS 7
|
||||
#define TARGET_MOTOR_COUNT 7
|
||||
|
||||
#define TARGET_IO_PORTA 0xffff
|
||||
#define TARGET_IO_PORTB 0xffff
|
||||
|
|
|
|||
|
|
@ -107,6 +107,7 @@
|
|||
|
||||
#define USE_TELEMETRY_SIM
|
||||
#define USE_FRSKYOSD
|
||||
#define USE_DJI_HD_OSD
|
||||
|
||||
#define NAV_NON_VOLATILE_WAYPOINT_CLI
|
||||
|
||||
|
|
|
|||
|
|
@ -185,7 +185,7 @@
|
|||
#if !defined(MAG3110_I2C_BUS)
|
||||
#define MAG3110_I2C_BUS MAG_I2C_BUS
|
||||
#endif
|
||||
BUSDEV_REGISTER_I2C(busdev_mag3110, DEVHW_MAG3110, MAG3110_I2C_BUS, 0x0C, NONE, DEVFLAGS_NONE);
|
||||
BUSDEV_REGISTER_I2C(busdev_mag3110, DEVHW_MAG3110, MAG3110_I2C_BUS, 0x0E, NONE, DEVFLAGS_NONE);
|
||||
#endif
|
||||
|
||||
#if defined(USE_MAG_LIS3MDL)
|
||||
|
|
|
|||
|
|
@ -490,7 +490,7 @@ void initCrsfTelemetry(void)
|
|||
}
|
||||
#endif
|
||||
#if defined(USE_BARO) || defined(USE_GPS)
|
||||
if (sensors(SENSOR_BARO) || (STATE(FIXED_WING) && feature(FEATURE_GPS))) {
|
||||
if (sensors(SENSOR_BARO) || (STATE(FIXED_WING_LEGACY) && feature(FEATURE_GPS))) {
|
||||
crsfSchedule[index++] = BV(CRSF_FRAME_VARIO_SENSOR_INDEX);
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -128,9 +128,9 @@ static ibusAddress_t getAddress(uint8_t ibusPacket[static IBUS_MIN_LEN]) {
|
|||
return (ibusPacket[1] & 0x0F);
|
||||
}
|
||||
|
||||
// MANUAL, ACRO, ANGLE, HRZN, ALTHOLD, POSHOLD, RTH, WP, LAUNCH, FAILSAFE
|
||||
static uint8_t flightModeToIBusTelemetryMode1[FLM_COUNT] = { 0, 1, 3, 2, 5, 6, 7, 4, 8, 9 };
|
||||
static uint8_t flightModeToIBusTelemetryMode2[FLM_COUNT] = { 5, 1, 0, 7, 2, 8, 6, 3, 4, 9 };
|
||||
// MANUAL, ACRO, AIR, ANGLE, HRZN, ALTHOLD, POSHOLD, RTH, WP, CRUISE, LAUNCH, FAILSAFE
|
||||
static uint8_t flightModeToIBusTelemetryMode1[FLM_COUNT] = { 0, 1, 1, 3, 2, 5, 6, 7, 4, 4, 8, 9 };
|
||||
static uint8_t flightModeToIBusTelemetryMode2[FLM_COUNT] = { 5, 1, 1, 0, 7, 2, 8, 6, 3, 3, 4, 9 };
|
||||
static uint8_t dispatchMeasurementRequest(ibusAddress_t address) {
|
||||
#if defined(USE_GPS)
|
||||
uint8_t fix = 0;
|
||||
|
|
|
|||
|
|
@ -467,7 +467,7 @@ void mavlinkSendHUDAndHeartbeat(void)
|
|||
flightModeForTelemetry_e flm = getFlightModeForTelemetry();
|
||||
uint8_t mavCustomMode;
|
||||
|
||||
if (STATE(FIXED_WING)) {
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
mavCustomMode = inavToArduPlaneMap[flm];
|
||||
}
|
||||
else {
|
||||
|
|
|
|||
|
|
@ -132,7 +132,7 @@ static uint8_t simModuleState = SIM_MODULE_NOT_DETECTED;
|
|||
static int simRssi;
|
||||
static uint8_t accEvent = ACC_EVENT_NONE;
|
||||
static char* accEventDescriptions[] = { "", "HIT! ", "DROP ", "HIT " };
|
||||
static char* modeDescriptions[] = { "MAN", "ACR", "ANG", "HOR", "ALH", "POS", "RTH", "WP", "LAU", "FS" };
|
||||
static char* modeDescriptions[] = { "MAN", "ACR", "AIR", "ANG", "HOR", "ALH", "POS", "RTH", "WP", "CRS", "LAU", "FS" };
|
||||
static const char gpsFixIndicators[] = { '!', '*', ' ' };
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -214,7 +214,7 @@ void telemetryProcess(timeUs_t currentTimeUs)
|
|||
handleJetiExBusTelemetry();
|
||||
#endif
|
||||
|
||||
#if defined(USE_TELEMETRY_IBUS)
|
||||
#if defined(USE_SERIALRX_IBUS) && defined(USE_TELEMETRY_IBUS)
|
||||
handleIbusTelemetry();
|
||||
#endif
|
||||
|
||||
|
|
|
|||
|
|
@ -281,10 +281,10 @@ bool feature(uint32_t mask)
|
|||
return false;
|
||||
}
|
||||
|
||||
throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
|
||||
throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t mid_throttle_deadband)
|
||||
{
|
||||
UNUSED(*rxConfig);
|
||||
UNUSED(deadband3d_throttle);
|
||||
UNUSED(mid_throttle_deadband);
|
||||
return THROTTLE_HIGH;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -420,10 +420,10 @@ uint32_t millis(void)
|
|||
return sysTickUptime;
|
||||
}
|
||||
|
||||
throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
|
||||
throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t mid_throttle_deadband)
|
||||
{
|
||||
UNUSED(rxConfig);
|
||||
UNUSED(deadband3d_throttle);
|
||||
UNUSED(mid_throttle_deadband);
|
||||
return throttleStatus;
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue