mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-24 00:35:34 +03:00
2nd
This commit is contained in:
Scavanger
parent
9370fdd49f
commit
d7b36611f9
6 changed files with 69 additions and 31 deletions
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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]);
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue