Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 17:55:28 +03:00
Code Issues Wiki Activity

Drop NAV_BLACKBOX define

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2021-08-26 09:43:35 +02:00
parent 1ee5ed366e
commit 2e84ca62c5
5 changed files with 8 additions and 36 deletions
Download patch file Download diff file Expand all files Collapse all files

12
src/main/blackbox/blackbox.c
View file

@ -325,7 +325,6 @@ 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)},
@ -346,7 +345,6 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
{"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
};
#ifdef USE_GPS
@ -476,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;
@ -489,7 +486,6 @@ typedef struct blackboxMainState_s {
int16_t navHeading;
int16_t navTargetHeading;
int16_t navSurface;
#endif
} blackboxMainState_t;
typedef struct blackboxGpsState_s {
@ -849,7 +845,6 @@ static void writeIntraframe(void)
}
}
#ifdef NAV_BLACKBOX
blackboxWriteSignedVB(blackboxCurrent->navState);
blackboxWriteSignedVB(blackboxCurrent->navFlags);
@ -877,7 +872,6 @@ static void writeIntraframe(void)
}
blackboxWriteSignedVB(blackboxCurrent->navSurface);
#endif
//Rotate our history buffers:
@ -1065,7 +1059,6 @@ 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);
@ -1093,7 +1086,6 @@ static void writeInterframe(void)
}
blackboxWriteSignedVB(blackboxCurrent->navSurface - blackboxLast->navSurface);
#endif
//Rotate our history buffers
blackboxHistory[2] = blackboxHistory[1];
@ -1494,7 +1486,6 @@ static void loadMainState(timeUs_t currentTimeUs)
blackboxCurrent->servo[i] = servo[i];
}
#ifdef NAV_BLACKBOX
blackboxCurrent->navState = navCurrentState;
blackboxCurrent->navFlags = navFlags;
blackboxCurrent->navEPH = navEPH;
@ -1507,7 +1498,6 @@ static void loadMainState(timeUs_t currentTimeUs)
blackboxCurrent->navTargetPos[i] = navTargetPosition[i];
}
blackboxCurrent->navSurface = navActualSurface;
#endif
}
/**
@ -1764,9 +1754,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);

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

@ -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
}
/*-----------------------------------------------------------

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

@ -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

4
src/main/navigation/navigation_multicopter.c
View file

@ -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)

5
src/main/navigation/navigation_pos_estimator.c
View file

@ -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
}
}