Add (untested) support for recording Tricopter tail servo position
This commit is contained in:
parent
8c41772584
commit
e492aa6a39
11766
obj/cleanflight_NAZE.hex
11766
obj/cleanflight_NAZE.hex
File diff suppressed because it is too large
Load Diff
|
@ -212,6 +212,29 @@ static const char * const blackboxHeaderFields[] = {
|
||||||
ENCODING(SIGNED_VB)
|
ENCODING(SIGNED_VB)
|
||||||
};
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Additional fields to tack on to those above for tricopters (to record tail servo position)
|
||||||
|
*/
|
||||||
|
static const char * const blackboxAdditionalFieldsTricopter[] = {
|
||||||
|
//Field I name
|
||||||
|
"servo[5]",
|
||||||
|
|
||||||
|
//Field I signed
|
||||||
|
"0",
|
||||||
|
|
||||||
|
//Field I predictor
|
||||||
|
PREDICT(1500),
|
||||||
|
|
||||||
|
//Field I encoding:
|
||||||
|
ENCODING(SIGNED_VB),
|
||||||
|
|
||||||
|
//Field P predictor:
|
||||||
|
PREDICT(PREVIOUS),
|
||||||
|
|
||||||
|
//Field P encoding:
|
||||||
|
ENCODING(SIGNED_VB)
|
||||||
|
};
|
||||||
|
|
||||||
static const char blackboxGpsHeader[] =
|
static const char blackboxGpsHeader[] =
|
||||||
"H Field G name:"
|
"H Field G name:"
|
||||||
"GPS_numSat,GPS_coord[0],GPS_coord[1],GPS_altitude,GPS_speed\n"
|
"GPS_numSat,GPS_coord[0],GPS_coord[1],GPS_altitude,GPS_speed\n"
|
||||||
|
@ -270,6 +293,10 @@ static blackbox_values_t blackboxHistoryRing[3];
|
||||||
// These point into blackboxHistoryRing, use them to know where to store history of a given age (0, 1 or 2 generations old)
|
// These point into blackboxHistoryRing, use them to know where to store history of a given age (0, 1 or 2 generations old)
|
||||||
static blackbox_values_t* blackboxHistory[3];
|
static blackbox_values_t* blackboxHistory[3];
|
||||||
|
|
||||||
|
static int isTricopter()
|
||||||
|
{
|
||||||
|
return masterConfig.mixerConfiguration == MULTITYPE_TRI;
|
||||||
|
}
|
||||||
|
|
||||||
static void blackboxWrite(uint8_t value)
|
static void blackboxWrite(uint8_t value)
|
||||||
{
|
{
|
||||||
|
@ -425,6 +452,9 @@ static void writeIntraframe(void)
|
||||||
for (x = 1; x < motorCount; x++)
|
for (x = 1; x < motorCount; x++)
|
||||||
writeSignedVB(blackboxCurrent->motor[x] - blackboxCurrent->motor[0]);
|
writeSignedVB(blackboxCurrent->motor[x] - blackboxCurrent->motor[0]);
|
||||||
|
|
||||||
|
if (isTricopter())
|
||||||
|
writeSignedVB(blackboxHistory[0]->servo[5] - 1500);
|
||||||
|
|
||||||
//Rotate our history buffers:
|
//Rotate our history buffers:
|
||||||
|
|
||||||
//The current state becomes the new "before" state
|
//The current state becomes the new "before" state
|
||||||
|
@ -481,6 +511,9 @@ static void writeInterframe(void)
|
||||||
for (x = 0; x < motorCount; x++)
|
for (x = 0; x < motorCount; x++)
|
||||||
writeSignedVB(blackboxHistory[0]->motor[x] - (blackboxHistory[1]->motor[x] + blackboxHistory[2]->motor[x]) / 2);
|
writeSignedVB(blackboxHistory[0]->motor[x] - (blackboxHistory[1]->motor[x] + blackboxHistory[2]->motor[x]) / 2);
|
||||||
|
|
||||||
|
if (isTricopter())
|
||||||
|
writeSignedVB(blackboxCurrent->servo[5] - blackboxLast->servo[5]);
|
||||||
|
|
||||||
//Rotate our history buffers
|
//Rotate our history buffers
|
||||||
blackboxHistory[2] = blackboxHistory[1];
|
blackboxHistory[2] = blackboxHistory[1];
|
||||||
blackboxHistory[1] = blackboxHistory[0];
|
blackboxHistory[1] = blackboxHistory[0];
|
||||||
|
@ -576,10 +609,43 @@ static void writeGPSFrame()
|
||||||
gpsHistory.GPS_coord[1] = GPS_coord[1];
|
gpsHistory.GPS_coord[1] = GPS_coord[1];
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Fill the current state of the blackbox using values read from the flight controller
|
||||||
|
*/
|
||||||
|
static void loadBlackboxState(void)
|
||||||
|
{
|
||||||
|
blackbox_values_t *blackboxCurrent = blackboxHistory[0];
|
||||||
|
int i;
|
||||||
|
|
||||||
|
blackboxCurrent->time = currentTime;
|
||||||
|
|
||||||
|
for (i = 0; i < 3; i++)
|
||||||
|
blackboxCurrent->axisP[i] = axisP[i];
|
||||||
|
for (i = 0; i < 3; i++)
|
||||||
|
blackboxCurrent->axisI[i] = axisI[i];
|
||||||
|
for (i = 0; i < 3; i++)
|
||||||
|
blackboxCurrent->axisD[i] = axisD[i];
|
||||||
|
|
||||||
|
for (i = 0; i < 4; i++)
|
||||||
|
blackboxCurrent->rcCommand[i] = rcCommand[i];
|
||||||
|
|
||||||
|
for (i = 0; i < 3; i++)
|
||||||
|
blackboxCurrent->gyroData[i] = gyroData[i];
|
||||||
|
|
||||||
|
for (i = 0; i < 3; i++)
|
||||||
|
blackboxCurrent->accSmooth[i] = accSmooth[i];
|
||||||
|
|
||||||
|
for (i = 0; i < motorCount; i++)
|
||||||
|
blackboxCurrent->motor[i] = motor[i];
|
||||||
|
|
||||||
|
//Tail servo for tricopters
|
||||||
|
blackboxCurrent->servo[5] = servo[5];
|
||||||
|
}
|
||||||
|
|
||||||
void handleBlackbox(void)
|
void handleBlackbox(void)
|
||||||
{
|
{
|
||||||
int i;
|
int i;
|
||||||
blackbox_values_t *blackboxCurrent;
|
const char *additionalHeader;
|
||||||
const int SERIAL_CHUNK_SIZE = 16;
|
const int SERIAL_CHUNK_SIZE = 16;
|
||||||
static int charXmitIndex = 0;
|
static int charXmitIndex = 0;
|
||||||
int motorsToRemove, endIndex;
|
int motorsToRemove, endIndex;
|
||||||
|
@ -607,8 +673,10 @@ void handleBlackbox(void)
|
||||||
break;
|
break;
|
||||||
case BLACKBOX_STATE_SEND_FIELDINFO:
|
case BLACKBOX_STATE_SEND_FIELDINFO:
|
||||||
/*
|
/*
|
||||||
* Once again, chunking up the data so we don't exceed our datarate. This time we're removing the excess field defs
|
* Send information about the fields we're recording to the log.
|
||||||
* for motors we don't have.
|
*
|
||||||
|
* Once again, we're chunking up the data so we don't exceed our datarate. This time we're trimming the
|
||||||
|
* excess field defs off the end of the header for motors we don't have.
|
||||||
*/
|
*/
|
||||||
motorsToRemove = 8 - motorCount;
|
motorsToRemove = 8 - motorCount;
|
||||||
|
|
||||||
|
@ -618,7 +686,16 @@ void handleBlackbox(void)
|
||||||
for (i = charXmitIndex; i < charXmitIndex + SERIAL_CHUNK_SIZE && i < endIndex; i++)
|
for (i = charXmitIndex; i < charXmitIndex + SERIAL_CHUNK_SIZE && i < endIndex; i++)
|
||||||
blackboxWrite(blackboxHeaderFields[headerXmitIndex][i]);
|
blackboxWrite(blackboxHeaderFields[headerXmitIndex][i]);
|
||||||
|
|
||||||
|
// Did we complete this line?
|
||||||
if (i == endIndex) {
|
if (i == endIndex) {
|
||||||
|
if (isTricopter()) {
|
||||||
|
//Add fields to the end for the tail servo
|
||||||
|
blackboxWrite(',');
|
||||||
|
|
||||||
|
for (additionalHeader = blackboxAdditionalFieldsTricopter[headerXmitIndex]; *additionalHeader; additionalHeader++)
|
||||||
|
blackboxWrite(*additionalHeader);
|
||||||
|
}
|
||||||
|
|
||||||
blackboxWrite('\n');
|
blackboxWrite('\n');
|
||||||
headerXmitIndex++;
|
headerXmitIndex++;
|
||||||
charXmitIndex = 0;
|
charXmitIndex = 0;
|
||||||
|
@ -626,6 +703,8 @@ void handleBlackbox(void)
|
||||||
charXmitIndex = i;
|
charXmitIndex = i;
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
|
//Completed sending field information
|
||||||
|
|
||||||
if (feature(FEATURE_GPS)) {
|
if (feature(FEATURE_GPS)) {
|
||||||
blackboxState = BLACKBOX_STATE_SEND_GPS_HEADERS;
|
blackboxState = BLACKBOX_STATE_SEND_GPS_HEADERS;
|
||||||
} else {
|
} else {
|
||||||
|
@ -678,29 +757,8 @@ void handleBlackbox(void)
|
||||||
headerXmitIndex++;
|
headerXmitIndex++;
|
||||||
break;
|
break;
|
||||||
case BLACKBOX_STATE_RUNNING:
|
case BLACKBOX_STATE_RUNNING:
|
||||||
// Copy current system values into the blackbox to be written out
|
// Copy current system values into the blackbox
|
||||||
blackboxCurrent = blackboxHistory[0];
|
loadBlackboxState();
|
||||||
|
|
||||||
blackboxCurrent->time = currentTime;
|
|
||||||
|
|
||||||
for (i = 0; i < motorCount; i++)
|
|
||||||
blackboxCurrent->motor[i] = motor[i];
|
|
||||||
|
|
||||||
for (i = 0; i < 3; i++)
|
|
||||||
blackboxCurrent->axisP[i] = axisP[i];
|
|
||||||
for (i = 0; i < 3; i++)
|
|
||||||
blackboxCurrent->axisI[i] = axisI[i];
|
|
||||||
for (i = 0; i < 3; i++)
|
|
||||||
blackboxCurrent->axisD[i] = axisD[i];
|
|
||||||
|
|
||||||
for (i = 0; i < 4; i++)
|
|
||||||
blackboxCurrent->rcCommand[i] = rcCommand[i];
|
|
||||||
|
|
||||||
for (i = 0; i < 3; i++)
|
|
||||||
blackboxCurrent->gyroData[i] = gyroData[i];
|
|
||||||
|
|
||||||
for (i = 0; i < 3; i++)
|
|
||||||
blackboxCurrent->accSmooth[i] = accSmooth[i];
|
|
||||||
|
|
||||||
// Write a keyframe every 32 frames so we can resynchronise upon missing frames
|
// Write a keyframe every 32 frames so we can resynchronise upon missing frames
|
||||||
int blackboxIntercycleIndex = blackboxIteration % 32;
|
int blackboxIntercycleIndex = blackboxIteration % 32;
|
||||||
|
|
|
@ -27,7 +27,8 @@ typedef struct blackbox_values_t {
|
||||||
int16_t rcCommand[4];
|
int16_t rcCommand[4];
|
||||||
int16_t gyroData[3];
|
int16_t gyroData[3];
|
||||||
int16_t accSmooth[3];
|
int16_t accSmooth[3];
|
||||||
int16_t motor[8];
|
int16_t motor[MAX_SUPPORTED_MOTORS];
|
||||||
|
int16_t servo[MAX_SUPPORTED_SERVOS];
|
||||||
} blackbox_values_t;
|
} blackbox_values_t;
|
||||||
|
|
||||||
void initBlackbox(void);
|
void initBlackbox(void);
|
||||||
|
|
|
@ -41,6 +41,9 @@
|
||||||
//Predict this GPS co-ordinate is the GPS home co-ordinate (or no prediction if that coordinate is not set)
|
//Predict this GPS co-ordinate is the GPS home co-ordinate (or no prediction if that coordinate is not set)
|
||||||
#define FLIGHT_LOG_FIELD_PREDICTOR_HOME_COORD 7
|
#define FLIGHT_LOG_FIELD_PREDICTOR_HOME_COORD 7
|
||||||
|
|
||||||
|
//Predict 1500
|
||||||
|
#define FLIGHT_LOG_FIELD_PREDICTOR_1500 8
|
||||||
|
|
||||||
|
|
||||||
#define FLIGHT_LOG_FIELD_ENCODING_SIGNED_VB 0
|
#define FLIGHT_LOG_FIELD_ENCODING_SIGNED_VB 0
|
||||||
#define FLIGHT_LOG_FIELD_ENCODING_UNSIGNED_VB 1
|
#define FLIGHT_LOG_FIELD_ENCODING_UNSIGNED_VB 1
|
||||||
|
|
Loading…
Reference in New Issue