rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Minor blackbox code tidy

This commit is contained in:
Martin Budden 2017-05-02 07:13:55 +01:00
parent b8a8f6d2d8
commit 50ac13d79b
5 changed files with 48 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

88
src/main/blackbox/blackbox.c
View File

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

9
src/main/common/maths.c
View File

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

1
src/main/common/maths.h
View File

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

2
src/main/flight/failsafe.c
View File

@ -88,7 +88,7 @@ void failsafeInit(void)
return;
}
failsafePhase_e failsafePhase()
failsafePhase_e failsafePhase(void)
{
return failsafeState.phase;
}

13
src/test/unit/blackbox_encoding_unittest.cc
View File

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