mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-26 17:55:28 +03:00
Merge branch 'iNavFlight:master' into airspeed-alarms
This commit is contained in:
Igor
GitHub
commit
10a8dc4384
47 changed files with 773 additions and 722 deletions
|
|
@ -33,7 +33,7 @@ function(enable_settings exe name)
|
||||||
add_custom_command(
|
add_custom_command(
|
||||||
OUTPUT ${output}
|
OUTPUT ${output}
|
||||||
COMMAND
|
COMMAND
|
||||||
${CMAKE_COMMAND} -E env CFLAGS="${cflags}" TARGET=${name} PATH=$ENV{PATH} SETTINGS_CXX=${args_SETTINGS_CXX}
|
${CMAKE_COMMAND} -E env CFLAGS="${cflags}" TARGET=${name} PATH="$ENV{PATH}" SETTINGS_CXX=${args_SETTINGS_CXX}
|
||||||
${RUBY_EXECUTABLE} ${SETTINGS_GENERATOR} ${MAIN_DIR} ${SETTINGS_FILE} -o "${dir}"
|
${RUBY_EXECUTABLE} ${SETTINGS_GENERATOR} ${MAIN_DIR} ${SETTINGS_FILE} -o "${dir}"
|
||||||
DEPENDS ${SETTINGS_GENERATOR} ${SETTINGS_FILE}
|
DEPENDS ${SETTINGS_GENERATOR} ${SETTINGS_FILE}
|
||||||
)
|
)
|
||||||
|
|
|
||||||
|
|
@ -189,7 +189,7 @@ endfunction()
|
||||||
|
|
||||||
function(add_hex_target name exe hex)
|
function(add_hex_target name exe hex)
|
||||||
add_custom_target(${name} ALL
|
add_custom_target(${name} ALL
|
||||||
cmake -E env PATH=$ENV{PATH}
|
cmake -E env PATH="$ENV{PATH}"
|
||||||
# TODO: Overriding the start address with --set-start 0x08000000
|
# TODO: Overriding the start address with --set-start 0x08000000
|
||||||
# seems to be required due to some incorrect assumptions about .hex
|
# seems to be required due to some incorrect assumptions about .hex
|
||||||
# files in the configurator. Verify wether that's the case and fix
|
# files in the configurator. Verify wether that's the case and fix
|
||||||
|
|
@ -201,7 +201,7 @@ endfunction()
|
||||||
|
|
||||||
function(add_bin_target name exe bin)
|
function(add_bin_target name exe bin)
|
||||||
add_custom_target(${name}
|
add_custom_target(${name}
|
||||||
cmake -E env PATH=$ENV{PATH}
|
cmake -E env PATH="$ENV{PATH}"
|
||||||
${CMAKE_OBJCOPY} -Obinary $<TARGET_FILE:${exe}> ${bin}
|
${CMAKE_OBJCOPY} -Obinary $<TARGET_FILE:${exe}> ${bin}
|
||||||
BYPRODUCTS ${bin}
|
BYPRODUCTS ${bin}
|
||||||
)
|
)
|
||||||
|
|
|
||||||
|
|
@ -28,7 +28,6 @@ The following adjustments can be made in flight as well as on the ground.
|
||||||
* Throttle Expo
|
* Throttle Expo
|
||||||
* Pitch, Roll, Yaw Rates
|
* Pitch, Roll, Yaw Rates
|
||||||
* Pitch, Roll, Yaw PIDs
|
* Pitch, Roll, Yaw PIDs
|
||||||
* Rate profile
|
|
||||||
* Manual rates
|
* Manual rates
|
||||||
* FW cruise_throttle, pitch2thr, min_throttle_down_pitch_angle
|
* FW cruise_throttle, pitch2thr, min_throttle_down_pitch_angle
|
||||||
* Board alignment
|
* Board alignment
|
||||||
|
|
@ -62,7 +61,7 @@ Hint: With OpenTX transmitters you can combine two momentary OFF-ON switches to
|
||||||
|
|
||||||
The CLI command `adjrange` is used to configure adjustment ranges.
|
The CLI command `adjrange` is used to configure adjustment ranges.
|
||||||
|
|
||||||
12 adjustment ranges can be defined.
|
20 adjustment ranges can be defined.
|
||||||
4 adjustments can be made at the same time, each simultaneous adjustment requires an adjustment slot.
|
4 adjustments can be made at the same time, each simultaneous adjustment requires an adjustment slot.
|
||||||
|
|
||||||
Show the current ranges using:
|
Show the current ranges using:
|
||||||
|
|
@ -77,12 +76,12 @@ Configure a range using:
|
||||||
|
|
||||||
| Argument | Value | Meaning |
|
| Argument | Value | Meaning |
|
||||||
| -------- | ----- |-------- |
|
| -------- | ----- |-------- |
|
||||||
| Index | 0 - 11 | Select the adjustment range to configure |
|
| Index | 0 - 19 | Select the adjustment range to configure |
|
||||||
| Slot | 0 - 3 | Select the adjustment slot to use |
|
| Slot | 0 - 3 | Select the adjustment slot to use |
|
||||||
| Range Channel | 0 based index, AUX1 = 0, AUX2 = 1 | The AUX channel to use to select an adjustment for a switch/pot |
|
| Range Channel | 0 based index, AUX1 = 0, AUX2 = 1 | The AUX channel to use to select an adjustment for a switch/pot |
|
||||||
| Range Start | 900 - 2100. Steps of 25, e.g. 900, 925, 950... | Start of range |
|
| Range Start | 900 - 2100. Steps of 25, e.g. 900, 925, 950... | Start of range |
|
||||||
| Range End | 900 - 2100 | End of range |
|
| Range End | 900 - 2100 | End of range |
|
||||||
| Adjustment function | 0 - 11 | See Adjustment function table |
|
| Adjustment function | 0 - 56 | See Adjustment function table |
|
||||||
| Adjustment channel | 0 based index, AUX1 = 0, AUX2 = 1 | The channel that is controlled by a 3 Position switch/Pot |
|
| Adjustment channel | 0 based index, AUX1 = 0, AUX2 = 1 | The channel that is controlled by a 3 Position switch/Pot |
|
||||||
|
|
||||||
Range Start/End values should match the values sent by your receiver.
|
Range Start/End values should match the values sent by your receiver.
|
||||||
|
|
@ -100,8 +99,8 @@ this reason ensure that you define enough ranges to cover the range channel's us
|
||||||
|
|
||||||
### Adjustment function
|
### Adjustment function
|
||||||
|
|
||||||
| Value | Adjustment | Notes |
|
| Value | Adjustment |
|
||||||
| ----- | ---------- |------ |
|
| ----- | ---------- |
|
||||||
| 0 | None |
|
| 0 | None |
|
||||||
| 1 | RC RATE |
|
| 1 | RC RATE |
|
||||||
| 2 | RC_EXPO |
|
| 2 | RC_EXPO |
|
||||||
|
|
@ -111,47 +110,54 @@ this reason ensure that you define enough ranges to cover the range channel's us
|
||||||
| 6 | PITCH_ROLL_P |
|
| 6 | PITCH_ROLL_P |
|
||||||
| 7 | PITCH_ROLL_I |
|
| 7 | PITCH_ROLL_I |
|
||||||
| 8 | PITCH_ROLL_D |
|
| 8 | PITCH_ROLL_D |
|
||||||
| 9 | YAW_P |
|
| 9 | PITCH_ROLL_FF |
|
||||||
| 10 | YAW_I |
|
| 10 | PITCH_P |
|
||||||
| 11 | YAW_D_FF |
|
| 11 | PITCH_I |
|
||||||
| 12 | RATE_PROFILE | Switch between 3 rate profiles using a 3 position switch. |
|
| 12 | PITCH_D |
|
||||||
| 13 | PITCH_RATE |
|
| 13 | PITCH_FF |
|
||||||
| 14 | ROLL_RATE |
|
| 14 | ROLL_P |
|
||||||
| 15 | PITCH_P |
|
| 15 | ROLL_I |
|
||||||
| 16 | PITCH_I |
|
| 16 | ROLL_D |
|
||||||
| 17 | PITCH_D_FF |
|
| 17 | ROL_FF |
|
||||||
| 18 | ROLL_P |
|
| 18 | YAW_P |
|
||||||
| 19 | ROLL_I |
|
| 19 | YAW_I |
|
||||||
| 20 | ROLL_D_FF |
|
| 20 | YAW_D |
|
||||||
| 21 | RC_YAW_EXPO |
|
| 21 | YAW_FF
|
||||||
| 22 | MANUAL_RC_EXPO |
|
| 22 | Unused |
|
||||||
| 23 | MANUAL_RC_YAW_EXPO |
|
| 23 | PITCH_RATE |
|
||||||
| 24 | MANUAL_PITCH_ROLL_RATE |
|
| 24 | ROLL_RATE |
|
||||||
| 25 | MANUAL_ROLL_RATE |
|
| 25 | RC_YAW_EXPO |
|
||||||
| 26 | MANUAL_PITCH_RATE |
|
| 26 | MANUAL_RC_EXPO |
|
||||||
| 27 | MANUAL_YAW_RATE |
|
| 27 | MANUAL_RC_YAW_EXPO |
|
||||||
| 28 | NAV_FW_CRUISE_THROTTLE |
|
| 28 | MANUAL_PITCH_ROLL_RATE |
|
||||||
| 29 | NAV_FW_PITCH2THR |
|
| 29 | MANUAL_ROLL_RATE |
|
||||||
| 30 | ROLL_BOARD_ALIGNMENT |
|
| 30 | MANUAL_PITCH_RATE |
|
||||||
| 31 | PITCH_BOARD_ALIGNMENT |
|
| 31 | MANUAL_YAW_RATE |
|
||||||
| 32 | LEVEL_P |
|
| 32 | NAV_FW_CRUISE_THROTTLE |
|
||||||
| 33 | LEVEL_I |
|
| 33 | NAV_FW_PITCH2THR |
|
||||||
| 34 | LEVEL_D |
|
| 34 | ROLL_BOARD_ALIGNMENT |
|
||||||
| 35 | POS_XY_P |
|
| 35 | PITCH_BOARD_ALIGNMENT |
|
||||||
| 36 | POS_XY_I |
|
| 36 | LEVEL_P |
|
||||||
| 37 | POS_XY_D |
|
| 37 | LEVEL_I |
|
||||||
| 38 | POS_Z_P |
|
| 38 | LEVEL_D |
|
||||||
| 39 | POS_Z_I |
|
| 39 | POS_XY_P |
|
||||||
| 40 | POS_Z_D |
|
| 40 | POS_XY_I |
|
||||||
| 41 | HEADING_P |
|
| 41 | POS_XY_D |
|
||||||
| 42 | VEL_XY_P |
|
| 42 | POS_Z_P |
|
||||||
| 43 | VEL_XY_I |
|
| 43 | POS_Z_I |
|
||||||
| 44 | VEL_XY_D |
|
| 44 | POS_Z_D |
|
||||||
| 45 | VEL_Z_P |
|
| 45 | HEADING_P |
|
||||||
| 46 | VEL_Z_I |
|
| 46 | VEL_XY_P |
|
||||||
| 47 | VEL_Z_D |
|
| 47 | VEL_XY_I |
|
||||||
| 48 | FW_MIN_THROTTLE_DOWN_PITCH_ANGLE |
|
| 48 | VEL_XY_D |
|
||||||
| 49 | ADJUSTMENT_VTX_POWER_LEVEL |
|
| 49 | VEL_Z_P |
|
||||||
|
| 50 | VEL_Z_I |
|
||||||
|
| 51 | VEL_Z_D |
|
||||||
|
| 52 | FW_MIN_THROTTLE_DOWN_PITCH_ANGLE |
|
||||||
|
| 53 | ADJUSTMENT_VTX_POWER_LEVEL |
|
||||||
|
| 54 | TPA |
|
||||||
|
| 55 | TPA_BREAKPOINT |
|
||||||
|
| 56 | NAV_FW_CONTROL_SMOOTHNESS |
|
||||||
|
|
||||||
## Examples
|
## Examples
|
||||||
|
|
||||||
|
|
@ -193,9 +199,9 @@ range.
|
||||||
adjrange 3 2 1 900 1150 6 3
|
adjrange 3 2 1 900 1150 6 3
|
||||||
adjrange 4 2 1 1150 1300 7 3
|
adjrange 4 2 1 1150 1300 7 3
|
||||||
adjrange 5 2 1 1300 1500 8 3
|
adjrange 5 2 1 1300 1500 8 3
|
||||||
adjrange 6 2 1 1500 1700 9 3
|
adjrange 6 2 1 1500 1700 18 3
|
||||||
adjrange 7 2 1 1700 1850 10 3
|
adjrange 7 2 1 1700 1850 19 3
|
||||||
adjrange 8 2 1 1850 2100 11 3
|
adjrange 8 2 1 1850 2100 20 3
|
||||||
```
|
```
|
||||||
|
|
||||||
explained:
|
explained:
|
||||||
|
|
@ -210,38 +216,19 @@ explained:
|
||||||
(1) in the range 1300-1500 then use adjustment Pitch/Roll D (8) when aux 4
|
(1) in the range 1300-1500 then use adjustment Pitch/Roll D (8) when aux 4
|
||||||
(3) is in the appropriate position.
|
(3) is in the appropriate position.
|
||||||
* configure adjrange 6 to use adjustment slot 3 (2) so that when aux2
|
* configure adjrange 6 to use adjustment slot 3 (2) so that when aux2
|
||||||
(1) in the range 1500-1700 then use adjustment Yaw P (9) when aux 4
|
(1) in the range 1500-1700 then use adjustment Yaw P (18) when aux 4
|
||||||
(3) is in the appropriate position.
|
(3) is in the appropriate position.
|
||||||
* configure adjrange 7 to use adjustment slot 3 (2) so that when aux2
|
* configure adjrange 7 to use adjustment slot 3 (2) so that when aux2
|
||||||
(1) in the range 1700-1850 then use adjustment Yaw I (10) when aux 4
|
(1) in the range 1700-1850 then use adjustment Yaw I (19) when aux 4
|
||||||
(3) is in the appropriate position.
|
(3) is in the appropriate position.
|
||||||
* configure adjrange 8 to use adjustment slot 3 (2) so that when aux2
|
* configure adjrange 8 to use adjustment slot 3 (2) so that when aux2
|
||||||
(1) in the range 1850-2100 then use adjustment Yaw D (11) when aux 4
|
(1) in the range 1850-2100 then use adjustment Yaw D (20) when aux 4
|
||||||
(3) is in the appropriate position.
|
(3) is in the appropriate position.
|
||||||
|
|
||||||
### Example 4 - Use a single 3 position switch to change between 3 different rate profiles
|
|
||||||
|
|
||||||
adjrange 11 3 3 900 2100 12 3
|
|
||||||
|
|
||||||
explained:
|
|
||||||
|
|
||||||
* configure adjrange 11 to use adjustment slot 4 (3) so that when aux4
|
|
||||||
(3) in the range 900-2100 then use adjustment Rate Profile (12) when aux 4
|
|
||||||
(3) is in the appropriate position.
|
|
||||||
|
|
||||||
When the switch is low, rate profile 0 is selcted.
|
|
||||||
When the switch is medium, rate profile 1 is selcted.
|
|
||||||
When the switch is high, rate profile 2 is selcted.
|
|
||||||
|
|
||||||
|
|
||||||
### Configurator examples
|
### Configurator examples
|
||||||
|
|
||||||
The following 5 images show valid configurations. In all cases the entire usable range for the Range Channel is used.
|
The following 5 images show valid configurations. In all cases the entire usable range for the Range Channel is used.
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
---
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
|
||||||
154
docs/Profiles.md
154
docs/Profiles.md
|
|
@ -2,7 +2,7 @@
|
||||||
|
|
||||||
A profile is a set of configuration settings.
|
A profile is a set of configuration settings.
|
||||||
|
|
||||||
Currently three profiles are supported. The default profile is profile `0`.
|
Currently three profiles are supported. The default profile is profile `1`.
|
||||||
|
|
||||||
## Changing profiles
|
## Changing profiles
|
||||||
|
|
||||||
|
|
@ -10,9 +10,9 @@ Profiles can be selected using a GUI or the following stick combinations:
|
||||||
|
|
||||||
| Profile | Throttle | Yaw | Pitch | Roll |
|
| Profile | Throttle | Yaw | Pitch | Roll |
|
||||||
| ------- | -------- | ----- | ------ | ------ |
|
| ------- | -------- | ----- | ------ | ------ |
|
||||||
| 0 | Down | Left | Middle | Left |
|
| 1 | Down | Left | Middle | Left |
|
||||||
| 1 | Down | Left | Up | Middle |
|
| 2 | Down | Left | Up | Middle |
|
||||||
| 2 | Down | Left | Middle | Right |
|
| 3 | Down | Left | Middle | Right |
|
||||||
|
|
||||||
The CLI `profile` command can also be used:
|
The CLI `profile` command can also be used:
|
||||||
|
|
||||||
|
|
@ -20,45 +20,127 @@ The CLI `profile` command can also be used:
|
||||||
profile <index>
|
profile <index>
|
||||||
```
|
```
|
||||||
|
|
||||||
# Rate Profiles
|
The values contained within a profile can be seen by using the CLI `dump profile` command.
|
||||||
|
|
||||||
INAV supports rate profiles in addition to regular profiles.
|
|
||||||
|
|
||||||
Rate profiles contain settings that adjust how your craft behaves based on control input.
|
|
||||||
|
|
||||||
Three rate profiles are supported.
|
|
||||||
|
|
||||||
Rate profiles can be selected while flying using the inflight adjustments feature.
|
|
||||||
|
|
||||||
Each normal profile has a setting called 'default_rate_profile`. When a profile is activated the
|
|
||||||
corresponding rate profile is also activated.
|
|
||||||
|
|
||||||
Profile 0 has a default rate profile of 0.
|
|
||||||
Profile 1 has a default rate profile of 1.
|
|
||||||
Profile 2 has a default rate profile of 2.
|
|
||||||
|
|
||||||
The defaults are set this way so that it's simple to configure a profile and a rate profile at the same.
|
|
||||||
|
|
||||||
The current rate profile can be shown or set using the CLI `rateprofile` command:
|
|
||||||
|
|
||||||
```
|
|
||||||
rateprofile <index>
|
|
||||||
```
|
|
||||||
|
|
||||||
The values contained within a rate profile can be seen by using the CLI `dump rates` command.
|
|
||||||
|
|
||||||
e.g
|
e.g
|
||||||
```
|
```
|
||||||
# dump rates
|
# dump profile
|
||||||
|
|
||||||
# rateprofile
|
# profile
|
||||||
rateprofile 0
|
profile 1
|
||||||
|
|
||||||
set rc_expo = 65
|
set mc_p_pitch = 40
|
||||||
|
set mc_i_pitch = 30
|
||||||
|
set mc_d_pitch = 23
|
||||||
|
set mc_cd_pitch = 60
|
||||||
|
set mc_p_roll = 40
|
||||||
|
set mc_i_roll = 30
|
||||||
|
set mc_d_roll = 23
|
||||||
|
set mc_cd_roll = 60
|
||||||
|
set mc_p_yaw = 85
|
||||||
|
set mc_i_yaw = 45
|
||||||
|
set mc_d_yaw = 0
|
||||||
|
set mc_cd_yaw = 60
|
||||||
|
set mc_p_level = 20
|
||||||
|
set mc_i_level = 15
|
||||||
|
set mc_d_level = 75
|
||||||
|
set fw_p_pitch = 5
|
||||||
|
set fw_i_pitch = 7
|
||||||
|
set fw_d_pitch = 0
|
||||||
|
set fw_ff_pitch = 50
|
||||||
|
set fw_p_roll = 5
|
||||||
|
set fw_i_roll = 7
|
||||||
|
set fw_d_roll = 0
|
||||||
|
set fw_ff_roll = 50
|
||||||
|
set fw_p_yaw = 6
|
||||||
|
set fw_i_yaw = 10
|
||||||
|
set fw_d_yaw = 0
|
||||||
|
set fw_ff_yaw = 60
|
||||||
|
set fw_p_level = 20
|
||||||
|
set fw_i_level = 5
|
||||||
|
set fw_d_level = 75
|
||||||
|
set max_angle_inclination_rll = 300
|
||||||
|
set max_angle_inclination_pit = 300
|
||||||
|
set dterm_lpf_hz = 110
|
||||||
|
set dterm_lpf_type = PT2
|
||||||
|
set dterm_lpf2_hz = 0
|
||||||
|
set dterm_lpf2_type = PT1
|
||||||
|
set yaw_lpf_hz = 0
|
||||||
|
set fw_iterm_throw_limit = 165
|
||||||
|
set fw_loiter_direction = RIGHT
|
||||||
|
set fw_reference_airspeed = 1500.000
|
||||||
|
set fw_turn_assist_yaw_gain = 1.000
|
||||||
|
set fw_turn_assist_pitch_gain = 1.000
|
||||||
|
set fw_iterm_limit_stick_position = 0.500
|
||||||
|
set fw_yaw_iterm_freeze_bank_angle = 0
|
||||||
|
set pidsum_limit = 500
|
||||||
|
set pidsum_limit_yaw = 350
|
||||||
|
set iterm_windup = 50
|
||||||
|
set rate_accel_limit_roll_pitch = 0
|
||||||
|
set rate_accel_limit_yaw = 10000
|
||||||
|
set heading_hold_rate_limit = 90
|
||||||
|
set nav_mc_pos_z_p = 50
|
||||||
|
set nav_mc_vel_z_p = 100
|
||||||
|
set nav_mc_vel_z_i = 50
|
||||||
|
set nav_mc_vel_z_d = 10
|
||||||
|
set nav_mc_pos_xy_p = 65
|
||||||
|
set nav_mc_vel_xy_p = 40
|
||||||
|
set nav_mc_vel_xy_i = 15
|
||||||
|
set nav_mc_vel_xy_d = 100
|
||||||
|
set nav_mc_vel_xy_ff = 40
|
||||||
|
set nav_mc_heading_p = 60
|
||||||
|
set nav_mc_vel_xy_dterm_lpf_hz = 2.000
|
||||||
|
set nav_mc_vel_xy_dterm_attenuation = 90
|
||||||
|
set nav_mc_vel_xy_dterm_attenuation_start = 10
|
||||||
|
set nav_mc_vel_xy_dterm_attenuation_end = 60
|
||||||
|
set nav_fw_pos_z_p = 40
|
||||||
|
set nav_fw_pos_z_i = 5
|
||||||
|
set nav_fw_pos_z_d = 10
|
||||||
|
set nav_fw_pos_xy_p = 75
|
||||||
|
set nav_fw_pos_xy_i = 5
|
||||||
|
set nav_fw_pos_xy_d = 8
|
||||||
|
set nav_fw_heading_p = 60
|
||||||
|
set nav_fw_pos_hdg_p = 30
|
||||||
|
set nav_fw_pos_hdg_i = 2
|
||||||
|
set nav_fw_pos_hdg_d = 0
|
||||||
|
set nav_fw_pos_hdg_pidsum_limit = 350
|
||||||
|
set mc_iterm_relax = RP
|
||||||
|
set mc_iterm_relax_cutoff = 15
|
||||||
|
set d_boost_min = 0.500
|
||||||
|
set d_boost_max = 1.250
|
||||||
|
set d_boost_max_at_acceleration = 7500.000
|
||||||
|
set d_boost_gyro_delta_lpf_hz = 80
|
||||||
|
set antigravity_gain = 1.000
|
||||||
|
set antigravity_accelerator = 1.000
|
||||||
|
set antigravity_cutoff_lpf_hz = 15
|
||||||
|
set pid_type = AUTO
|
||||||
|
set mc_cd_lpf_hz = 30
|
||||||
|
set fw_level_pitch_trim = 0.000
|
||||||
|
set smith_predictor_strength = 0.500
|
||||||
|
set smith_predictor_delay = 0.000
|
||||||
|
set smith_predictor_lpf_hz = 50
|
||||||
|
set fw_level_pitch_gain = 5.000
|
||||||
set thr_mid = 50
|
set thr_mid = 50
|
||||||
set thr_expo = 0
|
set thr_expo = 0
|
||||||
set roll_pitch_rate = 0
|
|
||||||
set yaw_rate = 0
|
|
||||||
set tpa_rate = 0
|
set tpa_rate = 0
|
||||||
set tpa_breakpoint = 1500
|
set tpa_breakpoint = 1500
|
||||||
|
set fw_tpa_time_constant = 0
|
||||||
|
set rc_expo = 70
|
||||||
|
set rc_yaw_expo = 20
|
||||||
|
set roll_rate = 20
|
||||||
|
set pitch_rate = 20
|
||||||
|
set yaw_rate = 20
|
||||||
|
set manual_rc_expo = 70
|
||||||
|
set manual_rc_yaw_expo = 20
|
||||||
|
set manual_roll_rate = 100
|
||||||
|
set manual_pitch_rate = 100
|
||||||
|
set manual_yaw_rate = 100
|
||||||
|
set fpv_mix_degrees = 0
|
||||||
|
set rate_dynamics_center_sensitivity = 100
|
||||||
|
set rate_dynamics_end_sensitivity = 100
|
||||||
|
set rate_dynamics_center_correction = 10
|
||||||
|
set rate_dynamics_end_correction = 10
|
||||||
|
set rate_dynamics_center_weight = 0
|
||||||
|
set rate_dynamics_end_weight = 0
|
||||||
|
|
||||||
```
|
```
|
||||||
|
|
|
||||||
|
|
@ -74,7 +74,8 @@ IPF can be edited using INAV Configurator user interface, of via CLI
|
||||||
| 38 | RC_CHANNEL_OVERRIDE | Overrides channel set by `Operand A` to value of `Operand B` |
|
| 38 | RC_CHANNEL_OVERRIDE | Overrides channel set by `Operand A` to value of `Operand B` |
|
||||||
| 39 | SET_HEADING_TARGET | Sets heading-hold target to `Operand A`, in degrees. Value wraps-around. |
|
| 39 | SET_HEADING_TARGET | Sets heading-hold target to `Operand A`, in degrees. Value wraps-around. |
|
||||||
| 40 | MOD | Divide `Operand A` by `Operand B` and returns the remainder |
|
| 40 | MOD | Divide `Operand A` by `Operand B` and returns the remainder |
|
||||||
|
| 41 | LOITER_RADIUS_OVERRIDE | Sets the loiter radius to `Operand A` [`0` : `100000`] in cm. If the value is lower than the loiter radius set in the **Advanced Tuning**, that will be used. |
|
||||||
|
| 42 | SET_PROFILE | Sets the active config profile (PIDFF/Rates/Filters/etc) to `Operand A`. `Operand A` must be a valid profile number, currently from 1 to 3. If not, the profile will not change |
|
||||||
|
|
||||||
### Operands
|
### Operands
|
||||||
|
|
||||||
|
|
@ -125,8 +126,10 @@ IPF can be edited using INAV Configurator user interface, of via CLI
|
||||||
| 30 | ACTIVE_WAYPOINT_ACTION | See ACTIVE_WAYPOINT_ACTION paragraph |
|
| 30 | ACTIVE_WAYPOINT_ACTION | See ACTIVE_WAYPOINT_ACTION paragraph |
|
||||||
| 31 | 3D HOME_DISTANCE | in `meters`, calculated from HOME_DISTANCE and ALTITUDE using Pythagorean theorem |
|
| 31 | 3D HOME_DISTANCE | in `meters`, calculated from HOME_DISTANCE and ALTITUDE using Pythagorean theorem |
|
||||||
| 32 | CROSSFIRE LQ | Crossfire Link quality as returned by the CRSF protocol |
|
| 32 | CROSSFIRE LQ | Crossfire Link quality as returned by the CRSF protocol |
|
||||||
| 33 | CROSSFIRE SNR | Crossfire SNR as returned by the CRSF protocol |
|
| 33 | CROSSFIRE SNR | Crossfire SNR as returned by the CRSF protocol |
|
||||||
| 34 | GPS_VALID | boolean `0`/`1`. True when the GPS has a valid 3D Fix |
|
| 34 | GPS_VALID | boolean `0`/`1`. True when the GPS has a valid 3D Fix |
|
||||||
|
| 35 | LOITER_RADIUS | The current loiter radius in cm. |
|
||||||
|
| 36 | ACTIVE_PROFILE | integer for the active config profile `[1..MAX_PROFILE_COUNT]` |
|
||||||
|
|
||||||
#### ACTIVE_WAYPOINT_ACTION
|
#### ACTIVE_WAYPOINT_ACTION
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -788,17 +788,17 @@ Enable/disable dynamic gyro notch also known as Matrix Filter
|
||||||
|
|
||||||
| Default | Min | Max |
|
| Default | Min | Max |
|
||||||
| --- | --- | --- |
|
| --- | --- | --- |
|
||||||
| OFF | | |
|
| ON | | |
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
### dynamic_gyro_notch_min_hz
|
### dynamic_gyro_notch_min_hz
|
||||||
|
|
||||||
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`
|
Minimum frequency for dynamic notches. Default value of `150` works best with 5" multirotors. Should be lowered with increased size of propellers. Values around `100` work fine on 7" drones. 10" can go down to `60` - `70`
|
||||||
|
|
||||||
| Default | Min | Max |
|
| Default | Min | Max |
|
||||||
| --- | --- | --- |
|
| --- | --- | --- |
|
||||||
| 150 | 30 | 1000 |
|
| 50 | 30 | 1000 |
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
|
|
@ -812,16 +812,6 @@ Q factor for dynamic notches
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
### dynamic_gyro_notch_range
|
|
||||||
|
|
||||||
Range for dynamic gyro notches. `MEDIUM` for 5", `HIGH` for 3" and `MEDIUM`/`LOW` for 7" and bigger propellers
|
|
||||||
|
|
||||||
| Default | Min | Max |
|
|
||||||
| --- | --- | --- |
|
|
||||||
| MEDIUM | | |
|
|
||||||
|
|
||||||
---
|
|
||||||
|
|
||||||
### eleres_freq
|
### eleres_freq
|
||||||
|
|
||||||
_// TODO_
|
_// TODO_
|
||||||
|
|
@ -1542,36 +1532,6 @@ Enable use of Galileo satellites. This is at the expense of other regional const
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
### gyro_abg_alpha
|
|
||||||
|
|
||||||
Alpha factor for Gyro Alpha-Beta-Gamma filter
|
|
||||||
|
|
||||||
| Default | Min | Max |
|
|
||||||
| --- | --- | --- |
|
|
||||||
| 0 | 0 | 1 |
|
|
||||||
|
|
||||||
---
|
|
||||||
|
|
||||||
### gyro_abg_boost
|
|
||||||
|
|
||||||
Boost factor for Gyro Alpha-Beta-Gamma filter
|
|
||||||
|
|
||||||
| Default | Min | Max |
|
|
||||||
| --- | --- | --- |
|
|
||||||
| 0.35 | 0 | 2 |
|
|
||||||
|
|
||||||
---
|
|
||||||
|
|
||||||
### gyro_abg_half_life
|
|
||||||
|
|
||||||
Sample half-life for Gyro Alpha-Beta-Gamma filter
|
|
||||||
|
|
||||||
| Default | Min | Max |
|
|
||||||
| --- | --- | --- |
|
|
||||||
| 0.5 | 0 | 10 |
|
|
||||||
|
|
||||||
---
|
|
||||||
|
|
||||||
### gyro_anti_aliasing_lpf_hz
|
### gyro_anti_aliasing_lpf_hz
|
||||||
|
|
||||||
Gyro processing anti-aliasing filter cutoff frequency. In normal operation this filter setting should never be changed. In Hz
|
Gyro processing anti-aliasing filter cutoff frequency. In normal operation this filter setting should never be changed. In Hz
|
||||||
|
|
@ -2478,7 +2438,7 @@ Exposition value used for the PITCH/ROLL axes by the `MANUAL` flight mode [0-100
|
||||||
|
|
||||||
| Default | Min | Max |
|
| Default | Min | Max |
|
||||||
| --- | --- | --- |
|
| --- | --- | --- |
|
||||||
| 70 | 0 | 100 |
|
| 35 | 0 | 100 |
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
|
|
@ -2924,7 +2884,7 @@ Maximum climb/descent rate that UAV is allowed to reach during navigation modes.
|
||||||
|
|
||||||
### nav_auto_speed
|
### nav_auto_speed
|
||||||
|
|
||||||
Maximum velocity firmware is allowed in full auto modes (RTH, WP) [cm/s] [Multirotor only]
|
Speed in fully autonomous modes (RTH, WP) [cm/s]. Used for WP mode when no specific WP speed set. [Multirotor only]
|
||||||
|
|
||||||
| Default | Min | Max |
|
| Default | Min | Max |
|
||||||
| --- | --- | --- |
|
| --- | --- | --- |
|
||||||
|
|
@ -3424,7 +3384,7 @@ Maximum climb/descent rate firmware is allowed when processing pilot input for A
|
||||||
|
|
||||||
### nav_manual_speed
|
### nav_manual_speed
|
||||||
|
|
||||||
Maximum velocity firmware is allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only]
|
Maximum speed allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only]
|
||||||
|
|
||||||
| Default | Min | Max |
|
| Default | Min | Max |
|
||||||
| --- | --- | --- |
|
| --- | --- | --- |
|
||||||
|
|
@ -3442,6 +3402,16 @@ Max allowed altitude (above Home Point) that applies to all NAV modes (including
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
|
### nav_max_auto_speed
|
||||||
|
|
||||||
|
Maximum speed allowed in fully autonomous modes (RTH, WP) [cm/s] [Multirotor only]
|
||||||
|
|
||||||
|
| Default | Min | Max |
|
||||||
|
| --- | --- | --- |
|
||||||
|
| 1000 | 10 | 2000 |
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
### nav_max_terrain_follow_alt
|
### nav_max_terrain_follow_alt
|
||||||
|
|
||||||
Max allowed above the ground altitude for terrain following mode
|
Max allowed above the ground altitude for terrain following mode
|
||||||
|
|
@ -4142,6 +4112,16 @@ Value above which to make the OSD distance from home indicator blink (meters)
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
|
### osd_esc_rpm_precision
|
||||||
|
|
||||||
|
Number of characters used to display the RPM value.
|
||||||
|
|
||||||
|
| Default | Min | Max |
|
||||||
|
| --- | --- | --- |
|
||||||
|
| 3 | 3 | 6 |
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
### osd_esc_temp_alarm_max
|
### osd_esc_temp_alarm_max
|
||||||
|
|
||||||
Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade)
|
Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade)
|
||||||
|
|
@ -5208,7 +5188,7 @@ Enable Kalman filter on the gyro data
|
||||||
|
|
||||||
| Default | Min | Max |
|
| Default | Min | Max |
|
||||||
| --- | --- | --- |
|
| --- | --- | --- |
|
||||||
| OFF | | |
|
| ON | | |
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -71,7 +71,7 @@ Airplanes
|
||||||
|
|
||||||
Buzzer is supported with additional switching MOSFET transistor when connected to PWM Output #9.
|
Buzzer is supported with additional switching MOSFET transistor when connected to PWM Output #9.
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
## SD Card Detection
|
## SD Card Detection
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -9,4 +9,4 @@ BetFPVF722 does not have I2C pads broken out. I2C is shared with UART3
|
||||||
|
|
||||||
> I2C and UART3 can not be used at the same time! Connect the serial receiver to a different serial port when I2C device like MAG is used.
|
> I2C and UART3 can not be used at the same time! Connect the serial receiver to a different serial port when I2C device like MAG is used.
|
||||||
|
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -155,14 +155,14 @@ This board allows for single **SoftwareSerial** port on small soldering pads loc
|
||||||
Please note that this is *not* the motor PWM5/PWM6 pins, but small surface mount pads CH5/CH6.
|
Please note that this is *not* the motor PWM5/PWM6 pins, but small surface mount pads CH5/CH6.
|
||||||
|
|
||||||
### Omnibus F4 Pro SoftwareSerial Connections
|
### Omnibus F4 Pro SoftwareSerial Connections
|
||||||
|
|
||||||
|
|
||||||
| Pad | SoftwareSerial Role |
|
| Pad | SoftwareSerial Role |
|
||||||
| ---- | ---- |
|
| ---- | ---- |
|
||||||
| CH5 | RX |
|
| CH5 | RX |
|
||||||
| CH6 | TX |
|
| CH6 | TX |
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
### Omnibus F4 v3/v4/v5 SoftwareSerial Connections
|
### Omnibus F4 v3/v4/v5 SoftwareSerial Connections
|
||||||
|
|
||||||
|
|
@ -181,22 +181,22 @@ Following diagrams applies to _Pro_ version with integrated current meter and JS
|
||||||
|
|
||||||
## Board layout
|
## Board layout
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
## Flying wing motor and servos
|
## Flying wing motor and servos
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
## RX setup
|
## RX setup
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
## FPV setup
|
## FPV setup
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
## GPS setup
|
## GPS setup
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
_Diagrams created by Albert Kravcov (skaman82)_
|
_Diagrams created by Albert Kravcov (skaman82)_
|
||||||
|
|
|
||||||
|
|
@ -1,51 +0,0 @@
|
||||||
# Atomic Barrier Warning
|
|
||||||
|
|
||||||
If GCC is upgraded and a warning appears when compiling then the generated asm source must be verified.
|
|
||||||
|
|
||||||
e.g.
|
|
||||||
```
|
|
||||||
%% serial_softserial.c
|
|
||||||
warning "Please verify that ATOMIC_BARRIER works as intended"
|
|
||||||
```
|
|
||||||
|
|
||||||
To perform the verification, proceed as per discusson on issue #167 which reads:
|
|
||||||
|
|
||||||
I hope it's enough to check that optimized-away variable still has cleanup code at end of scope.
|
|
||||||
|
|
||||||
```
|
|
||||||
static int markme=0;
|
|
||||||
markme++;
|
|
||||||
ATOMIC_BLOCK_NB(0xff) {
|
|
||||||
ATOMIC_BARRIER(markme);
|
|
||||||
markme++;
|
|
||||||
};
|
|
||||||
markme++;
|
|
||||||
```
|
|
||||||
|
|
||||||
pass `-save-temps=obj` (or `-save-temps=cwd`, but lots of files will end up in same directory as makefile) to gcc link step (LTO is in use), find resulting `*.ltrans*.ltrans.s` (grep for `markme`, on linux it ends up in `/tmp`) and check that generated assembly sequence is:
|
|
||||||
|
|
||||||
```
|
|
||||||
MSR basepri_max, r3
|
|
||||||
# (possibly markme address load)
|
|
||||||
# barier (markme) start
|
|
||||||
|
|
||||||
# (increment markme, load and store to memory)
|
|
||||||
ldr r2, [r3]
|
|
||||||
adds r0, r2, #1
|
|
||||||
str r0, [r3]
|
|
||||||
|
|
||||||
# barier(markme) end
|
|
||||||
MSR basepri, r3
|
|
||||||
|
|
||||||
# (markme value should be cached in register on next increment)
|
|
||||||
```
|
|
||||||
|
|
||||||
The # barrier(markme) must surround access code and must be inside MSR basepri instructions ..
|
|
||||||
|
|
||||||
Similar approach is used for ATOMIC_BLOCK in avr libraries, so gcc should not break this behavior.
|
|
||||||
|
|
||||||
IMO attribute(cleanup) and asm volatile is defined in a way that should guarantee this.
|
|
||||||
|
|
||||||
attribute(cleanup) is probably safer way to implement atomic sections - another possibility is to explicitly place barriers in code, but that can (and will eventually) lead to missed barrier/basepri restore on same path creating very hard to find bug.
|
|
||||||
|
|
||||||
The MEMORY_BARRIER() code can be omitted and use ATOMIC_BLOCK with full memory barriers, but IMO it is better to explicitly state what memory is protected by barrier and gcc can use this knowledge to greatly improve generated code in future.
|
|
||||||
|
|
@ -148,7 +148,17 @@ If you see your Linux distribution is using WSL 2, this is the problem. WSL 2 is
|
||||||
1. Put your files on the Linux file system
|
1. Put your files on the Linux file system
|
||||||
2. Change to WSL 1
|
2. Change to WSL 1
|
||||||
|
|
||||||
I opted for the latter. To do this, in the elevated PowerShell window, you can see the name of your distro. Mine is **Ubuntu-20.04**, so I'll use that in this example. Simply type
|
#### Using the Linux file system (recommended)
|
||||||
|
To use the Linux file system, make sure the distro is running. Open File Explorer and navigate to `\\wsl$`. In that path you will find your distros listed. At this point, map a network drive to your distro. Inside the distro, you can find your home directory at `/home/~username~/`. Create your GitHub folders here.
|
||||||
|
|
||||||
|
If after this you have problems with writing to the directories from within VSCode. Open the application for your distro and type
|
||||||
|
```
|
||||||
|
sudo chown -R ~username~ GitHub
|
||||||
|
```
|
||||||
|
`~Username~` is your root distro user that you created and `GitHub` should be the root folder for your GitHub repositories.
|
||||||
|
|
||||||
|
#### To switch back to WSL 1
|
||||||
|
To do this, in the elevated PowerShell window, you can see the name of your distro. Mine is **Ubuntu-20.04**, so I'll use that in this example. Simply type
|
||||||
```
|
```
|
||||||
wsl --set-version Ubuntu-20.04 1
|
wsl --set-version Ubuntu-20.04 1
|
||||||
```
|
```
|
||||||
|
|
|
||||||
|
|
@ -1,80 +0,0 @@
|
||||||
# General Info
|
|
||||||
|
|
||||||
This is a guide on how to use Windows MSYS2 distribution and building platform to build iNav firmware. This environment is very simple to manage and does not require installing docker for Windows which may get in the way of VMWare or any other virtualization software you already have running for other reasons. Another benefit of this approach is that the compiler runs natively on Windows, so performance is much better than compiling in a virtual environment or a container. You can also integrate with whatever IDE you are using to make code edits and work with github, which makes the entire development and testing workflow a lot more efficient. In addition to MSYS2, this build environment also uses Arm Embedded GCC tolkit from The xPack Project, which provides many benefits over the toolkits maintained by arm.com
|
|
||||||
|
|
||||||
Some of those benefits are described here:
|
|
||||||
|
|
||||||
https://xpack.github.io/arm-none-eabi-gcc/
|
|
||||||
|
|
||||||
## Setting up build environment
|
|
||||||
|
|
||||||
Download MSYS2 for your architecture (most likely 64-bit)
|
|
||||||
|
|
||||||
https://www.msys2.org/wiki/MSYS2-installation/
|
|
||||||
|
|
||||||
Click on 64-bit, scroll all the way down for the latest release
|
|
||||||
|
|
||||||
pacman is the package manager which makes it a lot easier to install and maintain all the dependencies
|
|
||||||
|
|
||||||
## Installing dependencies
|
|
||||||
|
|
||||||
Once MSYS2 is installed, you can open up a new terminal window by running:
|
|
||||||
|
|
||||||
"C:\msys64\mingw64.exe"
|
|
||||||
|
|
||||||
You can also make a shortcut of this somewhere on your taskbar, desktop, etc, or even setup a shortcut key to open it up every time you need to get a terminal window. If you right click on the window you can customize the font and layout to make it more comfortable to work with. This is very similar to cygwin or any other terminal program you might have used before
|
|
||||||
|
|
||||||
This is the best part:
|
|
||||||
```
|
|
||||||
pacman -S git ruby make cmake gcc mingw-w64-x86_64-libwinpthread-git unzip wget
|
|
||||||
```
|
|
||||||
|
|
||||||
Now, each release needs a different version of arm toolchain. To setup the xPack ARM toolchain, use the following process:
|
|
||||||
|
|
||||||
First, setup your work path, get the release you want to build or master if you want the latest/greatest
|
|
||||||
```
|
|
||||||
mkdir /c/Workspace
|
|
||||||
cd /c/Workspace
|
|
||||||
# you can also check out your own fork here which makes contributing easier
|
|
||||||
git clone https://github.com/iNavFlight/inav
|
|
||||||
cd inav
|
|
||||||
# switch to release you want or skip next 2 lines if you want latest
|
|
||||||
git fetch origin
|
|
||||||
git checkout -b release_2.6.1 origin/release_2.6.1
|
|
||||||
```
|
|
||||||
Now create the build and xpack directories and get the toolkit version you need for your inav version
|
|
||||||
```
|
|
||||||
mkdir build
|
|
||||||
cd build
|
|
||||||
mkdir /c/Workspace/xpack
|
|
||||||
cd /c/Workspace/xpack
|
|
||||||
cat /c/Workspace/inav/cmake/arm-none-eabi-checks.cmake | grep "set(arm_none_eabi_gcc_version" | cut -d\" -f2
|
|
||||||
```
|
|
||||||
This will give you the version you need for any given release or master branch. You can get to all the releases here and find the version you need
|
|
||||||
|
|
||||||
https://github.com/xpack-dev-tools/arm-none-eabi-gcc-xpack/releases/
|
|
||||||
```
|
|
||||||
# for version 2.6.1, version needed is 9.2.1
|
|
||||||
wget https://github.com/xpack-dev-tools/arm-none-eabi-gcc-xpack/releases/download/v9.2.1-1.1/xpack-arm-none-eabi-gcc-9.2.1-1.1-win32-x64.zip
|
|
||||||
unzip xpack-arm-none-eabi-gcc-9.2.1-1.1-win32-x64.zip
|
|
||||||
```
|
|
||||||
This is important, put the toolkit first before your path so that it is picked up ahead of any other versions that may be present on your system
|
|
||||||
```
|
|
||||||
export PATH=/c/Workspace/xpack/xpack-arm-none-eabi-gcc-9.2.1-1.1/bin:$PATH
|
|
||||||
cd /c/Workspace/inav/build
|
|
||||||
```
|
|
||||||
You may need to run rm -rf * in build directory if you had any failed previous runs now run cmake
|
|
||||||
```
|
|
||||||
# while inside the build directory
|
|
||||||
cmake ..
|
|
||||||
```
|
|
||||||
Once that's done you can compile the firmware for your controller
|
|
||||||
```
|
|
||||||
make DALRCF405
|
|
||||||
```
|
|
||||||
To get a list of available targets in iNav, see the target src folder
|
|
||||||
https://github.com/tednv/inav/tree/master/src/main/target
|
|
||||||
|
|
||||||
The generated hex file will be in /c/Workspace/inav/build folder
|
|
||||||
|
|
||||||
At the time of writting this document, I believe this is the fastest, easiest, and most efficient Windows build environment that is available. I have used this approach several years ago and was very happy with it building iNav 2.1 and 2.2, and now I'm getting back into it so figured I would share my method
|
|
||||||
|
|
@ -161,6 +161,8 @@ main_sources(COMMON_SRC
|
||||||
drivers/compass/compass_rm3100.h
|
drivers/compass/compass_rm3100.h
|
||||||
drivers/compass/compass_vcm5883.c
|
drivers/compass/compass_vcm5883.c
|
||||||
drivers/compass/compass_vcm5883.h
|
drivers/compass/compass_vcm5883.h
|
||||||
|
drivers/compass/compass_mlx90393.c
|
||||||
|
drivers/compass/compass_mlx90393.h
|
||||||
drivers/compass/compass_msp.c
|
drivers/compass/compass_msp.c
|
||||||
drivers/compass/compass_msp.h
|
drivers/compass/compass_msp.h
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -1809,7 +1809,6 @@ static bool blackboxWriteSysinfo(void)
|
||||||
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_hz", "%d", gyroConfig()->gyro_main_lpf_hz);
|
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_hz", "%d", gyroConfig()->gyro_main_lpf_hz);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_type", "%d", gyroConfig()->gyro_main_lpf_type);
|
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_type", "%d", gyroConfig()->gyro_main_lpf_type);
|
||||||
#ifdef USE_DYNAMIC_FILTERS
|
#ifdef USE_DYNAMIC_FILTERS
|
||||||
BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchRange", "%d", gyroConfig()->dynamicGyroNotchRange);
|
|
||||||
BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchQ", "%d", gyroConfig()->dynamicGyroNotchQ);
|
BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchQ", "%d", gyroConfig()->dynamicGyroNotchQ);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchMinHz", "%d", gyroConfig()->dynamicGyroNotchMinHz);
|
BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchMinHz", "%d", gyroConfig()->dynamicGyroNotchMinHz);
|
||||||
#endif
|
#endif
|
||||||
|
|
|
||||||
|
|
@ -82,7 +82,6 @@ typedef enum {
|
||||||
DEBUG_AUTOLEVEL,
|
DEBUG_AUTOLEVEL,
|
||||||
DEBUG_IMU2,
|
DEBUG_IMU2,
|
||||||
DEBUG_ALTITUDE,
|
DEBUG_ALTITUDE,
|
||||||
DEBUG_GYRO_ALPHA_BETA_GAMMA,
|
|
||||||
DEBUG_SMITH_PREDICTOR,
|
DEBUG_SMITH_PREDICTOR,
|
||||||
DEBUG_AUTOTRIM,
|
DEBUG_AUTOTRIM,
|
||||||
DEBUG_AUTOTUNE,
|
DEBUG_AUTOTUNE,
|
||||||
|
|
|
||||||
|
|
@ -44,7 +44,8 @@ static const OSD_Entry cmsx_menuNavSettingsEntries[] =
|
||||||
OSD_LABEL_ENTRY("-- BASIC SETTINGS --"),
|
OSD_LABEL_ENTRY("-- BASIC SETTINGS --"),
|
||||||
|
|
||||||
OSD_SETTING_ENTRY("CONTROL MODE", SETTING_NAV_USER_CONTROL_MODE),
|
OSD_SETTING_ENTRY("CONTROL MODE", SETTING_NAV_USER_CONTROL_MODE),
|
||||||
OSD_SETTING_ENTRY("MAX NAV SPEED", SETTING_NAV_AUTO_SPEED),
|
OSD_SETTING_ENTRY("MC NAV SPEED", SETTING_NAV_AUTO_SPEED),
|
||||||
|
OSD_SETTING_ENTRY("MC MAX NAV SPEED", SETTING_NAV_MAX_AUTO_SPEED),
|
||||||
OSD_SETTING_ENTRY("MAX CRUISE SPEED", SETTING_NAV_MANUAL_SPEED),
|
OSD_SETTING_ENTRY("MAX CRUISE SPEED", SETTING_NAV_MANUAL_SPEED),
|
||||||
OSD_SETTING_ENTRY("MAX NAV CLIMB RATE", SETTING_NAV_AUTO_CLIMB_RATE),
|
OSD_SETTING_ENTRY("MAX NAV CLIMB RATE", SETTING_NAV_AUTO_CLIMB_RATE),
|
||||||
OSD_SETTING_ENTRY("MAX AH CLIMB RATE", SETTING_NAV_MANUAL_CLIMB_RATE),
|
OSD_SETTING_ENTRY("MAX AH CLIMB RATE", SETTING_NAV_MANUAL_CLIMB_RATE),
|
||||||
|
|
|
||||||
|
|
@ -317,72 +317,6 @@ FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint
|
||||||
filter->y2 = y2;
|
filter->y2 = y2;
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifdef USE_ALPHA_BETA_GAMMA_FILTER
|
|
||||||
void alphaBetaGammaFilterInit(alphaBetaGammaFilter_t *filter, float alpha, float boostGain, float halfLife, float dT) {
|
|
||||||
// beta, gamma, and eta gains all derived from
|
|
||||||
// http://yadda.icm.edu.pl/yadda/element/bwmeta1.element.baztech-922ff6cb-e991-417f-93f0-77448f1ef4ec/c/A_Study_Jeong_1_2017.pdf
|
|
||||||
|
|
||||||
const float xi = powf(-alpha + 1.0f, 0.25); // fourth rool of -a + 1
|
|
||||||
filter->xk = 0.0f;
|
|
||||||
filter->vk = 0.0f;
|
|
||||||
filter->ak = 0.0f;
|
|
||||||
filter->jk = 0.0f;
|
|
||||||
filter->a = alpha;
|
|
||||||
filter->b = (1.0f / 6.0f) * powf(1.0f - xi, 2) * (11.0f + 14.0f * xi + 11 * xi * xi);
|
|
||||||
filter->g = 2 * powf(1.0f - xi, 3) * (1 + xi);
|
|
||||||
filter->e = (1.0f / 6.0f) * powf(1 - xi, 4);
|
|
||||||
filter->dT = dT;
|
|
||||||
filter->dT2 = dT * dT;
|
|
||||||
filter->dT3 = dT * dT * dT;
|
|
||||||
pt1FilterInit(&filter->boostFilter, 100, dT);
|
|
||||||
|
|
||||||
const float boost = boostGain * 100;
|
|
||||||
|
|
||||||
filter->boost = (boost * boost / 10000) * 0.003;
|
|
||||||
filter->halfLife = halfLife != 0 ? powf(0.5f, dT / halfLife): 1.0f;
|
|
||||||
}
|
|
||||||
|
|
||||||
FAST_CODE float alphaBetaGammaFilterApply(alphaBetaGammaFilter_t *filter, float input) {
|
|
||||||
//xk - current system state (ie: position)
|
|
||||||
//vk - derivative of system state (ie: velocity)
|
|
||||||
//ak - derivative of system velociy (ie: acceleration)
|
|
||||||
//jk - derivative of system acceleration (ie: jerk)
|
|
||||||
float rk; // residual error
|
|
||||||
|
|
||||||
// give the filter limited history
|
|
||||||
filter->xk *= filter->halfLife;
|
|
||||||
filter->vk *= filter->halfLife;
|
|
||||||
filter->ak *= filter->halfLife;
|
|
||||||
filter->jk *= filter->halfLife;
|
|
||||||
|
|
||||||
// update our (estimated) state 'x' from the system (ie pos = pos + vel (last).dT)
|
|
||||||
filter->xk += filter->dT * filter->vk + (1.0f / 2.0f) * filter->dT2 * filter->ak + (1.0f / 6.0f) * filter->dT3 * filter->jk;
|
|
||||||
|
|
||||||
// update (estimated) velocity (also estimated dterm from measurement)
|
|
||||||
filter->vk += filter->dT * filter->ak + 0.5f * filter->dT2 * filter->jk;
|
|
||||||
filter->ak += filter->dT * filter->jk;
|
|
||||||
|
|
||||||
// what is our residual error (measured - estimated)
|
|
||||||
rk = input - filter->xk;
|
|
||||||
|
|
||||||
// artificially boost the error to increase the response of the filter
|
|
||||||
rk += pt1FilterApply(&filter->boostFilter, fabsf(rk) * rk * filter->boost);
|
|
||||||
if ((fabsf(rk * filter->a) > fabsf(input - filter->xk))) {
|
|
||||||
rk = (input - filter->xk) / filter->a;
|
|
||||||
}
|
|
||||||
filter->rk = rk; // for logging
|
|
||||||
|
|
||||||
// update our estimates given the residual error.
|
|
||||||
filter->xk += filter->a * rk;
|
|
||||||
filter->vk += filter->b / filter->dT * rk;
|
|
||||||
filter->ak += filter->g / (2.0f * filter->dT2) * rk;
|
|
||||||
filter->jk += filter->e / (6.0f * filter->dT3) * rk;
|
|
||||||
|
|
||||||
return filter->xk;
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif
|
|
||||||
|
|
||||||
FUNCTION_COMPILE_FOR_SIZE
|
FUNCTION_COMPILE_FOR_SIZE
|
||||||
void initFilter(const uint8_t filterType, filter_t *filter, const float cutoffFrequency, const uint32_t refreshRate) {
|
void initFilter(const uint8_t filterType, filter_t *filter, const float cutoffFrequency, const uint32_t refreshRate) {
|
||||||
const float dT = refreshRate * 1e-6f;
|
const float dT = refreshRate * 1e-6f;
|
||||||
|
|
|
||||||
|
|
@ -283,6 +283,21 @@ static bool bmi270AccReadScratchpad(accDev_t *acc)
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
static bool bmi270TemperatureRead(gyroDev_t *gyro, int16_t * data)
|
||||||
|
{
|
||||||
|
uint8_t buffer[3];
|
||||||
|
|
||||||
|
bool readStatus = busReadBuf(gyro->busDev, BMI270_REG_TEMPERATURE_LSB, &buffer[0], sizeof(buffer));
|
||||||
|
|
||||||
|
if (readStatus) {
|
||||||
|
// Convert to degC*10: degC = raw / 512 + 23
|
||||||
|
*data = 230 + ((10 * (int32_t)((int16_t)((buffer[2] << 8) | buffer[1]))) >> 9);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
static void bmi270GyroInit(gyroDev_t *gyro)
|
static void bmi270GyroInit(gyroDev_t *gyro)
|
||||||
{
|
{
|
||||||
bmi270AccAndGyroInit(gyro);
|
bmi270AccAndGyroInit(gyro);
|
||||||
|
|
@ -331,6 +346,7 @@ bool bmi270GyroDetect(gyroDev_t *gyro)
|
||||||
|
|
||||||
gyro->initFn = bmi270GyroInit;
|
gyro->initFn = bmi270GyroInit;
|
||||||
gyro->readFn = bmi270yroReadScratchpad;
|
gyro->readFn = bmi270yroReadScratchpad;
|
||||||
|
gyro->temperatureFn = bmi270TemperatureRead;
|
||||||
gyro->intStatusFn = gyroCheckDataReady;
|
gyro->intStatusFn = gyroCheckDataReady;
|
||||||
gyro->scale = 1.0f / 16.4f; // 2000 dps
|
gyro->scale = 1.0f / 16.4f; // 2000 dps
|
||||||
return true;
|
return true;
|
||||||
|
|
|
||||||
|
|
@ -118,6 +118,7 @@ typedef enum {
|
||||||
DEVHW_LIS3MDL,
|
DEVHW_LIS3MDL,
|
||||||
DEVHW_RM3100,
|
DEVHW_RM3100,
|
||||||
DEVHW_VCM5883,
|
DEVHW_VCM5883,
|
||||||
|
DEVHW_MLX90393,
|
||||||
|
|
||||||
/* Temp sensor chips */
|
/* Temp sensor chips */
|
||||||
DEVHW_LM75_0,
|
DEVHW_LM75_0,
|
||||||
|
|
|
||||||
176
src/main/drivers/compass/compass_mlx90393.c
Normal file
176
src/main/drivers/compass/compass_mlx90393.c
Normal file
|
|
@ -0,0 +1,176 @@
|
||||||
|
/*
|
||||||
|
* This file is part of iNav.
|
||||||
|
*
|
||||||
|
* iNav is free software: you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation, either version 3 of the License, or
|
||||||
|
* (at your option) any later version.
|
||||||
|
*
|
||||||
|
* iNav is distributed in the hope that it will be useful,
|
||||||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
* GNU General Public License for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License
|
||||||
|
* along with iNav. If not, see <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "platform.h"
|
||||||
|
|
||||||
|
#ifdef USE_MAG_MLX90393
|
||||||
|
|
||||||
|
#include <stdbool.h>
|
||||||
|
#include <stdint.h>
|
||||||
|
|
||||||
|
#include <math.h>
|
||||||
|
|
||||||
|
#include "build/debug.h"
|
||||||
|
|
||||||
|
#include "common/axis.h"
|
||||||
|
#include "common/maths.h"
|
||||||
|
#include "common/utils.h"
|
||||||
|
|
||||||
|
#include "drivers/time.h"
|
||||||
|
#include "drivers/io.h"
|
||||||
|
#include "drivers/bus.h"
|
||||||
|
|
||||||
|
#include "drivers/sensor.h"
|
||||||
|
#include "drivers/compass/compass.h"
|
||||||
|
|
||||||
|
#define MLX90393_MEASURE_3D 0b00001110
|
||||||
|
|
||||||
|
#define MLX90393_NOP 0b00000000
|
||||||
|
#define MLX90393_START_BURST_MODE 0b00010000 //uses with zyxt flags
|
||||||
|
#define MLX90393_START_WAKE_UP_ON_CHANGE_MODE 0b00100000 //uses with zyxt flags
|
||||||
|
#define MLX90393_START_SINGLE_MEASUREMENT_MODE 0b00110000 //uses with zyxt flags
|
||||||
|
#define MLX90393_READ_MEASUREMENT 0b01000000 //uses with zyxt flags
|
||||||
|
#define MLX90393_READ_REGISTER 0b01010000
|
||||||
|
#define MLX90393_WRITE_REGISTER 0b01100000
|
||||||
|
#define MLX90393_EXIT_MODE 0b10000000
|
||||||
|
#define MLX90393_MEMORY_RECALL 0b11010000
|
||||||
|
#define MLX90393_MEMORY_STORE 0b11100000
|
||||||
|
#define MLX90393_RESET 0b11110000
|
||||||
|
|
||||||
|
#define MLX90393_BURST_MODE_FLAG 0b10000000
|
||||||
|
#define MLX90393_WAKE_UP_ON_CHANGE_MODE_FLAG 0b01000000
|
||||||
|
#define MLX90393_SINGLE_MEASUREMENT_MODE_FLAG 0b00100000
|
||||||
|
#define MLX90393_ERROR_FLAG 0b00010000
|
||||||
|
#define MLX90393_SINGLE_ERROR_DETECTION_FLAG 0b00001000
|
||||||
|
#define MLX90393_RESET_FLAG 0b00000100
|
||||||
|
#define MLX90393_D1_FLAG 0b00000010
|
||||||
|
#define MLX90393_D0_FLAG 0b00000001
|
||||||
|
|
||||||
|
#define DETECTION_MAX_RETRY_COUNT 5
|
||||||
|
|
||||||
|
#define REG_BUF_LEN 3
|
||||||
|
|
||||||
|
// Register 1
|
||||||
|
#define GAIN_SEL_HALLCONF_REG 0x0 //from datasheet 0x0 << 2 = 0x0
|
||||||
|
// GAIN - 0b111
|
||||||
|
// Hall_conf - 0xC
|
||||||
|
#define GAIN_SEL_HALLCONF_REG_VALUE 0x007C
|
||||||
|
// Register 2
|
||||||
|
#define TCMP_EN_REG 0x4 //from datasheet 0x1 << 2 = 0x4
|
||||||
|
// Burst Data Rate 0b000100
|
||||||
|
#define TCMP_EN_REG_VALUE 0x62C4
|
||||||
|
// Register 3
|
||||||
|
#define RES_XYZ_OSR_FLT_REG 0x8 //from datasheet 0x2 << 2 = 0x8
|
||||||
|
// Oversampling 0b01
|
||||||
|
// Filtering 0b111
|
||||||
|
#define RES_XYZ_OSR_FLT_REG_VALUE 0x001D
|
||||||
|
|
||||||
|
|
||||||
|
static void mlx90393WriteRegister(magDev_t * mag, uint8_t reg_val, uint16_t value) {
|
||||||
|
|
||||||
|
uint8_t buf[REG_BUF_LEN] = {0};
|
||||||
|
|
||||||
|
buf[0] = (value >> 8) & 0xFF;
|
||||||
|
buf[1] = (value >> 0) & 0xFF;
|
||||||
|
buf[2] = reg_val;
|
||||||
|
|
||||||
|
busWriteBuf(mag->busDev, MLX90393_WRITE_REGISTER, buf, REG_BUF_LEN);
|
||||||
|
|
||||||
|
// PAUSE
|
||||||
|
delay(20);
|
||||||
|
// To skip ACK FLAG of Write
|
||||||
|
uint8_t sig = 0;
|
||||||
|
busRead(mag->busDev, MLX90393_NOP, &sig);
|
||||||
|
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// =======================================================================================
|
||||||
|
static bool mlx90393Read(magDev_t * mag)
|
||||||
|
{
|
||||||
|
|
||||||
|
uint8_t buf[7] = {0};
|
||||||
|
|
||||||
|
busReadBuf(mag->busDev, MLX90393_READ_MEASUREMENT | MLX90393_MEASURE_3D, buf, 7);
|
||||||
|
|
||||||
|
mag->magADCRaw[X] = ((short)(buf[1] << 8 | buf[2]));
|
||||||
|
mag->magADCRaw[Y] = ((short)(buf[3] << 8 | buf[4]));
|
||||||
|
mag->magADCRaw[Z] = ((short)(buf[5] << 8 | buf[6]));
|
||||||
|
|
||||||
|
return true;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
static bool deviceDetect(magDev_t * mag)
|
||||||
|
{
|
||||||
|
for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) {
|
||||||
|
uint8_t sig = 0;
|
||||||
|
|
||||||
|
bool ack = busRead(mag->busDev, MLX90393_RESET, &sig);
|
||||||
|
delay(20);
|
||||||
|
|
||||||
|
if (ack && ((sig & MLX90393_RESET_FLAG) == MLX90393_RESET_FLAG)) { // Check Reset Flag -> MLX90393
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
//--------------------------------------------------
|
||||||
|
static bool mlx90393Init(magDev_t * mag)
|
||||||
|
{
|
||||||
|
uint8_t sig = 0;
|
||||||
|
|
||||||
|
delay(20);
|
||||||
|
// Remove reset flag
|
||||||
|
busRead(mag->busDev, MLX90393_NOP, &sig);
|
||||||
|
// Exit if already in burst mode. (For example when external i2c source power the bus.)
|
||||||
|
busRead(mag->busDev, MLX90393_EXIT_MODE, &sig);
|
||||||
|
|
||||||
|
// Writing Registers
|
||||||
|
mlx90393WriteRegister(mag, GAIN_SEL_HALLCONF_REG, GAIN_SEL_HALLCONF_REG_VALUE);
|
||||||
|
mlx90393WriteRegister(mag, TCMP_EN_REG, TCMP_EN_REG_VALUE);
|
||||||
|
mlx90393WriteRegister(mag, RES_XYZ_OSR_FLT_REG, RES_XYZ_OSR_FLT_REG_VALUE);
|
||||||
|
|
||||||
|
// Start burst mode
|
||||||
|
busRead(mag->busDev, MLX90393_START_BURST_MODE | MLX90393_MEASURE_3D, &sig);
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
// ==========================================================================
|
||||||
|
|
||||||
|
bool mlx90393Detect(magDev_t * mag)
|
||||||
|
{
|
||||||
|
mag->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_MLX90393, mag->magSensorToUse, OWNER_COMPASS);
|
||||||
|
if (mag->busDev == NULL) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!deviceDetect(mag)) {
|
||||||
|
busDeviceDeInit(mag->busDev);
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
mag->init = mlx90393Init;
|
||||||
|
mag->read = mlx90393Read;
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
#endif
|
||||||
20
src/main/drivers/compass/compass_mlx90393.h
Normal file
20
src/main/drivers/compass/compass_mlx90393.h
Normal file
|
|
@ -0,0 +1,20 @@
|
||||||
|
/*
|
||||||
|
* This file is part of iNav.
|
||||||
|
*
|
||||||
|
* iNav is free software: you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation, either version 3 of the License, or
|
||||||
|
* (at your option) any later version.
|
||||||
|
*
|
||||||
|
* iNav is distributed in the hope that it will be useful,
|
||||||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
* GNU General Public License for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License
|
||||||
|
* along with iNav. If not, see <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
bool mlx90393Detect(magDev_t* mag);
|
||||||
|
|
@ -160,6 +160,7 @@
|
||||||
#define SYM_HEADING_DIVIDED_LINE 0xCC // 204 Heading Graphic
|
#define SYM_HEADING_DIVIDED_LINE 0xCC // 204 Heading Graphic
|
||||||
#define SYM_HEADING_LINE 0xCD // 205 Heading Graphic
|
#define SYM_HEADING_LINE 0xCD // 205 Heading Graphic
|
||||||
#define SYM_MAX 0xCE // 206 MAX symbol
|
#define SYM_MAX 0xCE // 206 MAX symbol
|
||||||
|
#define SYM_PROFILE 0xCF // 207 Profile symbol
|
||||||
|
|
||||||
|
|
||||||
#define SYM_LOGO_START 0x101 // 257 to 280, INAV logo
|
#define SYM_LOGO_START 0x101 // 257 to 280, INAV logo
|
||||||
|
|
|
||||||
|
|
@ -357,7 +357,7 @@ static int getDShotCommandRepeats(dshotCommands_e cmd) {
|
||||||
switch (cmd) {
|
switch (cmd) {
|
||||||
case DSHOT_CMD_SPIN_DIRECTION_NORMAL:
|
case DSHOT_CMD_SPIN_DIRECTION_NORMAL:
|
||||||
case DSHOT_CMD_SPIN_DIRECTION_REVERSED:
|
case DSHOT_CMD_SPIN_DIRECTION_REVERSED:
|
||||||
repeats = 6;
|
repeats = 10;
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
break;
|
break;
|
||||||
|
|
|
||||||
|
|
@ -107,7 +107,7 @@ PG_RESET_TEMPLATE(featureConfig_t, featureConfig,
|
||||||
.enabledFeatures = DEFAULT_FEATURES | COMMON_DEFAULT_FEATURES
|
.enabledFeatures = DEFAULT_FEATURES | COMMON_DEFAULT_FEATURES
|
||||||
);
|
);
|
||||||
|
|
||||||
PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 4);
|
PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 5);
|
||||||
|
|
||||||
PG_RESET_TEMPLATE(systemConfig_t, systemConfig,
|
PG_RESET_TEMPLATE(systemConfig_t, systemConfig,
|
||||||
.current_profile_index = 0,
|
.current_profile_index = 0,
|
||||||
|
|
|
||||||
|
|
@ -589,10 +589,11 @@ void tryArm(void)
|
||||||
|
|
||||||
//beep to indicate arming
|
//beep to indicate arming
|
||||||
#ifdef USE_NAV
|
#ifdef USE_NAV
|
||||||
if (navigationPositionEstimateIsHealthy())
|
if (navigationPositionEstimateIsHealthy()) {
|
||||||
beeper(BEEPER_ARMING_GPS_FIX);
|
beeper(BEEPER_ARMING_GPS_FIX);
|
||||||
else
|
} else {
|
||||||
beeper(BEEPER_ARMING);
|
beeper(BEEPER_ARMING);
|
||||||
|
}
|
||||||
#else
|
#else
|
||||||
beeper(BEEPER_ARMING);
|
beeper(BEEPER_ARMING);
|
||||||
#endif
|
#endif
|
||||||
|
|
@ -614,8 +615,9 @@ void processRx(timeUs_t currentTimeUs)
|
||||||
|
|
||||||
// in 3D mode, we need to be able to disarm by switch at any time
|
// in 3D mode, we need to be able to disarm by switch at any time
|
||||||
if (feature(FEATURE_REVERSIBLE_MOTORS)) {
|
if (feature(FEATURE_REVERSIBLE_MOTORS)) {
|
||||||
if (!IS_RC_MODE_ACTIVE(BOXARM))
|
if (!IS_RC_MODE_ACTIVE(BOXARM)) {
|
||||||
disarm(DISARM_SWITCH_3D);
|
disarm(DISARM_SWITCH_3D);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
updateRSSI(currentTimeUs);
|
updateRSSI(currentTimeUs);
|
||||||
|
|
|
||||||
|
|
@ -13,7 +13,7 @@ tables:
|
||||||
values: ["NONE", "BNO055", "BNO055_SERIAL"]
|
values: ["NONE", "BNO055", "BNO055_SERIAL"]
|
||||||
enum: secondaryImuType_e
|
enum: secondaryImuType_e
|
||||||
- name: mag_hardware
|
- name: mag_hardware
|
||||||
values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", "VCM5883", "FAKE"]
|
values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", "VCM5883", "MLX90393", "FAKE"]
|
||||||
enum: magSensor_e
|
enum: magSensor_e
|
||||||
- name: opflow_hardware
|
- name: opflow_hardware
|
||||||
values: ["NONE", "CXOF", "MSP", "FAKE"]
|
values: ["NONE", "CXOF", "MSP", "FAKE"]
|
||||||
|
|
@ -94,7 +94,7 @@ tables:
|
||||||
"VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
|
"VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
|
||||||
"ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY",
|
"ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY",
|
||||||
"IRLOCK", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "IMU2", "ALTITUDE",
|
"IRLOCK", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "IMU2", "ALTITUDE",
|
||||||
"GYRO_ALPHA_BETA_GAMMA", "SMITH_PREDICTOR", "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS"]
|
"SMITH_PREDICTOR", "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS"]
|
||||||
- name: async_mode
|
- name: async_mode
|
||||||
values: ["NONE", "GYRO", "ALL"]
|
values: ["NONE", "GYRO", "ALL"]
|
||||||
- name: aux_operator
|
- name: aux_operator
|
||||||
|
|
@ -144,9 +144,6 @@ tables:
|
||||||
- name: rssi_source
|
- name: rssi_source
|
||||||
values: ["NONE", "AUTO", "ADC", "CHANNEL", "PROTOCOL", "MSP"]
|
values: ["NONE", "AUTO", "ADC", "CHANNEL", "PROTOCOL", "MSP"]
|
||||||
enum: rssiSource_e
|
enum: rssiSource_e
|
||||||
- name: dynamicFilterRangeTable
|
|
||||||
values: ["HIGH", "MEDIUM", "LOW"]
|
|
||||||
enum: dynamicFilterRange_e
|
|
||||||
- name: pidTypeTable
|
- name: pidTypeTable
|
||||||
values: ["NONE", "PID", "PIFF", "AUTO"]
|
values: ["NONE", "PID", "PIFF", "AUTO"]
|
||||||
enum: pidType_e
|
enum: pidType_e
|
||||||
|
|
@ -280,16 +277,10 @@ groups:
|
||||||
max: 10
|
max: 10
|
||||||
- name: dynamic_gyro_notch_enabled
|
- name: dynamic_gyro_notch_enabled
|
||||||
description: "Enable/disable dynamic gyro notch also known as Matrix Filter"
|
description: "Enable/disable dynamic gyro notch also known as Matrix Filter"
|
||||||
default_value: OFF
|
default_value: ON
|
||||||
field: dynamicGyroNotchEnabled
|
field: dynamicGyroNotchEnabled
|
||||||
condition: USE_DYNAMIC_FILTERS
|
condition: USE_DYNAMIC_FILTERS
|
||||||
type: bool
|
type: bool
|
||||||
- name: dynamic_gyro_notch_range
|
|
||||||
description: "Range for dynamic gyro notches. `MEDIUM` for 5\", `HIGH` for 3\" and `MEDIUM`/`LOW` for 7\" and bigger propellers"
|
|
||||||
default_value: "MEDIUM"
|
|
||||||
field: dynamicGyroNotchRange
|
|
||||||
condition: USE_DYNAMIC_FILTERS
|
|
||||||
table: dynamicFilterRangeTable
|
|
||||||
- name: dynamic_gyro_notch_q
|
- name: dynamic_gyro_notch_q
|
||||||
description: "Q factor for dynamic notches"
|
description: "Q factor for dynamic notches"
|
||||||
default_value: 120
|
default_value: 120
|
||||||
|
|
@ -298,8 +289,8 @@ groups:
|
||||||
min: 1
|
min: 1
|
||||||
max: 1000
|
max: 1000
|
||||||
- name: dynamic_gyro_notch_min_hz
|
- name: dynamic_gyro_notch_min_hz
|
||||||
description: "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`"
|
description: "Minimum frequency for dynamic notches. Default value of `150` works best with 5\" multirotors. Should be lowered with increased size of propellers. Values around `100` work fine on 7\" drones. 10\" can go down to `60` - `70`"
|
||||||
default_value: 150
|
default_value: 50
|
||||||
field: dynamicGyroNotchMinHz
|
field: dynamicGyroNotchMinHz
|
||||||
condition: USE_DYNAMIC_FILTERS
|
condition: USE_DYNAMIC_FILTERS
|
||||||
min: 30
|
min: 30
|
||||||
|
|
@ -309,30 +300,9 @@ groups:
|
||||||
min: 0
|
min: 0
|
||||||
max: 2
|
max: 2
|
||||||
default_value: 0
|
default_value: 0
|
||||||
- name: gyro_abg_alpha
|
|
||||||
description: "Alpha factor for Gyro Alpha-Beta-Gamma filter"
|
|
||||||
default_value: 0
|
|
||||||
field: alphaBetaGammaAlpha
|
|
||||||
condition: USE_ALPHA_BETA_GAMMA_FILTER
|
|
||||||
min: 0
|
|
||||||
max: 1
|
|
||||||
- name: gyro_abg_boost
|
|
||||||
description: "Boost factor for Gyro Alpha-Beta-Gamma filter"
|
|
||||||
default_value: 0.35
|
|
||||||
field: alphaBetaGammaBoost
|
|
||||||
condition: USE_ALPHA_BETA_GAMMA_FILTER
|
|
||||||
min: 0
|
|
||||||
max: 2
|
|
||||||
- name: gyro_abg_half_life
|
|
||||||
description: "Sample half-life for Gyro Alpha-Beta-Gamma filter"
|
|
||||||
default_value: 0.5
|
|
||||||
field: alphaBetaGammaHalfLife
|
|
||||||
condition: USE_ALPHA_BETA_GAMMA_FILTER
|
|
||||||
min: 0
|
|
||||||
max: 10
|
|
||||||
- name: setpoint_kalman_enabled
|
- name: setpoint_kalman_enabled
|
||||||
description: "Enable Kalman filter on the gyro data"
|
description: "Enable Kalman filter on the gyro data"
|
||||||
default_value: OFF
|
default_value: ON
|
||||||
condition: USE_GYRO_KALMAN
|
condition: USE_GYRO_KALMAN
|
||||||
field: kalmanEnabled
|
field: kalmanEnabled
|
||||||
type: bool
|
type: bool
|
||||||
|
|
@ -1401,7 +1371,7 @@ groups:
|
||||||
max: 180
|
max: 180
|
||||||
- name: manual_rc_expo
|
- name: manual_rc_expo
|
||||||
description: "Exposition value used for the PITCH/ROLL axes by the `MANUAL` flight mode [0-100]"
|
description: "Exposition value used for the PITCH/ROLL axes by the `MANUAL` flight mode [0-100]"
|
||||||
default_value: 70
|
default_value: 35
|
||||||
field: manual.rcExpo8
|
field: manual.rcExpo8
|
||||||
min: 0
|
min: 0
|
||||||
max: 100
|
max: 100
|
||||||
|
|
@ -2473,8 +2443,14 @@ groups:
|
||||||
field: general.waypoint_safe_distance
|
field: general.waypoint_safe_distance
|
||||||
max: 65000
|
max: 65000
|
||||||
- name: nav_auto_speed
|
- name: nav_auto_speed
|
||||||
description: "Maximum velocity firmware is allowed in full auto modes (RTH, WP) [cm/s] [Multirotor only]"
|
description: "Speed in fully autonomous modes (RTH, WP) [cm/s]. Used for WP mode when no specific WP speed set. [Multirotor only]"
|
||||||
default_value: 300
|
default_value: 300
|
||||||
|
field: general.auto_speed
|
||||||
|
min: 10
|
||||||
|
max: 2000
|
||||||
|
- name: nav_max_auto_speed
|
||||||
|
description: "Maximum speed allowed in fully autonomous modes (RTH, WP) [cm/s] [Multirotor only]"
|
||||||
|
default_value: 1000
|
||||||
field: general.max_auto_speed
|
field: general.max_auto_speed
|
||||||
min: 10
|
min: 10
|
||||||
max: 2000
|
max: 2000
|
||||||
|
|
@ -2485,7 +2461,7 @@ groups:
|
||||||
min: 10
|
min: 10
|
||||||
max: 2000
|
max: 2000
|
||||||
- name: nav_manual_speed
|
- name: nav_manual_speed
|
||||||
description: "Maximum velocity firmware is allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only]"
|
description: "Maximum speed allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only]"
|
||||||
default_value: 500
|
default_value: 500
|
||||||
field: general.max_manual_speed
|
field: general.max_manual_speed
|
||||||
min: 10
|
min: 10
|
||||||
|
|
@ -3353,7 +3329,6 @@ groups:
|
||||||
description: "How many navigation waypoints are displayed, set to 0 (zero) to disable. As sample, if set to 2, and you just passed the 3rd waypoint of the mission, you'll see markers for the 4th waypoint (marked 1) and the 5th waypoint (marked 2)"
|
description: "How many navigation waypoints are displayed, set to 0 (zero) to disable. As sample, if set to 2, and you just passed the 3rd waypoint of the mission, you'll see markers for the 4th waypoint (marked 1) and the 5th waypoint (marked 2)"
|
||||||
default_value: 0
|
default_value: 0
|
||||||
field: hud_wp_disp
|
field: hud_wp_disp
|
||||||
|
|
||||||
min: 0
|
min: 0
|
||||||
max: 3
|
max: 3
|
||||||
- name: osd_left_sidebar_scroll
|
- name: osd_left_sidebar_scroll
|
||||||
|
|
@ -3489,6 +3464,13 @@ groups:
|
||||||
min: -36
|
min: -36
|
||||||
max: 36
|
max: 36
|
||||||
|
|
||||||
|
- name: osd_esc_rpm_precision
|
||||||
|
description: Number of characters used to display the RPM value.
|
||||||
|
field: esc_rpm_precision
|
||||||
|
min: 3
|
||||||
|
max: 6
|
||||||
|
default_value: 3
|
||||||
|
|
||||||
- name: PG_OSD_COMMON_CONFIG
|
- name: PG_OSD_COMMON_CONFIG
|
||||||
type: osdCommonConfig_t
|
type: osdCommonConfig_t
|
||||||
headers: ["io/osd_common.h"]
|
headers: ["io/osd_common.h"]
|
||||||
|
|
|
||||||
|
|
@ -378,7 +378,7 @@ static bool failsafeCheckStickMotion(void)
|
||||||
|
|
||||||
static failsafeProcedure_e failsafeChooseFailsafeProcedure(void)
|
static failsafeProcedure_e failsafeChooseFailsafeProcedure(void)
|
||||||
{
|
{
|
||||||
if (FLIGHT_MODE(NAV_WP_MODE) && !failsafeConfig()->failsafe_mission) {
|
if ((FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive()) && !failsafeConfig()->failsafe_mission) {
|
||||||
return FAILSAFE_PROCEDURE_NONE;
|
return FAILSAFE_PROCEDURE_NONE;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -46,6 +46,14 @@ FILE_COMPILE_FOR_SPEED
|
||||||
|
|
||||||
#include "gyroanalyse.h"
|
#include "gyroanalyse.h"
|
||||||
|
|
||||||
|
enum {
|
||||||
|
STEP_ARM_CFFT_F32,
|
||||||
|
STEP_BITREVERSAL_AND_STAGE_RFFT_F32,
|
||||||
|
STEP_MAGNITUDE_AND_FREQUENCY,
|
||||||
|
STEP_UPDATE_FILTERS_AND_HANNING,
|
||||||
|
STEP_COUNT
|
||||||
|
};
|
||||||
|
|
||||||
// The FFT splits the frequency domain into an number of bins
|
// The FFT splits the frequency domain into an number of bins
|
||||||
// A sampling frequency of 1000 and max frequency of 500 at a window size of 32 gives 16 frequency bins each 31.25Hz wide
|
// A sampling frequency of 1000 and max frequency of 500 at a window size of 32 gives 16 frequency bins each 31.25Hz wide
|
||||||
// Eg [0,31), [31,62), [62, 93) etc
|
// Eg [0,31), [31,62), [62, 93) etc
|
||||||
|
|
@ -54,62 +62,48 @@ FILE_COMPILE_FOR_SPEED
|
||||||
#define FFT_BIN_COUNT (FFT_WINDOW_SIZE / 2)
|
#define FFT_BIN_COUNT (FFT_WINDOW_SIZE / 2)
|
||||||
// smoothing frequency for FFT centre frequency
|
// smoothing frequency for FFT centre frequency
|
||||||
#define DYN_NOTCH_SMOOTH_FREQ_HZ 50
|
#define DYN_NOTCH_SMOOTH_FREQ_HZ 50
|
||||||
// we need 4 steps for each axis
|
|
||||||
#define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * 4)
|
/*
|
||||||
|
* Slow down gyro sample acquisition. This lowers the max frequency but increases the resolution.
|
||||||
|
* On default 500us looptime and denominator 1, max frequency is 1000Hz with a resolution of 31.25Hz
|
||||||
|
* On default 500us looptime and denominator 2, max frequency is 500Hz with a resolution of 15.6Hz
|
||||||
|
*/
|
||||||
|
#define FFT_SAMPLING_DENOMINATOR 2
|
||||||
|
|
||||||
void gyroDataAnalyseStateInit(
|
void gyroDataAnalyseStateInit(
|
||||||
gyroAnalyseState_t *state,
|
gyroAnalyseState_t *state,
|
||||||
uint16_t minFrequency,
|
uint16_t minFrequency,
|
||||||
uint8_t range,
|
|
||||||
uint32_t targetLooptimeUs
|
uint32_t targetLooptimeUs
|
||||||
) {
|
) {
|
||||||
state->fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_LOW;
|
|
||||||
state->minFrequency = minFrequency;
|
state->minFrequency = minFrequency;
|
||||||
|
|
||||||
if (range == DYN_NOTCH_RANGE_HIGH) {
|
state->fftSamplingRateHz = 1e6f / targetLooptimeUs / FFT_SAMPLING_DENOMINATOR;
|
||||||
state->fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_HIGH;
|
state->maxFrequency = state->fftSamplingRateHz / 2; //max possible frequency is half the sampling rate
|
||||||
}
|
state->fftResolution = (float)state->maxFrequency / FFT_BIN_COUNT;
|
||||||
else if (range == DYN_NOTCH_RANGE_MEDIUM) {
|
|
||||||
state->fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_MEDIUM;
|
|
||||||
}
|
|
||||||
|
|
||||||
// If we get at least 3 samples then use the default FFT sample frequency
|
|
||||||
// otherwise we need to calculate a FFT sample frequency to ensure we get 3 samples (gyro loops < 4K)
|
|
||||||
const int gyroLoopRateHz = lrintf((1.0f / targetLooptimeUs) * 1e6f);
|
|
||||||
|
|
||||||
state->fftSamplingRateHz = MIN((gyroLoopRateHz / 3), state->fftSamplingRateHz);
|
|
||||||
|
|
||||||
state->fftResolution = (float)state->fftSamplingRateHz / FFT_WINDOW_SIZE;
|
|
||||||
|
|
||||||
state->fftStartBin = state->minFrequency / lrintf(state->fftResolution);
|
state->fftStartBin = state->minFrequency / lrintf(state->fftResolution);
|
||||||
|
|
||||||
state->maxFrequency = state->fftSamplingRateHz / 2; //Nyquist
|
|
||||||
|
|
||||||
for (int i = 0; i < FFT_WINDOW_SIZE; i++) {
|
for (int i = 0; i < FFT_WINDOW_SIZE; i++) {
|
||||||
state->hanningWindow[i] = (0.5f - 0.5f * cos_approx(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1)));
|
state->hanningWindow[i] = (0.5f - 0.5f * cos_approx(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1)));
|
||||||
}
|
}
|
||||||
|
|
||||||
const uint16_t samplingFrequency = 1000000 / targetLooptimeUs;
|
|
||||||
state->maxSampleCount = samplingFrequency / state->fftSamplingRateHz;
|
|
||||||
state->maxSampleCountRcp = 1.f / state->maxSampleCount;
|
|
||||||
|
|
||||||
arm_rfft_fast_init_f32(&state->fftInstance, FFT_WINDOW_SIZE);
|
arm_rfft_fast_init_f32(&state->fftInstance, FFT_WINDOW_SIZE);
|
||||||
|
|
||||||
// recalculation of filters takes 4 calls per axis => each filter gets updated every DYN_NOTCH_CALC_TICKS calls
|
// Frequency filter is executed every 12 cycles. 4 steps per cycle, 3 axises
|
||||||
// at 4khz gyro loop rate this means 4khz / 4 / 3 = 333Hz => update every 3ms
|
const uint32_t filterUpdateUs = targetLooptimeUs * STEP_COUNT * XYZ_AXIS_COUNT;
|
||||||
// for gyro rate > 16kHz, we have update frequency of 1kHz => 1ms
|
|
||||||
const float looptime = MAX(1000000u / state->fftSamplingRateHz, targetLooptimeUs * DYN_NOTCH_CALC_TICKS);
|
|
||||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||||
// any init value
|
// any init value
|
||||||
state->centerFreq[axis] = state->maxFrequency;
|
state->centerFreq[axis] = state->maxFrequency;
|
||||||
state->prevCenterFreq[axis] = state->maxFrequency;
|
state->prevCenterFreq[axis] = state->maxFrequency;
|
||||||
biquadFilterInitLPF(&state->detectedFrequencyFilter[axis], DYN_NOTCH_SMOOTH_FREQ_HZ, looptime);
|
|
||||||
|
biquadFilterInitLPF(&state->detectedFrequencyFilter[axis], DYN_NOTCH_SMOOTH_FREQ_HZ, filterUpdateUs);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void gyroDataAnalysePush(gyroAnalyseState_t *state, const int axis, const float sample)
|
void gyroDataAnalysePush(gyroAnalyseState_t *state, const int axis, const float sample)
|
||||||
{
|
{
|
||||||
state->oversampledGyroAccumulator[axis] += sample;
|
state->currentSample[axis] = sample;
|
||||||
}
|
}
|
||||||
|
|
||||||
static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state);
|
static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state);
|
||||||
|
|
@ -121,160 +115,101 @@ void gyroDataAnalyse(gyroAnalyseState_t *state)
|
||||||
{
|
{
|
||||||
state->filterUpdateExecute = false; //This will be changed to true only if new data is present
|
state->filterUpdateExecute = false; //This will be changed to true only if new data is present
|
||||||
|
|
||||||
// samples should have been pushed by `gyroDataAnalysePush`
|
static uint8_t samplingIndex = 0;
|
||||||
// if gyro sampling is > 1kHz, accumulate multiple samples
|
|
||||||
state->sampleCount++;
|
|
||||||
|
|
||||||
// this runs at 1kHz
|
|
||||||
if (state->sampleCount == state->maxSampleCount) {
|
|
||||||
state->sampleCount = 0;
|
|
||||||
|
|
||||||
|
if (samplingIndex == 0) {
|
||||||
// calculate mean value of accumulated samples
|
// calculate mean value of accumulated samples
|
||||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||||
float sample = state->oversampledGyroAccumulator[axis] * state->maxSampleCountRcp;
|
state->downsampledGyroData[axis][state->circularBufferIdx] = state->currentSample[axis];
|
||||||
state->downsampledGyroData[axis][state->circularBufferIdx] = sample;
|
|
||||||
|
|
||||||
state->oversampledGyroAccumulator[axis] = 0;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
state->circularBufferIdx = (state->circularBufferIdx + 1) % FFT_WINDOW_SIZE;
|
state->circularBufferIdx = (state->circularBufferIdx + 1) % FFT_WINDOW_SIZE;
|
||||||
|
|
||||||
// We need DYN_NOTCH_CALC_TICKS tick to update all axis with newly sampled value
|
|
||||||
state->updateTicks = DYN_NOTCH_CALC_TICKS;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// calculate FFT and update filters
|
samplingIndex = (samplingIndex + 1) % FFT_SAMPLING_DENOMINATOR;
|
||||||
if (state->updateTicks > 0) {
|
|
||||||
gyroDataAnalyseUpdate(state);
|
gyroDataAnalyseUpdate(state);
|
||||||
--state->updateTicks;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void stage_rfft_f32(arm_rfft_fast_instance_f32 *S, float32_t *p, float32_t *pOut);
|
void stage_rfft_f32(arm_rfft_fast_instance_f32 *S, float32_t *p, float32_t *pOut);
|
||||||
void arm_cfft_radix8by2_f32(arm_cfft_instance_f32 *S, float32_t *p1);
|
|
||||||
void arm_cfft_radix8by4_f32(arm_cfft_instance_f32 *S, float32_t *p1);
|
void arm_cfft_radix8by4_f32(arm_cfft_instance_f32 *S, float32_t *p1);
|
||||||
void arm_radix8_butterfly_f32(float32_t *pSrc, uint16_t fftLen, const float32_t *pCoef, uint16_t twidCoefModifier);
|
|
||||||
void arm_bitreversal_32(uint32_t *pSrc, const uint16_t bitRevLen, const uint16_t *pBitRevTable);
|
void arm_bitreversal_32(uint32_t *pSrc, const uint16_t bitRevLen, const uint16_t *pBitRevTable);
|
||||||
|
|
||||||
|
static uint8_t findPeakBinIndex(gyroAnalyseState_t *state) {
|
||||||
|
uint8_t peakBinIndex = state->fftStartBin;
|
||||||
|
float peakValue = 0;
|
||||||
|
for (int i = state->fftStartBin; i < FFT_BIN_COUNT; i++) {
|
||||||
|
if (state->fftData[i] > peakValue) {
|
||||||
|
peakValue = state->fftData[i];
|
||||||
|
peakBinIndex = i;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return peakBinIndex;
|
||||||
|
}
|
||||||
|
|
||||||
|
static float computeParabolaMean(gyroAnalyseState_t *state, uint8_t peakBinIndex) {
|
||||||
|
float preciseBin = peakBinIndex;
|
||||||
|
|
||||||
|
// Height of peak bin (y1) and shoulder bins (y0, y2)
|
||||||
|
const float y0 = state->fftData[peakBinIndex - 1];
|
||||||
|
const float y1 = state->fftData[peakBinIndex];
|
||||||
|
const float y2 = state->fftData[peakBinIndex - 1];
|
||||||
|
|
||||||
|
// Estimate true peak position aka. preciseBin (fit parabola y(x) over y0, y1 and y2, solve dy/dx=0 for x)
|
||||||
|
const float denom = 2.0f * (y0 - 2 * y1 + y2);
|
||||||
|
if (denom != 0.0f) {
|
||||||
|
preciseBin += (y0 - y2) / denom;
|
||||||
|
}
|
||||||
|
|
||||||
|
return preciseBin;
|
||||||
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Analyse last gyro data from the last FFT_WINDOW_SIZE milliseconds
|
* Analyse last gyro data from the last FFT_WINDOW_SIZE milliseconds
|
||||||
*/
|
*/
|
||||||
static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
|
static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
|
||||||
{
|
{
|
||||||
enum {
|
|
||||||
STEP_ARM_CFFT_F32,
|
|
||||||
STEP_BITREVERSAL,
|
|
||||||
STEP_STAGE_RFFT_F32,
|
|
||||||
STEP_ARM_CMPLX_MAG_F32,
|
|
||||||
STEP_CALC_FREQUENCIES,
|
|
||||||
STEP_UPDATE_FILTERS,
|
|
||||||
STEP_HANNING,
|
|
||||||
STEP_COUNT
|
|
||||||
};
|
|
||||||
|
|
||||||
arm_cfft_instance_f32 *Sint = &(state->fftInstance.Sint);
|
arm_cfft_instance_f32 *Sint = &(state->fftInstance.Sint);
|
||||||
|
|
||||||
switch (state->updateStep) {
|
switch (state->updateStep) {
|
||||||
case STEP_ARM_CFFT_F32:
|
case STEP_ARM_CFFT_F32:
|
||||||
{
|
{
|
||||||
switch (FFT_BIN_COUNT) {
|
// Important this works only with FFT windows size of 64 elements!
|
||||||
case 16:
|
arm_cfft_radix8by4_f32(Sint, state->fftData);
|
||||||
// 16us
|
|
||||||
arm_cfft_radix8by2_f32(Sint, state->fftData);
|
|
||||||
break;
|
|
||||||
case 32:
|
|
||||||
// 35us
|
|
||||||
arm_cfft_radix8by4_f32(Sint, state->fftData);
|
|
||||||
break;
|
|
||||||
case 64:
|
|
||||||
// 70us
|
|
||||||
arm_radix8_butterfly_f32(state->fftData, FFT_BIN_COUNT, Sint->pTwiddle, 1);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
case STEP_BITREVERSAL:
|
case STEP_BITREVERSAL_AND_STAGE_RFFT_F32:
|
||||||
{
|
{
|
||||||
// 6us
|
|
||||||
arm_bitreversal_32((uint32_t*) state->fftData, Sint->bitRevLength, Sint->pBitRevTable);
|
arm_bitreversal_32((uint32_t*) state->fftData, Sint->bitRevLength, Sint->pBitRevTable);
|
||||||
state->updateStep++;
|
|
||||||
FALLTHROUGH;
|
|
||||||
}
|
|
||||||
case STEP_STAGE_RFFT_F32:
|
|
||||||
{
|
|
||||||
// 14us
|
|
||||||
// this does not work in place => fftData AND rfftData needed
|
|
||||||
stage_rfft_f32(&state->fftInstance, state->fftData, state->rfftData);
|
stage_rfft_f32(&state->fftInstance, state->fftData, state->rfftData);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
case STEP_ARM_CMPLX_MAG_F32:
|
case STEP_MAGNITUDE_AND_FREQUENCY:
|
||||||
{
|
{
|
||||||
// 8us
|
// 8us
|
||||||
arm_cmplx_mag_f32(state->rfftData, state->fftData, FFT_BIN_COUNT);
|
arm_cmplx_mag_f32(state->rfftData, state->fftData, FFT_BIN_COUNT);
|
||||||
state->updateStep++;
|
|
||||||
FALLTHROUGH;
|
//Find peak frequency
|
||||||
}
|
uint8_t peakBin = findPeakBinIndex(state);
|
||||||
case STEP_CALC_FREQUENCIES:
|
|
||||||
{
|
// Failsafe to ensure the last bin is not a peak bin
|
||||||
bool fftIncreased = false;
|
peakBin = constrain(peakBin, state->fftStartBin, FFT_BIN_COUNT - 1);
|
||||||
float dataMax = 0;
|
|
||||||
uint8_t binStart = 0;
|
/*
|
||||||
uint8_t binMax = 0;
|
* Calculate center frequency using the parabola method
|
||||||
//for bins after initial decline, identify start bin and max bin
|
*/
|
||||||
for (int i = state->fftStartBin; i < FFT_BIN_COUNT; i++) {
|
float preciseBin = computeParabolaMean(state, peakBin);
|
||||||
if (fftIncreased || (state->fftData[i] > state->fftData[i - 1])) {
|
float peakFrequency = preciseBin * state->fftResolution;
|
||||||
if (!fftIncreased) {
|
|
||||||
binStart = i; // first up-step bin
|
peakFrequency = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], peakFrequency);
|
||||||
fftIncreased = true;
|
peakFrequency = constrainf(peakFrequency, state->minFrequency, state->maxFrequency);
|
||||||
}
|
|
||||||
if (state->fftData[i] > dataMax) {
|
|
||||||
dataMax = state->fftData[i];
|
|
||||||
binMax = i; // tallest bin
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
// accumulate fftSum and fftWeightedSum from peak bin, and shoulder bins either side of peak
|
|
||||||
float cubedData = state->fftData[binMax] * state->fftData[binMax] * state->fftData[binMax];
|
|
||||||
float fftSum = cubedData;
|
|
||||||
float fftWeightedSum = cubedData * (binMax + 1);
|
|
||||||
// accumulate upper shoulder
|
|
||||||
for (int i = binMax; i < FFT_BIN_COUNT - 1; i++) {
|
|
||||||
if (state->fftData[i] > state->fftData[i + 1]) {
|
|
||||||
cubedData = state->fftData[i] * state->fftData[i] * state->fftData[i];
|
|
||||||
fftSum += cubedData;
|
|
||||||
fftWeightedSum += cubedData * (i + 1);
|
|
||||||
} else {
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
// accumulate lower shoulder
|
|
||||||
for (int i = binMax; i > binStart + 1; i--) {
|
|
||||||
if (state->fftData[i] > state->fftData[i - 1]) {
|
|
||||||
cubedData = state->fftData[i] * state->fftData[i] * state->fftData[i];
|
|
||||||
fftSum += cubedData;
|
|
||||||
fftWeightedSum += cubedData * (i + 1);
|
|
||||||
} else {
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
// get weighted center of relevant frequency range (this way we have a better resolution than 31.25Hz)
|
|
||||||
float centerFreq = state->maxFrequency;
|
|
||||||
float fftMeanIndex = 0;
|
|
||||||
// idx was shifted by 1 to start at 1, not 0
|
|
||||||
if (fftSum > 0) {
|
|
||||||
fftMeanIndex = (fftWeightedSum / fftSum) - 1;
|
|
||||||
// the index points at the center frequency of each bin so index 0 is actually 16.125Hz
|
|
||||||
centerFreq = fftMeanIndex * state->fftResolution;
|
|
||||||
} else {
|
|
||||||
centerFreq = state->prevCenterFreq[state->updateAxis];
|
|
||||||
}
|
|
||||||
centerFreq = fmax(centerFreq, state->minFrequency);
|
|
||||||
centerFreq = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], centerFreq);
|
|
||||||
state->prevCenterFreq[state->updateAxis] = state->centerFreq[state->updateAxis];
|
state->prevCenterFreq[state->updateAxis] = state->centerFreq[state->updateAxis];
|
||||||
state->centerFreq[state->updateAxis] = centerFreq;
|
state->centerFreq[state->updateAxis] = peakFrequency;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
case STEP_UPDATE_FILTERS:
|
case STEP_UPDATE_FILTERS_AND_HANNING:
|
||||||
{
|
{
|
||||||
// 7us
|
// 7us
|
||||||
// calculate cutoffFreq and notch Q, update notch filter =1.8+((A2-150)*0.004)
|
// calculate cutoffFreq and notch Q, update notch filter =1.8+((A2-150)*0.004)
|
||||||
|
|
@ -287,20 +222,12 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
|
||||||
state->filterUpdateFrequency = state->centerFreq[state->updateAxis];
|
state->filterUpdateFrequency = state->centerFreq[state->updateAxis];
|
||||||
}
|
}
|
||||||
|
|
||||||
|
//Switch to the next axis
|
||||||
state->updateAxis = (state->updateAxis + 1) % XYZ_AXIS_COUNT;
|
state->updateAxis = (state->updateAxis + 1) % XYZ_AXIS_COUNT;
|
||||||
state->updateStep++;
|
|
||||||
FALLTHROUGH;
|
|
||||||
}
|
|
||||||
case STEP_HANNING:
|
|
||||||
{
|
|
||||||
// 5us
|
|
||||||
// apply hanning window to gyro samples and store result in fftData
|
// apply hanning window to gyro samples and store result in fftData
|
||||||
// hanning starts and ends with 0, could be skipped for minor speed improvement
|
// hanning starts and ends with 0, could be skipped for minor speed improvement
|
||||||
const uint8_t ringBufIdx = FFT_WINDOW_SIZE - state->circularBufferIdx;
|
arm_mult_f32(state->downsampledGyroData[state->updateAxis], state->hanningWindow, state->fftData, FFT_WINDOW_SIZE);
|
||||||
arm_mult_f32(&state->downsampledGyroData[state->updateAxis][state->circularBufferIdx], &state->hanningWindow[0], &state->fftData[0], ringBufIdx);
|
|
||||||
if (state->circularBufferIdx > 0) {
|
|
||||||
arm_mult_f32(&state->downsampledGyroData[state->updateAxis][0], &state->hanningWindow[ringBufIdx], &state->fftData[ringBufIdx], state->circularBufferIdx);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -25,15 +25,15 @@
|
||||||
#include "arm_math.h"
|
#include "arm_math.h"
|
||||||
#include "common/filter.h"
|
#include "common/filter.h"
|
||||||
|
|
||||||
// max for F3 targets
|
/*
|
||||||
#define FFT_WINDOW_SIZE 32
|
* Current code works only with 64 window size. Changing it do a different size would require
|
||||||
|
* adapting the gyroDataAnalyseUpdate in STEP_ARM_CFFT_F32 step
|
||||||
|
*/
|
||||||
|
#define FFT_WINDOW_SIZE 64
|
||||||
|
|
||||||
typedef struct gyroAnalyseState_s {
|
typedef struct gyroAnalyseState_s {
|
||||||
// accumulator for oversampled data => no aliasing and less noise
|
// accumulator for oversampled data => no aliasing and less noise
|
||||||
uint8_t sampleCount;
|
float currentSample[XYZ_AXIS_COUNT];
|
||||||
uint8_t maxSampleCount;
|
|
||||||
float maxSampleCountRcp;
|
|
||||||
float oversampledGyroAccumulator[XYZ_AXIS_COUNT];
|
|
||||||
|
|
||||||
// downsampled gyro data circular buffer for frequency analysis
|
// downsampled gyro data circular buffer for frequency analysis
|
||||||
uint8_t circularBufferIdx;
|
uint8_t circularBufferIdx;
|
||||||
|
|
@ -69,7 +69,6 @@ STATIC_ASSERT(FFT_WINDOW_SIZE <= (uint8_t) -1, window_size_greater_than_underlyi
|
||||||
void gyroDataAnalyseStateInit(
|
void gyroDataAnalyseStateInit(
|
||||||
gyroAnalyseState_t *state,
|
gyroAnalyseState_t *state,
|
||||||
uint16_t minFrequency,
|
uint16_t minFrequency,
|
||||||
uint8_t range,
|
|
||||||
uint32_t targetLooptimeUs
|
uint32_t targetLooptimeUs
|
||||||
);
|
);
|
||||||
void gyroDataAnalysePush(gyroAnalyseState_t *gyroAnalyse, int axis, float sample);
|
void gyroDataAnalysePush(gyroAnalyseState_t *gyroAnalyse, int axis, float sample);
|
||||||
|
|
|
||||||
|
|
@ -1037,17 +1037,37 @@ static void osdFormatRpm(char *buff, uint32_t rpm)
|
||||||
{
|
{
|
||||||
buff[0] = SYM_RPM;
|
buff[0] = SYM_RPM;
|
||||||
if (rpm) {
|
if (rpm) {
|
||||||
if (rpm >= 1000) {
|
if ( digitCount(rpm) > osdConfig()->esc_rpm_precision) {
|
||||||
osdFormatCentiNumber(buff + 1, rpm / 10, 0, 1, 1, 2);
|
uint8_t rpmMaxDecimals = (osdConfig()->esc_rpm_precision - 3);
|
||||||
buff[3] = 'K';
|
osdFormatCentiNumber(buff + 1, rpm / 10, 0, rpmMaxDecimals, rpmMaxDecimals, osdConfig()->esc_rpm_precision-1);
|
||||||
buff[4] = '\0';
|
buff[osdConfig()->esc_rpm_precision] = 'K';
|
||||||
|
buff[osdConfig()->esc_rpm_precision+1] = '\0';
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
tfp_sprintf(buff + 1, "%3lu", rpm);
|
switch(osdConfig()->esc_rpm_precision) {
|
||||||
|
case 6:
|
||||||
|
tfp_sprintf(buff + 1, "%6lu", rpm);
|
||||||
|
break;
|
||||||
|
case 5:
|
||||||
|
tfp_sprintf(buff + 1, "%5lu", rpm);
|
||||||
|
break;
|
||||||
|
case 4:
|
||||||
|
tfp_sprintf(buff + 1, "%4lu", rpm);
|
||||||
|
break;
|
||||||
|
case 3:
|
||||||
|
default:
|
||||||
|
tfp_sprintf(buff + 1, "%3lu", rpm);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
strcpy(buff + 1, "---");
|
uint8_t buffPos = 1;
|
||||||
|
while (buffPos <=( osdConfig()->esc_rpm_precision)) {
|
||||||
|
strcpy(buff + buffPos++, "-");
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
@ -2267,6 +2287,11 @@ static bool osdDrawSingleElement(uint8_t item)
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
case OSD_ACTIVE_PROFILE:
|
||||||
|
tfp_sprintf(buff, "%c%u", SYM_PROFILE, (getConfigProfile() + 1));
|
||||||
|
displayWrite(osdDisplayPort, elemPosX, elemPosY, buff);
|
||||||
|
break;
|
||||||
|
|
||||||
case OSD_ROLL_PIDS:
|
case OSD_ROLL_PIDS:
|
||||||
osdDisplayFlightPIDValues(elemPosX, elemPosY, "ROL", PID_ROLL, ADJUSTMENT_ROLL_P, ADJUSTMENT_ROLL_I, ADJUSTMENT_ROLL_D, ADJUSTMENT_ROLL_FF);
|
osdDisplayFlightPIDValues(elemPosX, elemPosY, "ROL", PID_ROLL, ADJUSTMENT_ROLL_P, ADJUSTMENT_ROLL_I, ADJUSTMENT_ROLL_D, ADJUSTMENT_ROLL_FF);
|
||||||
return true;
|
return true;
|
||||||
|
|
@ -3157,6 +3182,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
|
||||||
.osd_home_position_arm_screen = SETTING_OSD_HOME_POSITION_ARM_SCREEN_DEFAULT,
|
.osd_home_position_arm_screen = SETTING_OSD_HOME_POSITION_ARM_SCREEN_DEFAULT,
|
||||||
.pan_servo_index = SETTING_OSD_PAN_SERVO_INDEX_DEFAULT,
|
.pan_servo_index = SETTING_OSD_PAN_SERVO_INDEX_DEFAULT,
|
||||||
.pan_servo_pwm2centideg = SETTING_OSD_PAN_SERVO_PWM2CENTIDEG_DEFAULT,
|
.pan_servo_pwm2centideg = SETTING_OSD_PAN_SERVO_PWM2CENTIDEG_DEFAULT,
|
||||||
|
.esc_rpm_precision = SETTING_OSD_ESC_RPM_PRECISION_DEFAULT,
|
||||||
|
|
||||||
.units = SETTING_OSD_UNITS_DEFAULT,
|
.units = SETTING_OSD_UNITS_DEFAULT,
|
||||||
.main_voltage_decimals = SETTING_OSD_MAIN_VOLTAGE_DECIMALS_DEFAULT,
|
.main_voltage_decimals = SETTING_OSD_MAIN_VOLTAGE_DECIMALS_DEFAULT,
|
||||||
|
|
@ -4116,13 +4142,13 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
|
||||||
if (buff != NULL) {
|
if (buff != NULL) {
|
||||||
const char *message = NULL;
|
const char *message = NULL;
|
||||||
char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)];
|
char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)];
|
||||||
if (ARMING_FLAG(ARMED)) {
|
// We might have up to 5 messages to show.
|
||||||
// Aircraft is armed. We might have up to 5
|
const char *messages[5];
|
||||||
// messages to show.
|
unsigned messageCount = 0;
|
||||||
const char *messages[5];
|
const char *failsafeInfoMessage = NULL;
|
||||||
unsigned messageCount = 0;
|
const char *invertedInfoMessage = NULL;
|
||||||
const char *failsafeInfoMessage = NULL;
|
|
||||||
|
|
||||||
|
if (ARMING_FLAG(ARMED)) {
|
||||||
if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
|
if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
|
||||||
if (isWaypointMissionRTHActive()) {
|
if (isWaypointMissionRTHActive()) {
|
||||||
// if RTH activated whilst WP mode selected, remind pilot to cancel WP mode to exit RTH
|
// if RTH activated whilst WP mode selected, remind pilot to cancel WP mode to exit RTH
|
||||||
|
|
@ -4197,45 +4223,55 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
if (messageCount > 0) {
|
|
||||||
message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)];
|
|
||||||
if (message == failsafeInfoMessage) {
|
|
||||||
// failsafeInfoMessage is not useful for recovering
|
|
||||||
// a lost model, but might help avoiding a crash.
|
|
||||||
// Blink to grab user attention.
|
|
||||||
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
|
|
||||||
}
|
|
||||||
// We're shoing either failsafePhaseMessage or
|
|
||||||
// navStateMessage. Don't BLINK here since
|
|
||||||
// having this text available might be crucial
|
|
||||||
// during a lost aircraft recovery and blinking
|
|
||||||
// will cause it to be missing from some frames.
|
|
||||||
}
|
|
||||||
} else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) {
|
} else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) {
|
||||||
unsigned invalidIndex;
|
unsigned invalidIndex;
|
||||||
|
|
||||||
// Check if we're unable to arm for some reason
|
// Check if we're unable to arm for some reason
|
||||||
if (ARMING_FLAG(ARMING_DISABLED_INVALID_SETTING) && !settingsValidate(&invalidIndex)) {
|
if (ARMING_FLAG(ARMING_DISABLED_INVALID_SETTING) && !settingsValidate(&invalidIndex)) {
|
||||||
if (OSD_ALTERNATING_CHOICES(1000, 2) == 0) {
|
|
||||||
const setting_t *setting = settingGet(invalidIndex);
|
const setting_t *setting = settingGet(invalidIndex);
|
||||||
settingGetName(setting, messageBuf);
|
settingGetName(setting, messageBuf);
|
||||||
for (int ii = 0; messageBuf[ii]; ii++) {
|
for (int ii = 0; messageBuf[ii]; ii++) {
|
||||||
messageBuf[ii] = sl_toupper(messageBuf[ii]);
|
messageBuf[ii] = sl_toupper(messageBuf[ii]);
|
||||||
}
|
}
|
||||||
message = messageBuf;
|
invertedInfoMessage = messageBuf;
|
||||||
} else {
|
messages[messageCount++] = invertedInfoMessage;
|
||||||
message = OSD_MESSAGE_STR(OSD_MSG_INVALID_SETTING);
|
|
||||||
TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr);
|
invertedInfoMessage = OSD_MESSAGE_STR(OSD_MSG_INVALID_SETTING);
|
||||||
}
|
messages[messageCount++] = invertedInfoMessage;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
if (OSD_ALTERNATING_CHOICES(1000, 2) == 0) {
|
|
||||||
message = OSD_MESSAGE_STR(OSD_MSG_UNABLE_ARM);
|
invertedInfoMessage = OSD_MESSAGE_STR(OSD_MSG_UNABLE_ARM);
|
||||||
TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr);
|
messages[messageCount++] = invertedInfoMessage;
|
||||||
} else {
|
|
||||||
// Show the reason for not arming
|
// Show the reason for not arming
|
||||||
message = osdArmingDisabledReasonMessage();
|
messages[messageCount++] = osdArmingDisabledReasonMessage();
|
||||||
}
|
|
||||||
|
}
|
||||||
|
} else if (!ARMING_FLAG(ARMED)) {
|
||||||
|
if (isWaypointListValid()) {
|
||||||
|
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_WP_MISSION_LOADED);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (messageCount > 0) {
|
||||||
|
message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)];
|
||||||
|
if (message == failsafeInfoMessage) {
|
||||||
|
// failsafeInfoMessage is not useful for recovering
|
||||||
|
// a lost model, but might help avoiding a crash.
|
||||||
|
// Blink to grab user attention.
|
||||||
|
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
|
||||||
|
} else if (message == invertedInfoMessage) {
|
||||||
|
TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr);
|
||||||
|
}
|
||||||
|
// We're shoing either failsafePhaseMessage or
|
||||||
|
// navStateMessage. Don't BLINK here since
|
||||||
|
// having this text available might be crucial
|
||||||
|
// during a lost aircraft recovery and blinking
|
||||||
|
// will cause it to be missing from some frames.
|
||||||
|
}
|
||||||
|
|
||||||
osdFormatMessage(buff, buff_size, message, isCenteredText);
|
osdFormatMessage(buff, buff_size, message, isCenteredText);
|
||||||
}
|
}
|
||||||
return elemAttr;
|
return elemAttr;
|
||||||
|
|
|
||||||
|
|
@ -86,6 +86,7 @@
|
||||||
#define OSD_MSG_WP_FINISHED "WP END>HOLDING POSITION"
|
#define OSD_MSG_WP_FINISHED "WP END>HOLDING POSITION"
|
||||||
#define OSD_MSG_PREPARE_NEXT_WP "PREPARING FOR NEXT WAYPOINT"
|
#define OSD_MSG_PREPARE_NEXT_WP "PREPARING FOR NEXT WAYPOINT"
|
||||||
#define OSD_MSG_WP_RTH_CANCEL "CANCEL WP TO EXIT RTH"
|
#define OSD_MSG_WP_RTH_CANCEL "CANCEL WP TO EXIT RTH"
|
||||||
|
#define OSD_MSG_WP_MISSION_LOADED "* MISSION LOADED *"
|
||||||
#define OSD_MSG_EMERG_LANDING "EMERGENCY LANDING"
|
#define OSD_MSG_EMERG_LANDING "EMERGENCY LANDING"
|
||||||
#define OSD_MSG_LANDING "LANDING"
|
#define OSD_MSG_LANDING "LANDING"
|
||||||
#define OSD_MSG_LOITERING_HOME "LOITERING AROUND HOME"
|
#define OSD_MSG_LOITERING_HOME "LOITERING AROUND HOME"
|
||||||
|
|
@ -234,6 +235,7 @@ typedef enum {
|
||||||
OSD_GPS_MAX_SPEED,
|
OSD_GPS_MAX_SPEED,
|
||||||
OSD_3D_MAX_SPEED,
|
OSD_3D_MAX_SPEED,
|
||||||
OSD_AIR_MAX_SPEED,
|
OSD_AIR_MAX_SPEED,
|
||||||
|
OSD_ACTIVE_PROFILE,
|
||||||
OSD_ITEM_COUNT // MUST BE LAST
|
OSD_ITEM_COUNT // MUST BE LAST
|
||||||
} osd_items_e;
|
} osd_items_e;
|
||||||
|
|
||||||
|
|
@ -387,6 +389,7 @@ typedef struct osdConfig_s {
|
||||||
uint8_t crsf_lq_format;
|
uint8_t crsf_lq_format;
|
||||||
uint8_t sidebar_height; // sidebar height in rows, 0 turns off sidebars leaving only level indicator arrows
|
uint8_t sidebar_height; // sidebar height in rows, 0 turns off sidebars leaving only level indicator arrows
|
||||||
uint8_t telemetry; // use telemetry on displayed pixel line 0
|
uint8_t telemetry; // use telemetry on displayed pixel line 0
|
||||||
|
uint8_t esc_rpm_precision; // Number of characters used for the RPM numbers.
|
||||||
|
|
||||||
} osdConfig_t;
|
} osdConfig_t;
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -492,14 +492,9 @@ static void saReceiveFramer(uint8_t c)
|
||||||
|
|
||||||
static void saSendFrame(uint8_t *buf, int len)
|
static void saSendFrame(uint8_t *buf, int len)
|
||||||
{
|
{
|
||||||
switch (smartAudioSerialPort->identifier) {
|
// TBS SA definition requires that the line is low before frame is sent
|
||||||
case SERIAL_PORT_SOFTSERIAL1:
|
// (for both soft and hard serial). It can be done by sending first 0x00
|
||||||
case SERIAL_PORT_SOFTSERIAL2:
|
serialWrite(smartAudioSerialPort, 0x00);
|
||||||
break;
|
|
||||||
default:
|
|
||||||
serialWrite(smartAudioSerialPort, 0x00); // Generate 1st start bit
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
for (int i = 0 ; i < len ; i++) {
|
for (int i = 0 ; i < len ; i++) {
|
||||||
serialWrite(smartAudioSerialPort, buf[i]);
|
serialWrite(smartAudioSerialPort, buf[i]);
|
||||||
|
|
|
||||||
|
|
@ -96,7 +96,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang
|
||||||
PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0);
|
PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 13);
|
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 14);
|
||||||
|
|
||||||
PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
||||||
.general = {
|
.general = {
|
||||||
|
|
@ -121,7 +121,8 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
||||||
.waypoint_radius = SETTING_NAV_WP_RADIUS_DEFAULT, // 2m diameter
|
.waypoint_radius = SETTING_NAV_WP_RADIUS_DEFAULT, // 2m diameter
|
||||||
.waypoint_safe_distance = SETTING_NAV_WP_SAFE_DISTANCE_DEFAULT, // centimeters - first waypoint should be closer than this
|
.waypoint_safe_distance = SETTING_NAV_WP_SAFE_DISTANCE_DEFAULT, // centimeters - first waypoint should be closer than this
|
||||||
.waypoint_load_on_boot = SETTING_NAV_WP_LOAD_ON_BOOT_DEFAULT, // load waypoints automatically during boot
|
.waypoint_load_on_boot = SETTING_NAV_WP_LOAD_ON_BOOT_DEFAULT, // load waypoints automatically during boot
|
||||||
.max_auto_speed = SETTING_NAV_AUTO_SPEED_DEFAULT, // 3 m/s = 10.8 km/h
|
.auto_speed = SETTING_NAV_AUTO_SPEED_DEFAULT, // speed in autonomous modes (3 m/s = 10.8 km/h)
|
||||||
|
.max_auto_speed = SETTING_NAV_MAX_AUTO_SPEED_DEFAULT, // max allowed speed autonomous modes
|
||||||
.max_auto_climb_rate = SETTING_NAV_AUTO_CLIMB_RATE_DEFAULT, // 5 m/s
|
.max_auto_climb_rate = SETTING_NAV_AUTO_CLIMB_RATE_DEFAULT, // 5 m/s
|
||||||
.max_manual_speed = SETTING_NAV_MANUAL_SPEED_DEFAULT,
|
.max_manual_speed = SETTING_NAV_MANUAL_SPEED_DEFAULT,
|
||||||
.max_manual_climb_rate = SETTING_NAV_MANUAL_CLIMB_RATE_DEFAULT,
|
.max_manual_climb_rate = SETTING_NAV_MANUAL_CLIMB_RATE_DEFAULT,
|
||||||
|
|
@ -3013,7 +3014,7 @@ float getActiveWaypointSpeed(void)
|
||||||
return navConfig()->general.max_manual_speed;
|
return navConfig()->general.max_manual_speed;
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
uint16_t waypointSpeed = navConfig()->general.max_auto_speed;
|
uint16_t waypointSpeed = navConfig()->general.auto_speed;
|
||||||
|
|
||||||
if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) {
|
if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) {
|
||||||
if (posControl.waypointCount > 0 && (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND)) {
|
if (posControl.waypointCount > 0 && (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND)) {
|
||||||
|
|
@ -3025,6 +3026,8 @@ float getActiveWaypointSpeed(void)
|
||||||
|
|
||||||
if (wpSpecificSpeed >= 50.0f && wpSpecificSpeed <= navConfig()->general.max_auto_speed) {
|
if (wpSpecificSpeed >= 50.0f && wpSpecificSpeed <= navConfig()->general.max_auto_speed) {
|
||||||
waypointSpeed = wpSpecificSpeed;
|
waypointSpeed = wpSpecificSpeed;
|
||||||
|
} else if (wpSpecificSpeed > navConfig()->general.max_auto_speed) {
|
||||||
|
waypointSpeed = navConfig()->general.max_auto_speed;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
@ -3188,14 +3191,15 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
|
||||||
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
|
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* Keep Emergency landing mode active once triggered. Is deactivated when landing in progress if WP or RTH cancelled
|
/* Keep Emergency landing mode active once triggered. Is cancelled when landing in progress if position sensors working again.
|
||||||
* or position sensors working again or if Manual or Althold modes selected.
|
* If failsafe not active landing also cancelled if WP or RTH deselected or if Manual or Althold modes selected.
|
||||||
* Remains active if landing finished regardless of sensor status or if WP or RTH modes still selected */
|
* Remains active if landing finished regardless of sensor status or flight mode selection */
|
||||||
|
bool autonomousNavNotPossible = !(canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME));
|
||||||
|
bool emergLandingCancel = IS_RC_MODE_ACTIVE(BOXMANUAL) || (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) && canActivateAltHold) ||
|
||||||
|
!(IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVRTH));
|
||||||
|
|
||||||
if (navigationIsExecutingAnEmergencyLanding()) {
|
if (navigationIsExecutingAnEmergencyLanding()) {
|
||||||
if (!(canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME)) &&
|
if (autonomousNavNotPossible && (!emergLandingCancel || FLIGHT_MODE(FAILSAFE_MODE))) {
|
||||||
!IS_RC_MODE_ACTIVE(BOXMANUAL) &&
|
|
||||||
!(IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) && canActivateAltHold) &&
|
|
||||||
(IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVRTH))) {
|
|
||||||
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
|
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -202,7 +202,8 @@ typedef struct navConfig_s {
|
||||||
uint16_t waypoint_radius; // if we are within this distance to a waypoint then we consider it reached (distance is in cm)
|
uint16_t waypoint_radius; // if we are within this distance to a waypoint then we consider it reached (distance is in cm)
|
||||||
uint16_t waypoint_safe_distance; // Waypoint mission sanity check distance
|
uint16_t waypoint_safe_distance; // Waypoint mission sanity check distance
|
||||||
bool waypoint_load_on_boot; // load waypoints automatically during boot
|
bool waypoint_load_on_boot; // load waypoints automatically during boot
|
||||||
uint16_t max_auto_speed; // autonomous navigation speed cm/sec
|
uint16_t auto_speed; // autonomous navigation speed cm/sec
|
||||||
|
uint16_t max_auto_speed; // maximum allowed autonomous navigation speed cm/sec
|
||||||
uint16_t max_auto_climb_rate; // max vertical speed limitation cm/sec
|
uint16_t max_auto_climb_rate; // max vertical speed limitation cm/sec
|
||||||
uint16_t max_manual_speed; // manual velocity control max horizontal speed
|
uint16_t max_manual_speed; // manual velocity control max horizontal speed
|
||||||
uint16_t max_manual_climb_rate; // manual velocity control max vertical speed
|
uint16_t max_manual_climb_rate; // manual velocity control max vertical speed
|
||||||
|
|
|
||||||
|
|
@ -49,6 +49,8 @@
|
||||||
#include "navigation/navigation.h"
|
#include "navigation/navigation.h"
|
||||||
#include "navigation/navigation_private.h"
|
#include "navigation/navigation_private.h"
|
||||||
|
|
||||||
|
#include "programming/logic_condition.h"
|
||||||
|
|
||||||
#include "rx/rx.h"
|
#include "rx/rx.h"
|
||||||
|
|
||||||
#include "sensors/battery.h"
|
#include "sensors/battery.h"
|
||||||
|
|
@ -271,10 +273,12 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
|
||||||
// Limit minimum forward velocity to 1 m/s
|
// Limit minimum forward velocity to 1 m/s
|
||||||
float trackingDistance = trackingPeriod * MAX(posControl.actualState.velXY, 100.0f);
|
float trackingDistance = trackingPeriod * MAX(posControl.actualState.velXY, 100.0f);
|
||||||
|
|
||||||
|
uint32_t navLoiterRadius = getLoiterRadius(navConfig()->fw.loiter_radius);
|
||||||
|
|
||||||
// If angular visibility of a waypoint is less than 30deg, don't calculate circular loiter, go straight to the target
|
// If angular visibility of a waypoint is less than 30deg, don't calculate circular loiter, go straight to the target
|
||||||
#define TAN_15DEG 0.26795f
|
#define TAN_15DEG 0.26795f
|
||||||
bool needToCalculateCircularLoiter = (isApproachingLastWaypoint() || isWaypointWait())
|
bool needToCalculateCircularLoiter = (isApproachingLastWaypoint() || isWaypointWait())
|
||||||
&& (distanceToActualTarget <= (navConfig()->fw.loiter_radius / TAN_15DEG))
|
&& (distanceToActualTarget <= (navLoiterRadius / TAN_15DEG))
|
||||||
&& (distanceToActualTarget > 50.0f)
|
&& (distanceToActualTarget > 50.0f)
|
||||||
&& !FLIGHT_MODE(NAV_COURSE_HOLD_MODE);
|
&& !FLIGHT_MODE(NAV_COURSE_HOLD_MODE);
|
||||||
|
|
||||||
|
|
@ -283,8 +287,8 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
|
||||||
// We are closing in on a waypoint, calculate circular loiter
|
// We are closing in on a waypoint, calculate circular loiter
|
||||||
float loiterAngle = atan2_approx(-posErrorY, -posErrorX) + DEGREES_TO_RADIANS(loiterDirection() * 45.0f);
|
float loiterAngle = atan2_approx(-posErrorY, -posErrorX) + DEGREES_TO_RADIANS(loiterDirection() * 45.0f);
|
||||||
|
|
||||||
float loiterTargetX = posControl.desiredState.pos.x + navConfig()->fw.loiter_radius * cos_approx(loiterAngle);
|
float loiterTargetX = posControl.desiredState.pos.x + navLoiterRadius * cos_approx(loiterAngle);
|
||||||
float loiterTargetY = posControl.desiredState.pos.y + navConfig()->fw.loiter_radius * sin_approx(loiterAngle);
|
float loiterTargetY = posControl.desiredState.pos.y + navLoiterRadius * sin_approx(loiterAngle);
|
||||||
|
|
||||||
// We have temporary loiter target. Recalculate distance and position error
|
// We have temporary loiter target. Recalculate distance and position error
|
||||||
posErrorX = loiterTargetX - navGetCurrentActualPositionAndVelocity()->pos.x;
|
posErrorX = loiterTargetX - navGetCurrentActualPositionAndVelocity()->pos.x;
|
||||||
|
|
|
||||||
|
|
@ -34,6 +34,7 @@
|
||||||
#include "common/utils.h"
|
#include "common/utils.h"
|
||||||
#include "rx/rx.h"
|
#include "rx/rx.h"
|
||||||
#include "common/maths.h"
|
#include "common/maths.h"
|
||||||
|
#include "fc/config.h"
|
||||||
#include "fc/fc_core.h"
|
#include "fc/fc_core.h"
|
||||||
#include "fc/rc_controls.h"
|
#include "fc/rc_controls.h"
|
||||||
#include "fc/runtime_config.h"
|
#include "fc/runtime_config.h"
|
||||||
|
|
@ -82,10 +83,10 @@ void pgResetFn_logicConditions(logicCondition_t *instance)
|
||||||
logicConditionState_t logicConditionStates[MAX_LOGIC_CONDITIONS];
|
logicConditionState_t logicConditionStates[MAX_LOGIC_CONDITIONS];
|
||||||
|
|
||||||
static int logicConditionCompute(
|
static int logicConditionCompute(
|
||||||
int currentVaue,
|
int32_t currentVaue,
|
||||||
logicOperation_e operation,
|
logicOperation_e operation,
|
||||||
int operandA,
|
int32_t operandA,
|
||||||
int operandB
|
int32_t operandB
|
||||||
) {
|
) {
|
||||||
int temporaryValue;
|
int temporaryValue;
|
||||||
vtxDeviceCapability_t vtxDeviceCapability;
|
vtxDeviceCapability_t vtxDeviceCapability;
|
||||||
|
|
@ -176,20 +177,20 @@ static int logicConditionCompute(
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case LOGIC_CONDITION_ADD:
|
case LOGIC_CONDITION_ADD:
|
||||||
return constrain(operandA + operandB, INT16_MIN, INT16_MAX);
|
return constrain(operandA + operandB, INT32_MIN, INT32_MAX);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case LOGIC_CONDITION_SUB:
|
case LOGIC_CONDITION_SUB:
|
||||||
return constrain(operandA - operandB, INT16_MIN, INT16_MAX);
|
return constrain(operandA - operandB, INT32_MIN, INT32_MAX);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case LOGIC_CONDITION_MUL:
|
case LOGIC_CONDITION_MUL:
|
||||||
return constrain(operandA * operandB, INT16_MIN, INT16_MAX);
|
return constrain(operandA * operandB, INT32_MIN, INT32_MAX);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case LOGIC_CONDITION_DIV:
|
case LOGIC_CONDITION_DIV:
|
||||||
if (operandB != 0) {
|
if (operandB != 0) {
|
||||||
return constrain(operandA / operandB, INT16_MIN, INT16_MAX);
|
return constrain(operandA / operandB, INT32_MIN, INT32_MAX);
|
||||||
} else {
|
} else {
|
||||||
return operandA;
|
return operandA;
|
||||||
}
|
}
|
||||||
|
|
@ -327,12 +328,34 @@ static int logicConditionCompute(
|
||||||
|
|
||||||
case LOGIC_CONDITION_MODULUS:
|
case LOGIC_CONDITION_MODULUS:
|
||||||
if (operandB != 0) {
|
if (operandB != 0) {
|
||||||
return constrain(operandA % operandB, INT16_MIN, INT16_MAX);
|
return constrain(operandA % operandB, INT32_MIN, INT32_MAX);
|
||||||
} else {
|
} else {
|
||||||
return operandA;
|
return operandA;
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
case LOGIC_CONDITION_SET_PROFILE:
|
||||||
|
operandA--;
|
||||||
|
if ( getConfigProfile() != operandA && (operandA >= 0 && operandA < MAX_PROFILE_COUNT)) {
|
||||||
|
bool profileChanged = false;
|
||||||
|
if (setConfigProfile(operandA)) {
|
||||||
|
pidInit();
|
||||||
|
pidInitFilters();
|
||||||
|
schedulePidGainsUpdate();
|
||||||
|
profileChanged = true;
|
||||||
|
}
|
||||||
|
return profileChanged;
|
||||||
|
} else {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case LOGIC_CONDITION_LOITER_OVERRIDE:
|
||||||
|
logicConditionValuesByType[LOGIC_CONDITION_LOITER_OVERRIDE] = constrain(operandA, 0, 100000);
|
||||||
|
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS);
|
||||||
|
return true;
|
||||||
|
break;
|
||||||
|
|
||||||
default:
|
default:
|
||||||
return false;
|
return false;
|
||||||
break;
|
break;
|
||||||
|
|
@ -533,6 +556,13 @@ static int logicConditionGetFlightOperandValue(int operand) {
|
||||||
#endif
|
#endif
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
case LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE: // int
|
||||||
|
return getConfigProfile() + 1;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS:
|
||||||
|
return getLoiterRadius(navConfig()->fw.loiter_radius);
|
||||||
|
|
||||||
default:
|
default:
|
||||||
return 0;
|
return 0;
|
||||||
break;
|
break;
|
||||||
|
|
@ -720,3 +750,16 @@ int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue) {
|
||||||
return originalValue;
|
return originalValue;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
uint32_t getLoiterRadius(uint32_t loiterRadius) {
|
||||||
|
#ifdef USE_PROGRAMMING_FRAMEWORK
|
||||||
|
if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS) &&
|
||||||
|
!(FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding())) {
|
||||||
|
return constrain(logicConditionValuesByType[LOGIC_CONDITION_LOITER_OVERRIDE], loiterRadius, 100000);
|
||||||
|
} else {
|
||||||
|
return loiterRadius;
|
||||||
|
}
|
||||||
|
#else
|
||||||
|
return loiterRadius;
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
|
||||||
|
|
@ -70,7 +70,9 @@ typedef enum {
|
||||||
LOGIC_CONDITION_RC_CHANNEL_OVERRIDE = 38,
|
LOGIC_CONDITION_RC_CHANNEL_OVERRIDE = 38,
|
||||||
LOGIC_CONDITION_SET_HEADING_TARGET = 39,
|
LOGIC_CONDITION_SET_HEADING_TARGET = 39,
|
||||||
LOGIC_CONDITION_MODULUS = 40,
|
LOGIC_CONDITION_MODULUS = 40,
|
||||||
LOGIC_CONDITION_LAST = 41,
|
LOGIC_CONDITION_LOITER_OVERRIDE = 41,
|
||||||
|
LOGIC_CONDITION_SET_PROFILE = 42,
|
||||||
|
LOGIC_CONDITION_LAST = 43,
|
||||||
} logicOperation_e;
|
} logicOperation_e;
|
||||||
|
|
||||||
typedef enum logicOperandType_s {
|
typedef enum logicOperandType_s {
|
||||||
|
|
@ -120,6 +122,8 @@ typedef enum {
|
||||||
LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ, // 32
|
LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ, // 32
|
||||||
LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_SNR, // 33
|
LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_SNR, // 33
|
||||||
LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID, // 0/1 // 34
|
LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID, // 0/1 // 34
|
||||||
|
LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS, // 35
|
||||||
|
LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE, //int // 36
|
||||||
|
|
||||||
} logicFlightOperands_e;
|
} logicFlightOperands_e;
|
||||||
|
|
||||||
|
|
@ -148,6 +152,7 @@ typedef enum {
|
||||||
LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE = (1 << 6),
|
LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE = (1 << 6),
|
||||||
LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_OSD_LAYOUT = (1 << 7),
|
LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_OSD_LAYOUT = (1 << 7),
|
||||||
LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RC_CHANNEL = (1 << 8),
|
LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RC_CHANNEL = (1 << 8),
|
||||||
|
LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS = (1 << 9),
|
||||||
} logicConditionsGlobalFlags_t;
|
} logicConditionsGlobalFlags_t;
|
||||||
|
|
||||||
typedef enum {
|
typedef enum {
|
||||||
|
|
@ -198,3 +203,4 @@ void logicConditionReset(void);
|
||||||
float getThrottleScale(float globalThrottleScale);
|
float getThrottleScale(float globalThrottleScale);
|
||||||
int16_t getRcCommandOverride(int16_t command[], uint8_t axis);
|
int16_t getRcCommandOverride(int16_t command[], uint8_t axis);
|
||||||
int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue);
|
int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue);
|
||||||
|
uint32_t getLoiterRadius(uint32_t loiterRadius);
|
||||||
|
|
|
||||||
|
|
@ -42,6 +42,7 @@
|
||||||
#include "drivers/compass/compass_lis3mdl.h"
|
#include "drivers/compass/compass_lis3mdl.h"
|
||||||
#include "drivers/compass/compass_rm3100.h"
|
#include "drivers/compass/compass_rm3100.h"
|
||||||
#include "drivers/compass/compass_vcm5883.h"
|
#include "drivers/compass/compass_vcm5883.h"
|
||||||
|
#include "drivers/compass/compass_mlx90393.h"
|
||||||
#include "drivers/compass/compass_msp.h"
|
#include "drivers/compass/compass_msp.h"
|
||||||
#include "drivers/io.h"
|
#include "drivers/io.h"
|
||||||
#include "drivers/light_led.h"
|
#include "drivers/light_led.h"
|
||||||
|
|
@ -260,6 +261,19 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse)
|
||||||
}
|
}
|
||||||
FALLTHROUGH;
|
FALLTHROUGH;
|
||||||
|
|
||||||
|
case MAG_MLX90393:
|
||||||
|
#ifdef USE_MAG_MLX90393
|
||||||
|
if (mlx90393Detect(dev)) {
|
||||||
|
magHardware = MAG_MLX90393;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
/* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
|
||||||
|
if (magHardwareToUse != MAG_AUTODETECT) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
FALLTHROUGH;
|
||||||
|
|
||||||
case MAG_FAKE:
|
case MAG_FAKE:
|
||||||
#ifdef USE_FAKE_MAG
|
#ifdef USE_FAKE_MAG
|
||||||
if (fakeMagDetect(dev)) {
|
if (fakeMagDetect(dev)) {
|
||||||
|
|
|
||||||
|
|
@ -43,7 +43,8 @@ typedef enum {
|
||||||
MAG_MSP = 12,
|
MAG_MSP = 12,
|
||||||
MAG_RM3100 = 13,
|
MAG_RM3100 = 13,
|
||||||
MAG_VCM5883 = 14,
|
MAG_VCM5883 = 14,
|
||||||
MAG_FAKE = 15,
|
MAG_MLX90393 = 15,
|
||||||
|
MAG_FAKE = 16,
|
||||||
MAG_MAX = MAG_FAKE
|
MAG_MAX = MAG_FAKE
|
||||||
} magSensor_e;
|
} magSensor_e;
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -103,14 +103,7 @@ EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState;
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef USE_ALPHA_BETA_GAMMA_FILTER
|
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 0);
|
||||||
|
|
||||||
STATIC_FASTRAM filterApplyFnPtr abgFilterApplyFn;
|
|
||||||
STATIC_FASTRAM alphaBetaGammaFilter_t abgFilter[XYZ_AXIS_COUNT];
|
|
||||||
|
|
||||||
#endif
|
|
||||||
|
|
||||||
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 15);
|
|
||||||
|
|
||||||
PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
|
PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
|
||||||
.gyro_lpf = SETTING_GYRO_HARDWARE_LPF_DEFAULT,
|
.gyro_lpf = SETTING_GYRO_HARDWARE_LPF_DEFAULT,
|
||||||
|
|
@ -131,16 +124,10 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
|
||||||
.gyroDynamicLpfMaxHz = SETTING_GYRO_DYN_LPF_MAX_HZ_DEFAULT,
|
.gyroDynamicLpfMaxHz = SETTING_GYRO_DYN_LPF_MAX_HZ_DEFAULT,
|
||||||
.gyroDynamicLpfCurveExpo = SETTING_GYRO_DYN_LPF_CURVE_EXPO_DEFAULT,
|
.gyroDynamicLpfCurveExpo = SETTING_GYRO_DYN_LPF_CURVE_EXPO_DEFAULT,
|
||||||
#ifdef USE_DYNAMIC_FILTERS
|
#ifdef USE_DYNAMIC_FILTERS
|
||||||
.dynamicGyroNotchRange = SETTING_DYNAMIC_GYRO_NOTCH_RANGE_DEFAULT,
|
|
||||||
.dynamicGyroNotchQ = SETTING_DYNAMIC_GYRO_NOTCH_Q_DEFAULT,
|
.dynamicGyroNotchQ = SETTING_DYNAMIC_GYRO_NOTCH_Q_DEFAULT,
|
||||||
.dynamicGyroNotchMinHz = SETTING_DYNAMIC_GYRO_NOTCH_MIN_HZ_DEFAULT,
|
.dynamicGyroNotchMinHz = SETTING_DYNAMIC_GYRO_NOTCH_MIN_HZ_DEFAULT,
|
||||||
.dynamicGyroNotchEnabled = SETTING_DYNAMIC_GYRO_NOTCH_ENABLED_DEFAULT,
|
.dynamicGyroNotchEnabled = SETTING_DYNAMIC_GYRO_NOTCH_ENABLED_DEFAULT,
|
||||||
#endif
|
#endif
|
||||||
#ifdef USE_ALPHA_BETA_GAMMA_FILTER
|
|
||||||
.alphaBetaGammaAlpha = SETTING_GYRO_ABG_ALPHA_DEFAULT,
|
|
||||||
.alphaBetaGammaBoost = SETTING_GYRO_ABG_BOOST_DEFAULT,
|
|
||||||
.alphaBetaGammaHalfLife = SETTING_GYRO_ABG_HALF_LIFE_DEFAULT,
|
|
||||||
#endif
|
|
||||||
#ifdef USE_GYRO_KALMAN
|
#ifdef USE_GYRO_KALMAN
|
||||||
.kalman_q = SETTING_SETPOINT_KALMAN_Q_DEFAULT,
|
.kalman_q = SETTING_SETPOINT_KALMAN_Q_DEFAULT,
|
||||||
.kalmanEnabled = SETTING_SETPOINT_KALMAN_ENABLED_DEFAULT,
|
.kalmanEnabled = SETTING_SETPOINT_KALMAN_ENABLED_DEFAULT,
|
||||||
|
|
@ -313,23 +300,6 @@ static void gyroInitFilters(void)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifdef USE_ALPHA_BETA_GAMMA_FILTER
|
|
||||||
|
|
||||||
abgFilterApplyFn = (filterApplyFnPtr)nullFilterApply;
|
|
||||||
|
|
||||||
if (gyroConfig()->alphaBetaGammaAlpha > 0) {
|
|
||||||
abgFilterApplyFn = (filterApplyFnPtr)alphaBetaGammaFilterApply;
|
|
||||||
for (int axis = 0; axis < 3; axis++) {
|
|
||||||
alphaBetaGammaFilterInit(
|
|
||||||
&abgFilter[axis],
|
|
||||||
gyroConfig()->alphaBetaGammaAlpha,
|
|
||||||
gyroConfig()->alphaBetaGammaBoost,
|
|
||||||
gyroConfig()->alphaBetaGammaHalfLife,
|
|
||||||
getLooptime() * 1e-6f
|
|
||||||
);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
#ifdef USE_GYRO_KALMAN
|
#ifdef USE_GYRO_KALMAN
|
||||||
if (gyroConfig()->kalmanEnabled) {
|
if (gyroConfig()->kalmanEnabled) {
|
||||||
gyroKalmanInitialize(gyroConfig()->kalman_q);
|
gyroKalmanInitialize(gyroConfig()->kalman_q);
|
||||||
|
|
@ -384,7 +354,6 @@ bool gyroInit(void)
|
||||||
gyroDataAnalyseStateInit(
|
gyroDataAnalyseStateInit(
|
||||||
&gyroAnalyseState,
|
&gyroAnalyseState,
|
||||||
gyroConfig()->dynamicGyroNotchMinHz,
|
gyroConfig()->dynamicGyroNotchMinHz,
|
||||||
gyroConfig()->dynamicGyroNotchRange,
|
|
||||||
getLooptime()
|
getLooptime()
|
||||||
);
|
);
|
||||||
#endif
|
#endif
|
||||||
|
|
@ -503,12 +472,6 @@ void FAST_CODE NOINLINE gyroFilter()
|
||||||
gyroADCf = gyroLpf2ApplyFn((filter_t *) &gyroLpf2State[axis], gyroADCf);
|
gyroADCf = gyroLpf2ApplyFn((filter_t *) &gyroLpf2State[axis], gyroADCf);
|
||||||
gyroADCf = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf);
|
gyroADCf = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf);
|
||||||
|
|
||||||
#ifdef USE_ALPHA_BETA_GAMMA_FILTER
|
|
||||||
DEBUG_SET(DEBUG_GYRO_ALPHA_BETA_GAMMA, axis, gyroADCf);
|
|
||||||
gyroADCf = abgFilterApplyFn(&abgFilter[axis], gyroADCf);
|
|
||||||
DEBUG_SET(DEBUG_GYRO_ALPHA_BETA_GAMMA, axis + 3, gyroADCf);
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifdef USE_DYNAMIC_FILTERS
|
#ifdef USE_DYNAMIC_FILTERS
|
||||||
if (dynamicGyroNotchState.enabled) {
|
if (dynamicGyroNotchState.enabled) {
|
||||||
gyroDataAnalysePush(&gyroAnalyseState, axis, gyroADCf);
|
gyroDataAnalysePush(&gyroAnalyseState, axis, gyroADCf);
|
||||||
|
|
|
||||||
|
|
@ -41,16 +41,6 @@ typedef enum {
|
||||||
GYRO_FAKE
|
GYRO_FAKE
|
||||||
} gyroSensor_e;
|
} gyroSensor_e;
|
||||||
|
|
||||||
typedef enum {
|
|
||||||
DYN_NOTCH_RANGE_HIGH = 0,
|
|
||||||
DYN_NOTCH_RANGE_MEDIUM,
|
|
||||||
DYN_NOTCH_RANGE_LOW
|
|
||||||
} dynamicFilterRange_e;
|
|
||||||
|
|
||||||
#define DYN_NOTCH_RANGE_HZ_HIGH 2000
|
|
||||||
#define DYN_NOTCH_RANGE_HZ_MEDIUM 1333
|
|
||||||
#define DYN_NOTCH_RANGE_HZ_LOW 1000
|
|
||||||
|
|
||||||
typedef struct gyro_s {
|
typedef struct gyro_s {
|
||||||
bool initialized;
|
bool initialized;
|
||||||
uint32_t targetLooptime;
|
uint32_t targetLooptime;
|
||||||
|
|
@ -78,16 +68,10 @@ typedef struct gyroConfig_s {
|
||||||
uint16_t gyroDynamicLpfMaxHz;
|
uint16_t gyroDynamicLpfMaxHz;
|
||||||
uint8_t gyroDynamicLpfCurveExpo;
|
uint8_t gyroDynamicLpfCurveExpo;
|
||||||
#ifdef USE_DYNAMIC_FILTERS
|
#ifdef USE_DYNAMIC_FILTERS
|
||||||
uint8_t dynamicGyroNotchRange;
|
|
||||||
uint16_t dynamicGyroNotchQ;
|
uint16_t dynamicGyroNotchQ;
|
||||||
uint16_t dynamicGyroNotchMinHz;
|
uint16_t dynamicGyroNotchMinHz;
|
||||||
uint8_t dynamicGyroNotchEnabled;
|
uint8_t dynamicGyroNotchEnabled;
|
||||||
#endif
|
#endif
|
||||||
#ifdef USE_ALPHA_BETA_GAMMA_FILTER
|
|
||||||
float alphaBetaGammaAlpha;
|
|
||||||
float alphaBetaGammaBoost;
|
|
||||||
float alphaBetaGammaHalfLife;
|
|
||||||
#endif
|
|
||||||
#ifdef USE_GYRO_KALMAN
|
#ifdef USE_GYRO_KALMAN
|
||||||
uint16_t kalman_q;
|
uint16_t kalman_q;
|
||||||
uint8_t kalmanEnabled;
|
uint8_t kalmanEnabled;
|
||||||
|
|
|
||||||
|
|
@ -144,6 +144,7 @@
|
||||||
#define USE_MAG_IST8310
|
#define USE_MAG_IST8310
|
||||||
#define USE_MAG_IST8308
|
#define USE_MAG_IST8308
|
||||||
#define USE_MAG_LIS3MDL
|
#define USE_MAG_LIS3MDL
|
||||||
|
#define USE_MAG_MLX90393
|
||||||
|
|
||||||
#define TEMPERATURE_I2C_BUS BUS_I2C1
|
#define TEMPERATURE_I2C_BUS BUS_I2C1
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -26,14 +26,14 @@
|
||||||
|
|
||||||
const timerHardware_t timerHardware[] = {
|
const timerHardware_t timerHardware[] = {
|
||||||
#ifdef ZEEZF7V2
|
#ifdef ZEEZF7V2
|
||||||
DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR, 0, 0), // S1
|
DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1
|
||||||
DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR, 0, 0), // S2
|
DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2
|
||||||
DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR, 0, 0), // S3
|
DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3
|
||||||
DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR, 0, 0), // S4
|
DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4
|
||||||
DEF_TIM(TIM3, CH3, PC8, TIM_USE_MC_MOTOR, 0, 0), // S5
|
DEF_TIM(TIM3, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5
|
||||||
DEF_TIM(TIM3, CH2, PC7, TIM_USE_MC_MOTOR, 0, 0), // S6
|
DEF_TIM(TIM3, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6
|
||||||
DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR, 0, 0), // S7
|
DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S7
|
||||||
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR, 0, 0), // S8
|
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S8
|
||||||
#else
|
#else
|
||||||
DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR, 0, 0), // S1
|
DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR, 0, 0), // S1
|
||||||
DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR, 0, 0), // S2
|
DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR, 0, 0), // S2
|
||||||
|
|
|
||||||
|
|
@ -87,7 +87,6 @@
|
||||||
#define USE_PITOT
|
#define USE_PITOT
|
||||||
#define USE_PITOT_ADC
|
#define USE_PITOT_ADC
|
||||||
|
|
||||||
#define USE_ALPHA_BETA_GAMMA_FILTER
|
|
||||||
#define USE_DYNAMIC_FILTERS
|
#define USE_DYNAMIC_FILTERS
|
||||||
#define USE_GYRO_KALMAN
|
#define USE_GYRO_KALMAN
|
||||||
#define USE_SMITH_PREDICTOR
|
#define USE_SMITH_PREDICTOR
|
||||||
|
|
@ -190,7 +189,7 @@
|
||||||
//#define USE_MSP_RC_OVERRIDE
|
//#define USE_MSP_RC_OVERRIDE
|
||||||
#define USE_SERIALRX_CRSF
|
#define USE_SERIALRX_CRSF
|
||||||
#define USE_SERIAL_PASSTHROUGH
|
#define USE_SERIAL_PASSTHROUGH
|
||||||
#define NAV_MAX_WAYPOINTS 60
|
#define NAV_MAX_WAYPOINTS 120
|
||||||
#define USE_RCDEVICE
|
#define USE_RCDEVICE
|
||||||
|
|
||||||
//Enable VTX control
|
//Enable VTX control
|
||||||
|
|
|
||||||
|
|
@ -260,6 +260,13 @@
|
||||||
BUSDEV_REGISTER_I2C(busdev_vcm5883, DEVHW_VCM5883, VCM5883_I2C_BUS, 0x0C, NONE, DEVFLAGS_NONE, 0);
|
BUSDEV_REGISTER_I2C(busdev_vcm5883, DEVHW_VCM5883, VCM5883_I2C_BUS, 0x0C, NONE, DEVFLAGS_NONE, 0);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#if defined(USE_MAG_MLX90393)
|
||||||
|
#if !defined(MLX90393_I2C_BUS)
|
||||||
|
#define MLX90393_I2C_BUS MAG_I2C_BUS
|
||||||
|
#endif
|
||||||
|
BUSDEV_REGISTER_I2C(busdev_mlx90393, DEVHW_MLX90393, MLX90393_I2C_BUS, 0x0C, NONE, DEVFLAGS_NONE, 0);
|
||||||
|
#endif
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -332,9 +332,9 @@ static void crsfFrameFlightMode(sbuf_t *dst)
|
||||||
} else if (FLIGHT_MODE(NAV_POSHOLD_MODE)) {
|
} else if (FLIGHT_MODE(NAV_POSHOLD_MODE)) {
|
||||||
flightMode = "HOLD";
|
flightMode = "HOLD";
|
||||||
} else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) {
|
} else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) {
|
||||||
flightMode = "3CRS";
|
flightMode = "CRUZ";
|
||||||
} else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
|
} else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
|
||||||
flightMode = "CRS";
|
flightMode = "CRSH";
|
||||||
} else if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) {
|
} else if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) {
|
||||||
flightMode = "AH";
|
flightMode = "AH";
|
||||||
} else if (FLIGHT_MODE(NAV_WP_MODE)) {
|
} else if (FLIGHT_MODE(NAV_WP_MODE)) {
|
||||||
|
|
|
||||||
|
|
@ -777,7 +777,7 @@ void mavlinkSendBatteryTemperatureStatusText(void)
|
||||||
if (cell < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN) {
|
if (cell < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN) {
|
||||||
batteryVoltages[cell] = getBatteryAverageCellVoltage() * 10;
|
batteryVoltages[cell] = getBatteryAverageCellVoltage() * 10;
|
||||||
} else {
|
} else {
|
||||||
batteryVoltagesExt[cell] = getBatteryAverageCellVoltage() * 10;
|
batteryVoltagesExt[cell-MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN] = getBatteryAverageCellVoltage() * 10;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
||||||
Loading…
Add table
Add a link
Reference in a new issue