mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-24 00:35:34 +03:00
Move appraoch to extra config
This commit is contained in:
Scavanger
parent
cdc1a05e7c
commit
02806b17f7
11 changed files with 193 additions and 616 deletions
|
|
@ -124,7 +124,8 @@
|
|||
#define PG_LEDPIN_CONFIG 1034
|
||||
#define PG_OSD_JOYSTICK_CONFIG 1035
|
||||
#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)
|
||||
//#define PG_OSD_FONT_CONFIG 2047
|
||||
|
|
|
|||
|
|
@ -1309,58 +1309,41 @@ static void cliTempSensor(char *cmdline)
|
|||
}
|
||||
#endif
|
||||
|
||||
#if defined(USE_SAFE_HOME)
|
||||
static void printSafeHomes(uint8_t dumpMask, const navSafeHome_t *navSafeHome, const navSafeHome_t *defaultSafeHome)
|
||||
static void printFwAutolandApproach(uint8_t dumpMask, const navFwAutolandApproach_t *navFwAutolandApproach, const navFwAutolandApproach_t *defaultFwAutolandApproach)
|
||||
{
|
||||
const char *format = "safehome %u %u %d %d %d %d %u %d %d %u"; // uint8_t enabled, int32_t lat; int32_t lon
|
||||
for (uint8_t i = 0; i < MAX_SAFE_HOMES; i++) {
|
||||
const char *format = "fwapproach %u %d %d %d %u %d %d %u";
|
||||
for (uint8_t i = 0; i < MAX_FW_LAND_APPOACH_SETTINGS; 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
|
||||
&& navSafeHome[i].approachDirection == defaultSafeHome[i].approachDirection
|
||||
&& navSafeHome[i].approachAlt == defaultSafeHome[i].approachAlt
|
||||
&& navSafeHome[i].landAlt == defaultSafeHome[i].landAlt
|
||||
&& navSafeHome[i].landApproachHeading1 == defaultSafeHome[i].landApproachHeading1
|
||||
&& navSafeHome[i].landApproachHeading2 == defaultSafeHome[i].landApproachHeading2
|
||||
&& navSafeHome[i].isSeaLevelRef == defaultSafeHome[i].isSeaLevelRef;
|
||||
if (defaultFwAutolandApproach) {
|
||||
equalsDefault = navFwAutolandApproach[i].approachDirection == defaultFwAutolandApproach[i].approachDirection
|
||||
&& navFwAutolandApproach[i].approachAlt == defaultFwAutolandApproach[i].approachAlt
|
||||
&& navFwAutolandApproach[i].landAlt == defaultFwAutolandApproach[i].landAlt
|
||||
&& navFwAutolandApproach[i].landApproachHeading1 == defaultFwAutolandApproach[i].landApproachHeading1
|
||||
&& navFwAutolandApproach[i].landApproachHeading2 == defaultFwAutolandApproach[i].landApproachHeading2
|
||||
&& navFwAutolandApproach[i].isSeaLevelRef == defaultFwAutolandApproach[i].isSeaLevelRef;
|
||||
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,
|
||||
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)) {
|
||||
printSafeHomes(DUMP_MASTER, safeHomeConfig(0), NULL);
|
||||
printFwAutolandApproach(DUMP_MASTER, fwAutolandApproachConfig(0), NULL);
|
||||
} else if (sl_strcasecmp(cmdline, "reset") == 0) {
|
||||
resetSafeHomes();
|
||||
resetFwAutolandApproach();
|
||||
} else {
|
||||
int32_t lat=0, lon=0, approachAlt = 0, heading1 = 0, heading2 = 0, landDirection = 0, landAlt = 0;
|
||||
bool enabled=false, isSeaLevelRef = false;
|
||||
int32_t approachAlt = 0, heading1 = 0, heading2 = 0, landDirection = 0, landAlt = 0;
|
||||
bool isSeaLevelRef = 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);
|
||||
cliShowArgumentRangeError("fwapproach index", 0, MAX_FW_LAND_APPOACH_SETTINGS - 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))) {
|
||||
approachAlt = fastA2I(ptr);
|
||||
validArgumentCount++;
|
||||
|
|
@ -1414,18 +1397,75 @@ static void cliSafeHomes(char *cmdline)
|
|||
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();
|
||||
} else {
|
||||
safeHomeConfigMutable(i)->enabled = enabled;
|
||||
safeHomeConfigMutable(i)->lat = lat;
|
||||
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));
|
||||
#endif
|
||||
|
||||
cliPrintHashLine("Fixed Wing Approach");
|
||||
printFwAutolandApproach(dumpMask, fwAutolandApproachConfig_CopyArray, fwAutolandApproachConfig(0));
|
||||
|
||||
cliPrintHashLine("features");
|
||||
printFeature(dumpMask, &featureConfig_Copy, featureConfig());
|
||||
|
||||
|
|
@ -4105,6 +4148,7 @@ const clicmd_t cmdTable[] = {
|
|||
CLI_COMMAND_DEF("flash_write", NULL, "<address> <message>", cliFlashWrite),
|
||||
#endif
|
||||
#endif
|
||||
CLI_COMMAND_DEF("fwapproach", "Fixed Wing Approach Settings", NULL, cliFwAutolandApproach),
|
||||
CLI_COMMAND_DEF("get", "get variable value", "[name]", cliGet),
|
||||
#ifdef USE_GPS
|
||||
CLI_COMMAND_DEF("gpspassthrough", "passthrough gps to serial", NULL, cliGpsPassthrough),
|
||||
|
|
|
|||
|
|
@ -1653,12 +1653,6 @@ static mspResult_e mspFcSafeHomeOutCommand(sbuf_t *dst, sbuf_t *src)
|
|||
sbufWriteU8(dst, safeHomeConfig(safe_home_no)->enabled);
|
||||
sbufWriteU32(dst, safeHomeConfig(safe_home_no)->lat);
|
||||
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;
|
||||
} else {
|
||||
return MSP_RESULT_ERROR;
|
||||
|
|
@ -1666,6 +1660,22 @@ static mspResult_e mspFcSafeHomeOutCommand(sbuf_t *dst, sbuf_t *src)
|
|||
}
|
||||
#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) {
|
||||
const uint8_t idx = sbufReadU8(src);
|
||||
|
|
@ -3119,7 +3129,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
|
|||
#endif
|
||||
#ifdef USE_SAFE_HOME
|
||||
case MSP2_INAV_SET_SAFEHOME:
|
||||
if (dataSize == 24) {
|
||||
if (dataSize == 10) {
|
||||
uint8_t i;
|
||||
if (!sbufReadU8Safe(&i, src) || i >= MAX_SAFE_HOMES) {
|
||||
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)->lat = sbufReadU32(src);
|
||||
safeHomeConfigMutable(i)->lon = sbufReadU32(src);
|
||||
safeHomeConfigMutable(i)->approachAlt = sbufReadU32(src);
|
||||
safeHomeConfigMutable(i)->landAlt = sbufReadU32(src);
|
||||
safeHomeConfigMutable(i)->approachDirection = sbufReadU8(src);
|
||||
} else {
|
||||
return MSP_RESULT_ERROR;
|
||||
}
|
||||
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;
|
||||
if (sbufReadI16Safe(&head1, src)) {
|
||||
safeHomeConfigMutable(i)->landApproachHeading1 = head1;
|
||||
fwAutolandApproachConfigMutable(i)->landApproachHeading1 = head1;
|
||||
}
|
||||
if (sbufReadI16Safe(&head2, src)) {
|
||||
safeHomeConfigMutable(i)->landApproachHeading2 = head2;
|
||||
fwAutolandApproachConfigMutable(i)->landApproachHeading2 = head2;
|
||||
}
|
||||
safeHomeConfigMutable(i)->isSeaLevelRef = sbufReadU8(src);
|
||||
fwAutolandApproachConfigMutable(i)->isSeaLevelRef = sbufReadU8(src);
|
||||
} else {
|
||||
return MSP_RESULT_ERROR;
|
||||
}
|
||||
|
|
@ -3657,6 +3679,9 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
|
|||
*ret = mspFcSafeHomeOutCommand(dst, src);
|
||||
break;
|
||||
#endif
|
||||
case MSP2_INAV_FW_APPROACH:
|
||||
*ret = mspFwApproachOutCommand(dst, src);
|
||||
break;
|
||||
|
||||
#ifdef USE_SIMULATOR
|
||||
case MSP_SIMULATOR:
|
||||
|
|
|
|||
|
|
@ -593,7 +593,7 @@ static float computePidLevelTarget(flight_dynamics_index_t axis) {
|
|||
#endif
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -92,6 +92,9 @@
|
|||
#define MSP2_INAV_LED_STRIP_CONFIG_EX 0x2048
|
||||
#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_SET_RATE_DYNAMICS 0x2061
|
||||
|
||||
|
|
|
|||
|
|
@ -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_WITH_RESET_TEMPLATE(navFwAutolandConfig_t, navFwAutolandConfig, PG_FW_AUTOLAND_CONFIG, 0);
|
||||
|
||||
PG_RESET_TEMPLATE(navFwAutolandConfig_t, navFwAutolandConfig,
|
||||
.approachLength = SETTING_NAV_FW_LAND_APPROACH_LENGTH_DEFAULT,
|
||||
.finalApproachPitchToThrottleMod = SETTING_NAV_FW_LAND_FINAL_APPROACH_PITCH2THROTTLE_MOD_DEFAULT,
|
||||
|
|
@ -102,6 +103,7 @@ PG_RESET_TEMPLATE(navFwAutolandConfig_t, navFwAutolandConfig,
|
|||
.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
|
||||
|
||||
|
|
@ -292,7 +294,6 @@ static bool rthAltControlStickOverrideCheck(unsigned axis);
|
|||
static void updateRthTrackback(bool forceSaveTrackPoint);
|
||||
static fpVector3_t * rthGetTrackbackPos(void);
|
||||
|
||||
static bool allowFwAutoland(void);
|
||||
static int32_t calcWindDiff(int32_t heading, int32_t windHeading);
|
||||
static int32_t calcFinalApproachHeading(int32_t approachHeading, int32_t windAngle);
|
||||
static void setLandWaypoint(const fpVector3_t *pos, const fpVector3_t *nextWpPos);
|
||||
|
|
@ -1694,8 +1695,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
|
|||
}
|
||||
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
if (allowFwAutoland()) {
|
||||
resetFwAutoland();
|
||||
// FW Autoland configuured?
|
||||
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;
|
||||
} else {
|
||||
return NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME;
|
||||
|
|
@ -2178,6 +2184,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_CLIMB_TO_LOI
|
|||
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)) {
|
||||
updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
|
||||
return NAV_FSM_EVENT_SUCCESS;
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
||||
if (posControl.fwLandLoiterStartTime == 0) {
|
||||
posControl.fwLandLoiterStartTime = micros();
|
||||
} else if (micros() - posControl.fwLandLoiterStartTime > FW_LAND_LOITER_MIN_TIME) {
|
||||
if (micros() - posControl.fwLandLoiterStartTime > FW_LAND_LOITER_MIN_TIME) {
|
||||
if (isEstimatedWindSpeedValid()) {
|
||||
|
||||
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
|
||||
if (windSpeed < navFwAutolandConfig()->maxTailwind) {
|
||||
if (safeHomeConfig(safehome_index)->landApproachHeading1 != 0) {
|
||||
approachHeading = posControl.fwLandingDirection = ABS(DEGREES_TO_CENTIDEGREES(safeHomeConfig(safehome_index)->landApproachHeading1));
|
||||
} else if ((safeHomeConfig(safehome_index)->landApproachHeading2 != 0) ) {
|
||||
approachHeading = posControl.fwLandingDirection = ABS(DEGREES_TO_CENTIDEGREES(safeHomeConfig(safehome_index)->landApproachHeading2));
|
||||
if (fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landApproachHeading1 != 0) {
|
||||
approachHeading = posControl.fwLandingDirection = ABS(DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landApproachHeading1));
|
||||
} else if ((fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landApproachHeading2 != 0) ) {
|
||||
approachHeading = posControl.fwLandingDirection = ABS(DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landApproachHeading2));
|
||||
} else {
|
||||
approachHeading = posControl.fwLandingDirection = -1;
|
||||
}
|
||||
} else {
|
||||
int32_t heading1 = calcFinalApproachHeading(DEGREES_TO_CENTIDEGREES(safeHomeConfig(safehome_index)->landApproachHeading1), windAngle);
|
||||
int32_t heading2 = calcFinalApproachHeading(DEGREES_TO_CENTIDEGREES(safeHomeConfig(safehome_index)->landApproachHeading2), windAngle);
|
||||
int32_t heading1 = calcFinalApproachHeading(DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landApproachHeading1), windAngle);
|
||||
int32_t heading2 = calcFinalApproachHeading(DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landApproachHeading2), windAngle);
|
||||
|
||||
if (heading1 == heading2 || heading1 == wrap_36000(heading2 + 18000)) {
|
||||
heading2 = -1;
|
||||
|
|
@ -2240,17 +2248,17 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_LOITER(navig
|
|||
|
||||
if (heading1 == -1 && heading2 >= 0) {
|
||||
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) {
|
||||
posControl.fwLandingDirection = heading1;
|
||||
approachHeading = DEGREES_TO_CENTIDEGREES(safeHomeConfig(safehome_index)->landApproachHeading1);
|
||||
approachHeading = DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landApproachHeading1);
|
||||
} else {
|
||||
if (calcWindDiff(heading1, windAngle) < calcWindDiff(heading2, windAngle)) {
|
||||
posControl.fwLandingDirection = heading1;
|
||||
approachHeading = DEGREES_TO_CENTIDEGREES(safeHomeConfig(safehome_index)->landApproachHeading1);
|
||||
approachHeading = DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landApproachHeading1);
|
||||
} else {
|
||||
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 dir = 0;
|
||||
if (safeHomeConfig(safehome_index)->approachDirection == FW_AUTOLAND_APPROACH_DIRECTION_LEFT) {
|
||||
dir = wrap_36000(approachHeading - 9000);
|
||||
if (fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->approachDirection == FW_AUTOLAND_APPROACH_DIRECTION_LEFT) {
|
||||
dir = wrap_36000(ABS(approachHeading) - 9000);
|
||||
} 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;
|
||||
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;
|
||||
posControl.fwLandWaypoint[FW_AUTOLAND_WP_FINAL_APPROACH] = tmpPos;
|
||||
|
||||
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;
|
||||
|
||||
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;
|
||||
} else {
|
||||
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
|
||||
posControl.fwLandLoiterStartTime = micros();
|
||||
}
|
||||
} else {
|
||||
posControl.fwLandLoiterStartTime = micros();
|
||||
}
|
||||
}
|
||||
|
||||
fpVector3_t tmpPoint = posControl.rthState.homePosition.pos;
|
||||
fpVector3_t tmpPoint = posControl.fwLandPos;
|
||||
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;
|
||||
}
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
||||
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();
|
||||
posControl.fwLandState = FW_AUTOLAND_STATE_GLIDE;
|
||||
return NAV_FSM_EVENT_SUCCESS;
|
||||
|
|
@ -4117,6 +4125,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
|
|||
posControl.flags.rthTrackbackActive = false;
|
||||
posControl.rthTBWrapAroundCounter = -1;
|
||||
posControl.fwLandAborted = false;
|
||||
posControl.fwApproachSettingIdx = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
|
|
@ -4986,15 +4995,12 @@ int32_t navigationGetHeadingError(void)
|
|||
|
||||
static void resetFwAutoland(void)
|
||||
{
|
||||
posControl.fwLandAltAgl = safeHomeConfig(safehome_index)->isSeaLevelRef ? safeHomeConfig(safehome_index)->landAlt - GPS_home.alt : safeHomeConfig(safehome_index)->landAlt;
|
||||
posControl.fwLandAproachAltAgl = safeHomeConfig(safehome_index)->isSeaLevelRef ? safeHomeConfig(safehome_index)->approachAlt - GPS_home.alt : safeHomeConfig(safehome_index)->approachAlt;
|
||||
posControl.fwLandAltAgl = 0;
|
||||
posControl.fwLandAproachAltAgl = 0;
|
||||
posControl.fwLandLoiterStartTime = 0;
|
||||
posControl.fwLandWpReached = false;
|
||||
}
|
||||
|
||||
static bool allowFwAutoland(void)
|
||||
{
|
||||
return !posControl.fwLandAborted && safehome_index >= 0 && (safeHomeConfig(safehome_index)->landApproachHeading1 != 0 || safeHomeConfig(safehome_index)->landApproachHeading2 != 0);
|
||||
posControl.fwApproachSettingIdx = 0;
|
||||
posControl.fwLandPosHeading = -1;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
void resetFwAutolandApproach(void)
|
||||
{
|
||||
memset(fwAutolandApproachConfigMutable(0), 0, sizeof(navFwAutolandApproach_t) * MAX_FW_LAND_APPOACH_SETTINGS);
|
||||
}
|
||||
|
||||
bool isFwLandInProgess(void)
|
||||
{
|
||||
return posControl.navState == NAV_STATE_FW_LANDING_CLIMB_TO_LOITER
|
||||
|
|
|
|||
|
|
@ -31,6 +31,7 @@
|
|||
|
||||
#include "io/gps.h"
|
||||
|
||||
|
||||
/* GPS Home location data */
|
||||
extern gpsLocation_t GPS_home;
|
||||
extern uint32_t GPS_distanceToHome; // distance to home point in meters
|
||||
|
|
@ -43,6 +44,14 @@ void onNewGPSData(void);
|
|||
#if defined(USE_SAFE_HOME)
|
||||
|
||||
#define MAX_SAFE_HOMES 8
|
||||
#define MAX_FW_LAND_APPOACH_SETTINGS (MAX_SAFE_HOMES + 9)
|
||||
|
||||
typedef struct {
|
||||
uint8_t enabled;
|
||||
int32_t lat;
|
||||
int32_t lon;
|
||||
int8_t fwLandSettingsIdx;
|
||||
} navSafeHome_t;
|
||||
|
||||
typedef enum {
|
||||
FW_AUTOLAND_APPROACH_DIRECTION_LEFT,
|
||||
|
|
@ -57,17 +66,17 @@ typedef enum {
|
|||
FW_AUTOLAND_STATE_GLIDE,
|
||||
FW_AUTOLAND_STATE_FLARE
|
||||
} fwAutolandState_t;
|
||||
|
||||
typedef struct {
|
||||
uint8_t enabled;
|
||||
int32_t lat;
|
||||
int32_t lon;
|
||||
int32_t approachAlt;
|
||||
int32_t landAlt;
|
||||
bool isSeaLevelRef;
|
||||
fwAutolandApproachDirection_e approachDirection;
|
||||
int16_t landApproachHeading1;
|
||||
int16_t landApproachHeading2;
|
||||
} navSafeHome_t;
|
||||
} navFwAutolandApproach_t;
|
||||
|
||||
PG_DECLARE_ARRAY(navFwAutolandApproach_t, MAX_FW_LAND_APPOACH_SETTINGS, fwAutolandApproachConfig);
|
||||
|
||||
typedef enum {
|
||||
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 bool safehome_applied; // whether the safehome has been applied to home.
|
||||
|
||||
void resetFwAutolandApproach(void);
|
||||
void resetSafeHomes(void); // remove all safehomes
|
||||
bool findNearestSafeHome(void); // Find nearest safehome
|
||||
|
||||
|
|
|
|||
|
|
@ -624,16 +624,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
|
|||
rcCommand[PITCH] = -pidAngleToRcCommand(pitchCorrection, pidProfile()->max_angle_inclination[FD_PITCH]);
|
||||
int16_t throttleCorrection = fixedWingPitchToThrottleCorrection(pitchCorrection, currentTimeUs);
|
||||
|
||||
#ifdef NAV_FIXED_WING_LANDING
|
||||
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
|
||||
if ((navStateFlags & NAV_CTL_POS) && !(navStateFlags & NAV_CTL_LAND)) {
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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);
|
||||
|
|
@ -458,9 +458,12 @@ typedef struct {
|
|||
/* Fixedwing autoland */
|
||||
timeUs_t fwLandLoiterStartTime;
|
||||
fpVector3_t fwLandWaypoint[FW_AUTOLAND_WP_COUNT];
|
||||
fpVector3_t fwLandPos;
|
||||
int32_t fwLandPosHeading;
|
||||
int32_t fwLandingDirection;
|
||||
int32_t fwLandAproachAltAgl;
|
||||
int32_t fwLandAltAgl;
|
||||
uint8_t fwApproachSettingIdx;
|
||||
bool fwLandWpReached;
|
||||
fwAutolandWayppoints_t fwLandCurrentWp;
|
||||
bool fwLandAborted;
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue