1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 00:35:34 +03:00

Merge remote-tracking branch 'origin/master' into dzikuvx-new-msp-frames-for-servos

This commit is contained in:
Pawel Spychalski (DzikuVx) 2024-04-06 09:57:47 +02:00
commit bf9bcdf3c2
67 changed files with 1788 additions and 957 deletions

View file

@ -21,7 +21,6 @@ doc_files=(
'Buzzer.md'
'Sonar.md'
'Profiles.md'
'Modes.md'
'Inflight Adjustments.md'
'Controls.md'
'Gtune.md'
@ -49,7 +48,7 @@ if which gimli >/dev/null; then
done
rm -f ${filename}.pdf
gimli -f ${filename}.md -stylesheet override.css \
-w '--toc --title "Cleanflight Manual" --footer-right "[page]" --toc-depth 1'
-w '--toc --title "INAV Manual" --footer-right "[page]" --toc-depth 1'
rm ${filename}.md
popd >/dev/null
else

View file

@ -47,7 +47,7 @@ function(arm_none_eabi_gcc_install)
host_uname_machine(machine)
if(machine STREQUAL "x86_64" OR machine STREQUAL "amd64")
set(dist ${arm_none_eabi_darwin_amd64})
elseif(machine STREQUAL "aarch64")
elseif(machine STREQUAL "aarch64" OR machine STREQUAL "arm64")
set(dist ${arm_none_eabi_darwin_aarch64})
else()
message("-- no precompiled ${arm_none_eabi_triplet} toolchain for machine ${machine}")

View file

@ -23,7 +23,7 @@ Unassigned slots have rangeStartStep == rangeEndStep. Each element contains the
| Data | Type | Notes |
|------|------|-------|
| permanentId | uint8 | See Modes.md for a definition of the permanent ids |
| permanentId | uint8 | See [Modes in the wiki](https://github.com/iNavFlight/inav/wiki/Modes) for a definition of the permanent ids |
| auxChannelIndex | uint8 | The Aux switch number (indexed from 0) |
| rangeStartStep | uint8 | The start value for this element in 'blocks' of 25 where 0 == 900 and 48 == 2100 |
| rangeEndStep | uint8 | The end value for this element in 'blocks' of 25 where 0 == 900 and 48 == 2100 |
@ -45,7 +45,7 @@ sending this message for all auxiliary slots.
| Data | Type | Notes |
|------|------|-------|
| sequence id | uint8 | A monotonically increasing ID, from 0 to the number of slots -1 |
| permanentId | uint8 | See Modes.md for a definition of the permanent ids |
| permanentId | uint8 | See [Modes in the wiki](https://github.com/iNavFlight/inav/wiki/Modes) for a definition of the permanent ids |
| auxChannelIndex | uint8 | The Aux channel number (indexed from 0) |
| rangeStartStep | uint8 | The start value for this element in 'blocks' of 25 where 0 == 900 and 48 == 2100 |
| rangeEndStep | uint8 | The end value for this element in 'blocks' of 25 where 0 == 900 and 48 == 2100 |
@ -157,5 +157,5 @@ INAV.
See also
--------
Modes.md describes the user visible implementation for the INAV
[The wiki](https://github.com/iNavFlight/inav/wiki/Modes) describes the user visible implementation for the INAV
modes extension.

View file

@ -70,7 +70,7 @@ Now, there are two ways to [configure CF](Configuration.md); via the Configurat
* Verify the range of each channel goes from ~1000 to ~2000. See also [controls](Controls.md). and `rx_min_usec` and `rx_max_usec`.
* You can also set EXPO here instead of your Tx.
* Click Save!
* Modes tab: Setup the desired modes. See the [modes](Modes.md) chapter for what each mode does, but for the beginning you mainly need HORIZON, if any.
* Modes tab: Setup the desired modes. See the [Modes in the wiki](https://github.com/iNavFlight/inav/wiki/Modes) for what each mode does.
* Before finishing this section, you should calibrate the ESCs, install the FC to the frame, and connect the RSSI cable, buzzer and battery if you have chosen to use those.

View file

@ -19,155 +19,156 @@ Not all OSDs are created equally. This table shows the differences between the d
## OSD Elements
Here are the OSD Elements provided by INAV.
| ID | Element | Added |
|-----|--------------------------------------------------|--------|
| 0 | OSD_RSSI_VALUE | 1.0.0 |
| 1 | OSD_MAIN_BATT_VOLTAGE | 1.0.0 |
| 2 | OSD_CROSSHAIRS | 1.0.0 |
| 3 | OSD_ARTIFICIAL_HORIZON | 1.0.0 |
| 4 | OSD_HORIZON_SIDEBARS | 1.0.0 |
| 5 | OSD_ONTIME | 1.0.0 |
| 6 | OSD_FLYTIME | 1.0.0 |
| 7 | OSD_FLYMODE | 1.0.0 |
| 8 | OSD_CRAFT_NAME | 1.0.0 |
| 9 | OSD_THROTTLE_POS | 1.0.0 |
| 10 | OSD_VTX_CHANNEL | 1.0.0 |
| 11 | OSD_CURRENT_DRAW | 1.0.0 |
| 12 | OSD_MAH_DRAWN | 1.0.0 |
| 13 | OSD_GPS_SPEED | 1.0.0 |
| 14 | OSD_GPS_SATS | 1.0.0 |
| 15 | OSD_ALTITUDE | 1.0.0 |
| 16 | OSD_ROLL_PIDS | 1.6.0 |
| 17 | OSD_PITCH_PIDS | 1.6.0 |
| 18 | OSD_YAW_PIDS | 1.6.0 |
| 19 | OSD_POWER | 1.6.0 |
| 20 | OSD_GPS_LON | 1.6.0 |
| 21 | OSD_GPS_LAT | 1.6.0 |
| 22 | OSD_HOME_DIR | 1.6.0 |
| 23 | OSD_HOME_DIST | 1.6.0 |
| 24 | OSD_HEADING | 1.6.0 |
| 25 | OSD_VARIO | 1.6.0 |
| 26 | OSD_VARIO_NUM | 1.6.0 |
| 27 | OSD_AIR_SPEED | 1.7.3 |
| 28 | OSD_ONTIME_FLYTIME | 1.8.0 |
| 29 | OSD_RTC_TIME | 1.8.0 |
| 30 | OSD_MESSAGES | 1.8.0 |
| 31 | OSD_GPS_HDOP | 1.8.0 |
| 32 | OSD_MAIN_BATT_CELL_VOLTAGE | 1.8.0 |
| 33 | OSD_SCALED_THROTTLE_POS | 1.8.0 |
| 34 | OSD_HEADING_GRAPH | 1.8.0 |
| 35 | OSD_EFFICIENCY_MAH_PER_KM | 1.9.0 |
| 36 | OSD_WH_DRAWN | 1.9.0 |
| 37 | OSD_BATTERY_REMAINING_CAPACITY | 1.9.0 |
| 38 | OSD_BATTERY_REMAINING_PERCENT | 1.9.0 |
| 39 | OSD_EFFICIENCY_WH_PER_KM | 1.9.0 |
| 40 | OSD_TRIP_DIST | 1.9.1 |
| 41 | OSD_ATTITUDE_PITCH | 2.0.0 |
| 42 | OSD_ATTITUDE_ROLL | 2.0.0 |
| 43 | OSD_MAP_NORTH | 2.0.0 |
| 44 | OSD_MAP_TAKEOFF | 2.0.0 |
| 45 | OSD_RADAR | 2.0.0 |
| 46 | OSD_WIND_SPEED_HORIZONTAL | 2.0.0 |
| 47 | OSD_WIND_SPEED_VERTICAL | 2.0.0 |
| 48 | OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH | 2.0.0 |
| 49 | OSD_REMAINING_DISTANCE_BEFORE_RTH | 2.0.0 |
| 50 | OSD_HOME_HEADING_ERROR | 2.0.0 |
| 51 | OSD_COURSE_HOLD_ERROR | 2.0.0 |
| 52 | OSD_COURSE_HOLD_ADJUSTMENT | 2.0.0 |
| 53 | OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE | 2.0.0 |
| 54 | OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE | 2.0.0 |
| 55 | OSD_POWER_SUPPLY_IMPEDANCE | 2.0.0 |
| 56 | OSD_LEVEL_PIDS | 2.0.0 |
| 57 | OSD_POS_XY_PIDS | 2.0.0 |
| 58 | OSD_POS_Z_PIDS | 2.0.0 |
| 59 | OSD_VEL_XY_PIDS | 2.0.0 |
| 60 | OSD_VEL_Z_PIDS | 2.0.0 |
| 61 | OSD_HEADING_P | 2.0.0 |
| 62 | OSD_BOARD_ALIGN_ROLL | 2.0.0 |
| 63 | OSD_BOARD_ALIGN_PITCH | 2.0.0 |
| 64 | OSD_RC_EXPO | 2.0.0 |
| 65 | OSD_RC_YAW_EXPO | 2.0.0 |
| 66 | OSD_THROTTLE_EXPO | 2.0.0 |
| 67 | OSD_PITCH_RATE | 2.0.0 |
| 68 | OSD_ROLL_RATE | 2.0.0 |
| 69 | OSD_YAW_RATE | 2.0.0 |
| 70 | OSD_MANUAL_RC_EXPO | 2.0.0 |
| 71 | OSD_MANUAL_RC_YAW_EXPO | 2.0.0 |
| 72 | OSD_MANUAL_PITCH_RATE | 2.0.0 |
| 73 | OSD_MANUAL_ROLL_RATE | 2.0.0 |
| 74 | OSD_MANUAL_YAW_RATE | 2.0.0 |
| 75 | OSD_NAV_FW_CRUISE_THR | 2.0.0 |
| 76 | OSD_NAV_FW_PITCH2THR | 2.0.0 |
| 77 | OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE | 2.0.0 |
| 78 | OSD_DEBUG | 2.0.0 |
| 79 | OSD_FW_ALT_PID_OUTPUTS | 2.0.0 |
| 80 | OSD_FW_POS_PID_OUTPUTS | 2.0.0 |
| 81 | OSD_MC_VEL_X_PID_OUTPUTS | 2.0.0 |
| 82 | OSD_MC_VEL_Y_PID_OUTPUTS | 2.0.0 |
| 83 | OSD_MC_VEL_Z_PID_OUTPUTS | 2.0.0 |
| 84 | OSD_MC_POS_XYZ_P_OUTPUTS | 2.0.0 |
| 85 | OSD_3D_SPEED | 2.1.0 |
| 86 | OSD_IMU_TEMPERATURE | 2.1.0 |
| 87 | OSD_BARO_TEMPERATURE | 2.1.0 |
| 88 | OSD_TEMP_SENSOR_0_TEMPERATURE | 2.1.0 |
| 89 | OSD_TEMP_SENSOR_1_TEMPERATURE | 2.1.0 |
| 90 | OSD_TEMP_SENSOR_2_TEMPERATURE | 2.1.0 |
| 91 | OSD_TEMP_SENSOR_3_TEMPERATURE | 2.1.0 |
| 92 | OSD_TEMP_SENSOR_4_TEMPERATURE | 2.1.0 |
| 93 | OSD_TEMP_SENSOR_5_TEMPERATURE | 2.1.0 |
| 94 | OSD_TEMP_SENSOR_6_TEMPERATURE | 2.1.0 |
| 95 | OSD_TEMP_SENSOR_7_TEMPERATURE | 2.1.0 |
| 96 | OSD_ALTITUDE_MSL | 2.1.0 |
| 97 | OSD_PLUS_CODE | 2.1.0 |
| 98 | OSD_MAP_SCALE | 2.2.0 |
| 99 | OSD_MAP_REFERENCE | 2.2.0 |
| 100 | OSD_GFORCE | 2.2.0 |
| 101 | OSD_GFORCE_X | 2.2.0 |
| 102 | OSD_GFORCE_Y | 2.2.0 |
| 103 | OSD_GFORCE_Z | 2.2.0 |
| 104 | OSD_RC_SOURCE | 2.2.0 |
| 105 | OSD_VTX_POWER | 2.2.0 |
| 106 | OSD_ESC_RPM | 2.3.0 |
| 107 | OSD_ESC_TEMPERATURE | 2.5.0 |
| 108 | OSD_AZIMUTH | 2.6.0 |
| 109 | OSD_CRSF_RSSI_DBM | 2.6.0 |
| 110 | OSD_CRSF_LQ | 2.6.0 |
| 111 | OSD_CRSF_SNR_DB | 2.6.0 |
| 112 | OSD_CRSF_TX_POWER | 2.6.0 |
| 113 | OSD_GVAR_0 | 2.6.0 |
| 114 | OSD_GVAR_1 | 2.6.0 |
| 115 | OSD_GVAR_2 | 2.6.0 |
| 116 | OSD_GVAR_3 | 2.6.0 |
| 117 | OSD_TPA | 2.6.0 |
| 118 | OSD_NAV_FW_CONTROL_SMOOTHNESS | 2.6.0 |
| 119 | OSD_VERSION | 3.0.0 |
| 120 | OSD_RANGEFINDER | 3.0.0 |
| 121 | OSD_PLIMIT_REMAINING_BURST_TIME | 3.0.0 |
| 122 | OSD_PLIMIT_ACTIVE_CURRENT_LIMIT | 3.0.0 |
| 123 | OSD_PLIMIT_ACTIVE_POWER_LIMIT | 3.0.0 |
| 124 | OSD_GLIDESLOPE | 3.0.1 |
| 125 | OSD_GPS_MAX_SPEED | 4.0.0 |
| 126 | OSD_3D_MAX_SPEED | 4.0.0 |
| 127 | OSD_AIR_MAX_SPEED | 4.0.0 |
| 128 | OSD_ACTIVE_PROFILE | 4.0.0 |
| 129 | OSD_MISSION | 4.0.0 |
| 130 | OSD_SWITCH_INDICATOR_0 | 5.0.0 |
| 131 | OSD_SWITCH_INDICATOR_1 | 5.0.0 |
| 132 | OSD_SWITCH_INDICATOR_2 | 5.0.0 |
| 133 | OSD_SWITCH_INDICATOR_3 | 5.0.0 |
| 134 | OSD_TPA_TIME_CONSTANT | 5.0.0 |
| 135 | OSD_FW_LEVEL_TRIM | 5.0.0 |
| 136 | OSD_GLIDE_TIME_REMAINING | 6.0.0 |
| 137 | OSD_GLIDE_RANGE | 6.0.0 |
| 138 | OSD_CLIMB_EFFICIENCY | 6.0.0 |
| 139 | OSD_NAV_WP_MULTI_MISSION_INDEX | 6.0.0 |
| 140 | OSD_GROUND_COURSE | 6.0.0 |
| 141 | OSD_CROSS_TRACK_ERROR | 6.0.0 |
| 142 | OSD_PILOT_NAME | 6.0.0 |
| 143 | OSD_PAN_SERVO_CENTRED | 6.0.0 |
| 144 | OSD_MULTI_FUNCTION | 7.0.0 |
| 145 | OSD_ODOMETER | 7.0.0 |
| 146 | OSD_PILOT_LOGO | 7.0.0 |
| ID | Element | Added | Notes |
|-----|--------------------------------------------------|--------|-------|
| 0 | OSD_RSSI_VALUE | 1.0.0 | |
| 1 | OSD_MAIN_BATT_VOLTAGE | 1.0.0 | |
| 2 | OSD_CROSSHAIRS | 1.0.0 | |
| 3 | OSD_ARTIFICIAL_HORIZON | 1.0.0 | |
| 4 | OSD_HORIZON_SIDEBARS | 1.0.0 | |
| 5 | OSD_ONTIME | 1.0.0 | |
| 6 | OSD_FLYTIME | 1.0.0 | |
| 7 | OSD_FLYMODE | 1.0.0 | |
| 8 | OSD_CRAFT_NAME | 1.0.0 | |
| 9 | OSD_THROTTLE_POS | 1.0.0 | |
| 10 | OSD_VTX_CHANNEL | 1.0.0 | |
| 11 | OSD_CURRENT_DRAW | 1.0.0 | |
| 12 | OSD_MAH_DRAWN | 1.0.0 | |
| 13 | OSD_GPS_SPEED | 1.0.0 | |
| 14 | OSD_GPS_SATS | 1.0.0 | |
| 15 | OSD_ALTITUDE | 1.0.0 | |
| 16 | OSD_ROLL_PIDS | 1.6.0 | |
| 17 | OSD_PITCH_PIDS | 1.6.0 | |
| 18 | OSD_YAW_PIDS | 1.6.0 | |
| 19 | OSD_POWER | 1.6.0 | |
| 20 | OSD_GPS_LON | 1.6.0 | |
| 21 | OSD_GPS_LAT | 1.6.0 | |
| 22 | OSD_HOME_DIR | 1.6.0 | |
| 23 | OSD_HOME_DIST | 1.6.0 | |
| 24 | OSD_HEADING | 1.6.0 | |
| 25 | OSD_VARIO | 1.6.0 | |
| 26 | OSD_VARIO_NUM | 1.6.0 | |
| 27 | OSD_AIR_SPEED | 1.7.3 | |
| 28 | OSD_ONTIME_FLYTIME | 1.8.0 | |
| 29 | OSD_RTC_TIME | 1.8.0 | |
| 30 | OSD_MESSAGES | 1.8.0 | |
| 31 | OSD_GPS_HDOP | 1.8.0 | |
| 32 | OSD_MAIN_BATT_CELL_VOLTAGE | 1.8.0 | |
| 33 | OSD_SCALED_THROTTLE_POS | 1.8.0 | |
| 34 | OSD_HEADING_GRAPH | 1.8.0 | |
| 35 | OSD_EFFICIENCY_MAH_PER_KM | 1.9.0 | |
| 36 | OSD_WH_DRAWN | 1.9.0 | |
| 37 | OSD_BATTERY_REMAINING_CAPACITY | 1.9.0 | |
| 38 | OSD_BATTERY_REMAINING_PERCENT | 1.9.0 | |
| 39 | OSD_EFFICIENCY_WH_PER_KM | 1.9.0 | |
| 40 | OSD_TRIP_DIST | 1.9.1 | |
| 41 | OSD_ATTITUDE_PITCH | 2.0.0 | |
| 42 | OSD_ATTITUDE_ROLL | 2.0.0 | |
| 43 | OSD_MAP_NORTH | 2.0.0 | |
| 44 | OSD_MAP_TAKEOFF | 2.0.0 | |
| 45 | OSD_RADAR | 2.0.0 | |
| 46 | OSD_WIND_SPEED_HORIZONTAL | 2.0.0 | |
| 47 | OSD_WIND_SPEED_VERTICAL | 2.0.0 | |
| 48 | OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH | 2.0.0 | |
| 49 | OSD_REMAINING_DISTANCE_BEFORE_RTH | 2.0.0 | |
| 50 | OSD_HOME_HEADING_ERROR | 2.0.0 | |
| 51 | OSD_COURSE_HOLD_ERROR | 2.0.0 | |
| 52 | OSD_COURSE_HOLD_ADJUSTMENT | 2.0.0 | |
| 53 | OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE | 2.0.0 | |
| 54 | OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE | 2.0.0 | |
| 55 | OSD_POWER_SUPPLY_IMPEDANCE | 2.0.0 | |
| 56 | OSD_LEVEL_PIDS | 2.0.0 | |
| 57 | OSD_POS_XY_PIDS | 2.0.0 | |
| 58 | OSD_POS_Z_PIDS | 2.0.0 | |
| 59 | OSD_VEL_XY_PIDS | 2.0.0 | |
| 60 | OSD_VEL_Z_PIDS | 2.0.0 | |
| 61 | OSD_HEADING_P | 2.0.0 | |
| 62 | OSD_BOARD_ALIGN_ROLL | 2.0.0 | |
| 63 | OSD_BOARD_ALIGN_PITCH | 2.0.0 | |
| 64 | OSD_RC_EXPO | 2.0.0 | |
| 65 | OSD_RC_YAW_EXPO | 2.0.0 | |
| 66 | OSD_THROTTLE_EXPO | 2.0.0 | |
| 67 | OSD_PITCH_RATE | 2.0.0 | |
| 68 | OSD_ROLL_RATE | 2.0.0 | |
| 69 | OSD_YAW_RATE | 2.0.0 | |
| 70 | OSD_MANUAL_RC_EXPO | 2.0.0 | |
| 71 | OSD_MANUAL_RC_YAW_EXPO | 2.0.0 | |
| 72 | OSD_MANUAL_PITCH_RATE | 2.0.0 | |
| 73 | OSD_MANUAL_ROLL_RATE | 2.0.0 | |
| 74 | OSD_MANUAL_YAW_RATE | 2.0.0 | |
| 75 | OSD_NAV_FW_CRUISE_THR | 2.0.0 | |
| 76 | OSD_NAV_FW_PITCH2THR | 2.0.0 | |
| 77 | OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE | 2.0.0 | |
| 78 | OSD_DEBUG | 2.0.0 | |
| 79 | OSD_FW_ALT_PID_OUTPUTS | 2.0.0 | |
| 80 | OSD_FW_POS_PID_OUTPUTS | 2.0.0 | |
| 81 | OSD_MC_VEL_X_PID_OUTPUTS | 2.0.0 | |
| 82 | OSD_MC_VEL_Y_PID_OUTPUTS | 2.0.0 | |
| 83 | OSD_MC_VEL_Z_PID_OUTPUTS | 2.0.0 | |
| 84 | OSD_MC_POS_XYZ_P_OUTPUTS | 2.0.0 | |
| 85 | OSD_3D_SPEED | 2.1.0 | |
| 86 | OSD_IMU_TEMPERATURE | 2.1.0 | |
| 87 | OSD_BARO_TEMPERATURE | 2.1.0 | |
| 88 | OSD_TEMP_SENSOR_0_TEMPERATURE | 2.1.0 | |
| 89 | OSD_TEMP_SENSOR_1_TEMPERATURE | 2.1.0 | |
| 90 | OSD_TEMP_SENSOR_2_TEMPERATURE | 2.1.0 | |
| 91 | OSD_TEMP_SENSOR_3_TEMPERATURE | 2.1.0 | |
| 92 | OSD_TEMP_SENSOR_4_TEMPERATURE | 2.1.0 | |
| 93 | OSD_TEMP_SENSOR_5_TEMPERATURE | 2.1.0 | |
| 94 | OSD_TEMP_SENSOR_6_TEMPERATURE | 2.1.0 | |
| 95 | OSD_TEMP_SENSOR_7_TEMPERATURE | 2.1.0 | |
| 96 | OSD_ALTITUDE_MSL | 2.1.0 | |
| 97 | OSD_PLUS_CODE | 2.1.0 | |
| 98 | OSD_MAP_SCALE | 2.2.0 | |
| 99 | OSD_MAP_REFERENCE | 2.2.0 | |
| 100 | OSD_GFORCE | 2.2.0 | |
| 101 | OSD_GFORCE_X | 2.2.0 | |
| 102 | OSD_GFORCE_Y | 2.2.0 | |
| 103 | OSD_GFORCE_Z | 2.2.0 | |
| 104 | OSD_RC_SOURCE | 2.2.0 | |
| 105 | OSD_VTX_POWER | 2.2.0 | |
| 106 | OSD_ESC_RPM | 2.3.0 | |
| 107 | OSD_ESC_TEMPERATURE | 2.5.0 | |
| 108 | OSD_AZIMUTH | 2.6.0 | |
| 109 | OSD_CRSF_RSSI_DBM | 2.6.0 | |
| 110 | OSD_CRSF_LQ | 2.6.0 | |
| 111 | OSD_CRSF_SNR_DB | 2.6.0 | |
| 112 | OSD_CRSF_TX_POWER | 2.6.0 | |
| 113 | OSD_GVAR_0 | 2.6.0 | |
| 114 | OSD_GVAR_1 | 2.6.0 | |
| 115 | OSD_GVAR_2 | 2.6.0 | |
| 116 | OSD_GVAR_3 | 2.6.0 | |
| 117 | OSD_TPA | 2.6.0 | |
| 118 | OSD_NAV_FW_CONTROL_SMOOTHNESS | 2.6.0 | |
| 119 | OSD_VERSION | 3.0.0 | |
| 120 | OSD_RANGEFINDER | 3.0.0 | |
| 121 | OSD_PLIMIT_REMAINING_BURST_TIME | 3.0.0 | |
| 122 | OSD_PLIMIT_ACTIVE_CURRENT_LIMIT | 3.0.0 | |
| 123 | OSD_PLIMIT_ACTIVE_POWER_LIMIT | 3.0.0 | |
| 124 | OSD_GLIDESLOPE | 3.0.1 | |
| 125 | OSD_GPS_MAX_SPEED | 4.0.0 | |
| 126 | OSD_3D_MAX_SPEED | 4.0.0 | |
| 127 | OSD_AIR_MAX_SPEED | 4.0.0 | |
| 128 | OSD_ACTIVE_PROFILE | 4.0.0 | |
| 129 | OSD_MISSION | 4.0.0 | |
| 130 | OSD_SWITCH_INDICATOR_0 | 5.0.0 | |
| 131 | OSD_SWITCH_INDICATOR_1 | 5.0.0 | |
| 132 | OSD_SWITCH_INDICATOR_2 | 5.0.0 | |
| 133 | OSD_SWITCH_INDICATOR_3 | 5.0.0 | |
| 134 | OSD_TPA_TIME_CONSTANT | 5.0.0 | |
| 135 | OSD_FW_LEVEL_TRIM | 5.0.0 | |
| 136 | OSD_GLIDE_TIME_REMAINING | 6.0.0 | |
| 137 | OSD_GLIDE_RANGE | 6.0.0 | |
| 138 | OSD_CLIMB_EFFICIENCY | 6.0.0 | |
| 139 | OSD_NAV_WP_MULTI_MISSION_INDEX | 6.0.0 | |
| 140 | OSD_GROUND_COURSE | 6.0.0 | |
| 141 | OSD_CROSS_TRACK_ERROR | 6.0.0 | |
| 142 | OSD_PILOT_NAME | 6.0.0 | |
| 143 | OSD_PAN_SERVO_CENTRED | 6.0.0 | |
| 144 | OSD_MULTI_FUNCTION | 7.0.0 | |
| 145 | OSD_ODOMETER | 7.0.0 | For this to work correctly, stats must be enabled (`set stats=ON`). Otherwise, this will show the total flight distance. |
| 146 | OSD_PILOT_LOGO | 7.0.0 | |
| 147 | OSD_BLACKBOX | 8.0.0 | The element will be hidden unless blackbox recording is attempted. |
# Pilot Logos
@ -192,3 +193,43 @@ This is an example of the arming screen with the pilot logo enabled. This is usi
This is an example of setting the `osd_inav_to_pilot_logo_spacing` to 0. This will allow a larger, single logo.
![Power on screen example with 0 spacing between logos](https://user-images.githubusercontent.com/17590174/271817352-6206402c-9da4-4682-9d83-790cc2396b00.png)
# Post Flight Statistics
The post flight statistcs are set in the firmware. Statistics are only hidden if the supporting hardware is not present. Due to size contraints. The post flight statistics are spread over 2 pages on analogue systems.
## Statistics shown
| Statistic | Requirement | Page | |
|-------------------------------|-----------------------|-------|-|
| Flight Time | | 1 | The total time from arm to disarm. |
| Flight Distance | | 1 | |
| Maximum Distance From Home | GPS | 1 | |
| Maximum Speed | GPS | 1 | |
| Average Speed | GPS | 1 | |
| Maximum Altitude | Baro/GPS | 1 | |
| Minimum Average Cell Voltage | | 1 | |
| Minimum Pack Voltage | | 1 | |
| Maximum Current | Current Sensor | 1 | |
| Maximum Power | Current Sensor | 1 | |
| Energy Used (Flight) | Current Sensor | 1 | |
| Energy Used (Battery Total) | Current Sensor | 1 | This data is not reset on arming. |
| Average Efficiency | Current Sensor & GPS | 1 | |
| Minimum RSSI | | 2 | |
| Minimum LQ | CRSF | 2 | |
| Minmum dBm | CRSF | 2 | |
| Minimum Satellites | GPS | 2 | |
| Maximum Satellites | GPS | 2 | |
| Minimum ESC Temperature | ESC Telemetry | 2 | |
| Maximum ESC Temperature | ESC Telemetry | 2 | |
| Maximum G-Force | | 2 | |
| Minimum Z axis G-Force | | 2 | |
| Maximum Z axis G-Force | | 2 | |
| Blackbox file number | Blackbox recording | 2 | |
| Disarm method | | 1 & 2 | |
| Settings save status | | 1 & 2 | Shows a message if the settings are being saved or have been saved on disarm. |
## Configuration
There are a couple of settings that allow you to adjust parts of the post flights statistics.
- `osd_stats_page_auto_swap_time` allows you to specify how long each stats page is displayed [seconds]. Reverts to manual control when Roll stick used to change pages. Disabled when set to 0.
- `osd_stats_energy_unit` allows you to choose the unit used for the drawn energy in the OSD stats [MAH/WH] (milliAmpere hour/ Watt hour). Default is MAH.
- `osd_stats_show_metric_efficiency` if you use non-metric units on your OSD. Enabling this option will also show the efficiency in metric.

View file

@ -20,6 +20,7 @@ Following rangefinders are supported:
* UIB - experimental
* MSP - experimental
* TOF10120 - small & lightweight laser range sensor, usable up to 200cm
* TERARANGER EVO - 30cm to 600cm, depends on version https://www.terabee.com/sensors-modules/lidar-tof-range-finders/#individual-distance-measurement-sensors
## Connections

View file

@ -6,7 +6,7 @@ As many can attest, multirotors and RC models in general can be very dangerous,
## Before Installing
Please consult the [Cli](Cli.md), [Controls](Controls.md), [Failsafe](Failsafe.md) and [Modes](Modes.md)
Please consult the [Cli](Cli.md), [Controls](Controls.md), [Failsafe](Failsafe.md) and [Modes](https://github.com/iNavFlight/inav/wiki/Modes)
pages for further important information.
You are highly advised to use the Receiver tab in the INAV Configurator, making sure your Rx channel

View file

@ -752,16 +752,6 @@ Re-purpose the craft name field for messages.
---
### dji_workarounds
Enables workarounds for different versions of MSP protocol used
| Default | Min | Max |
| --- | --- | --- |
| 1 | 0 | 255 |
---
### dshot_beeper_enabled
Whether using DShot motors as beepers is enabled
@ -1572,16 +1562,6 @@ Gyro processing anti-aliasing filter cutoff frequency. In normal operation this
---
### gyro_anti_aliasing_lpf_type
Specifies the type of the software LPF of the gyro signals.
| Default | Min | Max |
| --- | --- | --- |
| PT1 | | |
---
### gyro_dyn_lpf_curve_expo
Expo value for the throttle-to-frequency mapping for Dynamic LPF
@ -1612,16 +1592,6 @@ Minimum frequency of the gyro Dynamic LPF
---
### gyro_hardware_lpf
Hardware lowpass filter for gyro. This value should never be changed without a very strong reason! If you have to set gyro lpf below 256HZ, it means the frame is vibrating too much, and that should be fixed first.
| Default | Min | Max |
| --- | --- | --- |
| 256HZ | | |
---
### gyro_main_lpf_hz
Software based gyro main lowpass filter. Value is cutoff frequency (Hz)
@ -1632,16 +1602,6 @@ Software based gyro main lowpass filter. Value is cutoff frequency (Hz)
---
### gyro_main_lpf_type
Defines the type of the main gyro LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation.
| Default | Min | Max |
| --- | --- | --- |
| BIQUAD | | |
---
### gyro_to_use
On multi-gyro targets, allows to choose which gyro to use. 0 = first gyro, 1 = second gyro
@ -2964,11 +2924,11 @@ Max. tailwind (in cm/s) if no landing direction with downwind is available
### nav_fw_launch_accel
Forward acceleration threshold for bungee launch of throw launch [cm/s/s], 1G = 981 cm/s/s
Forward acceleration threshold for bungee launch or throw launch [cm/s/s], 1G = 981 cm/s/s
| Default | Min | Max |
| --- | --- | --- |
| 1863 | 1000 | 20000 |
| 1863 | 1500 | 20000 |
---
@ -3122,6 +3082,16 @@ Forward velocity threshold for swing-launch detection [cm/s]
---
### nav_fw_launch_wiggle_to_wake_idle
Trigger the idle throttle by wiggling the plane. 0 = disabled. 1 and 2 signify 1 or 2 yaw wiggles to activate. 1 wiggle has a higher detection point, for airplanes without a tail. 2 wiggles has a lower detection point, but requires the repeated action. This is intended for larger models and airplanes with tails.
| Default | Min | Max |
| --- | --- | --- |
| 0 | 0 | 2 |
---
### nav_fw_loiter_radius
PosHold radius. 3000 to 7500 is a good value (30-75m) [cm]
@ -4362,6 +4332,16 @@ Use wind estimation for remaining flight time/distance estimation
---
### osd_estimations_wind_mps
Wind speed estimation in m/s
| Default | Min | Max |
| --- | --- | --- |
| OFF | OFF | ON |
---
### osd_failsafe_switch_layout
If enabled the OSD automatically switches to the first layout during failsafe
@ -4882,16 +4862,6 @@ Unit used for the drawn energy in the OSD stats [MAH/WH] (milliAmpere hour/ Watt
---
### osd_stats_min_voltage_unit
Display minimum voltage of the `BATTERY` or the average per `CELL` in the OSD stats.
| Default | Min | Max |
| --- | --- | --- |
| BATTERY | | |
---
### osd_stats_page_auto_swap_time
Auto swap display time interval between disarm stats pages (seconds). Reverts to manual control when Roll stick used to change pages. Disabled when set to 0.
@ -4902,6 +4872,16 @@ Auto swap display time interval between disarm stats pages (seconds). Reverts to
---
### osd_stats_show_metric_efficiency
Enabling this option will show metric efficiency statistics on the post flight stats screen. In addition to the efficiency statistics in your chosen units.
| Default | Min | Max |
| --- | --- | --- |
| OFF | OFF | ON |
---
### osd_switch_indicator_one_channel
RC Channel to use for OSD switch indicator 1.

View file

@ -2,4 +2,8 @@
Contrary to what the documentation suggests, VTX power is actually on USER2.
# Dual Gyros
INAV 7.1 changed the default gyro of the board from the gyro on SPI4 back to the one on SPI1. A new tagrt ```MAMBAH743_2022B_GYRO2``` was added to use gyro on SPI4, in case you suspect an issue with the gyro on SPI1, you can switch to the gyro on SPI4 by using the new target.

View file

@ -1,6 +1,6 @@
# Building in Windows 10 with Linux subsystem [Recommended]
# Building in Windows 10/11 with Linux subsystem (WSL) [Recommended]
Linux subsystem for Windows 10 is probably the simplest way of building INAV under Windows 10.
Linux subsystem for Windows (WSL) 10/11 is probably the simplest way of building INAV under Windows.
## Setting up the environment
@ -9,7 +9,6 @@ run `windows features`
enable `windows subsytem for linux`
reboot
Install Ubuntu:
1. Go to Microsoft store https://www.microsoft.com/en-gb/store/b/home
1. Search and install most recent Ubuntu LTS version
@ -56,12 +55,12 @@ You can fix this with by remounting the drive using the following commands
1. `sudo umount /mnt/c`
2. `sudo mount -t drvfs C: /mnt/c -o metadata`
## Building (example):
## Building with Make (example):
For detailed build instrusctions see [Building in Linux](Building%20in%20Linux.md)
For detailed build instructions see [Building in Linux](Building%20in%20Linux.md)
Launch Ubuntu:
Click Windows Start button then scroll and lauch "Ubuntu"
Click Windows Start button then scroll and launch "Ubuntu"
Building from Ubuntu command line
@ -80,6 +79,39 @@ cd build
make MATEKF722
```
## Building with Ninja (example):
[Ninja](https://ninja-build.org/) is a popular cross-platform tool. It is both lightweight and executes parallel builds by default. It is advantageous to use this over the old _make_ method. There are detailed instructions for building with Ninja in [Building in Linux](Building%20in%20Linux.md#building-with-ninja).
Launch Ubuntu:
Click Windows Start button. Then scroll and launch **Ubuntu**.
> [!TIP]
> Before using Ninja, you will need to install it. From the Ubuntu command prompt type `sudo apt-get install ninja-build -y` and press enter.
Building from the command line:
First, change to the INAV directory with
```cd /mnt/c/inav```
Before building, you will need to prepare the build environment. You only need to do this once, unless you reinstall WSL or cmake.
```
mkdir build
cd build
cmake -GNinja ..
```
From then on, you can build your target by calling the following from inside the build directory.
```
ninja MATEKF722
```
If you want to build multiple targets. You can use:
```
ninja MATEKF722 MATEKF405SE SPEEDYBEEF405
```
## Updating the documents
```
cd /mnt/c/inav

View file

@ -1,6 +1,6 @@
# INAV - navigation capable flight controller
# PSA
# F411 PSA
> INAV no longer accepts targets based on STM32 F411 MCU.
@ -8,11 +8,32 @@
![INAV](http://static.rcgroups.net/forums/attachments/6/1/0/3/7/6/a9088858-102-inav.png)
# PosHold, Navigation and RTH without compass PSA
Attention all drone pilots and enthusiasts,
Are you ready to take your flights to new heights with INAV 7.1? We've got some important information to share with you.
INAV 7.1 brings an exciting update to navigation capabilities. Now, you can soar through the skies, navigate waypoints, and even return to home without relying on a compass. Yes, you heard that right! But before you launch into the air, there's something crucial to consider.
While INAV 7.1 may not require a compass for basic navigation functions, we strongly advise you to install one for optimal flight performance. Here's why:
🛰️ Better Flight Precision: A compass provides essential data for accurate navigation, ensuring smoother and more precise flight paths.
🌐 Enhanced Reliability: With a compass onboard, your drone can maintain stability even in challenging environments, low speeds and strong wind.
🚀 Minimize Risks: Although INAV 7.1 can get you where you need to go without a compass, flying without one may result in a bumpier ride and increased risk of drift or inaccurate positioning.
Remember, safety and efficiency are paramount when operating drones. By installing a compass, you're not just enhancing your flight experience, but also prioritizing safety for yourself and those around you.
So, before you take off on your next adventure, make sure to equip your drone with a compass. It's the smart choice for smoother flights and better navigation.
Fly safe, fly smart with INAV 7.1 and a compass by your side!
# INAV Community
* [INAV Discord Server](https://discord.gg/peg2hhbYwN)
* [INAV Official on Facebook](https://www.facebook.com/groups/INAVOfficial)
* [INAV Official on Telegram](https://t.me/INAVFlight)
## Features
@ -22,14 +43,15 @@
* Outstanding performance out of the box
* Position Hold, Altitude Hold, Return To Home and Waypoint Missions
* Excellent support for fixed wing UAVs: airplanes, flying wings
* Blackbox flight recorder logging
* Advanced gyro filtering
* Fully configurable mixer that allows to run any hardware you want: multirotor, fixed wing, rovers, boats and other experimental devices
* Multiple sensor support: GPS, Pitot tube, sonar, lidar, temperature, ESC with BlHeli_32 telemetry
* Logic Conditions, Global Functions and Global Variables: you can program INAV with a GUI
* SmartAudio and IRC Tramp VTX support
* Blackbox flight recorder logging
* Telemetry: SmartPort, FPort, MAVlink, LTM, CRSF
* Multi-color RGB LED Strip support
* Advanced gyro filtering
* Logic Conditions, Global Functions and Global Variables: you can program INAV with a GUI
* On Screen Display (OSD) - both character and pixel style
* And many more!
For a list of features, changes and some discussion please review consult the releases [page](https://github.com/iNavFlight/inav/releases) and the documentation.
@ -52,10 +74,6 @@ Command line tools (`blackbox_decode`, `blackbox_render`) for Blackbox log conve
Users of EdgeTX and OpenTX radios (Taranis, Horus, Jumper, Radiomaster, Nirvana) can use INAV OpenTX Telemetry Widget screen. Software and installation instruction are available here: [https://github.com/iNavFlight/OpenTX-Telemetry-Widget](https://github.com/iNavFlight/OpenTX-Telemetry-Widget)
### INAV magnetometer alignment helper
[INAV Magnetometer Alignment helper](https://kernel-machine.github.io/INavMagAlignHelper/) allows to align INAV magnetometer despite position and orientation. This simplifies the process of INAV setup on multirotors with tilted GPS modules.
### OSD layout Copy, Move, or Replace helper tool
[Easy INAV OSD switcher tool](https://www.mrd-rc.com/tutorials-tools-and-testing/useful-tools/inav-osd-switcher-tool/) allows you to easily switch your OSD layouts around in INAV. Choose the from and to OSD layouts, and the method of transfering the layouts.

View file

@ -237,6 +237,8 @@ main_sources(COMMON_SRC
drivers/rangefinder/rangefinder_us42.h
drivers/rangefinder/rangefinder_tof10120_i2c.c
drivers/rangefinder/rangefinder_tof10120_i2c.h
drivers/rangefinder/rangefinder_teraranger_evo.c
drivers/rangefinder/rangefinder_teraranger_evo.h
drivers/resource.c
drivers/resource.h

View file

@ -1915,9 +1915,8 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_type", "%d", pidProfile()->dterm_lpf_type);
BLACKBOX_PRINT_HEADER_LINE("deadband", "%d", rcControlsConfig()->deadband);
BLACKBOX_PRINT_HEADER_LINE("yaw_deadband", "%d", rcControlsConfig()->yaw_deadband);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf", "%d", gyroConfig()->gyro_lpf);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf", "%d", GYRO_LPF_256HZ);
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);
#ifdef USE_DYNAMIC_FILTERS
BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchQ", "%d", gyroConfig()->dynamicGyroNotchQ);
BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchMinHz", "%d", gyroConfig()->dynamicGyroNotchMinHz);

View file

@ -35,6 +35,10 @@
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#ifdef USE_SDCARD
#include "drivers/sdcard/sdcard.h"
#endif
#include "io/asyncfatfs/asyncfatfs.h"
#include "io/flashfs.h"
#include "io/serial.h"
@ -507,6 +511,36 @@ bool isBlackboxDeviceFull(void)
}
}
bool isBlackboxDeviceWorking(void)
{
switch (blackboxConfig()->device) {
case BLACKBOX_DEVICE_SERIAL:
return blackboxPort != NULL;
#ifdef USE_SDCARD
case BLACKBOX_DEVICE_SDCARD:
return sdcard_isInserted() && sdcard_isFunctional() && (afatfs_getFilesystemState() == AFATFS_FILESYSTEM_STATE_READY);
#endif
#ifdef USE_FLASHFS
case BLACKBOX_DEVICE_FLASH:
return flashfsIsReady();
#endif
default:
return false;
}
}
int32_t blackboxGetLogNumber(void)
{
switch (blackboxConfig()->device) {
#ifdef USE_SDCARD
case BLACKBOX_DEVICE_SDCARD:
return blackboxSDCard.largestLogFileNumber;
#endif
default:
return -1;
}
}
/**
* Call once every loop iteration in order to maintain the global blackboxHeaderBudget with the number of bytes we can
* transmit this iteration.

View file

@ -67,6 +67,8 @@ bool blackboxDeviceBeginLog(void);
bool blackboxDeviceEndLog(bool retainLog);
bool isBlackboxDeviceFull(void);
bool isBlackboxDeviceWorking(void);
int32_t blackboxGetLogNumber(void);
void blackboxReplenishHeaderBudget(void);
blackboxBufferReserveStatus_e blackboxDeviceReserveBufferSpace(int32_t bytes);

View file

@ -422,7 +422,6 @@ static const CMS_Menu cmsx_menuProfileOther = {
static const OSD_Entry cmsx_menuFilterPerProfileEntries[] =
{
OSD_LABEL_DATA_ENTRY("-- FILTERING --", profileIndexString),
OSD_SETTING_ENTRY("HARDWARE LPF", SETTING_GYRO_HARDWARE_LPF),
OSD_SETTING_ENTRY("GYRO MAIN", SETTING_GYRO_MAIN_LPF_HZ),
OSD_SETTING_ENTRY("DTERM LPF", SETTING_DTERM_LPF_HZ),
#ifdef USE_DYNAMIC_FILTERS

View file

@ -56,6 +56,8 @@
#define RADIANS_TO_CENTIDEGREES(angle) (((angle) * 100.0f) / RAD)
#define CENTIDEGREES_TO_RADIANS(angle) (((angle) * 0.01f) * RAD)
#define MILLIMETERS_TO_CENTIMETERS(mm) (mm / 10.0f)
#define CENTIMETERS_TO_CENTIFEET(cm) (cm / 0.3048f)
#define CENTIMETERS_TO_FEET(cm) (cm / 30.48f)
#define CENTIMETERS_TO_METERS(cm) (cm * 0.01f)
@ -65,6 +67,8 @@
#define METERS_TO_MILES(m) (m / 1609.344f)
#define METERS_TO_NAUTICALMILES(m) (m / 1852.00f)
#define MWH_TO_WH(mWh) (mWh / 1000.0f)
#define CMSEC_TO_CENTIMPH(cms) (cms * 2.2369363f)
#define CMSEC_TO_CENTIKPH(cms) (cms * 3.6f)
#define CMSEC_TO_CENTIKNOTS(cms) (cms * 1.943845f)

View file

@ -108,15 +108,6 @@ static const gyroFilterAndRateConfig_t gyroConfigs[] = {
{ GYRO_LPF_256HZ, 3200, { BMI160_BWP_OSR4 | BMI160_ODR_3200_Hz} },
{ GYRO_LPF_256HZ, 1600, { BMI160_BWP_OSR2 | BMI160_ODR_1600_Hz} },
{ GYRO_LPF_256HZ, 800, { BMI160_BWP_NORMAL | BMI160_ODR_800_Hz } },
{ GYRO_LPF_188HZ, 800, { BMI160_BWP_OSR2 | BMI160_ODR_800_Hz } }, // ODR = 800 Hz, LPF = 128 Hz
{ GYRO_LPF_188HZ, 400, { BMI160_BWP_NORMAL | BMI160_ODR_400_Hz } }, // ODR = 400 Hz, LPF = 137 Hz
{ GYRO_LPF_98HZ, 800, { BMI160_BWP_OSR4 | BMI160_ODR_800_Hz } }, // ODR = 800 Hz, LPF = 63 Hz
{ GYRO_LPF_98HZ, 400, { BMI160_BWP_OSR2 | BMI160_ODR_400_Hz } }, // ODR = 400 Hz, LPF = 68 Hz
{ GYRO_LPF_42HZ, 800, { BMI160_BWP_OSR4 | BMI160_ODR_800_Hz } }, // ODR = 800 Hz, LPF = 63 Hz
{ GYRO_LPF_42HZ, 400, { BMI160_BWP_OSR4 | BMI160_ODR_400_Hz } }, // ODR = 400 Hz, LPF = 34 Hz
};
static void bmi160AccAndGyroInit(gyroDev_t *gyro)

View file

@ -134,15 +134,6 @@ static const gyroFilterAndRateConfig_t gyroConfigs[] = {
{ GYRO_LPF_256HZ, 3200, { BMI270_BWP_OSR4 | BMI270_ODR_3200} },
{ GYRO_LPF_256HZ, 1600, { BMI270_BWP_OSR2 | BMI270_ODR_1600} },
{ GYRO_LPF_256HZ, 800, { BMI270_BWP_NORM | BMI270_ODR_800 } },
{ GYRO_LPF_188HZ, 800, { BMI270_BWP_OSR2 | BMI270_ODR_800 } },
{ GYRO_LPF_188HZ, 400, { BMI270_BWP_NORM | BMI270_ODR_400 } },
{ GYRO_LPF_98HZ, 800, { BMI270_BWP_OSR4 | BMI270_ODR_800 } },
{ GYRO_LPF_98HZ, 400, { BMI270_BWP_OSR2 | BMI270_ODR_400 } },
{ GYRO_LPF_42HZ, 800, { BMI270_BWP_OSR4 | BMI270_ODR_800 } },
{ GYRO_LPF_42HZ, 400, { BMI270_BWP_OSR4 | BMI270_ODR_400 } },
};
// Toggle the CS to switch the device into SPI mode.

View file

@ -198,21 +198,6 @@ static const gyroFilterAndRateConfig_t icm42605GyroConfigs[] = {
{ GYRO_LPF_256HZ, 2000, { 3, 5 } }, /* 250 Hz LPF */
{ GYRO_LPF_256HZ, 1000, { 1, 6 } }, /* 250 Hz LPF */
{ GYRO_LPF_256HZ, 500, { 0, 15 } }, /* 250 Hz LPF */
{ GYRO_LPF_188HZ, 1000, { 3, 6 } }, /* 125 HZ */
{ GYRO_LPF_188HZ, 500, { 1, 15 } }, /* 125 HZ */
{ GYRO_LPF_98HZ, 1000, { 4, 6 } }, /* 100 HZ*/
{ GYRO_LPF_98HZ, 500, { 2, 15 } }, /* 100 HZ*/
{ GYRO_LPF_42HZ, 1000, { 6, 6 } }, /* 50 HZ */
{ GYRO_LPF_42HZ, 500, { 4, 15 } },
{ GYRO_LPF_20HZ, 1000, { 7, 6 } }, /* 25 HZ */
{ GYRO_LPF_20HZ, 500, { 6, 15 } },
{ GYRO_LPF_10HZ, 1000, { 7, 6 } }, /* 25 HZ */
{ GYRO_LPF_10HZ, 500, { 7, 15 } } /* 12.5 HZ */
};
static void icm42605AccAndGyroInit(gyroDev_t *gyro)

View file

@ -50,21 +50,6 @@ static const gyroFilterAndRateConfig_t mpuGyroConfigs[] = {
{ GYRO_LPF_256HZ, 1000, { MPU_DLPF_256HZ, 7 } },
{ GYRO_LPF_256HZ, 666, { MPU_DLPF_256HZ, 11 } },
{ GYRO_LPF_256HZ, 500, { MPU_DLPF_256HZ, 15 } },
{ GYRO_LPF_188HZ, 1000, { MPU_DLPF_188HZ, 0 } },
{ GYRO_LPF_188HZ, 500, { MPU_DLPF_188HZ, 1 } },
{ GYRO_LPF_98HZ, 1000, { MPU_DLPF_98HZ, 0 } },
{ GYRO_LPF_98HZ, 500, { MPU_DLPF_98HZ, 1 } },
{ GYRO_LPF_42HZ, 1000, { MPU_DLPF_42HZ, 0 } },
{ GYRO_LPF_42HZ, 500, { MPU_DLPF_42HZ, 1 } },
{ GYRO_LPF_20HZ, 1000, { MPU_DLPF_20HZ, 0 } },
{ GYRO_LPF_20HZ, 500, { MPU_DLPF_20HZ, 1 } },
{ GYRO_LPF_10HZ, 1000, { MPU_DLPF_10HZ, 0 } },
{ GYRO_LPF_10HZ, 500, { MPU_DLPF_10HZ, 1 } }
};
const gyroFilterAndRateConfig_t * mpuChooseGyroConfig(uint8_t desiredLpf, uint16_t desiredRateHz)

View file

@ -135,6 +135,7 @@ typedef enum {
DEVHW_VL53L1X,
DEVHW_US42,
DEVHW_TOF10120_I2C,
DEVHW_TERARANGER_EVO_I2C,
/* Other hardware */
DEVHW_MS4525, // Pitot meter

View file

@ -178,6 +178,8 @@
#define SYM_ALERT 0xDD // 221 General alert symbol
#define SYM_TERRAIN_FOLLOWING 0xFB // 251 Terrain following (also Alt adjust)
#define SYM_CROSS_TRACK_ERROR 0xFC // 252 Cross track error
#define SYM_ADSB 0xFD // 253 ADBS
#define SYM_BLACKBOX 0xFE // 254 Blackbox
#define SYM_ADSB 0xFD // 253 ADSB

View file

@ -0,0 +1,128 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight 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.
*
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include "platform.h"
#include "common/maths.h"
#if defined(USE_RANGEFINDER) && defined(USE_RANGEFINDER_TERARANGER_EVO_I2C)
#include "build/build_config.h"
#include "common/crc.h"
#include "drivers/time.h"
#include "drivers/bus_i2c.h"
#include "drivers/rangefinder/rangefinder.h"
#include "drivers/rangefinder/rangefinder_teraranger_evo.h"
#include "build/debug.h"
#define TERARANGER_EVO_DETECTION_CONE_DECIDEGREES 900
#define TERARANGER_EVO_DETECTION_CONE_EXTENDED_DECIDEGREES 900
#define TERARANGER_EVO_I2C_ADDRESS 0x31
#define TERARANGER_EVO_I2C_REGISTRY_TRIGGER_READING 0x00 // Write this command to the TeraRanger Evo and after a wait of approximately 500us read the 3 byte distance response
#define TERARANGER_EVO_I2C_REGISTRY_WHO_AM_I 0x01 // Write this value to TeraRanger Evo via I2C and the device responds with 0xA
#define TERARANGER_EVO_I2C_REGISTRY_CHANGE_BASE_ADDR 0xA2 // This command assigns a base address that will be memorised by the TerRanger Evo ie. power cycling the Evo will not restore the default I2C address.
#define TERARANGER_EVO_I2C_ANSWER_WHO_AM_I 0xA1
#define TERARANGER_EVO_VALUE_TOO_CLOSE 0x0000
#define TERARANGER_EVO_VALUE_OUT_OF_RANGE 0xffff
static int16_t minimumReadingIntervalMs = 50;
uint8_t triggerValue[0];
volatile int32_t teraRangerMeasurementCm;
static uint32_t timeOfLastMeasurementMs;
static uint8_t dataBuff[3];
static void teraRangerInit(rangefinderDev_t *rangefinder){
busWriteBuf(rangefinder->busDev, TERARANGER_EVO_I2C_REGISTRY_TRIGGER_READING, triggerValue, 1);
}
void teraRangerUpdate(rangefinderDev_t *rangefinder){
if (busReadBuf(rangefinder->busDev, TERARANGER_EVO_I2C_REGISTRY_TRIGGER_READING, dataBuff, 3)) {
teraRangerMeasurementCm = MILLIMETERS_TO_CENTIMETERS(((int32_t)dataBuff[0] << 8 | (int32_t)dataBuff[1]));
if(dataBuff[2] == crc8_update(0, &dataBuff, 2)){
if (teraRangerMeasurementCm == TERARANGER_EVO_VALUE_TOO_CLOSE || teraRangerMeasurementCm == TERARANGER_EVO_VALUE_OUT_OF_RANGE) {
teraRangerMeasurementCm = RANGEFINDER_OUT_OF_RANGE;
}
}
} else {
teraRangerMeasurementCm = RANGEFINDER_HARDWARE_FAILURE;
}
const timeMs_t timeNowMs = millis();
if (timeNowMs > timeOfLastMeasurementMs + minimumReadingIntervalMs) {
// measurement repeat interval should be greater than minimumReadingIntervalMs
// to avoid interference between connective measurements.
timeOfLastMeasurementMs = timeNowMs;
busWriteBuf(rangefinder->busDev, TERARANGER_EVO_I2C_ADDRESS, triggerValue, 1);
}
}
int32_t teraRangerGetDistance(rangefinderDev_t *rangefinder){
UNUSED(rangefinder);
return teraRangerMeasurementCm;
}
static bool deviceDetect(busDevice_t * busDev){
for (int retry = 0; retry < 5; retry++) {
uint8_t whoIamResult;
delay(150);
bool ack = busRead(busDev, TERARANGER_EVO_I2C_REGISTRY_WHO_AM_I, &whoIamResult);
if (ack && whoIamResult == TERARANGER_EVO_I2C_ANSWER_WHO_AM_I) {
return true;
}
};
return false;
}
bool teraRangerDetect(rangefinderDev_t *rangefinder){
rangefinder->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_TERARANGER_EVO_I2C, 0, OWNER_RANGEFINDER);
if (rangefinder->busDev == NULL) {
return false;
}
if (!deviceDetect(rangefinder->busDev)) {
busDeviceDeInit(rangefinder->busDev);
return false;
}
rangefinder->delayMs = RANGEFINDER_TERA_EVO_TASK_PERIOD_MS;
rangefinder->maxRangeCm = RANGEFINDER_TERA_EVO_MAX_RANGE_CM;
rangefinder->detectionConeDeciDegrees = TERARANGER_EVO_DETECTION_CONE_DECIDEGREES;
rangefinder->detectionConeExtendedDeciDegrees = TERARANGER_EVO_DETECTION_CONE_EXTENDED_DECIDEGREES;
rangefinder->init = &teraRangerInit;
rangefinder->update = &teraRangerUpdate;
rangefinder->read = &teraRangerGetDistance;
return true;
}
#endif

View file

@ -0,0 +1,29 @@
/*
* This file is part of INAV.
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
* You can obtain one at http://mozilla.org/MPL/2.0/.
*
* Alternatively, the contents of this file may be used under the terms
* of the GNU General Public License Version 3, as described below:
*
* This file is free software: you may copy, redistribute and/or modify
* it under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or (at your
* option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
* Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*/
#pragma once
#define RANGEFINDER_TERA_EVO_TASK_PERIOD_MS 70
#define RANGEFINDER_TERA_EVO_MAX_RANGE_CM 600 // max distance depends on model https://www.terabee.com/sensors-modules/lidar-tof-range-finders/#individual-distance-measurement-sensors
bool teraRangerDetect(rangefinderDev_t *dev);

View file

@ -499,18 +499,6 @@ bool emergencyArmingUpdate(bool armingSwitchIsOn, bool forceArm)
return counter >= EMERGENCY_ARMING_MIN_ARM_COUNT;
}
uint16_t emergencyInFlightRearmTimeMS(void)
{
uint16_t rearmMS = 0;
if (STATE(IN_FLIGHT_EMERG_REARM)) {
timeMs_t currentTimeMs = millis();
rearmMS = (uint16_t)((US2MS(lastDisarmTimeUs) + EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS) - currentTimeMs);
}
return rearmMS;
}
bool emergInflightRearmEnabled(void)
{
/* Emergency rearm allowed within 5s timeout period after disarm if craft still flying */
@ -884,6 +872,7 @@ static void applyThrottleTiltCompensation(void)
void taskMainPidLoop(timeUs_t currentTimeUs)
{
cycleTime = getTaskDeltaTime(TASK_SELF);
dT = (float)cycleTime * 0.000001f;
@ -902,8 +891,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
}
}
if (armTime > 1 * USECS_PER_SEC) {
// reset in flight emerg rearm flag 1 sec after arming once it's served its purpose
if (armTime > 1 * USECS_PER_SEC) { // reset in flight emerg rearm flag 1 sec after arming once it's served its purpose
DISABLE_STATE(IN_FLIGHT_EMERG_REARM);
}
@ -1024,6 +1012,10 @@ float getFlightTime(void)
return US2S(flightTime);
}
void resetFlightTime(void) {
flightTime = 0;
}
float getArmTime(void)
{
return US2S(armTime);

View file

@ -41,12 +41,11 @@ timeUs_t getLastDisarmTimeUs(void);
void tryArm(void);
disarmReason_t getDisarmReason(void);
uint16_t emergencyInFlightRearmTimeMS(void);
bool emergencyArmingUpdate(bool armingSwitchIsOn, bool forceArm);
bool emergInflightRearmEnabled(void);
bool areSensorsCalibrating(void);
float getFlightTime(void);
void resetFlightTime(void);
float getArmTime(void);
void fcReboot(bool bootLoader);

View file

@ -1180,26 +1180,6 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
break;
#endif
case MSP_OSD_CONFIG:
#ifdef USE_OSD
sbufWriteU8(dst, OSD_DRIVER_MAX7456); // OSD supported
// send video system (AUTO/PAL/NTSC)
sbufWriteU8(dst, osdConfig()->video_system);
sbufWriteU8(dst, osdConfig()->units);
sbufWriteU8(dst, osdConfig()->rssi_alarm);
sbufWriteU16(dst, currentBatteryProfile->capacity.warning);
sbufWriteU16(dst, osdConfig()->time_alarm);
sbufWriteU16(dst, osdConfig()->alt_alarm);
sbufWriteU16(dst, osdConfig()->dist_alarm);
sbufWriteU16(dst, osdConfig()->neg_alt_alarm);
for (int i = 0; i < OSD_ITEM_COUNT; i++) {
sbufWriteU16(dst, osdLayoutsConfig()->item_pos[0][i]);
}
#else
sbufWriteU8(dst, OSD_DRIVER_NONE); // OSD not supported
#endif
break;
case MSP_3D:
sbufWriteU16(dst, reversibleMotorsConfig()->deadband_low);
sbufWriteU16(dst, reversibleMotorsConfig()->deadband_high);
@ -1282,7 +1262,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU8(dst, pidProfile()->heading_hold_rate_limit);
sbufWriteU8(dst, HEADING_HOLD_ERROR_LPF_FREQ);
sbufWriteU16(dst, 0);
sbufWriteU8(dst, gyroConfig()->gyro_lpf);
sbufWriteU8(dst, GYRO_LPF_256HZ);
sbufWriteU8(dst, accelerometerConfig()->acc_lpf_hz);
sbufWriteU8(dst, 0); //reserved
sbufWriteU8(dst, 0); //reserved
@ -1394,18 +1374,6 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
break;
case MSP_POSITION_ESTIMATION_CONFIG:
sbufWriteU16(dst, positionEstimationConfig()->w_z_baro_p * 100); // inav_w_z_baro_p float as value * 100
sbufWriteU16(dst, positionEstimationConfig()->w_z_gps_p * 100); // 2 inav_w_z_gps_p float as value * 100
sbufWriteU16(dst, positionEstimationConfig()->w_z_gps_v * 100); // 2 inav_w_z_gps_v float as value * 100
sbufWriteU16(dst, positionEstimationConfig()->w_xy_gps_p * 100); // 2 inav_w_xy_gps_p float as value * 100
sbufWriteU16(dst, positionEstimationConfig()->w_xy_gps_v * 100); // 2 inav_w_xy_gps_v float as value * 100
sbufWriteU8(dst, gpsConfigMutable()->gpsMinSats); // 1
sbufWriteU8(dst, 1); // 1 inav_use_gps_velned ON/OFF
break;
case MSP_REBOOT:
if (!ARMING_FLAG(ARMED)) {
if (mspPostProcessFn) {
@ -2304,7 +2272,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
pidProfileMutable()->heading_hold_rate_limit = sbufReadU8(src);
sbufReadU8(src); //HEADING_HOLD_ERROR_LPF_FREQ
sbufReadU16(src); //Legacy yaw_jump_prevention_limit
gyroConfigMutable()->gyro_lpf = sbufReadU8(src);
sbufReadU8(src); // was gyro lpf
accelerometerConfigMutable()->acc_lpf_hz = sbufReadU8(src);
sbufReadU8(src); //reserved
sbufReadU8(src); //reserved
@ -2437,19 +2405,6 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
return MSP_RESULT_ERROR;
break;
case MSP_SET_POSITION_ESTIMATION_CONFIG:
if (dataSize == 12) {
positionEstimationConfigMutable()->w_z_baro_p = constrainf(sbufReadU16(src) / 100.0f, 0.0f, 10.0f);
positionEstimationConfigMutable()->w_z_gps_p = constrainf(sbufReadU16(src) / 100.0f, 0.0f, 10.0f);
positionEstimationConfigMutable()->w_z_gps_v = constrainf(sbufReadU16(src) / 100.0f, 0.0f, 10.0f);
positionEstimationConfigMutable()->w_xy_gps_p = constrainf(sbufReadU16(src) / 100.0f, 0.0f, 10.0f);
positionEstimationConfigMutable()->w_xy_gps_v = constrainf(sbufReadU16(src) / 100.0f, 0.0f, 10.0f);
gpsConfigMutable()->gpsMinSats = constrain(sbufReadU8(src), 5, 10);
sbufReadU8(src); // was positionEstimationConfigMutable()->use_gps_velned
} else
return MSP_RESULT_ERROR;
break;
case MSP_RESET_CONF:
if (!ARMING_FLAG(ARMED)) {
suspendRxSignal();
@ -2508,36 +2463,6 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
#endif
#ifdef USE_OSD
case MSP_SET_OSD_CONFIG:
sbufReadU8Safe(&tmp_u8, src);
// set all the other settings
if ((int8_t)tmp_u8 == -1) {
if (dataSize >= 10) {
osdConfigMutable()->video_system = sbufReadU8(src);
osdConfigMutable()->units = sbufReadU8(src);
osdConfigMutable()->rssi_alarm = sbufReadU8(src);
currentBatteryProfileMutable->capacity.warning = sbufReadU16(src);
osdConfigMutable()->time_alarm = sbufReadU16(src);
osdConfigMutable()->alt_alarm = sbufReadU16(src);
// Won't be read if they weren't provided
sbufReadU16Safe(&osdConfigMutable()->dist_alarm, src);
sbufReadU16Safe(&osdConfigMutable()->neg_alt_alarm, src);
} else
return MSP_RESULT_ERROR;
} else {
// set a position setting
if ((dataSize >= 3) && (tmp_u8 < OSD_ITEM_COUNT)) // tmp_u8 == addr
osdLayoutsConfigMutable()->item_pos[0][tmp_u8] = sbufReadU16(src);
else
return MSP_RESULT_ERROR;
}
// Either a element position change or a units change needs
// a full redraw, since an element can change size significantly
// and the old position or the now unused space due to the
// size change need to be erased.
osdStartFullRedraw();
break;
case MSP_OSD_CHAR_WRITE:
if (dataSize >= 55) {
osdCharacter_t chr;

View file

@ -219,9 +219,6 @@ void initActiveBoxIds(void)
const bool navFlowDeadReckoning = sensors(SENSOR_OPFLOW) && sensors(SENSOR_ACC) && positionEstimationConfig()->allow_dead_reckoning;
bool navReadyPosControl = sensors(SENSOR_ACC) && feature(FEATURE_GPS);
if (STATE(MULTIROTOR)) {
navReadyPosControl = navReadyPosControl && getHwCompassStatus() != HW_SENSOR_NONE;
}
if (STATE(ALTITUDE_CONTROL) && navReadyAltControl && (navReadyPosControl || navFlowDeadReckoning)) {
ADD_ACTIVE_BOX(BOXNAVPOSHOLD);

View file

@ -1,13 +1,11 @@
tables:
- name: alignment
values: ["DEFAULT", "CW0", "CW90", "CW180", "CW270", "CW0FLIP", "CW90FLIP", "CW180FLIP", "CW270FLIP"]
- name: gyro_lpf
values: ["256HZ", "188HZ", "98HZ", "42HZ", "20HZ", "10HZ"]
- name: acc_hardware
values: ["NONE", "AUTO", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "BMI270","LSM6DXX", "FAKE"]
enum: accelerationSensor_e
- name: rangefinder_hardware
values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C", "FAKE"]
values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C", "FAKE", "TERARANGER_EVO"]
enum: rangefinderType_e
- name: mag_hardware
values: ["NONE", "AUTO", "HMC5883", "AK8975", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", "VCM5883", "MLX90393", "FAKE"]
@ -68,9 +66,6 @@ tables:
- name: osd_stats_energy_unit
values: ["MAH", "WH"]
enum: osd_stats_energy_unit_e
- name: osd_stats_min_voltage_unit
values: ["BATTERY", "CELL"]
enum: osd_stats_min_voltage_unit_e
- name: osd_video_system
values: ["AUTO", "PAL", "NTSC", "HDZERO", "DJIWTF", "AVATAR", "BF43COMPAT", "BFHDCOMPAT"]
enum: videoSystem_e
@ -182,7 +177,7 @@ tables:
values: ["OFF", "ON", "FS"]
enum: rthTrackbackMode_e
- name: dynamic_gyro_notch_mode
values: ["2D", "3D_R", "3D_P", "3D_Y", "3D_RP", "3D_RY", "3D_PY", "3D"]
values: ["2D", "3D"]
enum: dynamicGyroNotchMode_e
- name: nav_fw_wp_turn_smoothing
values: ["OFF", "ON", "ON-CUT"]
@ -220,32 +215,17 @@ groups:
description: "This is the main loop time (in us). Changing this affects PID effect with some PID controllers (see PID section for details). A very conservative value of 3500us/285Hz should work for everyone. Setting it to zero does not limit loop time, so it will go as fast as possible."
default_value: 1000
max: 9000
- name: gyro_hardware_lpf
description: "Hardware lowpass filter for gyro. This value should never be changed without a very strong reason! If you have to set gyro lpf below 256HZ, it means the frame is vibrating too much, and that should be fixed first."
default_value: "256HZ"
field: gyro_lpf
table: gyro_lpf
- name: gyro_anti_aliasing_lpf_hz
description: "Gyro processing anti-aliasing filter cutoff frequency. In normal operation this filter setting should never be changed. In Hz"
default_value: 250
field: gyro_anti_aliasing_lpf_hz
max: 1000
- name: gyro_anti_aliasing_lpf_type
description: "Specifies the type of the software LPF of the gyro signals."
default_value: "PT1"
field: gyro_anti_aliasing_lpf_type
table: filter_type
- name: gyro_main_lpf_hz
description: "Software based gyro main lowpass filter. Value is cutoff frequency (Hz)"
default_value: 60
field: gyro_main_lpf_hz
min: 0
max: 500
- name: gyro_main_lpf_type
description: "Defines the type of the main gyro LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation."
default_value: BIQUAD
field: gyro_main_lpf_type
table: filter_type
- name: gyro_use_dyn_lpf
description: "Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro LPF based on the throttle position."
default_value: OFF
@ -1204,8 +1184,6 @@ groups:
field: mixer_config.tailsitterOrientationOffset
type: bool
- name: PG_REVERSIBLE_MOTORS_CONFIG
type: reversibleMotorsConfig_t
members:
@ -2877,10 +2855,10 @@ groups:
min: 100
max: 10000
- name: nav_fw_launch_accel
description: "Forward acceleration threshold for bungee launch of throw launch [cm/s/s], 1G = 981 cm/s/s"
description: "Forward acceleration threshold for bungee launch or throw launch [cm/s/s], 1G = 981 cm/s/s"
default_value: 1863
field: fw.launch_accel_thresh
min: 1000
min: 1500
max: 20000
- name: nav_fw_launch_max_angle
description: "Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg]"
@ -2900,6 +2878,13 @@ groups:
field: fw.launch_idle_motor_timer
min: 0
max: 60000
- name: nav_fw_launch_wiggle_to_wake_idle
description: "Trigger the idle throttle by wiggling the plane. 0 = disabled. 1 and 2 signify 1 or 2 yaw wiggles to activate. 1 wiggle has a higher detection point, for airplanes without a tail. 2 wiggles has a lower detection point, but requires the repeated action. This is intended for larger models and airplanes with tails."
field: fw.launch_wiggle_wake_idle
type: uint8_t
default_value: 0
min: 0
max: 2
- name: nav_fw_launch_motor_delay
description: "Delay between detected launch and launch sequence start and throttling up (ms)"
default_value: 500
@ -3168,18 +3153,17 @@ groups:
field: stats_energy_unit
table: osd_stats_energy_unit
type: uint8_t
- name: osd_stats_min_voltage_unit
description: "Display minimum voltage of the `BATTERY` or the average per `CELL` in the OSD stats."
default_value: "BATTERY"
field: stats_min_voltage_unit
table: osd_stats_min_voltage_unit
type: uint8_t
- name: osd_stats_page_auto_swap_time
description: "Auto swap display time interval between disarm stats pages (seconds). Reverts to manual control when Roll stick used to change pages. Disabled when set to 0."
default_value: 3
field: stats_page_auto_swap_time
min: 0
max: 10
- name: osd_stats_show_metric_efficiency
description: "Enabling this option will show metric efficiency statistics on the post flight stats screen. In addition to the efficiency statistics in your chosen units."
default_value: OFF
type: bool
field: stats_show_metric_efficiency
- name: osd_rssi_alarm
description: "Value below which to make the OSD RSSI indicator blink"
default_value: 20
@ -3500,6 +3484,12 @@ groups:
condition: USE_WIND_ESTIMATOR
field: estimations_wind_compensation
type: bool
- name: osd_estimations_wind_mps
description: "Wind speed estimation in m/s"
default_value: OFF
condition: USE_WIND_ESTIMATOR
field: estimations_wind_mps
type: bool
- name: osd_failsafe_switch_layout
description: "If enabled the OSD automatically switches to the first layout during failsafe"
@ -4001,12 +3991,6 @@ groups:
headers: ["io/osd_dji_hd.h"]
condition: USE_DJI_HD_OSD
members:
- name: dji_workarounds
description: "Enables workarounds for different versions of MSP protocol used"
field: proto_workarounds
min: 0
max: UINT8_MAX
default_value: 1
- name: dji_use_name_for_messages
description: "Re-purpose the craft name field for messages."
default_value: ON

View file

@ -77,11 +77,9 @@ void ezTuneUpdate(void) {
//Set main gyro filter
gyroConfigMutable()->gyro_main_lpf_hz = ezTune()->filterHz;
gyroConfigMutable()->gyro_main_lpf_type = FILTER_PT1;
//Set anti-aliasing filter
gyroConfigMutable()->gyro_anti_aliasing_lpf_hz = SETTING_GYRO_ANTI_ALIASING_LPF_HZ_DEFAULT;
gyroConfigMutable()->gyro_anti_aliasing_lpf_type = FILTER_PT1;
//Enable Smith predictor
pidProfileMutable()->smithPredictorDelay = computePt1FilterDelayMs(ezTune()->filterHz);

View file

@ -44,46 +44,25 @@ void secondaryDynamicGyroNotchFiltersInit(secondaryDynamicGyroNotchState_t *stat
state->enabled = gyroConfig()->dynamicGyroNotchMode != DYNAMIC_NOTCH_MODE_2D;
state->looptime = getLooptime();
if (
gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_R ||
gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_RP ||
gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_RY ||
gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_3D
) {
if (gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_3D) {
/*
* Enable ROLL filter
*/
biquadFilterInit(&state->filters[FD_ROLL], SECONDARY_DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH);
state->filtersApplyFn[FD_ROLL] = (filterApplyFnPtr)biquadFilterApplyDF1;
}
if (
gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_P ||
gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_RP ||
gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_PY ||
gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_3D
) {
/*
* Enable PITCH filter
*/
biquadFilterInit(&state->filters[FD_PITCH], SECONDARY_DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH);
state->filtersApplyFn[FD_PITCH] = (filterApplyFnPtr)biquadFilterApplyDF1;
}
if (
gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_Y ||
gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_PY ||
gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_RY ||
gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_3D
) {
/*
* Enable YAW filter
*/
biquadFilterInit(&state->filters[FD_YAW], SECONDARY_DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH);
state->filtersApplyFn[FD_YAW] = (filterApplyFnPtr)biquadFilterApplyDF1;
}
}
void secondaryDynamicGyroNotchFiltersUpdate(secondaryDynamicGyroNotchState_t *state, int axis, float frequency[]) {

File diff suppressed because it is too large Load diff

View file

@ -285,6 +285,7 @@ typedef enum {
OSD_CUSTOM_ELEMENT_3,
OSD_ADSB_WARNING,
OSD_ADSB_INFO,
OSD_BLACKBOX,
OSD_ITEM_COUNT // MUST BE LAST
} osd_items_e;
@ -303,11 +304,6 @@ typedef enum {
OSD_STATS_ENERGY_UNIT_WH,
} osd_stats_energy_unit_e;
typedef enum {
OSD_STATS_MIN_VOLTAGE_UNIT_BATTERY,
OSD_STATS_MIN_VOLTAGE_UNIT_CELL,
} osd_stats_min_voltage_unit_e;
typedef enum {
OSD_CROSSHAIRS_STYLE_DEFAULT,
OSD_CROSSHAIRS_STYLE_AIRCRAFT,
@ -417,11 +413,12 @@ typedef struct osdConfig_s {
uint8_t units; // from osd_unit_e
uint8_t stats_energy_unit; // from osd_stats_energy_unit_e
uint8_t stats_min_voltage_unit; // from osd_stats_min_voltage_unit_e
uint8_t stats_page_auto_swap_time; // stats page auto swap interval time (seconds)
bool stats_show_metric_efficiency; // If true, show metric efficiency as well as for the selected units
#ifdef USE_WIND_ESTIMATOR
bool estimations_wind_compensation; // use wind compensation for estimated remaining flight/distance
bool estimations_wind_mps; // wind speed estimation in m/s
#endif
uint8_t coordinate_digits;
bool osd_failsafe_switch_layout;

View file

@ -122,11 +122,10 @@
* but reuse the packet decoder to minimize code duplication
*/
PG_REGISTER_WITH_RESET_TEMPLATE(djiOsdConfig_t, djiOsdConfig, PG_DJI_OSD_CONFIG, 2);
PG_REGISTER_WITH_RESET_TEMPLATE(djiOsdConfig_t, djiOsdConfig, PG_DJI_OSD_CONFIG, 3);
PG_RESET_TEMPLATE(djiOsdConfig_t, djiOsdConfig,
.use_name_for_messages = SETTING_DJI_USE_NAME_FOR_MESSAGES_DEFAULT,
.esc_temperature_source = SETTING_DJI_ESC_TEMP_SOURCE_DEFAULT,
.proto_workarounds = SETTING_DJI_WORKAROUNDS_DEFAULT,
.messageSpeedSource = SETTING_DJI_MESSAGE_SPEED_SOURCE_DEFAULT,
.rssi_source = SETTING_DJI_RSSI_SOURCE_DEFAULT,
.useAdjustments = SETTING_DJI_USE_ADJUSTMENTS_DEFAULT,
@ -1347,12 +1346,11 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms
break;
case DJI_MSP_ESC_SENSOR_DATA:
if (djiOsdConfig()->proto_workarounds & DJI_OSD_USE_NON_STANDARD_MSP_ESC_SENSOR_DATA) {
// Version 1.00.06 of DJI firmware is not using the standard MSP_ESC_SENSOR_DATA
{
uint16_t protoRpm = 0;
int16_t protoTemp = 0;
#if defined(USE_ESC_SENSOR)
#if defined(USE_ESC_SENSOR)
if (STATE(ESC_SENSOR_ENABLED) && getMotorCount() > 0) {
uint32_t motorRpmAcc = 0;
int32_t motorTempAcc = 0;
@ -1366,7 +1364,7 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms
protoRpm = motorRpmAcc / getMotorCount();
protoTemp = motorTempAcc / getMotorCount();
}
#endif
#endif
switch (djiOsdConfig()->esc_temperature_source) {
// This is ESC temperature (as intended)
@ -1391,47 +1389,6 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms
sbufWriteU8(dst, protoTemp);
sbufWriteU16(dst, protoRpm);
}
else {
// Use standard MSP_ESC_SENSOR_DATA message
sbufWriteU8(dst, getMotorCount());
for (int i = 0; i < getMotorCount(); i++) {
uint16_t motorRpm = 0;
int16_t motorTemp = 0;
// If ESC_SENSOR is enabled, pull the telemetry data and get motor RPM
#if defined(USE_ESC_SENSOR)
if (STATE(ESC_SENSOR_ENABLED)) {
const escSensorData_t * escSensor = getEscTelemetry(i);
motorRpm = escSensor->rpm;
motorTemp = escSensor->temperature;
}
#endif
// Now populate temperature field (which we may override for different purposes)
switch (djiOsdConfig()->esc_temperature_source) {
// This is ESC temperature (as intended)
case DJI_OSD_TEMP_ESC:
// No-op, temperature is already set to ESC
break;
// Re-purpose the field for core temperature
case DJI_OSD_TEMP_CORE:
getIMUTemperature(&motorTemp);
motorTemp = motorTemp / 10;
break;
// Re-purpose the field for baro temperature
case DJI_OSD_TEMP_BARO:
getBaroTemperature(&motorTemp);
motorTemp = motorTemp / 10;
break;
}
// Add data for this motor to the packet
sbufWriteU8(dst, motorTemp);
sbufWriteU16(dst, motorRpm);
}
}
break;
case DJI_MSP_OSD_CONFIG:
@ -1454,7 +1411,7 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms
sbufWriteU16(dst, 0); // BF: gyroConfig()->gyro_soft_notch_hz_2
sbufWriteU16(dst, 1); // BF: gyroConfig()->gyro_soft_notch_cutoff_2
sbufWriteU8(dst, 0); // BF: currentPidProfile->dterm_filter_type
sbufWriteU8(dst, gyroConfig()->gyro_lpf); // BF: gyroConfig()->gyro_hardware_lpf);
sbufWriteU8(dst, GYRO_LPF_256HZ); // BF: gyroConfig()->gyro_hardware_lpf);
sbufWriteU8(dst, 0); // BF: DEPRECATED: gyro_32khz_hardware_lpf
sbufWriteU16(dst, gyroConfig()->gyro_main_lpf_hz); // BF: gyroConfig()->gyro_lowpass_hz);
sbufWriteU16(dst, 0); // BF: gyroConfig()->gyro_lowpass2_hz);

View file

@ -84,7 +84,6 @@ enum djiOsdProtoWorkarounds_e {
typedef struct djiOsdConfig_s {
uint8_t use_name_for_messages;
uint8_t esc_temperature_source;
uint8_t proto_workarounds;
uint8_t messageSpeedSource;
uint8_t rssi_source;
uint8_t useAdjustments;

View file

@ -110,9 +110,6 @@
#define MSP_CALIBRATION_DATA 14
#define MSP_SET_CALIBRATION_DATA 15
#define MSP_POSITION_ESTIMATION_CONFIG 16
#define MSP_SET_POSITION_ESTIMATION_CONFIG 17
#define MSP_WP_MISSION_LOAD 18 // Load mission from NVRAM
#define MSP_WP_MISSION_SAVE 19 // Save mission to NVRAM
#define MSP_WP_GETINFO 20
@ -189,9 +186,6 @@
#define MSP_TRANSPONDER_CONFIG 82 //out message Get transponder settings
#define MSP_SET_TRANSPONDER_CONFIG 83 //in message Set transponder settings
#define MSP_OSD_CONFIG 84 //out message Get osd settings - betaflight
#define MSP_SET_OSD_CONFIG 85 //in message Set osd settings - betaflight
#define MSP_OSD_CHAR_READ 86 //out message Get osd settings - betaflight
#define MSP_OSD_CHAR_WRITE 87 //in message Set osd settings - betaflight

View file

@ -175,7 +175,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.land_detect_sensitivity = SETTING_NAV_LAND_DETECT_SENSITIVITY_DEFAULT, // Changes sensitivity of landing detection
.auto_disarm_delay = SETTING_NAV_AUTO_DISARM_DELAY_DEFAULT, // 2000 ms - time delay to disarm when auto disarm after landing enabled
.rth_linear_descent_start_distance = SETTING_NAV_RTH_LINEAR_DESCENT_START_DISTANCE_DEFAULT,
.cruise_yaw_rate = SETTING_NAV_CRUISE_YAW_RATE_DEFAULT, // 20dps
.cruise_yaw_rate = SETTING_NAV_CRUISE_YAW_RATE_DEFAULT, // 20dps
.rth_fs_landing_delay = SETTING_NAV_RTH_FS_LANDING_DELAY_DEFAULT, // Delay before landing in FS. 0 = immedate landing
},
@ -204,44 +204,44 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
// Fixed wing
.fw = {
.max_bank_angle = SETTING_NAV_FW_BANK_ANGLE_DEFAULT, // degrees
.max_manual_climb_rate = SETTING_NAV_FW_MANUAL_CLIMB_RATE_DEFAULT, // 3 m/s
.max_climb_angle = SETTING_NAV_FW_CLIMB_ANGLE_DEFAULT, // degrees
.max_dive_angle = SETTING_NAV_FW_DIVE_ANGLE_DEFAULT, // degrees
.cruise_speed = SETTING_NAV_FW_CRUISE_SPEED_DEFAULT, // cm/s
.max_bank_angle = SETTING_NAV_FW_BANK_ANGLE_DEFAULT, // degrees
.max_manual_climb_rate = SETTING_NAV_FW_MANUAL_CLIMB_RATE_DEFAULT, // 3 m/s
.max_climb_angle = SETTING_NAV_FW_CLIMB_ANGLE_DEFAULT, // degrees
.max_dive_angle = SETTING_NAV_FW_DIVE_ANGLE_DEFAULT, // degrees
.cruise_speed = SETTING_NAV_FW_CRUISE_SPEED_DEFAULT, // cm/s
.control_smoothness = SETTING_NAV_FW_CONTROL_SMOOTHNESS_DEFAULT,
.pitch_to_throttle_smooth = SETTING_NAV_FW_PITCH2THR_SMOOTHING_DEFAULT,
.pitch_to_throttle_thresh = SETTING_NAV_FW_PITCH2THR_THRESHOLD_DEFAULT,
.minThrottleDownPitchAngle = SETTING_FW_MIN_THROTTLE_DOWN_PITCH_DEFAULT,
.loiter_radius = SETTING_NAV_FW_LOITER_RADIUS_DEFAULT, // 75m
.loiter_radius = SETTING_NAV_FW_LOITER_RADIUS_DEFAULT, // 75m
.loiter_direction = SETTING_FW_LOITER_DIRECTION_DEFAULT,
//Fixed wing landing
.land_dive_angle = SETTING_NAV_FW_LAND_DIVE_ANGLE_DEFAULT, // 2 degrees dive by default
.land_dive_angle = SETTING_NAV_FW_LAND_DIVE_ANGLE_DEFAULT, // 2 degrees dive by default
// Fixed wing launch
.launch_velocity_thresh = SETTING_NAV_FW_LAUNCH_VELOCITY_DEFAULT, // 3 m/s
.launch_accel_thresh = SETTING_NAV_FW_LAUNCH_ACCEL_DEFAULT, // cm/s/s (1.9*G)
.launch_time_thresh = SETTING_NAV_FW_LAUNCH_DETECT_TIME_DEFAULT, // 40ms
.launch_motor_timer = SETTING_NAV_FW_LAUNCH_MOTOR_DELAY_DEFAULT, // ms
.launch_idle_motor_timer = SETTING_NAV_FW_LAUNCH_IDLE_MOTOR_DELAY_DEFAULT, // ms
.launch_motor_spinup_time = SETTING_NAV_FW_LAUNCH_SPINUP_TIME_DEFAULT, // ms, time to gredually increase throttle from idle to launch
.launch_end_time = SETTING_NAV_FW_LAUNCH_END_TIME_DEFAULT, // ms, time to gradually decrease/increase throttle and decrease pitch angle from launch to the current flight mode
.launch_min_time = SETTING_NAV_FW_LAUNCH_MIN_TIME_DEFAULT, // ms, min time in launch mode
.launch_timeout = SETTING_NAV_FW_LAUNCH_TIMEOUT_DEFAULT, // ms, timeout for launch procedure
.launch_max_altitude = SETTING_NAV_FW_LAUNCH_MAX_ALTITUDE_DEFAULT, // cm, altitude where to consider launch ended
.launch_climb_angle = SETTING_NAV_FW_LAUNCH_CLIMB_ANGLE_DEFAULT, // 18 degrees
.launch_max_angle = SETTING_NAV_FW_LAUNCH_MAX_ANGLE_DEFAULT, // 45 deg
.launch_manual_throttle = SETTING_NAV_FW_LAUNCH_MANUAL_THROTTLE_DEFAULT,// OFF
.launch_land_abort_deadband = SETTING_NAV_FW_LAUNCH_LAND_ABORT_DEADBAND_DEFAULT, // 100 us
.launch_velocity_thresh = SETTING_NAV_FW_LAUNCH_VELOCITY_DEFAULT, // 3 m/s
.launch_accel_thresh = SETTING_NAV_FW_LAUNCH_ACCEL_DEFAULT, // cm/s/s (1.9*G)
.launch_time_thresh = SETTING_NAV_FW_LAUNCH_DETECT_TIME_DEFAULT, // 40ms
.launch_motor_timer = SETTING_NAV_FW_LAUNCH_MOTOR_DELAY_DEFAULT, // ms
.launch_idle_motor_timer = SETTING_NAV_FW_LAUNCH_IDLE_MOTOR_DELAY_DEFAULT, // ms
.launch_wiggle_wake_idle = SETTING_NAV_FW_LAUNCH_WIGGLE_TO_WAKE_IDLE_DEFAULT, // uint8_t
.launch_motor_spinup_time = SETTING_NAV_FW_LAUNCH_SPINUP_TIME_DEFAULT, // ms, time to greaually increase throttle from idle to launch
.launch_end_time = SETTING_NAV_FW_LAUNCH_END_TIME_DEFAULT, // ms, time to gradually decrease/increase throttle and decrease pitch angle from launch to the current flight mode
.launch_min_time = SETTING_NAV_FW_LAUNCH_MIN_TIME_DEFAULT, // ms, min time in launch mode
.launch_timeout = SETTING_NAV_FW_LAUNCH_TIMEOUT_DEFAULT, // ms, timeout for launch procedure
.launch_max_altitude = SETTING_NAV_FW_LAUNCH_MAX_ALTITUDE_DEFAULT, // cm, altitude where to consider launch ended
.launch_climb_angle = SETTING_NAV_FW_LAUNCH_CLIMB_ANGLE_DEFAULT, // 18 degrees
.launch_max_angle = SETTING_NAV_FW_LAUNCH_MAX_ANGLE_DEFAULT, // 45 deg
.launch_manual_throttle = SETTING_NAV_FW_LAUNCH_MANUAL_THROTTLE_DEFAULT, // OFF
.launch_land_abort_deadband = SETTING_NAV_FW_LAUNCH_LAND_ABORT_DEADBAND_DEFAULT, // 100 us
.allow_manual_thr_increase = SETTING_NAV_FW_ALLOW_MANUAL_THR_INCREASE_DEFAULT,
.useFwNavYawControl = SETTING_NAV_USE_FW_YAW_CONTROL_DEFAULT,
.yawControlDeadband = SETTING_NAV_FW_YAW_DEADBAND_DEFAULT,
.soaring_pitch_deadband = SETTING_NAV_FW_SOARING_PITCH_DEADBAND_DEFAULT,// pitch angle mode deadband when Saoring mode enabled
.wp_tracking_accuracy = SETTING_NAV_FW_WP_TRACKING_ACCURACY_DEFAULT, // 0, improves course tracking accuracy during FW WP missions
.wp_tracking_max_angle = SETTING_NAV_FW_WP_TRACKING_MAX_ANGLE_DEFAULT, // 60 degs
.wp_turn_smoothing = SETTING_NAV_FW_WP_TURN_SMOOTHING_DEFAULT, // 0, smooths turns during FW WP mode missions
.soaring_pitch_deadband = SETTING_NAV_FW_SOARING_PITCH_DEADBAND_DEFAULT, // pitch angle mode deadband when Saoring mode enabled
.wp_tracking_accuracy = SETTING_NAV_FW_WP_TRACKING_ACCURACY_DEFAULT, // 0, improves course tracking accuracy during FW WP missions
.wp_tracking_max_angle = SETTING_NAV_FW_WP_TRACKING_MAX_ANGLE_DEFAULT, // 60 degs
.wp_turn_smoothing = SETTING_NAV_FW_WP_TURN_SMOOTHING_DEFAULT, // 0, smooths turns during FW WP mode missions
}
);

View file

@ -193,9 +193,9 @@ typedef enum {
} navRTHClimbFirst_e;
typedef enum { // keep aligned with fixedWingLaunchState_t
FW_LAUNCH_DETECTED = 4,
FW_LAUNCH_ABORTED = 9,
FW_LAUNCH_FLYING = 10,
FW_LAUNCH_DETECTED = 5,
FW_LAUNCH_ABORTED = 10,
FW_LAUNCH_FLYING = 11,
} navFwLaunchStatus_e;
typedef enum {
@ -366,6 +366,7 @@ typedef struct navConfig_s {
uint16_t launch_time_thresh; // Time threshold for launch detection (ms)
uint16_t launch_motor_timer; // Time to wait before setting launch_throttle (ms)
uint16_t launch_idle_motor_timer; // Time to wait before motor starts at_idle throttle (ms)
uint8_t launch_wiggle_wake_idle; // Activate the idle throttle by wiggling the plane. 0 = disabled, 1 or 2 specify the number of wiggles.
uint16_t launch_motor_spinup_time; // Time to speed-up motors from idle to launch_throttle (ESC desync prevention)
uint16_t launch_end_time; // Time to make the transition from launch angle to leveled and throttle transition from launch throttle to the stick position
uint16_t launch_min_time; // Minimum time in launch mode to prevent possible bump of the sticks from leaving launch mode early

View file

@ -49,6 +49,7 @@
#include "io/gps.h"
#include "sensors/battery.h"
#include "sensors/gyro.h"
#define SWING_LAUNCH_MIN_ROTATION_RATE DEGREES_TO_RADIANS(100) // expect minimum 100dps rotation rate
#define LAUNCH_MOTOR_IDLE_SPINUP_TIME 1500 // ms
@ -81,20 +82,22 @@ typedef enum {
typedef enum { // if changed update navFwLaunchStatus_e
FW_LAUNCH_STATE_WAIT_THROTTLE = 0,
FW_LAUNCH_STATE_IDLE_WIGGLE_WAIT,
FW_LAUNCH_STATE_IDLE_MOTOR_DELAY,
FW_LAUNCH_STATE_MOTOR_IDLE,
FW_LAUNCH_STATE_WAIT_DETECTION,
FW_LAUNCH_STATE_DETECTED, // 4
FW_LAUNCH_STATE_DETECTED, // FW_LAUNCH_DETECTED = 5
FW_LAUNCH_STATE_MOTOR_DELAY,
FW_LAUNCH_STATE_MOTOR_SPINUP,
FW_LAUNCH_STATE_IN_PROGRESS,
FW_LAUNCH_STATE_FINISH,
FW_LAUNCH_STATE_ABORTED, // 9
FW_LAUNCH_STATE_FLYING, // 10
FW_LAUNCH_STATE_ABORTED, // FW_LAUNCH_ABORTED = 10
FW_LAUNCH_STATE_FLYING, // FW_LAUNCH_FLYING = 11
FW_LAUNCH_STATE_COUNT
} fixedWingLaunchState_t;
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs_t currentTimeUs);
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE_WIGGLE_WAIT(timeUs_t currentTimeUs);
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE_MOTOR_DELAY(timeUs_t currentTimeUs);
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs);
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeUs_t currentTimeUs);
@ -126,12 +129,22 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE
[FW_LAUNCH_STATE_WAIT_THROTTLE] = {
.onEntry = fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE,
.onEvent = {
[FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_IDLE_MOTOR_DELAY,
[FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_IDLE_WIGGLE_WAIT,
[FW_LAUNCH_EVENT_GOTO_DETECTION] = FW_LAUNCH_STATE_WAIT_DETECTION
},
.messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE
},
[FW_LAUNCH_STATE_IDLE_WIGGLE_WAIT] = {
.onEntry = fwLaunchState_FW_LAUNCH_STATE_IDLE_WIGGLE_WAIT,
.onEvent = {
[FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_MOTOR_IDLE,
[FW_LAUNCH_EVENT_GOTO_DETECTION] = FW_LAUNCH_STATE_IDLE_MOTOR_DELAY,
[FW_LAUNCH_EVENT_THROTTLE_LOW] = FW_LAUNCH_STATE_WAIT_THROTTLE,
},
.messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_IDLE
},
[FW_LAUNCH_STATE_IDLE_MOTOR_DELAY] = {
.onEntry = fwLaunchState_FW_LAUNCH_STATE_IDLE_MOTOR_DELAY,
.onEvent = {
@ -243,6 +256,47 @@ static void applyThrottleIdleLogic(bool forceMixerIdle)
}
}
static bool hasIdleWakeWiggleSucceeded(timeUs_t currentTimeUs) {
static timeMs_t wiggleTime = 0;
static timeMs_t wigglesTime = 0;
static int8_t wiggleStageOne = 0;
static uint8_t wiggleCount = 0;
const bool isAircraftWithinLaunchAngle = (calculateCosTiltAngle() >= cos_approx(DEGREES_TO_RADIANS(navConfig()->fw.launch_max_angle)));
const uint8_t wiggleStrength = (navConfig()->fw.launch_wiggle_wake_idle == 1) ? 50 : 40;
int8_t wiggleDirection = 0;
int16_t yawRate = (int16_t)(gyroRateDps(YAW) * (4 / 16.4));
// Check to see if yaw rate has exceeded 50 dps. If so proceed to the next stage or continue to idle
if ((yawRate < -wiggleStrength || yawRate > wiggleStrength) && isAircraftWithinLaunchAngle) {
wiggleDirection = (yawRate > 0) ? 1 : -1;
if (wiggleStageOne == 0) {
wiggleStageOne = wiggleDirection;
wigglesTime = US2MS(currentTimeUs);
} else if (wiggleStageOne != wiggleDirection) {
wiggleStageOne = 0;
wiggleCount++;
if (wiggleCount == navConfig()->fw.launch_wiggle_wake_idle) {
return true;
}
}
wiggleTime = US2MS(currentTimeUs);
}
// If time between wiggle stages is > 100ms, or the time between two wiggles is > 1s. Reset the wiggle
if (
((wiggleStageOne != 0) && (US2MS(currentTimeUs) > (wiggleTime + 100))) ||
((wiggleCount != 0) && (US2MS(currentTimeUs) > (wigglesTime + 500)))
) {
wiggleStageOne = 0;
wiggleCount = 0;
}
return false;
}
static inline bool isLaunchMaxAltitudeReached(void)
{
return (navConfig()->fw.launch_max_altitude > 0) && (getEstimatedActualPosition(Z) >= navConfig()->fw.launch_max_altitude);
@ -300,6 +354,25 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs
return FW_LAUNCH_EVENT_NONE;
}
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE_WIGGLE_WAIT(timeUs_t currentTimeUs) {
if (throttleStickIsLow()) {
return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE
}
if (navConfig()->fw.launch_wiggle_wake_idle == 0 || navConfig()->fw.launch_idle_motor_timer > 0 ) {
return FW_LAUNCH_EVENT_GOTO_DETECTION;
}
applyThrottleIdleLogic(true);
if (hasIdleWakeWiggleSucceeded(currentTimeUs)) {
idleMotorAboutToStart = false;
return FW_LAUNCH_EVENT_SUCCESS;
}
return FW_LAUNCH_EVENT_NONE;
}
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE_MOTOR_DELAY(timeUs_t currentTimeUs)
{
if (throttleStickIsLow()) {
@ -308,10 +381,11 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE_MOTOR_DELAY(tim
applyThrottleIdleLogic(true);
if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_idle_motor_timer) {
if ((currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_idle_motor_timer) || (navConfig()->fw.launch_wiggle_wake_idle > 0 && hasIdleWakeWiggleSucceeded(currentTimeUs))) {
idleMotorAboutToStart = false;
return FW_LAUNCH_EVENT_SUCCESS;
}
// 5 second warning motor about to start at idle, changes Beeper sound
idleMotorAboutToStart = navConfig()->fw.launch_idle_motor_timer - currentStateElapsedMs(currentTimeUs) < 5000;

View file

@ -45,7 +45,6 @@ enum {
CRSF_FRAME_GPS_PAYLOAD_SIZE = 15,
CRSF_FRAME_VARIO_SENSOR_PAYLOAD_SIZE = 2,
CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE = 8,
CRSF_FRAME_BAROVARIO_SENSOR_PAYLOAD_SIZE = 4,
CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE = 10,
CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE = 22, // 11 bits per channel * 16 channels = 22 bytes.
CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE = 6,
@ -87,7 +86,6 @@ typedef enum {
CRSF_FRAMETYPE_GPS = 0x02,
CRSF_FRAMETYPE_VARIO_SENSOR = 0x07,
CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08,
CRSF_FRAMETYPE_BAROVARIO_SENSOR = 0x09,
CRSF_FRAMETYPE_LINK_STATISTICS = 0x14,
CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16,
CRSF_FRAMETYPE_ATTITUDE = 0x1E,

View file

@ -529,6 +529,15 @@ float accGetMeasuredMaxG(void)
return acc.maxG;
}
void resetGForceStats(void) {
acc.maxG = 0.0f;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
acc.extremes[axis].min = 100;
acc.extremes[axis].max = -100;
}
}
void accUpdate(void)
{
#ifdef USE_SIMULATOR

View file

@ -86,6 +86,7 @@ void accGetMeasuredAcceleration(fpVector3_t *measuredAcc);
const acc_extremes_t* accGetMeasuredExtremes(void);
float accGetMeasuredMaxG(void);
void updateAccExtremes(void);
void resetGForceStats(void);
void accGetVibrationLevels(fpVector3_t *accVibeLevels);
float accGetVibrationLevel(void);
uint32_t accGetClipCount(void);

View file

@ -95,18 +95,15 @@ EXTENDED_FASTRAM secondaryDynamicGyroNotchState_t secondaryDynamicGyroNotchState
#endif
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 6);
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 8);
PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_lpf = SETTING_GYRO_HARDWARE_LPF_DEFAULT,
.gyro_anti_aliasing_lpf_hz = SETTING_GYRO_ANTI_ALIASING_LPF_HZ_DEFAULT,
.gyro_anti_aliasing_lpf_type = SETTING_GYRO_ANTI_ALIASING_LPF_TYPE_DEFAULT,
.looptime = SETTING_LOOPTIME_DEFAULT,
#ifdef USE_DUAL_GYRO
.gyro_to_use = SETTING_GYRO_TO_USE_DEFAULT,
#endif
.gyro_main_lpf_hz = SETTING_GYRO_MAIN_LPF_HZ_DEFAULT,
.gyro_main_lpf_type = SETTING_GYRO_MAIN_LPF_TYPE_DEFAULT,
.useDynamicLpf = SETTING_GYRO_USE_DYN_LPF_DEFAULT,
.gyroDynamicLpfMinHz = SETTING_GYRO_DYN_LPF_MIN_HZ_DEFAULT,
.gyroDynamicLpfMaxHz = SETTING_GYRO_DYN_LPF_MAX_HZ_DEFAULT,
@ -233,24 +230,13 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
return gyroHardware;
}
static void initGyroFilter(filterApplyFnPtr *applyFn, filter_t state[], uint8_t type, uint16_t cutoff, uint32_t looptime)
static void initGyroFilter(filterApplyFnPtr *applyFn, filter_t state[], uint16_t cutoff, uint32_t looptime)
{
*applyFn = nullFilterApply;
if (cutoff > 0) {
switch (type)
{
case FILTER_PT1:
*applyFn = (filterApplyFnPtr)pt1FilterApply;
for (int axis = 0; axis < 3; axis++) {
pt1FilterInit(&state[axis].pt1, cutoff, US2S(looptime));
}
break;
case FILTER_BIQUAD:
*applyFn = (filterApplyFnPtr)biquadFilterApply;
for (int axis = 0; axis < 3; axis++) {
biquadFilterInitLPF(&state[axis].biquad, cutoff, looptime);
}
break;
*applyFn = (filterApplyFnPtr)pt1FilterApply;
for (int axis = 0; axis < 3; axis++) {
pt1FilterInit(&state[axis].pt1, cutoff, US2S(looptime));
}
}
}
@ -258,10 +244,10 @@ static void initGyroFilter(filterApplyFnPtr *applyFn, filter_t state[], uint8_t
static void gyroInitFilters(void)
{
//First gyro LPF running at full gyro frequency 8kHz
initGyroFilter(&gyroLpfApplyFn, gyroLpfState, gyroConfig()->gyro_anti_aliasing_lpf_type, gyroConfig()->gyro_anti_aliasing_lpf_hz, getGyroLooptime());
initGyroFilter(&gyroLpfApplyFn, gyroLpfState, gyroConfig()->gyro_anti_aliasing_lpf_hz, getGyroLooptime());
//Second gyro LPF runnig and PID frequency - this filter is dynamic when gyro_use_dyn_lpf = ON
initGyroFilter(&gyroLpf2ApplyFn, gyroLpf2State, gyroConfig()->gyro_main_lpf_type, gyroConfig()->gyro_main_lpf_hz, getLooptime());
initGyroFilter(&gyroLpf2ApplyFn, gyroLpf2State, gyroConfig()->gyro_main_lpf_hz, getLooptime());
#ifdef USE_GYRO_KALMAN
if (gyroConfig()->kalmanEnabled) {
@ -295,7 +281,7 @@ bool gyroInit(void)
sensorsSet(SENSOR_GYRO);
// Driver initialisation
gyroDev[0].lpf = gyroConfig()->gyro_lpf;
gyroDev[0].lpf = GYRO_LPF_256HZ;
gyroDev[0].requestedSampleIntervalUs = TASK_GYRO_LOOPTIME;
gyroDev[0].sampleRateIntervalUs = TASK_GYRO_LOOPTIME;
gyroDev[0].initFn(&gyroDev[0]);
@ -566,14 +552,8 @@ int16_t gyroRateDps(int axis)
}
void gyroUpdateDynamicLpf(float cutoffFreq) {
if (gyroConfig()->gyro_main_lpf_type == FILTER_PT1) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
pt1FilterUpdateCutoff(&gyroLpf2State[axis].pt1, cutoffFreq);
}
} else if (gyroConfig()->gyro_main_lpf_type == FILTER_BIQUAD) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterUpdate(&gyroLpf2State[axis].biquad, cutoffFreq, getLooptime(), BIQUAD_Q, FILTER_LPF);
}
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
pt1FilterUpdateCutoff(&gyroLpf2State[axis].pt1, cutoffFreq);
}
}

View file

@ -49,12 +49,6 @@ typedef enum {
typedef enum {
DYNAMIC_NOTCH_MODE_2D = 0,
DYNAMIC_NOTCH_MODE_R,
DYNAMIC_NOTCH_MODE_P,
DYNAMIC_NOTCH_MODE_Y,
DYNAMIC_NOTCH_MODE_RP,
DYNAMIC_NOTCH_MODE_RY,
DYNAMIC_NOTCH_MODE_PY,
DYNAMIC_NOTCH_MODE_3D
} dynamicGyroNotchMode_e;
@ -70,14 +64,11 @@ extern dynamicGyroNotchState_t dynamicGyroNotchState;
typedef struct gyroConfig_s {
uint16_t looptime; // imu loop time in us
uint8_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
uint16_t gyro_anti_aliasing_lpf_hz;
uint8_t gyro_anti_aliasing_lpf_type;
#ifdef USE_DUAL_GYRO
uint8_t gyro_to_use;
#endif
uint16_t gyro_main_lpf_hz;
uint8_t gyro_main_lpf_type;
uint8_t useDynamicLpf;
uint16_t gyroDynamicLpfMinHz;
uint16_t gyroDynamicLpfMaxHz;

View file

@ -41,6 +41,7 @@
#include "drivers/rangefinder/rangefinder_vl53l1x.h"
#include "drivers/rangefinder/rangefinder_virtual.h"
#include "drivers/rangefinder/rangefinder_us42.h"
#include "drivers/rangefinder/rangefinder_teraranger_evo.h"
#include "drivers/rangefinder/rangefinder_tof10120_i2c.h"
#include "fc/config.h"
@ -88,6 +89,14 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar
#endif
break;
case RANGEFINDER_TERARANGER_EVO:
#if defined(USE_RANGEFINDER_TERARANGER_EVO_I2C)
if (teraRangerDetect(dev)) {
rangefinderHardware = RANGEFINDER_TERARANGER_EVO;
rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_TERA_EVO_TASK_PERIOD_MS));
}
#endif
break;
case RANGEFINDER_VL53L0X:
#if defined(USE_RANGEFINDER_VL53L0X)
if (vl53l0xDetect(dev)) {

View file

@ -22,15 +22,16 @@
#include "drivers/rangefinder/rangefinder.h"
typedef enum {
RANGEFINDER_NONE = 0,
RANGEFINDER_SRF10 = 1,
RANGEFINDER_VL53L0X = 2,
RANGEFINDER_MSP = 3,
RANGEFINDER_BENEWAKE = 4,
RANGEFINDER_VL53L1X = 5,
RANGEFINDER_US42 = 6,
RANGEFINDER_TOF10102I2C = 7,
RANGEFINDER_FAKE = 8,
RANGEFINDER_NONE = 0,
RANGEFINDER_SRF10 = 1,
RANGEFINDER_VL53L0X = 2,
RANGEFINDER_MSP = 3,
RANGEFINDER_BENEWAKE = 4,
RANGEFINDER_VL53L1X = 5,
RANGEFINDER_US42 = 6,
RANGEFINDER_TOF10102I2C = 7,
RANGEFINDER_FAKE = 8,
RANGEFINDER_TERARANGER_EVO = 9,
} rangefinderType_e;
typedef struct rangefinderConfig_s {

View file

@ -142,7 +142,8 @@
#define SERIALRX_UART SERIAL_PORT_USART2
#define DEFAULT_RX_TYPE RX_TYPE_SERIAL
#define SERIALRX_PROVIDER SERIALRX_CRSF
#define CURRENT_METER_SCALE 500
#define CURRENT_METER_SCALE 589
#define CURRENT_METER_OFFSET -204
/*** Optical Flow & Lidar ***/
#define USE_RANGEFINDER

View file

@ -0,0 +1,31 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight 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.
*
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdint.h>
#include "platform.h"
#include "fc/fc_msp_box.h"
#if defined(FOXEERF722V2)
#include "io/piniobox.h"
void targetConfiguration(void)
{
pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1;
}
#endif

View file

@ -22,6 +22,7 @@
#include "drivers/timer.h"
#include "drivers/sensor.h"
#include "drivers/pwm_mapping.h"
#include "drivers/pinio.h"
BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6000_ALIGN);
#if defined(FOXEERF722DUAL)

View file

@ -147,3 +147,13 @@
#define TARGET_IO_PORTC 0xffff
#define TARGET_IO_PORTD (BIT(2))
// *************** PINIO ***************************
#if defined(FOXEERF722V2)
#define USE_PINIO
#define USE_PINIOBOX
#define PINIO1_PIN PC6 // Enable GPS power
#define PINIO1_FLAGS PINIO_FLAGS_INVERTED
#endif

View file

@ -0,0 +1 @@
target_stm32h743xi(IFLIGHT_2RAW_H743)

View file

@ -0,0 +1,31 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight 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.
*
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdint.h>
#include "platform.h"
#include "fc/fc_msp_box.h"
#include "fc/config.h"
#include "io/piniobox.h"
void targetConfiguration(void)
{
pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1;
pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2;
}

View file

@ -0,0 +1,49 @@
/*
* 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 <stdint.h>
#include "platform.h"
#include "drivers/bus.h"
#include "drivers/io.h"
#include "drivers/pwm_mapping.h"
#include "drivers/timer.h"
#include "drivers/pinio.h"
#include "drivers/sensor.h"
timerHardware_t timerHardware[] = {
DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1
DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 1), // S2
DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 2), // S3
DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 3), // S4
DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 4), // S5
DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 5), // S6
DEF_TIM(TIM4, CH1, PD12, TIM_USE_OUTPUT_AUTO, 0, 6), // S7
DEF_TIM(TIM4, CH2, PD13, TIM_USE_OUTPUT_AUTO, 0, 7), // S8
DEF_TIM(TIM4, CH3, PD14, TIM_USE_OUTPUT_AUTO, 0, 0), // S9
DEF_TIM(TIM4, CH4, PD15, TIM_USE_OUTPUT_AUTO, 0, 0), // S10 DMA_NONE
DEF_TIM(TIM15, CH1, PE5, TIM_USE_OUTPUT_AUTO, 0, 0), // S11
DEF_TIM(TIM15, CH2, PE6, TIM_USE_OUTPUT_AUTO, 0, 0), // S12 DMA_NONE
DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 9), // LED_2812
};
const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

View file

@ -0,0 +1,167 @@
/*
* 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
#define TARGET_BOARD_IDENTIFIER "2RH7"
#define USBD_PRODUCT_STRING "IFLIGHT_2RAW_H743"
#define LED0 PE3
#define LED1 PE4
#define BEEPER PC9
#define BEEPER_INVERTED
// SPI Buses
#define USE_SPI
#define USE_SPI_DEVICE_1
#define SPI1_SCK_PIN PA5
#define SPI1_MISO_PIN PA6
#define SPI1_MOSI_PIN PD7
#define USE_SPI_DEVICE_2
#define SPI2_SCK_PIN PB13
#define SPI2_MISO_PIN PB14
#define SPI2_MOSI_PIN PB15
#define USE_SPI_DEVICE_3
#define SPI3_SCK_PIN PC10
#define SPI3_MISO_PIN PC11
#define SPI3_MOSI_PIN PC12
// I2C
#define USE_I2C
#define USE_I2C_DEVICE_1
#define I2C1_SCL PB6
#define I2C1_SDA PB7
#define USE_I2C_DEVICE_2
#define I2C2_SCL PB10
#define I2C2_SDA PB11
// Gyro
#define USE_IMU_ICM42605
#define IMU_ICM42605_ALIGN CW0_DEG
#define ICM42605_SPI_BUS BUS_SPI1
#define ICM42605_CS_PIN PC15
// OSD
// #define USE_MAX7456
// #define MAX7456_SPI_BUS BUS_SPI2
// #define MAX7456_CS_PIN PB12
#define USE_BARO
#define BARO_I2C_BUS BUS_I2C2
#define USE_BARO_ALL
#define USE_MAG
#define MAG_I2C_BUS BUS_I2C1
#define USE_MAG_ALL
#define TEMPERATURE_I2C_BUS BUS_I2C1
#define PITOT_I2C_BUS BUS_I2C1
#define RANGEFINDER_I2C_BUS BUS_I2C1
// *************** UART *****************************
#define USE_VCP
#define USE_UART1
#define UART1_TX_PIN PA9
#define UART1_RX_PIN PA10
#define USE_UART2
#define UART2_TX_PIN PD5
#define UART2_RX_PIN PD6
#define USE_UART3
#define UART3_TX_PIN PD8
#define UART3_RX_PIN PD9
#define USE_UART4
#define UART4_TX_PIN PB9
#define UART4_RX_PIN PB8
#define USE_UART6
#define UART6_TX_PIN PC6
#define UART6_RX_PIN PC7
#define USE_UART7
#define UART7_TX_PIN PE8
#define UART7_RX_PIN PE7
#define USE_UART8
#define UART8_TX_PIN PE1
#define UART8_RX_PIN PE0
#define SERIAL_PORT_COUNT 8
#define DEFAULT_RX_TYPE RX_TYPE_SERIAL
#define SERIALRX_PROVIDER SERIALRX_CRSF
#define SERIALRX_UART SERIAL_PORT_USART6
// SD Card
#define USE_SDCARD
#define USE_SDCARD_SPI
#define SDCARD_SPI_BUS BUS_SPI3
#define SDCARD_CS_PIN PA15
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
// ADC
#define USE_ADC
#define ADC_INSTANCE ADC1
#define ADC_CHANNEL_1_PIN PC0
#define ADC_CHANNEL_2_PIN PC1
#define ADC_CHANNEL_3_PIN PC5
#define ADC_CHANNEL_4_PIN PC4
#define VBAT_ADC_CHANNEL ADC_CHN_1
#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2
#define RSSI_ADC_CHANNEL ADC_CHN_3
#define AIRSPEED_ADC_CHANNEL ADC_CHN_4
#define VBAT_SCALE_DEFAULT 2100
// PINIO
#define USE_PINIO
#define USE_PINIOBOX
#define PINIO1_PIN PD10
#define PINIO2_PIN PD11
// *************** LEDSTRIP ************************
// #define USE_LED_STRIP
// #define WS2811_PIN PA8
#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX)
#define CURRENT_METER_SCALE 250
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
#define TARGET_IO_PORTA 0xffff
#define TARGET_IO_PORTB 0xffff
#define TARGET_IO_PORTC 0xffff
#define TARGET_IO_PORTD 0xffff
#define TARGET_IO_PORTE 0xffff
#define MAX_PWM_OUTPUT_PORTS 12
#define USE_DSHOT
#define USE_ESC_SENSOR

View file

@ -1,2 +1,3 @@
target_stm32h743xi(MAMBAH743)
target_stm32h743xi(MAMBAH743_2022B)
target_stm32h743xi(MAMBAH743_2022B_GYRO2)

View file

@ -29,7 +29,7 @@
BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6000_ALIGN);
BUSDEV_REGISTER_SPI_TAG(busdev_bmi270, DEVHW_BMI270, BMI270_SPI_BUS, BMI270_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_BMI270_ALIGN);
#ifdef MAMBAH743_2022B
#ifdef USE_IMU_ICM42605
BUSDEV_REGISTER_SPI_TAG(busdev_icm42605, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_ICM42605_ALIGN);
#endif

View file

@ -22,6 +22,11 @@
#define TARGET_BOARD_IDENTIFIER "M743"
#define USBD_PRODUCT_STRING "MAMBAH743_2022B"
#elif defined(MAMBAH743_2022B_GYRO2)
#define TARGET_BOARD_IDENTIFIER "M743"
#define USBD_PRODUCT_STRING "MAMBAH743_2022B_GYRO2"
#else
#define TARGET_BOARD_IDENTIFIER "M743"
@ -58,13 +63,22 @@
#define BMI270_SPI_BUS BUS_SPI1
#define BMI270_CS_PIN PA4
#ifdef MAMBAH743_2022B
#define USE_SPI_DEVICE_4
#define SPI4_SCK_PIN PE12
#define SPI4_MISO_PIN PE13
#define SPI4_MOSI_PIN PE14
#ifdef MAMBAH743_2022B
#define USE_IMU_ICM42605
#define IMU_ICM42605_ALIGN CW0_DEG
#define ICM42605_SPI_BUS BUS_SPI1
#define ICM42605_CS_PIN PA4
#endif
#ifdef MAMBAH743_2022B_GYRO2
#define USE_IMU_ICM42605
#define IMU_ICM42605_ALIGN CW270_DEG
#define ICM42605_SPI_BUS BUS_SPI4
@ -175,7 +189,7 @@
#define USE_ADC
#define ADC_INSTANCE ADC3
#ifdef MAMBAH743_2022B
#if defined(MAMBAH743_2022B) || defined(MAMBAH743_2022B_GYRO2)
#define ADC_CHANNEL_1_PIN PC1
#define ADC_CHANNEL_2_PIN PC3
@ -203,7 +217,7 @@
#define USE_PINIO
#define USE_PINIOBOX
#ifdef MAMBAH743_2022B
#if defined(MAMBAH743_2022B) || defined(MAMBAH743_2022B_GYRO2)
#define PINIO1_PIN PC2
#define PINIO2_PIN PC5

View file

@ -1 +1,2 @@
target_stm32f405xg(SKYSTARSF405HD)
target_stm32f405xg(SKYSTARSF405HD2)

View file

@ -28,6 +28,10 @@ timerHardware_t timerHardware[] = {
DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0),
DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0),
DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0),
#if defined(SKYSTARSF405HD2)
DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0),
DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0),
#endif
DEF_TIM(TIM8, CH3, PC8, TIM_USE_LED, 0, 0),
};

View file

@ -156,4 +156,8 @@
#define USE_DSHOT
#define USE_ESC_SENSOR
#if defined(SKYSTARSF405HD2)
#define MAX_PWM_OUTPUT_PORTS 6
#else
#define MAX_PWM_OUTPUT_PORTS 4
#endif

View file

@ -148,8 +148,8 @@
#define ADC_CHANNEL_4_PIN PC4
#define VBAT_ADC_CHANNEL ADC_CHN_1
#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2
#define RSSI_ADC_CHANNEL ADC_CHN_4
#define AIRSPEED_ADC_CHANNEL ADC_CHN_3
#define RSSI_ADC_CHANNEL ADC_CHN_3
#define AIRSPEED_ADC_CHANNEL ADC_CHN_4
// *************** LED2812 ************************
#define USE_LED_STRIP

View file

@ -83,6 +83,7 @@
#define USE_RANGEFINDER_VL53L1X
#define USE_RANGEFINDER_US42
#define USE_RANGEFINDER_TOF10120_I2C
#define USE_RANGEFINDER_TERARANGER_EVO_I2C
// Allow default optic flow boards
#define USE_OPFLOW
@ -111,7 +112,6 @@
#define USE_FRSKYOSD
#define USE_DJI_HD_OSD
#define USE_MSP_OSD
#define USE_SMARTPORT_MASTER
#define NAV_NON_VOLATILE_WAYPOINT_CLI
@ -199,7 +199,15 @@
#define USE_HOTT_TEXTMODE
#define USE_24CHANNELS
#define MAX_MIXER_PROFILE_COUNT 2
#define USE_SMARTPORT_MASTER
#elif !defined(STM32F7)
#define MAX_MIXER_PROFILE_COUNT 1
#endif
#if (MCU_FLASH_SIZE <= 512)
#define SKIP_CLI_COMMAND_HELP
#undef USE_SERIALRX_SPEKTRUM
#undef USE_TELEMETRY_SRXL
#endif
#define USE_EZ_TUNE

View file

@ -341,6 +341,15 @@
#endif
#endif
#if defined(USE_RANGEFINDER_TERARANGER_EVO_I2C)
#if !defined(TERARANGER_EVO_I2C_BUS) && defined(RANGEFINDER_I2C_BUS)
#define TERARANGER_EVO_I2C_BUS RANGEFINDER_I2C_BUS
#endif
#if defined(TERARANGER_EVO_I2C_BUS)
BUSDEV_REGISTER_I2C(busdev_teraranger_evo, DEVHW_TERARANGER_EVO_I2C, TERARANGER_EVO_I2C_BUS, 0x31, NONE, DEVFLAGS_NONE, 0);
#endif
#endif
#if defined(USE_RANGEFINDER_TOF10120_I2C) && (defined(TOF10120_I2C_BUS) || defined(RANGEFINDER_I2C_BUS))
#if !defined(TOF10120_I2C_BUS)
#define TOF10120_I2C_BUS RANGEFINDER_I2C_BUS

View file

@ -57,7 +57,7 @@
#include "sensors/battery.h"
#include "sensors/sensors.h"
#include "sensors/barometer.h"
#include "telemetry/crsf.h"
#include "telemetry/telemetry.h"
#include "telemetry/msp_shared.h"
@ -258,22 +258,6 @@ static void crsfFrameBatterySensor(sbuf_t *dst)
crsfSerialize8(dst, batteryRemainingPercentage);
}
/*
0x09 Baro+Vario sensor
Payload:
uint16 Altitude
int16 Vertical speed ( cm/s )
*/
static void crsfFrameBaroVarioSensor(sbuf_t *dst)
{
// use sbufWrite since CRC does not include frame length
sbufWriteU8(dst, CRSF_FRAME_BAROVARIO_SENSOR_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
crsfSerialize8(dst, CRSF_FRAMETYPE_BAROVARIO_SENSOR);
int32_t altitude = baroGetLatestAltitude() / 10; // Altitude in decimeters
crsfSerialize16(dst, altitude + 10000 < 0x8000 ? altitude + 10000 : 0x8000 + altitude / 10);
crsfSerialize16(dst, lrintf(getEstimatedActualVelocity(Z)));
}
typedef enum {
CRSF_ACTIVE_ANTENNA1 = 0,
CRSF_ACTIVE_ANTENNA2 = 1
@ -431,7 +415,6 @@ typedef enum {
CRSF_FRAME_FLIGHT_MODE_INDEX,
CRSF_FRAME_GPS_INDEX,
CRSF_FRAME_VARIO_SENSOR_INDEX,
CRSF_FRAME_BAROVARIO_SENSOR_INDEX,
CRSF_SCHEDULE_COUNT_MAX
} crsfFrameTypeIndex_e;
@ -491,18 +474,13 @@ static void processCrsf(void)
crsfFrameGps(dst);
crsfFinalize(dst);
}
#endif
#if defined(USE_BARO) || defined(USE_GPS)
if (currentSchedule & BV(CRSF_FRAME_VARIO_SENSOR_INDEX)) {
crsfInitializeFrame(dst);
crsfFrameVarioSensor(dst);
crsfFinalize(dst);
}
#endif
#if defined(USE_BARO)
if (currentSchedule & BV(CRSF_FRAME_BAROVARIO_SENSOR_INDEX)) {
crsfInitializeFrame(dst);
crsfFrameBaroVarioSensor(dst);
crsfFinalize(dst);
}
#endif
crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount;
}
@ -527,23 +505,14 @@ void initCrsfTelemetry(void)
crsfSchedule[index++] = BV(CRSF_FRAME_ATTITUDE_INDEX);
crsfSchedule[index++] = BV(CRSF_FRAME_BATTERY_SENSOR_INDEX);
crsfSchedule[index++] = BV(CRSF_FRAME_FLIGHT_MODE_INDEX);
#if defined(USE_GPS)
#ifdef USE_GPS
if (feature(FEATURE_GPS)) {
crsfSchedule[index++] = BV(CRSF_FRAME_GPS_INDEX);
#if !defined(USE_BARO)
crsfSchedule[index++] = BV(CRSF_FRAME_VARIO_SENSOR_INDEX);
#endif
}
#endif
#if defined(USE_BARO)
if (sensors(SENSOR_BARO)) {
crsfSchedule[index++] = BV(CRSF_FRAME_BAROVARIO_SENSOR_INDEX);
} else {
#if defined(USE_GPS)
if (feature(FEATURE_GPS)) {
crsfSchedule[index++] = BV(CRSF_FRAME_VARIO_SENSOR_INDEX);
}
#endif
#if defined(USE_BARO) || defined(USE_GPS)
if (sensors(SENSOR_BARO) || (STATE(FIXED_WING_LEGACY) && feature(FEATURE_GPS))) {
crsfSchedule[index++] = BV(CRSF_FRAME_VARIO_SENSOR_INDEX);
}
#endif
crsfScheduleCount = (uint8_t)index;
@ -622,11 +591,7 @@ int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType)
case CRSF_FRAMETYPE_VARIO_SENSOR:
crsfFrameVarioSensor(sbuf);
break;
case CRSF_FRAMETYPE_BAROVARIO_SENSOR:
crsfFrameBaroVarioSensor(sbuf);
break;
}
const int frameSize = crsfFinalizeBuf(sbuf, frame);
return frameSize;
}

View file

@ -54,6 +54,8 @@
#include "usbd_cdc_interface.h"
#include "stdbool.h"
#include "drivers/time.h"
#include "drivers/nvic.h"
#include "build/atomic.h"
/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
@ -366,14 +368,13 @@ uint32_t CDC_Receive_BytesAvailable(void)
uint32_t CDC_Send_FreeBytes(void)
{
/*
return the bytes free in the circular buffer
uint32_t freeBytes;
functionally equivalent to:
(APP_Rx_ptr_out > APP_Rx_ptr_in ? APP_Rx_ptr_out - APP_Rx_ptr_in : APP_RX_DATA_SIZE - APP_Rx_ptr_in + APP_Rx_ptr_in)
but without the impact of the condition check.
*/
return ((UserTxBufPtrOut - UserTxBufPtrIn) + (-((int)(UserTxBufPtrOut <= UserTxBufPtrIn)) & APP_TX_DATA_SIZE)) - 1;
ATOMIC_BLOCK(NVIC_PRIO_VCP) {
freeBytes = ((UserTxBufPtrOut - UserTxBufPtrIn) + (-((int)(UserTxBufPtrOut <= UserTxBufPtrIn)) & APP_TX_DATA_SIZE)) - 1;
}
return freeBytes;
}
/**