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:
parent
0871ef3612
commit
d48971a48f
11 changed files with 133 additions and 31 deletions
|
@ -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),
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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"
|
||||||
|
|
|
@ -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);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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) {
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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];
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue