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

limit update rate + use Ms

Browse source
This commit is contained in:
breadoven 2023-01-25 20:55:58 +00:00
parent 3560bb57ea
commit dfabcbdd1d
6 changed files with 22 additions and 17 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/common/time.h
View file

@ -51,6 +51,7 @@ typedef uint32_t timeUs_t;
#define USECS_PER_SEC (1000 * 1000)
#define HZ2US(hz) (1000000 / (hz))
#define HZ2MS(hz) (1000 / (hz))
#define US2S(us) ((us) * 1e-6f)
#define US2MS(us) ((us) * 1e-3f)
#define MS2US(ms) ((ms) * 1000)

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

@ -838,7 +838,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
if (!ARMING_FLAG(ARMED)) {
armTime = 0;
processDelayedSave();
}
@ -858,7 +858,6 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
if (isRXDataNew) {
updateWaypointsAndNavigationMode();
}
isRXDataNew = false;
updatePositionEstimator();
@ -921,10 +920,9 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
writeMotors();
}
#endif
// Check if landed, FW and MR
if (STATE(ALTITUDE_CONTROL)) {
updateLandingStatus();
updateLandingStatus(US2MS(currentTimeUs));
}
#ifdef USE_BLACKBOX

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

@ -2801,12 +2801,18 @@ void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t bearing, int32_t d
/*-----------------------------------------------------------
* NAV land detector
*-----------------------------------------------------------*/
void updateLandingStatus(void)
void updateLandingStatus(timeMs_t currentTimeMs)
{
if (STATE(AIRPLANE) && !navConfig()->general.flags.disarm_on_landing) {
return; // no point using this with a fixed wing if not set to disarm
}
static timeMs_t lastUpdateTimeMs = 0;
if (currentTimeMs < lastUpdateTimeMs + HZ2MS(100)) { // limit update to 100Hz
return;
}
lastUpdateTimeMs = currentTimeMs;
static bool landingDetectorIsActive;
DEBUG_SET(DEBUG_LANDING, 0, landingDetectorIsActive);
@ -2831,7 +2837,7 @@ void updateLandingStatus(void)
if (navConfig()->general.flags.disarm_on_landing) {
ENABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED);
disarm(DISARM_LANDING);
} else if (!navigationIsFlyingAutonomousMode()) {
} else if (!navigationInAutomaticThrottleMode()) {
// for multirotor only - reactivate landing detector without disarm when throttle raised toward hover throttle
landingDetectorIsActive = rxGetChannelValue(THROTTLE) < (0.5 * (currentBatteryProfile->nav.mc.hover_throttle + getThrottleIdleValue()));
}

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

@ -606,7 +606,7 @@ const char * fixedWingLaunchStateMessage(void);
float calculateAverageSpeed(void);
void updateLandingStatus(void);
void updateLandingStatus(timeMs_t currentTimeMs);
const navigationPIDControllers_t* getNavigationPIDControllers(void);

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

@ -723,7 +723,7 @@ bool isMulticopterFlying(void)
bool isMulticopterLandingDetected(void)
{
DEBUG_SET(DEBUG_LANDING, 4, 0);
static timeUs_t landingDetectorStartedAt;
static timeMs_t landingDetectorStartedAt;
/* Basic condition to start looking for landing
* Prevent landing detection if WP mission allowed during Failsafe (except landing states) */
@ -747,7 +747,7 @@ bool isMulticopterLandingDetected(void)
DEBUG_SET(DEBUG_LANDING, 3, gyroCondition);
bool possibleLandingDetected = false;
const timeUs_t currentTimeUs = micros();
const timeMs_t currentTimeMs = millis();
if (navGetCurrentStateFlags() & NAV_CTL_LAND) {
// We have likely landed if throttle is 40 units below average descend throttle
@ -761,13 +761,13 @@ bool isMulticopterLandingDetected(void)
if (!landingDetectorStartedAt) {
landingThrSum = landingThrSamples = 0;
landingDetectorStartedAt = currentTimeUs;
landingDetectorStartedAt = currentTimeMs;
}
if (!landingThrSamples) {
if (currentTimeUs - landingDetectorStartedAt < (USECS_PER_SEC * MC_LAND_THR_STABILISE_DELAY)) { // Wait for 1 second so throttle has stabilized.
if (currentTimeMs - landingDetectorStartedAt < S2MS(MC_LAND_THR_STABILISE_DELAY)) { // Wait for 1 second so throttle has stabilized.
return false;
} else {
landingDetectorStartedAt = currentTimeUs;
landingDetectorStartedAt = currentTimeMs;
}
}
landingThrSamples += 1;
@ -783,7 +783,7 @@ bool isMulticopterLandingDetected(void)
if (landingDetectorStartedAt) {
possibleLandingDetected = velCondition && gyroCondition;
} else {
landingDetectorStartedAt = currentTimeUs;
landingDetectorStartedAt = currentTimeMs;
return false;
}
}
@ -798,10 +798,10 @@ bool isMulticopterLandingDetected(void)
DEBUG_SET(DEBUG_LANDING, 5, possibleLandingDetected);
if (possibleLandingDetected) {
timeUs_t safetyTimeDelay = MS2US(1000 + navConfig()->general.auto_disarm_delay); // check conditions stable for 1s + optional extra delay
return (currentTimeUs - landingDetectorStartedAt > safetyTimeDelay);
timeMs_t safetyTimeDelay = 1000 + navConfig()->general.auto_disarm_delay; // check conditions stable for 1s + optional extra delay
return (currentTimeMs - landingDetectorStartedAt > safetyTimeDelay);
} else {
landingDetectorStartedAt = currentTimeUs;
landingDetectorStartedAt = currentTimeMs;
return false;
}
}

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

@ -42,7 +42,7 @@
#define MC_LAND_CHECK_VEL_XY_MOVING 100.0f // cm/s
#define MC_LAND_CHECK_VEL_Z_MOVING 100.0f // cm/s
#define MC_LAND_THR_STABILISE_DELAY 1 // seconds
#define MC_LAND_DESCEND_THROTTLE 40 // uS
#define MC_LAND_DESCEND_THROTTLE 40 // RC pwm units (us)
#define MC_LAND_SAFE_SURFACE 5.0f // cm
#define NAV_RTH_TRACKBACK_POINTS 50 // max number RTH trackback points