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

Merge remote-tracking branch 'origin/master' into sh_mixer_profile

Browse source
This commit is contained in:
shota 2023-08-28 17:23:47 +09:00
commit bf0c0d8a50
34 changed files with 384 additions and 227 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/
/build_SITL/
@ -32,3 +32,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/Settings.md
View file

@ -1362,6 +1362,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.
@ -3312,6 +3322,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
@ -3782,16 +3802,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.

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

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;

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

@ -174,6 +174,8 @@
#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_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

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;

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

@ -123,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
@ -143,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;
}
@ -265,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);
}

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

@ -1290,7 +1290,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;
@ -2256,7 +2256,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;

23
src/main/fc/settings.yaml
View file

@ -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
@ -1531,6 +1537,7 @@ groups:
min: 1
max: 3000
- name: PG_GPS_CONFIG
headers: [ "io/gps.h" ]
type: gpsConfig_t
condition: USE_GPS
members:
@ -1562,6 +1569,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
@ -2340,11 +2353,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);

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

@ -888,8 +888,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;
@ -1128,7 +1126,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]);

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

@ -2022,8 +2022,25 @@ 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:
{
@ -3552,10 +3569,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;
}
}

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

@ -108,7 +108,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 +179,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
@ -236,10 +236,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;
@ -1335,17 +1335,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) {
@ -1461,7 +1456,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);
@ -1480,21 +1475,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;
}
@ -1546,7 +1527,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;
}
@ -1569,7 +1550,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
@ -1772,12 +1753,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();
@ -2920,7 +2896,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;
}
@ -3011,28 +2987,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;
}
@ -3058,10 +3042,12 @@ 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)
{
@ -3710,6 +3696,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);
}
/*-----------------------------------------------------------
@ -3980,10 +3968,9 @@ bool navigationRequiresTurnAssistance(void)
// For airplanes turn assistant is always required when controlling position
return (currentState & (NAV_CTL_POS | NAV_CTL_ALT));
}
else {
return false;
}
}
/**
* An indicator that NAV is in charge of heading control (a signal to disable other heading controllers)

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

@ -175,6 +175,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 +223,6 @@ typedef struct navConfig_s {
struct {
struct {
uint8_t use_thr_mid_for_althold; // Don't remember throttle when althold was initiated, assume that throttle is at Thr Mid = zero climb rate
uint8_t extra_arming_safety; // from navExtraArmingSafety_e
uint8_t user_control_mode; // NAV_GPS_ATTI or NAV_GPS_CRUISE
uint8_t rth_alt_control_mode; // Controls the logic for choosing the RTH altitude
@ -287,6 +292,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 {
@ -638,6 +644,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

@ -118,13 +118,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;
}
@ -158,9 +158,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);
@ -765,7 +770,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
@ -869,27 +865,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);
}
@ -902,15 +898,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_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 {
@ -458,7 +459,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);

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

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

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

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