mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-24 16:55:29 +03:00
Add preprocessor definitions
This commit is contained in:
Scavanger
parent
0871ef3612
commit
d48971a48f
11 changed files with 133 additions and 31 deletions
|
|
@ -1309,6 +1309,7 @@ static void cliTempSensor(char *cmdline)
|
|||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
static void printFwAutolandApproach(uint8_t dumpMask, const navFwAutolandApproach_t *navFwAutolandApproach, const navFwAutolandApproach_t *defaultFwAutolandApproach)
|
||||
{
|
||||
const char *format = "fwapproach %u %d %d %u %d %d %u";
|
||||
|
|
@ -1410,6 +1411,7 @@ static void cliFwAutolandApproach(char * cmdline)
|
|||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(USE_SAFE_HOME)
|
||||
static void printSafeHomes(uint8_t dumpMask, const navSafeHome_t *navSafeHome, const navSafeHome_t *defaultSafeHome)
|
||||
|
|
@ -3901,8 +3903,10 @@ static void printConfig(const char *cmdline, bool doDiff)
|
|||
printSafeHomes(dumpMask, safeHomeConfig_CopyArray, safeHomeConfig(0));
|
||||
#endif
|
||||
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
cliPrintHashLine("Fixed Wing Approach");
|
||||
printFwAutolandApproach(dumpMask, fwAutolandApproachConfig_CopyArray, fwAutolandApproachConfig(0));
|
||||
#endif
|
||||
|
||||
cliPrintHashLine("features");
|
||||
printFeature(dumpMask, &featureConfig_Copy, featureConfig());
|
||||
|
|
@ -4148,7 +4152,9 @@ const clicmd_t cmdTable[] = {
|
|||
CLI_COMMAND_DEF("flash_write", NULL, "<address> <message>", cliFlashWrite),
|
||||
#endif
|
||||
#endif
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
CLI_COMMAND_DEF("fwapproach", "Fixed Wing Approach Settings", NULL, cliFwAutolandApproach),
|
||||
#endif
|
||||
CLI_COMMAND_DEF("get", "get variable value", "[name]", cliGet),
|
||||
#ifdef USE_GPS
|
||||
CLI_COMMAND_DEF("gpspassthrough", "passthrough gps to serial", NULL, cliGpsPassthrough),
|
||||
|
|
|
|||
|
|
@ -1660,6 +1660,7 @@ static mspResult_e mspFcSafeHomeOutCommand(sbuf_t *dst, sbuf_t *src)
|
|||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
static mspResult_e mspFwApproachOutCommand(sbuf_t *dst, sbuf_t *src)
|
||||
{
|
||||
const uint8_t idx = sbufReadU8(src);
|
||||
|
|
@ -1676,6 +1677,7 @@ static mspResult_e mspFwApproachOutCommand(sbuf_t *dst, sbuf_t *src)
|
|||
return MSP_RESULT_ERROR;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
static mspResult_e mspFcLogicConditionCommand(sbuf_t *dst, sbuf_t *src) {
|
||||
const uint8_t idx = sbufReadU8(src);
|
||||
|
|
@ -2668,7 +2670,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
|
|||
|
||||
case MSP_SET_WP:
|
||||
if (dataSize == 21) {
|
||||
static uint8_t mmIdx = 0;
|
||||
|
||||
const uint8_t msp_wp_no = sbufReadU8(src); // get the waypoint number
|
||||
navWaypoint_t msp_wp;
|
||||
msp_wp.action = sbufReadU8(src); // action
|
||||
|
|
@ -2681,7 +2683,8 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
|
|||
msp_wp.flag = sbufReadU8(src); // future: to set nav flag
|
||||
setWaypoint(msp_wp_no, &msp_wp);
|
||||
|
||||
uint8_t fwAppraochStartIdx = 8;
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
static uint8_t mmIdx = 0, fwAppraochStartIdx = 8;
|
||||
#ifdef USE_SAFE_HOME
|
||||
fwAppraochStartIdx = MAX_SAFE_HOMES;
|
||||
#endif
|
||||
|
|
@ -2691,8 +2694,11 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
|
|||
mmIdx++;
|
||||
}
|
||||
resetFwAutolandApproach(fwAppraochStartIdx + mmIdx);
|
||||
} else
|
||||
#endif
|
||||
} else {
|
||||
return MSP_RESULT_ERROR;
|
||||
}
|
||||
|
||||
break;
|
||||
case MSP2_COMMON_SET_RADAR_POS:
|
||||
if (dataSize == 19) {
|
||||
|
|
@ -3149,13 +3155,16 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
|
|||
safeHomeConfigMutable(i)->enabled = sbufReadU8(src);
|
||||
safeHomeConfigMutable(i)->lat = sbufReadU32(src);
|
||||
safeHomeConfigMutable(i)->lon = sbufReadU32(src);
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
resetFwAutolandApproach(i);
|
||||
#endif
|
||||
} else {
|
||||
return MSP_RESULT_ERROR;
|
||||
}
|
||||
break;
|
||||
#endif
|
||||
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
case MSP2_INAV_SET_FW_APPROACH:
|
||||
if (dataSize == 15) {
|
||||
uint8_t i;
|
||||
|
|
@ -3178,6 +3187,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
|
|||
return MSP_RESULT_ERROR;
|
||||
}
|
||||
break;
|
||||
#endif
|
||||
|
||||
#ifdef USE_EZ_TUNE
|
||||
|
||||
|
|
@ -3691,10 +3701,11 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
|
|||
*ret = mspFcSafeHomeOutCommand(dst, src);
|
||||
break;
|
||||
#endif
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
case MSP2_INAV_FW_APPROACH:
|
||||
*ret = mspFwApproachOutCommand(dst, src);
|
||||
break;
|
||||
|
||||
#endif
|
||||
#ifdef USE_SIMULATOR
|
||||
case MSP_SIMULATOR:
|
||||
tmp_u8 = sbufReadU8(src); // Get the Simulator MSP version
|
||||
|
|
|
|||
|
|
@ -4093,7 +4093,7 @@ groups:
|
|||
- name: PG_FW_AUTOLAND_CONFIG
|
||||
type: navFwAutolandConfig_t
|
||||
headers: ["navigation/navigation.h"]
|
||||
condition: USE_SAFE_HOME
|
||||
condition: USE_FW_AUTOLAND
|
||||
members:
|
||||
- name: nav_fw_land_approach_length
|
||||
description: "Length of the final approach"
|
||||
|
|
|
|||
|
|
@ -593,7 +593,11 @@ static float computePidLevelTarget(flight_dynamics_index_t axis) {
|
|||
#endif
|
||||
|
||||
// Automatically pitch down if the throttle is manually controlled and reduced bellow cruise throttle
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle() && !isFwLandInProgess()) {
|
||||
#else
|
||||
if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle()) {
|
||||
#endif
|
||||
angleTarget += scaleRange(MAX(0, currentBatteryProfile->nav.fw.cruise_throttle - rcCommand[THROTTLE]), 0, currentBatteryProfile->nav.fw.cruise_throttle - PWM_RANGE_MIN, 0, navConfig()->fw.minThrottleDownPitchAngle);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -1004,15 +1004,21 @@ static const char * osdFailsafeInfoMessage(void)
|
|||
}
|
||||
return OSD_MESSAGE_STR(OSD_MSG_RC_RX_LINK_LOST);
|
||||
}
|
||||
|
||||
#if defined(USE_SAFE_HOME)
|
||||
static const char * divertingToSafehomeMessage(void)
|
||||
{
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
if (!posControl.fwLandState.landWp && (NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && posControl.safehomeState.isApplied)) {
|
||||
#else
|
||||
if (NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && posControl.safehomeState.isApplied) {
|
||||
#endif
|
||||
return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME);
|
||||
}
|
||||
#endif
|
||||
return NULL;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
static const char * navigationStateMessage(void)
|
||||
{
|
||||
|
|
@ -2269,9 +2275,12 @@ static bool osdDrawSingleElement(uint8_t item)
|
|||
case OSD_FLYMODE:
|
||||
{
|
||||
char *p = "ACRO";
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
if (isFwLandInProgess())
|
||||
p = "LAND";
|
||||
else if (FLIGHT_MODE(FAILSAFE_MODE))
|
||||
else
|
||||
#endif
|
||||
if (FLIGHT_MODE(FAILSAFE_MODE))
|
||||
p = "!FS!";
|
||||
else if (FLIGHT_MODE(MANUAL_MODE))
|
||||
p = "MANU";
|
||||
|
|
@ -5134,8 +5143,13 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
|
|||
const char *invertedInfoMessage = NULL;
|
||||
|
||||
if (ARMING_FLAG(ARMED)) {
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding() || isFwLandInProgess()) {
|
||||
if (isWaypointMissionRTHActive() && !posControl.fwLandState.landWp) {
|
||||
#else
|
||||
if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
|
||||
if (isWaypointMissionRTHActive()) {
|
||||
#endif
|
||||
// if RTH activated whilst WP mode selected, remind pilot to cancel WP mode to exit RTH
|
||||
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_WP_RTH_CANCEL);
|
||||
}
|
||||
|
|
@ -5165,16 +5179,23 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
|
|||
tfp_sprintf(messageBuf, "LANDING DELAY: %3u SECONDS", remainingHoldSec);
|
||||
|
||||
messages[messageCount++] = messageBuf;
|
||||
} else {
|
||||
}
|
||||
|
||||
else {
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
if (canFwLandCanceld()) {
|
||||
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_MOVE_STICKS);
|
||||
} else if (!isFwLandInProgess()) {
|
||||
#endif
|
||||
const char *navStateMessage = navigationStateMessage();
|
||||
if (navStateMessage) {
|
||||
messages[messageCount++] = navStateMessage;
|
||||
}
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
#if defined(USE_SAFE_HOME)
|
||||
const char *safehomeMessage = divertingToSafehomeMessage();
|
||||
if (safehomeMessage) {
|
||||
|
|
|
|||
|
|
@ -90,11 +90,11 @@ int16_t GPS_directionToHome; // direction to home point in degrees
|
|||
|
||||
radar_pois_t radar_pois[RADAR_MAX_POIS];
|
||||
|
||||
#if defined(USE_SAFE_HOME)
|
||||
PG_REGISTER_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig, PG_SAFE_HOME_CONFIG , 0);
|
||||
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(navFwAutolandConfig_t, navFwAutolandConfig, PG_FW_AUTOLAND_CONFIG, 0);
|
||||
|
||||
PG_REGISTER_ARRAY(navFwAutolandApproach_t, MAX_FW_LAND_APPOACH_SETTINGS, fwAutolandApproachConfig, PG_FW_AUTOLAND_APPROACH_CONFIG, 0);
|
||||
|
||||
PG_RESET_TEMPLATE(navFwAutolandConfig_t, navFwAutolandConfig,
|
||||
.approachLength = SETTING_NAV_FW_LAND_APPROACH_LENGTH_DEFAULT,
|
||||
.finalApproachPitchToThrottleMod = SETTING_NAV_FW_LAND_FINAL_APPROACH_PITCH2THROTTLE_MOD_DEFAULT,
|
||||
|
|
@ -104,9 +104,10 @@ PG_RESET_TEMPLATE(navFwAutolandConfig_t, navFwAutolandConfig,
|
|||
.maxTailwind = SETTING_NAV_FW_LAND_MAX_TAILWIND_DEFAULT,
|
||||
.glidePitch = SETTING_NAV_FW_LAND_GLIDE_PITCH_DEFAULT,
|
||||
);
|
||||
#endif
|
||||
|
||||
PG_REGISTER_ARRAY(navFwAutolandApproach_t, MAX_FW_LAND_APPOACH_SETTINGS, fwAutolandApproachConfig, PG_FW_AUTOLAND_APPROACH_CONFIG, 0);
|
||||
|
||||
#if defined(USE_SAFE_HOME)
|
||||
PG_REGISTER_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig, PG_SAFE_HOME_CONFIG , 0);
|
||||
#endif
|
||||
|
||||
// waypoint 254, 255 are special waypoints
|
||||
|
|
@ -270,7 +271,11 @@ static void resetAltitudeController(bool useTerrainFollowing);
|
|||
static void resetPositionController(void);
|
||||
static void setupAltitudeController(void);
|
||||
static void resetHeadingController(void);
|
||||
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
static void resetFwAutoland(void);
|
||||
#endif
|
||||
|
||||
void resetGCSFlags(void);
|
||||
|
||||
static void setupJumpCounters(void);
|
||||
|
|
@ -299,10 +304,12 @@ static bool rthAltControlStickOverrideCheck(unsigned axis);
|
|||
static void updateRthTrackback(bool forceSaveTrackPoint);
|
||||
static fpVector3_t * rthGetTrackbackPos(void);
|
||||
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
static float getLandAltitude(void);
|
||||
static int32_t calcWindDiff(int32_t heading, int32_t windHeading);
|
||||
static int32_t calcFinalApproachHeading(int32_t approachHeading, int32_t windAngle);
|
||||
static void setLandWaypoint(const fpVector3_t *pos, const fpVector3_t *nextWpPos);
|
||||
#endif
|
||||
|
||||
/*************************************************************************************************/
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState_t previousState);
|
||||
|
|
@ -342,12 +349,14 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS(navi
|
|||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_INITIALIZE(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_ABORT(navigationFSMState_t previousState);
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_CLIMB_TO_LOITER(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_LOITER(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FLARE(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(navigationFSMState_t previousState);
|
||||
#endif
|
||||
|
||||
static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
||||
/** Idle state ******************************************************/
|
||||
|
|
@ -1036,6 +1045,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
}
|
||||
},
|
||||
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
[NAV_STATE_FW_LANDING_CLIMB_TO_LOITER] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_FW_LANDING_CLIMB_TO_LOITER,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_CLIMB_TO_LOITER,
|
||||
|
|
@ -1150,6 +1160,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
|
||||
}
|
||||
},
|
||||
#endif
|
||||
};
|
||||
|
||||
static navigationFSMStateFlags_t navGetStateFlags(navigationFSMState_t state)
|
||||
|
|
@ -1181,7 +1192,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState
|
|||
resetAltitudeController(false);
|
||||
resetHeadingController();
|
||||
resetPositionController();
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
resetFwAutoland();
|
||||
#endif
|
||||
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
|
|
@ -1391,7 +1404,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
|
|||
}
|
||||
|
||||
if (previousState != NAV_STATE_FW_LANDING_ABORT) {
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
posControl.fwLandState.landAborted = false;
|
||||
#endif
|
||||
if (STATE(FIXED_WING_LEGACY) && (posControl.homeDistance < navConfig()->general.min_rth_distance) && !posControl.flags.forcedRTHActivated) {
|
||||
// Prevent RTH from activating on airplanes if too close to home unless it's a failsafe RTH
|
||||
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
|
||||
|
|
@ -1681,6 +1696,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(na
|
|||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationFSMState_t previousState)
|
||||
{
|
||||
#ifndef USE_FW_AUTOLAND
|
||||
UNUSED(previousState);
|
||||
#endif
|
||||
|
||||
//On ROVER and BOAT we immediately switch to the next event
|
||||
if (!STATE(ALTITUDE_CONTROL)) {
|
||||
|
|
@ -1701,6 +1719,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
|
|||
return NAV_FSM_EVENT_SWITCH_TO_MIXERAT;
|
||||
}
|
||||
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
if (STATE(AIRPLANE)) {
|
||||
int8_t missionIdx = -1, shIdx = -1, missionFwLandConfigStartIdx = 8;
|
||||
#ifdef USE_MULTI_MISSION
|
||||
|
|
@ -1728,6 +1747,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
|
|||
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
float descentVelLimited = 0;
|
||||
|
||||
|
|
@ -2054,7 +2074,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITI
|
|||
{
|
||||
UNUSED(previousState);
|
||||
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
|
||||
#endif
|
||||
|
||||
if ((posControl.flags.estPosStatus >= EST_USABLE)) {
|
||||
resetPositionController();
|
||||
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, 0, NAV_POS_UPDATE_XY);
|
||||
|
|
@ -2202,6 +2225,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_ABORT(navigatio
|
|||
return NAV_FSM_EVENT_SUCCESS;
|
||||
}
|
||||
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_CLIMB_TO_LOITER(navigationFSMState_t previousState)
|
||||
{
|
||||
UNUSED(previousState);
|
||||
|
|
@ -2416,6 +2440,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(naviga
|
|||
|
||||
return posControl.fwLandState.landWp ? NAV_FSM_EVENT_SWITCH_TO_WAYPOINT : NAV_FSM_EVENT_SWITCH_TO_RTH;
|
||||
}
|
||||
#endif
|
||||
|
||||
static navigationFSMState_t navSetNewFSMState(navigationFSMState_t newState)
|
||||
{
|
||||
|
|
@ -4146,9 +4171,11 @@ void applyWaypointNavigationAndAltitudeHold(void)
|
|||
posControl.flags.horizontalPositionDataConsumed = false;
|
||||
posControl.flags.verticalPositionDataConsumed = false;
|
||||
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
if (!isFwLandInProgess()) {
|
||||
posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
|
||||
}
|
||||
#endif
|
||||
|
||||
/* Process controllers */
|
||||
navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
|
||||
|
|
@ -5029,6 +5056,8 @@ uint8_t getActiveWpNumber(void)
|
|||
return NAV_Status.activeWpNumber;
|
||||
}
|
||||
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
|
||||
static void resetFwAutoland(void)
|
||||
{
|
||||
posControl.fwLandState.landAltAgl = 0;
|
||||
|
|
@ -5122,3 +5151,5 @@ bool canFwLandCanceld(void)
|
|||
|| posControl.navState == NAV_STATE_FW_LANDING_APPROACH
|
||||
|| posControl.navState == NAV_STATE_FW_LANDING_GLIDE;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -41,18 +41,38 @@ extern bool autoThrottleManuallyIncreased;
|
|||
|
||||
/* Navigation system updates */
|
||||
void onNewGPSData(void);
|
||||
|
||||
#if defined(USE_SAFE_HOME)
|
||||
|
||||
#define MAX_SAFE_HOMES 8
|
||||
#define MAX_FW_LAND_APPOACH_SETTINGS (MAX_SAFE_HOMES + 9)
|
||||
|
||||
typedef struct {
|
||||
uint8_t enabled;
|
||||
int32_t lat;
|
||||
int32_t lon;
|
||||
int8_t fwLandSettingsIdx;
|
||||
} navSafeHome_t;
|
||||
|
||||
typedef enum {
|
||||
SAFEHOME_USAGE_OFF = 0, // Don't use safehomes
|
||||
SAFEHOME_USAGE_RTH = 1, // Default - use safehome for RTH
|
||||
SAFEHOME_USAGE_RTH_FS = 2, // Use safehomes for RX failsafe only
|
||||
} safehomeUsageMode_e;
|
||||
|
||||
PG_DECLARE_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig);
|
||||
|
||||
void resetSafeHomes(void); // remove all safehomes
|
||||
bool findNearestSafeHome(void); // Find nearest safehome
|
||||
|
||||
#endif // defined(USE_SAFE_HOME)
|
||||
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
|
||||
#ifndef MAX_SAFE_HOMES
|
||||
#define MAX_SAFE_HOMES 0
|
||||
#endif
|
||||
|
||||
#define MAX_FW_LAND_APPOACH_SETTINGS (MAX_SAFE_HOMES + 9)
|
||||
|
||||
typedef enum {
|
||||
FW_AUTOLAND_APPROACH_DIRECTION_LEFT,
|
||||
FW_AUTOLAND_APPROACH_DIRECTION_RIGHT
|
||||
|
|
@ -79,14 +99,6 @@ typedef struct {
|
|||
|
||||
PG_DECLARE_ARRAY(navFwAutolandApproach_t, MAX_FW_LAND_APPOACH_SETTINGS, fwAutolandApproachConfig);
|
||||
|
||||
typedef enum {
|
||||
SAFEHOME_USAGE_OFF = 0, // Don't use safehomes
|
||||
SAFEHOME_USAGE_RTH = 1, // Default - use safehome for RTH
|
||||
SAFEHOME_USAGE_RTH_FS = 2, // Use safehomes for RX failsafe only
|
||||
} safehomeUsageMode_e;
|
||||
|
||||
PG_DECLARE_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig);
|
||||
|
||||
typedef struct navFwAutolandConfig_s
|
||||
{
|
||||
uint32_t approachLength;
|
||||
|
|
@ -101,11 +113,8 @@ typedef struct navFwAutolandConfig_s
|
|||
PG_DECLARE(navFwAutolandConfig_t, navFwAutolandConfig);
|
||||
|
||||
void resetFwAutolandApproach(int8_t idx);
|
||||
void resetSafeHomes(void); // remove all safehomes
|
||||
bool findNearestSafeHome(void); // Find nearest safehome
|
||||
|
||||
|
||||
#endif // defined(USE_SAFE_HOME)
|
||||
#endif
|
||||
|
||||
#ifndef NAV_MAX_WAYPOINTS
|
||||
#define NAV_MAX_WAYPOINTS 15
|
||||
|
|
@ -683,8 +692,10 @@ uint8_t getActiveWpNumber(void);
|
|||
*/
|
||||
int32_t navigationGetHomeHeading(void);
|
||||
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
bool isFwLandInProgess(void);
|
||||
bool canFwLandCanceld(void);
|
||||
#endif
|
||||
|
||||
/* Compatibility data */
|
||||
extern navSystemStatus_t NAV_Status;
|
||||
|
|
|
|||
|
|
@ -589,9 +589,12 @@ int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs
|
|||
int16_t filteredPitch = (int16_t)pt1FilterApply4(&pitchToThrFilterState, pitch, getPitchToThrottleSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicrosPitchToThrCorr));
|
||||
|
||||
int16_t pitchToThrottle = currentBatteryProfile->nav.fw.pitch_to_throttle;
|
||||
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
if (pitch < 0 && posControl.fwLandState.landState == FW_AUTOLAND_STATE_FINAL_APPROACH) {
|
||||
pitchToThrottle *= navFwAutolandConfig()->finalApproachPitchToThrottleMod / 100.0f;
|
||||
}
|
||||
#endif
|
||||
|
||||
if (ABS(pitch - filteredPitch) > navConfig()->fw.pitch_to_throttle_thresh) {
|
||||
// Unfiltered throttle correction outside of pitch deadband
|
||||
|
|
@ -640,7 +643,11 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
|
|||
uint16_t correctedThrottleValue = constrain(currentBatteryProfile->nav.fw.cruise_throttle + throttleCorrection, currentBatteryProfile->nav.fw.min_throttle, currentBatteryProfile->nav.fw.max_throttle);
|
||||
|
||||
// Manual throttle increase
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
if (navConfig()->fw.allow_manual_thr_increase && !FLIGHT_MODE(FAILSAFE_MODE) && !isFwLandInProgess()) {
|
||||
#else
|
||||
if (navConfig()->fw.allow_manual_thr_increase && !FLIGHT_MODE(FAILSAFE_MODE)) {
|
||||
#endif
|
||||
if (rcCommand[THROTTLE] < PWM_RANGE_MIN + (PWM_RANGE_MAX - PWM_RANGE_MIN) * 0.95){
|
||||
correctedThrottleValue += MAX(0, rcCommand[THROTTLE] - currentBatteryProfile->nav.fw.cruise_throttle);
|
||||
} else {
|
||||
|
|
@ -654,6 +661,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
|
|||
rcCommand[THROTTLE] = setDesiredThrottle(correctedThrottleValue, false);
|
||||
}
|
||||
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
// Advanced autoland
|
||||
if (posControl.navState == NAV_STATE_FW_LANDING_GLIDE || posControl.navState == NAV_STATE_FW_LANDING_FLARE || STATE(LANDING_DETECTED)) {
|
||||
// Set motor to min. throttle and stop it when MOTOR_STOP feature is enabled
|
||||
|
|
@ -668,7 +676,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
|
|||
rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(navFwAutolandConfig()->flarePitch), pidProfile()->max_angle_inclination[FD_PITCH]);
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
// "Traditional" landing as fallback option
|
||||
if (navStateFlags & NAV_CTL_LAND) {
|
||||
int32_t finalAltitude = navConfig()->general.land_slowdown_minalt + posControl.rthState.homeTmpWaypoint.z;
|
||||
|
|
|
|||
|
|
@ -368,6 +368,7 @@ typedef struct {
|
|||
bool rthLinearDescentActive; // Activation status of Linear Descent
|
||||
} rthState_t;
|
||||
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
typedef enum {
|
||||
FW_AUTOLAND_WP_TURN,
|
||||
FW_AUTOLAND_WP_FINAL_APPROACH,
|
||||
|
|
@ -389,6 +390,7 @@ typedef struct {
|
|||
bool landWp;
|
||||
fwAutolandState_t landState;
|
||||
} fwLandState_t;
|
||||
#endif
|
||||
|
||||
typedef enum {
|
||||
RTH_HOME_ENROUTE_INITIAL, // Initial position for RTH approach
|
||||
|
|
@ -471,8 +473,10 @@ typedef struct {
|
|||
int8_t activeRthTBPointIndex;
|
||||
int8_t rthTBWrapAroundCounter; // stores trackpoint array overwrite index position
|
||||
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
/* Fixedwing autoland */
|
||||
fwLandState_t fwLandState;
|
||||
#endif
|
||||
|
||||
/* Internals & statistics */
|
||||
int16_t rcAdjustment[4];
|
||||
|
|
|
|||
|
|
@ -744,7 +744,12 @@ static int logicConditionGetFlightOperandValue(int operand) {
|
|||
break;
|
||||
|
||||
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING: // 0/1
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
return ((navGetCurrentStateFlags() & NAV_CTL_LAND) || isFwLandInProgess()) ? 1 : 0;
|
||||
#else
|
||||
return ((navGetCurrentStateFlags() & NAV_CTL_LAND)) ? 1 : 0;
|
||||
#endif
|
||||
|
||||
break;
|
||||
|
||||
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_FAILSAFE: // 0/1
|
||||
|
|
@ -809,11 +814,11 @@ static int logicConditionGetFlightOperandValue(int operand) {
|
|||
case LOGIC_CONDITION_OPERAND_FLIGHT_RANGEFINDER_RAW:
|
||||
return rangefinderGetLatestRawAltitude();
|
||||
break;
|
||||
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
case LOGIC_CONDITION_OPERAND_FLIGHT_FW_LAND_STATE:
|
||||
return posControl.fwLandState.landState;
|
||||
break;
|
||||
|
||||
#endif
|
||||
default:
|
||||
return 0;
|
||||
break;
|
||||
|
|
|
|||
|
|
@ -141,6 +141,7 @@
|
|||
#define USE_POWER_LIMITS
|
||||
|
||||
#define USE_SAFE_HOME
|
||||
#define USE_FW_AUTOLAND
|
||||
#define USE_AUTOTUNE_FIXED_WING
|
||||
#define USE_LOG
|
||||
#define USE_STATS
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue