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

Merge branch 'master' of https://github.com/RomanLut/inav into submit-gps-fix-estimation

Browse source
This commit is contained in:
Roman Lut 2023-08-04 00:57:19 +02:00
commit 2b9a5a6a8d
178 changed files with 5846 additions and 2036 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -101,7 +101,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang
PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 2);
#endif
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 3);
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 4);
PG_RESET_TEMPLATE(navConfig_t, navConfig,
.general = {
@ -125,6 +125,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.soaring_motor_stop = SETTING_NAV_FW_SOARING_MOTOR_STOP_DEFAULT, // stops motor when Saoring mode enabled
.rth_trackback_mode = SETTING_NAV_RTH_TRACKBACK_MODE_DEFAULT, // RTH trackback useage mode
.rth_use_linear_descent = SETTING_NAV_RTH_USE_LINEAR_DESCENT_DEFAULT, // Use linear descent during RTH
.landing_bump_detection = SETTING_NAV_LANDING_BUMP_DETECTION_DEFAULT, // Detect landing based on touchdown G bump
},
// General navigation parameters
@ -1247,7 +1248,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
}
const uint8_t rthClimbMarginPercent = STATE(FIXED_WING_LEGACY) ? FW_RTH_CLIMB_MARGIN_PERCENT : MR_RTH_CLIMB_MARGIN_PERCENT;
const float rthAltitudeMargin = MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (rthClimbMarginPercent/100.0) * fabsf(posControl.rthState.rthInitialAltitude - posControl.rthState.homePosition.pos.z));
const float rthAltitudeMargin = MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (rthClimbMarginPercent/100.0f) * fabsf(posControl.rthState.rthInitialAltitude - posControl.rthState.homePosition.pos.z));
// If we reached desired initial RTH altitude or we don't want to climb first
if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (navConfig()->general.flags.rth_climb_first == RTH_CLIMB_OFF) || rthAltControlStickOverrideCheck(ROLL) || rthClimbStageActiveAndComplete()) {
@ -1722,6 +1723,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi
timeMs_t currentTime = millis();
if (posControl.waypointList[posControl.activeWaypointIndex].p1 <= 0 ||
posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT ||
(posControl.wpReachedTime != 0 && currentTime - posControl.wpReachedTime >= (timeMs_t)posControl.waypointList[posControl.activeWaypointIndex].p1*1000L)) {
return NAV_FSM_EVENT_SUCCESS;
}
@ -1877,7 +1879,7 @@ static navigationFSMState_t navSetNewFSMState(navigationFSMState_t newState)
static void navProcessFSMEvents(navigationFSMEvent_t injectedEvent)
{
const timeMs_t currentMillis = millis();
navigationFSMState_t previousState;
navigationFSMState_t previousState = NAV_STATE_UNDEFINED;
static timeMs_t lastStateProcessTime = 0;
/* Process new injected event if event defined,
@ -2635,7 +2637,7 @@ static bool rthAltControlStickOverrideCheck(unsigned axis)
* transiton in to turn.
* Limited to fixed wing only.
* --------------------------------------------------- */
bool rthClimbStageActiveAndComplete() {
bool rthClimbStageActiveAndComplete(void) {
if ((STATE(FIXED_WING_LEGACY) || STATE(AIRPLANE)) && (navConfig()->general.rth_climb_first_stage_altitude > 0)) {
if (posControl.actualState.abs.pos.z >= posControl.rthState.rthClimbStageAltitude) {
return true;
@ -3229,7 +3231,7 @@ void loadSelectedMultiMission(uint8_t missionIndex)
posControl.geoWaypointCount = 0;
for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) {
if ((missionCount == missionIndex)) {
if (missionCount == missionIndex) {
/* store details of selected mission: start wp index, mission wp count, geo wp count */
if (!(posControl.waypointList[i].action == NAV_WP_ACTION_SET_POI ||
posControl.waypointList[i].action == NAV_WP_ACTION_SET_HEAD ||
@ -4397,7 +4399,7 @@ int32_t navigationGetHomeHeading(void)
}
// returns m/s
float calculateAverageSpeed() {
float calculateAverageSpeed(void) {
float flightTime = getFlightTime();
if (flightTime == 0.0f) return 0;
return (float)getTotalTravelDistance() / (flightTime * 100);