Minor blackbox code tidy
This commit is contained in:
parent
b8a8f6d2d8
commit
50ac13d79b
|
@ -449,6 +449,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
|
|||
|
||||
case FLIGHT_LOG_FIELD_CONDITION_NEVER:
|
||||
return false;
|
||||
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
|
@ -456,11 +457,8 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
|
|||
|
||||
static void blackboxBuildConditionCache(void)
|
||||
{
|
||||
FlightLogFieldCondition cond;
|
||||
|
||||
blackboxConditionCache = 0;
|
||||
|
||||
for (cond = FLIGHT_LOG_FIELD_CONDITION_FIRST; cond <= FLIGHT_LOG_FIELD_CONDITION_LAST; cond++) {
|
||||
for (FlightLogFieldCondition cond = FLIGHT_LOG_FIELD_CONDITION_FIRST; cond <= FLIGHT_LOG_FIELD_CONDITION_LAST; cond++) {
|
||||
if (testBlackboxConditionUncached(cond)) {
|
||||
blackboxConditionCache |= 1 << cond;
|
||||
}
|
||||
|
@ -765,10 +763,10 @@ static void loadSlowState(blackboxSlowState_t *slow)
|
|||
* If allowPeriodicWrite is true, the frame is also logged if it has been more than SLOW_FRAME_INTERVAL logging iterations
|
||||
* since the field was last logged.
|
||||
*/
|
||||
static void writeSlowFrameIfNeeded(bool allowPeriodicWrite)
|
||||
static void writeSlowFrameIfNeeded(void)
|
||||
{
|
||||
// Write the slow frame peridocially so it can be recovered if we ever lose sync
|
||||
bool shouldWrite = allowPeriodicWrite && blackboxSlowFrameIterationTimer >= SLOW_FRAME_INTERVAL;
|
||||
bool shouldWrite = blackboxSlowFrameIterationTimer >= SLOW_FRAME_INTERVAL;
|
||||
|
||||
if (shouldWrite) {
|
||||
loadSlowState(&slowHistory);
|
||||
|
@ -790,15 +788,6 @@ static void writeSlowFrameIfNeeded(bool allowPeriodicWrite)
|
|||
}
|
||||
}
|
||||
|
||||
static int gcd(int num, int denom)
|
||||
{
|
||||
if (denom == 0) {
|
||||
return num;
|
||||
}
|
||||
|
||||
return gcd(denom, num % denom);
|
||||
}
|
||||
|
||||
void blackboxValidateConfig(void)
|
||||
{
|
||||
if (blackboxConfig()->rate_num == 0 || blackboxConfig()->rate_denom == 0
|
||||
|
@ -1011,50 +1000,36 @@ static void writeGPSFrame(timeUs_t currentTimeUs)
|
|||
static void loadMainState(timeUs_t currentTimeUs)
|
||||
{
|
||||
blackboxMainState_t *blackboxCurrent = blackboxHistory[0];
|
||||
int i;
|
||||
|
||||
blackboxCurrent->time = currentTimeUs;
|
||||
|
||||
for (i = 0; i < XYZ_AXIS_COUNT; i++) {
|
||||
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
|
||||
blackboxCurrent->axisPID_P[i] = axisPID_P[i];
|
||||
}
|
||||
for (i = 0; i < XYZ_AXIS_COUNT; i++) {
|
||||
blackboxCurrent->axisPID_I[i] = axisPID_I[i];
|
||||
}
|
||||
for (i = 0; i < XYZ_AXIS_COUNT; i++) {
|
||||
blackboxCurrent->axisPID_D[i] = axisPID_D[i];
|
||||
blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]);
|
||||
blackboxCurrent->accSmooth[i] = acc.accSmooth[i];
|
||||
#ifdef MAG
|
||||
blackboxCurrent->magADC[i] = mag.magADC[i];
|
||||
#endif
|
||||
}
|
||||
|
||||
for (i = 0; i < 4; i++) {
|
||||
for (int i = 0; i < 4; i++) {
|
||||
blackboxCurrent->rcCommand[i] = rcCommand[i];
|
||||
}
|
||||
|
||||
for (i = 0; i < XYZ_AXIS_COUNT; i++) {
|
||||
blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]);
|
||||
}
|
||||
|
||||
for (i = 0; i < XYZ_AXIS_COUNT; i++) {
|
||||
blackboxCurrent->accSmooth[i] = acc.accSmooth[i];
|
||||
}
|
||||
|
||||
for (i = 0; i < 4; i++) {
|
||||
for (int i = 0; i < DEBUG16_VALUE_COUNT; i++) {
|
||||
blackboxCurrent->debug[i] = debug[i];
|
||||
}
|
||||
|
||||
const int motorCount = getMotorCount();
|
||||
for (i = 0; i < motorCount; i++) {
|
||||
for (int i = 0; i < motorCount; i++) {
|
||||
blackboxCurrent->motor[i] = motor[i];
|
||||
}
|
||||
|
||||
blackboxCurrent->vbatLatest = getBatteryVoltageLatest();
|
||||
blackboxCurrent->amperageLatest = getAmperageLatest();
|
||||
|
||||
#ifdef MAG
|
||||
for (i = 0; i < XYZ_AXIS_COUNT; i++) {
|
||||
blackboxCurrent->magADC[i] = mag.magADC[i];
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef BARO
|
||||
blackboxCurrent->BaroAlt = baro.BaroAlt;
|
||||
#endif
|
||||
|
@ -1425,6 +1400,22 @@ static bool blackboxShouldLogIFrame(void)
|
|||
return blackboxPFrameIndex == 0;
|
||||
}
|
||||
|
||||
/*
|
||||
* If the GPS home point has been updated, or every 128 I-frames (~10 seconds), write the
|
||||
* GPS home position.
|
||||
*
|
||||
* We write it periodically so that if one Home Frame goes missing, the GPS coordinates can
|
||||
* still be interpreted correctly.
|
||||
*/
|
||||
STATIC_UNIT_TESTED bool blackboxShouldLogGpsHomeFrame(void)
|
||||
{
|
||||
if (GPS_home[0] != gpsHistory.GPS_home[0] || GPS_home[1] != gpsHistory.GPS_home[1]
|
||||
|| (blackboxPFrameIndex == BLACKBOX_I_INTERVAL / 2 && blackboxIFrameIndex % 128 == 0)) {
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// Called once every FC loop in order to keep track of how many FC loop iterations have passed
|
||||
static void blackboxAdvanceIterationTimers(void)
|
||||
{
|
||||
|
@ -1447,7 +1438,9 @@ static void blackboxLogIteration(timeUs_t currentTimeUs)
|
|||
* Don't log a slow frame if the slow data didn't change ("I" frames are already large enough without adding
|
||||
* an additional item to write at the same time). Unless we're *only* logging "I" frames, then we have no choice.
|
||||
*/
|
||||
writeSlowFrameIfNeeded(blackboxIsOnlyLoggingIntraframes());
|
||||
if (blackboxIsOnlyLoggingIntraframes()) {
|
||||
writeSlowFrameIfNeeded();
|
||||
}
|
||||
|
||||
loadMainState(currentTimeUs);
|
||||
writeIntraframe();
|
||||
|
@ -1460,23 +1453,14 @@ static void blackboxLogIteration(timeUs_t currentTimeUs)
|
|||
* We assume that slow frames are only interesting in that they aid the interpretation of the main data stream.
|
||||
* So only log slow frames during loop iterations where we log a main frame.
|
||||
*/
|
||||
writeSlowFrameIfNeeded(true);
|
||||
writeSlowFrameIfNeeded();
|
||||
|
||||
loadMainState(currentTimeUs);
|
||||
writeInterframe();
|
||||
}
|
||||
#ifdef GPS
|
||||
if (feature(FEATURE_GPS)) {
|
||||
/*
|
||||
* If the GPS home point has been updated, or every 128 intraframes (~10 seconds), write the
|
||||
* GPS home position.
|
||||
*
|
||||
* We write it periodically so that if one Home Frame goes missing, the GPS coordinates can
|
||||
* still be interpreted correctly.
|
||||
*/
|
||||
if (GPS_home[0] != gpsHistory.GPS_home[0] || GPS_home[1] != gpsHistory.GPS_home[1]
|
||||
|| (blackboxPFrameIndex == BLACKBOX_I_INTERVAL / 2 && blackboxIFrameIndex % 128 == 0)) {
|
||||
|
||||
if (blackboxShouldLogGpsHomeFrame()) {
|
||||
writeGPSHomeFrame();
|
||||
writeGPSFrame(currentTimeUs);
|
||||
} else if (GPS_numSat != gpsHistory.GPS_numSat || GPS_coord[0] != gpsHistory.GPS_coord[0]
|
||||
|
@ -1497,8 +1481,6 @@ static void blackboxLogIteration(timeUs_t currentTimeUs)
|
|||
*/
|
||||
void blackboxUpdate(timeUs_t currentTimeUs)
|
||||
{
|
||||
int i;
|
||||
|
||||
switch (blackboxState) {
|
||||
case BLACKBOX_STATE_STOPPED:
|
||||
if (ARMING_FLAG(ARMED)) {
|
||||
|
@ -1526,7 +1508,7 @@ void blackboxUpdate(timeUs_t currentTimeUs)
|
|||
*/
|
||||
if (millis() > xmitState.u.startTime + 100) {
|
||||
if (blackboxDeviceReserveBufferSpace(BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION) == BLACKBOX_RESERVE_SUCCESS) {
|
||||
for (i = 0; i < BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION && blackboxHeader[xmitState.headerIndex] != '\0'; i++, xmitState.headerIndex++) {
|
||||
for (int i = 0; i < BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION && blackboxHeader[xmitState.headerIndex] != '\0'; i++, xmitState.headerIndex++) {
|
||||
blackboxWrite(blackboxHeader[xmitState.headerIndex]);
|
||||
blackboxHeaderBudget--;
|
||||
}
|
||||
|
|
|
@ -99,6 +99,15 @@ float acos_approx(float x)
|
|||
}
|
||||
#endif
|
||||
|
||||
int gcd(int num, int denom)
|
||||
{
|
||||
if (denom == 0) {
|
||||
return num;
|
||||
}
|
||||
|
||||
return gcd(denom, num % denom);
|
||||
}
|
||||
|
||||
float powerf(float base, int exp) {
|
||||
float result = base;
|
||||
for (int count = 1; count < exp; count++) result *= base;
|
||||
|
|
|
@ -76,6 +76,7 @@ typedef union {
|
|||
fp_angles_def angles;
|
||||
} fp_angles_t;
|
||||
|
||||
int gcd(int num, int denom);
|
||||
float powerf(float base, int exp);
|
||||
int32_t applyDeadband(int32_t value, int32_t deadband);
|
||||
|
||||
|
|
|
@ -88,7 +88,7 @@ void failsafeInit(void)
|
|||
return;
|
||||
}
|
||||
|
||||
failsafePhase_e failsafePhase()
|
||||
failsafePhase_e failsafePhase(void)
|
||||
{
|
||||
return failsafeState.phase;
|
||||
}
|
||||
|
|
|
@ -29,15 +29,6 @@ extern "C" {
|
|||
|
||||
#include "drivers/serial.h"
|
||||
#include "io/serial.h"
|
||||
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0);
|
||||
|
||||
PG_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig,
|
||||
.device = DEFAULT_BLACKBOX_DEVICE,
|
||||
.rate_num = 1,
|
||||
.rate_denom = 1,
|
||||
.on_motor_test = 0 // default off
|
||||
);
|
||||
}
|
||||
|
||||
#include "unittest_macros.h"
|
||||
|
@ -115,7 +106,7 @@ void serialTestResetBuffers()
|
|||
serialWritePos = 0;
|
||||
}
|
||||
|
||||
TEST(BlackboxTest, Test1)
|
||||
TEST(BlackboxEncodingTest, TestWriteUnsignedVB)
|
||||
{
|
||||
blackboxPort = &serialTestInstance;
|
||||
blackboxWriteUnsignedVB(0);
|
||||
|
@ -127,6 +118,7 @@ TEST(BlackboxTest, Test1)
|
|||
|
||||
// STUBS
|
||||
extern "C" {
|
||||
PG_REGISTER(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0);
|
||||
int32_t blackboxHeaderBudget;
|
||||
void mspSerialAllocatePorts(void) {}
|
||||
void blackboxWrite(uint8_t value) {serialWrite(blackboxPort, value);}
|
||||
|
@ -140,5 +132,4 @@ int blackboxPrint(const char *s)
|
|||
const int length = pos - (uint8_t*)s;
|
||||
return length;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue