mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-25 17:25:18 +03:00
Merge pull request #7378 from iNavFlight/dzikuvx-customizable-blackbox-fields
Customizable blackbox fields
This commit is contained in:
Paweł Spychalski
GitHub
commit
f3171a6ee4
9 changed files with 263 additions and 110 deletions
|
|
@ -97,15 +97,30 @@
|
|||
#define BLACKBOX_INVERTED_CARD_DETECTION 0
|
||||
#endif
|
||||
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 1);
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 2);
|
||||
|
||||
PG_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig,
|
||||
.device = DEFAULT_BLACKBOX_DEVICE,
|
||||
.rate_num = SETTING_BLACKBOX_RATE_NUM_DEFAULT,
|
||||
.rate_denom = SETTING_BLACKBOX_RATE_DENOM_DEFAULT,
|
||||
.invertedCardDetection = BLACKBOX_INVERTED_CARD_DETECTION,
|
||||
.includeFlags = BLACKBOX_FEATURE_NAV_PID | BLACKBOX_FEATURE_NAV_POS | BLACKBOX_FEATURE_MAG | BLACKBOX_FEATURE_ACC | BLACKBOX_FEATURE_ATTITUDE,
|
||||
);
|
||||
|
||||
void blackboxIncludeFlagSet(uint32_t mask)
|
||||
{
|
||||
blackboxConfigMutable()->includeFlags |= mask;
|
||||
}
|
||||
|
||||
void blackboxIncludeFlagClear(uint32_t mask)
|
||||
{
|
||||
blackboxConfigMutable()->includeFlags &= ~(mask);
|
||||
}
|
||||
|
||||
bool blackboxIncludeFlag(uint32_t mask) {
|
||||
return (blackboxConfig()->includeFlags & mask) == mask;
|
||||
}
|
||||
|
||||
#define BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS 200
|
||||
static const int32_t blackboxSInterval = 4096;
|
||||
|
||||
|
|
@ -267,12 +282,12 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
|
|||
{"gyroADC", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||
{"gyroADC", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||
{"gyroADC", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||
{"accSmooth", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||
{"accSmooth", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||
{"accSmooth", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||
{"attitude", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||
{"attitude", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||
{"attitude", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||
{"accSmooth", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC},
|
||||
{"accSmooth", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC},
|
||||
{"accSmooth", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC},
|
||||
{"attitude", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ATTITUDE},
|
||||
{"attitude", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ATTITUDE},
|
||||
{"attitude", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ATTITUDE},
|
||||
{"debug", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_DEBUG},
|
||||
{"debug", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_DEBUG},
|
||||
{"debug", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_DEBUG},
|
||||
|
|
@ -310,28 +325,26 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
|
|||
{"servo", 14, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
|
||||
{"servo", 15, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
|
||||
|
||||
#ifdef NAV_BLACKBOX
|
||||
{"navState", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||
{"navFlags", -1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||
{"navEPH", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||
{"navEPV", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||
{"navPos", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||
{"navPos", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||
{"navPos", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||
{"navVel", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||
{"navVel", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||
{"navVel", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||
{"navAcc", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||
{"navAcc", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||
{"navAcc", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||
{"navTgtVel", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||
{"navTgtVel", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||
{"navTgtVel", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||
{"navTgtPos", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||
{"navTgtPos", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||
{"navTgtPos", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||
{"navSurf", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||
#endif
|
||||
{"navEPH", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
|
||||
{"navEPV", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
|
||||
{"navPos", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
|
||||
{"navPos", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
|
||||
{"navPos", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
|
||||
{"navVel", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
|
||||
{"navVel", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
|
||||
{"navVel", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
|
||||
{"navTgtVel", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
|
||||
{"navTgtVel", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
|
||||
{"navTgtVel", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
|
||||
{"navTgtPos", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
|
||||
{"navTgtPos", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
|
||||
{"navTgtPos", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
|
||||
{"navSurf", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
|
||||
{"navAcc", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_ACC},
|
||||
{"navAcc", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_ACC},
|
||||
{"navAcc", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_ACC},
|
||||
};
|
||||
|
||||
#ifdef USE_GPS
|
||||
|
|
@ -461,7 +474,6 @@ typedef struct blackboxMainState_s {
|
|||
int32_t surfaceRaw;
|
||||
#endif
|
||||
uint16_t rssi;
|
||||
#ifdef NAV_BLACKBOX
|
||||
int16_t navState;
|
||||
uint16_t navFlags;
|
||||
uint16_t navEPH;
|
||||
|
|
@ -474,7 +486,6 @@ typedef struct blackboxMainState_s {
|
|||
int16_t navHeading;
|
||||
int16_t navTargetHeading;
|
||||
int16_t navSurface;
|
||||
#endif
|
||||
} blackboxMainState_t;
|
||||
|
||||
typedef struct blackboxGpsState_s {
|
||||
|
|
@ -600,7 +611,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
|
|||
|
||||
case FLIGHT_LOG_FIELD_CONDITION_MAG:
|
||||
#ifdef USE_MAG
|
||||
return sensors(SENSOR_MAG);
|
||||
return sensors(SENSOR_MAG) && blackboxIncludeFlag(BLACKBOX_FEATURE_MAG);
|
||||
#else
|
||||
return false;
|
||||
#endif
|
||||
|
|
@ -634,14 +645,14 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
|
|||
|
||||
case FLIGHT_LOG_FIELD_CONDITION_FIXED_WING_NAV:
|
||||
#ifdef USE_NAV
|
||||
return STATE(FIXED_WING_LEGACY);
|
||||
return STATE(FIXED_WING_LEGACY) && blackboxIncludeFlag(BLACKBOX_FEATURE_NAV_PID);
|
||||
#else
|
||||
return false;
|
||||
#endif
|
||||
|
||||
case FLIGHT_LOG_FIELD_CONDITION_MC_NAV:
|
||||
#ifdef USE_NAV
|
||||
return !STATE(FIXED_WING_LEGACY);
|
||||
return !STATE(FIXED_WING_LEGACY) && blackboxIncludeFlag(BLACKBOX_FEATURE_NAV_PID);
|
||||
#else
|
||||
return false;
|
||||
#endif
|
||||
|
|
@ -657,9 +668,22 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
|
|||
case FLIGHT_LOG_FIELD_CONDITION_DEBUG:
|
||||
return debugMode != DEBUG_NONE;
|
||||
|
||||
case FLIGHT_LOG_FIELD_CONDITION_NAV_ACC:
|
||||
return blackboxIncludeFlag(BLACKBOX_FEATURE_NAV_ACC);
|
||||
|
||||
case FLIGHT_LOG_FIELD_CONDITION_NAV_POS:
|
||||
return blackboxIncludeFlag(BLACKBOX_FEATURE_NAV_POS);
|
||||
|
||||
case FLIGHT_LOG_FIELD_CONDITION_ACC:
|
||||
return blackboxIncludeFlag(BLACKBOX_FEATURE_ACC);
|
||||
|
||||
case FLIGHT_LOG_FIELD_CONDITION_ATTITUDE:
|
||||
return blackboxIncludeFlag(BLACKBOX_FEATURE_ATTITUDE);
|
||||
|
||||
case FLIGHT_LOG_FIELD_CONDITION_NEVER:
|
||||
return false;
|
||||
|
||||
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
|
|
@ -811,8 +835,14 @@ static void writeIntraframe(void)
|
|||
}
|
||||
|
||||
blackboxWriteSigned16VBArray(blackboxCurrent->gyroADC, XYZ_AXIS_COUNT);
|
||||
blackboxWriteSigned16VBArray(blackboxCurrent->accADC, XYZ_AXIS_COUNT);
|
||||
blackboxWriteSigned16VBArray(blackboxCurrent->attitude, XYZ_AXIS_COUNT);
|
||||
|
||||
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ACC)) {
|
||||
blackboxWriteSigned16VBArray(blackboxCurrent->accADC, XYZ_AXIS_COUNT);
|
||||
}
|
||||
|
||||
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ATTITUDE)) {
|
||||
blackboxWriteSigned16VBArray(blackboxCurrent->attitude, XYZ_AXIS_COUNT);
|
||||
}
|
||||
|
||||
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_DEBUG)) {
|
||||
blackboxWriteSignedVBArray(blackboxCurrent->debug, DEBUG32_VALUE_COUNT);
|
||||
|
|
@ -834,36 +864,41 @@ static void writeIntraframe(void)
|
|||
}
|
||||
}
|
||||
|
||||
#ifdef NAV_BLACKBOX
|
||||
blackboxWriteSignedVB(blackboxCurrent->navState);
|
||||
|
||||
blackboxWriteSignedVB(blackboxCurrent->navFlags);
|
||||
blackboxWriteSignedVB(blackboxCurrent->navEPH);
|
||||
blackboxWriteSignedVB(blackboxCurrent->navEPV);
|
||||
|
||||
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
|
||||
blackboxWriteSignedVB(blackboxCurrent->navPos[x]);
|
||||
/*
|
||||
* NAV_POS fields
|
||||
*/
|
||||
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NAV_POS)) {
|
||||
blackboxWriteSignedVB(blackboxCurrent->navEPH);
|
||||
blackboxWriteSignedVB(blackboxCurrent->navEPV);
|
||||
|
||||
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
|
||||
blackboxWriteSignedVB(blackboxCurrent->navPos[x]);
|
||||
}
|
||||
|
||||
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
|
||||
blackboxWriteSignedVB(blackboxCurrent->navRealVel[x]);
|
||||
}
|
||||
|
||||
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
|
||||
blackboxWriteSignedVB(blackboxCurrent->navTargetVel[x]);
|
||||
}
|
||||
|
||||
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
|
||||
blackboxWriteSignedVB(blackboxCurrent->navTargetPos[x]);
|
||||
}
|
||||
|
||||
blackboxWriteSignedVB(blackboxCurrent->navSurface);
|
||||
}
|
||||
|
||||
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
|
||||
blackboxWriteSignedVB(blackboxCurrent->navRealVel[x]);
|
||||
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NAV_ACC)) {
|
||||
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
|
||||
blackboxWriteSignedVB(blackboxCurrent->navAccNEU[x]);
|
||||
}
|
||||
}
|
||||
|
||||
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
|
||||
blackboxWriteSignedVB(blackboxCurrent->navAccNEU[x]);
|
||||
}
|
||||
|
||||
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
|
||||
blackboxWriteSignedVB(blackboxCurrent->navTargetVel[x]);
|
||||
}
|
||||
|
||||
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
|
||||
blackboxWriteSignedVB(blackboxCurrent->navTargetPos[x]);
|
||||
}
|
||||
|
||||
blackboxWriteSignedVB(blackboxCurrent->navSurface);
|
||||
#endif
|
||||
|
||||
//Rotate our history buffers:
|
||||
|
||||
//The current state becomes the new "before" state
|
||||
|
|
@ -1039,8 +1074,15 @@ static void writeInterframe(void)
|
|||
|
||||
//Since gyros, accs and motors are noisy, base their predictions on the average of the history:
|
||||
blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, gyroADC), XYZ_AXIS_COUNT);
|
||||
blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, accADC), XYZ_AXIS_COUNT);
|
||||
blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, attitude), XYZ_AXIS_COUNT);
|
||||
|
||||
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ACC)) {
|
||||
blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, accADC), XYZ_AXIS_COUNT);
|
||||
}
|
||||
|
||||
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ATTITUDE)) {
|
||||
blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, attitude), XYZ_AXIS_COUNT);
|
||||
}
|
||||
|
||||
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_DEBUG)) {
|
||||
blackboxWriteArrayUsingAveragePredictor32(offsetof(blackboxMainState_t, debug), DEBUG32_VALUE_COUNT);
|
||||
}
|
||||
|
|
@ -1050,36 +1092,43 @@ static void writeInterframe(void)
|
|||
blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, servo), MAX_SUPPORTED_SERVOS);
|
||||
}
|
||||
|
||||
#ifdef NAV_BLACKBOX
|
||||
blackboxWriteSignedVB(blackboxCurrent->navState - blackboxLast->navState);
|
||||
|
||||
blackboxWriteSignedVB(blackboxCurrent->navFlags - blackboxLast->navFlags);
|
||||
blackboxWriteSignedVB(blackboxCurrent->navEPH - blackboxLast->navEPH);
|
||||
blackboxWriteSignedVB(blackboxCurrent->navEPV - blackboxLast->navEPV);
|
||||
|
||||
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
|
||||
blackboxWriteSignedVB(blackboxCurrent->navPos[x] - blackboxLast->navPos[x]);
|
||||
/*
|
||||
* NAV_POS fields
|
||||
*/
|
||||
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NAV_POS)) {
|
||||
blackboxWriteSignedVB(blackboxCurrent->navEPH - blackboxLast->navEPH);
|
||||
blackboxWriteSignedVB(blackboxCurrent->navEPV - blackboxLast->navEPV);
|
||||
|
||||
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
|
||||
blackboxWriteSignedVB(blackboxCurrent->navPos[x] - blackboxLast->navPos[x]);
|
||||
}
|
||||
|
||||
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
|
||||
blackboxWriteSignedVB(blackboxHistory[0]->navRealVel[x] - (blackboxHistory[1]->navRealVel[x] + blackboxHistory[2]->navRealVel[x]) / 2);
|
||||
}
|
||||
|
||||
|
||||
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
|
||||
blackboxWriteSignedVB(blackboxHistory[0]->navTargetVel[x] - (blackboxHistory[1]->navTargetVel[x] + blackboxHistory[2]->navTargetVel[x]) / 2);
|
||||
}
|
||||
|
||||
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
|
||||
blackboxWriteSignedVB(blackboxHistory[0]->navTargetPos[x] - blackboxLast->navTargetPos[x]);
|
||||
}
|
||||
|
||||
blackboxWriteSignedVB(blackboxCurrent->navSurface - blackboxLast->navSurface);
|
||||
}
|
||||
|
||||
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
|
||||
blackboxWriteSignedVB(blackboxHistory[0]->navRealVel[x] - (blackboxHistory[1]->navRealVel[x] + blackboxHistory[2]->navRealVel[x]) / 2);
|
||||
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NAV_ACC)) {
|
||||
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
|
||||
blackboxWriteSignedVB(blackboxHistory[0]->navAccNEU[x] - (blackboxHistory[1]->navAccNEU[x] + blackboxHistory[2]->navAccNEU[x]) / 2);
|
||||
}
|
||||
}
|
||||
|
||||
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
|
||||
blackboxWriteSignedVB(blackboxHistory[0]->navAccNEU[x] - (blackboxHistory[1]->navAccNEU[x] + blackboxHistory[2]->navAccNEU[x]) / 2);
|
||||
}
|
||||
|
||||
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
|
||||
blackboxWriteSignedVB(blackboxHistory[0]->navTargetVel[x] - (blackboxHistory[1]->navTargetVel[x] + blackboxHistory[2]->navTargetVel[x]) / 2);
|
||||
}
|
||||
|
||||
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
|
||||
blackboxWriteSignedVB(blackboxHistory[0]->navTargetPos[x] - blackboxLast->navTargetPos[x]);
|
||||
}
|
||||
|
||||
blackboxWriteSignedVB(blackboxCurrent->navSurface - blackboxLast->navSurface);
|
||||
#endif
|
||||
|
||||
//Rotate our history buffers
|
||||
blackboxHistory[2] = blackboxHistory[1];
|
||||
blackboxHistory[1] = blackboxHistory[0];
|
||||
|
|
@ -1479,7 +1528,6 @@ static void loadMainState(timeUs_t currentTimeUs)
|
|||
blackboxCurrent->servo[i] = servo[i];
|
||||
}
|
||||
|
||||
#ifdef NAV_BLACKBOX
|
||||
blackboxCurrent->navState = navCurrentState;
|
||||
blackboxCurrent->navFlags = navFlags;
|
||||
blackboxCurrent->navEPH = navEPH;
|
||||
|
|
@ -1492,7 +1540,6 @@ static void loadMainState(timeUs_t currentTimeUs)
|
|||
blackboxCurrent->navTargetPos[i] = navTargetPosition[i];
|
||||
}
|
||||
blackboxCurrent->navSurface = navActualSurface;
|
||||
#endif
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -1749,9 +1796,7 @@ static bool blackboxWriteSysinfo(void)
|
|||
BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate", "%d", motorConfig()->motorPwmRate);
|
||||
BLACKBOX_PRINT_HEADER_LINE("debug_mode", "%d", systemConfig()->debug_mode);
|
||||
BLACKBOX_PRINT_HEADER_LINE("features", "%d", featureConfig()->enabledFeatures);
|
||||
#ifdef NAV_BLACKBOX
|
||||
BLACKBOX_PRINT_HEADER_LINE("waypoints", "%d,%d", getWaypointCount(),isWaypointListValid());
|
||||
#endif
|
||||
BLACKBOX_PRINT_HEADER_LINE("acc_notch_hz", "%d", accelerometerConfig()->acc_notch_hz);
|
||||
BLACKBOX_PRINT_HEADER_LINE("acc_notch_cutoff", "%d", accelerometerConfig()->acc_notch_cutoff);
|
||||
BLACKBOX_PRINT_HEADER_LINE("pidSumLimit", "%d", pidProfile()->pidSumLimit);
|
||||
|
|
|
|||
|
|
@ -21,11 +21,22 @@
|
|||
|
||||
#include "config/parameter_group.h"
|
||||
|
||||
typedef enum {
|
||||
BLACKBOX_FEATURE_NAV_ACC = 1 << 0,
|
||||
BLACKBOX_FEATURE_NAV_POS = 1 << 1,
|
||||
BLACKBOX_FEATURE_NAV_PID = 1 << 2,
|
||||
BLACKBOX_FEATURE_MAG = 1 << 3,
|
||||
BLACKBOX_FEATURE_ACC = 1 << 4,
|
||||
BLACKBOX_FEATURE_ATTITUDE = 1 << 5,
|
||||
} blackboxFeatureMask_e;
|
||||
|
||||
|
||||
typedef struct blackboxConfig_s {
|
||||
uint16_t rate_num;
|
||||
uint16_t rate_denom;
|
||||
uint8_t device;
|
||||
uint8_t invertedCardDetection;
|
||||
uint32_t includeFlags;
|
||||
} blackboxConfig_t;
|
||||
|
||||
PG_DECLARE(blackboxConfig_t, blackboxConfig);
|
||||
|
|
@ -37,3 +48,6 @@ void blackboxUpdate(timeUs_t currentTimeUs);
|
|||
void blackboxStart(void);
|
||||
void blackboxFinish(void);
|
||||
bool blackboxMayEditConfig(void);
|
||||
void blackboxIncludeFlagSet(uint32_t mask);
|
||||
void blackboxIncludeFlagClear(uint32_t mask);
|
||||
bool blackboxIncludeFlag(uint32_t mask);
|
||||
|
|
@ -52,6 +52,12 @@ typedef enum FlightLogFieldCondition {
|
|||
|
||||
FLIGHT_LOG_FIELD_CONDITION_DEBUG,
|
||||
|
||||
FLIGHT_LOG_FIELD_CONDITION_NAV_ACC,
|
||||
FLIGHT_LOG_FIELD_CONDITION_NAV_POS,
|
||||
FLIGHT_LOG_FIELD_CONDITION_NAV_PID,
|
||||
FLIGHT_LOG_FIELD_CONDITION_ACC,
|
||||
FLIGHT_LOG_FIELD_CONDITION_ATTITUDE,
|
||||
|
||||
FLIGHT_LOG_FIELD_CONDITION_NEVER,
|
||||
|
||||
FLIGHT_LOG_FIELD_CONDITION_FIRST = FLIGHT_LOG_FIELD_CONDITION_ALWAYS,
|
||||
|
|
|
|||
|
|
@ -162,6 +162,12 @@ static const char * const featureNames[] = {
|
|||
"OSD", "FW_LAUNCH", "FW_AUTOTRIM", NULL
|
||||
};
|
||||
|
||||
#ifdef USE_BLACKBOX
|
||||
static const char * const blackboxIncludeFlagNames[] = {
|
||||
"NAV_ACC", "NAV_POS", "NAV_PID", "MAG", "ACC", "ATTI", NULL
|
||||
};
|
||||
#endif
|
||||
|
||||
/* Sensor names (used in lookup tables for *_hardware settings and in status command output) */
|
||||
// sync with gyroSensor_e
|
||||
static const char * const gyroNames[] = { "NONE", "AUTO", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "BMI270", "FAKE"};
|
||||
|
|
@ -2504,6 +2510,94 @@ static void cliFeature(char *cmdline)
|
|||
}
|
||||
}
|
||||
|
||||
#ifdef USE_BLACKBOX
|
||||
static void printBlackbox(uint8_t dumpMask, const blackboxConfig_t *config, const blackboxConfig_t *configDefault)
|
||||
{
|
||||
|
||||
UNUSED(configDefault);
|
||||
|
||||
uint32_t mask = config->includeFlags;
|
||||
|
||||
for (uint8_t i = 0; ; i++) { // reenable what we want.
|
||||
if (blackboxIncludeFlagNames[i] == NULL) {
|
||||
break;
|
||||
}
|
||||
|
||||
const char *formatOn = "blackbox %s";
|
||||
const char *formatOff = "blackbox -%s";
|
||||
|
||||
if (mask & (1 << i)) {
|
||||
cliDumpPrintLinef(dumpMask, false, formatOn, blackboxIncludeFlagNames[i]);
|
||||
cliDefaultPrintLinef(dumpMask, false, formatOn, blackboxIncludeFlagNames[i]);
|
||||
} else {
|
||||
cliDumpPrintLinef(dumpMask, false, formatOff, blackboxIncludeFlagNames[i]);
|
||||
cliDefaultPrintLinef(dumpMask, false, formatOff, blackboxIncludeFlagNames[i]);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
static void cliBlackbox(char *cmdline)
|
||||
{
|
||||
uint32_t len = strlen(cmdline);
|
||||
uint32_t mask = blackboxConfig()->includeFlags;
|
||||
|
||||
if (len == 0) {
|
||||
cliPrint("Enabled: ");
|
||||
for (uint8_t i = 0; ; i++) {
|
||||
if (blackboxIncludeFlagNames[i] == NULL) {
|
||||
break;
|
||||
}
|
||||
|
||||
if (mask & (1 << i))
|
||||
cliPrintf("%s ", blackboxIncludeFlagNames[i]);
|
||||
}
|
||||
cliPrintLinefeed();
|
||||
} else if (sl_strncasecmp(cmdline, "list", len) == 0) {
|
||||
cliPrint("Available: ");
|
||||
for (uint32_t i = 0; ; i++) {
|
||||
if (blackboxIncludeFlagNames[i] == NULL) {
|
||||
break;
|
||||
}
|
||||
|
||||
cliPrintf("%s ", blackboxIncludeFlagNames[i]);
|
||||
}
|
||||
cliPrintLinefeed();
|
||||
return;
|
||||
} else {
|
||||
bool remove = false;
|
||||
if (cmdline[0] == '-') {
|
||||
// remove feature
|
||||
remove = true;
|
||||
cmdline++; // skip over -
|
||||
len--;
|
||||
}
|
||||
|
||||
for (uint32_t i = 0; ; i++) {
|
||||
if (blackboxIncludeFlagNames[i] == NULL) {
|
||||
cliPrintErrorLine("Invalid name");
|
||||
break;
|
||||
}
|
||||
|
||||
if (sl_strncasecmp(cmdline, blackboxIncludeFlagNames[i], len) == 0) {
|
||||
|
||||
mask = 1 << i;
|
||||
|
||||
if (remove) {
|
||||
blackboxIncludeFlagClear(mask);
|
||||
cliPrint("Disabled");
|
||||
} else {
|
||||
blackboxIncludeFlagSet(mask);
|
||||
cliPrint("Enabled");
|
||||
}
|
||||
cliPrintLinef(" %s", blackboxIncludeFlagNames[i]);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(BEEPER) || defined(USE_DSHOT)
|
||||
static void printBeeper(uint8_t dumpMask, const beeperConfig_t *beeperConfig, const beeperConfig_t *beeperConfigDefault)
|
||||
{
|
||||
|
|
@ -3505,6 +3599,11 @@ static void printConfig(const char *cmdline, bool doDiff)
|
|||
printBeeper(dumpMask, &beeperConfig_Copy, beeperConfig());
|
||||
#endif
|
||||
|
||||
#ifdef USE_BLACKBOX
|
||||
cliPrintHashLine("blackbox");
|
||||
printBlackbox(dumpMask, &blackboxConfig_Copy, blackboxConfig());
|
||||
#endif
|
||||
|
||||
cliPrintHashLine("map");
|
||||
printMap(dumpMask, &rxConfig_Copy, rxConfig());
|
||||
|
||||
|
|
@ -3698,6 +3797,11 @@ const clicmd_t cmdTable[] = {
|
|||
CLI_COMMAND_DEF("feature", "configure features",
|
||||
"list\r\n"
|
||||
"\t<+|->[name]", cliFeature),
|
||||
#ifdef USE_BLACKBOX
|
||||
CLI_COMMAND_DEF("blackbox", "configure blackbox fields",
|
||||
"list\r\n"
|
||||
"\t<+|->[name]", cliBlackbox),
|
||||
#endif
|
||||
#ifdef USE_FLASHFS
|
||||
CLI_COMMAND_DEF("flash_erase", "erase flash chip", NULL, cliFlashErase),
|
||||
CLI_COMMAND_DEF("flash_info", "show flash chip info", NULL, cliFlashInfo),
|
||||
|
|
|
|||
|
|
@ -30,7 +30,7 @@
|
|||
#define MAX_NAME_LENGTH 16
|
||||
|
||||
#define TASK_GYRO_LOOPTIME 250 // Task gyro always runs at 4kHz
|
||||
typedef enum {
|
||||
typedef enum {
|
||||
FEATURE_THR_VBAT_COMP = 1 << 0,
|
||||
FEATURE_VBAT = 1 << 1,
|
||||
FEATURE_TX_PROF_SEL = 1 << 2, // Profile selection by TX stick command
|
||||
|
|
|
|||
|
|
@ -199,7 +199,7 @@ navigationPosControl_t posControl;
|
|||
navSystemStatus_t NAV_Status;
|
||||
EXTENDED_FASTRAM multicopterPosXyCoefficients_t multicopterPosXyCoefficients;
|
||||
|
||||
#if defined(NAV_BLACKBOX)
|
||||
// Blackbox states
|
||||
int16_t navCurrentState;
|
||||
int16_t navActualVelocity[3];
|
||||
int16_t navDesiredVelocity[3];
|
||||
|
|
@ -212,7 +212,7 @@ uint16_t navFlags;
|
|||
uint16_t navEPH;
|
||||
uint16_t navEPV;
|
||||
int16_t navAccNEU[3];
|
||||
#endif
|
||||
//End of blackbox states
|
||||
|
||||
static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode);
|
||||
static void updateDesiredRTHAltitude(void);
|
||||
|
|
@ -1950,12 +1950,11 @@ void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelVali
|
|||
posControl.flags.horizontalPositionDataNew = 0;
|
||||
}
|
||||
|
||||
#if defined(NAV_BLACKBOX)
|
||||
//Update blackbox data
|
||||
navLatestActualPosition[X] = newX;
|
||||
navLatestActualPosition[Y] = newY;
|
||||
navActualVelocity[X] = constrain(newVelX, -32678, 32767);
|
||||
navActualVelocity[Y] = constrain(newVelY, -32678, 32767);
|
||||
#endif
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
|
|
@ -2005,10 +2004,9 @@ void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, flo
|
|||
posControl.actualState.surfaceMin = -1;
|
||||
}
|
||||
|
||||
#if defined(NAV_BLACKBOX)
|
||||
//Update blackbox data
|
||||
navLatestActualPosition[Z] = navGetCurrentActualPositionAndVelocity()->pos.z;
|
||||
navActualVelocity[Z] = constrain(navGetCurrentActualPositionAndVelocity()->vel.z, -32678, 32767);
|
||||
#endif
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
|
|
@ -3066,7 +3064,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
|
|||
{
|
||||
const timeUs_t currentTimeUs = micros();
|
||||
|
||||
#if defined(NAV_BLACKBOX)
|
||||
//Updata blackbox data
|
||||
navFlags = 0;
|
||||
if (posControl.flags.estAltStatus == EST_TRUSTED) navFlags |= (1 << 0);
|
||||
if (posControl.flags.estAglStatus == EST_TRUSTED) navFlags |= (1 << 1);
|
||||
|
|
@ -3076,7 +3074,6 @@ void applyWaypointNavigationAndAltitudeHold(void)
|
|||
if (isGPSGlitchDetected()) navFlags |= (1 << 4);
|
||||
#endif
|
||||
if (posControl.flags.estHeadingStatus == EST_TRUSTED) navFlags |= (1 << 5);
|
||||
#endif
|
||||
|
||||
// Reset all navigation requests - NAV controllers will set them if necessary
|
||||
DISABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
|
||||
|
|
@ -3110,8 +3107,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
|
|||
if (posControl.flags.verticalPositionDataConsumed)
|
||||
posControl.flags.verticalPositionDataNew = 0;
|
||||
|
||||
|
||||
#if defined(NAV_BLACKBOX)
|
||||
//Update blackbox data
|
||||
if (posControl.flags.isAdjustingPosition) navFlags |= (1 << 6);
|
||||
if (posControl.flags.isAdjustingAltitude) navFlags |= (1 << 7);
|
||||
if (posControl.flags.isAdjustingHeading) navFlags |= (1 << 8);
|
||||
|
|
@ -3119,7 +3115,6 @@ void applyWaypointNavigationAndAltitudeHold(void)
|
|||
navTargetPosition[X] = lrintf(posControl.desiredState.pos.x);
|
||||
navTargetPosition[Y] = lrintf(posControl.desiredState.pos.y);
|
||||
navTargetPosition[Z] = lrintf(posControl.desiredState.pos.z);
|
||||
#endif
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
|
|
@ -3463,9 +3458,8 @@ void updateWaypointsAndNavigationMode(void)
|
|||
// Map navMode back to enabled flight modes
|
||||
switchNavigationFlightModes();
|
||||
|
||||
#if defined(NAV_BLACKBOX)
|
||||
//Update Blackbox data
|
||||
navCurrentState = (int16_t)posControl.navPersistentId;
|
||||
#endif
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -69,9 +69,6 @@ bool findNearestSafeHome(void); // Find nearest safehome
|
|||
#endif // defined(USE_SAFE_HOME)
|
||||
|
||||
#if defined(USE_NAV)
|
||||
#if defined(USE_BLACKBOX)
|
||||
#define NAV_BLACKBOX
|
||||
#endif
|
||||
|
||||
#ifndef NAV_MAX_WAYPOINTS
|
||||
#define NAV_MAX_WAYPOINTS 15
|
||||
|
|
|
|||
|
|
@ -98,9 +98,7 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
|
|||
posControl.desiredState.vel.z = targetVel;
|
||||
}
|
||||
|
||||
#if defined(NAV_BLACKBOX)
|
||||
navDesiredVelocity[Z] = constrain(lrintf(posControl.desiredState.vel.z), -32678, 32767);
|
||||
#endif
|
||||
}
|
||||
|
||||
static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros)
|
||||
|
|
@ -467,10 +465,8 @@ static void updatePositionVelocityController_MC(const float maxSpeed)
|
|||
posControl.desiredState.vel.x = newVelX * velHeadFactor * velExpoFactor;
|
||||
posControl.desiredState.vel.y = newVelY * velHeadFactor * velExpoFactor;
|
||||
|
||||
#if defined(NAV_BLACKBOX)
|
||||
navDesiredVelocity[X] = constrain(lrintf(posControl.desiredState.vel.x), -32678, 32767);
|
||||
navDesiredVelocity[Y] = constrain(lrintf(posControl.desiredState.vel.y), -32678, 32767);
|
||||
#endif
|
||||
}
|
||||
|
||||
static float computeNormalizedVelocity(const float value, const float maxValue)
|
||||
|
|
|
|||
|
|
@ -443,12 +443,10 @@ static void updateIMUTopic(timeUs_t currentTimeUs)
|
|||
posEstimator.imu.accelNEU.z = 0;
|
||||
}
|
||||
|
||||
#if defined(NAV_BLACKBOX)
|
||||
/* Update blackbox values */
|
||||
navAccNEU[X] = posEstimator.imu.accelNEU.x;
|
||||
navAccNEU[Y] = posEstimator.imu.accelNEU.y;
|
||||
navAccNEU[Z] = posEstimator.imu.accelNEU.z;
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -785,10 +783,9 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs)
|
|||
updateActualAltitudeAndClimbRate(false, posEstimator.est.pos.z, 0, posEstimator.est.aglAlt, 0, EST_NONE);
|
||||
}
|
||||
|
||||
#if defined(NAV_BLACKBOX)
|
||||
//Update Blackbox states
|
||||
navEPH = posEstimator.est.eph;
|
||||
navEPV = posEstimator.est.epv;
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue