From d7b36611f91f1cb9603d40efb6079b1b4c6c5125 Mon Sep 17 00:00:00 2001 From: Scavanger Date: Thu, 9 Nov 2023 16:45:03 -0300 Subject: [PATCH] 2nd --- src/main/fc/cli.c | 39 +++++++++++++++----- src/main/fc/fc_msp.c | 34 +++++++++++++---- src/main/fc/settings.yaml | 4 +- src/main/navigation/navigation.c | 8 ++-- src/main/navigation/navigation.h | 5 ++- src/main/navigation/navigation_fw_autoland.c | 10 ++--- 6 files changed, 69 insertions(+), 31 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index a98b26a68f..65352d6d1c 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -1312,7 +1312,7 @@ static void cliTempSensor(char *cmdline) #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 %d %d %u %d %d"; // uint8_t enabled, int32_t lat; int32_t lon + 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++) { bool equalsDefault = false; if (defaultSafeHome) { @@ -1320,15 +1320,16 @@ static void printSafeHomes(uint8_t dumpMask, const navSafeHome_t *navSafeHome, c && navSafeHome[i].lat == defaultSafeHome[i].lat && navSafeHome[i].lon == defaultSafeHome[i].lon && navSafeHome[i].approachDirection == defaultSafeHome[i].approachDirection - && navSafeHome[i].approachAltMSL == defaultSafeHome[i].approachAltMSL - && navSafeHome[i].landAltMSL == defaultSafeHome[i].landAltMSL + && 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].landApproachHeading2 == defaultSafeHome[i].landApproachHeading2 + && navSafeHome[i].isSeaLevelRef == defaultSafeHome[i].isSeaLevelRef; cliDefaultPrintLinef(dumpMask, equalsDefault, format, i, - defaultSafeHome[i].enabled, defaultSafeHome[i].lat, defaultSafeHome[i].lon, defaultSafeHome[i].approachAltMSL, defaultSafeHome[i].landAltMSL, defaultSafeHome[i].approachDirection, defaultSafeHome[i].landApproachHeading1, defaultSafeHome[i].landApproachHeading2); + 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); } cliDumpPrintLinef(dumpMask, equalsDefault, format, i, - navSafeHome[i].enabled, navSafeHome[i].lat, navSafeHome[i].lon, navSafeHome[i].approachAltMSL, navSafeHome[i].landAltMSL, navSafeHome[i].approachDirection, navSafeHome[i].landApproachHeading1, navSafeHome[i].landApproachHeading2); + 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); } } @@ -1340,7 +1341,7 @@ static void cliSafeHomes(char *cmdline) resetSafeHomes(); } else { int32_t lat=0, lon=0, approachAlt = 0, heading1 = 0, heading2 = 0, landDirection = 0, landAlt = 0; - bool enabled=false; + bool enabled=false, isSeaLevelRef = false; uint8_t validArgumentCount = 0; const char *ptr = cmdline; int8_t i = fastA2I(ptr); @@ -1383,11 +1384,28 @@ static void cliSafeHomes(char *cmdline) if ((ptr = nextArg(ptr))) { heading1 = fastA2I(ptr); + + if (heading1 < -360 || heading1 > 360) { + cliShowParseError(); + return; + } + validArgumentCount++; } if ((ptr = nextArg(ptr))) { heading2 = fastA2I(ptr); + + if (heading2 < -360 || heading2 > 360) { + cliShowParseError(); + return; + } + + validArgumentCount++; + } + + if ((ptr = nextArg(ptr))) { + isSeaLevelRef = fastA2I(ptr); validArgumentCount++; } @@ -1396,17 +1414,18 @@ static void cliSafeHomes(char *cmdline) validArgumentCount++; } - if (validArgumentCount != 8) { + if (validArgumentCount != 9) { cliShowParseError(); } else { safeHomeConfigMutable(i)->enabled = enabled; safeHomeConfigMutable(i)->lat = lat; safeHomeConfigMutable(i)->lon = lon; - safeHomeConfigMutable(i)->approachAltMSL = approachAlt; - safeHomeConfigMutable(i)->landAltMSL = landAlt; + 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; } } } diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 952f13dddb..c5f9ac649d 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1653,6 +1653,12 @@ 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; @@ -3113,14 +3119,26 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) #endif #ifdef USE_SAFE_HOME case MSP2_INAV_SET_SAFEHOME: - if (dataSize == 10) { - uint8_t i; - if (!sbufReadU8Safe(&i, src) || i >= MAX_SAFE_HOMES) { - return MSP_RESULT_ERROR; - } - safeHomeConfigMutable(i)->enabled = sbufReadU8(src); - safeHomeConfigMutable(i)->lat = sbufReadU32(src); - safeHomeConfigMutable(i)->lon = sbufReadU32(src); + if (dataSize == 24) { + uint8_t i; + if (!sbufReadU8Safe(&i, src) || i >= MAX_SAFE_HOMES) { + return MSP_RESULT_ERROR; + } + 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); + + int16_t head1 = 0, head2 = 0; + if (sbufReadI16Safe(&head1, src)) { + safeHomeConfigMutable(i)->landApproachHeading1 = head1; + } + if (sbufReadI16Safe(&head2, src)) { + safeHomeConfigMutable(i)->landApproachHeading2 = head2; + } + safeHomeConfigMutable(i)->isSeaLevelRef = sbufReadU8(src); } else { return MSP_RESULT_ERROR; } diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 18d8fafc61..988df18cc2 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4096,7 +4096,7 @@ groups: min: 100 max: 100000 - name: nav_fw_land_final_approach_pitch2throttle_mod - description: "Modifier for pitch to throttle at final approach. In Percent." + description: "Modifier for pitch to throttle ratio at final approach. In Percent." default_value: 100 field: finalApproachPitchToThrottleMod min: 100 @@ -4108,7 +4108,7 @@ groups: min: 100 max: 5000 - name: nav_fw_land_flare_alt - description: "Initial altitude of the flare" + description: "Initial altitude of the flare phase" default_value: 2000 field: flareAltitude min: 0 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 770ea69b00..04a668c99b 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2310,8 +2310,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(nav if (isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) { return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT; } - - if (getEstimatedActualPosition(Z) <= safeHomeConfig(safehome_index)->landAltMSL + navFwAutolandConfig()->glideAltitude - GPS_home.alt) { + + if (getEstimatedActualPosition(Z) <= safeHomeConfig(safehome_index)->landAlt + navFwAutolandConfig()->glideAltitude - (safeHomeConfig(safehome_index)->isSeaLevelRef ? GPS_home.alt : 0)) { resetPositionController(); posControl.fwLandState = FW_AUTOLAND_STATE_GLIDE; return NAV_FSM_EVENT_SUCCESS; @@ -4986,8 +4986,8 @@ int32_t navigationGetHeadingError(void) static void resetFwAutoland(void) { - posControl.fwLandAltAgl = safeHomeConfig(safehome_index)->landAltMSL - GPS_home.alt; - posControl.fwLandAproachAltAgl = safeHomeConfig(safehome_index)->approachAltMSL + posControl.fwLandAltAgl - GPS_home.alt; + 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.fwLandLoiterStartTime = 0; posControl.fwLandWpReached = false; } diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index da5dd0cf0a..c1d0e590eb 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -61,8 +61,9 @@ typedef struct { uint8_t enabled; int32_t lat; int32_t lon; - int32_t approachAltMSL; - int32_t landAltMSL; + int32_t approachAlt; + int32_t landAlt; + bool isSeaLevelRef; fwAutolandApproachDirection_e approachDirection; int16_t landApproachHeading1; int16_t landApproachHeading2; diff --git a/src/main/navigation/navigation_fw_autoland.c b/src/main/navigation/navigation_fw_autoland.c index 4f97dbf30f..5c726a01d3 100644 --- a/src/main/navigation/navigation_fw_autoland.c +++ b/src/main/navigation/navigation_fw_autoland.c @@ -230,7 +230,7 @@ static bool isLandWpReached(const fpVector3_t * waypointPos, const int32_t * way static fwAutolandEvent_t fwAutolandState_ABOVE_LOITER_ALT(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); - int32_t approachAltAbs = safeHomeConfig(safehome_index)->approachAltMSL - GPS_home.alt; + int32_t approachAltAbs = safeHomeConfig(safehome_index)->approachAlt - GPS_home.alt; if (ABS(getEstimatedActualPosition(Z) - approachAltAbs) < TARGET_ALT_TOLERANCE) { return FW_AUTOLAND_EVENT_SUCCSESS; @@ -249,8 +249,8 @@ static fwAutolandEvent_t fwAutolandState_LOITER(timeUs_t currentTimeUs) return FW_AUTOLAND_EVENT_ABORT; #endif - int32_t landAltAbs = safeHomeConfig(safehome_index)->landAltMSL - GPS_home.alt; - int32_t approachAltAbs = safeHomeConfig(safehome_index)->approachAltMSL + landAltAbs - GPS_home.alt; + 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) { @@ -364,7 +364,7 @@ static fwAutolandEvent_t fwAutolandState_BASE(timeUs_t currentTimeUs) static fwAutolandEvent_t fwAutolandState_FINAL_APPROACH(timeUs_t currentTimeUs) { - if (getEstimatedActualPosition(Z) <= safeHomeConfig(safehome_index)->landAltMSL + navFwAutolandConfig()->glideAltitude - GPS_home.alt) { + if (getEstimatedActualPosition(Z) <= safeHomeConfig(safehome_index)->landAlt + navFwAutolandConfig()->glideAltitude - GPS_home.alt) { setLandWaypoint(&fwAutoland.waypoint[LAND_WP_LAND], NULL); return FW_AUTOLAND_EVENT_SUCCSESS; } @@ -375,7 +375,7 @@ static fwAutolandEvent_t fwAutolandState_FINAL_APPROACH(timeUs_t currentTimeUs) static fwAutolandEvent_t fwAutolandState_GLIDE(timeUs_t currentTimeUs) { - if (getEstimatedActualPosition(Z) <= safeHomeConfig(safehome_index)->landAltMSL + navFwAutolandConfig()->flareAltitude - GPS_home.alt) { + 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]);