Skip to content

Commit e4cd324

Browse files
breadovenbreadoven
committed
CR151.1 + CR152.1 + CR153
1 parent e0e2238 commit e4cd324

7 files changed

Lines changed: 19 additions & 57 deletions

File tree

‎src/main/fc/multifunction.c‎

Lines changed: 0 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -45,9 +45,6 @@ static void multiFunctionApply(multi_function_e selectedItem)
4545
switch (selectedItem) {
4646
case MULTI_FUNC_NONE:
4747
break;
48-
// case MULTI_FUNC_1: // redisplay current warnings
49-
// osdResetWarningFlags();
50-
// break;
5148
case MULTI_FUNC_1: // control manual emergency landing
5249
checkManualEmergencyLandingControl(ARMING_FLAG(ARMED));
5350
break;

‎src/main/fc/multifunction.h‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -54,7 +54,6 @@ typedef enum {
5454
MULTI_FUNC_3,
5555
MULTI_FUNC_4,
5656
MULTI_FUNC_5,
57-
// MULTI_FUNC_6,
5857
MULTI_FUNC_END,
5958
} multi_function_e;
6059

‎src/main/fc/settings.yaml‎

Lines changed: 1 addition & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -3934,14 +3934,7 @@ groups:
39343934
field: osd_switch_indicators_align_left
39353935
type: bool
39363936
default_value: ON
3937-
# CR151
3938-
- name: multifunction_warning_cycle_time
3939-
description: "Cycle time for display of full multifunction warning messages [s]. Full messages will be displayed for 5s every cycle period. Set to 0 to constantly display full warnings."
3940-
field: multifunction_warning_cycle_time
3941-
min: 0
3942-
max: 60
3943-
default_value: 5
3944-
# CR151
3937+
39453938
- name: PG_OSD_COMMON_CONFIG
39463939
type: osdCommonConfig_t
39473940
headers: ["io/osd_common.h"]

‎src/main/io/osd.c‎

Lines changed: 3 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -206,7 +206,6 @@ static uint8_t armState;
206206
// Multifunction display
207207
static textAttributes_t osdGetMultiFunctionMessage(char *buff);
208208
multiFunctionWarning_t multiFunctionWarning;
209-
// static uint16_t osdWarningsFlags = 0;
210209

211210
typedef struct osdMapData_s {
212211
uint32_t scale;
@@ -4369,7 +4368,6 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
43694368
.use_pilot_logo = SETTING_OSD_USE_PILOT_LOGO_DEFAULT,
43704369
.inav_to_pilot_logo_spacing = SETTING_OSD_INAV_TO_PILOT_LOGO_SPACING_DEFAULT,
43714370
.arm_screen_display_time = SETTING_OSD_ARM_SCREEN_DISPLAY_TIME_DEFAULT,
4372-
.multifunction_warning_cycle_time = SETTING_MULTIFUNCTION_WARNING_CYCLE_TIME_DEFAULT, // CR151
43734371

43744372
#ifdef USE_WIND_ESTIMATOR
43754373
.estimations_wind_compensation = SETTING_OSD_ESTIMATIONS_WIND_COMPENSATION_DEFAULT,
@@ -6441,8 +6439,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
64416439
return elemAttr;
64426440
}
64436441

6444-
// static bool newWarningActive = false; // CR151.1
6445-
static bool osdCheckWarning(bool condition, uint8_t warningFlag) //, uint8_t *warningsCount)
6442+
static bool osdCheckWarning(bool condition, uint8_t warningFlag)
64466443
{
64476444
const timeMs_t currentTimeMs = millis();
64486445
static timeMs_t newWarningEndTime;
@@ -6470,7 +6467,6 @@ static bool osdCheckWarning(bool condition, uint8_t warningFlag) //, uint8_t *w
64706467
multiFunctionWarning.newWarningActive = false;
64716468
}
64726469
return true;
6473-
// *warningsCount += 1;
64746470
} else if (multiFunctionWarning.osdWarningsFlags & warningFlag) {
64756471
multiFunctionWarning.osdWarningsFlags &= ~warningFlag;
64766472
}
@@ -6483,7 +6479,6 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
64836479
/* Message length limit 10 char max */
64846480

64856481
textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE;
6486-
// static uint8_t warningsCount;
64876482
const char *message = NULL;
64886483

64896484
#ifdef USE_MULTI_FUNCTIONS
@@ -6495,10 +6490,6 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
64956490

64966491
switch (selectedFunction) {
64976492
case MULTI_FUNC_NONE:
6498-
// case MULTI_FUNC_1:
6499-
// // message = warningsCount ? "WARNINGS !" : "0 WARNINGS";
6500-
// message = "0 WARNINGS";
6501-
// break;
65026493
case MULTI_FUNC_1:
65036494
message = posControl.flags.manualEmergLandActive ? "ABORT LAND" : "EMERG LAND";
65046495
break;
@@ -6553,13 +6544,11 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
65536544
const char *messages[9];
65546545
uint8_t messageCount = 0;
65556546
bool warningCondition = false;
6556-
// warningsCount = 0;
65576547
uint16_t warningFlagID = 1;
65586548

65596549
// Low Battery Voltage
65606550
const batteryState_e batteryVoltageState = checkBatteryVoltageState();
65616551
warningCondition = batteryVoltageState == BATTERY_CRITICAL || batteryVoltageState == BATTERY_WARNING;
6562-
// if (osdCheckWarning(warningCondition, warningFlagID, &warningsCount)) {
65636552
if (osdCheckWarning(warningCondition, warningFlagID)) {
65646553
messages[messageCount++] = batteryVoltageState == BATTERY_CRITICAL ? "VBATT LAND" : "VBATT LOW ";
65656554
}
@@ -6568,15 +6557,13 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
65686557
if (batteryUsesCapacityThresholds()) {
65696558
const batteryState_e batteryState = getBatteryState();
65706559
warningCondition = batteryState == BATTERY_CRITICAL || batteryState == BATTERY_WARNING;
6571-
// if (osdCheckWarning(warningCondition, warningFlagID <<= 1, &warningsCount)) {
65726560
if (osdCheckWarning(warningCondition, warningFlagID <<= 1)) {
65736561
messages[messageCount++] = batteryState == BATTERY_CRITICAL ? "BATT EMPTY" : "BATT DYING";
65746562
}
65756563
}
65766564
#if defined(USE_GPS)
65776565
// GPS Fix and Failure
65786566
if (feature(FEATURE_GPS)) {
6579-
// if (osdCheckWarning(!STATE(GPS_FIX), warningFlagID <<= 1, &warningsCount)) {
65806567
if (osdCheckWarning(!STATE(GPS_FIX), warningFlagID <<= 1)) {
65816568
bool gpsFailed = getHwGPSStatus() == HW_SENSOR_UNAVAILABLE;
65826569
messages[messageCount++] = gpsFailed ? "GPS FAILED" : "NO GPS FIX";
@@ -6586,13 +6573,11 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
65866573
// RTH sanity (warning if RTH heads 200m further away from home than closest point)
65876574
warningCondition = NAV_Status.state == MW_NAV_STATE_RTH_ENROUTE && !posControl.flags.rthTrackbackActive &&
65886575
(posControl.homeDistance - posControl.rthSanityChecker.minimalDistanceToHome) > 20000;
6589-
// if (osdCheckWarning(warningCondition, warningFlagID <<= 1, &warningsCount)) {
65906576
if (osdCheckWarning(warningCondition, warningFlagID <<= 1)) {
65916577
messages[messageCount++] = "RTH SANITY";
65926578
}
65936579

65946580
// Altitude sanity (warning if significant mismatch between estimated and GPS altitude)
6595-
// if (osdCheckWarning(posControl.flags.gpsCfEstimatedAltitudeMismatch, warningFlagID <<= 1, &warningsCount)) {
65966581
if (osdCheckWarning(posControl.flags.gpsCfEstimatedAltitudeMismatch, warningFlagID <<= 1)) {
65976582
messages[messageCount++] = "ALT SANITY";
65986583
}
@@ -6601,7 +6586,6 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
66016586
// Magnetometer failure
66026587
if (requestedSensors[SENSOR_INDEX_MAG] != MAG_NONE) {
66036588
hardwareSensorStatus_e magStatus = getHwCompassStatus();
6604-
// if (osdCheckWarning(magStatus == HW_SENSOR_UNAVAILABLE || magStatus == HW_SENSOR_UNHEALTHY, warningFlagID <<= 1, &warningsCount)) {
66056589
if (osdCheckWarning(magStatus == HW_SENSOR_UNAVAILABLE || magStatus == HW_SENSOR_UNHEALTHY, warningFlagID <<= 1)) {
66066590
messages[messageCount++] = "MAG FAILED";
66076591
}
@@ -6611,7 +6595,6 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
66116595
#if defined(USE_PITOT)
66126596
// Pitot sensor validation failure (blocked/failed pitot tube)
66136597
if (sensors(SENSOR_PITOT) && detectedSensors[SENSOR_INDEX_PITOT] != PITOT_VIRTUAL) {
6614-
// if (osdCheckWarning(pitotHasFailed(), warningFlagID <<= 1, &warningsCount)) {
66156598
if (osdCheckWarning(pitotHasFailed(), warningFlagID <<= 1)) {
66166599
messages[messageCount++] = "PITOT FAIL";
66176600
}
@@ -6622,20 +6605,18 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
66226605
const float vibrationLevel = accGetVibrationLevel();
66236606
// DEBUG_SET(DEBUG_ALWAYS, 0, vibrationLevel * 100);
66246607
warningCondition = vibrationLevel > 1.5f;
6625-
// if (osdCheckWarning(warningCondition, warningFlagID <<= 1, &warningsCount)) {
66266608
if (osdCheckWarning(warningCondition, warningFlagID <<= 1)) {
66276609
messages[messageCount++] = vibrationLevel > 2.5f ? "BAD VIBRTN" : "VIBRATION!";
66286610
}
66296611

66306612
#ifdef USE_DEV_TOOLS
6631-
// if (osdCheckWarning(systemConfig()->groundTestMode, warningFlagID <<= 1, &warningsCount)) {
66326613
if (osdCheckWarning(systemConfig()->groundTestMode, warningFlagID <<= 1)) {
66336614
messages[messageCount++] = "GRD TEST !";
66346615
}
66356616
#endif
66366617

66376618
// #if defined(USE_BARO)
6638-
// if (osdCheckWarning(getHwBarometerStatus() == HW_SENSOR_UNAVAILABLE, warningFlagID <<= 1, &warningsCount)) {
6619+
// if (osdCheckWarning(getHwBarometerStatus() == HW_SENSOR_UNAVAILABLE, warningFlagID <<= 1)) {
66396620
// messages[messageCount++] = "BARO FAIL";
66406621
// }
66416622
// #endif
@@ -6646,10 +6627,7 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
66466627
if (multiFunctionWarning.newWarningActive) {
66476628
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
66486629
}
6649-
} // else if (warningsCount) {
6650-
// buff[0] = SYM_ALERT;
6651-
// tfp_sprintf(buff + 1, "%u ", warningsCount);
6652-
// }
6630+
}
66536631

66546632
return elemAttr;
66556633
}

‎src/main/io/osd.h‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -529,7 +529,6 @@ typedef struct osdConfig_s {
529529
bool use_pilot_logo; // If enabled, the pilot logo (last 40 characters of page 2 font) will be used with the INAV logo.
530530
uint8_t inav_to_pilot_logo_spacing; // The space between the INAV and pilot logos, if pilot logo is used. This number may be adjusted so that it fits the odd/even col width.
531531
uint16_t arm_screen_display_time; // Length of time the arm screen is displayed
532-
uint8_t multifunction_warning_cycle_time; // Cycle time for display of full multifunction warning messages (s) CR151
533532
#ifndef DISABLE_MSP_DJI_COMPAT
534533
bool highlight_djis_missing_characters; // If enabled, show question marks where there is no character in DJI's font to represent an OSD element symbol
535534
#endif

‎src/main/navigation/navigation_multicopter.c‎

Lines changed: 11 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -139,24 +139,19 @@ bool adjustMulticopterAltitudeFromRCInput(void)
139139
return true;
140140
}
141141
else {
142-
// const int16_t rcThrottleAdjustment = applyDeadbandRescaled(rcCommand[THROTTLE] - altHoldThrottleRCZero, rcControlsConfig()->alt_hold_deadband, -500, 500);
143-
const int16_t rcThrottleAdjustment = applyDeadbandRescaled(rxGetChannelValue(THROTTLE) - altHoldThrottleRCZero, rcControlsConfig()->alt_hold_deadband, -500, 500); // CR139
142+
const uint8_t deadband = rcControlsConfig()->alt_hold_deadband; // CR153
143+
const int16_t rcThrottleAdjustment = applyDeadband(rcCommand[THROTTLE] - altHoldThrottleRCZero, deadband); // CR153
144144

145145
if (rcThrottleAdjustment) {
146146
/* Set velocity proportional to stick movement
147147
* Scale from altHoldThrottleRCZero to maxthrottle or minthrottle to altHoldThrottleRCZero */
148+
// CR153
149+
int16_t controlRange = -deadband;
150+
// controlRange += rcThrottleAdjustment > 0 ? PWM_RANGE_MAX - altHoldThrottleRCZero : altHoldThrottleRCZero - PWM_RANGE_MIN;
151+
controlRange += rcThrottleAdjustment > 0 ? getMaxThrottle() - altHoldThrottleRCZero : altHoldThrottleRCZero - getThrottleIdleValue();
148152

149-
// Calculate max up or min down limit value scaled for deadband
150-
// CR139
151-
// int16_t limitValue = rcThrottleAdjustment > 0 ? getMaxThrottle() : getThrottleIdleValue();
152-
// int16_t limitValue = rcThrottleAdjustment > 0 ? PWM_RANGE_MAX : PWM_RANGE_MIN;
153-
// limitValue = applyDeadbandRescaled(limitValue - altHoldThrottleRCZero, rcControlsConfig()->alt_hold_deadband, -500, 500);
154-
// CR139
155-
// int16_t rcClimbRate = ABS(rcThrottleAdjustment) * navConfig()->mc.max_manual_climb_rate / limitValue;
156-
int16_t rcClimbRate = rcThrottleAdjustment * navConfig()->mc.max_manual_climb_rate / 500;
157-
158-
// DEBUG_SET(DEBUG_ALWAYS, 4, rcThrottleAdjustment);
159-
// DEBUG_SET(DEBUG_ALWAYS, 5, rcClimbRate);
153+
const int16_t rcClimbRate = rcThrottleAdjustment * navConfig()->mc.max_manual_climb_rate / controlRange;
154+
// CR153
160155
updateClimbRateToAltitudeController(rcClimbRate, 0, ROC_TO_ALT_CONSTANT);
161156

162157
return true;
@@ -180,11 +175,12 @@ void setupMulticopterAltitudeController(void)
180175
if (throttleType == MC_ALT_HOLD_STICK && !throttleIsLow) {
181176
// Only use current throttle if not LOW - use Thr Mid otherwise
182177
altHoldThrottleRCZero = rcCommand[THROTTLE];
178+
// altHoldThrottleRCZero = rxGetChannelValue(THROTTLE);
183179
} else if (throttleType == MC_ALT_HOLD_HOVER) {
184180
altHoldThrottleRCZero = currentBatteryProfile->nav.mc.hover_throttle;
185181
} else {
186-
// altHoldThrottleRCZero = rcLookupThrottleMid();
187-
altHoldThrottleRCZero = PWM_RANGE_MIDDLE; // CR139
182+
altHoldThrottleRCZero = rcLookupThrottleMid();
183+
// altHoldThrottleRCZero = PWM_RANGE_MIDDLE;
188184
}
189185

190186
// Make sure we are able to satisfy the deadband

‎src/main/navigation/navigation_pos_estimator.c‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -834,10 +834,10 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
834834
/* Calculate new EPH and EPV for the case we didn't update position */
835835
// ctx.newEPH = posEstimator.est.eph * ((posEstimator.est.eph <= max_eph_epv) ? 1.0f + ctx.dt : 1.0f); // CR152
836836
// ctx.newEPV = posEstimator.est.epv * ((posEstimator.est.epv <= max_eph_epv) ? 1.0f + ctx.dt : 1.0f); // CR152
837-
bool XYPosUpdateTimeout = US2MS(currentTimeUs) - lastXYSensorUpdateMs > 200; // CR152.1
838-
ctx.newEPH = posEstimator.est.eph + ((posEstimator.est.eph <= max_eph_epv && XYPosUpdateTimeout) ? 100.0f * ctx.dt : 0.0f); // CR152.1
839-
bool ZPosUpdateTimeout = US2MS(currentTimeUs) - lastZSensorUpdateMs > 200; // CR152.1
840-
ctx.newEPV = posEstimator.est.epv + ((posEstimator.est.epv <= max_eph_epv && ZPosUpdateTimeout) ? 100.0f * ctx.dt : 0.0f); // CR152.1
837+
bool XYSensorUpdateTimeout = US2MS(currentTimeUs) - lastXYSensorUpdateMs > 200; // CR152.1
838+
ctx.newEPH = posEstimator.est.eph + ((posEstimator.est.eph <= max_eph_epv && XYSensorUpdateTimeout) ? 100.0f * ctx.dt : 0.0f); // CR152.1
839+
bool ZSensorUpdateTimeout = US2MS(currentTimeUs) - lastZSensorUpdateMs > 200; // CR152.1
840+
ctx.newEPV = posEstimator.est.epv + ((posEstimator.est.epv <= max_eph_epv && ZSensorUpdateTimeout) ? 100.0f * ctx.dt : 0.0f); // CR152.1
841841
// CR152.1
842842

843843
ctx.newFlags = calculateCurrentValidityFlags(currentTimeUs);

0 commit comments

Comments
 (0)