mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-19 14:25:16 +03:00
Add the AT_LEAST_LINEAR_DESCENT RTH altitude mode (#4188)
This commit is contained in:
Michel Pastor
GitHub
parent
5ba4e5b674
commit
644259f936
9 changed files with 105 additions and 66 deletions
|
|
@ -55,6 +55,10 @@
|
|||
#define RADIANS_TO_CENTIDEGREES(angle) (((angle) * 100.0f) / RAD)
|
||||
#define CENTIDEGREES_TO_RADIANS(angle) (((angle) / 100.0f) * RAD)
|
||||
|
||||
#define CENTIMETERS_TO_CENTIFEET(cm) (cm * (328 / 100.0))
|
||||
#define CENTIMETERS_TO_FEET(cm) (cm * (328 / 10000.0))
|
||||
#define CENTIMETERS_TO_METERS(cm) (cm / 100)
|
||||
|
||||
// copied from https://code.google.com/p/cxutil/source/browse/include/cxutil/utility.h#70
|
||||
#define _CHOOSE2(binoper, lexpr, lvar, rexpr, rvar) \
|
||||
( __extension__ ({ \
|
||||
|
|
|
|||
|
|
@ -54,7 +54,7 @@ tables:
|
|||
- name: nav_user_control_mode
|
||||
values: ["ATTI", "CRUISE"]
|
||||
- name: nav_rth_alt_mode
|
||||
values: ["CURRENT", "EXTRA", "FIXED", "MAX", "AT_LEAST"]
|
||||
values: ["CURRENT", "EXTRA", "FIXED", "MAX", "AT_LEAST", "AT_LEAST_LINEAR_DESCENT"]
|
||||
- name: osd_unit
|
||||
values: ["IMPERIAL", "METRIC", "UK"]
|
||||
enum: osd_unit_e
|
||||
|
|
|
|||
|
|
@ -20,6 +20,18 @@
|
|||
|
||||
#if defined(USE_ADC) && defined(USE_GPS)
|
||||
|
||||
/* INPUTS:
|
||||
* - heading degrees
|
||||
* - horizontalWindSpeed
|
||||
* - windHeading degrees
|
||||
* OUTPUT:
|
||||
* returns same unit as horizontalWindSpeed
|
||||
*/
|
||||
static float forwardWindSpeed(float heading, float horizontalWindSpeed, float windHeading) {
|
||||
return horizontalWindSpeed * cos_approx(DEGREES_TO_RADIANS(windHeading - heading));
|
||||
}
|
||||
|
||||
#ifdef USE_WIND_ESTIMATOR
|
||||
/* INPUTS:
|
||||
* - forwardSpeed (same unit as horizontalWindSpeed)
|
||||
* - heading degrees
|
||||
|
|
@ -44,17 +56,6 @@ static float windDriftCorrectedForwardSpeed(float forwardSpeed, float heading, f
|
|||
return forwardSpeed * cos_approx(DEGREES_TO_RADIANS(windDriftCompensationAngle(forwardSpeed, heading, horizontalWindSpeed, windHeading)));
|
||||
}
|
||||
|
||||
/* INPUTS:
|
||||
* - heading degrees
|
||||
* - horizontalWindSpeed
|
||||
* - windHeading degrees
|
||||
* OUTPUT:
|
||||
* returns same unit as horizontalWindSpeed
|
||||
*/
|
||||
static float forwardWindSpeed(float heading, float horizontalWindSpeed, float windHeading) {
|
||||
return horizontalWindSpeed * cos_approx(DEGREES_TO_RADIANS(windHeading - heading));
|
||||
}
|
||||
|
||||
/* INPUTS:
|
||||
* - forwardSpeed (same unit as horizontalWindSpeed)
|
||||
* - heading degrees
|
||||
|
|
@ -66,17 +67,17 @@ static float forwardWindSpeed(float heading, float horizontalWindSpeed, float wi
|
|||
static float windCompensatedForwardSpeed(float forwardSpeed, float heading, float horizontalWindSpeed, float windHeading) {
|
||||
return windDriftCorrectedForwardSpeed(forwardSpeed, heading, horizontalWindSpeed, windHeading) + forwardWindSpeed(heading, horizontalWindSpeed, windHeading);
|
||||
}
|
||||
#endif
|
||||
|
||||
// returns degrees
|
||||
static int8_t RTHAltitudeChangePitchAngle(float altitudeChange) {
|
||||
static int8_t RTHInitialAltitudeChangePitchAngle(float altitudeChange) {
|
||||
return altitudeChange < 0 ? navConfig()->fw.max_dive_angle : -navConfig()->fw.max_climb_angle;
|
||||
}
|
||||
|
||||
// altitudeChange is in meters
|
||||
// idle_power and cruise_power are in deciWatt
|
||||
// output is in Watt
|
||||
static float estimateRTHAltitudeChangePower(float altitudeChange) {
|
||||
uint16_t altitudeChangeThrottle = navConfig()->fw.cruise_throttle - RTHAltitudeChangePitchAngle(altitudeChange) * navConfig()->fw.pitch_to_throttle;
|
||||
// pitch in degrees
|
||||
// output in Watt
|
||||
static float estimatePitchPower(float pitch) {
|
||||
int16_t altitudeChangeThrottle = fixedWingPitchToThrottleCorrection(DEGREES_TO_DECIDEGREES(pitch));
|
||||
altitudeChangeThrottle = constrain(altitudeChangeThrottle, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle);
|
||||
const float altitudeChangeThrToCruiseThrRatio = (float)(altitudeChangeThrottle - motorConfig()->minthrottle) / (navConfig()->fw.cruise_throttle - motorConfig()->minthrottle);
|
||||
return (float)heatLossesCompensatedPower(batteryMetersConfig()->idle_power + batteryMetersConfig()->cruise_power * altitudeChangeThrToCruiseThrRatio) / 100;
|
||||
|
|
@ -88,7 +89,7 @@ static float estimateRTHAltitudeChangePower(float altitudeChange) {
|
|||
// output is in seconds
|
||||
static float estimateRTHAltitudeChangeTime(float altitudeChange, float verticalWindSpeed) {
|
||||
// Assuming increase in throttle keeps air speed at cruise speed
|
||||
const float estimatedVerticalSpeed = (float)navConfig()->fw.cruise_speed / 100 * sin_approx(DEGREES_TO_RADIANS(RTHAltitudeChangePitchAngle(altitudeChange))) + verticalWindSpeed;
|
||||
const float estimatedVerticalSpeed = (float)navConfig()->fw.cruise_speed / 100 * sin_approx(DEGREES_TO_RADIANS(RTHInitialAltitudeChangePitchAngle(altitudeChange))) + verticalWindSpeed;
|
||||
return altitudeChange / estimatedVerticalSpeed;
|
||||
}
|
||||
|
||||
|
|
@ -100,17 +101,22 @@ static float estimateRTHAltitudeChangeTime(float altitudeChange, float verticalW
|
|||
// output is in meters
|
||||
static float estimateRTHAltitudeChangeGroundDistance(float altitudeChange, float horizontalWindSpeed, float windHeading, float verticalWindSpeed) {
|
||||
// Assuming increase in throttle keeps air speed at cruise speed
|
||||
float estimatedHorizontalSpeed = (float)navConfig()->fw.cruise_speed / 100 * cos_approx(DEGREES_TO_RADIANS(RTHAltitudeChangePitchAngle(altitudeChange))) + forwardWindSpeed(DECIDEGREES_TO_DEGREES((float)attitude.values.yaw), horizontalWindSpeed, windHeading);
|
||||
const float estimatedHorizontalSpeed = (float)navConfig()->fw.cruise_speed / 100 * cos_approx(DEGREES_TO_RADIANS(RTHInitialAltitudeChangePitchAngle(altitudeChange))) + forwardWindSpeed(DECIDEGREES_TO_DEGREES((float)attitude.values.yaw), horizontalWindSpeed, windHeading);
|
||||
return estimateRTHAltitudeChangeTime(altitudeChange, verticalWindSpeed) * estimatedHorizontalSpeed;
|
||||
}
|
||||
|
||||
// altitudeChange is in m
|
||||
// verticalWindSpeed is in m/s
|
||||
// output is in Wh
|
||||
static uint16_t estimateRTHAltitudeChangeEnergy(float altitudeChange, float verticalWindSpeed) {
|
||||
return estimateRTHAltitudeChangePower(altitudeChange) * estimateRTHAltitudeChangeTime(altitudeChange, verticalWindSpeed) / 3600;
|
||||
static float estimateRTHInitialAltitudeChangeEnergy(float altitudeChange, float verticalWindSpeed) {
|
||||
const float RTHInitialAltitudeChangePower = estimatePitchPower(altitudeChange < 0 ? navConfig()->fw.max_dive_angle : -navConfig()->fw.max_climb_angle);
|
||||
return RTHInitialAltitudeChangePower * estimateRTHAltitudeChangeTime(altitudeChange, verticalWindSpeed) / 3600;
|
||||
}
|
||||
|
||||
// horizontalWindSpeed is in m/s
|
||||
// windHeading is in degrees
|
||||
// verticalWindSpeed is in m/s
|
||||
// altitudeChange is in m
|
||||
// returns distance in m
|
||||
// *heading is in degrees
|
||||
static float estimateRTHDistanceAndHeadingAfterAltitudeChange(float altitudeChange, float horizontalWindSpeed, float windHeading, float verticalWindSpeed, float *heading) {
|
||||
|
|
@ -128,8 +134,17 @@ static float estimateRTHDistanceAndHeadingAfterAltitudeChange(float altitudeChan
|
|||
}
|
||||
}
|
||||
|
||||
// returns mWh
|
||||
static int32_t calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) {
|
||||
// distanceToHome is in meters
|
||||
// output in Watt
|
||||
static float estimateRTHEnergyAfterInitialClimb(float distanceToHome, float speedToHome) {
|
||||
const float timeToHome = distanceToHome / speedToHome; // seconds
|
||||
const float altitudeChangeDescentToHome = CENTIMETERS_TO_METERS(navConfig()->general.flags.rth_alt_control_mode == NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT ? MAX(0, getEstimatedActualPosition(Z) - RTHAltitude()) : 0);
|
||||
const float pitchToHome = MIN(RADIANS_TO_DEGREES(atan2_approx(altitudeChangeDescentToHome, distanceToHome)), navConfig()->fw.max_dive_angle);
|
||||
return estimatePitchPower(pitchToHome) * timeToHome / 3600;
|
||||
}
|
||||
|
||||
// returns Wh
|
||||
static float calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) {
|
||||
// Fixed wing only for now
|
||||
if (!STATE(FIXED_WING))
|
||||
return -1;
|
||||
|
|
@ -141,35 +156,40 @@ static int32_t calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) {
|
|||
))
|
||||
return -1;
|
||||
|
||||
const float RTH_initial_altitude_change = MAX(0, (RTHAltitude() - getEstimatedActualPosition(Z)) / 100);
|
||||
|
||||
float RTH_heading; // degrees
|
||||
#ifdef USE_WIND_ESTIMATOR
|
||||
uint16_t windHeading; // centidegrees
|
||||
const float horizontalWindSpeed = takeWindIntoAccount ? getEstimatedHorizontalWindSpeed(&windHeading) / 100 : 0; // m/s
|
||||
const float windHeadingDegrees = CENTIDEGREES_TO_DEGREES((float)windHeading);
|
||||
const float verticalWindSpeed = getEstimatedWindSpeed(Z) / 100;
|
||||
|
||||
const float RTH_distance = estimateRTHDistanceAndHeadingAfterAltitudeChange(RTH_initial_altitude_change, horizontalWindSpeed, windHeadingDegrees, verticalWindSpeed, &RTH_heading);
|
||||
const float RTH_speed = windCompensatedForwardSpeed((float)navConfig()->fw.cruise_speed / 100, RTH_heading, horizontalWindSpeed, windHeadingDegrees);
|
||||
#else
|
||||
UNUSED(takeWindIntoAccount);
|
||||
const float horizontalWindSpeed = 0; // m/s
|
||||
const float windHeadingDegrees = 0;
|
||||
const float verticalWindSpeed = 0;
|
||||
const float RTH_distance = estimateRTHDistanceAndHeadingAfterAltitudeChange(RTH_initial_altitude_change, 0, 0, 0, &RTH_heading);
|
||||
const float RTH_speed = (float)navConfig()->fw.cruise_speed / 100;
|
||||
#endif
|
||||
|
||||
const float RTH_altitude_change = (RTHAltitude() - getEstimatedActualPosition(Z)) / 100;
|
||||
float RTH_heading; // degrees
|
||||
const float RTH_distance = estimateRTHDistanceAndHeadingAfterAltitudeChange(RTH_altitude_change, horizontalWindSpeed, windHeadingDegrees, verticalWindSpeed, &RTH_heading);
|
||||
const float RTH_speed = windCompensatedForwardSpeed((float)navConfig()->fw.cruise_speed / 100, DECIDEGREES_TO_DEGREES(attitude.values.yaw), horizontalWindSpeed, windHeadingDegrees);
|
||||
|
||||
DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 0, lrintf(RTH_altitude_change * 100));
|
||||
DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 0, lrintf(RTH_initial_altitude_change * 100));
|
||||
DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 1, lrintf(RTH_distance * 100));
|
||||
DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 2, lrintf(RTH_speed * 100));
|
||||
#ifdef USE_WIND_ESTIMATOR
|
||||
DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 3, lrintf(horizontalWindSpeed * 100));
|
||||
#endif
|
||||
|
||||
if (RTH_speed <= 0)
|
||||
return -2; // wind is too strong
|
||||
return -2; // wind is too strong to return at cruise throttle (TODO: might be possible to take into account min speed thr boost)
|
||||
|
||||
const uint32_t time_to_home = RTH_distance / RTH_speed; // seconds
|
||||
const uint32_t energy_to_home = estimateRTHAltitudeChangeEnergy(RTH_altitude_change, verticalWindSpeed) * 1000 + heatLossesCompensatedPower(batteryMetersConfig()->idle_power + batteryMetersConfig()->cruise_power) * time_to_home / 360; // mWh
|
||||
const uint32_t energy_margin_abs = (currentBatteryProfile->capacity.value - currentBatteryProfile->capacity.critical) * batteryMetersConfig()->rth_energy_margin / 100; // mWh
|
||||
const int32_t remaining_energy_before_rth = getBatteryRemainingCapacity() - energy_margin_abs - energy_to_home; // mWh
|
||||
#ifdef USE_WIND_ESTIMATOR
|
||||
const float energy_to_home = estimateRTHInitialAltitudeChangeEnergy(RTH_initial_altitude_change, verticalWindSpeed) + estimateRTHEnergyAfterInitialClimb(RTH_distance, RTH_speed); // Wh
|
||||
#else
|
||||
const float energy_to_home = estimateRTHInitialAltitudeChangeEnergy(RTH_initial_altitude_change, 0) + estimateRTHEnergyAfterInitialClimb(RTH_distance, RTH_speed); // Wh
|
||||
#endif
|
||||
const float energy_margin_abs = (currentBatteryProfile->capacity.value - currentBatteryProfile->capacity.critical) * batteryMetersConfig()->rth_energy_margin / 100000; // Wh
|
||||
const float remaining_energy_before_rth = getBatteryRemainingCapacity() / 1000 - energy_margin_abs - energy_to_home; // Wh
|
||||
|
||||
if (remaining_energy_before_rth < 0) // No energy left = No time left
|
||||
return 0;
|
||||
|
|
@ -178,31 +198,28 @@ static int32_t calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) {
|
|||
}
|
||||
|
||||
// returns seconds
|
||||
int32_t calculateRemainingFlightTimeBeforeRTH(bool takeWindIntoAccount) {
|
||||
float calculateRemainingFlightTimeBeforeRTH(bool takeWindIntoAccount) {
|
||||
|
||||
const int32_t remainingEnergyBeforeRTH = calculateRemainingEnergyBeforeRTH(takeWindIntoAccount);
|
||||
const float remainingEnergyBeforeRTH = calculateRemainingEnergyBeforeRTH(takeWindIntoAccount);
|
||||
|
||||
// error: return error code directly
|
||||
if (remainingEnergyBeforeRTH < 0)
|
||||
return remainingEnergyBeforeRTH;
|
||||
|
||||
const int32_t averagePower = calculateAveragePower();
|
||||
const float averagePower = (float)calculateAveragePower() / 100;
|
||||
|
||||
if (averagePower == 0)
|
||||
return -1;
|
||||
|
||||
const uint32_t time_before_rth = remainingEnergyBeforeRTH * 360 / averagePower;
|
||||
|
||||
if (time_before_rth > 0x7FFFFFFF) // int32 overflow
|
||||
return -1;
|
||||
const float time_before_rth = remainingEnergyBeforeRTH * 3600 / averagePower;
|
||||
|
||||
return time_before_rth;
|
||||
}
|
||||
|
||||
// returns meters
|
||||
int32_t calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount) {
|
||||
float calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount) {
|
||||
|
||||
const int32_t remainingFlightTimeBeforeRTH = calculateRemainingFlightTimeBeforeRTH(takeWindIntoAccount);
|
||||
const float remainingFlightTimeBeforeRTH = calculateRemainingFlightTimeBeforeRTH(takeWindIntoAccount);
|
||||
|
||||
// error: return error code directly
|
||||
if (remainingFlightTimeBeforeRTH < 0)
|
||||
|
|
|
|||
|
|
@ -1,5 +1,5 @@
|
|||
|
||||
#if defined(USE_ADC) && defined(USE_GPS)
|
||||
int32_t calculateRemainingFlightTimeBeforeRTH(bool takeWindIntoAccount);
|
||||
int32_t calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount);
|
||||
float calculateRemainingFlightTimeBeforeRTH(bool takeWindIntoAccount);
|
||||
float calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount);
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -98,9 +98,6 @@
|
|||
#define VIDEO_BUFFER_CHARS_PAL 480
|
||||
#define IS_DISPLAY_PAL (displayScreenSize(osdDisplayPort) == VIDEO_BUFFER_CHARS_PAL)
|
||||
|
||||
#define CENTIMETERS_TO_CENTIFEET(cm) (cm * (328 / 100.0))
|
||||
#define CENTIMETERS_TO_FEET(cm) (cm * (328 / 10000.0))
|
||||
#define CENTIMETERS_TO_METERS(cm) (cm / 100)
|
||||
#define FEET_PER_MILE 5280
|
||||
#define FEET_PER_KILOFEET 1000 // Used for altitude
|
||||
#define METERS_PER_KILOMETER 1000
|
||||
|
|
|
|||
|
|
@ -1119,6 +1119,15 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
|
|||
initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos);
|
||||
}
|
||||
|
||||
posControl.rthInitialHomeDistance = posControl.homeDistance;
|
||||
|
||||
if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING)) {
|
||||
setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
|
||||
}
|
||||
else {
|
||||
setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
|
||||
}
|
||||
|
||||
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HEAD_HOME
|
||||
}
|
||||
else {
|
||||
|
|
@ -1144,8 +1153,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
|
|||
|
||||
if (navConfig()->general.flags.rth_tail_first) {
|
||||
setDesiredPosition(&pos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
|
||||
}
|
||||
else {
|
||||
} else {
|
||||
setDesiredPosition(&pos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
|
||||
}
|
||||
}
|
||||
|
|
@ -1179,13 +1187,17 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
|
|||
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
|
||||
}
|
||||
else {
|
||||
// Update XYZ-position target
|
||||
if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING)) {
|
||||
setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
|
||||
}
|
||||
else {
|
||||
setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
|
||||
if (navConfig()->general.flags.rth_alt_control_mode == NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT) {
|
||||
fpVector3_t pos;
|
||||
uint16_t loiterDistanceFromHome = STATE(FIXED_WING) ? navConfig()->fw.loiter_radius : 0;
|
||||
uint32_t distanceToLoiterToTravelFromRTHStart = posControl.rthInitialHomeDistance - loiterDistanceFromHome;
|
||||
uint32_t distanceToLoiterTraveled = constrain((int32_t)posControl.rthInitialHomeDistance - posControl.homeDistance, 0, distanceToLoiterToTravelFromRTHStart);
|
||||
float RTHStartAltitude = posControl.homeWaypointAbove.pos.z;
|
||||
float RTHFinalAltitude = posControl.homePosition.pos.z + navConfig()->general.rth_altitude;
|
||||
pos.z = RTHStartAltitude - scaleRange(distanceToLoiterTraveled, 0, distanceToLoiterToTravelFromRTHStart, 0, RTHStartAltitude - RTHFinalAltitude);
|
||||
setDesiredPosition(&pos, 0, NAV_POS_UPDATE_Z);
|
||||
}
|
||||
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -68,6 +68,8 @@ enum {
|
|||
NAV_RTH_CONST_ALT = 2, // Climb/descend to predefined altitude
|
||||
NAV_RTH_MAX_ALT = 3, // Track maximum altitude and climb to it when RTH
|
||||
NAV_RTH_AT_LEAST_ALT = 4, // Climb to predefined altitude if below it
|
||||
NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT = 5, // Climb to predefined altitude if below it,
|
||||
// descend linearly to reach home at predefined altitude if above it
|
||||
};
|
||||
|
||||
enum {
|
||||
|
|
@ -400,6 +402,7 @@ bool loadNonVolatileWaypointList(void);
|
|||
bool saveNonVolatileWaypointList(void);
|
||||
|
||||
float RTHAltitude(void);
|
||||
int16_t fixedWingPitchToThrottleCorrection(int16_t pitch);
|
||||
|
||||
/* Geodetic functions */
|
||||
typedef enum {
|
||||
|
|
|
|||
|
|
@ -432,6 +432,11 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs)
|
|||
return throttleSpeedAdjustment;
|
||||
}
|
||||
|
||||
int16_t fixedWingPitchToThrottleCorrection(int16_t pitch)
|
||||
{
|
||||
return DECIDEGREES_TO_DEGREES(pitch) * navConfig()->fw.pitch_to_throttle;
|
||||
}
|
||||
|
||||
void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs)
|
||||
{
|
||||
int16_t minThrottleCorrection = navConfig()->fw.min_throttle - navConfig()->fw.cruise_throttle;
|
||||
|
|
@ -447,7 +452,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
|
|||
// PITCH >0 dive, <0 climb
|
||||
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]);
|
||||
int16_t throttleCorrection = DECIDEGREES_TO_DEGREES(pitchCorrection) * navConfig()->fw.pitch_to_throttle;
|
||||
int16_t throttleCorrection = fixedWingPitchToThrottleCorrection(pitchCorrection);
|
||||
|
||||
#ifdef NAV_FIXED_WING_LANDING
|
||||
if (navStateFlags & NAV_CTL_LAND) {
|
||||
|
|
|
|||
|
|
@ -324,6 +324,7 @@ typedef struct {
|
|||
navWaypointPosition_t homePosition; // Special waypoint, stores original yaw (heading when launched)
|
||||
navWaypointPosition_t homeWaypointAbove; // NEU-coordinates and initial bearing + desired RTH altitude
|
||||
navigationHomeFlags_t homeFlags;
|
||||
uint32_t rthInitialHomeDistance; // Distance to home after RTH has been initiated and the initial climb/descent is done
|
||||
|
||||
uint32_t homeDistance; // cm
|
||||
int32_t homeDirection; // deg*100
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue