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

Add trackback suspend + vibration warning

Browse source
This commit is contained in:
breadoven 2023-02-18 13:14:05 +00:00
parent a5e2550193
commit 36caed01ab
6 changed files with 40 additions and 17 deletions
Download patch file Download diff file Expand all files Collapse all files

14
src/main/fc/multifunction.c
View file

@ -34,11 +34,13 @@
#include "io/osd.h"
#include "navigation/navigation.h"
uint8_t multiFunctionFlags;
static void multiFunctionApply(multi_function_e selectedItem)
{
switch (selectedItem) {
case MULTI_FUNC_NONE:
return;
break;
case MULTI_FUNC_1: // redisplay current warnings
osdResetWarningFlags();
break;
@ -48,15 +50,19 @@ static void multiFunctionApply(multi_function_e selectedItem)
case MULTI_FUNC_3: // toggle Safehome suspend
#if defined(USE_SAFE_HOME)
if (navConfig()->general.flags.safehome_usage_mode != SAFEHOME_USAGE_OFF) {
suspendSafehome();
MULTI_FUNC_FLAG(SUSPEND_SAFEHOMES) ? MULTI_FUNC_FLAG_DISABLE(SUSPEND_SAFEHOMES) : MULTI_FUNC_FLAG_ENABLE(SUSPEND_SAFEHOMES);
}
#endif
break;
case MULTI_FUNC_4: // emergency ARM
case MULTI_FUNC_4:
if (navConfig()->general.flags.rth_trackback_mode != RTH_TRACKBACK_OFF) {
MULTI_FUNC_FLAG(SUSPEND_TRACKBACK) ? MULTI_FUNC_FLAG_DISABLE(SUSPEND_TRACKBACK) : MULTI_FUNC_FLAG_ENABLE(SUSPEND_TRACKBACK);
}
break;
case MULTI_FUNC_5: // emergency ARM
if (!ARMING_FLAG(ARMED)) {
emergencyArmingUpdate(true, true);
}
break;
case MULTI_FUNC_END:
break;
}

12
src/main/fc/multifunction.h
View file

@ -24,12 +24,24 @@
#pragma once
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 {
SUSPEND_SAFEHOMES = (1 << 0),
SUSPEND_TRACKBACK = (1 << 1),
} multiFunctionFlags_e;
typedef enum {
MULTI_FUNC_NONE,
MULTI_FUNC_1,
MULTI_FUNC_2,
MULTI_FUNC_3,
MULTI_FUNC_4,
MULTI_FUNC_5,
MULTI_FUNC_END,
} multi_function_e;

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

@ -4751,6 +4751,7 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
/* --- FUNCTIONS --- */
multi_function_e selectedFunction = multiFunctionSelection();
if (selectedFunction) {
message = "N/A NEXT >"; // Default message if function unavailable
switch (selectedFunction) {
case MULTI_FUNC_NONE:
case MULTI_FUNC_1:
@ -4760,14 +4761,18 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
message = posControl.flags.manualEmergLandActive ? "ABORT LAND" : "EMERG LAND";
break;
case MULTI_FUNC_3:
message = "NO SFHOME ";
#if defined(USE_SAFE_HOME)
if (navConfig()->general.flags.safehome_usage_mode != SAFEHOME_USAGE_OFF) {
message = posControl.safehomeState.isSuspended ? "USE SFHOME" : "CAN SFHOME";
message = MULTI_FUNC_FLAG(SUSPEND_SAFEHOMES) ? "USE SFHOME" : "SUS SFHOME";
}
#endif
break;
case MULTI_FUNC_4:
if (navConfig()->general.flags.rth_trackback_mode != RTH_TRACKBACK_OFF) {
message = MULTI_FUNC_FLAG(SUSPEND_TRACKBACK) ? "USE TKBACK" : "SUS TKBACK";
}
break;
case MULTI_FUNC_5:
message = ARMING_FLAG(ARMED) ? "NOW ARMED " : "EMERG ARM ";
break;
case MULTI_FUNC_END:
@ -4779,7 +4784,7 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
}
/* --- WARNINGS --- */
const char *messages[5];
const char *messages[6];
uint8_t messageCount = 0;
bool warningCondition = false;
warningsCount = 0;
@ -4792,6 +4797,13 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
messages[messageCount++] = batteryState == BATTERY_CRITICAL ? "BATT EMPTY" : "BATT LOW !";
}
// Vibration levels
const float vibrationLevel = accGetVibrationLevel();
warningCondition = vibrationLevel > 1.5f;
if (osdCheckWarning(warningCondition, warningFlagID <<= 1, &warningsCount)) {
messages[messageCount++] = vibrationLevel > 2.5f ? "BAD VIBRTN" : "VIBRATION!";
}
#if defined(USE_GPS)
// GPS Fix and Failure
if (feature(FEATURE_GPS)) {

11
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"
@ -1312,7 +1313,7 @@ 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;
const bool cancelTrackback = distFromStartTrackback > navConfig()->general.rth_trackback_distance ||
(rthAltControlStickOverrideCheck(ROLL) && !posControl.flags.forcedRTHActivated);
((rthAltControlStickOverrideCheck(ROLL) || MULTI_FUNC_FLAG(SUSPEND_TRACKBACK)) && !posControl.flags.forcedRTHActivated);
if (posControl.activeRthTBPointIndex < 0 || cancelTrackback) {
posControl.rthTBWrapAroundCounter = posControl.activeRthTBPointIndex = -1;
@ -2456,16 +2457,10 @@ static navigationHomeFlags_t navigationActualStateHomeValidity(void)
}
#if defined(USE_SAFE_HOME)
void suspendSafehome(void)
{
// toggle Safehome suspend each call
posControl.safehomeState.isSuspended = !posControl.safehomeState.isSuspended;
}
void checkSafeHomeState(bool shouldBeEnabled)
{
const bool safehomeNotApplicable = navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF ||
posControl.safehomeState.isSuspended ||
(MULTI_FUNC_FLAG(SUSPEND_SAFEHOMES) && !posControl.flags.forcedRTHActivated) ||
posControl.flags.rthTrackbackActive ||
(!posControl.safehomeState.isApplied && posControl.homeDistance < navConfig()->general.min_rth_distance);

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

@ -60,7 +60,6 @@ PG_DECLARE_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig);
void resetSafeHomes(void); // remove all safehomes
bool findNearestSafeHome(void); // Find nearest safehome
void suspendSafehome(void); // Suspend safehome on demand
#endif // defined(USE_SAFE_HOME)
#ifndef NAV_MAX_WAYPOINTS

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

@ -352,7 +352,6 @@ typedef struct {
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
bool isSuspended; // used to suspend Safehome on demand
} safehomeState_t;
typedef struct {