Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 08:15:26 +03:00
Code Issues Wiki Activity

Merge branch 'master' into abo_mc_course_hold_cruise

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2023-09-05 20:55:01 +02:00
commit c5c1603064
84 changed files with 2191 additions and 317 deletions
Download patch file Download diff file Expand all files Collapse all files

5
.dir-locals.el Normal file
View file

@ -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"))))

3
.gitignore vendored
View file

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

9
.vimrc Normal file
View file

@ -0,0 +1,9 @@
filetype on
filetype indent on
set expandtab
set bs=2
set sw=4
set ts=4

12
.vscode/settings.json vendored Normal file
View file

@ -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 }"
}

30
docs/Programming Framework.md
View file

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

30
docs/Settings.md
View file

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

15
docs/boards/SPEEDYBEEF405V3.md
View file

@ -1,3 +1,18 @@
# SpeedyBee F405 V3
> 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.

8
docs/development/serial_printf_debugging.md
View file

@ -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;
```

4
src/main/CMakeLists.txt Normal file → Executable file
View file

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

16
src/main/blackbox/blackbox.c
View file

@ -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;
@ -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();
@ -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;
}

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

@ -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),

5
src/main/common/calibration.c
View file

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

38
src/main/drivers/adc.c
View file

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

4
src/main/drivers/adc.h
View file

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

1
src/main/drivers/bus.h
View file

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

3
src/main/drivers/osd_symbols.h
View file

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

1
src/main/drivers/pitotmeter/pitotmeter.h
View file

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

1
src/main/drivers/pitotmeter/pitotmeter_adc.c
View file

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

165
src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c Executable file
View file

@ -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 <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <platform.h>
#include <build/debug.h>
#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;
}

20
src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.h Executable file
View file

@ -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 <http://www.gnu.org/licenses/>.
*/
#pragma once
bool dlvrDetect(pitotDev_t *pitot);

1
src/main/drivers/pitotmeter/pitotmeter_fake.c
View file

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

9
src/main/drivers/pitotmeter/pitotmeter_ms4525.c
View file

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

2
src/main/drivers/pitotmeter/pitotmeter_msp.c
View file

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

1
src/main/drivers/pitotmeter/pitotmeter_virtual.c
View file

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

10
src/main/drivers/pwm_mapping.c
View file

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

73
src/main/drivers/pwm_output.c
View file

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

12
src/main/drivers/timer.c
View file

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

23
src/main/drivers/timer.h
View file

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

5
src/main/drivers/timer_impl.h
View file

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

103
src/main/drivers/timer_impl_hal.c
View file

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

98
src/main/drivers/timer_impl_stdperiph.c
View file

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

53
src/main/fc/fc_core.c
View file

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

2
src/main/fc/fc_core.h
View file

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

4
src/main/fc/fc_msp.c
View file

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

8
src/main/fc/fc_msp_box.c
View file

@ -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]]) {

2
src/main/fc/fc_tasks.c
View file

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

132
src/main/fc/multifunction.c Normal file
View file

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

55
src/main/fc/multifunction.h Normal file
View file

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

2
src/main/fc/rc_controls.c
View file

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

1
src/main/fc/rc_modes.h
View file

@ -78,6 +78,7 @@ typedef enum {
BOXUSER4 = 49,
BOXCHANGEMISSION = 50,
BOXBEEPERMUTE = 51,
BOXMULTIFUNCTION = 52,
CHECKBOX_ITEM_COUNT
} boxId_e;

25
src/main/fc/settings.yaml Normal file → Executable file
View file

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

3
src/main/flight/dynamic_lpf.c
View file

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

6
src/main/flight/failsafe.c
View file

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

4
src/main/flight/pid.c
View file

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

15
src/main/io/gps.c
View file

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

2
src/main/io/gps.h
View file

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

26
src/main/io/gps_ublox.c
View file

@ -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,8 +382,8 @@ 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},
@ -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]);

253
src/main/io/osd.c
View file

@ -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,8 +2029,24 @@ static bool osdDrawSingleElement(uint8_t item)
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
}
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

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

@ -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);
/**

149
src/main/navigation/navigation.c
View file

@ -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.
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,
@ -180,6 +173,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.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
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);
}
} 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.
if (STATE(FIXED_WING_LEGACY)) {
tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM;
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
} else {
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);
}
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);
}
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,17 +2473,17 @@ 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)) {
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;
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(&original_rth_home, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
safehome_applied = false;
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(&currentSafeHome);
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;
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);
}
else { // ROC_TO_ALT_NORMAL
/*
* 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);
}
} 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;
}
}
/**
@ -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

16
src/main/navigation/navigation.h
View file

@ -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
#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
@ -288,6 +287,7 @@ 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
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;

14
src/main/navigation/navigation_fixedwing.c
View file

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

61
src/main/navigation/navigation_multicopter.c
View file

@ -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,18 +176,15 @@ 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) {
altHoldThrottleRCZero = rcLookupThrottleMid();
}
else {
// If throttle is LOW - use Thr Mid anyway
if (throttleIsLow) {
altHoldThrottleRCZero = rcLookupThrottleMid();
}
else {
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();
}
// Make sure we are able to satisfy the deadband
@ -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;
}
}

5
src/main/navigation/navigation_pos_estimator.c
View file

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

23
src/main/navigation/navigation_private.h
View file

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

56
src/main/sensors/pitotmeter.c Normal file → Executable file
View file

@ -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.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)) {
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;

2
src/main/sensors/pitotmeter.h Normal file → Executable file
View file

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

2
src/main/sensors/sensors.h
View file

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

2
src/main/target/FOXEERH743/target.h
View file

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

1
src/main/target/GEPRCF405/CMakeLists.txt Normal file
View file

@ -0,0 +1 @@
target_stm32f405xg(GEPRCF405)

46
src/main/target/GEPRCF405/target.c Normal file
View file

@ -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 <http://www.gnu.org/licenses/>.
*/
#include <stdint.h>
#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]);

177
src/main/target/GEPRCF405/target.h Normal file
View file

@ -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 <http://www.gnu.org/licenses/>.
*/
#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

1
src/main/target/GEPRCF722/CMakeLists.txt Normal file
View file

@ -0,0 +1 @@
target_stm32f722xe(GEPRCF722)

44
src/main/target/GEPRCF722/target.c Normal file
View file

@ -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 <http://www.gnu.org/licenses/>.
*/
#include <stdint.h>
#include "platform.h"
#include "drivers/bus.h"
#include "drivers/io.h"
#include "drivers/pwm_mapping.h"
#include "drivers/timer.h"
#include "drivers/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]);

173
src/main/target/GEPRCF722/target.h Normal file
View file

@ -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 <http://www.gnu.org/licenses/>.
*/
#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

4
src/main/target/MAMBAF405US/target.h
View file

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

2
src/main/target/NEUTRONRCF435MINI/target.c
View file

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

2
src/main/target/NEUTRONRCF435MINI/target.h
View file

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

4
src/main/target/RUSH_BLADE_F7/target.c
View file

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

7
src/main/target/SITL/sim/realFlight.c
View file

@ -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", "<RestoreOriginalControllerDevice><a>1</a><b>2</b></RestoreOriginalControllerDevice>");
endRequest();
free(endRequest());
startRequest("InjectUAVControllerInterface", "<InjectUAVControllerInterface><a>1</a><b>2</b></InjectUAVControllerInterface>");
endRequest();
free(endRequest());
exchangeData();
ENABLE_ARMING_FLAG(SIMULATOR_MODE_SITL);

3
src/main/target/SITL/sim/simple_soap_client.c
View file

@ -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, ...)

11
src/main/target/SKYSTARSH743HD/target.c
View file

@ -36,13 +36,10 @@ 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
};

2
src/main/target/SPEEDYBEEF405MINI/CMakeLists.txt Normal file
View file

@ -0,0 +1,2 @@
target_stm32f405xg(SPEEDYBEEF405MINI)
target_stm32f405xg(SPEEDYBEEF405MINI_6OUTPUTS)

37
src/main/target/SPEEDYBEEF405MINI/config.c Normal file
View file

@ -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 <http://www.gnu.org/licenses/>.
*/
#include <stdint.h>
#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;
}

46
src/main/target/SPEEDYBEEF405MINI/target.c Normal file
View file

@ -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 <http://www.gnu.org/licenses/>.
*/
#include <stdint.h>
#include "platform.h"
#include "drivers/bus.h"
#include "drivers/io.h"
#include "drivers/pwm_mapping.h"
#include "drivers/timer.h"
#include "drivers/pinio.h"
#include "drivers/sensor.h"
timerHardware_t timerHardware[] = {
DEF_TIM(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]);

186
src/main/target/SPEEDYBEEF405MINI/target.h Normal file
View file

@ -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 <http://www.gnu.org/licenses/>.
*/
#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

3
src/main/target/SPEEDYBEEF405V3/target.h
View file

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

1
src/main/target/SPEEDYBEEF7MINI/CMakeLists.txt
View file

@ -1 +1,2 @@
target_stm32f722xe(SPEEDYBEEF7MINI)
target_stm32f722xe(SPEEDYBEEF7MINIV2)

4
src/main/target/SPEEDYBEEF7MINI/target.c
View file

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

16
src/main/target/SPEEDYBEEF7MINI/target.h
View file

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

1
src/main/target/SPEEDYBEEF7V3/target.h
View file

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

2
src/main/target/common.h Normal file → Executable file
View file

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

10
src/main/target/common_hardware.c
View file

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