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

Merge branch 'master' into dzikuvx-sqrtf-update

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2021-06-13 14:55:09 +02:00
commit f737fe1901
53 changed files with 6612 additions and 940 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -186,6 +186,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.launch_throttle = SETTING_NAV_FW_LAUNCH_THR_DEFAULT,
.launch_idle_throttle = SETTING_NAV_FW_LAUNCH_IDLE_THR_DEFAULT, // Motor idle or MOTOR_STOP
.launch_motor_timer = SETTING_NAV_FW_LAUNCH_MOTOR_DELAY_DEFAULT, // ms
.launch_idle_motor_timer = SETTING_NAV_FW_LAUNCH_IDLE_MOTOR_DELAY_DEFAULT, // ms
.launch_motor_spinup_time = SETTING_NAV_FW_LAUNCH_SPINUP_TIME_DEFAULT, // ms, time to gredually increase throttle from idle to launch
.launch_end_time = SETTING_NAV_FW_LAUNCH_END_TIME_DEFAULT, // ms, time to gradually decrease/increase throttle and decrease pitch angle from launch to the current flight mode
.launch_min_time = SETTING_NAV_FW_LAUNCH_MIN_TIME_DEFAULT, // ms, min time in launch mode
@ -1727,7 +1728,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationF
//allow to leave NAV_LAUNCH_MODE if it has being enabled as feature by moving sticks with low throttle.
if (feature(FEATURE_FW_LAUNCH)) {
throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
if ((throttleStatus == THROTTLE_LOW) && (areSticksDeflectedMoreThanPosHoldDeadband())) {
if ((throttleStatus == THROTTLE_LOW) && (isRollPitchStickDeflected())) {
abortFixedWingLaunch();
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
}
@ -2579,11 +2580,11 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
}
else {
/*
/*
* 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 &&
if (navConfig()->general.max_altitude > 0 &&
altitudeToUse >= navConfig()->general.max_altitude &&
desiredClimbRate > 0
) {
@ -2832,13 +2833,23 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS) && !ARMING_FLAG(ARMED)) {
if (wpData->action == NAV_WP_ACTION_WAYPOINT || wpData->action == NAV_WP_ACTION_JUMP || wpData->action == NAV_WP_ACTION_RTH || wpData->action == NAV_WP_ACTION_HOLD_TIME || wpData->action == NAV_WP_ACTION_LAND || wpData->action == NAV_WP_ACTION_SET_POI || wpData->action == NAV_WP_ACTION_SET_HEAD ) {
// Only allow upload next waypoint (continue upload mission) or first waypoint (new mission)
static int8_t nonGeoWaypointCount = 0;
if (wpNumber == (posControl.waypointCount + 1) || wpNumber == 1) {
posControl.waypointList[wpNumber - 1] = *wpData;
if(wpData->action == NAV_WP_ACTION_JUMP) {
posControl.waypointList[wpNumber - 1].p1 -= 1; // make index (vice WP #)
if(wpData->action == NAV_WP_ACTION_SET_POI || wpData->action == NAV_WP_ACTION_SET_HEAD || wpData->action == NAV_WP_ACTION_JUMP) {
nonGeoWaypointCount += 1;
if(wpData->action == NAV_WP_ACTION_JUMP) {
posControl.waypointList[wpNumber - 1].p1 -= 1; // make index (vice WP #)
}
}
posControl.waypointCount = wpNumber;
posControl.waypointListValid = (wpData->flag == NAV_WP_FLAG_LAST);
posControl.geoWaypointCount = posControl.waypointCount - nonGeoWaypointCount;
if (posControl.waypointListValid) {
nonGeoWaypointCount = 0;
}
}
}
}
@ -2850,6 +2861,7 @@ void resetWaypointList(void)
if (!ARMING_FLAG(ARMED)) {
posControl.waypointCount = 0;
posControl.waypointListValid = false;
posControl.geoWaypointCount = 0;
}
}