1
0
Fork 0
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 2024-02-12 16:40:19 -03:00
parent 0871ef3612
commit d48971a48f
11 changed files with 133 additions and 31 deletions

View file

@ -1309,6 +1309,7 @@ static void cliTempSensor(char *cmdline)
} }
#endif #endif
#ifdef USE_FW_AUTOLAND
static void printFwAutolandApproach(uint8_t dumpMask, const navFwAutolandApproach_t *navFwAutolandApproach, const navFwAutolandApproach_t *defaultFwAutolandApproach) 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"; 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) #if defined(USE_SAFE_HOME)
static void printSafeHomes(uint8_t dumpMask, const navSafeHome_t *navSafeHome, const navSafeHome_t *defaultSafeHome) 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)); printSafeHomes(dumpMask, safeHomeConfig_CopyArray, safeHomeConfig(0));
#endif #endif
#ifdef USE_FW_AUTOLAND
cliPrintHashLine("Fixed Wing Approach"); cliPrintHashLine("Fixed Wing Approach");
printFwAutolandApproach(dumpMask, fwAutolandApproachConfig_CopyArray, fwAutolandApproachConfig(0)); printFwAutolandApproach(dumpMask, fwAutolandApproachConfig_CopyArray, fwAutolandApproachConfig(0));
#endif
cliPrintHashLine("features"); cliPrintHashLine("features");
printFeature(dumpMask, &featureConfig_Copy, featureConfig()); printFeature(dumpMask, &featureConfig_Copy, featureConfig());
@ -4148,7 +4152,9 @@ const clicmd_t cmdTable[] = {
CLI_COMMAND_DEF("flash_write", NULL, "<address> <message>", cliFlashWrite), CLI_COMMAND_DEF("flash_write", NULL, "<address> <message>", cliFlashWrite),
#endif #endif
#endif #endif
#ifdef USE_FW_AUTOLAND
CLI_COMMAND_DEF("fwapproach", "Fixed Wing Approach Settings", NULL, cliFwAutolandApproach), CLI_COMMAND_DEF("fwapproach", "Fixed Wing Approach Settings", NULL, cliFwAutolandApproach),
#endif
CLI_COMMAND_DEF("get", "get variable value", "[name]", cliGet), CLI_COMMAND_DEF("get", "get variable value", "[name]", cliGet),
#ifdef USE_GPS #ifdef USE_GPS
CLI_COMMAND_DEF("gpspassthrough", "passthrough gps to serial", NULL, cliGpsPassthrough), CLI_COMMAND_DEF("gpspassthrough", "passthrough gps to serial", NULL, cliGpsPassthrough),

View file

@ -1660,6 +1660,7 @@ static mspResult_e mspFcSafeHomeOutCommand(sbuf_t *dst, sbuf_t *src)
} }
#endif #endif
#ifdef USE_FW_AUTOLAND
static mspResult_e mspFwApproachOutCommand(sbuf_t *dst, sbuf_t *src) static mspResult_e mspFwApproachOutCommand(sbuf_t *dst, sbuf_t *src)
{ {
const uint8_t idx = sbufReadU8(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; return MSP_RESULT_ERROR;
} }
} }
#endif
static mspResult_e mspFcLogicConditionCommand(sbuf_t *dst, sbuf_t *src) { static mspResult_e mspFcLogicConditionCommand(sbuf_t *dst, sbuf_t *src) {
const uint8_t idx = sbufReadU8(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: case MSP_SET_WP:
if (dataSize == 21) { if (dataSize == 21) {
static uint8_t mmIdx = 0;
const uint8_t msp_wp_no = sbufReadU8(src); // get the waypoint number const uint8_t msp_wp_no = sbufReadU8(src); // get the waypoint number
navWaypoint_t msp_wp; navWaypoint_t msp_wp;
msp_wp.action = sbufReadU8(src); // action 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 msp_wp.flag = sbufReadU8(src); // future: to set nav flag
setWaypoint(msp_wp_no, &msp_wp); 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 #ifdef USE_SAFE_HOME
fwAppraochStartIdx = MAX_SAFE_HOMES; fwAppraochStartIdx = MAX_SAFE_HOMES;
#endif #endif
@ -2691,8 +2694,11 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
mmIdx++; mmIdx++;
} }
resetFwAutolandApproach(fwAppraochStartIdx + mmIdx); resetFwAutolandApproach(fwAppraochStartIdx + mmIdx);
} else #endif
} else {
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
}
break; break;
case MSP2_COMMON_SET_RADAR_POS: case MSP2_COMMON_SET_RADAR_POS:
if (dataSize == 19) { if (dataSize == 19) {
@ -3149,13 +3155,16 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
safeHomeConfigMutable(i)->enabled = sbufReadU8(src); safeHomeConfigMutable(i)->enabled = sbufReadU8(src);
safeHomeConfigMutable(i)->lat = sbufReadU32(src); safeHomeConfigMutable(i)->lat = sbufReadU32(src);
safeHomeConfigMutable(i)->lon = sbufReadU32(src); safeHomeConfigMutable(i)->lon = sbufReadU32(src);
#ifdef USE_FW_AUTOLAND
resetFwAutolandApproach(i); resetFwAutolandApproach(i);
#endif
} else { } else {
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
} }
break; break;
#endif #endif
#ifdef USE_FW_AUTOLAND
case MSP2_INAV_SET_FW_APPROACH: case MSP2_INAV_SET_FW_APPROACH:
if (dataSize == 15) { if (dataSize == 15) {
uint8_t i; uint8_t i;
@ -3178,6 +3187,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
} }
break; break;
#endif
#ifdef USE_EZ_TUNE #ifdef USE_EZ_TUNE
@ -3691,10 +3701,11 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
*ret = mspFcSafeHomeOutCommand(dst, src); *ret = mspFcSafeHomeOutCommand(dst, src);
break; break;
#endif #endif
#ifdef USE_FW_AUTOLAND
case MSP2_INAV_FW_APPROACH: case MSP2_INAV_FW_APPROACH:
*ret = mspFwApproachOutCommand(dst, src); *ret = mspFwApproachOutCommand(dst, src);
break; break;
#endif
#ifdef USE_SIMULATOR #ifdef USE_SIMULATOR
case MSP_SIMULATOR: case MSP_SIMULATOR:
tmp_u8 = sbufReadU8(src); // Get the Simulator MSP version tmp_u8 = sbufReadU8(src); // Get the Simulator MSP version

View file

@ -4093,7 +4093,7 @@ groups:
- name: PG_FW_AUTOLAND_CONFIG - name: PG_FW_AUTOLAND_CONFIG
type: navFwAutolandConfig_t type: navFwAutolandConfig_t
headers: ["navigation/navigation.h"] headers: ["navigation/navigation.h"]
condition: USE_SAFE_HOME condition: USE_FW_AUTOLAND
members: members:
- name: nav_fw_land_approach_length - name: nav_fw_land_approach_length
description: "Length of the final approach" description: "Length of the final approach"

View file

@ -593,7 +593,11 @@ static float computePidLevelTarget(flight_dynamics_index_t axis) {
#endif #endif
// Automatically pitch down if the throttle is manually controlled and reduced bellow cruise throttle // 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()) { 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); angleTarget += scaleRange(MAX(0, currentBatteryProfile->nav.fw.cruise_throttle - rcCommand[THROTTLE]), 0, currentBatteryProfile->nav.fw.cruise_throttle - PWM_RANGE_MIN, 0, navConfig()->fw.minThrottleDownPitchAngle);
} }

View file

@ -1004,15 +1004,21 @@ static const char * osdFailsafeInfoMessage(void)
} }
return OSD_MESSAGE_STR(OSD_MSG_RC_RX_LINK_LOST); return OSD_MESSAGE_STR(OSD_MSG_RC_RX_LINK_LOST);
} }
#if defined(USE_SAFE_HOME) #if defined(USE_SAFE_HOME)
static const char * divertingToSafehomeMessage(void) 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)) { 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); return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME);
} }
return NULL;
}
#endif #endif
return NULL;
}
static const char * navigationStateMessage(void) static const char * navigationStateMessage(void)
{ {
@ -2269,9 +2275,12 @@ static bool osdDrawSingleElement(uint8_t item)
case OSD_FLYMODE: case OSD_FLYMODE:
{ {
char *p = "ACRO"; char *p = "ACRO";
#ifdef USE_FW_AUTOLAND
if (isFwLandInProgess()) if (isFwLandInProgess())
p = "LAND"; p = "LAND";
else if (FLIGHT_MODE(FAILSAFE_MODE)) else
#endif
if (FLIGHT_MODE(FAILSAFE_MODE))
p = "!FS!"; p = "!FS!";
else if (FLIGHT_MODE(MANUAL_MODE)) else if (FLIGHT_MODE(MANUAL_MODE))
p = "MANU"; p = "MANU";
@ -5134,8 +5143,13 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
const char *invertedInfoMessage = NULL; const char *invertedInfoMessage = NULL;
if (ARMING_FLAG(ARMED)) { 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 (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding() || isFwLandInProgess()) {
if (isWaypointMissionRTHActive() && !posControl.fwLandState.landWp) { 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 // 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); 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); tfp_sprintf(messageBuf, "LANDING DELAY: %3u SECONDS", remainingHoldSec);
messages[messageCount++] = messageBuf; messages[messageCount++] = messageBuf;
} else { }
else {
#ifdef USE_FW_AUTOLAND
if (canFwLandCanceld()) { if (canFwLandCanceld()) {
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_MOVE_STICKS); messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_MOVE_STICKS);
} else if (!isFwLandInProgess()) { } else if (!isFwLandInProgess()) {
#endif
const char *navStateMessage = navigationStateMessage(); const char *navStateMessage = navigationStateMessage();
if (navStateMessage) { if (navStateMessage) {
messages[messageCount++] = navStateMessage; messages[messageCount++] = navStateMessage;
} }
#ifdef USE_FW_AUTOLAND
} }
#endif
} }
#if defined(USE_SAFE_HOME) #if defined(USE_SAFE_HOME)
const char *safehomeMessage = divertingToSafehomeMessage(); const char *safehomeMessage = divertingToSafehomeMessage();
if (safehomeMessage) { if (safehomeMessage) {

View file

@ -90,11 +90,11 @@ int16_t GPS_directionToHome; // direction to home point in degrees
radar_pois_t radar_pois[RADAR_MAX_POIS]; radar_pois_t radar_pois[RADAR_MAX_POIS];
#if defined(USE_SAFE_HOME) #ifdef USE_FW_AUTOLAND
PG_REGISTER_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig, PG_SAFE_HOME_CONFIG , 0);
PG_REGISTER_WITH_RESET_TEMPLATE(navFwAutolandConfig_t, navFwAutolandConfig, PG_FW_AUTOLAND_CONFIG, 0); 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, PG_RESET_TEMPLATE(navFwAutolandConfig_t, navFwAutolandConfig,
.approachLength = SETTING_NAV_FW_LAND_APPROACH_LENGTH_DEFAULT, .approachLength = SETTING_NAV_FW_LAND_APPROACH_LENGTH_DEFAULT,
.finalApproachPitchToThrottleMod = SETTING_NAV_FW_LAND_FINAL_APPROACH_PITCH2THROTTLE_MOD_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, .maxTailwind = SETTING_NAV_FW_LAND_MAX_TAILWIND_DEFAULT,
.glidePitch = SETTING_NAV_FW_LAND_GLIDE_PITCH_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 #endif
// waypoint 254, 255 are special waypoints // waypoint 254, 255 are special waypoints
@ -270,7 +271,11 @@ static void resetAltitudeController(bool useTerrainFollowing);
static void resetPositionController(void); static void resetPositionController(void);
static void setupAltitudeController(void); static void setupAltitudeController(void);
static void resetHeadingController(void); static void resetHeadingController(void);
#ifdef USE_FW_AUTOLAND
static void resetFwAutoland(void); static void resetFwAutoland(void);
#endif
void resetGCSFlags(void); void resetGCSFlags(void);
static void setupJumpCounters(void); static void setupJumpCounters(void);
@ -299,10 +304,12 @@ static bool rthAltControlStickOverrideCheck(unsigned axis);
static void updateRthTrackback(bool forceSaveTrackPoint); static void updateRthTrackback(bool forceSaveTrackPoint);
static fpVector3_t * rthGetTrackbackPos(void); static fpVector3_t * rthGetTrackbackPos(void);
#ifdef USE_FW_AUTOLAND
static float getLandAltitude(void); static float getLandAltitude(void);
static int32_t calcWindDiff(int32_t heading, int32_t windHeading); static int32_t calcWindDiff(int32_t heading, int32_t windHeading);
static int32_t calcFinalApproachHeading(int32_t approachHeading, int32_t windAngle); static int32_t calcFinalApproachHeading(int32_t approachHeading, int32_t windAngle);
static void setLandWaypoint(const fpVector3_t *pos, const fpVector3_t *nextWpPos); static void setLandWaypoint(const fpVector3_t *pos, const fpVector3_t *nextWpPos);
#endif
/*************************************************************************************************/ /*************************************************************************************************/
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState_t previousState); 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_INITIALIZE(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(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); 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_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_LOITER(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(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_GLIDE(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FLARE(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); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(navigationFSMState_t previousState);
#endif
static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
/** Idle state ******************************************************/ /** 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] = { [NAV_STATE_FW_LANDING_CLIMB_TO_LOITER] = {
.persistentId = NAV_PERSISTENT_ID_FW_LANDING_CLIMB_TO_LOITER, .persistentId = NAV_PERSISTENT_ID_FW_LANDING_CLIMB_TO_LOITER,
.onEntry = navOnEnteringState_NAV_STATE_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, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
} }
}, },
#endif
}; };
static navigationFSMStateFlags_t navGetStateFlags(navigationFSMState_t state) static navigationFSMStateFlags_t navGetStateFlags(navigationFSMState_t state)
@ -1181,7 +1192,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState
resetAltitudeController(false); resetAltitudeController(false);
resetHeadingController(); resetHeadingController();
resetPositionController(); resetPositionController();
#ifdef USE_FW_AUTOLAND
resetFwAutoland(); resetFwAutoland();
#endif
return NAV_FSM_EVENT_NONE; 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) { if (previousState != NAV_STATE_FW_LANDING_ABORT) {
#ifdef USE_FW_AUTOLAND
posControl.fwLandState.landAborted = false; posControl.fwLandState.landAborted = false;
#endif
if (STATE(FIXED_WING_LEGACY) && (posControl.homeDistance < navConfig()->general.min_rth_distance) && !posControl.flags.forcedRTHActivated) { 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 // Prevent RTH from activating on airplanes if too close to home unless it's a failsafe RTH
return NAV_FSM_EVENT_SWITCH_TO_IDLE; 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) 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 //On ROVER and BOAT we immediately switch to the next event
if (!STATE(ALTITUDE_CONTROL)) { if (!STATE(ALTITUDE_CONTROL)) {
@ -1701,6 +1719,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
return NAV_FSM_EVENT_SWITCH_TO_MIXERAT; return NAV_FSM_EVENT_SWITCH_TO_MIXERAT;
} }
#ifdef USE_FW_AUTOLAND
if (STATE(AIRPLANE)) { if (STATE(AIRPLANE)) {
int8_t missionIdx = -1, shIdx = -1, missionFwLandConfigStartIdx = 8; int8_t missionIdx = -1, shIdx = -1, missionFwLandConfigStartIdx = 8;
#ifdef USE_MULTI_MISSION #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; return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING;
} }
} }
#endif
float descentVelLimited = 0; float descentVelLimited = 0;
@ -2054,7 +2074,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITI
{ {
UNUSED(previousState); UNUSED(previousState);
#ifdef USE_FW_AUTOLAND
posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE; posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
#endif
if ((posControl.flags.estPosStatus >= EST_USABLE)) { if ((posControl.flags.estPosStatus >= EST_USABLE)) {
resetPositionController(); resetPositionController();
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, 0, NAV_POS_UPDATE_XY); 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; return NAV_FSM_EVENT_SUCCESS;
} }
#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_CLIMB_TO_LOITER(navigationFSMState_t previousState)
{ {
UNUSED(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; return posControl.fwLandState.landWp ? NAV_FSM_EVENT_SWITCH_TO_WAYPOINT : NAV_FSM_EVENT_SWITCH_TO_RTH;
} }
#endif
static navigationFSMState_t navSetNewFSMState(navigationFSMState_t newState) static navigationFSMState_t navSetNewFSMState(navigationFSMState_t newState)
{ {
@ -4146,9 +4171,11 @@ void applyWaypointNavigationAndAltitudeHold(void)
posControl.flags.horizontalPositionDataConsumed = false; posControl.flags.horizontalPositionDataConsumed = false;
posControl.flags.verticalPositionDataConsumed = false; posControl.flags.verticalPositionDataConsumed = false;
#ifdef USE_FW_AUTOLAND
if (!isFwLandInProgess()) { if (!isFwLandInProgess()) {
posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE; posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
} }
#endif
/* Process controllers */ /* Process controllers */
navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState); navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
@ -5029,6 +5056,8 @@ uint8_t getActiveWpNumber(void)
return NAV_Status.activeWpNumber; return NAV_Status.activeWpNumber;
} }
#ifdef USE_FW_AUTOLAND
static void resetFwAutoland(void) static void resetFwAutoland(void)
{ {
posControl.fwLandState.landAltAgl = 0; posControl.fwLandState.landAltAgl = 0;
@ -5122,3 +5151,5 @@ bool canFwLandCanceld(void)
|| posControl.navState == NAV_STATE_FW_LANDING_APPROACH || posControl.navState == NAV_STATE_FW_LANDING_APPROACH
|| posControl.navState == NAV_STATE_FW_LANDING_GLIDE; || posControl.navState == NAV_STATE_FW_LANDING_GLIDE;
} }
#endif

View file

@ -41,18 +41,38 @@ extern bool autoThrottleManuallyIncreased;
/* Navigation system updates */ /* Navigation system updates */
void onNewGPSData(void); void onNewGPSData(void);
#if defined(USE_SAFE_HOME) #if defined(USE_SAFE_HOME)
#define MAX_SAFE_HOMES 8 #define MAX_SAFE_HOMES 8
#define MAX_FW_LAND_APPOACH_SETTINGS (MAX_SAFE_HOMES + 9)
typedef struct { typedef struct {
uint8_t enabled; uint8_t enabled;
int32_t lat; int32_t lat;
int32_t lon; int32_t lon;
int8_t fwLandSettingsIdx;
} navSafeHome_t; } 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 { typedef enum {
FW_AUTOLAND_APPROACH_DIRECTION_LEFT, FW_AUTOLAND_APPROACH_DIRECTION_LEFT,
FW_AUTOLAND_APPROACH_DIRECTION_RIGHT FW_AUTOLAND_APPROACH_DIRECTION_RIGHT
@ -79,14 +99,6 @@ typedef struct {
PG_DECLARE_ARRAY(navFwAutolandApproach_t, MAX_FW_LAND_APPOACH_SETTINGS, fwAutolandApproachConfig); 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 typedef struct navFwAutolandConfig_s
{ {
uint32_t approachLength; uint32_t approachLength;
@ -101,11 +113,8 @@ typedef struct navFwAutolandConfig_s
PG_DECLARE(navFwAutolandConfig_t, navFwAutolandConfig); PG_DECLARE(navFwAutolandConfig_t, navFwAutolandConfig);
void resetFwAutolandApproach(int8_t idx); void resetFwAutolandApproach(int8_t idx);
void resetSafeHomes(void); // remove all safehomes
bool findNearestSafeHome(void); // Find nearest safehome
#endif
#endif // defined(USE_SAFE_HOME)
#ifndef NAV_MAX_WAYPOINTS #ifndef NAV_MAX_WAYPOINTS
#define NAV_MAX_WAYPOINTS 15 #define NAV_MAX_WAYPOINTS 15
@ -683,8 +692,10 @@ uint8_t getActiveWpNumber(void);
*/ */
int32_t navigationGetHomeHeading(void); int32_t navigationGetHomeHeading(void);
#ifdef USE_FW_AUTOLAND
bool isFwLandInProgess(void); bool isFwLandInProgess(void);
bool canFwLandCanceld(void); bool canFwLandCanceld(void);
#endif
/* Compatibility data */ /* Compatibility data */
extern navSystemStatus_t NAV_Status; extern navSystemStatus_t NAV_Status;

View file

@ -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 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; int16_t pitchToThrottle = currentBatteryProfile->nav.fw.pitch_to_throttle;
#ifdef USE_FW_AUTOLAND
if (pitch < 0 && posControl.fwLandState.landState == FW_AUTOLAND_STATE_FINAL_APPROACH) { if (pitch < 0 && posControl.fwLandState.landState == FW_AUTOLAND_STATE_FINAL_APPROACH) {
pitchToThrottle *= navFwAutolandConfig()->finalApproachPitchToThrottleMod / 100.0f; pitchToThrottle *= navFwAutolandConfig()->finalApproachPitchToThrottleMod / 100.0f;
} }
#endif
if (ABS(pitch - filteredPitch) > navConfig()->fw.pitch_to_throttle_thresh) { if (ABS(pitch - filteredPitch) > navConfig()->fw.pitch_to_throttle_thresh) {
// Unfiltered throttle correction outside of pitch deadband // 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); uint16_t correctedThrottleValue = constrain(currentBatteryProfile->nav.fw.cruise_throttle + throttleCorrection, currentBatteryProfile->nav.fw.min_throttle, currentBatteryProfile->nav.fw.max_throttle);
// Manual throttle increase // Manual throttle increase
#ifdef USE_FW_AUTOLAND
if (navConfig()->fw.allow_manual_thr_increase && !FLIGHT_MODE(FAILSAFE_MODE) && !isFwLandInProgess()) { 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){ if (rcCommand[THROTTLE] < PWM_RANGE_MIN + (PWM_RANGE_MAX - PWM_RANGE_MIN) * 0.95){
correctedThrottleValue += MAX(0, rcCommand[THROTTLE] - currentBatteryProfile->nav.fw.cruise_throttle); correctedThrottleValue += MAX(0, rcCommand[THROTTLE] - currentBatteryProfile->nav.fw.cruise_throttle);
} else { } else {
@ -654,6 +661,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
rcCommand[THROTTLE] = setDesiredThrottle(correctedThrottleValue, false); rcCommand[THROTTLE] = setDesiredThrottle(correctedThrottleValue, false);
} }
#ifdef USE_FW_AUTOLAND
// Advanced autoland // Advanced autoland
if (posControl.navState == NAV_STATE_FW_LANDING_GLIDE || posControl.navState == NAV_STATE_FW_LANDING_FLARE || STATE(LANDING_DETECTED)) { 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 // 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]); rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(navFwAutolandConfig()->flarePitch), pidProfile()->max_angle_inclination[FD_PITCH]);
} }
} }
#endif
// "Traditional" landing as fallback option // "Traditional" landing as fallback option
if (navStateFlags & NAV_CTL_LAND) { if (navStateFlags & NAV_CTL_LAND) {
int32_t finalAltitude = navConfig()->general.land_slowdown_minalt + posControl.rthState.homeTmpWaypoint.z; int32_t finalAltitude = navConfig()->general.land_slowdown_minalt + posControl.rthState.homeTmpWaypoint.z;

View file

@ -368,6 +368,7 @@ typedef struct {
bool rthLinearDescentActive; // Activation status of Linear Descent bool rthLinearDescentActive; // Activation status of Linear Descent
} rthState_t; } rthState_t;
#ifdef USE_FW_AUTOLAND
typedef enum { typedef enum {
FW_AUTOLAND_WP_TURN, FW_AUTOLAND_WP_TURN,
FW_AUTOLAND_WP_FINAL_APPROACH, FW_AUTOLAND_WP_FINAL_APPROACH,
@ -389,6 +390,7 @@ typedef struct {
bool landWp; bool landWp;
fwAutolandState_t landState; fwAutolandState_t landState;
} fwLandState_t; } fwLandState_t;
#endif
typedef enum { typedef enum {
RTH_HOME_ENROUTE_INITIAL, // Initial position for RTH approach RTH_HOME_ENROUTE_INITIAL, // Initial position for RTH approach
@ -471,8 +473,10 @@ typedef struct {
int8_t activeRthTBPointIndex; int8_t activeRthTBPointIndex;
int8_t rthTBWrapAroundCounter; // stores trackpoint array overwrite index position int8_t rthTBWrapAroundCounter; // stores trackpoint array overwrite index position
#ifdef USE_FW_AUTOLAND
/* Fixedwing autoland */ /* Fixedwing autoland */
fwLandState_t fwLandState; fwLandState_t fwLandState;
#endif
/* Internals & statistics */ /* Internals & statistics */
int16_t rcAdjustment[4]; int16_t rcAdjustment[4];

View file

@ -744,7 +744,12 @@ static int logicConditionGetFlightOperandValue(int operand) {
break; break;
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING: // 0/1 case LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING: // 0/1
#ifdef USE_FW_AUTOLAND
return ((navGetCurrentStateFlags() & NAV_CTL_LAND) || isFwLandInProgess()) ? 1 : 0; return ((navGetCurrentStateFlags() & NAV_CTL_LAND) || isFwLandInProgess()) ? 1 : 0;
#else
return ((navGetCurrentStateFlags() & NAV_CTL_LAND)) ? 1 : 0;
#endif
break; break;
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_FAILSAFE: // 0/1 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: case LOGIC_CONDITION_OPERAND_FLIGHT_RANGEFINDER_RAW:
return rangefinderGetLatestRawAltitude(); return rangefinderGetLatestRawAltitude();
break; break;
#ifdef USE_FW_AUTOLAND
case LOGIC_CONDITION_OPERAND_FLIGHT_FW_LAND_STATE: case LOGIC_CONDITION_OPERAND_FLIGHT_FW_LAND_STATE:
return posControl.fwLandState.landState; return posControl.fwLandState.landState;
break; break;
#endif
default: default:
return 0; return 0;
break; break;

View file

@ -141,6 +141,7 @@
#define USE_POWER_LIMITS #define USE_POWER_LIMITS
#define USE_SAFE_HOME #define USE_SAFE_HOME
#define USE_FW_AUTOLAND
#define USE_AUTOTUNE_FIXED_WING #define USE_AUTOTUNE_FIXED_WING
#define USE_LOG #define USE_LOG
#define USE_STATS #define USE_STATS