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

2nd

Browse source
This commit is contained in:
Scavanger 2023-11-09 16:45:03 -03:00
parent 9370fdd49f
commit d7b36611f9
6 changed files with 69 additions and 31 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1312,7 +1312,7 @@ static void cliTempSensor(char *cmdline)
#if defined(USE_SAFE_HOME) #if defined(USE_SAFE_HOME)
static void printSafeHomes(uint8_t dumpMask, const navSafeHome_t *navSafeHome, const navSafeHome_t *defaultSafeHome) static void printSafeHomes(uint8_t dumpMask, const navSafeHome_t *navSafeHome, const navSafeHome_t *defaultSafeHome)
{ {
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++) { for (uint8_t i = 0; i < MAX_SAFE_HOMES; i++) {
bool equalsDefault = false; bool equalsDefault = false;
if (defaultSafeHome) { 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].lat == defaultSafeHome[i].lat
&& navSafeHome[i].lon == defaultSafeHome[i].lon && navSafeHome[i].lon == defaultSafeHome[i].lon
&& navSafeHome[i].approachDirection == defaultSafeHome[i].approachDirection && navSafeHome[i].approachDirection == defaultSafeHome[i].approachDirection
&& navSafeHome[i].approachAltMSL == defaultSafeHome[i].approachAltMSL && navSafeHome[i].approachAlt == defaultSafeHome[i].approachAlt
&& navSafeHome[i].landAltMSL == defaultSafeHome[i].landAltMSL && navSafeHome[i].landAlt == defaultSafeHome[i].landAlt
&& navSafeHome[i].landApproachHeading1 == defaultSafeHome[i].landApproachHeading1 && 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, 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, 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(); resetSafeHomes();
} else { } else {
int32_t lat=0, lon=0, approachAlt = 0, heading1 = 0, heading2 = 0, landDirection = 0, landAlt = 0; 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; uint8_t validArgumentCount = 0;
const char *ptr = cmdline; const char *ptr = cmdline;
int8_t i = fastA2I(ptr); int8_t i = fastA2I(ptr);
@ -1383,11 +1384,28 @@ static void cliSafeHomes(char *cmdline)
if ((ptr = nextArg(ptr))) { if ((ptr = nextArg(ptr))) {
heading1 = fastA2I(ptr); heading1 = fastA2I(ptr);
if (heading1 < -360 || heading1 > 360) {
cliShowParseError();
return;
}
validArgumentCount++; validArgumentCount++;
} }
if ((ptr = nextArg(ptr))) { if ((ptr = nextArg(ptr))) {
heading2 = fastA2I(ptr); heading2 = fastA2I(ptr);
if (heading2 < -360 || heading2 > 360) {
cliShowParseError();
return;
}
validArgumentCount++;
}
if ((ptr = nextArg(ptr))) {
isSeaLevelRef = fastA2I(ptr);
validArgumentCount++; validArgumentCount++;
} }
@ -1396,17 +1414,18 @@ static void cliSafeHomes(char *cmdline)
validArgumentCount++; validArgumentCount++;
} }
if (validArgumentCount != 8) { if (validArgumentCount != 9) {
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)->approachAltMSL = approachAlt; safeHomeConfigMutable(i)->approachAlt = approachAlt;
safeHomeConfigMutable(i)->landAltMSL = landAlt; safeHomeConfigMutable(i)->landAlt = landAlt;
safeHomeConfigMutable(i)->approachDirection = (fwAutolandApproachDirection_e)landDirection; safeHomeConfigMutable(i)->approachDirection = (fwAutolandApproachDirection_e)landDirection;
safeHomeConfigMutable(i)->landApproachHeading1 = (int16_t)heading1; safeHomeConfigMutable(i)->landApproachHeading1 = (int16_t)heading1;
safeHomeConfigMutable(i)->landApproachHeading2 = (int16_t)heading2; safeHomeConfigMutable(i)->landApproachHeading2 = (int16_t)heading2;
safeHomeConfigMutable(i)->isSeaLevelRef = isSeaLevelRef;
} }
} }
} }

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

@ -1653,6 +1653,12 @@ 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;
@ -3113,14 +3119,26 @@ 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 == 10) { if (dataSize == 24) {
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;
} }
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);
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 { } else {
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
} }

4
src/main/fc/settings.yaml
View file

@ -4096,7 +4096,7 @@ groups:
min: 100 min: 100
max: 100000 max: 100000
- name: nav_fw_land_final_approach_pitch2throttle_mod - 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 default_value: 100
field: finalApproachPitchToThrottleMod field: finalApproachPitchToThrottleMod
min: 100 min: 100
@ -4108,7 +4108,7 @@ groups:
min: 100 min: 100
max: 5000 max: 5000
- name: nav_fw_land_flare_alt - name: nav_fw_land_flare_alt
description: "Initial altitude of the flare" description: "Initial altitude of the flare phase"
default_value: 2000 default_value: 2000
field: flareAltitude field: flareAltitude
min: 0 min: 0

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

@ -2310,8 +2310,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(nav
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 (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(); resetPositionController();
posControl.fwLandState = FW_AUTOLAND_STATE_GLIDE; posControl.fwLandState = FW_AUTOLAND_STATE_GLIDE;
return NAV_FSM_EVENT_SUCCESS; return NAV_FSM_EVENT_SUCCESS;
@ -4986,8 +4986,8 @@ int32_t navigationGetHeadingError(void)
static void resetFwAutoland(void) static void resetFwAutoland(void)
{ {
posControl.fwLandAltAgl = safeHomeConfig(safehome_index)->landAltMSL - 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)->approachAltMSL + posControl.fwLandAltAgl - GPS_home.alt; posControl.fwLandAproachAltAgl = safeHomeConfig(safehome_index)->isSeaLevelRef ? safeHomeConfig(safehome_index)->approachAlt - GPS_home.alt : safeHomeConfig(safehome_index)->approachAlt;
posControl.fwLandLoiterStartTime = 0; posControl.fwLandLoiterStartTime = 0;
posControl.fwLandWpReached = false; posControl.fwLandWpReached = false;
} }

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

@ -61,8 +61,9 @@ typedef struct {
uint8_t enabled; uint8_t enabled;
int32_t lat; int32_t lat;
int32_t lon; int32_t lon;
int32_t approachAltMSL; int32_t approachAlt;
int32_t landAltMSL; int32_t landAlt;
bool isSeaLevelRef;
fwAutolandApproachDirection_e approachDirection; fwAutolandApproachDirection_e approachDirection;
int16_t landApproachHeading1; int16_t landApproachHeading1;
int16_t landApproachHeading2; int16_t landApproachHeading2;

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

@ -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) static fwAutolandEvent_t fwAutolandState_ABOVE_LOITER_ALT(timeUs_t currentTimeUs)
{ {
UNUSED(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) { if (ABS(getEstimatedActualPosition(Z) - approachAltAbs) < TARGET_ALT_TOLERANCE) {
return FW_AUTOLAND_EVENT_SUCCSESS; return FW_AUTOLAND_EVENT_SUCCSESS;
@ -249,8 +249,8 @@ static fwAutolandEvent_t fwAutolandState_LOITER(timeUs_t currentTimeUs)
return FW_AUTOLAND_EVENT_ABORT; return FW_AUTOLAND_EVENT_ABORT;
#endif #endif
int32_t landAltAbs = safeHomeConfig(safehome_index)->landAltMSL - GPS_home.alt; int32_t landAltAbs = safeHomeConfig(safehome_index)->landAlt - GPS_home.alt;
int32_t approachAltAbs = safeHomeConfig(safehome_index)->approachAltMSL + landAltAbs - GPS_home.alt; int32_t approachAltAbs = safeHomeConfig(safehome_index)->approachAlt + landAltAbs - GPS_home.alt;
if (fwAutoland.loiterStartTime == 0) { if (fwAutoland.loiterStartTime == 0) {
fwAutoland.loiterStartTime = currentTimeUs; fwAutoland.loiterStartTime = currentTimeUs;
} else if (micros() - fwAutoland.loiterStartTime > LOITER_MIN_TIME) { } 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) 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); setLandWaypoint(&fwAutoland.waypoint[LAND_WP_LAND], NULL);
return FW_AUTOLAND_EVENT_SUCCSESS; 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) 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; return FW_AUTOLAND_EVENT_SUCCSESS;
} }
posControl.wpDistance = calculateDistanceToDestination(&fwAutoland.waypoint[LAND_WP_LAND]); posControl.wpDistance = calculateDistanceToDestination(&fwAutoland.waypoint[LAND_WP_LAND]);