diff --git a/.dir-locals.el b/.dir-locals.el new file mode 100644 index 00000000000..9c3727aef8e --- /dev/null +++ b/.dir-locals.el @@ -0,0 +1,5 @@ +;;; Directory Local Variables -*- no-byte-compile: t -*- +;;; For more information see (info "(emacs) Directory Variables") + +((nil . ((c-basic-offset . 4) + (c-default-style . "k&r")))) diff --git a/.gitignore b/.gitignore index ea13b9f1789..3cbe312ed07 100644 --- a/.gitignore +++ b/.gitignore @@ -12,7 +12,7 @@ __pycache__ startup_stm32f10x_md_gcc.s .vagrant/ -.vscode/ +#.vscode/ cov-int* /build/ /obj/ @@ -31,3 +31,4 @@ README.pdf # local changes only make/local.mk +launch.json diff --git a/.vimrc b/.vimrc new file mode 100644 index 00000000000..547d37812cb --- /dev/null +++ b/.vimrc @@ -0,0 +1,9 @@ +filetype on +filetype indent on + +set expandtab +set bs=2 +set sw=4 +set ts=4 + + diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 00000000000..2cece3ee127 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,12 @@ +{ + "files.associations": { + "chrono": "c", + "cmath": "c", + "ranges": "c" + }, + "editor.tabSize": 4, + "editor.insertSpaces": true, + "editor.detectIndentation": false, + "editor.expandTabs": true, + "C_Cpp.clang_format_fallbackStyle": "{ BasedOnStyle: Google, IndentWidth: 4, BreakBeforeBraces: Mozilla }" +} diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md index c2ef089afca..ed9647ae92a 100644 --- a/docs/Programming Framework.md +++ b/docs/Programming Framework.md @@ -46,16 +46,16 @@ IPF can be edited using INAV Configurator user interface, or via CLI | 10 | NAND | `false` if `Operand A` and `Operand B` are both `true`| | 11 | NOR | `true` if `Operand A` and `Operand B` are both `false` | | 12 | NOT | The boolean opposite to `Operand A` | -| 13 | STICKY | `Operand A` is the activation operator, `Operand B` is the deactivation operator. After the activation is `true`, the operator will return `true` until Operand B is evaluated as `true`| -| 14 | ADD | Add `Operand A` to `Operand B` and returns the result | -| 15 | SUB | Substract `Operand B` from `Operand A` and returns the result | -| 16 | MUL | Multiply `Operand A` by `Operand B` and returns the result | -| 17 | DIV | Divide `Operand A` by `Operand B` and returns the result | -| 18 | GVAR SET | Store value from `Operand B` into the Global Variable addressed by +| 13 | Sticky | `Operand A` is the activation operator, `Operand B` is the deactivation operator. After the activation is `true`, the operator will return `true` until Operand B is evaluated as `true`| +| 14 | Basic: Add | Add `Operand A` to `Operand B` and returns the result | +| 15 | Basic: Subtract | Substract `Operand B` from `Operand A` and returns the result | +| 16 | Basic: Multiply | Multiply `Operand A` by `Operand B` and returns the result | +| 17 | Basic: Divide | Divide `Operand A` by `Operand B` and returns the result | +| 18 | Set GVAR | Store value from `Operand B` into the Global Variable addressed by `Operand A`. Bear in mind, that operand `Global Variable` means: Value stored in Global Variable of an index! To store in GVAR 1 use `Value 1` not `Global Variable 1` | -| 19 | GVAR INC | Increase the GVAR indexed by `Operand A` (use `Value 1` for Global Variable 1) with value from `Operand B` | -| 20 | GVAR DEC | Decrease the GVAR indexed by `Operand A` (use `Value 1` for Global Variable 1) with value from `Operand B` | -| 21 | IO PORT SET | Set I2C IO Expander pin `Operand A` to value of `Operand B`. `Operand A` accepts values `0-7` and `Operand B` accepts `0` and `1` | +| 19 | Increase GVAR | Increase the GVAR indexed by `Operand A` (use `Value 1` for Global Variable 1) with value from `Operand B` | +| 20 | Decrease GVAR | Decrease the GVAR indexed by `Operand A` (use `Value 1` for Global Variable 1) with value from `Operand B` | +| 21 | Set IO Port | Set I2C IO Expander pin `Operand A` to value of `Operand B`. `Operand A` accepts values `0-7` and `Operand B` accepts `0` and `1` | | 22 | OVERRIDE_ARMING_SAFETY | Allows the craft to arm on any angle even without GPS fix. WARNING: This bypasses all safety checks, even that the throttle is low, so use with caution. If you only want to check for certain conditions, such as arm without GPS fix. You will need to add logic conditions to check the throttle is low. | | 23 | OVERRIDE_THROTTLE_SCALE | Override throttle scale to the value defined by operand. Operand type `0` and value `50` means throttle will be scaled by 50%. | | 24 | SWAP_ROLL_YAW | basically, when activated, yaw stick will control roll and roll stick will control yaw. Required for tail-sitters VTOL during vertical-horizonral transition when body frame changes | @@ -67,18 +67,18 @@ IPF can be edited using INAV Configurator user interface, or via CLI | 30 | SET_VTX_BAND | Sets VTX band. Accepted values are `1-5` | | 31 | SET_VTX_CHANNEL | Sets VTX channel. Accepted values are `1-8` | | 32 | SET_OSD_LAYOUT | Sets OSD layout. Accepted values are `0-3` | -| 33 | SIN | Computes SIN of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` | -| 34 | COS | Computes COS of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` | -| 35 | TAN | Computes TAN of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` | +| 33 | Trigonometry: Sine | Computes SIN of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` | +| 34 | Trigonometry: Cosine | Computes COS of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` | +| 35 | Trigonometry: Tangent | Computes TAN of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` | | 36 | MAP_INPUT | Scales `Operand A` from [`0` : `Operand B`] to [`0` : `1000`]. Note: input will be constrained and then scaled | | 37 | MAP_OUTPUT | Scales `Operand A` from [`0` : `1000`] to [`0` : `Operand B`]. Note: input will be constrained and then scaled | | 38 | RC_CHANNEL_OVERRIDE | Overrides channel set by `Operand A` to value of `Operand B` | | 39 | SET_HEADING_TARGET | Sets heading-hold target to `Operand A`, in degrees. Value wraps-around. | -| 40 | MOD | Modulo. Divide `Operand A` by `Operand B` and returns the remainder | +| 40 | Modulo | Modulo. Divide `Operand A` by `Operand B` and returns the remainder | | 41 | LOITER_RADIUS_OVERRIDE | Sets the loiter radius to `Operand A` [`0` : `100000`] in cm. If the value is lower than the loiter radius set in the **Advanced Tuning**, that will be used. | | 42 | SET_PROFILE | Sets the active config profile (PIDFF/Rates/Filters/etc) to `Operand A`. `Operand A` must be a valid profile number, currently from 1 to 3. If not, the profile will not change | -| 43 | MIN | Finds the lowest value of `Operand A` and `Operand B` | -| 44 | MAX | Finds the highest value of `Operand A` and `Operand B` | +| 43 | Use Lowest Value | Finds the lowest value of `Operand A` and `Operand B` | +| 44 | Use Highest Value | Finds the highest value of `Operand A` and `Operand B` | | 45 | FLIGHT_AXIS_ANGLE_OVERRIDE | Sets the target attitude angle for axis. In other words, when active, it enforces Angle mode (Heading Hold for Yaw) on this axis (Angle mode does not have to be active). `Operand A` defines the axis: `0` - Roll, `1` - Pitch, `2` - Yaw. `Operand B` defines the angle in degrees | | 46 | FLIGHT_AXIS_RATE_OVERRIDE | Sets the target rate (rotation speed) for axis. `Operand A` defines the axis: `0` - Roll, `1` - Pitch, `2` - Yaw. `Operand B` defines the rate in degrees per second | | 47 | EDGE | Momentarily true when triggered by `Operand A`. `Operand A` is the activation operator [`boolean`], `Operand B` _(Optional)_ is the time for the edge to stay active [ms]. After activation, operator will return `true` until the time in Operand B is reached. If a pure momentary edge is wanted. Just leave `Operand B` as the default `Value: 0` setting. | diff --git a/docs/Settings.md b/docs/Settings.md index d4c888aaf31..3fe84833ab7 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1372,6 +1372,16 @@ Automatic configuration of GPS baudrate(The specified baudrate in configured in --- +### gps_auto_baud_max_supported + +Max baudrate supported by GPS unit. This is used during autobaud. M8 supports up to 460400, M10 supports up to 921600 and 230400 is the value used before INAV 7.0 + +| Default | Min | Max | +| --- | --- | --- | +| 230400 | | | + +--- + ### gps_auto_config Enable automatic configuration of UBlox GPS receivers. @@ -3272,6 +3282,16 @@ Max allowed above the ground altitude for terrain following mode --- +### nav_mc_althold_throttle + +If set to STICK the FC remembers the throttle stick position when enabling ALTHOLD and treats it as the neutral midpoint for holding altitude. If set to MID_STICK or HOVER the neutral midpoint is set to the mid stick position or the hover throttle position respectively. + +| Default | Min | Max | +| --- | --- | --- | +| STICK | | | + +--- + ### nav_mc_bank_angle Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude @@ -3742,16 +3762,6 @@ Enables or Disables the use of the heading PID controller on fixed wing. Heading --- -### nav_use_midthr_for_althold - -If set to OFF, the FC remembers your throttle stick position when enabling ALTHOLD and treats it as a neutral midpoint for holding altitude - -| Default | Min | Max | -| --- | --- | --- | -| OFF | OFF | ON | - ---- - ### nav_user_control_mode Defines how Pitch/Roll input from RC receiver affects flight in POSHOLD mode: ATTI - pitch/roll controls attitude like in ANGLE mode; CRUISE - pitch/roll controls velocity in forward and right direction. diff --git a/docs/boards/SPEEDYBEEF405V3.md b/docs/boards/SPEEDYBEEF405V3.md index 8dd310c3851..7ec4b1ce944 100644 --- a/docs/boards/SPEEDYBEEF405V3.md +++ b/docs/boards/SPEEDYBEEF405V3.md @@ -1,3 +1,18 @@ # SpeedyBee F405 V3 -> Notice: At the moment, DSHOT is not supported on SpeedyBe F405 V3. Use Multishot instead \ No newline at end of file +> Notice: At the moment, DSHOT is not supported on SpeedyBe F405 V3. Use Multishot instead + +> Notice: The analog OSD and the SD card (blackbox) share the same SPI bus, which can cause problems when trying to use analog OSD and blackbox at the same time. + +## Flashing firmware + +We often see reports of users having problems flashing new firmware with this baord. The following sugestions usually seem to fix the issues. + +* Remove SD Card +* Disconnect devices from UART1 and UART3 + +Try removing the SD Card first, and if that still does not fix the issue, disconnect the devices connected to UART1 and UART3. + + + + diff --git a/docs/development/serial_printf_debugging.md b/docs/development/serial_printf_debugging.md index c9977e98bd3..6cbeec6cfdd 100644 --- a/docs/development/serial_printf_debugging.md +++ b/docs/development/serial_printf_debugging.md @@ -76,9 +76,11 @@ If the CLI `log_topics` is non-zero, then all topics matching the mask will be d A set of macros `LOG_ERROR()` (log error) through `LOG_DEBUG()` (log debug) may be used, subject to compile time log level constraints. These provide `printf` style logging for a given topic. +Note that the `topic` is specified without the `LOG_TOPIC_` prefix: + ``` // LOG_DEBUG(topic, fmt, ...) -LOG_DEBUG(LOG_TOPIC_SYSTEM, "This is %s topic debug message, value %d", "system", 42); +LOG_DEBUG(SYSTEM, "This is %s topic debug message, value %d", "system", 42); ``` It is also possible to dump a hex representation of arbitrary data, using functions named variously `LOG_BUFFER_` (`ERROR`) and `LOG_BUF_` (anything else, alas) e.g.: @@ -89,7 +91,7 @@ It is also possible to dump a hex representation of arbitrary data, using funct struct {...} tstruct; ... -LOG_BUF_DEBUG(LOG_TOPIC_TEMPERATURE, &tstruct, sizeof(tstruct)); +LOG_BUF_DEBUG(TEMPERATURE, &tstruct, sizeof(tstruct)); ``` ## Output Support @@ -104,7 +106,7 @@ Log messages are transmitted through the `FUNCTION_LOG` serial port as MSP messa For example, with the final lines of `src/main/fc/fc_init.c` set to: ``` - LOG_ERROR(LOG_TOPIC_SYSTEM, "Init is complete"); + LOG_ERROR(SYSTEM, "Init is complete"); systemState |= SYSTEM_STATE_READY; ``` diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt old mode 100644 new mode 100755 index 5badd5c5f1e..c6d445900a1 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -208,6 +208,8 @@ main_sources(COMMON_SRC drivers/pitotmeter/pitotmeter_adc.h drivers/pitotmeter/pitotmeter_ms4525.c drivers/pitotmeter/pitotmeter_ms4525.h + drivers/pitotmeter/pitotmeter_dlvr_l10d.c + drivers/pitotmeter/pitotmeter_dlvr_l10d.h drivers/pitotmeter/pitotmeter_msp.c drivers/pitotmeter/pitotmeter_msp.h drivers/pitotmeter/pitotmeter_virtual.c @@ -285,6 +287,8 @@ main_sources(COMMON_SRC fc/firmware_update.h fc/firmware_update_common.c fc/firmware_update_common.h + fc/multifunction.c + fc/multifunction.h fc/rc_smoothing.c fc/rc_smoothing.h fc/rc_adjustments.c diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 0169a30995f..cb12a7a8968 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -289,23 +289,23 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = { {"gyroADC", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, {"gyroADC", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, {"gyroADC", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, - + {"gyroRaw", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_RAW}, {"gyroRaw", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_RAW}, {"gyroRaw", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_RAW}, - + {"gyroPeakRoll", 0, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_ROLL}, {"gyroPeakRoll", 1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_ROLL}, {"gyroPeakRoll", 2, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_ROLL}, - + {"gyroPeakPitch", 0, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_PITCH}, {"gyroPeakPitch", 1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_PITCH}, {"gyroPeakPitch", 2, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_PITCH}, - + {"gyroPeakYaw", 0, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_YAW}, {"gyroPeakYaw", 1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_YAW}, {"gyroPeakYaw", 2, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_YAW}, - + {"accSmooth", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC}, {"accSmooth", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC}, @@ -366,6 +366,7 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = { {"navTgtPos", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS}, {"navTgtPos", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS}, {"navTgtPos", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS}, + {"navTgtHdg", -1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS}, {"navSurf", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS}, {"navAcc", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_ACC}, {"navAcc", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_ACC}, @@ -517,7 +518,7 @@ typedef struct blackboxMainState_s { int16_t navTargetVel[XYZ_AXIS_COUNT]; int32_t navTargetPos[XYZ_AXIS_COUNT]; int16_t navHeading; - int16_t navTargetHeading; + uint16_t navTargetHeading; int16_t navSurface; } blackboxMainState_t; @@ -740,7 +741,7 @@ static void blackboxBuildConditionCache(void) { blackboxConditionCache = 0; for (uint8_t cond = FLIGHT_LOG_FIELD_CONDITION_FIRST; cond <= FLIGHT_LOG_FIELD_CONDITION_LAST; cond++) { - + const uint64_t position = ((uint64_t)1) << cond; if (testBlackboxConditionUncached(cond)) { @@ -891,7 +892,7 @@ static void writeIntraframe(void) } blackboxWriteSigned16VBArray(blackboxCurrent->gyroADC, XYZ_AXIS_COUNT); - + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_GYRO_RAW)) { blackboxWriteSigned16VBArray(blackboxCurrent->gyroRaw, XYZ_AXIS_COUNT); } @@ -970,6 +971,7 @@ static void writeIntraframe(void) blackboxWriteSignedVB(blackboxCurrent->navTargetPos[x]); } + blackboxWriteSignedVB(blackboxCurrent->navTargetHeading); blackboxWriteSignedVB(blackboxCurrent->navSurface); } @@ -1226,6 +1228,7 @@ static void writeInterframe(void) blackboxWriteSignedVB(blackboxHistory[0]->navTargetPos[x] - blackboxLast->navTargetPos[x]); } + blackboxWriteSignedVB(blackboxCurrent->navTargetHeading - blackboxLast->navTargetHeading); blackboxWriteSignedVB(blackboxCurrent->navSurface - blackboxLast->navSurface); } @@ -1298,6 +1301,16 @@ static void writeSlowFrame(void) static void loadSlowState(blackboxSlowState_t *slow) { memcpy(&slow->flightModeFlags, &rcModeActivationMask, sizeof(slow->flightModeFlags)); //was flightModeFlags; + // Also log Nav auto selected flight modes rather than just those selected by boxmode + if (!IS_RC_MODE_ACTIVE(BOXANGLE) && FLIGHT_MODE(ANGLE_MODE)) { + slow->flightModeFlags |= (1 << BOXANGLE); + } + if (navigationGetHeadingControlState() == NAV_HEADING_CONTROL_AUTO) { + slow->flightModeFlags |= (1 << BOXHEADINGHOLD); + } + if (navigationRequiresTurnAssistance()) { + slow->flightModeFlags |= (1 << BOXTURNASSIST); + } slow->stateFlags = stateFlags; slow->failsafePhase = failsafePhase(); slow->rxSignalReceived = rxIsReceivingSignal(); @@ -1328,7 +1341,7 @@ static void loadSlowState(blackboxSlowState_t *slow) bool valid_temp; int16_t newTemp = 0; valid_temp = getIMUTemperature(&newTemp); - if (valid_temp) + if (valid_temp) slow->imuTemperature = newTemp; else slow->imuTemperature = TEMPERATURE_INVALID_VALUE; @@ -1661,6 +1674,7 @@ static void loadMainState(timeUs_t currentTimeUs) blackboxCurrent->navTargetVel[i] = navDesiredVelocity[i]; blackboxCurrent->navTargetPos[i] = navTargetPosition[i]; } + blackboxCurrent->navTargetHeading = navDesiredHeading; blackboxCurrent->navSurface = navActualSurface; } diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c index d89cb2c249b..a54843afc40 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/src/main/cms/cms_menu_navigation.c @@ -48,7 +48,7 @@ static const OSD_Entry cmsx_menuNavSettingsEntries[] = OSD_SETTING_ENTRY("MAX NAV CLIMB RATE", SETTING_NAV_AUTO_CLIMB_RATE), OSD_SETTING_ENTRY("MAX AH CLIMB RATE", SETTING_NAV_MANUAL_CLIMB_RATE), OSD_SETTING_ENTRY("MC MAX BANK ANGLE", SETTING_NAV_MC_BANK_ANGLE), - OSD_SETTING_ENTRY("MID THR FOR AH", SETTING_NAV_USE_MIDTHR_FOR_ALTHOLD), + OSD_SETTING_ENTRY("MC ALTHOLD THROT", SETTING_NAV_MC_ALTHOLD_THROTTLE), OSD_SETTING_ENTRY("MC HOVER THR", SETTING_NAV_MC_HOVER_THR), OSD_SETTING_ENTRY("LANDING DISARM", SETTING_NAV_DISARM_ON_LANDING), diff --git a/src/main/common/calibration.c b/src/main/common/calibration.c index be67a0932f8..69420c59f77 100644 --- a/src/main/common/calibration.c +++ b/src/main/common/calibration.c @@ -30,6 +30,7 @@ #include "drivers/time.h" #include "common/calibration.h" + void zeroCalibrationStartS(zeroCalibrationScalar_t * s, timeMs_t window, float threshold, bool allowFailure) { // Reset parameters and state @@ -75,9 +76,11 @@ void zeroCalibrationAddValueS(zeroCalibrationScalar_t * s, const float v) // Check if calibration is complete if ((millis() - s->params.startTimeMs) > s->params.windowSizeMs) { const float stddev = devStandardDeviation(&s->val.stdDev); + if (stddev > s->params.stdDevThreshold) { if (!s->params.allowFailure) { - // If deviation is too big - restart calibration + // If deviation is too big && no failure allowed - restart calibration + // TODO :: some safeguard should exist which will not allow to keep on calibrating for ever s->params.startTimeMs = millis(); s->params.sampleCount = 0; s->val.accumulatedValue = 0; diff --git a/src/main/drivers/adc.c b/src/main/drivers/adc.c index eeb65acfab9..679723e0a83 100644 --- a/src/main/drivers/adc.c +++ b/src/main/drivers/adc.c @@ -47,6 +47,13 @@ #ifndef ADC_CHANNEL_4_INSTANCE #define ADC_CHANNEL_4_INSTANCE ADC_INSTANCE #endif +#ifndef ADC_CHANNEL_5_INSTANCE +#define ADC_CHANNEL_5_INSTANCE ADC_INSTANCE +#endif +#ifndef ADC_CHANNEL_6_INSTANCE +#define ADC_CHANNEL_6_INSTANCE ADC_INSTANCE +#endif + #ifdef USE_ADC @@ -99,7 +106,7 @@ uint16_t adcGetChannel(uint8_t function) } } -#if defined(ADC_CHANNEL_1_PIN) || defined(ADC_CHANNEL_2_PIN) || defined(ADC_CHANNEL_3_PIN) || defined(ADC_CHANNEL_4_PIN) +#if defined(ADC_CHANNEL_1_PIN) || defined(ADC_CHANNEL_2_PIN) || defined(ADC_CHANNEL_3_PIN) || defined(ADC_CHANNEL_4_PIN) || defined(ADC_CHANNEL_5_PIN) || defined(ADC_CHANNEL_6_PIN) static bool isChannelInUse(int channel) { for (int i = 0; i < ADC_FUNCTION_COUNT; i++) { @@ -111,7 +118,7 @@ static bool isChannelInUse(int channel) } #endif -#if !defined(ADC_CHANNEL_1_PIN) || !defined(ADC_CHANNEL_2_PIN) || !defined(ADC_CHANNEL_3_PIN) || !defined(ADC_CHANNEL_4_PIN) +#if !defined(ADC_CHANNEL_1_PIN) || !defined(ADC_CHANNEL_2_PIN) || !defined(ADC_CHANNEL_3_PIN) || !defined(ADC_CHANNEL_4_PIN) || !defined(ADC_CHANNEL_5_PIN) || !defined(ADC_CHANNEL_6_PIN) static void disableChannelMapping(int channel) { for (int i = 0; i < ADC_FUNCTION_COUNT; i++) { @@ -192,6 +199,33 @@ void adcInit(drv_adc_config_t *init) disableChannelMapping(ADC_CHN_4); #endif +#ifdef ADC_CHANNEL_5_PIN + if (isChannelInUse(ADC_CHN_5)) { + adcConfig[ADC_CHN_5].adcDevice = adcDeviceByInstance(ADC_CHANNEL_5_INSTANCE); + if (adcConfig[ADC_CHN_5].adcDevice != ADCINVALID) { + adcConfig[ADC_CHN_5].tag = IO_TAG(ADC_CHANNEL_5_PIN); +#if defined(USE_ADC_AVERAGING) + activeChannelCount[adcConfig[ADC_CHN_5].adcDevice] += 1; +#endif + } + } +#else + disableChannelMapping(ADC_CHN_5); +#endif + +#ifdef ADC_CHANNEL_6_PIN + if (isChannelInUse(ADC_CHN_6)) { + adcConfig[ADC_CHN_6].adcDevice = adcDeviceByInstance(ADC_CHANNEL_6_INSTANCE); + if (adcConfig[ADC_CHN_6].adcDevice != ADCINVALID) { + adcConfig[ADC_CHN_6].tag = IO_TAG(ADC_CHANNEL_6_PIN); +#if defined(USE_ADC_AVERAGING) + activeChannelCount[adcConfig[ADC_CHN_6].adcDevice] += 1; +#endif + } + } +#else + disableChannelMapping(ADC_CHN_6); +#endif adcHardwareInit(init); } diff --git a/src/main/drivers/adc.h b/src/main/drivers/adc.h index 5ba4b999da0..1749c9fa63e 100644 --- a/src/main/drivers/adc.h +++ b/src/main/drivers/adc.h @@ -33,7 +33,9 @@ typedef enum { ADC_CHN_2, ADC_CHN_3, ADC_CHN_4, - ADC_CHN_MAX = ADC_CHN_4, + ADC_CHN_5, + ADC_CHN_6, + ADC_CHN_MAX = ADC_CHN_6, ADC_CHN_COUNT } adcChannel_e; diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index c27f21d6006..eca213b7218 100644 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -138,6 +138,7 @@ typedef enum { /* Other hardware */ DEVHW_MS4525, // Pitot meter + DEVHW_DLVR, // Pitot meter DEVHW_M25P16, // SPI NOR flash DEVHW_W25N01G, // SPI 128MB flash DEVHW_UG2864, // I2C OLED display diff --git a/src/main/drivers/osd_symbols.h b/src/main/drivers/osd_symbols.h index a127234579d..147725ab463 100644 --- a/src/main/drivers/osd_symbols.h +++ b/src/main/drivers/osd_symbols.h @@ -174,6 +174,9 @@ #define SYM_FLIGHT_MINS_REMAINING 0xDA // 218 Flight time (mins) remaining #define SYM_FLIGHT_HOURS_REMAINING 0xDB // 219 Flight time (hours) remaining #define SYM_GROUND_COURSE 0xDC // 220 Ground course +#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_LOGO_START 0x101 // 257 to 297, INAV logo diff --git a/src/main/drivers/pitotmeter/pitotmeter.h b/src/main/drivers/pitotmeter/pitotmeter.h index 474e488d6cc..562a87d8cd0 100644 --- a/src/main/drivers/pitotmeter/pitotmeter.h +++ b/src/main/drivers/pitotmeter/pitotmeter.h @@ -26,6 +26,7 @@ typedef void (*pitotCalculateFuncPtr)(struct pitotDev_s * pitot, float *pressure typedef struct pitotDev_s { busDevice_t * busDev; uint16_t delay; + float calibThreshold; pitotOpFuncPtr start; pitotOpFuncPtr get; pitotCalculateFuncPtr calculate; diff --git a/src/main/drivers/pitotmeter/pitotmeter_adc.c b/src/main/drivers/pitotmeter/pitotmeter_adc.c index 4d0237b3a95..dcc47caf788 100644 --- a/src/main/drivers/pitotmeter/pitotmeter_adc.c +++ b/src/main/drivers/pitotmeter/pitotmeter_adc.c @@ -67,6 +67,7 @@ static void adcPitotCalculate(pitotDev_t *pitot, float *pressure, float *tempera bool adcPitotDetect(pitotDev_t *pitot) { pitot->delay = 10000; + pitot->calibThreshold = 0.00001f; // TODO :: should be tested !!! pitot->start = adcPitotStart; pitot->get = adcPitotRead; pitot->calculate = adcPitotCalculate; diff --git a/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c b/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c new file mode 100755 index 00000000000..d899ac05a9e --- /dev/null +++ b/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c @@ -0,0 +1,165 @@ +/* + * 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 . + */ + +#include +#include + +#include +#include + +#include "common/log.h" +#include "common/utils.h" +#include "common/maths.h" +#include "drivers/bus_i2c.h" +#include "drivers/time.h" +#include "drivers/pitotmeter/pitotmeter.h" + +#define DLVR_L10D_ADDR 0x28 // this var is not used !!! + +// //--------------------------------------------------- +// // Gas Constant is from Aerodynamics for Engineering Students, Third Edition, E.L.Houghton and N.B.Carruthers +// #define ISA_GAS_CONSTANT 287.26f +// #define ISA_LAPSE_RATE 0.0065f + +// // Standard Sea Level values +// // Ref: https://en.wikipedia.org/wiki/Standard_sea_level +// #define SSL_AIR_DENSITY 1.225f // kg/m^3 +// #define SSL_AIR_PRESSURE 101325.01576f // Pascal +// #define SSL_AIR_TEMPERATURE 288.15f // K +// //--------------------------------------------------- + +#define INCH_OF_H2O_TO_PASCAL 248.84f + +#define INCH_H2O_TO_PASCAL(press) (INCH_OF_H2O_TO_PASCAL * (press)) + +#define RANGE_INCH_H2O 10 +#define DLVR_OFFSET 8192.0f +#define DLVR_SCALE 16384.0f +// NOTE :: DLVR_OFFSET_CORR can be used for offset correction. Now firmware relies on zero calibration +#define DLVR_OFFSET_CORR 0.0f //-9.0f + + +typedef struct __attribute__ ((__packed__)) dlvrCtx_s { + bool dataValid; + uint32_t dlvr_ut; + uint32_t dlvr_up; +} dlvrCtx_t; + +STATIC_ASSERT(sizeof(dlvrCtx_t) < BUS_SCRATCHPAD_MEMORY_SIZE, busDevice_scratchpad_memory_too_small); + +static bool dlvr_start(pitotDev_t * pitot) +{ + UNUSED(pitot); + return true; +} + +static bool dlvr_read(pitotDev_t * pitot) +{ + uint8_t rxbuf1[4]; + + dlvrCtx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev); + ctx->dataValid = false; + + if (!busReadBuf(pitot->busDev, 0xFF, rxbuf1, 4)) { + return false; + } + + // status = 00 -> ok, new data + // status = 01 -> reserved + // status = 10 -> ok, data stale + // status = 11 -> error + const uint8_t status = ((rxbuf1[0] & 0xC0) >> 6); + + if (status) { + // anything other then 00 in the status bits is an error + LOG_DEBUG( PITOT, "DLVR: Bad status read. status = %u", (unsigned int)(status) ); + return false; + } + + int16_t dP_raw1, dT_raw1; + + dP_raw1 = 0x3FFF & ((rxbuf1[0] << 8) + rxbuf1[1]); + dT_raw1 = (0xFFE0 & ((rxbuf1[2] << 8) + rxbuf1[3])) >> 5; + + // Data valid, update ut/up values + ctx->dataValid = true; + ctx->dlvr_up = dP_raw1; + ctx->dlvr_ut = dT_raw1; + + return true; +} + +static void dlvr_calculate(pitotDev_t * pitot, float *pressure, float *temperature) +{ + dlvrCtx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev); + + + // pressure in inchH2O + float dP_inchH2O = 1.25f * 2.0f * RANGE_INCH_H2O * (((float)ctx->dlvr_up - (DLVR_OFFSET + DLVR_OFFSET_CORR) ) / DLVR_SCALE); + + // temperature in deg C + float T_C = (float)ctx->dlvr_ut * (200.0f / 2047.0f) - 50.0f; + + // result must fit inside the max pressure range + if ((dP_inchH2O > RANGE_INCH_H2O) || (dP_inchH2O < -RANGE_INCH_H2O)) { + LOG_DEBUG( PITOT,"DLVR: Out of range. pressure = %f", (double)(dP_inchH2O) ); + return; + } + + if (pressure) { + *pressure = INCH_H2O_TO_PASCAL( dP_inchH2O); // Pa + } + + if (temperature) { + *temperature = C_TO_KELVIN( T_C); // K + } +} + +bool dlvrDetect(pitotDev_t * pitot) +{ + uint8_t rxbuf[4]; + bool ack = false; + + pitot->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_DLVR, 0, OWNER_AIRSPEED); + if (pitot->busDev == NULL) { + return false; + } + + // Read twice to fix: + // Sending a start-stop condition without any transitions on the SCL line (no clock pulses in between) creates a + // communication error for the next communication, even if the next start condition is correct and the clock pulse is applied. + // An additional start condition must be sent, which results in restoration of proper communication. + ack = busReadBuf(pitot->busDev, 0xFF, rxbuf, 4); + ack = busReadBuf(pitot->busDev, 0xFF, rxbuf, 4); + if (!ack) { + return false; + } + + // Initialize busDev data + dlvrCtx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev); + ctx->dataValid = false; + ctx->dlvr_ut = 0; + ctx->dlvr_up = 0; + + // Initialize pitotDev object + pitot->delay = 10000; + pitot->calibThreshold = 0.00001f; // low noise sensor + pitot->start = dlvr_start; + pitot->get = dlvr_read; + pitot->calculate = dlvr_calculate; + return true; +} diff --git a/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.h b/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.h new file mode 100755 index 00000000000..275ac25c3f4 --- /dev/null +++ b/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.h @@ -0,0 +1,20 @@ +/* + * 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 . + */ + +#pragma once + +bool dlvrDetect(pitotDev_t *pitot); diff --git a/src/main/drivers/pitotmeter/pitotmeter_fake.c b/src/main/drivers/pitotmeter/pitotmeter_fake.c index f913b76d094..f8302a38d0c 100644 --- a/src/main/drivers/pitotmeter/pitotmeter_fake.c +++ b/src/main/drivers/pitotmeter/pitotmeter_fake.c @@ -75,6 +75,7 @@ bool fakePitotDetect(pitotDev_t *pitot) fakeTemperature = 273; // 0C pitot->delay = 10000; + pitot->calibThreshold = 0.00001f; pitot->start = fakePitotStart; pitot->get = fakePitotRead; pitot->calculate = fakePitotCalculate; diff --git a/src/main/drivers/pitotmeter/pitotmeter_ms4525.c b/src/main/drivers/pitotmeter/pitotmeter_ms4525.c index 3b731eaa492..c5272ccb217 100644 --- a/src/main/drivers/pitotmeter/pitotmeter_ms4525.c +++ b/src/main/drivers/pitotmeter/pitotmeter_ms4525.c @@ -74,6 +74,13 @@ static bool ms4525_read(pitotDev_t * pitot) dP_raw2 = 0x3FFF & ((rxbuf2[0] << 8) + rxbuf2[1]); dT_raw2 = (0xFFE0 & ((rxbuf2[2] << 8) + rxbuf2[3])) >> 5; + // reject any values that are the absolute minimum or maximums these + // can happen due to gnd lifts or communication errors on the bus + if (dP_raw1 == 0x3FFF || dP_raw1 == 0 || dT_raw1 == 0x7FF || dT_raw1 == 0 || + dP_raw2 == 0x3FFF || dP_raw2 == 0 || dT_raw2 == 0x7FF || dT_raw2 == 0) { + return false; + } + // reject any double reads where the value has shifted in the upper more than 0xFF if (ABS(dP_raw1 - dP_raw2) > 0xFF || ABS(dT_raw1 - dT_raw2) > 0xFF) { return false; @@ -83,6 +90,7 @@ static bool ms4525_read(pitotDev_t * pitot) ctx->dataValid = true; ctx->ms4525_up = (dP_raw1 + dP_raw2) / 2; ctx->ms4525_ut = (dT_raw1 + dT_raw2) / 2; + return true; } @@ -136,6 +144,7 @@ bool ms4525Detect(pitotDev_t * pitot) // Initialize pitotDev object pitot->delay = 10000; + pitot->calibThreshold = 0.00005f; // noisy sensor pitot->start = ms4525_start; pitot->get = ms4525_read; pitot->calculate = ms4525_calculate; diff --git a/src/main/drivers/pitotmeter/pitotmeter_msp.c b/src/main/drivers/pitotmeter/pitotmeter_msp.c index be1cf852c7f..f0a8e82768a 100644 --- a/src/main/drivers/pitotmeter/pitotmeter_msp.c +++ b/src/main/drivers/pitotmeter/pitotmeter_msp.c @@ -87,6 +87,7 @@ bool mspPitotmeterDetect(pitotDev_t *pitot) mspPitotTemperature = 27315; // 0 deg/c pitot->delay = 10000; + pitot->calibThreshold = 0.00001f; pitot->start = mspPitotStart; pitot->get = mspPitotRead; pitot->calculate = mspPitotCalculate; @@ -95,3 +96,4 @@ bool mspPitotmeterDetect(pitotDev_t *pitot) } #endif + \ No newline at end of file diff --git a/src/main/drivers/pitotmeter/pitotmeter_virtual.c b/src/main/drivers/pitotmeter/pitotmeter_virtual.c index ef3eb8d0343..525fae31007 100644 --- a/src/main/drivers/pitotmeter/pitotmeter_virtual.c +++ b/src/main/drivers/pitotmeter/pitotmeter_virtual.c @@ -88,6 +88,7 @@ static void virtualPitotCalculate(pitotDev_t *pitot, float *pressure, float *tem bool virtualPitotDetect(pitotDev_t *pitot) { pitot->delay = 10000; + pitot->calibThreshold = 0.00001f; pitot->start = virtualPitotStart; pitot->get = virtualPitotRead; pitot->calculate = virtualPitotCalculate; diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index eb14d6772b6..4fc5a00b83e 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -195,6 +195,16 @@ static bool checkPwmTimerConflicts(const timerHardware_t *timHw) return true; } #endif +#if defined(ADC_CHANNEL_5_PIN) + if (timHw->tag == IO_TAG(ADC_CHANNEL_5_PIN)) { + return true; + } +#endif +#if defined(ADC_CHANNEL_6_PIN) + if (timHw->tag == IO_TAG(ADC_CHANNEL_6_PIN)) { + return true; + } +#endif #endif return false; diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 300853513f4..bf38ec93404 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -42,6 +42,9 @@ #include "fc/config.h" #include "fc/runtime_config.h" +#include "drivers/timer_impl.h" +#include "drivers/timer.h" + #define MULTISHOT_5US_PW (MULTISHOT_TIMER_HZ * 5 / 1000000.0f) #define MULTISHOT_20US_MULT (MULTISHOT_TIMER_HZ * 20 / 1000000.0f / 1000.0f) @@ -56,7 +59,9 @@ #define DSHOT_MOTOR_BITLENGTH 20 #define DSHOT_DMA_BUFFER_SIZE 18 /* resolution + frame reset (2us) */ +#define MAX_DMA_TIMERS 8 +#define DSHOT_COMMAND_DELAY_US 1000 #define DSHOT_COMMAND_INTERVAL_US 10000 #define DSHOT_COMMAND_QUEUE_LENGTH 8 #define DHSOT_COMMAND_QUEUE_SIZE DSHOT_COMMAND_QUEUE_LENGTH * sizeof(dshotCommands_e) @@ -64,6 +69,10 @@ typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function pointer used to write motors +#ifdef USE_DSHOT_DMAR + timerDMASafeType_t dmaBurstBuffer[MAX_DMA_TIMERS][DSHOT_DMA_BUFFER_SIZE * 4]; +#endif + typedef struct { TCH_t * tch; bool configured; @@ -77,6 +86,9 @@ typedef struct { #ifdef USE_DSHOT // DSHOT parameters timerDMASafeType_t dmaBuffer[DSHOT_DMA_BUFFER_SIZE]; +#ifdef USE_DSHOT_DMAR + timerDMASafeType_t *dmaBurstBuffer; +#endif #endif } pwmOutputPort_t; @@ -249,6 +261,22 @@ uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType) } } +#ifdef USE_DSHOT_DMAR +burstDmaTimer_t burstDmaTimers[MAX_DMA_TIMERS]; +uint8_t burstDmaTimersCount = 0; + +static uint8_t getBurstDmaTimerIndex(TIM_TypeDef *timer) +{ + for (int i = 0; i < burstDmaTimersCount; i++) { + if (burstDmaTimers[i].timer == timer) { + return i; + } + } + burstDmaTimers[burstDmaTimersCount++].timer = timer; + return burstDmaTimersCount - 1; +} +#endif + static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware, uint32_t dshotHz, bool enableOutput) { // Try allocating new port @@ -259,15 +287,43 @@ static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware, } // Configure timer DMA +#ifdef USE_DSHOT_DMAR + //uint8_t timerIndex = lookupTimerIndex(timerHardware->tim); + uint8_t burstDmaTimerIndex = getBurstDmaTimerIndex(timerHardware->tim); + if (burstDmaTimerIndex >= MAX_DMA_TIMERS) { + return NULL; + } + + port->dmaBurstBuffer = &dmaBurstBuffer[burstDmaTimerIndex][0]; + burstDmaTimer_t *burstDmaTimer = &burstDmaTimers[burstDmaTimerIndex]; + burstDmaTimer->dmaBurstBuffer = port->dmaBurstBuffer; + + if (timerPWMConfigDMABurst(burstDmaTimer, port->tch, port->dmaBurstBuffer, sizeof(port->dmaBurstBuffer[0]), DSHOT_DMA_BUFFER_SIZE)) { + port->configured = true; + } +#else if (timerPWMConfigChannelDMA(port->tch, port->dmaBuffer, sizeof(port->dmaBuffer[0]), DSHOT_DMA_BUFFER_SIZE)) { // Only mark as DSHOT channel if DMA was set successfully ZERO_FARRAY(port->dmaBuffer); port->configured = true; } +#endif return port; } +#ifdef USE_DSHOT_DMAR +static void loadDmaBufferDshotStride(timerDMASafeType_t *dmaBuffer, int stride, uint16_t packet) +{ + int i; + for (i = 0; i < 16; i++) { + dmaBuffer[i * stride] = (packet & 0x8000) ? DSHOT_MOTOR_BIT_1 : DSHOT_MOTOR_BIT_0; // MSB first + packet <<= 1; + } + dmaBuffer[i++ * stride] = 0; + dmaBuffer[i++ * stride] = 0; +} +#else static void loadDmaBufferDshot(timerDMASafeType_t *dmaBuffer, uint16_t packet) { for (int i = 0; i < 16; i++) { @@ -275,6 +331,7 @@ static void loadDmaBufferDshot(timerDMASafeType_t *dmaBuffer, uint16_t packet) packet <<= 1; } } +#endif static uint16_t prepareDshotPacket(const uint16_t value, bool requestTelemetry) { @@ -378,6 +435,7 @@ static void executeDShotCommands(void){ return; } } + delayMicroseconds(DSHOT_COMMAND_DELAY_US); for (uint8_t i = 0; i < getMotorCount(); i++) { motors[i].requestTelemetry = true; motors[i].value = currentExecutingCommand.cmd; @@ -408,6 +466,20 @@ void pwmCompleteMotorUpdate(void) { executeDShotCommands(); +#ifdef USE_DSHOT_DMAR + for (int index = 0; index < motorCount; index++) { + if (motors[index].pwmPort && motors[index].pwmPort->configured) { + uint16_t packet = prepareDshotPacket(motors[index].value, motors[index].requestTelemetry); + loadDmaBufferDshotStride(&motors[index].pwmPort->dmaBurstBuffer[motors[index].pwmPort->tch->timHw->channelIndex], 4, packet); + motors[index].requestTelemetry = false; + } + } + + for (int burstDmaTimerIndex = 0; burstDmaTimerIndex < burstDmaTimersCount; burstDmaTimerIndex++) { + burstDmaTimer_t *burstDmaTimer = &burstDmaTimers[burstDmaTimerIndex]; + pwmBurstDMAStart(burstDmaTimer, DSHOT_DMA_BUFFER_SIZE * 4); + } +#else // Generate DMA buffers for (int index = 0; index < motorCount; index++) { if (motors[index].pwmPort && motors[index].pwmPort->configured) { @@ -424,6 +496,7 @@ void pwmCompleteMotorUpdate(void) { timerPWMStartDMA(motors[index].pwmPort->tch); } } +#endif } #endif } diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index ecdf1ba48d0..664bae3ca59 100644 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -287,3 +287,15 @@ bool timerPWMDMAInProgress(TCH_t * tch) { return tch->dmaState != TCH_DMA_IDLE; } + +#ifdef USE_DSHOT_DMAR +bool timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount) +{ + return impl_timerPWMConfigDMABurst(burstDmaTimer, tch, dmaBuffer, dmaBufferElementSize, dmaBufferElementCount); +} + +void pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstLength) +{ + impl_pwmBurstDMAStart(burstDmaTimer, BurstLength); +} +#endif diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index 5272c804b8f..b4b3ed8da03 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -158,6 +158,10 @@ typedef struct timHardwareContext_s { TIM_HandleTypeDef * timHandle; #endif TCH_t ch[CC_CHANNELS_PER_TIMER]; +#ifdef USE_DSHOT_DMAR + DMA_t dmaBurstRef; + uint16_t DMASource; +#endif } timHardwareContext_t; // Per MCU timer definitions @@ -168,6 +172,20 @@ extern const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT]; extern timerHardware_t timerHardware[]; extern const int timerHardwareCount; +#ifdef USE_DSHOT_DMAR +typedef struct { + TIM_TypeDef *timer; +#ifdef USE_HAL_DRIVER + DMA_TypeDef *dma; + uint32_t streamLL; +#else + DMA_Stream_TypeDef *dmaBurstStream; +#endif + timerDMASafeType_t *dmaBurstBuffer; + uint16_t burstRequestSource; +} burstDmaTimer_t; +#endif + typedef enum { TYPE_FREE, TYPE_PWMINPUT, @@ -229,3 +247,8 @@ void timerPWMStopDMA(TCH_t * tch); bool timerPWMDMAInProgress(TCH_t * tch); volatile timCCR_t *timerCCR(TCH_t * tch); + +#ifdef USE_DSHOT_DMAR +bool timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount); +void pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstLength); +#endif \ No newline at end of file diff --git a/src/main/drivers/timer_impl.h b/src/main/drivers/timer_impl.h index 82c09837219..4d0a87f9aa5 100644 --- a/src/main/drivers/timer_impl.h +++ b/src/main/drivers/timer_impl.h @@ -84,3 +84,8 @@ bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBuf void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferElementCount); void impl_timerPWMStartDMA(TCH_t * tch); void impl_timerPWMStopDMA(TCH_t * tch); + +#ifdef USE_DSHOT_DMAR +bool impl_timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount); +void impl_pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstLength); +#endif \ No newline at end of file diff --git a/src/main/drivers/timer_impl_hal.c b/src/main/drivers/timer_impl_hal.c index 0649513f210..e800a942353 100644 --- a/src/main/drivers/timer_impl_hal.c +++ b/src/main/drivers/timer_impl_hal.c @@ -408,6 +408,109 @@ bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBuf return true; } +#ifdef USE_DSHOT_DMAR +bool impl_timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount) +{ + tch->dma = dmaGetByTag(tch->timHw->dmaTag); + tch->dmaBuffer = dmaBuffer; + if (tch->dma == NULL) { + return false; + } + + // If DMA is already in use - abort + if (dmaGetOwner(tch->dma) != OWNER_FREE) { + return false; + } + + if (!tch->timCtx->dmaBurstRef) { + // We assume that timer channels are already initialized by calls to: + // timerConfigBase + // timerPWMConfigChannel + const uint32_t streamLL = lookupDMALLStreamTable[DMATAG_GET_STREAM(tch->timHw->dmaTag)]; + + LL_DMA_DeInit(tch->dma->dma, streamLL); + + LL_DMA_InitTypeDef init; + LL_DMA_StructInit(&init); + +#if defined(STM32H7) || defined(STM32G4) + // For H7 the DMA periphRequest is encoded in the DMA tag + init.PeriphRequest = DMATAG_GET_CHANNEL(tch->timHw->dmaTag); +#else + init.Channel = lookupDMALLChannelTable[DMATAG_GET_CHANNEL(tch->timHw->dmaTag)]; +#endif + + init.PeriphOrM2MSrcAddress = (uint32_t)&tch->timHw->tim->DMAR; + init.PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT; + + switch (dmaBufferElementSize) { + case 1: + init.MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_BYTE; + init.PeriphOrM2MSrcDataSize = LL_DMA_MDATAALIGN_BYTE; + break; + case 2: + init.MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_HALFWORD; + init.PeriphOrM2MSrcDataSize = LL_DMA_MDATAALIGN_HALFWORD; + break; + case 4: + init.MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_WORD; + init.PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_WORD; + break; + default: + // Programmer error + while(1) { + + } + } + + init.MemoryOrM2MDstAddress = (uint32_t)dmaBuffer; + init.MemoryOrM2MDstIncMode = LL_DMA_MEMORY_INCREMENT; + init.NbData = dmaBufferElementCount; + init.Direction = LL_DMA_DIRECTION_MEMORY_TO_PERIPH; + init.Mode = LL_DMA_MODE_NORMAL; + init.Priority = LL_DMA_PRIORITY_HIGH; + init.FIFOMode = LL_DMA_FIFOMODE_ENABLE; + init.FIFOThreshold = LL_DMA_FIFOTHRESHOLD_FULL; + init.MemBurst = LL_DMA_MBURST_SINGLE; + init.PeriphBurst = LL_DMA_PBURST_SINGLE; + + dmaInit(tch->dma, OWNER_TIMER, 0); + dmaSetHandler(tch->dma, impl_timerDMA_IRQHandler, NVIC_PRIO_TIMER_DMA, (uint32_t)tch); + + LL_DMA_Init(tch->dma->dma, streamLL, &init); + + tch->timCtx->dmaBurstRef = tch->dma; + burstDmaTimer->burstRequestSource = lookupDMASourceTable[tch->timHw->channelIndex]; + burstDmaTimer->streamLL = lookupDMALLStreamTable[DMATAG_GET_STREAM(tch->timHw->dmaTag)]; + burstDmaTimer->dma = tch->dma->dma; + + tch->dmaState = TCH_DMA_READY; + } + + // Start PWM generation + if (tch->timHw->output & TIMER_OUTPUT_N_CHANNEL) { + HAL_TIMEx_PWMN_Start(tch->timCtx->timHandle, lookupTIMChannelTable[tch->timHw->channelIndex]); + } + else { + HAL_TIM_PWM_Start(tch->timCtx->timHandle, lookupTIMChannelTable[tch->timHw->channelIndex]); + } + + return true; +} + +void impl_pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstLength) +{ + LL_DMA_SetDataLength(burstDmaTimer->dma, burstDmaTimer->streamLL, BurstLength); + LL_DMA_EnableIT_TC(burstDmaTimer->dma, burstDmaTimer->streamLL); + LL_DMA_EnableStream(burstDmaTimer->dma, burstDmaTimer->streamLL); + /* configure the DMA Burst Mode */ + LL_TIM_ConfigDMABurst(burstDmaTimer->timer, LL_TIM_DMABURST_BASEADDR_CCR1, LL_TIM_DMABURST_LENGTH_4TRANSFERS); + /* Enable the TIM DMA Request */ + //LL_TIM_EnableDMAReq_UPDATE(burstDmaTimer->timer); + LL_TIM_EnableDMAReq_CCx(burstDmaTimer->timer, burstDmaTimer->burstRequestSource); +} +#endif + void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferElementCount) { const uint32_t streamLL = lookupDMALLStreamTable[DMATAG_GET_STREAM(tch->timHw->dmaTag)]; diff --git a/src/main/drivers/timer_impl_stdperiph.c b/src/main/drivers/timer_impl_stdperiph.c index 599b8b5cb3f..5cb3457c77b 100644 --- a/src/main/drivers/timer_impl_stdperiph.c +++ b/src/main/drivers/timer_impl_stdperiph.c @@ -357,6 +357,104 @@ bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBuf return true; } +#ifdef USE_DSHOT_DMAR +bool impl_timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount) +{ + DMA_InitTypeDef DMA_InitStructure; + TIM_TypeDef * timer = tch->timHw->tim; + + if (!tch->timCtx->dmaBurstRef) { + tch->dma = dmaGetByTag(tch->timHw->dmaTag); + if (tch->dma == NULL) { + return false; + } + + // If DMA is already in use - abort + if (tch->dma->owner != OWNER_FREE) { + return false; + } + } + + // We assume that timer channels are already initialized by calls to: + // timerConfigBase + // timerPWMConfigChannel + + TIM_CtrlPWMOutputs(timer, ENABLE); + TIM_ARRPreloadConfig(timer, ENABLE); + + TIM_CCxCmd(timer, lookupTIMChannelTable[tch->timHw->channelIndex], TIM_CCx_Enable); + TIM_Cmd(timer, ENABLE); + + if (!tch->timCtx->dmaBurstRef) { + dmaInit(tch->dma, OWNER_TIMER, 0); + dmaSetHandler(tch->dma, impl_timerDMA_IRQHandler, NVIC_PRIO_TIMER_DMA, (uint32_t)tch); + + DMA_DeInit(tch->dma->ref); + DMA_Cmd(tch->dma->ref, DISABLE); + + DMA_StructInit(&DMA_InitStructure); + + DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&tch->timHw->tim->DMAR; + DMA_InitStructure.DMA_BufferSize = dmaBufferElementCount; + DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; + DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; + DMA_InitStructure.DMA_Mode = DMA_Mode_Normal; + + switch (dmaBufferElementSize) { + case 1: + DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; + DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; + break; + case 2: + DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord; + DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; + break; + case 4: + DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Word; + DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word; + break; + default: + // Programmer error + while(1) { + + } + } + +#ifdef STM32F4 + DMA_InitStructure.DMA_Channel = dmaGetChannelByTag(tch->timHw->dmaTag); + DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)dmaBuffer; + DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral; + DMA_InitStructure.DMA_Priority = DMA_Priority_High; +#else // F3 + DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)dmaBuffer; + DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST; + DMA_InitStructure.DMA_Priority = DMA_Priority_High; + DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; +#endif + + DMA_Init(tch->dma->ref, &DMA_InitStructure); + DMA_ITConfig(tch->dma->ref, DMA_IT_TC, ENABLE); + + tch->timCtx->dmaBurstRef = tch->dma; + tch->timCtx->DMASource = lookupDMASourceTable[tch->timHw->channelIndex]; + burstDmaTimer->dmaBurstStream = tch->timCtx->dmaBurstRef->ref; + burstDmaTimer->burstRequestSource = tch->timCtx->DMASource; + + tch->dmaState = TCH_DMA_READY; + } + + return true; +} + +void impl_pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstLength) +{ + DMA_SetCurrDataCounter(burstDmaTimer->dmaBurstStream, BurstLength); + DMA_Cmd(burstDmaTimer->dmaBurstStream, ENABLE); + TIM_DMAConfig(burstDmaTimer->timer, TIM_DMABase_CCR1, TIM_DMABurstLength_4Transfers); + TIM_DMACmd(burstDmaTimer->timer, burstDmaTimer->burstRequestSource, ENABLE); +} +#endif + void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferElementCount) { // Make sure we terminate any DMA transaction currently in progress diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index ac8cf9a335e..fc9b57dea67 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -54,6 +54,7 @@ #include "fc/cli.h" #include "fc/config.h" #include "fc/controlrate_profile.h" +#include "fc/multifunction.h" #include "fc/rc_adjustments.h" #include "fc/rc_smoothing.h" #include "fc/rc_controls.h" @@ -122,6 +123,21 @@ timeUs_t lastDisarmTimeUs = 0; static bool prearmWasReset = false; // Prearm must be reset (RC Mode not active) before arming is possible static timeMs_t prearmActivationTime = 0; +static bool isAccRequired(void) { + return isModeActivationConditionPresent(BOXNAVPOSHOLD) || + isModeActivationConditionPresent(BOXNAVRTH) || + isModeActivationConditionPresent(BOXNAVWP) || + isModeActivationConditionPresent(BOXANGLE) || + isModeActivationConditionPresent(BOXHORIZON) || + isModeActivationConditionPresent(BOXNAVALTHOLD) || + isModeActivationConditionPresent(BOXHEADINGHOLD) || + isModeActivationConditionPresent(BOXNAVLAUNCH) || + isModeActivationConditionPresent(BOXTURNASSIST) || + isModeActivationConditionPresent(BOXNAVCOURSEHOLD) || + isModeActivationConditionPresent(BOXSOARING) || + failsafeConfig()->failsafe_procedure != FAILSAFE_PROCEDURE_DROP_IT; +} + bool areSensorsCalibrating(void) { #ifdef USE_BARO @@ -142,11 +158,11 @@ bool areSensorsCalibrating(void) } #endif - if (!navIsCalibrationComplete()) { + if (!navIsCalibrationComplete() && isAccRequired()) { return true; } - if (!accIsCalibrationComplete() && sensors(SENSOR_ACC)) { + if (!accIsCalibrationComplete() && sensors(SENSOR_ACC) && isAccRequired()) { return true; } @@ -161,7 +177,7 @@ int16_t getAxisRcCommand(int16_t rawData, int16_t rate, int16_t deadband) { int16_t stickDeflection = 0; -#if defined(SITL_BUILD) // Workaround due to strange bug in GCC > 10.2 https://gcc.gnu.org/bugzilla/show_bug.cgi?id=108914 +#if defined(SITL_BUILD) // Workaround due to strange bug in GCC > 10.2 https://gcc.gnu.org/bugzilla/show_bug.cgi?id=108914 const int16_t value = rawData - PWM_RANGE_MIDDLE; if (value < -500) { stickDeflection = -500; @@ -171,9 +187,9 @@ int16_t getAxisRcCommand(int16_t rawData, int16_t rate, int16_t deadband) stickDeflection = value; } #else - stickDeflection = constrain(rawData - PWM_RANGE_MIDDLE, -500, 500); + stickDeflection = constrain(rawData - PWM_RANGE_MIDDLE, -500, 500); #endif - + stickDeflection = applyDeadbandRescaled(stickDeflection, deadband, -500, 500); return rcLookup(stickDeflection, rate); } @@ -264,21 +280,7 @@ static void updateArmingStatus(void) sensors(SENSOR_ACC) && !STATE(ACCELEROMETER_CALIBRATED) && // Require ACC calibration only if any of the setting might require it - ( - isModeActivationConditionPresent(BOXNAVPOSHOLD) || - isModeActivationConditionPresent(BOXNAVRTH) || - isModeActivationConditionPresent(BOXNAVWP) || - isModeActivationConditionPresent(BOXANGLE) || - isModeActivationConditionPresent(BOXHORIZON) || - isModeActivationConditionPresent(BOXNAVALTHOLD) || - isModeActivationConditionPresent(BOXHEADINGHOLD) || - isModeActivationConditionPresent(BOXNAVLAUNCH) || - isModeActivationConditionPresent(BOXTURNASSIST) || - isModeActivationConditionPresent(BOXNAVCOURSEHOLD) || - isModeActivationConditionPresent(BOXSOARING) || - failsafeConfig()->failsafe_procedure != FAILSAFE_PROCEDURE_DROP_IT - - ) + isAccRequired() ) { ENABLE_ARMING_FLAG(ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED); } @@ -374,7 +376,7 @@ static bool emergencyArmingCanOverrideArmingDisabled(void) static bool emergencyArmingIsEnabled(void) { - return emergencyArmingUpdate(IS_RC_MODE_ACTIVE(BOXARM)) && emergencyArmingCanOverrideArmingDisabled(); + return emergencyArmingUpdate(IS_RC_MODE_ACTIVE(BOXARM), false) && emergencyArmingCanOverrideArmingDisabled(); } static void processPilotAndFailSafeActions(float dT) @@ -466,7 +468,7 @@ disarmReason_t getDisarmReason(void) return lastDisarmReason; } -bool emergencyArmingUpdate(bool armingSwitchIsOn) +bool emergencyArmingUpdate(bool armingSwitchIsOn, bool forceArm) { if (ARMING_FLAG(ARMED)) { return false; @@ -495,6 +497,10 @@ bool emergencyArmingUpdate(bool armingSwitchIsOn) toggle = true; } + if (forceArm) { + counter = EMERGENCY_ARMING_MIN_ARM_COUNT; + } + return counter >= EMERGENCY_ARMING_MIN_ARM_COUNT; } @@ -517,9 +523,12 @@ void tryArm(void) } #ifdef USE_DSHOT - if (STATE(MULTIROTOR) && IS_RC_MODE_ACTIVE(BOXTURTLE) && !FLIGHT_MODE(TURTLE_MODE) && - emergencyArmingCanOverrideArmingDisabled() && isMotorProtocolDshot() - ) { +#ifdef USE_MULTI_FUNCTIONS + const bool turtleIsActive = IS_RC_MODE_ACTIVE(BOXTURTLE) || MULTI_FUNC_FLAG(MF_TURTLE_MODE); +#else + const bool turtleIsActive = IS_RC_MODE_ACTIVE(BOXTURTLE); +#endif + if (STATE(MULTIROTOR) && turtleIsActive && !FLIGHT_MODE(TURTLE_MODE) && emergencyArmingCanOverrideArmingDisabled() && isMotorProtocolDshot()) { sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_REVERSED); ENABLE_ARMING_FLAG(ARMED); enableFlightMode(TURTLE_MODE); @@ -836,7 +845,7 @@ static float calculateThrottleTiltCompensationFactor(uint8_t throttleTiltCompens void taskMainPidLoop(timeUs_t currentTimeUs) { - + cycleTime = getTaskDeltaTime(TASK_SELF); dT = (float)cycleTime * 0.000001f; diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index 37d0bbda79a..c66a0050ba2 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -42,7 +42,7 @@ timeUs_t getLastDisarmTimeUs(void); void tryArm(void); disarmReason_t getDisarmReason(void); -bool emergencyArmingUpdate(bool armingSwitchIsOn); +bool emergencyArmingUpdate(bool armingSwitchIsOn, bool forceArm); bool areSensorsCalibrating(void); float getFlightTime(void); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index dd28ae61663..821e4526337 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1058,7 +1058,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF legacyLedConfig |= ledConfig->led_function << shiftCount; shiftCount += 4; legacyLedConfig |= (ledConfig->led_overlay & 0x3F) << (shiftCount); - shiftCount += 6; + shiftCount += 6; legacyLedConfig |= (ledConfig->led_color) << (shiftCount); shiftCount += 4; legacyLedConfig |= (ledConfig->led_direction) << (shiftCount); @@ -1289,7 +1289,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU16(dst, navConfig()->general.max_manual_speed); sbufWriteU16(dst, navConfig()->general.max_manual_climb_rate); sbufWriteU8(dst, navConfig()->mc.max_bank_angle); - sbufWriteU8(dst, navConfig()->general.flags.use_thr_mid_for_althold); + sbufWriteU8(dst, navConfig()->mc.althold_throttle_type); sbufWriteU16(dst, currentBatteryProfile->nav.mc.hover_throttle); break; @@ -2255,7 +2255,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) navConfigMutable()->general.max_manual_speed = sbufReadU16(src); navConfigMutable()->general.max_manual_climb_rate = sbufReadU16(src); navConfigMutable()->mc.max_bank_angle = sbufReadU8(src); - navConfigMutable()->general.flags.use_thr_mid_for_althold = sbufReadU8(src); + navConfigMutable()->mc.althold_throttle_type = sbufReadU8(src); currentBatteryProfileMutable->nav.mc.hover_throttle = sbufReadU16(src); } else return MSP_RESULT_ERROR; @@ -2757,7 +2757,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) ledConfig->led_position = legacyConfig & 0xFF; ledConfig->led_function = (legacyConfig >> 8) & 0xF; ledConfig->led_overlay = (legacyConfig >> 12) & 0x3F; - ledConfig->led_color = (legacyConfig >> 18) & 0xF; + ledConfig->led_color = (legacyConfig >> 18) & 0xF; ledConfig->led_direction = (legacyConfig >> 22) & 0x3F; ledConfig->led_params = (legacyConfig >> 28) & 0xF; @@ -3482,7 +3482,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu #ifdef USE_SIMULATOR case MSP_SIMULATOR: tmp_u8 = sbufReadU8(src); // Get the Simulator MSP version - + // Check the MSP version of simulator if (tmp_u8 != SIMULATOR_MSP_VERSION) { break; @@ -3508,7 +3508,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu // Review: Many states were affected. Reboot? disarm(DISARM_SWITCH); // Disarm to prevent motor output!!! - } + } } else { if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once #ifdef USE_BARO @@ -3518,7 +3518,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu DISABLE_ARMING_FLAG(ARMING_DISABLED_HARDWARE_FAILURE); baroStartCalibration(); } -#endif +#endif #ifdef USE_MAG if (compassConfig()->mag_hardware != MAG_NONE) { @@ -3578,7 +3578,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu } else { sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT); } - + // Get the acceleration in 1G units acc.accADCf[X] = ((int16_t)sbufReadU16(src)) / 1000.0f; acc.accADCf[Y] = ((int16_t)sbufReadU16(src)) / 1000.0f; @@ -3586,7 +3586,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu acc.accVibeSq[X] = 0.0f; acc.accVibeSq[Y] = 0.0f; acc.accVibeSq[Z] = 0.0f; - + // Get the angular velocity in DPS gyro.gyroADCf[X] = ((int16_t)sbufReadU16(src)) / 16.0f; gyro.gyroADCf[Y] = ((int16_t)sbufReadU16(src)) / 16.0f; @@ -3621,7 +3621,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu simulatorData.airSpeed = sbufReadU16(src); } else { if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) { - sbufReadU16(src); + sbufReadU16(src); } } diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index fa595676805..59aba5f8fe9 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -98,6 +98,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { .boxId = BOXSOARING, .boxName = "SOARING", .permanentId = 56 }, { .boxId = BOXCHANGEMISSION, .boxName = "MISSION CHANGE", .permanentId = 59 }, { .boxId = BOXBEEPERMUTE, .boxName = "BEEPER MUTE", .permanentId = 60 }, + { .boxId = BOXMULTIFUNCTION, .boxName = "MULTI FUNCTION", .permanentId = 61 }, { .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL, .permanentId = 0xFF } }; @@ -179,6 +180,9 @@ void initActiveBoxIds(void) RESET_BOX_ID_COUNT; ADD_ACTIVE_BOX(BOXARM); ADD_ACTIVE_BOX(BOXPREARM); +#ifdef USE_MULTI_FUNCTIONS + ADD_ACTIVE_BOX(BOXMULTIFUNCTION); +#endif if (sensors(SENSOR_ACC) && STATE(ALTITUDE_CONTROL)) { ADD_ACTIVE_BOX(BOXANGLE); @@ -412,7 +416,9 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) #ifdef USE_MULTI_MISSION CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCHANGEMISSION)), BOXCHANGEMISSION); #endif - +#ifdef USE_MULTI_FUNCTIONS + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMULTIFUNCTION)), BOXMULTIFUNCTION); +#endif memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t)); for (uint32_t i = 0; i < activeBoxIdCount; i++) { if (activeBoxes[activeBoxIds[i]]) { diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index b41acdb51b1..af5f27b5272 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -503,7 +503,7 @@ cfTask_t cfTasks[TASK_COUNT] = { [TASK_PITOT] = { .taskName = "PITOT", .taskFunc = taskUpdatePitot, - .desiredPeriod = TASK_PERIOD_HZ(100), + .desiredPeriod = TASK_PERIOD_MS(20), .staticPriority = TASK_PRIORITY_MEDIUM, }, #endif diff --git a/src/main/fc/multifunction.c b/src/main/fc/multifunction.c new file mode 100644 index 00000000000..c217110842d --- /dev/null +++ b/src/main/fc/multifunction.c @@ -0,0 +1,132 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include "platform.h" +#include "build/debug.h" +#include "drivers/time.h" + +#ifdef USE_MULTI_FUNCTIONS + +#include "fc/fc_core.h" +#include "fc/multifunction.h" +#include "fc/rc_modes.h" +#include "fc/runtime_config.h" + +#include "io/osd.h" +#include "navigation/navigation.h" + +multi_function_e selectedItem = MULTI_FUNC_NONE; +uint8_t multiFunctionFlags; +bool nextItemIsAvailable = false; + +static void multiFunctionApply(multi_function_e selectedItem) +{ + switch (selectedItem) { + case MULTI_FUNC_NONE: + break; + case MULTI_FUNC_1: // redisplay current warnings + osdResetWarningFlags(); + break; + case MULTI_FUNC_2: // control manual emergency landing + checkManualEmergencyLandingControl(ARMING_FLAG(ARMED)); + break; + case MULTI_FUNC_3: // toggle Safehome suspend +#if defined(USE_SAFE_HOME) + if (navConfig()->general.flags.safehome_usage_mode != SAFEHOME_USAGE_OFF) { + MULTI_FUNC_FLAG(MF_SUSPEND_SAFEHOMES) ? MULTI_FUNC_FLAG_DISABLE(MF_SUSPEND_SAFEHOMES) : MULTI_FUNC_FLAG_ENABLE(MF_SUSPEND_SAFEHOMES); + } +#endif + break; + case MULTI_FUNC_4: // toggle RTH Trackback suspend + if (navConfig()->general.flags.rth_trackback_mode != RTH_TRACKBACK_OFF) { + MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK) ? MULTI_FUNC_FLAG_DISABLE(MF_SUSPEND_TRACKBACK) : MULTI_FUNC_FLAG_ENABLE(MF_SUSPEND_TRACKBACK); + } + break; + case MULTI_FUNC_5: +#ifdef USE_DSHOT + if (STATE(MULTIROTOR)) { // toggle Turtle mode + MULTI_FUNC_FLAG(MF_TURTLE_MODE) ? MULTI_FUNC_FLAG_DISABLE(MF_TURTLE_MODE) : MULTI_FUNC_FLAG_ENABLE(MF_TURTLE_MODE); + } +#endif + break; + case MULTI_FUNC_6: // emergency ARM + if (!ARMING_FLAG(ARMED)) { + emergencyArmingUpdate(true, true); + } + case MULTI_FUNC_END: + break; + } +} + +bool isNextMultifunctionItemAvailable(void) +{ + return nextItemIsAvailable; +} + +void setMultifunctionSelection(multi_function_e item) +{ + selectedItem = item == MULTI_FUNC_END ? MULTI_FUNC_1 : item; + nextItemIsAvailable = false; +} + +multi_function_e multiFunctionSelection(void) +{ + static timeMs_t startTimer; + static timeMs_t selectTimer; + static bool toggle = true; + const timeMs_t currentTime = millis(); + + if (IS_RC_MODE_ACTIVE(BOXMULTIFUNCTION)) { + if (selectTimer) { + if (currentTime - selectTimer > 3000) { // 3s selection duration to activate selected function + multiFunctionApply(selectedItem); + selectTimer = 0; + selectedItem = MULTI_FUNC_NONE; + nextItemIsAvailable = false; + } + } else if (toggle) { + if (selectedItem == MULTI_FUNC_NONE) { + selectedItem++; + } else { + selectTimer = currentTime; + nextItemIsAvailable = true; + } + } + startTimer = currentTime; + toggle = false; + } else if (startTimer) { + if (!toggle && selectTimer) { + setMultifunctionSelection(++selectedItem); + } + if (currentTime - startTimer > 4000) { // 4s reset delay after mode deselected + startTimer = 0; + selectedItem = MULTI_FUNC_NONE; + } + selectTimer = 0; + toggle = true; + } + + return selectedItem; +} +#endif diff --git a/src/main/fc/multifunction.h b/src/main/fc/multifunction.h new file mode 100644 index 00000000000..b5c1e1d9d69 --- /dev/null +++ b/src/main/fc/multifunction.h @@ -0,0 +1,55 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +#ifdef USE_MULTI_FUNCTIONS + +extern uint8_t multiFunctionFlags; + +#define MULTI_FUNC_FLAG_DISABLE(mask) (multiFunctionFlags &= ~(mask)) +#define MULTI_FUNC_FLAG_ENABLE(mask) (multiFunctionFlags |= (mask)) +#define MULTI_FUNC_FLAG(mask) (multiFunctionFlags & (mask)) + +typedef enum { + MF_SUSPEND_SAFEHOMES = (1 << 0), + MF_SUSPEND_TRACKBACK = (1 << 1), + MF_TURTLE_MODE = (1 << 2), +} multiFunctionFlags_e; + +typedef enum { + MULTI_FUNC_NONE, + MULTI_FUNC_1, + MULTI_FUNC_2, + MULTI_FUNC_3, + MULTI_FUNC_4, + MULTI_FUNC_5, + MULTI_FUNC_6, + MULTI_FUNC_END, +} multi_function_e; + +multi_function_e multiFunctionSelection(void); +bool isNextMultifunctionItemAvailable(void); +void setMultifunctionSelection(multi_function_e item); +#endif diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 67f9273d262..c396fa8814f 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -225,7 +225,7 @@ void processRcStickPositions(bool isThrottleLow) rcDisarmTimeMs = currentTimeMs; tryArm(); } else { - emergencyArmingUpdate(armingSwitchIsActive); + emergencyArmingUpdate(armingSwitchIsActive, false); // Disarming via ARM BOX // Don't disarm via switch if failsafe is active or receiver doesn't receive data - we can't trust receiver // and can't afford to risk disarming in the air diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 93b17e6e2fe..674fdded8d2 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -78,6 +78,7 @@ typedef enum { BOXUSER4 = 49, BOXCHANGEMISSION = 50, BOXBEEPERMUTE = 51, + BOXMULTIFUNCTION = 52, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml old mode 100644 new mode 100755 index 3e901022c9f..10a53df3e9b --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -19,7 +19,7 @@ tables: values: ["NONE", "AUTO", "BMP085", "MS5611", "BMP280", "MS5607", "LPS25H", "SPL06", "BMP388", "DPS310", "B2SMPB", "MSP", "FAKE"] enum: baroSensor_e - name: pitot_hardware - values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE", "MSP"] + values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE", "MSP", "DLVR-L10D"] enum: pitotSensor_e - name: receiver_type values: ["NONE", "SERIAL", "MSP", "SIM (SITL)"] @@ -190,6 +190,12 @@ tables: - name: nav_fw_wp_turn_smoothing values: ["OFF", "ON", "ON-CUT"] enum: wpFwTurnSmoothing_e + - name: gps_auto_baud_max + values: [ '115200', '57600', '38400', '19200', '9600', '230400', '460800', '921600'] + enum: gpsBaudRate_e + - name: nav_mc_althold_throttle + values: ["STICK", "MID_STICK", "HOVER"] + enum: navMcAltHoldThrottle_e constants: RPYL_PID_MIN: 0 @@ -1501,6 +1507,7 @@ groups: min: 1 max: 3000 - name: PG_GPS_CONFIG + headers: [ "io/gps.h" ] type: gpsConfig_t condition: USE_GPS members: @@ -1532,6 +1539,12 @@ groups: default_value: ON field: autoBaud type: bool + - name: gps_auto_baud_max_supported + description: "Max baudrate supported by GPS unit. This is used during autobaud. M8 supports up to 460400, M10 supports up to 921600 and 230400 is the value used before INAV 7.0" + default_value: "230400" + table: gps_auto_baud_max + field: autoBaudMax + type: uint8_t - name: gps_ublox_use_galileo description: "Enable use of Galileo satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires M8N and Ublox firmware 3.x (or later) [OFF/ON]." default_value: OFF @@ -2310,11 +2323,11 @@ groups: default_value: OFF field: general.flags.landing_bump_detection type: bool - - name: nav_use_midthr_for_althold - description: "If set to OFF, the FC remembers your throttle stick position when enabling ALTHOLD and treats it as a neutral midpoint for holding altitude" - default_value: OFF - field: general.flags.use_thr_mid_for_althold - type: bool + - name: nav_mc_althold_throttle + description: "If set to STICK the FC remembers the throttle stick position when enabling ALTHOLD and treats it as the neutral midpoint for holding altitude. If set to MID_STICK or HOVER the neutral midpoint is set to the mid stick position or the hover throttle position respectively." + default_value: "STICK" + field: mc.althold_throttle_type + table: nav_mc_althold_throttle - name: nav_extra_arming_safety description: "If set to ON drone won't arm if no GPS fix and any navigation mode like RTH or POSHOLD is configured. ALLOW_BYPASS allows the user to momentarily disable this check by holding yaw high (left stick held at the bottom right in mode 2) when switch arming is used" default_value: "ALLOW_BYPASS" diff --git a/src/main/flight/dynamic_lpf.c b/src/main/flight/dynamic_lpf.c index c93cd921462..5751c84e85e 100644 --- a/src/main/flight/dynamic_lpf.c +++ b/src/main/flight/dynamic_lpf.c @@ -41,7 +41,8 @@ void dynamicLpfGyroTask(void) { return; } - const float throttle = scaleRangef((float) rcCommand[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle, 0.0f, 1.0f); + const float throttleConstrained = (float) constrain(rcCommand[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle); + const float throttle = scaleRangef(throttleConstrained, getThrottleIdleValue(), motorConfig()->maxthrottle, 0.0f, 1.0f); const float cutoffFreq = dynLpfCutoffFreq(throttle, gyroConfig()->gyroDynamicLpfMinHz, gyroConfig()->gyroDynamicLpfMaxHz, gyroConfig()->gyroDynamicLpfCurveExpo); DEBUG_SET(DEBUG_DYNAMIC_GYRO_LPF, 0, cutoffFreq); diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index ab48fba2c6e..2e179f67bb1 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -353,11 +353,9 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void) // Craft is closer than minimum failsafe procedure distance (if set to non-zero) // GPS must also be working, and home position set - if (failsafeConfig()->failsafe_min_distance > 0 && - sensors(SENSOR_GPS) && STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) { - + if (failsafeConfig()->failsafe_min_distance > 0 && sensors(SENSOR_GPS) && STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) { // get the distance to the original arming point - uint32_t distance = calculateDistanceToDestination(&original_rth_home); + uint32_t distance = calculateDistanceToDestination(&posControl.rthState.originalHomePosition); if (distance < failsafeConfig()->failsafe_min_distance) { // Use the alternate, minimum distance failsafe procedure instead return failsafeConfig()->failsafe_min_distance_procedure; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 0161ac25d88..b2fd0383a25 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -886,8 +886,6 @@ static uint8_t getHeadingHoldState(void) } else if (ABS(rcCommand[YAW]) == 0 && FLIGHT_MODE(HEADING_MODE)) { return HEADING_HOLD_ENABLED; - } else { - return HEADING_HOLD_UPDATE_HEADING; } return HEADING_HOLD_UPDATE_HEADING; @@ -1124,7 +1122,7 @@ void FAST_CODE pidController(float dT) } } - if ((FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || navigationRequiresTurnAssistance())) { + if ((FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { float bankAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL])); float pitchAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_PITCH], pidProfile()->max_angle_inclination[FD_PITCH])); pidTurnAssistant(pidState, bankAngleTarget, pitchAngleTarget); diff --git a/src/main/io/gps.c b/src/main/io/gps.c index a66409d5651..4d7380e84df 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -113,7 +113,7 @@ static gpsProviderDescriptor_t gpsProviders[GPS_PROVIDER_COUNT] = { }; -PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 3); +PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 4); PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig, .provider = SETTING_GPS_PROVIDER_DEFAULT, @@ -125,13 +125,13 @@ PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig, .ubloxUseGalileo = SETTING_GPS_UBLOX_USE_GALILEO_DEFAULT, .ubloxUseBeidou = SETTING_GPS_UBLOX_USE_BEIDOU_DEFAULT, .ubloxUseGlonass = SETTING_GPS_UBLOX_USE_GLONASS_DEFAULT, - .ubloxNavHz = SETTING_GPS_UBLOX_NAV_HZ_DEFAULT + .ubloxNavHz = SETTING_GPS_UBLOX_NAV_HZ_DEFAULT, + .autoBaudMax = SETTING_GPS_AUTO_BAUD_MAX_SUPPORTED_DEFAULT ); - -int getGpsBaudrate(void) +int gpsBaudRateToInt(gpsBaudRate_e baudrate) { - switch(gpsState.baudrateIndex) + switch(baudrate) { case GPS_BAUDRATE_115200: return 115200; @@ -154,6 +154,11 @@ int getGpsBaudrate(void) } } +int getGpsBaudrate(void) +{ + return gpsBaudRateToInt(gpsState.baudrateIndex); +} + const char *getGpsHwVersion(void) { switch(gpsState.hwVersion) diff --git a/src/main/io/gps.h b/src/main/io/gps.h index 9f06e21f722..99b6aafbdf0 100755 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -99,6 +99,7 @@ typedef struct gpsConfig_s { bool ubloxUseGlonass; uint8_t gpsMinSats; uint8_t ubloxNavHz; + gpsBaudRate_e autoBaudMax; } gpsConfig_t; PG_DECLARE(gpsConfig_t, gpsConfig); @@ -175,6 +176,7 @@ uint8_t getGpsProtoMajorVersion(void); uint8_t getGpsProtoMinorVersion(void); int getGpsBaudrate(void); +int gpsBaudRateToInt(gpsBaudRate_e baudrate); #if defined(USE_GPS_FAKE) void gpsFakeSet( diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index ea202797539..2baa97ce9b7 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -55,17 +55,20 @@ // SBAS_AUTO, SBAS_EGNOS, SBAS_WAAS, SBAS_MSAS, SBAS_GAGAN, SBAS_NONE -// note PRNs last upadted 2020-12-18 +// note PRNs last upadted 2023-08-10 +// https://www.gps.gov/technical/prn-codes/L1-CA-PRN-code-assignments-2023-Apr.pdf #define SBASMASK1_BASE 120 #define SBASMASK1_BITS(prn) (1 << (prn-SBASMASK1_BASE)) static const uint32_t ubloxScanMode1[] = { 0x00000000, // AUTO - (SBASMASK1_BITS(123) | SBASMASK1_BITS(126) | SBASMASK1_BITS(136)), // SBAS - (SBASMASK1_BITS(131) | SBASMASK1_BITS(133) | SBASMASK1_BITS(138)), // WAAS - (SBASMASK1_BITS(129) | SBASMASK1_BITS(137)), // MAAS - (SBASMASK1_BITS(127) | SBASMASK1_BITS(128)), // GAGAN + (SBASMASK1_BITS(121) | SBASMASK1_BITS(123) | SBASMASK1_BITS(126) | SBASMASK1_BITS(136) | SBASMASK1_BITS(150)), // SBAS_EGNOS + (SBASMASK1_BITS(131) | SBASMASK1_BITS(133) | SBASMASK1_BITS(135) | SBASMASK1_BITS(138)), // WAAS + + (SBASMASK1_BITS(129) | SBASMASK1_BITS(137) | SBASMASK1_BITS(139)), // MSAS + + (SBASMASK1_BITS(127) | SBASMASK1_BITS(128) | SBASMASK1_BITS(132)), // GAGAN 0x00000000, // NONE }; @@ -76,8 +79,8 @@ static const char * baudInitDataNMEA[GPS_BAUDRATE_COUNT] = { "$PUBX,41,1,0003,0001,19200,0*23\r\n", // GPS_BAUDRATE_19200 "$PUBX,41,1,0003,0001,9600,0*16\r\n", // GPS_BAUDRATE_9600 "$PUBX,41,1,0003,0001,230400,0*1C\r\n", // GPS_BAUDRATE_230400 - "$PUBX,41,1,0003,0001,460800,0*1C\r\n", // GPS_BAUDRATE_460800 - "$PUBX,41,1,0003,0001,921600,0*1C\r\n" // GPS_BAUDRATE_921600 + "$PUBX,41,1,0003,0001,460800,0*13\r\n", // GPS_BAUDRATE_460800 + "$PUBX,41,1,0003,0001,921600,0*15\r\n" // GPS_BAUDRATE_921600 }; // Packet checksum accumulators @@ -379,9 +382,9 @@ static void configureGNSS10(void) { ubx_config_data8_payload_t gnssConfigValues[] = { // SBAS - {UBLOX_CFG_SIGNAL_SBAS_ENA, 1}, - {UBLOX_CFG_SIGNAL_SBAS_L1CA_ENA, 1}, - + {UBLOX_CFG_SIGNAL_SBAS_ENA, gpsState.gpsConfig->sbasMode == SBAS_NONE ? 0 : 1}, + {UBLOX_CFG_SIGNAL_SBAS_L1CA_ENA, gpsState.gpsConfig->sbasMode == SBAS_NONE ? 0 : 1}, + // Galileo {UBLOX_CFG_SIGNAL_GAL_ENA, gpsState.gpsConfig->ubloxUseGalileo}, {UBLOX_CFG_SIGNAL_GAL_E1_ENA, gpsState.gpsConfig->ubloxUseGalileo}, @@ -997,6 +1000,11 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) // Try sending baud rate switch command at all common baud rates gpsSetProtocolTimeout((GPS_BAUD_CHANGE_DELAY + 50) * (GPS_BAUDRATE_COUNT)); for (gpsState.autoBaudrateIndex = 0; gpsState.autoBaudrateIndex < GPS_BAUDRATE_COUNT; gpsState.autoBaudrateIndex++) { + if (gpsBaudRateToInt(gpsState.autoBaudrateIndex) > gpsBaudRateToInt(gpsState.gpsConfig->autoBaudMax)) { + // trying higher baud rates fails on m8 gps + // autoBaudRateIndex is not sorted by baud rate + continue; + } // 2. Set serial port to baud rate and send an $UBX command to switch the baud rate specified by portConfig [baudrateIndex] serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.autoBaudrateIndex]]); serialPrint(gpsState.gpsPort, baudInitDataNMEA[gpsState.baudrateIndex]); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index b05c1d5a50e..f6c97e4733d 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -76,6 +76,7 @@ #include "fc/controlrate_profile.h" #include "fc/fc_core.h" #include "fc/fc_tasks.h" +#include "fc/multifunction.h" #include "fc/rc_adjustments.h" #include "fc/rc_controls.h" #include "fc/rc_modes.h" @@ -99,6 +100,7 @@ #include "sensors/acceleration.h" #include "sensors/battery.h" #include "sensors/boardalignment.h" +#include "sensors/compass.h" #include "sensors/diagnostics.h" #include "sensors/sensors.h" #include "sensors/pitotmeter.h" @@ -186,6 +188,9 @@ static bool fullRedraw = false; static uint8_t armState; +static textAttributes_t osdGetMultiFunctionMessage(char *buff); +static uint8_t osdWarningsFlags = 0; + typedef struct osdMapData_s { uint32_t scale; char referenceSymbol; @@ -986,7 +991,7 @@ static const char * osdFailsafeInfoMessage(void) #if defined(USE_SAFE_HOME) static const char * divertingToSafehomeMessage(void) { - if (NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && safehome_applied) { + if (NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && posControl.safehomeState.isApplied) { return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME); } return NULL; @@ -1032,7 +1037,7 @@ static const char * navigationStateMessage(void) case MW_NAV_STATE_HOVER_ABOVE_HOME: if (STATE(FIXED_WING_LEGACY)) { #if defined(USE_SAFE_HOME) - if (safehome_applied) { + if (posControl.safehomeState.isApplied) { return OSD_MESSAGE_STR(OSD_MSG_LOITERING_SAFEHOME); } #endif @@ -1787,8 +1792,10 @@ static bool osdDrawSingleElement(uint8_t item) buff[1] = SYM_SAT_R; tfp_sprintf(buff + 2, "%2d", gpsSol.numSat); if (!STATE(GPS_FIX)) { - if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { - strcpy(buff + 2, "X!"); + hardwareSensorStatus_e sensorStatus = getHwGPSStatus(); + if (sensorStatus == HW_SENSOR_UNAVAILABLE || sensorStatus == HW_SENSOR_UNHEALTHY) { + buff[2] = SYM_ALERT; + buff[3] = '\0'; } TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } @@ -2022,7 +2029,23 @@ static bool osdDrawSingleElement(uint8_t item) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } - break; + displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr); + + if (STATE(MULTIROTOR) && posControl.flags.isAdjustingAltitude) { + /* Indicate MR altitude adjustment active with constant symbol at first blank position. + * Alternate symbol on/off with 600ms cycle if first position not blank (to maintain visibility of -ve sign) */ + int8_t blankPos; + for (blankPos = 2; blankPos >= 0; blankPos--) { + if (buff[blankPos] == SYM_BLANK) { + break; + } + } + if (blankPos >= 0 || OSD_ALTERNATING_CHOICES(600, 2) == 0) { + blankPos = blankPos < 0 ? 0 : blankPos; + displayWriteChar(osdDisplayPort, elemPosX + blankPos, elemPosY, SYM_TERRAIN_FOLLOWING); + } + } + return true; } case OSD_ALTITUDE_MSL: @@ -3458,6 +3481,21 @@ static bool osdDrawSingleElement(uint8_t item) } #endif // USE_ADC #endif // USE_POWER_LIMITS + case OSD_MULTI_FUNCTION: + { + // message shown infrequently so only write when needed + static bool clearMultiFunction = true; + elemAttr = osdGetMultiFunctionMessage(buff); + if (buff[0] == 0) { + if (clearMultiFunction) { + displayWrite(osdDisplayPort, elemPosX, elemPosY, " "); + clearMultiFunction = false; + } + return true; + } + clearMultiFunction = true; + break; + } default: return false; @@ -3552,10 +3590,10 @@ uint8_t osdIncElementIndex(uint8_t elementIndex) elementIndex = OSD_AIR_MAX_SPEED; } if (elementIndex == OSD_GLIDE_RANGE) { - elementIndex = feature(FEATURE_CURRENT_METER) ? OSD_CLIMB_EFFICIENCY : OSD_ITEM_COUNT; + elementIndex = feature(FEATURE_CURRENT_METER) ? OSD_CLIMB_EFFICIENCY : OSD_PILOT_NAME; } if (elementIndex == OSD_NAV_WP_MULTI_MISSION_INDEX) { - elementIndex = OSD_ITEM_COUNT; + elementIndex = OSD_PILOT_NAME; } } @@ -3835,6 +3873,8 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) osdLayoutsConfig->item_pos[0][OSD_GVAR_2] = OSD_POS(1, 3); osdLayoutsConfig->item_pos[0][OSD_GVAR_3] = OSD_POS(1, 4); + osdLayoutsConfig->item_pos[0][OSD_MULTI_FUNCTION] = OSD_POS(1, 4); + osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_0] = OSD_POS(2, 7); osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_1] = OSD_POS(2, 8); osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_2] = OSD_POS(2, 9); @@ -4402,13 +4442,13 @@ static void osdShowArmed(void) } y += 4; #if defined (USE_SAFE_HOME) - if (safehome_distance) { // safehome found during arming + if (posControl.safehomeState.distance) { // safehome found during arming if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF) { strcpy(buf, "SAFEHOME FOUND; MODE OFF"); } else { char buf2[12]; // format the distance first - osdFormatDistanceStr(buf2, safehome_distance); - tfp_sprintf(buf, "%c - %s -> SAFEHOME %u", SYM_HOME, buf2, safehome_index); + osdFormatDistanceStr(buf2, posControl.safehomeState.distance); + tfp_sprintf(buf, "%c - %s -> SAFEHOME %u", SYM_HOME, buf2, posControl.safehomeState.index); } textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT; // write this message above the ARMED message to make it obvious @@ -4536,7 +4576,7 @@ static void osdRefresh(timeUs_t currentTimeUs) osdShowArmed(); uint32_t delay = ARMED_SCREEN_DISPLAY_TIME; #if defined(USE_SAFE_HOME) - if (safehome_distance) + if (posControl.safehomeState.distance) delay *= 3; #endif osdSetNextRefreshIn(delay); @@ -4945,12 +4985,6 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter } } -#ifdef USE_DEV_TOOLS - if (systemConfig()->groundTestMode) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_GRD_TEST_MODE); - } -#endif - if (messageCount > 0) { message = messages[OSD_ALTERNATING_CHOICES(systemMessageCycleTime(messageCount, messages), messageCount)]; if (message == failsafeInfoMessage) { @@ -4973,4 +5007,191 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter return elemAttr; } +void osdResetWarningFlags(void) +{ + osdWarningsFlags = 0; +} + +static bool osdCheckWarning(bool condition, uint8_t warningFlag, uint8_t *warningsCount) +{ +#define WARNING_REDISPLAY_DURATION 5000; // milliseconds + + const timeMs_t currentTimeMs = millis(); + static timeMs_t warningDisplayStartTime = 0; + static timeMs_t redisplayStartTimeMs = 0; + static uint16_t osdWarningTimerDuration; + static uint8_t newWarningFlags; + + if (condition) { // condition required to trigger warning + if (!(osdWarningsFlags & warningFlag)) { + osdWarningsFlags |= warningFlag; + newWarningFlags |= warningFlag; + redisplayStartTimeMs = 0; + } +#ifdef USE_DEV_TOOLS + if (systemConfig()->groundTestMode) { + return true; + } +#endif + /* Warnings displayed in full for set time before shrinking down to alert symbol with warning count only. + * All current warnings then redisplayed for 5s on 30s rolling cycle. + * New warnings dislayed individually for 10s */ + if (currentTimeMs > redisplayStartTimeMs) { + warningDisplayStartTime = currentTimeMs; + osdWarningTimerDuration = newWarningFlags ? 10000 : WARNING_REDISPLAY_DURATION; + redisplayStartTimeMs = currentTimeMs + osdWarningTimerDuration + 30000; + } + + if (currentTimeMs - warningDisplayStartTime < osdWarningTimerDuration) { + return (newWarningFlags & warningFlag) || osdWarningTimerDuration == WARNING_REDISPLAY_DURATION; + } else { + newWarningFlags = 0; + } + *warningsCount += 1; + } else if (osdWarningsFlags & warningFlag) { + osdWarningsFlags &= ~warningFlag; + } + + return false; +} + +static textAttributes_t osdGetMultiFunctionMessage(char *buff) +{ + /* Message length limit 10 char max */ + + textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE; + static uint8_t warningsCount; + const char *message = NULL; + +#ifdef USE_MULTI_FUNCTIONS + /* --- FUNCTIONS --- */ + multi_function_e selectedFunction = multiFunctionSelection(); + + if (selectedFunction) { + multi_function_e activeFunction = selectedFunction; + + switch (selectedFunction) { + case MULTI_FUNC_NONE: + case MULTI_FUNC_1: + message = warningsCount ? "WARNINGS !" : "0 WARNINGS"; + break; + case MULTI_FUNC_2: + message = posControl.flags.manualEmergLandActive ? "ABORT LAND" : "EMERG LAND"; + break; + case MULTI_FUNC_3: +#if defined(USE_SAFE_HOME) + if (navConfig()->general.flags.safehome_usage_mode != SAFEHOME_USAGE_OFF) { + message = MULTI_FUNC_FLAG(MF_SUSPEND_SAFEHOMES) ? "USE SFHOME" : "SUS SFHOME"; + break; + } +#endif + activeFunction++; + FALLTHROUGH; + case MULTI_FUNC_4: + if (navConfig()->general.flags.rth_trackback_mode != RTH_TRACKBACK_OFF) { + message = MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK) ? "USE TKBACK" : "SUS TKBACK"; + break; + } + activeFunction++; + FALLTHROUGH; + case MULTI_FUNC_5: +#ifdef USE_DSHOT + if (STATE(MULTIROTOR)) { + message = MULTI_FUNC_FLAG(MF_TURTLE_MODE) ? "END TURTLE" : "USE TURTLE"; + break; + } +#endif + activeFunction++; + FALLTHROUGH; + case MULTI_FUNC_6: + message = ARMING_FLAG(ARMED) ? "NOW ARMED " : "EMERG ARM "; + break; + case MULTI_FUNC_END: + break; + } + + if (activeFunction != selectedFunction) { + setMultifunctionSelection(activeFunction); + } + + strcpy(buff, message); + + if (isNextMultifunctionItemAvailable()) { + // provides feedback indicating when a new selection command has been received by flight controller + buff[9] = '>'; + } + + return elemAttr; + } +#endif // MULTIFUNCTION - functions only, warnings always defined + + /* --- WARNINGS --- */ + const char *messages[7]; + uint8_t messageCount = 0; + bool warningCondition = false; + warningsCount = 0; + uint8_t warningFlagID = 1; + + // Low Battery + const batteryState_e batteryState = getBatteryState(); + warningCondition = batteryState == BATTERY_CRITICAL || batteryState == BATTERY_WARNING; + if (osdCheckWarning(warningCondition, warningFlagID, &warningsCount)) { + messages[messageCount++] = batteryState == BATTERY_CRITICAL ? "BATT EMPTY" : "BATT LOW !"; + } + +#if defined(USE_GPS) + // GPS Fix and Failure + if (feature(FEATURE_GPS)) { + if (osdCheckWarning(!STATE(GPS_FIX), warningFlagID <<= 1, &warningsCount)) { + bool gpsFailed = getHwGPSStatus() == HW_SENSOR_UNAVAILABLE; + messages[messageCount++] = gpsFailed ? "GPS FAILED" : "NO GPS FIX"; + } + } + + // RTH sanity (warning if RTH heads 200m further away from home than closest point) + warningCondition = NAV_Status.state == MW_NAV_STATE_RTH_ENROUTE && !posControl.flags.rthTrackbackActive && + (posControl.homeDistance - posControl.rthSanityChecker.minimalDistanceToHome) > 20000; + if (osdCheckWarning(warningCondition, warningFlagID <<= 1, &warningsCount)) { + messages[messageCount++] = "RTH SANITY"; + } + + // Altitude sanity (warning if significant mismatch between estimated and GPS altitude) + if (osdCheckWarning(posControl.flags.gpsCfEstimatedAltitudeMismatch, warningFlagID <<= 1, &warningsCount)) { + messages[messageCount++] = "ALT SANITY"; + } +#endif + +#if defined(USE_MAG) + // Magnetometer failure + if (requestedSensors[SENSOR_INDEX_MAG] != MAG_NONE) { + hardwareSensorStatus_e magStatus = getHwCompassStatus(); + if (osdCheckWarning(magStatus == HW_SENSOR_UNAVAILABLE || magStatus == HW_SENSOR_UNHEALTHY, warningFlagID <<= 1, &warningsCount)) { + messages[messageCount++] = "MAG FAILED"; + } + } +#endif + // Vibration levels TODO - needs better vibration measurement to be useful + // const float vibrationLevel = accGetVibrationLevel(); + // warningCondition = vibrationLevel > 1.5f; + // if (osdCheckWarning(warningCondition, warningFlagID <<= 1, &warningsCount)) { + // messages[messageCount++] = vibrationLevel > 2.5f ? "BAD VIBRTN" : "VIBRATION!"; + // } + +#ifdef USE_DEV_TOOLS + if (osdCheckWarning(systemConfig()->groundTestMode, warningFlagID <<= 1, &warningsCount)) { + messages[messageCount++] = "GRD TEST !"; + } +#endif + + if (messageCount) { + message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)]; // display each warning on 1s cycle + strcpy(buff, message); + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } else if (warningsCount) { + buff[0] = SYM_ALERT; + tfp_sprintf(buff + 1, "%u ", warningsCount); + } + + return elemAttr; +} #endif // OSD diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 2dad5ceb819..04794c6cec2 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -273,6 +273,7 @@ typedef enum { OSD_CROSS_TRACK_ERROR, OSD_PILOT_NAME, OSD_PAN_SERVO_CENTRED, + OSD_MULTI_FUNCTION, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; @@ -488,6 +489,8 @@ void osdFormatVelocityStr(char* buff, int32_t vel, bool _3D, bool _max); // Returns a heading angle in degrees normalized to [0, 360). int osdGetHeadingAngle(int angle); +void osdResetWarningFlags(void); + int16_t osdGetPanServoOffset(void); /** diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 1f6cc8f0c37..14da4282f8c 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -36,6 +36,7 @@ #include "fc/fc_core.h" #include "fc/config.h" +#include "fc/multifunction.h" #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" @@ -81,17 +82,10 @@ gpsLocation_t GPS_home; uint32_t GPS_distanceToHome; // distance to home point in meters int16_t GPS_directionToHome; // direction to home point in degrees -fpVector3_t original_rth_home; // the original rth home - save it, since it could be replaced by safehome or HOME_RESET - radar_pois_t radar_pois[RADAR_MAX_POIS]; -#if defined(USE_SAFE_HOME) -int8_t safehome_index = -1; // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise -uint32_t safehome_distance = 0; // distance to the nearest safehome -fpVector3_t nearestSafeHome; // The nearestSafeHome found during arming -bool safehome_applied = false; // whether the safehome has been applied to home. +#if defined(USE_SAFE_HOME) PG_REGISTER_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig, PG_SAFE_HOME_CONFIG , 0); - #endif // waypoint 254, 255 are special waypoints @@ -107,7 +101,6 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { .flags = { - .use_thr_mid_for_althold = SETTING_NAV_USE_MIDTHR_FOR_ALTHOLD_DEFAULT, .extra_arming_safety = SETTING_NAV_EXTRA_ARMING_SAFETY_DEFAULT, .user_control_mode = SETTING_NAV_USER_CONTROL_MODE_DEFAULT, .rth_alt_control_mode = SETTING_NAV_RTH_ALT_MODE_DEFAULT, @@ -177,9 +170,10 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .braking_bank_angle = SETTING_NAV_MC_BRAKING_BANK_ANGLE_DEFAULT, // Max braking angle #endif - .posDecelerationTime = SETTING_NAV_MC_POS_DECELERATION_TIME_DEFAULT, // posDecelerationTime * 100 - .posResponseExpo = SETTING_NAV_MC_POS_EXPO_DEFAULT, // posResponseExpo * 100 + .posDecelerationTime = SETTING_NAV_MC_POS_DECELERATION_TIME_DEFAULT, // posDecelerationTime * 100 + .posResponseExpo = SETTING_NAV_MC_POS_EXPO_DEFAULT, // posResponseExpo * 100 .slowDownForTurning = SETTING_NAV_MC_WP_SLOWDOWN_DEFAULT, + .althold_throttle_type = SETTING_NAV_MC_ALTHOLD_THROTTLE_DEFAULT, // STICK }, // Fixed wing @@ -235,10 +229,10 @@ EXTENDED_FASTRAM multicopterPosXyCoefficients_t multicopterPosXyCoefficients; int16_t navCurrentState; int16_t navActualVelocity[3]; int16_t navDesiredVelocity[3]; -int16_t navActualHeading; -int16_t navDesiredHeading; int32_t navTargetPosition[3]; int32_t navLatestActualPosition[3]; +int16_t navActualHeading; +uint16_t navDesiredHeading; int16_t navActualSurface; uint16_t navFlags; uint16_t navEPH; @@ -1298,17 +1292,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n } // Climb to safe altitude and turn to correct direction + // Until the initial climb phase is complete target slightly *above* the cruise altitude to ensure we actually reach + // it in a reasonable time. Immediately after we finish this phase - target the original altitude. if (STATE(FIXED_WING_LEGACY)) { - if (navConfig()->general.flags.rth_climb_first == RTH_CLIMB_ON_FW_SPIRAL) { - float altitudeChangeDirection = (tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM) > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1; - updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL); - } else { - tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM; - setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z); - } + tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM; + setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z); } else { - // Until the initial climb phase is complete target slightly *above* the cruise altitude to ensure we actually reach - // it in a reasonable time. Immediately after we finish this phase - target the original altitude. tmpHomePos->z += MR_RTH_CLIMB_OVERSHOOT_CM; if (navConfig()->general.flags.rth_tail_first) { @@ -1333,8 +1322,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_TRACKBACK(navigatio if (posControl.flags.estPosStatus >= EST_USABLE) { const int32_t distFromStartTrackback = calculateDistanceToDestination(&posControl.rthTBPointsList[posControl.rthTBLastSavedIndex]) / 100; +#ifdef USE_MULTI_FUNCTIONS + const bool overrideTrackback = rthAltControlStickOverrideCheck(ROLL) || MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK); +#else + const bool overrideTrackback = rthAltControlStickOverrideCheck(ROLL); +#endif const bool cancelTrackback = distFromStartTrackback > navConfig()->general.rth_trackback_distance || - (rthAltControlStickOverrideCheck(ROLL) && !posControl.flags.forcedRTHActivated); + (overrideTrackback && !posControl.flags.forcedRTHActivated); if (posControl.activeRthTBPointIndex < 0 || cancelTrackback) { posControl.rthTBWrapAroundCounter = posControl.activeRthTBPointIndex = -1; @@ -1420,7 +1414,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND // Wait until target heading is reached for MR (with 15 deg margin for error), or continue for Fixed Wing if ((ABS(wrap_18000(posControl.rthState.homePosition.heading - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY)) { resetLandingDetector(); // force reset landing detector just in case - updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET); + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; // success = land } else { fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL); @@ -1439,21 +1433,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(na } fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_HOVER); - - if (navConfig()->general.rth_home_altitude) { - float timeToReachHomeAltitude = fabsf(tmpHomePos->z - navGetCurrentActualPositionAndVelocity()->pos.z) / navConfig()->general.max_auto_climb_rate; - - if (timeToReachHomeAltitude < 1) { - // we almost reached the target home altitude so set the desired altitude to the desired home altitude - setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z); - } else { - float altitudeChangeDirection = tmpHomePos->z > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1; - updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL); - } - } - else { - setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z); - } + setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z); return NAV_FSM_EVENT_NONE; } @@ -1495,7 +1475,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF descentVelLimited = constrainf(descentVelScaled, navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd); } - updateClimbRateToAltitudeController(-descentVelLimited, ROC_TO_ALT_NORMAL); + updateClimbRateToAltitudeController(-descentVelLimited, 0, ROC_TO_ALT_CONSTANT); return NAV_FSM_EVENT_NONE; } @@ -1518,7 +1498,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigation UNUSED(previousState); if (STATE(ALTITUDE_CONTROL)) { - updateClimbRateToAltitudeController(-1.1f * navConfig()->general.land_minalt_vspd, ROC_TO_ALT_NORMAL); // FIXME + updateClimbRateToAltitudeController(-1.1f * navConfig()->general.land_minalt_vspd, 0, ROC_TO_ALT_CONSTANT); // FIXME } // Prevent I-terms growing when already landed @@ -1721,12 +1701,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi if (navConfig()->general.waypoint_enforce_altitude && !posControl.wpAltitudeReached) { // Adjust altitude to waypoint setting - if (STATE(AIRPLANE)) { - int8_t altitudeChangeDirection = posControl.activeWaypoint.pos.z > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1; - updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL); - } else { - setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_Z); - } + setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_Z); posControl.wpAltitudeReached = isWaypointAltitudeReached(); @@ -2087,7 +2062,7 @@ void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelVali /*----------------------------------------------------------- * Processes an update to Z-position and velocity *-----------------------------------------------------------*/ -void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus) +void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus, float gpsCfEstimatedAltitudeError) { posControl.actualState.abs.pos.z = newAltitude; posControl.actualState.abs.vel.z = newVelocity; @@ -2110,11 +2085,14 @@ void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, flo posControl.flags.estAltStatus = EST_TRUSTED; posControl.flags.verticalPositionDataNew = true; posControl.lastValidAltitudeTimeMs = millis(); + /* flag set if mismatch between relative GPS and estimated altitude exceeds 20m */ + posControl.flags.gpsCfEstimatedAltitudeMismatch = fabsf(gpsCfEstimatedAltitudeError) > 2000; } else { posControl.flags.estAltStatus = EST_NONE; posControl.flags.estAglStatus = EST_NONE; posControl.flags.verticalPositionDataNew = false; + posControl.flags.gpsCfEstimatedAltitudeMismatch = false; } if (ARMING_FLAG(ARMED)) { @@ -2479,12 +2457,13 @@ static navigationHomeFlags_t navigationActualStateHomeValidity(void) } #if defined(USE_SAFE_HOME) - void checkSafeHomeState(bool shouldBeEnabled) { - const bool safehomeNotApplicable = navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF || - posControl.flags.rthTrackbackActive || - (!safehome_applied && posControl.homeDistance < navConfig()->general.min_rth_distance); + bool safehomeNotApplicable = navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF || posControl.flags.rthTrackbackActive || + (!posControl.safehomeState.isApplied && posControl.homeDistance < navConfig()->general.min_rth_distance); +#ifdef USE_MULTI_FUNCTIONS + safehomeNotApplicable = safehomeNotApplicable || (MULTI_FUNC_FLAG(MF_SUSPEND_SAFEHOMES) && !posControl.flags.forcedRTHActivated); +#endif if (safehomeNotApplicable) { shouldBeEnabled = false; @@ -2494,19 +2473,19 @@ void checkSafeHomeState(bool shouldBeEnabled) shouldBeEnabled = posControl.flags.forcedRTHActivated; } // no safe homes found when arming or safehome feature in the correct state, then we don't need to do anything - if (safehome_distance == 0 || (safehome_applied == shouldBeEnabled)) { - return; - } + if (posControl.safehomeState.distance == 0 || posControl.safehomeState.isApplied == shouldBeEnabled) { + return; + } if (shouldBeEnabled) { - // set home to safehome - setHomePosition(&nearestSafeHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); - safehome_applied = true; - } else { - // set home to original arming point - setHomePosition(&original_rth_home, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); - safehome_applied = false; - } - // if we've changed the home position, update the distance and direction + // set home to safehome + setHomePosition(&posControl.safehomeState.nearestSafeHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); + posControl.safehomeState.isApplied = true; + } else { + // set home to original arming point + setHomePosition(&posControl.rthState.originalHomePosition, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); + posControl.safehomeState.isApplied = false; + } + // if we've changed the home position, update the distance and direction updateHomePosition(); } @@ -2516,7 +2495,7 @@ void checkSafeHomeState(bool shouldBeEnabled) **********************************************************/ bool findNearestSafeHome(void) { - safehome_index = -1; + posControl.safehomeState.index = -1; uint32_t nearest_safehome_distance = navConfig()->general.safehome_max_distance + 1; uint32_t distance_to_current; fpVector3_t currentSafeHome; @@ -2532,19 +2511,17 @@ bool findNearestSafeHome(void) distance_to_current = calculateDistanceToDestination(¤tSafeHome); if (distance_to_current < nearest_safehome_distance) { // this safehome is the nearest so far - keep track of it. - safehome_index = i; + posControl.safehomeState.index = i; nearest_safehome_distance = distance_to_current; - nearestSafeHome.x = currentSafeHome.x; - nearestSafeHome.y = currentSafeHome.y; - nearestSafeHome.z = currentSafeHome.z; + posControl.safehomeState.nearestSafeHome = currentSafeHome; } } - if (safehome_index >= 0) { - safehome_distance = nearest_safehome_distance; + if (posControl.safehomeState.index >= 0) { + posControl.safehomeState.distance = nearest_safehome_distance; } else { - safehome_distance = 0; + posControl.safehomeState.distance = 0; } - return safehome_distance > 0; + return posControl.safehomeState.distance > 0; } #endif @@ -2574,9 +2551,7 @@ void updateHomePosition(void) #endif setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); // save the current location in case it is replaced by a safehome or HOME_RESET - original_rth_home.x = posControl.rthState.homePosition.pos.x; - original_rth_home.y = posControl.rthState.homePosition.pos.y; - original_rth_home.z = posControl.rthState.homePosition.pos.z; + posControl.rthState.originalHomePosition = posControl.rthState.homePosition.pos; } } } @@ -2805,7 +2780,7 @@ void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlag // Z-position if ((useMask & NAV_POS_UPDATE_Z) != 0) { - updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET); // Reset RoC/RoD -> altitude controller + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); // Reset RoC/RoD -> altitude controller posControl.desiredState.pos.z = pos->z; } @@ -2896,28 +2871,36 @@ bool isFlightDetected(void) /*----------------------------------------------------------- * Z-position controller *-----------------------------------------------------------*/ -void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAltitudeControllerMode_e mode) +void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAltitude, climbRateToAltitudeControllerMode_e mode) { +#define MIN_TARGET_CLIMB_RATE 100.0f // cm/s + static timeUs_t lastUpdateTimeUs; timeUs_t currentTimeUs = micros(); // Terrain following uses different altitude measurement const float altitudeToUse = navGetCurrentActualPositionAndVelocity()->pos.z; - if (mode == ROC_TO_ALT_RESET) { - lastUpdateTimeUs = currentTimeUs; - posControl.desiredState.pos.z = altitudeToUse; - } - else { // ROC_TO_ALT_NORMAL + if (mode != ROC_TO_ALT_RESET && desiredClimbRate) { + /* ROC_TO_ALT_CONSTANT - constant climb rate + * ROC_TO_ALT_TARGET - constant climb rate until close to target altitude reducing to min rate when altitude reached + * Rate reduction starts at distance from target altitude of 5 x climb rate for FW, 1 x climb rate for MC */ + + if (mode == ROC_TO_ALT_TARGET && fabsf(desiredClimbRate) > MIN_TARGET_CLIMB_RATE) { + const int8_t direction = desiredClimbRate > 0 ? 1 : -1; + const float absClimbRate = fabsf(desiredClimbRate); + const uint16_t maxRateCutoffAlt = STATE(AIRPLANE) ? absClimbRate * 5 : absClimbRate; + const float verticalVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z - targetAltitude, + 0.0f, -maxRateCutoffAlt * direction, MIN_TARGET_CLIMB_RATE, absClimbRate); + + desiredClimbRate = direction * constrainf(verticalVelScaled, MIN_TARGET_CLIMB_RATE, absClimbRate); + } /* * If max altitude is set, reset climb rate if altitude is reached and climb rate is > 0 * In other words, when altitude is reached, allow it only to shrink */ - if (navConfig()->general.max_altitude > 0 && - altitudeToUse >= navConfig()->general.max_altitude && - desiredClimbRate > 0 - ) { + if (navConfig()->general.max_altitude > 0 && altitudeToUse >= navConfig()->general.max_altitude && desiredClimbRate > 0) { desiredClimbRate = 0; } @@ -2943,9 +2926,11 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti // Multicopter climb-rate control is closed-loop, it's possible to directly calculate desired altitude setpoint to yield the required RoC/RoD posControl.desiredState.pos.z = altitudeToUse + (desiredClimbRate / posControl.pids.pos[Z].param.kP); } - - lastUpdateTimeUs = currentTimeUs; + } else { // ROC_TO_ALT_RESET or zero desired climbrate + posControl.desiredState.pos.z = altitudeToUse; } + + lastUpdateTimeUs = currentTimeUs; } static void resetAltitudeController(bool useTerrainFollowing) @@ -3588,6 +3573,8 @@ void applyWaypointNavigationAndAltitudeHold(void) navTargetPosition[X] = lrintf(posControl.desiredState.pos.x); navTargetPosition[Y] = lrintf(posControl.desiredState.pos.y); navTargetPosition[Z] = lrintf(posControl.desiredState.pos.z); + + navDesiredHeading = wrap_36000(posControl.desiredState.yaw); } /*----------------------------------------------------------- @@ -3624,7 +3611,7 @@ static bool isWaypointMissionValid(void) return posControl.waypointListValid && (posControl.waypointCount > 0); } -static void checkManualEmergencyLandingControl(void) +void checkManualEmergencyLandingControl(bool forcedActivation) { static timeMs_t timeout = 0; static int8_t counter = 0; @@ -3649,7 +3636,7 @@ static void checkManualEmergencyLandingControl(void) } // Emergency landing toggled ON or OFF after 5 cycles of Poshold mode @ 1Hz minimum rate - if (counter >= 5) { + if (counter >= 5 || forcedActivation) { counter = 0; posControl.flags.manualEmergLandActive = !posControl.flags.manualEmergLandActive; @@ -3686,7 +3673,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) /* Emergency landing controlled manually by rapid switching of Poshold mode. * Landing toggled ON or OFF for each Poshold activation sequence */ - checkManualEmergencyLandingControl(); + checkManualEmergencyLandingControl(false); /* Emergency landing triggered by failsafe Landing or manually initiated */ if (posControl.flags.forcedEmergLandingActivated || posControl.flags.manualEmergLandActive) { @@ -3858,9 +3845,8 @@ bool navigationRequiresTurnAssistance(void) // For airplanes turn assistant is always required when controlling position return (currentState & (NAV_CTL_POS | NAV_CTL_ALT)); } - else { - return false; - } + + return false; } /** @@ -4208,6 +4194,7 @@ void navigationInit(void) posControl.wpPlannerActiveWPIndex = 0; posControl.flags.wpMissionPlannerActive = false; posControl.startWpIndex = 0; + posControl.safehomeState.isApplied = false; #ifdef USE_MULTI_MISSION posControl.multiMissionCount = 0; #endif diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 5c7dca762ba..3a9223ff639 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -35,7 +35,6 @@ extern gpsLocation_t GPS_home; extern uint32_t GPS_distanceToHome; // distance to home point in meters extern int16_t GPS_directionToHome; // direction to home point in degrees -extern fpVector3_t original_rth_home; // the original rth home - save it, since it could be replaced by safehome or HOME_RESET extern bool autoThrottleManuallyIncreased; @@ -59,13 +58,8 @@ typedef enum { PG_DECLARE_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig); -extern int8_t safehome_index; // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise -extern uint32_t safehome_distance; // distance to the nearest safehome -extern bool safehome_applied; // whether the safehome has been applied to home. - -void resetSafeHomes(void); // remove all safehomes -bool findNearestSafeHome(void); // Find nearest safehome - +void resetSafeHomes(void); // remove all safehomes +bool findNearestSafeHome(void); // Find nearest safehome #endif // defined(USE_SAFE_HOME) #ifndef NAV_MAX_WAYPOINTS @@ -175,6 +169,12 @@ typedef enum { WP_TURN_SMOOTHING_CUT, } wpFwTurnSmoothing_e; +typedef enum { + MC_ALT_HOLD_STICK, + MC_ALT_HOLD_MID, + MC_ALT_HOLD_HOVER, +} navMcAltHoldThrottle_e; + typedef struct positionEstimationConfig_s { uint8_t automatic_mag_declination; uint8_t reset_altitude_type; // from nav_reset_type_e @@ -217,7 +217,6 @@ typedef struct navConfig_s { struct { struct { - uint8_t use_thr_mid_for_althold; // Don't remember throttle when althold was initiated, assume that throttle is at Thr Mid = zero climb rate uint8_t extra_arming_safety; // from navExtraArmingSafety_e uint8_t user_control_mode; // NAV_GPS_ATTI or NAV_GPS_CRUISE uint8_t rth_alt_control_mode; // Controls the logic for choosing the RTH altitude @@ -287,7 +286,8 @@ typedef struct navConfig_s { uint8_t posDecelerationTime; // Brake time parameter uint8_t posResponseExpo; // Position controller expo (taret vel expo for MC) - bool slowDownForTurning; // Slow down during WP missions when changing heading on next waypoint + bool slowDownForTurning; // Slow down during WP missions when changing heading on next waypoint + uint8_t althold_throttle_type; // throttle zero datum type for alt hold } mc; struct { @@ -622,6 +622,7 @@ bool isAdjustingHeading(void); float getEstimatedAglPosition(void); bool isEstimatedAglTrusted(void); +void checkManualEmergencyLandingControl(bool forcedActivation); float updateBaroAltitudeRate(float newBaroAltRate, bool updateValue); /* Returns the heading recorded when home position was acquired. @@ -638,6 +639,7 @@ extern int16_t navActualVelocity[3]; extern int16_t navDesiredVelocity[3]; extern int32_t navTargetPosition[3]; extern int32_t navLatestActualPosition[3]; +extern uint16_t navDesiredHeading; extern int16_t navActualSurface; extern uint16_t navFlags; extern uint16_t navEPH; diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 0b4d8934d0a..9bc0fa25dc2 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -116,13 +116,13 @@ bool adjustFixedWingAltitudeFromRCInput(void) if (rcAdjustment) { // set velocity proportional to stick movement float rcClimbRate = -rcAdjustment * navConfig()->general.max_manual_climb_rate / (500.0f - rcControlsConfig()->alt_hold_deadband); - updateClimbRateToAltitudeController(rcClimbRate, ROC_TO_ALT_NORMAL); + updateClimbRateToAltitudeController(rcClimbRate, 0, ROC_TO_ALT_CONSTANT); return true; } else { // Adjusting finished - reset desired position to stay exactly where pilot released the stick if (posControl.flags.isAdjustingAltitude) { - updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET); + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); } return false; } @@ -156,9 +156,14 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros) const float pitchGainInv = 1.0f / 1.0f; // Here we use negative values for dive for better clarity - const float maxClimbDeciDeg = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle); + float maxClimbDeciDeg = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle); const float minDiveDeciDeg = -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle); + // Reduce max allowed climb pitch if performing loiter (stall prevention) + if (needToCalculateCircularLoiter) { + maxClimbDeciDeg = maxClimbDeciDeg * 0.67f; + } + // PID controller to translate energy balance error [J] into pitch angle [decideg] float targetPitchAngle = navPidApply3(&posControl.pids.fw_alt, demSEB, estSEB, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, 0, pitchGainInv, 1.0f); @@ -759,7 +764,8 @@ void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs) rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle; if (posControl.flags.estAltStatus >= EST_USABLE) { - updateClimbRateToAltitudeController(-1.0f * navConfig()->general.emerg_descent_rate, ROC_TO_ALT_NORMAL); + // target min descent rate 10m above takeoff altitude + updateClimbRateToAltitudeController(-navConfig()->general.emerg_descent_rate, 1000.0f, ROC_TO_ALT_TARGET); applyFixedWingAltitudeAndThrottleController(currentTimeUs); int16_t pitchCorrection = constrain(posControl.rcAdjustment[PITCH], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle)); diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 6c0f2fa8f60..93b6333af8a 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -113,16 +113,15 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros) static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros) { // Calculate min and max throttle boundaries (to compensate for integral windup) - const int16_t thrAdjustmentMin = (int16_t)getThrottleIdleValue() - (int16_t)currentBatteryProfile->nav.mc.hover_throttle; - const int16_t thrAdjustmentMax = (int16_t)motorConfig()->maxthrottle - (int16_t)currentBatteryProfile->nav.mc.hover_throttle; + const int16_t thrCorrectionMin = getThrottleIdleValue() - currentBatteryProfile->nav.mc.hover_throttle; + const int16_t thrCorrectionMax = motorConfig()->maxthrottle - currentBatteryProfile->nav.mc.hover_throttle; - float velocity_controller = navPidApply2(&posControl.pids.vel[Z], posControl.desiredState.vel.z, navGetCurrentActualPositionAndVelocity()->vel.z, US2S(deltaMicros), thrAdjustmentMin, thrAdjustmentMax, 0); + float velocity_controller = navPidApply2(&posControl.pids.vel[Z], posControl.desiredState.vel.z, navGetCurrentActualPositionAndVelocity()->vel.z, US2S(deltaMicros), thrCorrectionMin, thrCorrectionMax, 0); - posControl.rcAdjustment[THROTTLE] = pt1FilterApply4(&altholdThrottleFilterState, velocity_controller, NAV_THROTTLE_CUTOFF_FREQENCY_HZ, US2S(deltaMicros)); + int16_t rcThrottleCorrection = pt1FilterApply4(&altholdThrottleFilterState, velocity_controller, NAV_THROTTLE_CUTOFF_FREQENCY_HZ, US2S(deltaMicros)); + rcThrottleCorrection = constrain(rcThrottleCorrection, thrCorrectionMin, thrCorrectionMax); - posControl.rcAdjustment[THROTTLE] = constrain(posControl.rcAdjustment[THROTTLE], thrAdjustmentMin, thrAdjustmentMax); - - posControl.rcAdjustment[THROTTLE] = constrain((int16_t)currentBatteryProfile->nav.mc.hover_throttle + posControl.rcAdjustment[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle); + posControl.rcAdjustment[THROTTLE] = constrain(currentBatteryProfile->nav.mc.hover_throttle + rcThrottleCorrection, getThrottleIdleValue(), motorConfig()->maxthrottle); } bool adjustMulticopterAltitudeFromRCInput(void) @@ -133,11 +132,11 @@ bool adjustMulticopterAltitudeFromRCInput(void) // In terrain follow mode we apply different logic for terrain control if (posControl.flags.estAglStatus == EST_TRUSTED && altTarget > 10.0f) { // We have solid terrain sensor signal - directly map throttle to altitude - updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET); + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); posControl.desiredState.pos.z = altTarget; } else { - updateClimbRateToAltitudeController(-50.0f, ROC_TO_ALT_NORMAL); + updateClimbRateToAltitudeController(-50.0f, 0, ROC_TO_ALT_CONSTANT); } // In surface tracking we always indicate that we're adjusting altitude @@ -159,14 +158,14 @@ bool adjustMulticopterAltitudeFromRCInput(void) rcClimbRate = rcThrottleAdjustment * navConfig()->general.max_manual_climb_rate / (float)(altHoldThrottleRCZero - getThrottleIdleValue() - rcControlsConfig()->alt_hold_deadband); } - updateClimbRateToAltitudeController(rcClimbRate, ROC_TO_ALT_NORMAL); + updateClimbRateToAltitudeController(rcClimbRate, 0, ROC_TO_ALT_CONSTANT); return true; } else { // Adjusting finished - reset desired position to stay exactly where pilot released the stick if (posControl.flags.isAdjustingAltitude) { - updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET); + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); } return false; @@ -177,19 +176,16 @@ bool adjustMulticopterAltitudeFromRCInput(void) void setupMulticopterAltitudeController(void) { const bool throttleIsLow = throttleStickIsLow(); + const uint8_t throttleType = navConfig()->mc.althold_throttle_type; - if (navConfig()->general.flags.use_thr_mid_for_althold) { + if (throttleType == MC_ALT_HOLD_STICK && !throttleIsLow) { + // Only use current throttle if not LOW - use Thr Mid otherwise + altHoldThrottleRCZero = rcCommand[THROTTLE]; + } else if (throttleType == MC_ALT_HOLD_HOVER) { + altHoldThrottleRCZero = currentBatteryProfile->nav.mc.hover_throttle; + } else { altHoldThrottleRCZero = rcLookupThrottleMid(); } - else { - // If throttle is LOW - use Thr Mid anyway - if (throttleIsLow) { - altHoldThrottleRCZero = rcLookupThrottleMid(); - } - else { - altHoldThrottleRCZero = rcCommand[THROTTLE]; - } - } // Make sure we are able to satisfy the deadband altHoldThrottleRCZero = constrain(altHoldThrottleRCZero, @@ -213,7 +209,7 @@ void resetMulticopterAltitudeController(void) navPidReset(&posControl.pids.vel[Z]); navPidReset(&posControl.pids.surface); - posControl.rcAdjustment[THROTTLE] = 0; + posControl.rcAdjustment[THROTTLE] = currentBatteryProfile->nav.mc.hover_throttle; posControl.desiredState.vel.z = posToUse->vel.z; // Gradually transition from current climb @@ -916,27 +912,27 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs) /* Attempt to stabilise */ rcCommand[YAW] = 0; + rcCommand[ROLL] = 0; + rcCommand[PITCH] = 0; + rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle; - if ((posControl.flags.estAltStatus < EST_USABLE)) { - /* Sensors has gone haywire, attempt to land regardless */ + /* Altitude sensors gone haywire, attempt to land regardless */ + if (posControl.flags.estAltStatus < EST_USABLE) { if (failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_DROP_IT) { rcCommand[THROTTLE] = getThrottleIdleValue(); } - else { - rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle; - } - return; } - // Normal sensor data + // Normal sensor data available, use controlled landing descent if (posControl.flags.verticalPositionDataNew) { const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; previousTimePositionUpdate = currentTimeUs; // Check if last correction was not too long ago if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { - updateClimbRateToAltitudeController(-1.0f * navConfig()->general.emerg_descent_rate, ROC_TO_ALT_NORMAL); + // target min descent rate 5m above takeoff altitude + updateClimbRateToAltitudeController(-navConfig()->general.emerg_descent_rate, 500.0f, ROC_TO_ALT_TARGET); updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate); updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate); } @@ -949,15 +945,12 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs) posControl.flags.verticalPositionDataConsumed = true; } - // Update throttle controller + // Update throttle rcCommand[THROTTLE] = posControl.rcAdjustment[THROTTLE]; // Hold position if possible if ((posControl.flags.estPosStatus >= EST_USABLE)) { applyMulticopterPositionController(currentTimeUs); - } else { - rcCommand[ROLL] = 0; - rcCommand[PITCH] = 0; } } diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 0f804dca41b..5c314bfddda 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -816,11 +816,12 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs) /* Publish altitude update and set altitude validity */ if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) { + const float gpsCfEstimatedAltitudeError = STATE(GPS_FIX) ? posEstimator.gps.pos.z - posEstimator.est.pos.z : 0; navigationEstimateStatus_e aglStatus = (posEstimator.est.aglQual == SURFACE_QUAL_LOW) ? EST_USABLE : EST_TRUSTED; - updateActualAltitudeAndClimbRate(true, posEstimator.est.pos.z, posEstimator.est.vel.z, posEstimator.est.aglAlt, posEstimator.est.aglVel, aglStatus); + updateActualAltitudeAndClimbRate(true, posEstimator.est.pos.z, posEstimator.est.vel.z, posEstimator.est.aglAlt, posEstimator.est.aglVel, aglStatus, gpsCfEstimatedAltitudeError); } else { - updateActualAltitudeAndClimbRate(false, posEstimator.est.pos.z, 0, posEstimator.est.aglAlt, 0, EST_NONE); + updateActualAltitudeAndClimbRate(false, posEstimator.est.pos.z, 0, posEstimator.est.aglAlt, 0, EST_NONE, 0); } //Update Blackbox states diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index bc19a5e2979..f5072c42195 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -61,7 +61,8 @@ typedef enum { typedef enum { ROC_TO_ALT_RESET, - ROC_TO_ALT_NORMAL + ROC_TO_ALT_CONSTANT, + ROC_TO_ALT_TARGET } climbRateToAltitudeControllerMode_e; typedef enum { @@ -90,6 +91,7 @@ typedef struct navigationFlags_s { navigationEstimateStatus_e estVelStatus; // Indicates that GPS is working (or not) navigationEstimateStatus_e estAglStatus; navigationEstimateStatus_e estHeadingStatus; // Indicate valid heading - wither mag or GPS at certain speed on airplane + bool gpsCfEstimatedAltitudeMismatch; // Indicates a mismatch between GPS altitude and estimated altitude bool isAdjustingPosition; bool isAdjustingAltitude; @@ -336,6 +338,7 @@ typedef struct { float rthFinalAltitude; // Altitude at end of RTH approach float rthInitialDistance; // Distance when starting flight home fpVector3_t homeTmpWaypoint; // Temporary storage for home target + fpVector3_t originalHomePosition; // the original rth home - save it, since it could be replaced by safehome or HOME_RESET } rthState_t; typedef enum { @@ -346,6 +349,13 @@ typedef enum { RTH_HOME_FINAL_LAND, // Home position and altitude } rthTargetMode_e; +typedef struct { + fpVector3_t nearestSafeHome; // The nearestSafeHome found during arming + uint32_t distance; // distance to the nearest safehome + int8_t index; // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise + bool isApplied; // whether the safehome has been applied to home +} safehomeState_t; + typedef struct { /* Flags and navigation system state */ navigationFSMState_t navState; @@ -368,14 +378,15 @@ typedef struct { /* INAV GPS origin (position where GPS fix was first acquired) */ gpsOrigin_t gpsOrigin; - /* Home parameters (NEU coordinated), geodetic position of home (LLH) is stores in GPS_home variable */ + /* Home/RTH parameters - NEU coordinates (geodetic position of home (LLH) is stored in GPS_home variable) */ rthSanityChecker_t rthSanityChecker; rthState_t rthState; - - /* Home parameters */ uint32_t homeDistance; // cm int32_t homeDirection; // deg*100 + /* Safehome parameters */ + safehomeState_t safehomeState; + /* Cruise */ navCruise_t cruise; @@ -449,7 +460,7 @@ void setHomePosition(const fpVector3_t * pos, int32_t heading, navSetWaypointFla void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask); void setDesiredSurfaceOffset(float surfaceOffset); void setDesiredPositionToFarAwayTarget(int32_t yaw, int32_t distance, navSetWaypointFlags_t useMask); // NOT USED -void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAltitudeControllerMode_e mode); +void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAltitude, climbRateToAltitudeControllerMode_e mode); bool isNavHoldPositionActive(void); bool isLastMissionWaypoint(void); @@ -458,7 +469,7 @@ bool isWaypointNavTrackingActive(void); void updateActualHeading(bool headingValid, int32_t newHeading, int32_t newGroundCourse); void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY); -void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus); +void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus, float gpsCfEstimatedAltitudeError); bool checkForPositionSensorTimeout(void); diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c old mode 100644 new mode 100755 index 8f102068390..8d2b29c075c --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -31,6 +31,7 @@ #include "drivers/pitotmeter/pitotmeter.h" #include "drivers/pitotmeter/pitotmeter_ms4525.h" +#include "drivers/pitotmeter/pitotmeter_dlvr_l10d.h" #include "drivers/pitotmeter/pitotmeter_adc.h" #include "drivers/pitotmeter/pitotmeter_msp.h" #include "drivers/pitotmeter/pitotmeter_virtual.h" @@ -44,11 +45,18 @@ #include "scheduler/protothreads.h" #include "sensors/pitotmeter.h" +#include "sensors/barometer.h" #include "sensors/sensors.h" + +//#include "build/debug.h" + + #ifdef USE_PITOT -pitot_t pitot; +extern baro_t baro; + +pitot_t pitot = {.lastMeasurementUs = 0, .lastSeenHealthyMs = 0}; PG_REGISTER_WITH_RESET_TEMPLATE(pitotmeterConfig_t, pitotmeterConfig, PG_PITOTMETER_CONFIG, 2); @@ -86,6 +94,15 @@ bool pitotDetect(pitotDev_t *dev, uint8_t pitotHardwareToUse) } FALLTHROUGH; + case PITOT_DLVR: + + // Skip autodetection for DLVR (it is indistinguishable from MS4525) and allow only manual config + if (pitotHardwareToUse != PITOT_AUTODETECT && dlvrDetect(dev)) { + pitotHardware = PITOT_DLVR; + break; + } + FALLTHROUGH; + case PITOT_ADC: #if defined(USE_ADC) && defined(USE_PITOT_ADC) if (adcPitotDetect(dev)) { @@ -169,7 +186,7 @@ bool pitotIsCalibrationComplete(void) void pitotStartCalibration(void) { - zeroCalibrationStartS(&pitot.zeroCalibration, CALIBRATING_PITOT_TIME_MS, SSL_AIR_PRESSURE * 0.00001f, false); + zeroCalibrationStartS(&pitot.zeroCalibration, CALIBRATING_PITOT_TIME_MS, SSL_AIR_PRESSURE * pitot.dev.calibThreshold, false); } static void performPitotCalibrationCycle(void) @@ -187,10 +204,12 @@ STATIC_PROTOTHREAD(pitotThread) ptBegin(pitotThread); static float pitotPressureTmp; + static float pitotTemperatureTmp; timeUs_t currentTimeUs; // Init filter pitot.lastMeasurementUs = micros(); + pt1FilterInit(&pitot.lpfState, pitotmeterConfig()->pitot_lpf_milli_hz / 1000.0f, 0.0f); while(1) { @@ -201,19 +220,23 @@ STATIC_PROTOTHREAD(pitotThread) } #endif - // Start measurement - if (pitot.dev.start(&pitot.dev)) { - pitot.lastSeenHealthyMs = millis(); + if ( pitot.lastSeenHealthyMs == 0 ) { + if (pitot.dev.start(&pitot.dev)) { + pitot.lastSeenHealthyMs = millis(); + } } - ptDelayUs(pitot.dev.delay); + if ( (millis() - pitot.lastSeenHealthyMs) >= US2MS(pitot.dev.delay)) { + if (pitot.dev.get(&pitot.dev)) // read current data + pitot.lastSeenHealthyMs = millis(); - // Read and calculate data - if (pitot.dev.get(&pitot.dev)) { - pitot.lastSeenHealthyMs = millis(); + if (pitot.dev.start(&pitot.dev)) // init for next read + pitot.lastSeenHealthyMs = millis(); } - pitot.dev.calculate(&pitot.dev, &pitotPressureTmp, NULL); + + pitot.dev.calculate(&pitot.dev, &pitotPressureTmp, &pitotTemperatureTmp); + #ifdef USE_SIMULATOR if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { pitotPressureTmp = sq(simulatorData.airSpeed) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE; @@ -226,14 +249,9 @@ STATIC_PROTOTHREAD(pitotThread) #endif ptYield(); - // Filter pressure - currentTimeUs = micros(); - pitot.pressure = pt1FilterApply3(&pitot.lpfState, pitotPressureTmp, US2S(currentTimeUs - pitot.lastMeasurementUs)); - pitot.lastMeasurementUs = currentTimeUs; - ptDelayUs(pitot.dev.delay); - // Calculate IAS if (pitotIsCalibrationComplete()) { + // NOTE :: // https://en.wikipedia.org/wiki/Indicated_airspeed // Indicated airspeed (IAS) is the airspeed read directly from the airspeed indicator on an aircraft, driven by the pitot-static system. // The IAS is an important value for the pilot because it is the indicated speeds which are specified in the aircraft flight manual for @@ -242,11 +260,21 @@ STATIC_PROTOTHREAD(pitotThread) // // Therefore we shouldn't care about CAS/TAS and only calculate IAS since it's more indicative to the pilot and more useful in calculations // It also allows us to use pitot_scale to calibrate the dynamic pressure sensor scale - pitot.airSpeed = pitotmeterConfig()->pitot_scale * fast_fsqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / SSL_AIR_DENSITY) * 100; + + // NOTE ::filter pressure - apply filter when NOT calibrating for zero !!! + currentTimeUs = micros(); + pitot.pressure = pt1FilterApply3(&pitot.lpfState, pitotPressureTmp, US2S(currentTimeUs - pitot.lastMeasurementUs)); + pitot.lastMeasurementUs = currentTimeUs; + + pitot.airSpeed = pitotmeterConfig()->pitot_scale * fast_fsqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / SSL_AIR_DENSITY) * 100; // cm/s + pitot.temperature = pitotTemperatureTmp; // Kelvin + } else { + pitot.pressure = pitotPressureTmp; performPitotCalibrationCycle(); pitot.airSpeed = 0.0f; } + #ifdef USE_SIMULATOR if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { pitot.airSpeed = simulatorData.airSpeed; diff --git a/src/main/sensors/pitotmeter.h b/src/main/sensors/pitotmeter.h old mode 100644 new mode 100755 index 4f9bccb046c..aed924f8e4c --- a/src/main/sensors/pitotmeter.h +++ b/src/main/sensors/pitotmeter.h @@ -31,6 +31,7 @@ typedef enum { PITOT_VIRTUAL = 4, PITOT_FAKE = 5, PITOT_MSP = 6, + PITOT_DLVR = 7, } pitotSensor_e; #define PITOT_MAX PITOT_FAKE @@ -55,6 +56,7 @@ typedef struct pito_s { float pressureZero; float pressure; + float temperature; } pitot_t; #ifdef USE_PITOT diff --git a/src/main/sensors/sensors.h b/src/main/sensors/sensors.h index b9d067a59b2..2b706f7dfc6 100644 --- a/src/main/sensors/sensors.h +++ b/src/main/sensors/sensors.h @@ -40,7 +40,7 @@ typedef union flightDynamicsTrims_u { } flightDynamicsTrims_t; #define CALIBRATING_BARO_TIME_MS 2000 -#define CALIBRATING_PITOT_TIME_MS 2000 +#define CALIBRATING_PITOT_TIME_MS 4000 #define CALIBRATING_GYRO_TIME_MS 2000 #define CALIBRATING_ACC_TIME_MS 500 #define CALIBRATING_GYRO_MORON_THRESHOLD 32 diff --git a/src/main/target/FOXEERH743/target.h b/src/main/target/FOXEERH743/target.h index 72fef026b15..ffb582ea9ad 100644 --- a/src/main/target/FOXEERH743/target.h +++ b/src/main/target/FOXEERH743/target.h @@ -135,7 +135,7 @@ // *************** ADC ***************************** #define USE_ADC -#define ADC_INSTANCE ADC1 +#define ADC_INSTANCE ADC3 #define ADC_CHANNEL_1_PIN PC3 #define ADC_CHANNEL_2_PIN PC5 diff --git a/src/main/target/GEPRCF405/CMakeLists.txt b/src/main/target/GEPRCF405/CMakeLists.txt new file mode 100644 index 00000000000..d500b48baa1 --- /dev/null +++ b/src/main/target/GEPRCF405/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(GEPRCF405) diff --git a/src/main/target/GEPRCF405/target.c b/src/main/target/GEPRCF405/target.c new file mode 100644 index 00000000000..ace9b307366 --- /dev/null +++ b/src/main/target/GEPRCF405/target.c @@ -0,0 +1,46 @@ +/* +* This file is part of INAV Project. +* +* 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 . +*/ + +#include + +#include "platform.h" +#include "drivers/io.h" + +#include "drivers/dma.h" +#include "drivers/timer.h" +#include "drivers/timer_def.h" + + + + + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, 0, 0), + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), + DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), + DEF_TIM(TIM1, CH3, PA10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + + DEF_TIM(TIM3, CH4, PB1, TIM_USE_LED, 0, 0), +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); + + diff --git a/src/main/target/GEPRCF405/target.h b/src/main/target/GEPRCF405/target.h new file mode 100644 index 00000000000..3a628ffe596 --- /dev/null +++ b/src/main/target/GEPRCF405/target.h @@ -0,0 +1,177 @@ +/* + * This file is part of INAV Project. + * + * 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 . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "GEPR" + +#define USBD_PRODUCT_STRING "GEPRCF405" + +#define LED0 PC14 +#define LED1 PC15 + + + +#define BEEPER PC13 +#define BEEPER_INVERTED + +// *************** Gyro & ACC ********************** + + + +#define USE_SPI +#define USE_SPI_DEVICE_3 + +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + + +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW180_DEG +#define MPU6000_CS_PIN PA15 +#define MPU6000_SPI_BUS BUS_SPI3 + +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW180_DEG +#define BMI270_CS_PIN PA15 +#define BMI270_SPI_BUS BUS_SPI3 + +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW180_DEG +#define ICM42605_CS_PIN PA15 +#define ICM42605_SPI_BUS BUS_SPI3 + +// *************** I2C/Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_DPS310 +#define USE_BARO_MS5611 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL + +#define TEMPERATURE_I2C_BUS BUS_I2C1 + +#define PITOT_I2C_BUS BUS_I2C1 + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 + +// *************** FLASH ************************** +#define M25P16_CS_PIN PB12 +#define M25P16_SPI_BUS BUS_SPI2 +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +// *************** OSD ***************************** +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI1 +#define MAX7456_CS_PIN PA4 + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_RX_PIN PB7 +#define UART1_TX_PIN PB6 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PC11 +#define UART3_TX_PIN PC10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART5 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN PC12 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define SERIAL_PORT_COUNT 7 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART2 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 +#define ADC_CHANNEL_1_PIN PC1 +#define ADC_CHANNEL_2_PIN PC2 +#define ADC_CHANNEL_3_PIN PC0 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define RSSI_ADC_CHANNEL ADC_CHN_2 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_3 + + +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX) + + + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PB1 + + + +#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 (BIT(2)) + + +#define MAX_PWM_OUTPUT_PORTS 6 + +#define USE_DSHOT +#define USE_ESC_SENSOR diff --git a/src/main/target/GEPRCF722/CMakeLists.txt b/src/main/target/GEPRCF722/CMakeLists.txt new file mode 100644 index 00000000000..8164cb86230 --- /dev/null +++ b/src/main/target/GEPRCF722/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f722xe(GEPRCF722) diff --git a/src/main/target/GEPRCF722/target.c b/src/main/target/GEPRCF722/target.c new file mode 100644 index 00000000000..8e206e7b199 --- /dev/null +++ b/src/main/target/GEPRCF722/target.c @@ -0,0 +1,44 @@ +/* +* This file is part of INAV Project. +* +* 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 . +*/ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/sensor.h" + + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), + + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), + + +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/GEPRCF722/target.h b/src/main/target/GEPRCF722/target.h new file mode 100644 index 00000000000..bc08477c451 --- /dev/null +++ b/src/main/target/GEPRCF722/target.h @@ -0,0 +1,173 @@ +/* + * This file is part of INAV Project. + * + * 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 . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "GEPR" + +#define USBD_PRODUCT_STRING "GEPRCF722" + +#define LED0 PA13 +#define LED1 PA14 + +#define BEEPER PC13 +#define BEEPER_INVERTED + +// *************** Gyro & ACC ********************** + +#define USE_SPI +#define USE_SPI_DEVICE_3 + +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + + +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW270_DEG + +#define MPU6000_CS_PIN PA15 +#define MPU6000_SPI_BUS BUS_SPI3 + +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW180_DEG +#define BMI270_CS_PIN PA15 +#define BMI270_SPI_BUS BUS_SPI3 + +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW180_DEG +#define ICM42605_CS_PIN PA15 +#define ICM42605_SPI_BUS BUS_SPI3 + +// *************** I2C/Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_DPS310 +#define USE_BARO_MS5611 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL + +#define TEMPERATURE_I2C_BUS BUS_I2C1 + +#define PITOT_I2C_BUS BUS_I2C1 + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 + +// *************** FLASH ************************** +#define M25P16_CS_PIN PB12 +#define M25P16_SPI_BUS BUS_SPI2 +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +// *************** OSD ***************************** +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI1 +#define MAX7456_CS_PIN PB2 +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define SERIAL_PORT_COUNT 6 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART2 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC2 +#define ADC_CHANNEL_3_PIN PC1 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define RSSI_ADC_CHANNEL ADC_CHN_2 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_3 + + +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX) + + +// ************PINIO to other +#define PINIO1_PIN PC8 //Enable vsw +#define PINIO2_PIN PC9 + + + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PA8 + + + +#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 (BIT(2)) + +#define MAX_PWM_OUTPUT_PORTS 6 + +#define USE_DSHOT +#define USE_ESC_SENSOR diff --git a/src/main/target/MAMBAF405US/target.h b/src/main/target/MAMBAF405US/target.h index d7ae8462fe3..b6b95cb12b6 100644 --- a/src/main/target/MAMBAF405US/target.h +++ b/src/main/target/MAMBAF405US/target.h @@ -40,6 +40,10 @@ #define MPU6000_SPI_BUS BUS_SPI1 #define MPU6000_CS_PIN PA4 +#define USE_IMU_MPU6500 +#define IMU_MPU6500_ALIGN CW180_DEG +#define MPU6500_SPI_BUS BUS_SPI1 +#define MPU6500_CS_PIN PA4 // *************** Baro ************************** #define USE_I2C diff --git a/src/main/target/NEUTRONRCF435MINI/target.c b/src/main/target/NEUTRONRCF435MINI/target.c index e964c19156e..995bf8f3d1a 100644 --- a/src/main/target/NEUTRONRCF435MINI/target.c +++ b/src/main/target/NEUTRONRCF435MINI/target.c @@ -29,7 +29,7 @@ timerHardware_t timerHardware[] = { DEF_TIM(TMR1, CH1, PA8, TIM_USE_ANY |TIM_USE_LED, 0,7), // PWM1 - LED MCO1 DMA1 CH2 DEF_TIM(TMR4, CH1, PB6, TIM_USE_MC_MOTOR|TIM_USE_FW_MOTOR, 0,0), // motor1 DMA2 CH7 - DEF_TIM(TMR1, CH3, PA10, TIM_USE_MC_MOTOR|TIM_USE_FW_MOTOR, 0,2), // motor2 DMA2 CH6 + DEF_TIM(TMR4, CH2, PB7, TIM_USE_MC_MOTOR|TIM_USE_FW_MOTOR, 0,2), // motor2 DMA2 CH6 DEF_TIM(TMR2, CH4, PA3, TIM_USE_MC_MOTOR|TIM_USE_FW_SERVO, 0,1), // motor3 DMA2 CH5 DEF_TIM(TMR3, CH4, PB1, TIM_USE_MC_MOTOR|TIM_USE_FW_SERVO, 0,3), // motor4 DMA2 CH4 diff --git a/src/main/target/NEUTRONRCF435MINI/target.h b/src/main/target/NEUTRONRCF435MINI/target.h index 42cddf5681e..27fc03ae5e1 100644 --- a/src/main/target/NEUTRONRCF435MINI/target.h +++ b/src/main/target/NEUTRONRCF435MINI/target.h @@ -137,7 +137,7 @@ #define USE_USB_DETECT #define USE_UART1 -#define UART1_RX_PIN PB7 +#define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 #define USE_UART2 diff --git a/src/main/target/RUSH_BLADE_F7/target.c b/src/main/target/RUSH_BLADE_F7/target.c index 6052a198232..565c8a23465 100644 --- a/src/main/target/RUSH_BLADE_F7/target.c +++ b/src/main/target/RUSH_BLADE_F7/target.c @@ -27,8 +27,8 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), // S5 - DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), // S6 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S5 + DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S6 DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), // S7 DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), // S8 diff --git a/src/main/target/SITL/sim/realFlight.c b/src/main/target/SITL/sim/realFlight.c index eb164c42939..23e77bcde04 100644 --- a/src/main/target/SITL/sim/realFlight.c +++ b/src/main/target/SITL/sim/realFlight.c @@ -403,6 +403,9 @@ static void exchangeData(void) constrainToInt16(north.y * 16000.0f), constrainToInt16(north.z * 16000.0f) ); + + free(rfValues.m_currentAircraftStatus); + free(response); } static void* soapWorker(void* arg) @@ -412,9 +415,9 @@ static void* soapWorker(void* arg) { if (!isInitalised) { startRequest("RestoreOriginalControllerDevice", "12"); - endRequest(); + free(endRequest()); startRequest("InjectUAVControllerInterface", "12"); - endRequest(); + free(endRequest()); exchangeData(); ENABLE_ARMING_FLAG(SIMULATOR_MODE_SITL); diff --git a/src/main/target/SITL/sim/simple_soap_client.c b/src/main/target/SITL/sim/simple_soap_client.c index f715e10ace0..e6400ae3b14 100644 --- a/src/main/target/SITL/sim/simple_soap_client.c +++ b/src/main/target/SITL/sim/simple_soap_client.c @@ -93,6 +93,9 @@ void soapClientSendRequestVa(soap_client_t *client, const char* action, const ch } send(client->sockedFd, request, strlen(request), 0); + + free(requestBody); + free(request); } void soapClientSendRequest(soap_client_t *client, const char* action, const char *fmt, ...) diff --git a/src/main/target/SKYSTARSH743HD/CMakeLists.txt b/src/main/target/SKYSTARSH743HD/CMakeLists.txt index 926391fdb1a..39b6db376f1 100644 --- a/src/main/target/SKYSTARSH743HD/CMakeLists.txt +++ b/src/main/target/SKYSTARSH743HD/CMakeLists.txt @@ -1 +1 @@ -target_stm32h743xi(SKYSTARSH743HD) \ No newline at end of file +target_stm32h743xi(SKYSTARSH743HD) diff --git a/src/main/target/SKYSTARSH743HD/target.c b/src/main/target/SKYSTARSH743HD/target.c index dff8d0f967a..07a2b0ef701 100644 --- a/src/main/target/SKYSTARSH743HD/target.c +++ b/src/main/target/SKYSTARSH743HD/target.c @@ -36,15 +36,12 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 2), // S3 DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 3), // S4 - DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 4), // S5 - DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 5), // S6 - DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 6), // S7 - DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 7), // S8 - - DEF_TIM(TIM15, CH1, PE5, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S9 - DEF_TIM(TIM15, CH2, PE6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S10 DMA_NONE + DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 4), // S5 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 5), // S6 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 6), // S7 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 7), // S8 DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 9), // LED_2812 }; -const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); \ No newline at end of file +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/SPEEDYBEEF405MINI/CMakeLists.txt b/src/main/target/SPEEDYBEEF405MINI/CMakeLists.txt new file mode 100644 index 00000000000..e18c0cd5ada --- /dev/null +++ b/src/main/target/SPEEDYBEEF405MINI/CMakeLists.txt @@ -0,0 +1,2 @@ +target_stm32f405xg(SPEEDYBEEF405MINI) +target_stm32f405xg(SPEEDYBEEF405MINI_6OUTPUTS) diff --git a/src/main/target/SPEEDYBEEF405MINI/config.c b/src/main/target/SPEEDYBEEF405MINI/config.c new file mode 100644 index 00000000000..46aa7f00dcb --- /dev/null +++ b/src/main/target/SPEEDYBEEF405MINI/config.c @@ -0,0 +1,37 @@ +/* + * 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 . + */ + +#include + +#include "platform.h" + +#include "fc/fc_msp_box.h" + +#include "io/piniobox.h" +#include "io/serial.h" + +void targetConfiguration(void) +{ + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART2)].functionMask = FUNCTION_RX_SERIAL; + + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART5)].functionMask = FUNCTION_ESCSERIAL; + + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_MSP; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].msp_baudrateIndex = BAUD_115200; + + pinioBoxConfigMutable()->permanentId[0] = BOXARM; +} diff --git a/src/main/target/SPEEDYBEEF405MINI/target.c b/src/main/target/SPEEDYBEEF405MINI/target.c new file mode 100644 index 00000000000..1118940e6f6 --- /dev/null +++ b/src/main/target/SPEEDYBEEF405MINI/target.c @@ -0,0 +1,46 @@ +/* + * 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 . + */ + +#include + +#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(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1, 0), // S1 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1, 0), // S2 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S3 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S4 + +#ifdef SPEEDYBEEF405MINI_6OUTPUTS + DEF_TIM(TIM12, CH1, PB14, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 1, 0), // CAM_CTRL + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // LED +#else + DEF_TIM(TIM12, CH1, PB14, TIM_USE_ANY, 1, 0), // CAM_CTRL + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED +#endif + + DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), //TX2 softserial1_Tx +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/SPEEDYBEEF405MINI/target.h b/src/main/target/SPEEDYBEEF405MINI/target.h new file mode 100644 index 00000000000..dd5b9f4cfb3 --- /dev/null +++ b/src/main/target/SPEEDYBEEF405MINI/target.h @@ -0,0 +1,186 @@ +/* + * 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 . + */ + + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "SF4M" +#define USBD_PRODUCT_STRING "SPEEDYBEEF405MINI" + +#define LED0 PC13 + +#define BEEPER PC15 +#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 PA7 + +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PC2 +#define SPI2_MOSI_PIN PC3 + +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +/* + * I2C + */ +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +/* + * Serial + */ +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define USE_UART3 +#define UART3_TX_PIN PC10 +#define UART3_RX_PIN PC11 + +#define USE_UART4 //Internally routed to BLE +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define USE_UART5 +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 + +#define USE_UART6 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define USE_SOFTSERIAL1 +#define SOFTSERIAL_1_TX_PIN PA2 +#define SOFTSERIAL_1_RX_PIN PA2 + +#define SERIAL_PORT_COUNT 8 + +/* + * Gyro + */ +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW270_DEG +#define ICM42605_CS_PIN PA4 +#define ICM42605_SPI_BUS BUS_SPI1 + +/* + * Other + */ +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_DPS310 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_AK8975 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL + +#define TEMPERATURE_I2C_BUS BUS_I2C1 +#define PITOT_I2C_BUS BUS_I2C1 +#define RANGEFINDER_I2C_BUS BUS_I2C1 + +/* + * OSD + */ +#define USE_MAX7456 +#define MAX7456_CS_PIN PB12 +#define MAX7456_SPI_BUS BUS_SPI2 + +/* + * Blackbox + */ +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_SPI_BUS BUS_SPI3 +#define M25P16_CS_PIN PC14 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART2 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 + +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC1 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 + +// *************** PINIO *************************** +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PB11 // RF Switch +#define PINIO1_FLAGS PINIO_FLAGS_INVERTED + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) +#define CURRENT_METER_SCALE 250 +#define CURRENT_METER_OFFSET 0 + +#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 + +#ifdef SPEEDYBEEF405MINI_6OUTPUTS + +#define MAX_PWM_OUTPUT_PORTS 6 + +#else + +#define MAX_PWM_OUTPUT_PORTS 4 + +#endif + +#define USE_DSHOT +#define USE_SERIALSHOT +#define USE_ESC_SENSOR \ No newline at end of file diff --git a/src/main/target/SPEEDYBEEF405V3/target.h b/src/main/target/SPEEDYBEEF405V3/target.h index d7bb949b2e6..99711f82deb 100644 --- a/src/main/target/SPEEDYBEEF405V3/target.h +++ b/src/main/target/SPEEDYBEEF405V3/target.h @@ -152,7 +152,8 @@ #define RANGEFINDER_I2C_BUS BUS_I2C2 #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX) -// #define USE_DSHOT +#define USE_DSHOT +#define USE_DSHOT_DMAR // #define USE_ESC_SENSOR #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/SPEEDYBEEF7MINI/CMakeLists.txt b/src/main/target/SPEEDYBEEF7MINI/CMakeLists.txt index c7a92612742..1ae9febd177 100644 --- a/src/main/target/SPEEDYBEEF7MINI/CMakeLists.txt +++ b/src/main/target/SPEEDYBEEF7MINI/CMakeLists.txt @@ -1 +1,2 @@ target_stm32f722xe(SPEEDYBEEF7MINI) +target_stm32f722xe(SPEEDYBEEF7MINIV2) diff --git a/src/main/target/SPEEDYBEEF7MINI/target.c b/src/main/target/SPEEDYBEEF7MINI/target.c index 468ac3c2f62..7cc2506b66c 100644 --- a/src/main/target/SPEEDYBEEF7MINI/target.c +++ b/src/main/target/SPEEDYBEEF7MINI/target.c @@ -26,7 +26,11 @@ #include "drivers/pinio.h" #include "drivers/sensor.h" +#ifdef SPEEDYBEEF7MINIV2 +BUSDEV_REGISTER_SPI_TAG(busdev_bmi270, DEVHW_BMI270, BMI270_SPI_BUS, BMI270_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_BMI270_ALIGN); +#else BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6000_ALIGN); +#endif timerHardware_t timerHardware[] = { DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S1 UP1-2 D(1, 4, 5) diff --git a/src/main/target/SPEEDYBEEF7MINI/target.h b/src/main/target/SPEEDYBEEF7MINI/target.h index f6db8e86fa4..0dd3a6e7a83 100644 --- a/src/main/target/SPEEDYBEEF7MINI/target.h +++ b/src/main/target/SPEEDYBEEF7MINI/target.h @@ -19,7 +19,12 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "SBMN" + +#ifdef SPEEDYBEEF7MINIV2 +#define USBD_PRODUCT_STRING "SPEEDYBEEF7MINIV2" +#else #define USBD_PRODUCT_STRING "SPEEDYBEEF7MINI" +#endif #define LED0 PA14 //Blue SWCLK @@ -35,11 +40,22 @@ #define SPI1_MISO_PIN PA6 #define SPI1_MOSI_PIN PA7 +#ifdef SPEEDYBEEF7MINIV2 + +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW180_DEG +#define BMI270_CS_PIN PB2 +#define BMI270_SPI_BUS BUS_SPI1 + +#else + #define USE_IMU_MPU6000 #define IMU_MPU6000_ALIGN CW0_DEG #define MPU6000_CS_PIN PB2 #define MPU6000_SPI_BUS BUS_SPI1 +#endif + // *************** I2C /Baro/Mag ********************* #define USE_I2C #define USE_I2C_DEVICE_1 diff --git a/src/main/target/SPEEDYBEEF7V3/target.h b/src/main/target/SPEEDYBEEF7V3/target.h index f8705f97da7..7c1d565c377 100644 --- a/src/main/target/SPEEDYBEEF7V3/target.h +++ b/src/main/target/SPEEDYBEEF7V3/target.h @@ -149,6 +149,7 @@ // ********** Optiical Flow adn Lidar ************** #define USE_RANGEFINDER #define USE_RANGEFINDER_MSP +#define RANGEFINDER_I2C_BUS BUS_I2C1 #define USE_OPFLOW #define USE_OPFLOW_MSP diff --git a/src/main/target/common.h b/src/main/target/common.h old mode 100644 new mode 100755 index d18bdfb1a2e..815b3ae41c8 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -92,6 +92,7 @@ #define USE_PITOT #define USE_PITOT_MS4525 #define USE_PITOT_MSP +#define USE_PITOT_DLVR #define USE_1WIRE #define USE_1WIRE_DS2482 @@ -161,6 +162,7 @@ #define NAV_MAX_WAYPOINTS 120 #define USE_RCDEVICE #define USE_MULTI_MISSION +#define USE_MULTI_FUNCTIONS // defines functions only, warnings always defined //Enable VTX control #define USE_VTX_CONTROL diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c index 623fdcbbbc5..76ffbf19420 100755 --- a/src/main/target/common_hardware.c +++ b/src/main/target/common_hardware.c @@ -357,10 +357,18 @@ #endif #if defined(USE_PITOT_MS4525) && defined(MS4525_I2C_BUS) - BUSDEV_REGISTER_I2C(busdev_ms5425, DEVHW_MS4525, MS4525_I2C_BUS, 0x28, NONE, DEVFLAGS_USE_RAW_REGISTERS, 0); // Requires 0xFF to passthrough + BUSDEV_REGISTER_I2C(busdev_ms4525, DEVHW_MS4525, MS4525_I2C_BUS, 0x28, NONE, DEVFLAGS_USE_RAW_REGISTERS, 0); // Requires 0xFF to passthrough #endif +#if defined(PITOT_I2C_BUS) && !defined(DLVR_I2C_BUS) + #define DLVR_I2C_BUS PITOT_I2C_BUS +#endif + +#if defined(USE_PITOT_DLVR) && defined(DLVR_I2C_BUS) + BUSDEV_REGISTER_I2C(busdev_dlvr, DEVHW_DLVR, DLVR_I2C_BUS, 0x28, NONE, DEVFLAGS_USE_RAW_REGISTERS, 0); // Requires 0xFF to passthrough +#endif + /** OTHER HARDWARE **/ #if defined(USE_MAX7456)