Skip to content
This repository has been archived by the owner on Apr 19, 2023. It is now read-only.

Commit

Permalink
Add support for new gyro field name in 1.9.0
Browse files Browse the repository at this point in the history
  • Loading branch information
@thenickdude
thenickdude committed Jun 10, 2015
1 parent bd6944a commit 209995b
Show file tree
Hide file tree
Showing 7 changed files with 33 additions and 29 deletions.
16 changes: 8 additions & 8 deletions src/blackbox_decode.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -183,13 +183,13 @@ static bool fprintfMainFieldInUnit(flightLog_t *log, FILE *file, int fieldIndex,
}
break;
case UNIT_DEGREES_PER_SECOND:
if (fieldIndex >= log->mainFieldIndexes.gyroData[0] && fieldIndex <= log->mainFieldIndexes.gyroData[2]) {
if (fieldIndex >= log->mainFieldIndexes.gyroADC[0] && fieldIndex <= log->mainFieldIndexes.gyroADC[2]) {
fprintf(file, "%.2f", flightlogGyroToRadiansPerSecond(log, fieldValue) * (180 / M_PI));
return true;
}
break;
case UNIT_RADIANS_PER_SECOND:
if (fieldIndex >= log->mainFieldIndexes.gyroData[0] && fieldIndex <= log->mainFieldIndexes.gyroData[2]) {
if (fieldIndex >= log->mainFieldIndexes.gyroADC[0] && fieldIndex <= log->mainFieldIndexes.gyroADC[2]) {
fprintf(file, "%.2f", flightlogGyroToRadiansPerSecond(log, fieldValue));
return true;
}
Expand Down Expand Up @@ -333,7 +333,7 @@ void createGPSCSVFile(flightLog_t *log)

static void updateSimulations(flightLog_t *log, int32_t *frame, uint32_t currentTime)
{
int16_t gyroData[3];
int16_t gyroADC[3];
int16_t accSmooth[3];
int16_t magADC[3];

Expand All @@ -345,7 +345,7 @@ static void updateSimulations(flightLog_t *log, int32_t *frame, uint32_t current

if (options.simulateIMU) {
for (i = 0; i < 3; i++) {
gyroData[i] = (int16_t) frame[log->mainFieldIndexes.gyroData[i]];
gyroADC[i] = (int16_t) frame[log->mainFieldIndexes.gyroADC[i]];
accSmooth[i] = (int16_t) frame[log->mainFieldIndexes.accSmooth[i]];
}

Expand All @@ -355,7 +355,7 @@ static void updateSimulations(flightLog_t *log, int32_t *frame, uint32_t current
}
}

updateEstimatedAttitude(gyroData, accSmooth, hasMag && !options.imuIgnoreMag ? magADC : NULL,
updateEstimatedAttitude(gyroADC, accSmooth, hasMag && !options.imuIgnoreMag ? magADC : NULL,
currentTime, log->sysConfig.acc_1G, log->sysConfig.gyroScale, &attitude);
}

Expand Down Expand Up @@ -769,8 +769,8 @@ void applyFieldUnits(flightLog_t *log)
mainFieldUnit[log->mainFieldIndexes.accSmooth[i]] = options.unitAcceleration;
}

if (log->mainFieldIndexes.gyroData[i] > -1) {
mainFieldUnit[log->mainFieldIndexes.gyroData[i]] = options.unitRotation;
if (log->mainFieldIndexes.gyroADC[i] > -1) {
mainFieldUnit[log->mainFieldIndexes.gyroADC[i]] = options.unitRotation;
}
}

Expand Down Expand Up @@ -834,7 +834,7 @@ void onMetadataReady(flightLog_t *log)
if (log->frameDefs['I'].fieldCount == 0) {
fprintf(stderr, "No fields found in log, is it missing its header?\n");
return;
} else if (options.simulateIMU && (log->mainFieldIndexes.accSmooth[0] == -1 || log->mainFieldIndexes.gyroData[0] == -1)){
} else if (options.simulateIMU && (log->mainFieldIndexes.accSmooth[0] == -1 || log->mainFieldIndexes.gyroADC[0] == -1)){
fprintf(stderr, "Can't simulate the IMU because accelerometer or gyroscope data is missing\n");
options.simulateIMU = false;
}
Expand Down
18 changes: 9 additions & 9 deletions src/blackbox_render.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -260,7 +260,7 @@ void updateFieldMetadata()
fieldMeta.numMotors = 0;
fieldMeta.numServos = 0;

fieldMeta.hasGyros = flightLog->mainFieldIndexes.gyroData[0] > -1;
fieldMeta.hasGyros = flightLog->mainFieldIndexes.gyroADC[0] > -1;
fieldMeta.hasAccs = flightLog->mainFieldIndexes.accSmooth[0] > -1;
fieldMeta.hasMagADC = flightLog->mainFieldIndexes.magADC[0] > -1;

Expand Down Expand Up @@ -290,7 +290,7 @@ void updateFieldMetadata()
}

for (int axisIndex = 0; axisIndex < 3; axisIndex++) {
if (flightLog->mainFieldIndexes.gyroData[axisIndex]) {
if (flightLog->mainFieldIndexes.gyroADC[axisIndex]) {
if (options.plotGyros) {
if (options.bottomGraphSplitAxes)
fieldMeta.gyroColors[axisIndex] = lineColors[(PID_D + 2) % NUM_LINE_COLORS];
Expand Down Expand Up @@ -764,7 +764,7 @@ void drawPIDTable(cairo_t *cr, int32_t *frame)

if (pidType == PID_P - 1) {
if (fieldMeta.hasGyros) {
fieldValue = frame[flightLog->mainFieldIndexes.gyroData[axisIndex]];
fieldValue = frame[flightLog->mainFieldIndexes.gyroADC[axisIndex]];

if (options.gyroUnit == UNIT_DEGREES_PER_SEC) {
fieldValue = (int32_t) round(flightlogGyroToRadiansPerSecond(flightLog, fieldValue) * (180 / M_PI));
Expand Down Expand Up @@ -1163,7 +1163,7 @@ void renderAnimation(uint32_t startFrame, uint32_t endFrame)
cairo_set_line_width(cr, 3);

plotLine(cr, fieldMeta.gyroColors[axis], windowStartTime, windowEndTime, firstFrameIndex,
flightLog->mainFieldIndexes.gyroData[axis], gyroCurve, (int) (options.imageHeight * 0.15));
flightLog->mainFieldIndexes.gyroADC[axis], gyroCurve, (int) (options.imageHeight * 0.15));
}

const char *axisLabel;
Expand Down Expand Up @@ -1209,7 +1209,7 @@ void renderAnimation(uint32_t startFrame, uint32_t endFrame)

for (int axis = 0; axis < 3; axis++) {
plotLine(cr, fieldMeta.gyroColors[axis], windowStartTime, windowEndTime, firstFrameIndex,
flightLog->mainFieldIndexes.gyroData[axis], gyroCurve, (int) (options.imageHeight * 0.25));
flightLog->mainFieldIndexes.gyroADC[axis], gyroCurve, (int) (options.imageHeight * 0.25));
}

drawAxisLabel(cr, "Gyro");
Expand Down Expand Up @@ -1514,7 +1514,7 @@ void parseCommandlineOptions(int argc, char **argv)
static void applySmoothing() {
if (options.gyroSmoothing && fieldMeta.hasGyros) {
for (int axis = 0; axis < 3; axis++)
datapointsSmoothField(points, flightLog->mainFieldIndexes.gyroData[axis], options.gyroSmoothing);
datapointsSmoothField(points, flightLog->mainFieldIndexes.gyroADC[axis], options.gyroSmoothing);
}

if (options.pidSmoothing && fieldMeta.hasPIDs) {
Expand All @@ -1535,7 +1535,7 @@ static void applySmoothing() {
}

void computeExtraFields(void) {
int16_t accSmooth[3], gyroData[3], magADC[3];
int16_t accSmooth[3], gyroADC[3], magADC[3];
int64_t frameTime, lastFrameTime = 0;
int32_t frameIndex;
int32_t frame[FLIGHT_LOG_MAX_FIELDS];
Expand All @@ -1550,7 +1550,7 @@ void computeExtraFields(void) {
if (calculateAttitude) {
for (int axis = 0; axis < 3; axis++) {
accSmooth[axis] = frame[flightLog->mainFieldIndexes.accSmooth[axis]];
gyroData[axis] = frame[flightLog->mainFieldIndexes.gyroData[axis]];
gyroADC[axis] = frame[flightLog->mainFieldIndexes.gyroADC[axis]];
}

if (fieldMeta.hasMagADC) {
Expand All @@ -1559,7 +1559,7 @@ void computeExtraFields(void) {
}
}

updateEstimatedAttitude(gyroData, accSmooth, fieldMeta.hasMagADC ? magADC : 0, (uint32_t) frameTime, flightLog->sysConfig.acc_1G, flightLog->sysConfig.gyroScale, &attitude);
updateEstimatedAttitude(gyroADC, accSmooth, fieldMeta.hasMagADC ? magADC : 0, (uint32_t) frameTime, flightLog->sysConfig.acc_1G, flightLog->sysConfig.gyroScale, &attitude);

//Pack those floats into signed ints to store into the datapoints array:
datapointsSetFieldAtIndex(points, frameIndex, fieldMeta.roll, floatToInt(attitude.roll));
Expand Down
14 changes: 7 additions & 7 deletions src/encoder_testbed.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -113,9 +113,9 @@ static const blackboxMainFieldDefinition_t blackboxMainFields[] = {
#endif

/* Gyros and accelerometers base their P-predictions on the average of the previous 2 frames to reduce noise impact */
{"gyroData[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"gyroData[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"gyroData[2]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"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)},
Expand All @@ -138,7 +138,7 @@ typedef struct blackboxValues_t {
int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];

int16_t rcCommand[4];
int16_t gyroData[3];
int16_t gyroADC[3];
int16_t accSmooth[3];
int16_t motor[MAX_MOTORS];
int16_t servo[MAX_SERVOS];
Expand Down Expand Up @@ -560,7 +560,7 @@ static void writeIntraframe(void)
#endif

for (x = 0; x < XYZ_AXIS_COUNT; x++)
writeSignedVB(blackboxCurrent->gyroData[x]);
writeSignedVB(blackboxCurrent->gyroADC[x]);

for (x = 0; x < XYZ_AXIS_COUNT; x++)
writeSignedVB(blackboxCurrent->accSmooth[x]);
Expand Down Expand Up @@ -652,7 +652,7 @@ static void writeInterframe(void)

//Since gyros, accs and motors are noisy, base the prediction on the average of the history:
for (x = 0; x < XYZ_AXIS_COUNT; x++)
writeSignedVB(blackboxHistory[0]->gyroData[x] - (blackboxHistory[1]->gyroData[x] + blackboxHistory[2]->gyroData[x]) / 2);
writeSignedVB(blackboxHistory[0]->gyroADC[x] - (blackboxHistory[1]->gyroADC[x] + blackboxHistory[2]->gyroADC[x]) / 2);

for (x = 0; x < XYZ_AXIS_COUNT; x++)
writeSignedVB(blackboxHistory[0]->accSmooth[x] - (blackboxHistory[1]->accSmooth[x] + blackboxHistory[2]->accSmooth[x]) / 2);
Expand Down Expand Up @@ -721,7 +721,7 @@ void onFrameReady(flightLog_t *log, bool frameValid, int32_t *frame, uint8_t fra
blackboxCurrent->BaroAlt = frame[src++];

for (x = 0; x < XYZ_AXIS_COUNT; x++)
blackboxCurrent->gyroData[x] = frame[src++];
blackboxCurrent->gyroADC[x] = frame[src++];

for (x = 0; x < XYZ_AXIS_COUNT; x++)
blackboxCurrent->accSmooth[x] = frame[src++];
Expand Down
4 changes: 2 additions & 2 deletions src/imu.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -174,7 +174,7 @@ static float calculateHeading(t_fp_vector *vec, float angleradRoll, float angler
return hd;
}

void updateEstimatedAttitude(int16_t gyroData[3], int16_t accSmooth[3], int16_t magADC[3], uint32_t currentTime, uint16_t acc_1G, float gyroScale, attitude_t *attitude)
void updateEstimatedAttitude(int16_t gyroADC[3], int16_t accSmooth[3], int16_t magADC[3], uint32_t currentTime, uint16_t acc_1G, float gyroScale, attitude_t *attitude)
{
int32_t accMag = 0;
uint32_t deltaTime;
Expand All @@ -191,7 +191,7 @@ void updateEstimatedAttitude(int16_t gyroData[3], int16_t accSmooth[3], int16_t

// Initialization
for (int axis = 0; axis < 3; axis++) {
deltaGyroAngle[axis] = gyroData[axis] * scale;
deltaGyroAngle[axis] = gyroADC[axis] * scale;

accMag += (int32_t)accSmooth[axis] * accSmooth[axis];
}
Expand Down
2 changes: 1 addition & 1 deletion src/imu.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@ typedef struct attitude_t {
void imuInit(void);
void imuSetMagneticDeclination(double declination);

void updateEstimatedAttitude(int16_t gyroData[3], int16_t accSmooth[3], int16_t magADC[3], uint32_t currentTime, uint16_t acc_1G, float gyroScale, attitude_t *attitude);
void updateEstimatedAttitude(int16_t gyroADC[3], int16_t accSmooth[3], int16_t magADC[3], uint32_t currentTime, uint16_t acc_1G, float gyroScale, attitude_t *attitude);
t_fp_vector calculateAccelerationInEarthFrame(int16_t accSmooth[3], attitude_t *attitude, uint16_t acc_1G);

#endif
6 changes: 5 additions & 1 deletion src/parser.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -207,7 +207,11 @@ static void identifyMainFields(flightLog_t *log, flightLogFrameDef_t *frameDef)
} else if (startsWith(fieldName, "gyroData[")) {
int axisIndex = atoi(fieldName + strlen("gyroData["));

log->mainFieldIndexes.gyroData[axisIndex] = fieldIndex;
log->mainFieldIndexes.gyroADC[axisIndex] = fieldIndex;
} else if (startsWith(fieldName, "gyroADC[")) {
int axisIndex = atoi(fieldName + strlen("gyroADC["));

log->mainFieldIndexes.gyroADC[axisIndex] = fieldIndex;
} else if (startsWith(fieldName, "magADC[")) {
int axisIndex = atoi(fieldName + strlen("magADC["));

Expand Down
2 changes: 1 addition & 1 deletion src/parser.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -92,7 +92,7 @@ typedef struct mainFieldIndexes_t {
int magADC[3];
int BaroAlt;

int gyroData[3];
int gyroADC[3];
int accSmooth[3];

int motor[FLIGHT_LOG_MAX_MOTORS];
Expand Down

0 comments on commit 209995b

Please sign in to comment.