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

Merge remote-tracking branch 'upstream/master' into abo_landing_detection

Browse source
This commit is contained in:
breadoven 2022-01-09 21:55:13 +00:00
commit fe5e873356
354 changed files with 9601 additions and 7326 deletions
Download patch file Download diff file Expand all files Collapse all files

49
src/main/navigation/navigation_fixedwing.c
View file

@ -21,8 +21,6 @@
#include "platform.h"
#if defined(USE_NAV)
#include "build/build_config.h"
#include "build/debug.h"
@ -51,6 +49,8 @@
#include "navigation/navigation.h"
#include "navigation/navigation_private.h"
#include "programming/logic_condition.h"
#include "rx/rx.h"
#include "sensors/battery.h"
@ -188,7 +188,7 @@ void applyFixedWingAltitudeAndThrottleController(timeUs_t currentTimeUs)
}
// Indicate that information is no longer usable
posControl.flags.verticalPositionDataConsumed = 1;
posControl.flags.verticalPositionDataConsumed = true;
}
isPitchAdjustmentValid = true;
@ -273,20 +273,21 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
// Limit minimum forward velocity to 1 m/s
float trackingDistance = trackingPeriod * MAX(posControl.actualState.velXY, 100.0f);
uint32_t navLoiterRadius = getLoiterRadius(navConfig()->fw.loiter_radius);
// If angular visibility of a waypoint is less than 30deg, don't calculate circular loiter, go straight to the target
#define TAN_15DEG 0.26795f
bool needToCalculateCircularLoiter = (isApproachingLastWaypoint() || isWaypointWait())
&& (distanceToActualTarget <= (navConfig()->fw.loiter_radius / TAN_15DEG))
&& (distanceToActualTarget > 50.0f)
&& !FLIGHT_MODE(NAV_COURSE_HOLD_MODE);
bool needToCalculateCircularLoiter = isNavHoldPositionActive() &&
(distanceToActualTarget <= (navLoiterRadius / TAN_15DEG)) &&
(distanceToActualTarget > 50.0f);
// Calculate virtual position for straight movement
if (needToCalculateCircularLoiter) {
// We are closing in on a waypoint, calculate circular loiter
float loiterAngle = atan2_approx(-posErrorY, -posErrorX) + DEGREES_TO_RADIANS(loiterDirection() * 45.0f);
float loiterTargetX = posControl.desiredState.pos.x + navConfig()->fw.loiter_radius * cos_approx(loiterAngle);
float loiterTargetY = posControl.desiredState.pos.y + navConfig()->fw.loiter_radius * sin_approx(loiterAngle);
float loiterTargetX = posControl.desiredState.pos.x + navLoiterRadius * cos_approx(loiterAngle);
float loiterTargetY = posControl.desiredState.pos.y + navLoiterRadius * sin_approx(loiterAngle);
// We have temporary loiter target. Recalculate distance and position error
posErrorX = loiterTargetX - navGetCurrentActualPositionAndVelocity()->pos.x;
@ -438,7 +439,7 @@ void applyFixedWingPositionController(timeUs_t currentTimeUs)
}
// Indicate that information is no longer usable
posControl.flags.horizontalPositionDataConsumed = 1;
posControl.flags.horizontalPositionDataConsumed = true;
}
isRollAdjustmentValid = true;
@ -478,7 +479,7 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs)
}
// Indicate that information is no longer usable
posControl.flags.horizontalPositionDataConsumed = 1;
posControl.flags.horizontalPositionDataConsumed = true;
}
}
else {
@ -657,13 +658,21 @@ bool isFixedWingLandingDetected(void)
/*-----------------------------------------------------------
* FixedWing emergency landing
*-----------------------------------------------------------*/
void applyFixedWingEmergencyLandingController(void)
void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
{
// FIXME: Use altitude controller if available (similar to MC code)
rcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]);
rcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]);
rcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]);
rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
if (posControl.flags.estAltStatus >= EST_USABLE) {
updateClimbRateToAltitudeController(-1.0f * navConfig()->general.emerg_descent_rate, ROC_TO_ALT_NORMAL);
applyFixedWingAltitudeAndThrottleController(currentTimeUs);
int16_t pitchCorrection = constrain(posControl.rcAdjustment[PITCH], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle));
rcCommand[PITCH] = -pidAngleToRcCommand(pitchCorrection, pidProfile()->max_angle_inclination[FD_PITCH]);
} else {
rcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]);
}
}
/*-----------------------------------------------------------
@ -686,7 +695,7 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags,
applyFixedWingLaunchController(currentTimeUs);
}
else if (navStateFlags & NAV_CTL_EMERG) {
applyFixedWingEmergencyLandingController();
applyFixedWingEmergencyLandingController(currentTimeUs);
}
else {
#ifdef NAV_FW_LIMIT_MIN_FLY_VELOCITY
@ -696,8 +705,8 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags,
if (true) {
#endif
if (navStateFlags & NAV_CTL_ALT) {
if (getMotorStatus() == MOTOR_STOPPED_USER) {
// Motor has been stopped by user. Update target altitude and bypass navigation pitch/throttle control
if (getMotorStatus() == MOTOR_STOPPED_USER || FLIGHT_MODE(SOARING_MODE)) {
// Motor has been stopped by user or soaring mode enabled to override altitude control
resetFixedWingAltitudeController();
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);
} else {
@ -722,6 +731,10 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags,
if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_POS)) {
applyFixedWingPitchRollThrottleController(navStateFlags, currentTimeUs);
}
if (FLIGHT_MODE(SOARING_MODE) && navConfig()->general.flags.soaring_motor_stop) {
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
}
}
}
@ -729,5 +742,3 @@ int32_t navigationGetHeadingError(void)
{
return navHeadingError;
}
#endif // NAV