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

Move appraoch to extra config

Browse source
This commit is contained in:
Scavanger 2023-11-17 10:43:40 -03:00
parent cdc1a05e7c
commit 02806b17f7
11 changed files with 193 additions and 616 deletions
Download patch file Download diff file Expand all files Collapse all files

3
src/main/config/parameter_group_ids.h
View file

@ -124,7 +124,8 @@
#define PG_LEDPIN_CONFIG 1034 #define PG_LEDPIN_CONFIG 1034
#define PG_OSD_JOYSTICK_CONFIG 1035 #define PG_OSD_JOYSTICK_CONFIG 1035
#define PG_FW_AUTOLAND_CONFIG 1036 #define PG_FW_AUTOLAND_CONFIG 1036
#define PG_INAV_END PG_FW_AUTOLAND_CONFIG #define PG_FW_AUTOLAND_APPROACH_CONFIG 1037
#define PG_INAV_END PG_FW_AUTOLAND_APPROACH_CONFIG
// OSD configuration (subject to change) // OSD configuration (subject to change)
//#define PG_OSD_FONT_CONFIG 2047 //#define PG_OSD_FONT_CONFIG 2047

130
src/main/fc/cli.c
View file

@ -1309,58 +1309,41 @@ static void cliTempSensor(char *cmdline)
} }
#endif #endif
#if defined(USE_SAFE_HOME) static void printFwAutolandApproach(uint8_t dumpMask, const navFwAutolandApproach_t *navFwAutolandApproach, const navFwAutolandApproach_t *defaultFwAutolandApproach)
static void printSafeHomes(uint8_t dumpMask, const navSafeHome_t *navSafeHome, const navSafeHome_t *defaultSafeHome)
{ {
const char *format = "safehome %u %u %d %d %d %d %u %d %d %u"; // uint8_t enabled, int32_t lat; int32_t lon const char *format = "fwapproach %u %d %d %d %u %d %d %u";
for (uint8_t i = 0; i < MAX_SAFE_HOMES; i++) { for (uint8_t i = 0; i < MAX_FW_LAND_APPOACH_SETTINGS; i++) {
bool equalsDefault = false; bool equalsDefault = false;
if (defaultSafeHome) { if (defaultFwAutolandApproach) {
equalsDefault = navSafeHome[i].enabled == defaultSafeHome[i].enabled equalsDefault = navFwAutolandApproach[i].approachDirection == defaultFwAutolandApproach[i].approachDirection
&& navSafeHome[i].lat == defaultSafeHome[i].lat && navFwAutolandApproach[i].approachAlt == defaultFwAutolandApproach[i].approachAlt
&& navSafeHome[i].lon == defaultSafeHome[i].lon && navFwAutolandApproach[i].landAlt == defaultFwAutolandApproach[i].landAlt
&& navSafeHome[i].approachDirection == defaultSafeHome[i].approachDirection && navFwAutolandApproach[i].landApproachHeading1 == defaultFwAutolandApproach[i].landApproachHeading1
&& navSafeHome[i].approachAlt == defaultSafeHome[i].approachAlt && navFwAutolandApproach[i].landApproachHeading2 == defaultFwAutolandApproach[i].landApproachHeading2
&& navSafeHome[i].landAlt == defaultSafeHome[i].landAlt && navFwAutolandApproach[i].isSeaLevelRef == defaultFwAutolandApproach[i].isSeaLevelRef;
&& navSafeHome[i].landApproachHeading1 == defaultSafeHome[i].landApproachHeading1
&& navSafeHome[i].landApproachHeading2 == defaultSafeHome[i].landApproachHeading2
&& navSafeHome[i].isSeaLevelRef == defaultSafeHome[i].isSeaLevelRef;
cliDefaultPrintLinef(dumpMask, equalsDefault, format, i, cliDefaultPrintLinef(dumpMask, equalsDefault, format, i,
defaultSafeHome[i].enabled, defaultSafeHome[i].lat, defaultSafeHome[i].lon, defaultSafeHome[i].approachAlt, defaultSafeHome[i].landAlt, defaultSafeHome[i].approachDirection, defaultSafeHome[i].landApproachHeading1, defaultSafeHome[i].landApproachHeading2, defaultSafeHome[i].isSeaLevelRef); defaultFwAutolandApproach[i].approachAlt, defaultFwAutolandApproach[i].landAlt, defaultFwAutolandApproach[i].approachDirection, defaultFwAutolandApproach[i].landApproachHeading1, defaultFwAutolandApproach[i].landApproachHeading2, defaultFwAutolandApproach[i].isSeaLevelRef);
} }
cliDumpPrintLinef(dumpMask, equalsDefault, format, i, cliDumpPrintLinef(dumpMask, equalsDefault, format, i,
navSafeHome[i].enabled, navSafeHome[i].lat, navSafeHome[i].lon, navSafeHome[i].approachAlt, navSafeHome[i].landAlt, navSafeHome[i].approachDirection, navSafeHome[i].landApproachHeading1, navSafeHome[i].landApproachHeading2, navSafeHome[i].isSeaLevelRef); navFwAutolandApproach[i].approachAlt, navFwAutolandApproach[i].landAlt, navFwAutolandApproach[i].approachDirection, navFwAutolandApproach[i].landApproachHeading1, navFwAutolandApproach[i].landApproachHeading2, navFwAutolandApproach[i].isSeaLevelRef);
} }
} }
static void cliSafeHomes(char *cmdline) static void cliFwAutolandApproach(char * cmdline)
{ {
if (isEmpty(cmdline)) { if (isEmpty(cmdline)) {
printSafeHomes(DUMP_MASTER, safeHomeConfig(0), NULL); printFwAutolandApproach(DUMP_MASTER, fwAutolandApproachConfig(0), NULL);
} else if (sl_strcasecmp(cmdline, "reset") == 0) { } else if (sl_strcasecmp(cmdline, "reset") == 0) {
resetSafeHomes(); resetFwAutolandApproach();
} else { } else {
int32_t lat=0, lon=0, approachAlt = 0, heading1 = 0, heading2 = 0, landDirection = 0, landAlt = 0; int32_t approachAlt = 0, heading1 = 0, heading2 = 0, landDirection = 0, landAlt = 0;
bool enabled=false, isSeaLevelRef = false; bool isSeaLevelRef = false;
uint8_t validArgumentCount = 0; uint8_t validArgumentCount = 0;
const char *ptr = cmdline; const char *ptr = cmdline;
int8_t i = fastA2I(ptr); int8_t i = fastA2I(ptr);
if (i < 0 || i >= MAX_SAFE_HOMES) { if (i < 0 || i >= MAX_SAFE_HOMES) {
cliShowArgumentRangeError("safehome index", 0, MAX_SAFE_HOMES - 1); cliShowArgumentRangeError("fwapproach index", 0, MAX_FW_LAND_APPOACH_SETTINGS - 1);
} else { } else {
if ((ptr = nextArg(ptr))) {
enabled = fastA2I(ptr);
validArgumentCount++;
}
if ((ptr = nextArg(ptr))) {
lat = fastA2I(ptr);
validArgumentCount++;
}
if ((ptr = nextArg(ptr))) {
lon = fastA2I(ptr);
validArgumentCount++;
}
if ((ptr = nextArg(ptr))) { if ((ptr = nextArg(ptr))) {
approachAlt = fastA2I(ptr); approachAlt = fastA2I(ptr);
validArgumentCount++; validArgumentCount++;
@ -1414,18 +1397,75 @@ static void cliSafeHomes(char *cmdline)
validArgumentCount++; validArgumentCount++;
} }
if (validArgumentCount != 9) { if (validArgumentCount != 6) {
cliShowParseError();
} else {
fwAutolandApproachConfigMutable(i)->approachAlt = approachAlt;
fwAutolandApproachConfigMutable(i)->landAlt = landAlt;
fwAutolandApproachConfigMutable(i)->approachDirection = (fwAutolandApproachDirection_e)landDirection;
fwAutolandApproachConfigMutable(i)->landApproachHeading1 = (int16_t)heading1;
fwAutolandApproachConfigMutable(i)->landApproachHeading2 = (int16_t)heading2;
fwAutolandApproachConfigMutable(i)->isSeaLevelRef = isSeaLevelRef;
}
}
}
}
#if defined(USE_SAFE_HOME)
static void printSafeHomes(uint8_t dumpMask, const navSafeHome_t *navSafeHome, const navSafeHome_t *defaultSafeHome)
{
const char *format = "safehome %u %u %d %d"; // uint8_t enabled, int32_t lat; int32_t lon
for (uint8_t i = 0; i < MAX_SAFE_HOMES; i++) {
bool equalsDefault = false;
if (defaultSafeHome) {
equalsDefault = navSafeHome[i].enabled == defaultSafeHome[i].enabled
&& navSafeHome[i].lat == defaultSafeHome[i].lat
&& navSafeHome[i].lon == defaultSafeHome[i].lon;
cliDefaultPrintLinef(dumpMask, equalsDefault, format, i,
defaultSafeHome[i].enabled, defaultSafeHome[i].lat, defaultSafeHome[i].lon);
}
cliDumpPrintLinef(dumpMask, equalsDefault, format, i,
navSafeHome[i].enabled, navSafeHome[i].lat, navSafeHome[i].lon);
}
}
static void cliSafeHomes(char *cmdline)
{
if (isEmpty(cmdline)) {
printSafeHomes(DUMP_MASTER, safeHomeConfig(0), NULL);
} else if (sl_strcasecmp(cmdline, "reset") == 0) {
resetSafeHomes();
} else {
int32_t lat=0, lon=0;
bool enabled=false;
uint8_t validArgumentCount = 0;
const char *ptr = cmdline;
int8_t i = fastA2I(ptr);
if (i < 0 || i >= MAX_SAFE_HOMES) {
cliShowArgumentRangeError("safehome index", 0, MAX_SAFE_HOMES - 1);
} else {
if ((ptr = nextArg(ptr))) {
enabled = fastA2I(ptr);
validArgumentCount++;
}
if ((ptr = nextArg(ptr))) {
lat = fastA2I(ptr);
validArgumentCount++;
}
if ((ptr = nextArg(ptr))) {
lon = fastA2I(ptr);
validArgumentCount++;
}
if ((ptr = nextArg(ptr))) {
// check for too many arguments
validArgumentCount++;
}
if (validArgumentCount != 3) {
cliShowParseError(); cliShowParseError();
} else { } else {
safeHomeConfigMutable(i)->enabled = enabled; safeHomeConfigMutable(i)->enabled = enabled;
safeHomeConfigMutable(i)->lat = lat; safeHomeConfigMutable(i)->lat = lat;
safeHomeConfigMutable(i)->lon = lon; safeHomeConfigMutable(i)->lon = lon;
safeHomeConfigMutable(i)->approachAlt = approachAlt;
safeHomeConfigMutable(i)->landAlt = landAlt;
safeHomeConfigMutable(i)->approachDirection = (fwAutolandApproachDirection_e)landDirection;
safeHomeConfigMutable(i)->landApproachHeading1 = (int16_t)heading1;
safeHomeConfigMutable(i)->landApproachHeading2 = (int16_t)heading2;
safeHomeConfigMutable(i)->isSeaLevelRef = isSeaLevelRef;
} }
} }
} }
@ -3861,6 +3901,9 @@ static void printConfig(const char *cmdline, bool doDiff)
printSafeHomes(dumpMask, safeHomeConfig_CopyArray, safeHomeConfig(0)); printSafeHomes(dumpMask, safeHomeConfig_CopyArray, safeHomeConfig(0));
#endif #endif
cliPrintHashLine("Fixed Wing Approach");
printFwAutolandApproach(dumpMask, fwAutolandApproachConfig_CopyArray, fwAutolandApproachConfig(0));
cliPrintHashLine("features"); cliPrintHashLine("features");
printFeature(dumpMask, &featureConfig_Copy, featureConfig()); printFeature(dumpMask, &featureConfig_Copy, featureConfig());
@ -4105,6 +4148,7 @@ 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
CLI_COMMAND_DEF("fwapproach", "Fixed Wing Approach Settings", NULL, cliFwAutolandApproach),
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),

51
src/main/fc/fc_msp.c
View file

@ -1653,12 +1653,6 @@ static mspResult_e mspFcSafeHomeOutCommand(sbuf_t *dst, sbuf_t *src)
sbufWriteU8(dst, safeHomeConfig(safe_home_no)->enabled); sbufWriteU8(dst, safeHomeConfig(safe_home_no)->enabled);
sbufWriteU32(dst, safeHomeConfig(safe_home_no)->lat); sbufWriteU32(dst, safeHomeConfig(safe_home_no)->lat);
sbufWriteU32(dst, safeHomeConfig(safe_home_no)->lon); sbufWriteU32(dst, safeHomeConfig(safe_home_no)->lon);
sbufWriteU32(dst, safeHomeConfig(safe_home_no)->approachAlt);
sbufWriteU32(dst, safeHomeConfig(safe_home_no)->landAlt);
sbufWriteU8(dst, safeHomeConfig(safe_home_no)->approachDirection);
sbufWriteU16(dst, safeHomeConfig(safe_home_no)->landApproachHeading1);
sbufWriteU16(dst, safeHomeConfig(safe_home_no)->landApproachHeading2);
sbufWriteU8(dst, safeHomeConfig(safe_home_no)->isSeaLevelRef);
return MSP_RESULT_ACK; return MSP_RESULT_ACK;
} else { } else {
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
@ -1666,6 +1660,22 @@ static mspResult_e mspFcSafeHomeOutCommand(sbuf_t *dst, sbuf_t *src)
} }
#endif #endif
static mspResult_e mspFwApproachOutCommand(sbuf_t *dst, sbuf_t *src)
{
const uint8_t idx = sbufReadU8(src);
if(idx < MAX_FW_LAND_APPOACH_SETTINGS) {
sbufWriteU8(dst, idx);
sbufWriteU32(dst, fwAutolandApproachConfig(idx)->approachAlt);
sbufWriteU32(dst, fwAutolandApproachConfig(idx)->landAlt);
sbufWriteU8(dst, fwAutolandApproachConfig(idx)->approachDirection);
sbufWriteU16(dst, fwAutolandApproachConfig(idx)->landApproachHeading1);
sbufWriteU16(dst, fwAutolandApproachConfig(idx)->landApproachHeading2);
sbufWriteU8(dst, fwAutolandApproachConfig(idx)->isSeaLevelRef);
return MSP_RESULT_ACK;
} else {
return MSP_RESULT_ERROR;
}
}
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);
@ -3119,7 +3129,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
#endif #endif
#ifdef USE_SAFE_HOME #ifdef USE_SAFE_HOME
case MSP2_INAV_SET_SAFEHOME: case MSP2_INAV_SET_SAFEHOME:
if (dataSize == 24) { if (dataSize == 10) {
uint8_t i; uint8_t i;
if (!sbufReadU8Safe(&i, src) || i >= MAX_SAFE_HOMES) { if (!sbufReadU8Safe(&i, src) || i >= MAX_SAFE_HOMES) {
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
@ -3127,18 +3137,30 @@ 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);
safeHomeConfigMutable(i)->approachAlt = sbufReadU32(src); } else {
safeHomeConfigMutable(i)->landAlt = sbufReadU32(src); return MSP_RESULT_ERROR;
safeHomeConfigMutable(i)->approachDirection = sbufReadU8(src); }
break;
#endif
case MSP2_INAV_SET_FW_APPROACH:
if (dataSize == 15) {
uint8_t i;
if (!sbufReadU8Safe(&i, src) || i >= MAX_FW_LAND_APPOACH_SETTINGS) {
return MSP_RESULT_ERROR;
}
fwAutolandApproachConfigMutable(i)->approachAlt = sbufReadU32(src);
fwAutolandApproachConfigMutable(i)->landAlt = sbufReadU32(src);
fwAutolandApproachConfigMutable(i)->approachDirection = sbufReadU8(src);
int16_t head1 = 0, head2 = 0; int16_t head1 = 0, head2 = 0;
if (sbufReadI16Safe(&head1, src)) { if (sbufReadI16Safe(&head1, src)) {
safeHomeConfigMutable(i)->landApproachHeading1 = head1; fwAutolandApproachConfigMutable(i)->landApproachHeading1 = head1;
} }
if (sbufReadI16Safe(&head2, src)) { if (sbufReadI16Safe(&head2, src)) {
safeHomeConfigMutable(i)->landApproachHeading2 = head2; fwAutolandApproachConfigMutable(i)->landApproachHeading2 = head2;
} }
safeHomeConfigMutable(i)->isSeaLevelRef = sbufReadU8(src); fwAutolandApproachConfigMutable(i)->isSeaLevelRef = sbufReadU8(src);
} else { } else {
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
} }
@ -3657,6 +3679,9 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
*ret = mspFcSafeHomeOutCommand(dst, src); *ret = mspFcSafeHomeOutCommand(dst, src);
break; break;
#endif #endif
case MSP2_INAV_FW_APPROACH:
*ret = mspFwApproachOutCommand(dst, src);
break;
#ifdef USE_SIMULATOR #ifdef USE_SIMULATOR
case MSP_SIMULATOR: case MSP_SIMULATOR:

2
src/main/flight/pid.c
View file

@ -593,7 +593,7 @@ 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
if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle()) { if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle() && !isFwLandInProgess()) {
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);
} }

3
src/main/msp/msp_protocol_v2_inav.h
View file

@ -92,6 +92,9 @@
#define MSP2_INAV_LED_STRIP_CONFIG_EX 0x2048 #define MSP2_INAV_LED_STRIP_CONFIG_EX 0x2048
#define MSP2_INAV_SET_LED_STRIP_CONFIG_EX 0x2049 #define MSP2_INAV_SET_LED_STRIP_CONFIG_EX 0x2049
#define MSP2_INAV_FW_APPROACH 0x204A
#define MSP2_INAV_SET_FW_APPROACH 0x204B
#define MSP2_INAV_RATE_DYNAMICS 0x2060 #define MSP2_INAV_RATE_DYNAMICS 0x2060
#define MSP2_INAV_SET_RATE_DYNAMICS 0x2061 #define MSP2_INAV_SET_RATE_DYNAMICS 0x2061

77
src/main/navigation/navigation.c
View file

@ -93,6 +93,7 @@ radar_pois_t radar_pois[RADAR_MAX_POIS];
PG_REGISTER_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig, PG_SAFE_HOME_CONFIG , 0); 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_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,
@ -102,6 +103,7 @@ PG_RESET_TEMPLATE(navFwAutolandConfig_t, navFwAutolandConfig,
.glidePitch = SETTING_NAV_FW_LAND_GLIDE_PITCH_DEFAULT, .glidePitch = SETTING_NAV_FW_LAND_GLIDE_PITCH_DEFAULT,
); );
PG_REGISTER_ARRAY(navFwAutolandApproach_t, MAX_FW_LAND_APPOACH_SETTINGS, fwAutolandApproachConfig, PG_FW_AUTOLAND_APPROACH_CONFIG, 0);
#endif #endif
@ -292,7 +294,6 @@ 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);
static bool allowFwAutoland(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);
@ -1694,8 +1695,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
} }
if (STATE(FIXED_WING_LEGACY)) { if (STATE(FIXED_WING_LEGACY)) {
if (allowFwAutoland()) { // FW Autoland configuured?
resetFwAutoland(); if (!posControl.fwLandAborted && safehome_index >= 0 && (fwAutolandApproachConfig(safehome_index)->landApproachHeading1 != 0 || fwAutolandApproachConfig(safehome_index)->landApproachHeading2 != 0)) {
posControl.fwLandPos = posControl.rthState.homePosition.pos;
posControl.fwApproachSettingIdx = safehome_index;
posControl.fwLandAltAgl = fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->isSeaLevelRef ? fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landAlt - GPS_home.alt : fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landAlt;
posControl.fwLandAproachAltAgl = fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->isSeaLevelRef ? fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->approachAlt - GPS_home.alt : fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->approachAlt;
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING; return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING;
} else { } else {
return NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; return NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME;
@ -2177,6 +2183,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_CLIMB_TO_LOI
if (isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) { if (isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) {
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT; return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT;
} }
if (posControl.fwLandLoiterStartTime == 0) {
posControl.fwLandLoiterStartTime = micros();
}
if (ABS(getEstimatedActualPosition(Z) - posControl.fwLandAproachAltAgl) < (navConfig()->general.waypoint_enforce_altitude > 0 ? navConfig()->general.waypoint_enforce_altitude : FW_LAND_LOITER_ALT_TOLERANCE)) { if (ABS(getEstimatedActualPosition(Z) - posControl.fwLandAproachAltAgl) < (navConfig()->general.waypoint_enforce_altitude > 0 ? navConfig()->general.waypoint_enforce_altitude : FW_LAND_LOITER_ALT_TOLERANCE)) {
updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET); updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
@ -2211,9 +2221,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_LOITER(navig
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT; return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT;
} }
if (posControl.fwLandLoiterStartTime == 0) { if (micros() - posControl.fwLandLoiterStartTime > FW_LAND_LOITER_MIN_TIME) {
posControl.fwLandLoiterStartTime = micros();
} else if (micros() - posControl.fwLandLoiterStartTime > FW_LAND_LOITER_MIN_TIME) {
if (isEstimatedWindSpeedValid()) { if (isEstimatedWindSpeedValid()) {
uint16_t windAngle = 0; uint16_t windAngle = 0;
@ -2223,16 +2231,16 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_LOITER(navig
// Ignore low wind speed, could be the error of the wind estimator // Ignore low wind speed, could be the error of the wind estimator
if (windSpeed < navFwAutolandConfig()->maxTailwind) { if (windSpeed < navFwAutolandConfig()->maxTailwind) {
if (safeHomeConfig(safehome_index)->landApproachHeading1 != 0) { if (fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landApproachHeading1 != 0) {
approachHeading = posControl.fwLandingDirection = ABS(DEGREES_TO_CENTIDEGREES(safeHomeConfig(safehome_index)->landApproachHeading1)); approachHeading = posControl.fwLandingDirection = ABS(DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landApproachHeading1));
} else if ((safeHomeConfig(safehome_index)->landApproachHeading2 != 0) ) { } else if ((fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landApproachHeading2 != 0) ) {
approachHeading = posControl.fwLandingDirection = ABS(DEGREES_TO_CENTIDEGREES(safeHomeConfig(safehome_index)->landApproachHeading2)); approachHeading = posControl.fwLandingDirection = ABS(DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landApproachHeading2));
} else { } else {
approachHeading = posControl.fwLandingDirection = -1; approachHeading = posControl.fwLandingDirection = -1;
} }
} else { } else {
int32_t heading1 = calcFinalApproachHeading(DEGREES_TO_CENTIDEGREES(safeHomeConfig(safehome_index)->landApproachHeading1), windAngle); int32_t heading1 = calcFinalApproachHeading(DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landApproachHeading1), windAngle);
int32_t heading2 = calcFinalApproachHeading(DEGREES_TO_CENTIDEGREES(safeHomeConfig(safehome_index)->landApproachHeading2), windAngle); int32_t heading2 = calcFinalApproachHeading(DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landApproachHeading2), windAngle);
if (heading1 == heading2 || heading1 == wrap_36000(heading2 + 18000)) { if (heading1 == heading2 || heading1 == wrap_36000(heading2 + 18000)) {
heading2 = -1; heading2 = -1;
@ -2240,17 +2248,17 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_LOITER(navig
if (heading1 == -1 && heading2 >= 0) { if (heading1 == -1 && heading2 >= 0) {
posControl.fwLandingDirection = heading2; posControl.fwLandingDirection = heading2;
approachHeading = DEGREES_TO_CENTIDEGREES(safeHomeConfig(safehome_index)->landApproachHeading2); approachHeading = DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landApproachHeading2);
} else if (heading1 >= 0 && heading2 == -1) { } else if (heading1 >= 0 && heading2 == -1) {
posControl.fwLandingDirection = heading1; posControl.fwLandingDirection = heading1;
approachHeading = DEGREES_TO_CENTIDEGREES(safeHomeConfig(safehome_index)->landApproachHeading1); approachHeading = DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landApproachHeading1);
} else { } else {
if (calcWindDiff(heading1, windAngle) < calcWindDiff(heading2, windAngle)) { if (calcWindDiff(heading1, windAngle) < calcWindDiff(heading2, windAngle)) {
posControl.fwLandingDirection = heading1; posControl.fwLandingDirection = heading1;
approachHeading = DEGREES_TO_CENTIDEGREES(safeHomeConfig(safehome_index)->landApproachHeading1); approachHeading = DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landApproachHeading1);
} else { } else {
posControl.fwLandingDirection = heading2; posControl.fwLandingDirection = heading2;
approachHeading = DEGREES_TO_CENTIDEGREES(safeHomeConfig(safehome_index)->landApproachHeading2); approachHeading = DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landApproachHeading2);
} }
} }
} }
@ -2260,22 +2268,22 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_LOITER(navig
int32_t finalApproachAlt = posControl.fwLandAproachAltAgl / 3 * 2; int32_t finalApproachAlt = posControl.fwLandAproachAltAgl / 3 * 2;
int32_t dir = 0; int32_t dir = 0;
if (safeHomeConfig(safehome_index)->approachDirection == FW_AUTOLAND_APPROACH_DIRECTION_LEFT) { if (fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->approachDirection == FW_AUTOLAND_APPROACH_DIRECTION_LEFT) {
dir = wrap_36000(approachHeading - 9000); dir = wrap_36000(ABS(approachHeading) - 9000);
} else { } else {
dir = wrap_36000(approachHeading + 9000); dir = wrap_36000(ABS(approachHeading) + 9000);
} }
calculateFarAwayPos(&tmpPos, &posControl.rthState.homePosition.pos, posControl.fwLandingDirection, navFwAutolandConfig()->approachLength); calculateFarAwayPos(&tmpPos, &posControl.fwLandPos, posControl.fwLandingDirection, navFwAutolandConfig()->approachLength);
tmpPos.z = posControl.fwLandAltAgl - finalApproachAlt; tmpPos.z = posControl.fwLandAltAgl - finalApproachAlt;
posControl.fwLandWaypoint[FW_AUTOLAND_WP_LAND] = tmpPos; posControl.fwLandWaypoint[FW_AUTOLAND_WP_LAND] = tmpPos;
calculateFarAwayPos(&tmpPos, &posControl.rthState.homePosition.pos, wrap_36000(posControl.fwLandingDirection + 18000), navFwAutolandConfig()->approachLength); calculateFarAwayPos(&tmpPos, &posControl.fwLandPos, wrap_36000(posControl.fwLandingDirection + 18000), navFwAutolandConfig()->approachLength);
tmpPos.z = finalApproachAlt; tmpPos.z = finalApproachAlt;
posControl.fwLandWaypoint[FW_AUTOLAND_WP_FINAL_APPROACH] = tmpPos; posControl.fwLandWaypoint[FW_AUTOLAND_WP_FINAL_APPROACH] = tmpPos;
calculateFarAwayPos(&tmpPos, &posControl.fwLandWaypoint[FW_AUTOLAND_WP_FINAL_APPROACH], dir, navFwAutolandConfig()->approachLength / 2); calculateFarAwayPos(&tmpPos, &posControl.fwLandWaypoint[FW_AUTOLAND_WP_FINAL_APPROACH], dir, navFwAutolandConfig()->approachLength / 2);
tmpPos.z = finalApproachAlt; tmpPos.z = posControl.fwLandAltAgl;
posControl.fwLandWaypoint[FW_AUTOLAND_WP_TURN] = tmpPos; posControl.fwLandWaypoint[FW_AUTOLAND_WP_TURN] = tmpPos;
setLandWaypoint(&posControl.fwLandWaypoint[FW_AUTOLAND_WP_TURN], &posControl.fwLandWaypoint[FW_AUTOLAND_WP_FINAL_APPROACH]); setLandWaypoint(&posControl.fwLandWaypoint[FW_AUTOLAND_WP_TURN], &posControl.fwLandWaypoint[FW_AUTOLAND_WP_FINAL_APPROACH]);
@ -2284,16 +2292,16 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_LOITER(navig
return NAV_FSM_EVENT_SUCCESS; return NAV_FSM_EVENT_SUCCESS;
} else { } else {
return NAV_FSM_EVENT_SWITCH_TO_IDLE; posControl.fwLandLoiterStartTime = micros();
} }
} else { } else {
posControl.fwLandLoiterStartTime = micros(); posControl.fwLandLoiterStartTime = micros();
} }
} }
fpVector3_t tmpPoint = posControl.rthState.homePosition.pos; fpVector3_t tmpPoint = posControl.fwLandPos;
tmpPoint.z = posControl.fwLandAproachAltAgl; tmpPoint.z = posControl.fwLandAproachAltAgl;
setDesiredPosition(&tmpPoint, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); setDesiredPosition(&tmpPoint, posControl.fwLandPosHeading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
return NAV_FSM_EVENT_NONE; return NAV_FSM_EVENT_NONE;
} }
@ -2311,7 +2319,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(nav
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT; return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT;
} }
if (getEstimatedActualPosition(Z) <= safeHomeConfig(safehome_index)->landAlt + navFwAutolandConfig()->glideAltitude - (safeHomeConfig(safehome_index)->isSeaLevelRef ? GPS_home.alt : 0)) { if (getEstimatedActualPosition(Z) <= fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landAlt + navFwAutolandConfig()->glideAltitude - (fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->isSeaLevelRef ? GPS_home.alt : 0)) {
resetPositionController(); resetPositionController();
posControl.fwLandState = FW_AUTOLAND_STATE_GLIDE; posControl.fwLandState = FW_AUTOLAND_STATE_GLIDE;
return NAV_FSM_EVENT_SUCCESS; return NAV_FSM_EVENT_SUCCESS;
@ -4117,6 +4125,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
posControl.flags.rthTrackbackActive = false; posControl.flags.rthTrackbackActive = false;
posControl.rthTBWrapAroundCounter = -1; posControl.rthTBWrapAroundCounter = -1;
posControl.fwLandAborted = false; posControl.fwLandAborted = false;
posControl.fwApproachSettingIdx = 0;
return; return;
} }
@ -4986,15 +4995,12 @@ int32_t navigationGetHeadingError(void)
static void resetFwAutoland(void) static void resetFwAutoland(void)
{ {
posControl.fwLandAltAgl = safeHomeConfig(safehome_index)->isSeaLevelRef ? safeHomeConfig(safehome_index)->landAlt - GPS_home.alt : safeHomeConfig(safehome_index)->landAlt; posControl.fwLandAltAgl = 0;
posControl.fwLandAproachAltAgl = safeHomeConfig(safehome_index)->isSeaLevelRef ? safeHomeConfig(safehome_index)->approachAlt - GPS_home.alt : safeHomeConfig(safehome_index)->approachAlt; posControl.fwLandAproachAltAgl = 0;
posControl.fwLandLoiterStartTime = 0; posControl.fwLandLoiterStartTime = 0;
posControl.fwLandWpReached = false; posControl.fwLandWpReached = false;
} posControl.fwApproachSettingIdx = 0;
posControl.fwLandPosHeading = -1;
static bool allowFwAutoland(void)
{
return !posControl.fwLandAborted && safehome_index >= 0 && (safeHomeConfig(safehome_index)->landApproachHeading1 != 0 || safeHomeConfig(safehome_index)->landApproachHeading2 != 0);
} }
static int32_t calcFinalApproachHeading(int32_t approachHeading, int32_t windAngle) static int32_t calcFinalApproachHeading(int32_t approachHeading, int32_t windAngle)
@ -5037,6 +5043,11 @@ static void setLandWaypoint(const fpVector3_t *pos, const fpVector3_t *nextWpPos
posControl.wpAltitudeReached = false; posControl.wpAltitudeReached = false;
} }
void resetFwAutolandApproach(void)
{
memset(fwAutolandApproachConfigMutable(0), 0, sizeof(navFwAutolandApproach_t) * MAX_FW_LAND_APPOACH_SETTINGS);
}
bool isFwLandInProgess(void) bool isFwLandInProgess(void)
{ {
return posControl.navState == NAV_STATE_FW_LANDING_CLIMB_TO_LOITER return posControl.navState == NAV_STATE_FW_LANDING_CLIMB_TO_LOITER

18
src/main/navigation/navigation.h
View file

@ -31,6 +31,7 @@
#include "io/gps.h" #include "io/gps.h"
/* GPS Home location data */ /* GPS Home location data */
extern gpsLocation_t GPS_home; extern gpsLocation_t GPS_home;
extern uint32_t GPS_distanceToHome; // distance to home point in meters extern uint32_t GPS_distanceToHome; // distance to home point in meters
@ -43,6 +44,14 @@ 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 {
uint8_t enabled;
int32_t lat;
int32_t lon;
int8_t fwLandSettingsIdx;
} navSafeHome_t;
typedef enum { typedef enum {
FW_AUTOLAND_APPROACH_DIRECTION_LEFT, FW_AUTOLAND_APPROACH_DIRECTION_LEFT,
@ -57,17 +66,17 @@ typedef enum {
FW_AUTOLAND_STATE_GLIDE, FW_AUTOLAND_STATE_GLIDE,
FW_AUTOLAND_STATE_FLARE FW_AUTOLAND_STATE_FLARE
} fwAutolandState_t; } fwAutolandState_t;
typedef struct { typedef struct {
uint8_t enabled;
int32_t lat;
int32_t lon;
int32_t approachAlt; int32_t approachAlt;
int32_t landAlt; int32_t landAlt;
bool isSeaLevelRef; bool isSeaLevelRef;
fwAutolandApproachDirection_e approachDirection; fwAutolandApproachDirection_e approachDirection;
int16_t landApproachHeading1; int16_t landApproachHeading1;
int16_t landApproachHeading2; int16_t landApproachHeading2;
} navSafeHome_t; } navFwAutolandApproach_t;
PG_DECLARE_ARRAY(navFwAutolandApproach_t, MAX_FW_LAND_APPOACH_SETTINGS, fwAutolandApproachConfig);
typedef enum { typedef enum {
SAFEHOME_USAGE_OFF = 0, // Don't use safehomes SAFEHOME_USAGE_OFF = 0, // Don't use safehomes
@ -94,6 +103,7 @@ extern int8_t safehome_index; // -1 if no safehome, 0 to MAX_
extern uint32_t safehome_distance; // distance to the nearest safehome extern uint32_t safehome_distance; // distance to the nearest safehome
extern bool safehome_applied; // whether the safehome has been applied to home. extern bool safehome_applied; // whether the safehome has been applied to home.
void resetFwAutolandApproach(void);
void resetSafeHomes(void); // remove all safehomes void resetSafeHomes(void); // remove all safehomes
bool findNearestSafeHome(void); // Find nearest safehome bool findNearestSafeHome(void); // Find nearest safehome

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

@ -624,16 +624,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
rcCommand[PITCH] = -pidAngleToRcCommand(pitchCorrection, pidProfile()->max_angle_inclination[FD_PITCH]); rcCommand[PITCH] = -pidAngleToRcCommand(pitchCorrection, pidProfile()->max_angle_inclination[FD_PITCH]);
int16_t throttleCorrection = fixedWingPitchToThrottleCorrection(pitchCorrection, currentTimeUs); int16_t throttleCorrection = fixedWingPitchToThrottleCorrection(pitchCorrection, currentTimeUs);
#ifdef NAV_FIXED_WING_LANDING throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, maxThrottleCorrection);
if ((navStateFlags & NAV_CTL_LAND) || isFwLandInProgess()) {
// During LAND we do not allow to raise THROTTLE when nose is up to reduce speed
throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, 0);
} else {
#endif
throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, maxThrottleCorrection);
#ifdef NAV_FIXED_WING_LANDING
}
#endif
// Speed controller - only apply in POS mode when NOT NAV_CTL_LAND // Speed controller - only apply in POS mode when NOT NAV_CTL_LAND
if ((navStateFlags & NAV_CTL_POS) && !(navStateFlags & NAV_CTL_LAND)) { if ((navStateFlags & NAV_CTL_POS) && !(navStateFlags & NAV_CTL_LAND)) {

445
src/main/navigation/navigation_fw_autoland.c
View file

@ -1,445 +0,0 @@
/*
* This file is part of INAV.
*
* INAV is free software. You can redistribute this software
* and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* INAV is distributed in the hope that they will be
* useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <math.h>
#include "platform.h"
#if defined(USE_SAFE_HOME) && defined(USE_FW_AUTOLAND)
#include <navigation/navigation_fw_autoland.h>
#include "common/utils.h"
#include "flight/wind_estimator.h"
#include "fc/rc_controls.h"
#include "fc/settings.h"
#include "rx/rx.h"
#include "flight/mixer.h"
#include "navigation/navigation_private.h"
#include "config/parameter_group_ids.h"
#define LOITER_MIN_TIME 30000000 // usec (30 sec)
#define TARGET_ALT_TOLERANCE 150
#define LAND_WP_TURN 0
#define LAND_WP_FINAL_APPROACH 1
#define LAND_WP_LAND 2
typedef enum {
FW_AUTOLAND_EVENT_NONE,
FW_AUTOLAND_EVENT_SUCCSESS,
FW_AUTOLAND_EVENT_ABORT,
FW_AUTOLAND_EVENT_COUNT
} fwAutolandEvent_t;
typedef struct fixedWingAutolandStateDescriptor_s {
fwAutolandEvent_t (*onEntry)(timeUs_t currentTimeUs);
fwAutolandState_t onEvent[FW_AUTOLAND_EVENT_COUNT];
fwAutolandMessageState_t message;
} fixedWingAutolandStateDescriptor_t;
typedef struct fixedWingAutolandData_s {
timeUs_t currentStateTimeUs;
fwAutolandState_t currentState;
timeUs_t loiterStartTime;
fpVector3_t waypoint[3];
fpVector3_t flareWp;
uint32_t finalApproachLength;
} fixedWingAutolandData_t;
static EXTENDED_FASTRAM fixedWingAutolandData_t fwAutoland;
static fwAutolandEvent_t fwAutolandState_ABOVE_LOITER_ALT(timeUs_t currentTimeUs);
static fwAutolandEvent_t fwAutolandState_LOITER(timeUs_t currentTimeUs);
static fwAutolandEvent_t fwAutolandState_DOWNWIND(timeUs_t currentTimeUs);
static fwAutolandEvent_t fwAutolandState_BASE(timeUs_t currentTimeUs);
static fwAutolandEvent_t fwAutolandState_FINAL_APPROACH(timeUs_t currentTimeUs);
static fwAutolandEvent_t fwAutolandState_GLIDE(timeUs_t currentTimeUs);
static fwAutolandEvent_t fwAutolandState_FLARE(timeUs_t currentTimeUs);
static fwAutolandEvent_t fwAutolandState_ABORT(timeUs_t currentTimeUs);
static const fixedWingAutolandStateDescriptor_t autolandStateMachine[FW_AUTOLAND_STATE_COUNT] = {
[FW_AUTOLAND_STATE_ABOVE_LOITER_ALT] = {
.onEntry = fwAutolandState_ABOVE_LOITER_ALT,
.onEvent = {
[FW_AUTOLAND_EVENT_SUCCSESS] = FW_AUTOLAND_STATE_LOITER,
[FW_AUTOLAND_EVENT_ABORT] = FW_AUTOLAND_STATE_ABORT,
},
.message = FW_AUTOLAND_MESSAGE_ABOVE_LOITER_ALT
},
[FW_AUTOLAND_STATE_LOITER] = {
.onEntry = fwAutolandState_LOITER,
.onEvent = {
[FW_AUTOLAND_EVENT_SUCCSESS] = FW_AUTOLAND_STATE_DOWNWIND,
[FW_AUTOLAND_EVENT_ABORT] = FW_AUTOLAND_STATE_ABORT,
},
.message = FW_AUTOLAND_MESSAGE_LOITER
},
[FW_AUTOLAND_STATE_DOWNWIND] = {
.onEntry = fwAutolandState_DOWNWIND,
.onEvent = {
[FW_AUTOLAND_EVENT_SUCCSESS] = FW_AUTOLAND_STATE_BASE,
[FW_AUTOLAND_EVENT_ABORT] = FW_AUTOLAND_STATE_ABORT,
},
.message = FW_AUTOLAND_MESSAGE_LOITER
},
[FW_AUTOLAND_STATE_BASE] = {
.onEntry = fwAutolandState_BASE,
.onEvent = {
[FW_AUTOLAND_EVENT_SUCCSESS] = FW_AUTOLAND_STATE_FINAL_APPROACH,
[FW_AUTOLAND_EVENT_ABORT] = FW_AUTOLAND_STATE_ABORT,
},
.message = FW_AUTOLAND_MESSAGE_LOITER
},
[FW_AUTOLAND_STATE_FINAL_APPROACH] = {
.onEntry = fwAutolandState_FINAL_APPROACH,
.onEvent = {
[FW_AUTOLAND_EVENT_SUCCSESS] = FW_AUTOLAND_STATE_GLIDE,
[FW_AUTOLAND_EVENT_ABORT] = FW_AUTOLAND_STATE_ABORT,
},
.message = FW_AUTOLAND_MESSAGE_LOITER
},
[FW_AUTOLAND_STATE_GLIDE] = {
.onEntry = fwAutolandState_GLIDE,
.onEvent = {
[FW_AUTOLAND_EVENT_SUCCSESS] = FW_AUTOLAND_STATE_FLARE,
[FW_AUTOLAND_EVENT_ABORT] = FW_AUTOLAND_STATE_ABORT,
},
.message = FW_AUTOLAND_MESSAGE_LOITER
},
[FW_AUTOLAND_STATE_FLARE] = {
.onEntry = fwAutolandState_FLARE,
.onEvent = {
[FW_AUTOLAND_EVENT_SUCCSESS] = FW_AUTOLAND_STATE_LANDED,
[FW_AUTOLAND_EVENT_ABORT] = FW_AUTOLAND_STATE_ABORT,
},
.message = FW_AUTOLAND_MESSAGE_LOITER
}
};
PG_REGISTER_WITH_RESET_TEMPLATE(navFwAutolandConfig_t, navFwAutolandConfig, PG_FW_AUTOLAND_CONFIG, 0);
PG_RESET_TEMPLATE(navFwAutolandConfig_t, navFwAutolandConfig,
.approachAngle = SETTING_NAV_FW_LAND_APPROACH_ANGLE_DEFAULT,
.glideAltitude = SETTING_NAV_FW_LAND_GLIDE_ALT_DEFAULT,
.flareAltitude = SETTING_NAV_FW_LAND_FLARE_ALT_DEFAULT,
.flarePitch = SETTING_NAV_FW_LAND_FLARE_PITCH_DEFAULT,
.maxTailwind = SETTING_NAV_FW_LAND_MAX_TAILWIND_DEFAULT,
);
static uint32_t getFinalApproachHeading(int32_t approachHeading, int32_t windAngle, int32_t *windAngleRelToRunway)
{
if (approachHeading == 0) {
return 0;
}
*windAngleRelToRunway = wrap_36000(windAngle - ABS(approachHeading));
if (*windAngleRelToRunway > 27000 || *windAngleRelToRunway < 9000) {
return approachHeading;
}
if (approachHeading > 0) {
return wrap_36000(approachHeading + 18000);
}
return 0;
}
static int32_t calcApproachLength(int32_t finalApproachAlt, int16_t glidePitch)
{
return finalApproachAlt * 1.0f / sin_approx(DEGREES_TO_RADIANS(glidePitch)) * sin_approx(DEGREES_TO_RADIANS(90 - glidePitch));
}
static void setLandWaypoint(const fpVector3_t *pos, const fpVector3_t *nextWpPos)
{
if (posControl.activeWaypoint.pos.x == 0 && posControl.activeWaypoint.pos.y == 0) {
posControl.activeWaypoint.bearing = calculateBearingToDestination(pos);
} else {
posControl.activeWaypoint.bearing = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, pos);
}
posControl.activeWaypoint.pos = *pos;
setDesiredPosition(&posControl.activeWaypoint.pos, posControl.activeWaypoint.bearing, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
if (navConfig()->fw.wp_turn_smoothing && nextWpPos != NULL) {
int32_t bearingToNextWp = calculateBearingBetweenLocalPositions(pos, nextWpPos);
posControl.activeWaypoint.nextTurnAngle = wrap_18000(bearingToNextWp - posControl.activeWaypoint.bearing);
} else {
posControl.activeWaypoint.nextTurnAngle = -1;
}
posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos);
posControl.wpInitialAltitude = posControl.actualState.abs.pos.z;
posControl.wpAltitudeReached = false;
}
static void checkLandWpAndUpdateZ(void)
{
fpVector3_t tmpWaypoint;
tmpWaypoint.x = posControl.activeWaypoint.pos.x;
tmpWaypoint.y = posControl.activeWaypoint.pos.y;
tmpWaypoint.z = scaleRangef(constrainf(posControl.wpDistance, posControl.wpInitialDistance / 10.0f, posControl.wpInitialDistance),
posControl.wpInitialDistance, posControl.wpInitialDistance / 10.0f,
posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z);
setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
}
static bool isLandWpReached(const fpVector3_t * waypointPos, const int32_t * waypointBearing)
{
posControl.wpDistance = calculateDistanceToDestination(waypointPos);
/*
if (getFwAutolandState() == FW_AUTOLAND_STATE_BASE) {
return calculateDistanceToDestination(&fwAutoland.waypoint[LAND_WP_LAND]) <= fwAutoland.finalApproachLength - navConfig()->fw.loiter_radius;
}
*/
if (posControl.flags.wpTurnSmoothingActive) {
posControl.flags.wpTurnSmoothingActive = false;
return true;
}
uint16_t relativeBearing = posControl.flags.wpTurnSmoothingActive ? 6000 : 10000;
if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointBearing)) > relativeBearing) {
return true;
}
return posControl.wpDistance <= (navConfig()->general.waypoint_radius);
}
static fwAutolandEvent_t fwAutolandState_ABOVE_LOITER_ALT(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
int32_t approachAltAbs = safeHomeConfig(safehome_index)->approachAlt - GPS_home.alt;
if (ABS(getEstimatedActualPosition(Z) - approachAltAbs) < TARGET_ALT_TOLERANCE) {
return FW_AUTOLAND_EVENT_SUCCSESS;
} else {
fpVector3_t tmpPoint = posControl.rthState.homePosition.pos;
tmpPoint.z = approachAltAbs;
setDesiredPosition(&tmpPoint, 0, NAV_POS_UPDATE_Z);
}
return FW_AUTOLAND_EVENT_NONE;
}
static fwAutolandEvent_t fwAutolandState_LOITER(timeUs_t currentTimeUs)
{
#ifndef USE_WIND_ESTIMATOR
return FW_AUTOLAND_EVENT_ABORT;
#endif
int32_t landAltAbs = safeHomeConfig(safehome_index)->landAlt - GPS_home.alt;
int32_t approachAltAbs = safeHomeConfig(safehome_index)->approachAlt + landAltAbs - GPS_home.alt;
if (fwAutoland.loiterStartTime == 0) {
fwAutoland.loiterStartTime = currentTimeUs;
} else if (micros() - fwAutoland.loiterStartTime > LOITER_MIN_TIME) {
if (isEstimatedWindSpeedValid()) {
uint16_t windAngle = 0;
uint32_t landingDirection = 0;
float windSpeed = getEstimatedHorizontalWindSpeed(&windAngle);
windAngle = wrap_36000(windAngle + 18000);
int32_t windAngel1 = 0, windAngel2 = 0;
int32_t heading1 = getFinalApproachHeading(DEGREES_TO_CENTIDEGREES(safeHomeConfig(safehome_index)->landApproachHeading1), windAngle, &windAngel1);
int32_t heading2 = getFinalApproachHeading(DEGREES_TO_CENTIDEGREES(safeHomeConfig(safehome_index)->landApproachHeading2), windAngle, &windAngel2);
if (heading1 == 0 && heading2 == 0 && windSpeed < navFwAutolandConfig()->maxTailwind) {
heading1 = safeHomeConfig(safehome_index)->landApproachHeading1;
heading2 = safeHomeConfig(safehome_index)->landApproachHeading2;
}
if (heading1 == 0 && heading2 > 0) {
landingDirection = heading2;
} else if (heading1 > 0 && heading2 == 0) {
landingDirection = heading1;
} else {
if (windAngel1 < windAngel2) {
landingDirection = heading1;
}
else {
landingDirection = heading2;
}
}
if (landingDirection != 0) {
fpVector3_t tmpPos;
int32_t finalApproachAlt = approachAltAbs / 3 * 2;
fwAutoland.finalApproachLength = calcApproachLength(finalApproachAlt, navFwAutolandConfig()->approachAngle);
int32_t dir = 0;
if (safeHomeConfig(safehome_index)->approachDirection == FW_AUTOLAND_APPROACH_DIRECTION_LEFT) {
dir = wrap_36000(landingDirection - 9000);
} else {
dir = wrap_36000(landingDirection + 9000);
}
tmpPos = posControl.rthState.homePosition.pos;
tmpPos.z = landAltAbs;
fwAutoland.waypoint[LAND_WP_LAND] = tmpPos;
fpVector3_t tmp2;
calculateFarAwayPos(&tmp2, &posControl.rthState.homePosition.pos, wrap_36000(landingDirection + 18000), fwAutoland.finalApproachLength);
calculateFarAwayPos(&tmpPos, &tmp2, dir, navConfig()->fw.loiter_radius);
tmp2.z = finalApproachAlt;
fwAutoland.waypoint[LAND_WP_FINAL_APPROACH] = tmp2;
calculateFarAwayPos(&tmpPos, &fwAutoland.waypoint[LAND_WP_FINAL_APPROACH], dir, (fwAutoland.finalApproachLength / 2));
tmpPos.z = finalApproachAlt;
fwAutoland.waypoint[LAND_WP_TURN] = tmpPos;
setLandWaypoint(&fwAutoland.waypoint[LAND_WP_TURN], &fwAutoland.waypoint[LAND_WP_FINAL_APPROACH]);
return FW_AUTOLAND_EVENT_SUCCSESS;
} else {
return FW_AUTOLAND_EVENT_ABORT;
}
} else {
fwAutoland.loiterStartTime = currentTimeUs;
}
}
fpVector3_t tmpPoint = posControl.rthState.homePosition.pos;
tmpPoint.z = approachAltAbs;
setDesiredPosition(&tmpPoint, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
return FW_AUTOLAND_EVENT_NONE;
}
static fwAutolandEvent_t fwAutolandState_DOWNWIND(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
if (isLandWpReached(&fwAutoland.waypoint[LAND_WP_TURN], &posControl.activeWaypoint.bearing)) {
setLandWaypoint(&fwAutoland.waypoint[LAND_WP_FINAL_APPROACH], &fwAutoland.waypoint[LAND_WP_LAND]);
return FW_AUTOLAND_EVENT_SUCCSESS;
}
checkLandWpAndUpdateZ();
return FW_AUTOLAND_EVENT_NONE;
}
static fwAutolandEvent_t fwAutolandState_BASE(timeUs_t currentTimeUs)
{
if (isLandWpReached(&fwAutoland.waypoint[LAND_WP_FINAL_APPROACH], &posControl.activeWaypoint.bearing)) {
setLandWaypoint(&fwAutoland.waypoint[LAND_WP_LAND], NULL);
/*
resetGCSFlags();
resetPositionController();
resetAltitudeController(false);
setupAltitudeController();
posControl.cruise.course = calculateBearingToDestination(&fwAutoland.waypoint[LAND_WP_LAND]);
*/
return FW_AUTOLAND_EVENT_SUCCSESS;
}
checkLandWpAndUpdateZ();
return FW_AUTOLAND_EVENT_NONE;
}
static fwAutolandEvent_t fwAutolandState_FINAL_APPROACH(timeUs_t currentTimeUs)
{
if (getEstimatedActualPosition(Z) <= safeHomeConfig(safehome_index)->landAlt + navFwAutolandConfig()->glideAltitude - GPS_home.alt) {
setLandWaypoint(&fwAutoland.waypoint[LAND_WP_LAND], NULL);
return FW_AUTOLAND_EVENT_SUCCSESS;
}
posControl.wpDistance = calculateDistanceToDestination(&fwAutoland.waypoint[LAND_WP_LAND]);
checkLandWpAndUpdateZ();
return FW_AUTOLAND_EVENT_NONE;
}
static fwAutolandEvent_t fwAutolandState_GLIDE(timeUs_t currentTimeUs)
{
if (getEstimatedActualPosition(Z) <= safeHomeConfig(safehome_index)->landAlt + navFwAutolandConfig()->flareAltitude - GPS_home.alt) {
return FW_AUTOLAND_EVENT_SUCCSESS;
}
posControl.wpDistance = calculateDistanceToDestination(&fwAutoland.waypoint[LAND_WP_LAND]);
checkLandWpAndUpdateZ();
return FW_AUTOLAND_EVENT_NONE;
}
static fwAutolandEvent_t fwAutolandState_FLARE(timeUs_t currentTimeUs)
{
rcCommand[THROTTLE] = getThrottleIdleValue();
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
rxOverrideRcChannel(PITCH, -navFwAutolandConfig()->flarePitch);
if (isLandingDetected()) {
rxOverrideRcChannel(PITCH, -1);
return FW_AUTOLAND_EVENT_SUCCSESS;
}
return FW_AUTOLAND_EVENT_NONE;
}
static fwAutolandEvent_t fwAutolandState_ABORT(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
return FW_AUTOLAND_EVENT_NONE;
}
static void setCurrentState(fwAutolandState_t nextState, timeUs_t currentTimeUs)
{
fwAutoland.currentState = nextState;
fwAutoland.currentStateTimeUs = currentTimeUs;
}
// Public methods
fwAutolandState_t getFwAutolandState(void)
{
return fwAutoland.currentState;
}
bool isFwAutolandActive(void)
{
return fwAutoland.currentState >= FW_AUTOLAND_STATE_LOITER;
}
bool allowFwAutoland(void)
{
return safehome_index >= 0 && (safeHomeConfig(safehome_index)->landApproachHeading1 != 0 || safeHomeConfig(safehome_index)->landApproachHeading2 != 0);
}
void resetFwAutolandController(timeUs_t currentTimeUs)
{
setCurrentState(FW_AUTOLAND_STATE_ABOVE_LOITER_ALT, currentTimeUs);
rxOverrideRcChannel(PITCH, -1);
fwAutoland.loiterStartTime = 0;
}
void applyFixedWingAutolandController(timeUs_t currentTimeUs)
{
while (autolandStateMachine[fwAutoland.currentState].onEntry) {
fwAutolandEvent_t newEvent = autolandStateMachine[fwAutoland.currentState].onEntry(currentTimeUs);
if (newEvent == FW_AUTOLAND_EVENT_NONE) {
break;
}
setCurrentState(autolandStateMachine[fwAutoland.currentState].onEvent[newEvent], currentTimeUs);
}
}
#endif

66
src/main/navigation/navigation_fw_autoland.h
View file

@ -1,66 +0,0 @@
/*
* This file is part of INAV.
*
* INAV is free software. You can redistribute this software
* and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* INAV is distributed in the hope that they will be
* useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "drivers/time.h"
#include "config/parameter_group.h"
typedef enum {
FW_AUTOLAND_APPROACH_DIRECTION_LEFT,
FW_AUTOLAND_APPROACH_DIRECTION_RIGHT
} fwAutolandApproachDirection_e;
typedef struct navFwAutolandConfig_s
{
uint8_t approachAngle;
uint16_t glideAltitude;
uint16_t flareAltitude;
uint16_t flarePitch;
uint16_t maxTailwind;
} navFwAutolandConfig_t;
PG_DECLARE(navFwAutolandConfig_t, navFwAutolandConfig);
typedef enum {
FW_AUTOLAND_STATE_ABOVE_LOITER_ALT,
FW_AUTOLAND_STATE_LOITER,
FW_AUTOLAND_STATE_DOWNWIND,
FW_AUTOLAND_STATE_BASE,
FW_AUTOLAND_STATE_FINAL_APPROACH,
FW_AUTOLAND_STATE_GLIDE,
FW_AUTOLAND_STATE_FLARE,
FW_AUTOLAND_STATE_SWITCH_TO_EMERGENCY_LANDING,
FW_AUTOLAND_STATE_ABORT,
FW_AUTOLAND_STATE_LANDED,
FW_AUTOLAND_STATE_COUNT
} fwAutolandState_t;
typedef enum {
FW_AUTOLAND_MESSAGE_ABOVE_LOITER_ALT,
FW_AUTOLAND_MESSAGE_LOITER,
FW_AUTOLAND_MESSAGE_ABORT,
} fwAutolandMessageState_t;
fwAutolandState_t getFwAutolandState(void);
void resetFwAutolandController(timeUs_t currentTimeUs);
bool allowFwAutoland(void);
bool isFwAutolandActive(void);
void applyFixedWingAutolandController(timeUs_t currentTimeUs);

3
src/main/navigation/navigation_private.h
View file

@ -458,9 +458,12 @@ typedef struct {
/* Fixedwing autoland */ /* Fixedwing autoland */
timeUs_t fwLandLoiterStartTime; timeUs_t fwLandLoiterStartTime;
fpVector3_t fwLandWaypoint[FW_AUTOLAND_WP_COUNT]; fpVector3_t fwLandWaypoint[FW_AUTOLAND_WP_COUNT];
fpVector3_t fwLandPos;
int32_t fwLandPosHeading;
int32_t fwLandingDirection; int32_t fwLandingDirection;
int32_t fwLandAproachAltAgl; int32_t fwLandAproachAltAgl;
int32_t fwLandAltAgl; int32_t fwLandAltAgl;
uint8_t fwApproachSettingIdx;
bool fwLandWpReached; bool fwLandWpReached;
fwAutolandWayppoints_t fwLandCurrentWp; fwAutolandWayppoints_t fwLandCurrentWp;
bool fwLandAborted; bool fwLandAborted;