Header definitions now provided by a struct instead of lots of strings
Introduce mechanism for disabling log fields at logging-start Remove a div/mod from handleBlackbox() Bring code required to be executed upon state transitions into a central setState() routine.
This commit is contained in:
Nicholas Sherlock
parent
50c81aa00e
commit
9b9474250e
|
|
@ -79,6 +79,8 @@
|
||||||
#define BLACKBOX_INITIAL_PORT_MODE MODE_TX
|
#define BLACKBOX_INITIAL_PORT_MODE MODE_TX
|
||||||
#define BLACKBOX_I_INTERVAL 32
|
#define BLACKBOX_I_INTERVAL 32
|
||||||
|
|
||||||
|
#define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0]))
|
||||||
|
|
||||||
// Some macros to make writing FLIGHT_LOG_FIELD_PREDICTOR_* constants shorter:
|
// Some macros to make writing FLIGHT_LOG_FIELD_PREDICTOR_* constants shorter:
|
||||||
#define STR_HELPER(x) #x
|
#define STR_HELPER(x) #x
|
||||||
#define STR(x) STR_HELPER(x)
|
#define STR(x) STR_HELPER(x)
|
||||||
|
|
@ -86,8 +88,11 @@
|
||||||
#define CONCAT_HELPER(x,y) x ## y
|
#define CONCAT_HELPER(x,y) x ## y
|
||||||
#define CONCAT(x,y) CONCAT_HELPER(x, y)
|
#define CONCAT(x,y) CONCAT_HELPER(x, y)
|
||||||
|
|
||||||
#define PREDICT(x) STR(CONCAT(FLIGHT_LOG_FIELD_PREDICTOR_, x))
|
#define PREDICT(x) CONCAT(FLIGHT_LOG_FIELD_PREDICTOR_, x)
|
||||||
#define ENCODING(x) STR(CONCAT(FLIGHT_LOG_FIELD_ENCODING_, x))
|
#define ENCODING(x) CONCAT(FLIGHT_LOG_FIELD_ENCODING_, x)
|
||||||
|
#define CONDITION(x) CONCAT(FLIGHT_LOG_FIELD_CONDITION_, x)
|
||||||
|
#define UNSIGNED FLIGHT_LOG_FIELD_UNSIGNED
|
||||||
|
#define SIGNED FLIGHT_LOG_FIELD_SIGNED
|
||||||
|
|
||||||
static const char blackboxHeader[] =
|
static const char blackboxHeader[] =
|
||||||
"H Product:Blackbox flight data recorder by Nicholas Sherlock\n"
|
"H Product:Blackbox flight data recorder by Nicholas Sherlock\n"
|
||||||
|
|
@ -95,174 +100,115 @@ static const char blackboxHeader[] =
|
||||||
"H Data version:2\n"
|
"H Data version:2\n"
|
||||||
"H I interval:" STR(BLACKBOX_I_INTERVAL) "\n";
|
"H I interval:" STR(BLACKBOX_I_INTERVAL) "\n";
|
||||||
|
|
||||||
/* These headers have info for all 8 motors on them, we'll trim the final fields off to match the number of motors in the mixer: */
|
static const char* const blackboxMainHeaderNames[] = {
|
||||||
static const char * const blackboxHeaderFields[] = {
|
"I name",
|
||||||
"H Field I name:"
|
"I signed",
|
||||||
"loopIteration,time,"
|
"I predictor",
|
||||||
"axisP[0],axisP[1],axisP[2],"
|
"I encoding",
|
||||||
"axisI[0],axisI[1],axisI[2],"
|
"P predictor",
|
||||||
"axisD[0],axisD[1],axisD[2],"
|
"P encoding"
|
||||||
"rcCommand[0],rcCommand[1],rcCommand[2],rcCommand[3],"
|
|
||||||
"gyroData[0],gyroData[1],gyroData[2],"
|
|
||||||
"accSmooth[0],accSmooth[1],accSmooth[2],"
|
|
||||||
"motor[0],motor[1],motor[2],motor[3],"
|
|
||||||
"motor[4],motor[5],motor[6],motor[7]",
|
|
||||||
|
|
||||||
"H Field I signed:"
|
|
||||||
/* loopIteration, time: */
|
|
||||||
"0,0,"
|
|
||||||
/* PIDs: */
|
|
||||||
"1,1,1,1,1,1,1,1,1,"
|
|
||||||
/* rcCommand[0..2] */
|
|
||||||
"1,1,1,"
|
|
||||||
/* rcCommand[3] (Throttle): */
|
|
||||||
"0,"
|
|
||||||
/* gyroData[0..2]: */
|
|
||||||
"1,1,1,"
|
|
||||||
/* accSmooth[0..2]: */
|
|
||||||
"1,1,1,"
|
|
||||||
/* Motor[0..7]: */
|
|
||||||
"0,0,0,0,0,0,0,0",
|
|
||||||
|
|
||||||
"H Field I predictor:"
|
|
||||||
/* loopIteration, time: */
|
|
||||||
PREDICT(0) "," PREDICT(0) ","
|
|
||||||
/* PIDs: */
|
|
||||||
PREDICT(0) "," PREDICT(0) "," PREDICT(0) ","
|
|
||||||
PREDICT(0) "," PREDICT(0) "," PREDICT(0) ","
|
|
||||||
PREDICT(0) "," PREDICT(0) "," PREDICT(0) ","
|
|
||||||
/* rcCommand[0..2] */
|
|
||||||
PREDICT(0) "," PREDICT(0) "," PREDICT(0) ","
|
|
||||||
/* rcCommand[3] (Throttle): */
|
|
||||||
PREDICT(MINTHROTTLE) ","
|
|
||||||
/* gyroData[0..2]: */
|
|
||||||
PREDICT(0) "," PREDICT(0) "," PREDICT(0) ","
|
|
||||||
/* accSmooth[0..2]: */
|
|
||||||
PREDICT(0) "," PREDICT(0) "," PREDICT(0) ","
|
|
||||||
/* Motor[0]: */
|
|
||||||
PREDICT(MINTHROTTLE) ","
|
|
||||||
/* Motor[1..7]: */
|
|
||||||
PREDICT(MOTOR_0) "," PREDICT(MOTOR_0) "," PREDICT(MOTOR_0) ","
|
|
||||||
PREDICT(MOTOR_0) "," PREDICT(MOTOR_0) "," PREDICT(MOTOR_0) ","
|
|
||||||
PREDICT(MOTOR_0),
|
|
||||||
|
|
||||||
"H Field I encoding:"
|
|
||||||
/* loopIteration, time: */
|
|
||||||
ENCODING(UNSIGNED_VB) "," ENCODING(UNSIGNED_VB) ","
|
|
||||||
/* PIDs: */
|
|
||||||
ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) ","
|
|
||||||
ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) ","
|
|
||||||
ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) ","
|
|
||||||
/* rcCommand[0..2] */
|
|
||||||
ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) ","
|
|
||||||
/* rcCommand[3] (Throttle): */
|
|
||||||
ENCODING(UNSIGNED_VB) ","
|
|
||||||
/* gyroData[0..2]: */
|
|
||||||
ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) ","
|
|
||||||
/* accSmooth[0..2]: */
|
|
||||||
ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) ","
|
|
||||||
/* Motor[0]: */
|
|
||||||
ENCODING(UNSIGNED_VB) ","
|
|
||||||
/* Motor[1..7]: */
|
|
||||||
ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) ","
|
|
||||||
ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) ","
|
|
||||||
ENCODING(SIGNED_VB),
|
|
||||||
|
|
||||||
//Motors and gyros predict an average of the last two measurements (to reduce the impact of noise):
|
|
||||||
"H Field P predictor:"
|
|
||||||
/* loopIteration, time: */
|
|
||||||
PREDICT(INC) "," PREDICT(STRAIGHT_LINE) ","
|
|
||||||
/* PIDs: */
|
|
||||||
PREDICT(PREVIOUS) "," PREDICT(PREVIOUS) "," PREDICT(PREVIOUS) ","
|
|
||||||
PREDICT(PREVIOUS) "," PREDICT(PREVIOUS) "," PREDICT(PREVIOUS) ","
|
|
||||||
PREDICT(PREVIOUS) "," PREDICT(PREVIOUS) "," PREDICT(PREVIOUS) ","
|
|
||||||
/* rcCommand[0..2] */
|
|
||||||
PREDICT(PREVIOUS) "," PREDICT(PREVIOUS) "," PREDICT(PREVIOUS) ","
|
|
||||||
/* rcCommand[3] (Throttle): */
|
|
||||||
PREDICT(PREVIOUS) ","
|
|
||||||
/* gyroData[0..2]: */
|
|
||||||
PREDICT(AVERAGE_2) "," PREDICT(AVERAGE_2) "," PREDICT(AVERAGE_2) ","
|
|
||||||
/* accSmooth[0..2]: */
|
|
||||||
PREDICT(AVERAGE_2) "," PREDICT(AVERAGE_2) "," PREDICT(AVERAGE_2) ","
|
|
||||||
/* Motor[0]: */
|
|
||||||
PREDICT(AVERAGE_2) ","
|
|
||||||
/* Motor[1..7]: */
|
|
||||||
PREDICT(AVERAGE_2) "," PREDICT(AVERAGE_2) "," PREDICT(AVERAGE_2) ","
|
|
||||||
PREDICT(AVERAGE_2) "," PREDICT(AVERAGE_2) "," PREDICT(AVERAGE_2) ","
|
|
||||||
PREDICT(AVERAGE_2),
|
|
||||||
|
|
||||||
/* PID_I terms and RC fields are encoded together as groups, everything else is signed since they're diffs: */
|
|
||||||
"H Field P encoding:"
|
|
||||||
/* loopIteration, time: */
|
|
||||||
ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) ","
|
|
||||||
/* PIDs: */
|
|
||||||
ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) ","
|
|
||||||
ENCODING(TAG2_3S32) "," ENCODING(TAG2_3S32) "," ENCODING(TAG2_3S32) ","
|
|
||||||
ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) ","
|
|
||||||
/* rcCommand[0..3] */
|
|
||||||
ENCODING(TAG8_4S16) "," ENCODING(TAG8_4S16) "," ENCODING(TAG8_4S16) ","
|
|
||||||
ENCODING(TAG8_4S16) ","
|
|
||||||
/* gyroData[0..2]: */
|
|
||||||
ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) ","
|
|
||||||
/* accSmooth[0..2]: */
|
|
||||||
ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) ","
|
|
||||||
/* Motor[0]: */
|
|
||||||
ENCODING(SIGNED_VB) ","
|
|
||||||
/* Motor[1..7]: */
|
|
||||||
ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) ","
|
|
||||||
ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) ","
|
|
||||||
ENCODING(SIGNED_VB)
|
|
||||||
};
|
};
|
||||||
|
|
||||||
/**
|
static const char* const blackboxGPSGHeaderNames[] = {
|
||||||
* Additional fields to tack on to those above for tricopters (to record tail servo position)
|
"G name",
|
||||||
*/
|
"G signed",
|
||||||
static const char * const blackboxAdditionalFieldsTricopter[] = {
|
"G predictor",
|
||||||
//Field I name
|
"G encoding"
|
||||||
"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* const blackboxGPSHHeaderNames[] = {
|
||||||
"H Field G name:"
|
"H name",
|
||||||
"GPS_numSat,GPS_coord[0],GPS_coord[1],GPS_altitude,GPS_speed\n"
|
"H signed",
|
||||||
"H Field G signed:"
|
"H predictor",
|
||||||
"0,1,1,0,0\n"
|
"H encoding"
|
||||||
"H Field G predictor:"
|
};
|
||||||
PREDICT(0) "," PREDICT(HOME_COORD) "," PREDICT(HOME_COORD) "," PREDICT(0) "," PREDICT(0) "\n"
|
|
||||||
"H Field G encoding:"
|
|
||||||
ENCODING(UNSIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) ","
|
|
||||||
ENCODING(UNSIGNED_VB) "," ENCODING(UNSIGNED_VB) "\n"
|
|
||||||
|
|
||||||
"H Field H name:"
|
/* All field definition structs should look like this (but with longer arrs): */
|
||||||
"GPS_home[0],GPS_home[1]\n"
|
typedef struct blackboxFieldDefinition_t {
|
||||||
"H Field H signed:"
|
const char *name;
|
||||||
"1,1\n"
|
uint8_t arr[1];
|
||||||
"H Field H predictor:"
|
} blackboxFieldDefinition_t;
|
||||||
PREDICT(0) "," PREDICT(0) "\n"
|
|
||||||
"H Field H encoding:"
|
typedef struct blackboxMainFieldDefinition_t {
|
||||||
ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "\n";
|
const char *name;
|
||||||
|
uint8_t isSigned;
|
||||||
|
uint8_t Ipredict;
|
||||||
|
uint8_t Iencode;
|
||||||
|
uint8_t Ppredict;
|
||||||
|
uint8_t Pencode;
|
||||||
|
uint8_t condition; // Decide whether this field should appear in the log
|
||||||
|
} blackboxMainFieldDefinition_t;
|
||||||
|
|
||||||
|
typedef struct blackboxGPSFieldDefinition_t {
|
||||||
|
const char *name;
|
||||||
|
uint8_t isSigned;
|
||||||
|
uint8_t predict;
|
||||||
|
uint8_t encode;
|
||||||
|
} blackboxGPSFieldDefinition_t;
|
||||||
|
|
||||||
|
static const blackboxMainFieldDefinition_t blackboxMainFields[] = {
|
||||||
|
/* loopIteration doesn't appear in P frames since it always increments */
|
||||||
|
{"loopIteration", UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(INC), .Pencode = FLIGHT_LOG_FIELD_ENCODING_NULL, CONDITION(ALWAYS)},
|
||||||
|
/* Time advances pretty steadily so the P-frame prediction is a straight line */
|
||||||
|
{"time", UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(STRAIGHT_LINE), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||||
|
{"axisP[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||||
|
{"axisP[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||||
|
{"axisP[2]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||||
|
/* I terms get special packed encoding in P frames: */
|
||||||
|
{"axisI[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG2_3S32), CONDITION(ALWAYS)},
|
||||||
|
{"axisI[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG2_3S32), CONDITION(ALWAYS)},
|
||||||
|
{"axisI[2]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG2_3S32), CONDITION(ALWAYS)},
|
||||||
|
{"axisD[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||||
|
{"axisD[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||||
|
{"axisD[2]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||||
|
/* rcCommands are encoded together as a group in P-frames: */
|
||||||
|
{"rcCommand[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
|
||||||
|
{"rcCommand[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
|
||||||
|
{"rcCommand[2]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
|
||||||
|
/* Throttle is always in the range [minthrottle..maxthrottle]: */
|
||||||
|
{"rcCommand[3]", UNSIGNED, .Ipredict = PREDICT(MINTHROTTLE), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
|
||||||
|
/* 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)},
|
||||||
|
{"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)},
|
||||||
|
/* Motors only rarely drops under minthrottle (when stick falls below mincommand), so predict minthrottle for it and use *unsigned* encoding (which is large for negative numbers but more compact for positive ones): */
|
||||||
|
{"motor[0]", UNSIGNED, .Ipredict = PREDICT(MINTHROTTLE), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_1)},
|
||||||
|
/* Subsequent motors base their I-frame values on the first one, P-frame values on the average of last two frames: */
|
||||||
|
{"motor[1]", UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_2)},
|
||||||
|
{"motor[2]", UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_3)},
|
||||||
|
{"motor[3]", UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_4)},
|
||||||
|
{"motor[4]", UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_5)},
|
||||||
|
{"motor[5]", UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_6)},
|
||||||
|
{"motor[6]", UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_7)},
|
||||||
|
{"motor[7]", UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_8)},
|
||||||
|
{"servo[5]", UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(TRICOPTER)}
|
||||||
|
};
|
||||||
|
|
||||||
|
// GPS position/vel frame
|
||||||
|
static const blackboxGPSFieldDefinition_t blackboxGpsGFields[] = {
|
||||||
|
{"GPS_numSat", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
|
||||||
|
{"GPS_coord[0]", SIGNED, PREDICT(HOME_COORD), ENCODING(SIGNED_VB)},
|
||||||
|
{"GPS_coord[1]", SIGNED, PREDICT(HOME_COORD), ENCODING(SIGNED_VB)},
|
||||||
|
{"GPS_altitude", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
|
||||||
|
{"GPS_speed", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}
|
||||||
|
};
|
||||||
|
|
||||||
|
// GPS home frame
|
||||||
|
static const blackboxGPSFieldDefinition_t blackboxGpsHFields[] = {
|
||||||
|
{"GPS_home[0]", SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
|
||||||
|
{"GPS_home[1]", SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}
|
||||||
|
};
|
||||||
|
|
||||||
typedef enum BlackboxState {
|
typedef enum BlackboxState {
|
||||||
BLACKBOX_STATE_DISABLED = 0,
|
BLACKBOX_STATE_DISABLED = 0,
|
||||||
BLACKBOX_STATE_STOPPED,
|
BLACKBOX_STATE_STOPPED,
|
||||||
BLACKBOX_STATE_SEND_HEADER,
|
BLACKBOX_STATE_SEND_HEADER,
|
||||||
BLACKBOX_STATE_SEND_FIELDINFO,
|
BLACKBOX_STATE_SEND_FIELDINFO,
|
||||||
BLACKBOX_STATE_SEND_GPS_HEADERS,
|
BLACKBOX_STATE_SEND_GPS_H_HEADERS,
|
||||||
|
BLACKBOX_STATE_SEND_GPS_G_HEADERS,
|
||||||
BLACKBOX_STATE_SEND_SYSINFO,
|
BLACKBOX_STATE_SEND_SYSINFO,
|
||||||
BLACKBOX_STATE_RUNNING
|
BLACKBOX_STATE_RUNNING
|
||||||
} BlackboxState;
|
} BlackboxState;
|
||||||
|
|
@ -278,10 +224,16 @@ extern uint8_t motorCount;
|
||||||
//From mw.c:
|
//From mw.c:
|
||||||
extern uint32_t currentTime;
|
extern uint32_t currentTime;
|
||||||
|
|
||||||
|
static const int SERIAL_CHUNK_SIZE = 16;
|
||||||
|
|
||||||
static BlackboxState blackboxState = BLACKBOX_STATE_DISABLED;
|
static BlackboxState blackboxState = BLACKBOX_STATE_DISABLED;
|
||||||
|
|
||||||
static uint32_t startTime;
|
static uint32_t startTime;
|
||||||
static unsigned int headerXmitIndex;
|
static unsigned int headerXmitIndex;
|
||||||
|
static int fieldXmitIndex;
|
||||||
|
|
||||||
static uint32_t blackboxIteration;
|
static uint32_t blackboxIteration;
|
||||||
|
static uint32_t blackboxPFrameIndex, blackboxIFrameIndex;
|
||||||
|
|
||||||
static serialPort_t *blackboxPort;
|
static serialPort_t *blackboxPort;
|
||||||
static portMode_t previousPortMode;
|
static portMode_t previousPortMode;
|
||||||
|
|
@ -295,11 +247,6 @@ static blackboxValues_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 blackboxValues_t* blackboxHistory[3];
|
static blackboxValues_t* blackboxHistory[3];
|
||||||
|
|
||||||
static int isTricopter()
|
|
||||||
{
|
|
||||||
return masterConfig.mixerConfiguration == MULTITYPE_TRI;
|
|
||||||
}
|
|
||||||
|
|
||||||
static void blackboxWrite(uint8_t value)
|
static void blackboxWrite(uint8_t value)
|
||||||
{
|
{
|
||||||
serialWrite(blackboxPort, value);
|
serialWrite(blackboxPort, value);
|
||||||
|
|
@ -320,6 +267,19 @@ static void blackboxPrintf(char *fmt, ...)
|
||||||
va_end(va);
|
va_end(va);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Print the null-terminated string 's' to the serial port and return the number of bytes written
|
||||||
|
static int blackboxPrint(const char *s)
|
||||||
|
{
|
||||||
|
const char *pos = s;
|
||||||
|
|
||||||
|
while (*pos) {
|
||||||
|
serialWrite(blackboxPort, *pos);
|
||||||
|
pos++;
|
||||||
|
}
|
||||||
|
|
||||||
|
return pos - s;
|
||||||
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Write an unsigned integer to the blackbox serial port using variable byte encoding.
|
* Write an unsigned integer to the blackbox serial port using variable byte encoding.
|
||||||
*/
|
*/
|
||||||
|
|
@ -545,6 +505,64 @@ static void writeTag8_4S16(int32_t *values) {
|
||||||
blackboxWrite(buffer);
|
blackboxWrite(buffer);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
static bool testBlackboxCondition(FlightLogFieldCondition condition)
|
||||||
|
{
|
||||||
|
switch (condition) {
|
||||||
|
case FLIGHT_LOG_FIELD_CONDITION_ALWAYS:
|
||||||
|
return true;
|
||||||
|
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1:
|
||||||
|
return motorCount >= 1;
|
||||||
|
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_2:
|
||||||
|
return motorCount >= 2;
|
||||||
|
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_3:
|
||||||
|
return motorCount >= 3;
|
||||||
|
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_4:
|
||||||
|
return motorCount >= 4;
|
||||||
|
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_5:
|
||||||
|
return motorCount >= 5;
|
||||||
|
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_6:
|
||||||
|
return motorCount >= 6;
|
||||||
|
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_7:
|
||||||
|
return motorCount >= 7;
|
||||||
|
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_8:
|
||||||
|
return motorCount >= 8;
|
||||||
|
case FLIGHT_LOG_FIELD_CONDITION_TRICOPTER:
|
||||||
|
return masterConfig.mixerConfiguration == MULTITYPE_TRI;
|
||||||
|
case FLIGHT_LOG_FIELD_CONDITION_NEVER:
|
||||||
|
return false;
|
||||||
|
default:
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
static void blackboxSetState(BlackboxState newState)
|
||||||
|
{
|
||||||
|
//Perform initial setup required for the new state
|
||||||
|
switch (newState) {
|
||||||
|
case BLACKBOX_STATE_SEND_HEADER:
|
||||||
|
startTime = millis();
|
||||||
|
headerXmitIndex = 0;
|
||||||
|
break;
|
||||||
|
case BLACKBOX_STATE_SEND_FIELDINFO:
|
||||||
|
case BLACKBOX_STATE_SEND_GPS_G_HEADERS:
|
||||||
|
case BLACKBOX_STATE_SEND_GPS_H_HEADERS:
|
||||||
|
headerXmitIndex = 0;
|
||||||
|
fieldXmitIndex = -1;
|
||||||
|
break;
|
||||||
|
case BLACKBOX_STATE_SEND_SYSINFO:
|
||||||
|
headerXmitIndex = 0;
|
||||||
|
break;
|
||||||
|
case BLACKBOX_STATE_RUNNING:
|
||||||
|
blackboxIteration = 0;
|
||||||
|
blackboxPFrameIndex = 0;
|
||||||
|
blackboxIFrameIndex = 0;
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
;
|
||||||
|
}
|
||||||
|
blackboxState = newState;
|
||||||
|
}
|
||||||
|
|
||||||
static void writeIntraframe(void)
|
static void writeIntraframe(void)
|
||||||
{
|
{
|
||||||
blackboxValues_t *blackboxCurrent = blackboxHistory[0];
|
blackboxValues_t *blackboxCurrent = blackboxHistory[0];
|
||||||
|
|
@ -582,7 +600,7 @@ 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())
|
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER))
|
||||||
writeSignedVB(blackboxHistory[0]->servo[5] - 1500);
|
writeSignedVB(blackboxHistory[0]->servo[5] - 1500);
|
||||||
|
|
||||||
//Rotate our history buffers:
|
//Rotate our history buffers:
|
||||||
|
|
@ -647,7 +665,7 @@ 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())
|
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER))
|
||||||
writeSignedVB(blackboxCurrent->servo[5] - blackboxLast->servo[5]);
|
writeSignedVB(blackboxCurrent->servo[5] - blackboxLast->servo[5]);
|
||||||
|
|
||||||
//Rotate our history buffers
|
//Rotate our history buffers
|
||||||
|
|
@ -715,29 +733,26 @@ void startBlackbox(void)
|
||||||
configureBlackboxPort();
|
configureBlackboxPort();
|
||||||
|
|
||||||
if (!blackboxPort) {
|
if (!blackboxPort) {
|
||||||
blackboxState = BLACKBOX_STATE_DISABLED;
|
blackboxSetState(BLACKBOX_STATE_DISABLED);
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
startTime = millis();
|
|
||||||
headerXmitIndex = 0;
|
|
||||||
blackboxIteration = 0;
|
|
||||||
blackboxState = BLACKBOX_STATE_SEND_HEADER;
|
|
||||||
|
|
||||||
memset(&gpsHistory, 0, sizeof(gpsHistory));
|
memset(&gpsHistory, 0, sizeof(gpsHistory));
|
||||||
|
|
||||||
//No need to clear the content of blackboxHistoryRing since our first frame will be an intra which overwrites it
|
|
||||||
|
|
||||||
blackboxHistory[0] = &blackboxHistoryRing[0];
|
blackboxHistory[0] = &blackboxHistoryRing[0];
|
||||||
blackboxHistory[1] = &blackboxHistoryRing[1];
|
blackboxHistory[1] = &blackboxHistoryRing[1];
|
||||||
blackboxHistory[2] = &blackboxHistoryRing[2];
|
blackboxHistory[2] = &blackboxHistoryRing[2];
|
||||||
|
|
||||||
|
//No need to clear the content of blackboxHistoryRing since our first frame will be an intra which overwrites it
|
||||||
|
|
||||||
|
blackboxSetState(BLACKBOX_STATE_SEND_HEADER);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void finishBlackbox(void)
|
void finishBlackbox(void)
|
||||||
{
|
{
|
||||||
if (blackboxState != BLACKBOX_STATE_DISABLED && blackboxState != BLACKBOX_STATE_STOPPED) {
|
if (blackboxState != BLACKBOX_STATE_DISABLED && blackboxState != BLACKBOX_STATE_STOPPED) {
|
||||||
blackboxState = BLACKBOX_STATE_STOPPED;
|
blackboxSetState(BLACKBOX_STATE_STOPPED);
|
||||||
|
|
||||||
releaseBlackboxPort();
|
releaseBlackboxPort();
|
||||||
}
|
}
|
||||||
|
|
@ -803,14 +818,81 @@ static void loadBlackboxState(void)
|
||||||
blackboxCurrent->servo[5] = servo[5];
|
blackboxCurrent->servo[5] = servo[5];
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Transmit the header information for the given field definitions. Transmitted header lines look like:
|
||||||
|
*
|
||||||
|
* H Field I name:a,b,c
|
||||||
|
* H Field I predictor:0,1,2
|
||||||
|
*
|
||||||
|
* Provide an array 'conditions' of FlightLogFieldCondition enums if you want these conditions to decide whether a field
|
||||||
|
* should be included or not. Otherwise provide NULL for this parameter and NULL for secondCondition.
|
||||||
|
*
|
||||||
|
* Set headerXmitIndex to 0 and fieldXmitIndex to -1 before calling for the first time.
|
||||||
|
*
|
||||||
|
* secondFieldDefinition and secondCondition element pointers need to be provided in order to compute the stride of the
|
||||||
|
* fieldDefinition and secondCondition arrays.
|
||||||
|
*
|
||||||
|
* Returns true if there is still header left to transmit (so call again to continue transmission).
|
||||||
|
*/
|
||||||
|
static bool sendFieldDefinition(const char * const *headerNames, unsigned int headerCount, const void *fieldDefinitions,
|
||||||
|
const void *secondFieldDefinition, int fieldCount, const uint8_t *conditions, const uint8_t *secondCondition)
|
||||||
|
{
|
||||||
|
const blackboxFieldDefinition_t *def;
|
||||||
|
int charsWritten;
|
||||||
|
static bool needComma = false;
|
||||||
|
size_t definitionStride = (char*) secondFieldDefinition - (char*) fieldDefinitions;
|
||||||
|
size_t conditionsStride = (char*) secondCondition - (char*) conditions;
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Send information about the fields we're recording to the log. Once again, we're chunking up the data so we don't exceed our datarate.
|
||||||
|
*/
|
||||||
|
if (fieldXmitIndex == -1) {
|
||||||
|
if (headerXmitIndex >= headerCount)
|
||||||
|
return false; //Someone probably called us again after we had already completed transmission
|
||||||
|
|
||||||
|
charsWritten = blackboxPrint("H Field ");
|
||||||
|
charsWritten += blackboxPrint(headerNames[headerXmitIndex]);
|
||||||
|
charsWritten += blackboxPrint(":");
|
||||||
|
|
||||||
|
fieldXmitIndex++;
|
||||||
|
needComma = false;
|
||||||
|
} else
|
||||||
|
charsWritten = 0;
|
||||||
|
|
||||||
|
for (; fieldXmitIndex < fieldCount && charsWritten < SERIAL_CHUNK_SIZE; fieldXmitIndex++) {
|
||||||
|
def = (const blackboxFieldDefinition_t*) ((const char*)fieldDefinitions + definitionStride * fieldXmitIndex);
|
||||||
|
|
||||||
|
if (!conditions || testBlackboxCondition(conditions[conditionsStride * fieldXmitIndex])) {
|
||||||
|
if (needComma) {
|
||||||
|
blackboxWrite(',');
|
||||||
|
charsWritten++;
|
||||||
|
} else
|
||||||
|
needComma = true;
|
||||||
|
|
||||||
|
// The first header is a field name
|
||||||
|
if (headerXmitIndex == 0) {
|
||||||
|
charsWritten += blackboxPrint(def->name);
|
||||||
|
} else {
|
||||||
|
//The other headers are integers (format as single digit for now):
|
||||||
|
blackboxWrite(def->arr[headerXmitIndex - 1] + '0');
|
||||||
|
charsWritten++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Did we complete this line?
|
||||||
|
if (fieldXmitIndex == fieldCount) {
|
||||||
|
blackboxWrite('\n');
|
||||||
|
headerXmitIndex++;
|
||||||
|
fieldXmitIndex = -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
return headerXmitIndex < headerCount;
|
||||||
|
}
|
||||||
|
|
||||||
void handleBlackbox(void)
|
void handleBlackbox(void)
|
||||||
{
|
{
|
||||||
int i;
|
int i;
|
||||||
const char *additionalHeader;
|
|
||||||
const int SERIAL_CHUNK_SIZE = 16;
|
|
||||||
static int charXmitIndex = 0;
|
|
||||||
int motorsToRemove, endIndex;
|
|
||||||
int blackboxIntercycleIndex, blackboxIntracycleIndex;
|
|
||||||
|
|
||||||
union floatConvert_t {
|
union floatConvert_t {
|
||||||
float f;
|
float f;
|
||||||
|
|
@ -819,6 +901,8 @@ void handleBlackbox(void)
|
||||||
|
|
||||||
switch (blackboxState) {
|
switch (blackboxState) {
|
||||||
case BLACKBOX_STATE_SEND_HEADER:
|
case BLACKBOX_STATE_SEND_HEADER:
|
||||||
|
//On entry of this state, headerXmitIndex is 0 and startTime is intialised
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Once the UART has had time to init, transmit the header in chunks so we don't overflow our transmit
|
* Once the UART has had time to init, transmit the header in chunks so we don't overflow our transmit
|
||||||
* buffer.
|
* buffer.
|
||||||
|
|
@ -828,65 +912,36 @@ void handleBlackbox(void)
|
||||||
blackboxWrite(blackboxHeader[headerXmitIndex]);
|
blackboxWrite(blackboxHeader[headerXmitIndex]);
|
||||||
|
|
||||||
if (blackboxHeader[headerXmitIndex] == '\0') {
|
if (blackboxHeader[headerXmitIndex] == '\0') {
|
||||||
blackboxState = BLACKBOX_STATE_SEND_FIELDINFO;
|
blackboxSetState(BLACKBOX_STATE_SEND_FIELDINFO);
|
||||||
headerXmitIndex = 0;
|
|
||||||
charXmitIndex = 0;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
case BLACKBOX_STATE_SEND_FIELDINFO:
|
case BLACKBOX_STATE_SEND_FIELDINFO:
|
||||||
/*
|
//On entry of this state, headerXmitIndex is 0 and fieldXmitIndex is -1
|
||||||
* Send information about the fields we're recording to the log.
|
if (!sendFieldDefinition(blackboxMainHeaderNames, ARRAY_LENGTH(blackboxMainHeaderNames), blackboxMainFields, &blackboxMainFields[1],
|
||||||
*
|
ARRAY_LENGTH(blackboxMainFields), &blackboxMainFields[0].condition, &blackboxMainFields[1].condition)) {
|
||||||
* Once again, we're chunking up the data so we don't exceed our datarate. This time we're trimming the
|
if (feature(FEATURE_GPS))
|
||||||
* excess field defs off the end of the header for motors we don't have.
|
blackboxSetState(BLACKBOX_STATE_SEND_GPS_H_HEADERS);
|
||||||
*/
|
else
|
||||||
motorsToRemove = 8 - motorCount;
|
blackboxSetState(BLACKBOX_STATE_SEND_SYSINFO);
|
||||||
|
|
||||||
if (headerXmitIndex < sizeof(blackboxHeaderFields) / sizeof(blackboxHeaderFields[0])){
|
|
||||||
endIndex = strlen(blackboxHeaderFields[headerXmitIndex]) - (headerXmitIndex == 0 ? strlen(",motor[x]") : strlen(",x")) * motorsToRemove;
|
|
||||||
|
|
||||||
for (i = charXmitIndex; i < charXmitIndex + SERIAL_CHUNK_SIZE && i < endIndex; i++)
|
|
||||||
blackboxWrite(blackboxHeaderFields[headerXmitIndex][i]);
|
|
||||||
|
|
||||||
// Did we complete this line?
|
|
||||||
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');
|
|
||||||
headerXmitIndex++;
|
|
||||||
charXmitIndex = 0;
|
|
||||||
} else {
|
|
||||||
charXmitIndex = i;
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
//Completed sending field information
|
|
||||||
|
|
||||||
if (feature(FEATURE_GPS)) {
|
|
||||||
blackboxState = BLACKBOX_STATE_SEND_GPS_HEADERS;
|
|
||||||
} else {
|
|
||||||
blackboxState = BLACKBOX_STATE_SEND_SYSINFO;
|
|
||||||
}
|
|
||||||
headerXmitIndex = 0;
|
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
case BLACKBOX_STATE_SEND_GPS_HEADERS:
|
case BLACKBOX_STATE_SEND_GPS_H_HEADERS:
|
||||||
for (i = 0; i < SERIAL_CHUNK_SIZE && blackboxGpsHeader[headerXmitIndex] != '\0'; i++, headerXmitIndex++)
|
//On entry of this state, headerXmitIndex is 0 and fieldXmitIndex is -1
|
||||||
blackboxWrite(blackboxGpsHeader[headerXmitIndex]);
|
if (!sendFieldDefinition(blackboxGPSHHeaderNames, ARRAY_LENGTH(blackboxGPSHHeaderNames), blackboxGpsHFields, &blackboxGpsHFields[1],
|
||||||
|
ARRAY_LENGTH(blackboxGpsHFields), NULL, 0)) {
|
||||||
if (blackboxGpsHeader[headerXmitIndex] == '\0') {
|
blackboxSetState(BLACKBOX_STATE_SEND_GPS_G_HEADERS);
|
||||||
blackboxState = BLACKBOX_STATE_SEND_SYSINFO;
|
}
|
||||||
headerXmitIndex = 0;
|
break;
|
||||||
|
case BLACKBOX_STATE_SEND_GPS_G_HEADERS:
|
||||||
|
//On entry of this state, headerXmitIndex is 0 and fieldXmitIndex is -1
|
||||||
|
if (!sendFieldDefinition(blackboxGPSGHeaderNames, ARRAY_LENGTH(blackboxGPSGHeaderNames), blackboxGpsGFields, &blackboxGpsGFields[1],
|
||||||
|
ARRAY_LENGTH(blackboxGpsGFields), NULL, 0)) {
|
||||||
|
blackboxSetState(BLACKBOX_STATE_SEND_SYSINFO);
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
case BLACKBOX_STATE_SEND_SYSINFO:
|
case BLACKBOX_STATE_SEND_SYSINFO:
|
||||||
|
//On entry of this state, headerXmitIndex is 0
|
||||||
switch (headerXmitIndex) {
|
switch (headerXmitIndex) {
|
||||||
case 0:
|
case 0:
|
||||||
blackboxPrintf("H Firmware type:Cleanflight\n");
|
blackboxPrintf("H Firmware type:Cleanflight\n");
|
||||||
|
|
@ -926,22 +981,21 @@ void handleBlackbox(void)
|
||||||
// One more pause for good luck
|
// One more pause for good luck
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
blackboxState = BLACKBOX_STATE_RUNNING;
|
blackboxSetState(BLACKBOX_STATE_RUNNING);
|
||||||
}
|
}
|
||||||
|
|
||||||
headerXmitIndex++;
|
headerXmitIndex++;
|
||||||
break;
|
break;
|
||||||
case BLACKBOX_STATE_RUNNING:
|
case BLACKBOX_STATE_RUNNING:
|
||||||
// Write a keyframe every 32 frames so we can resynchronise upon missing frames
|
// On entry to this state, blackboxIteration, blackboxPFrameIndex and blackboxIFrameIndex are reset to 0
|
||||||
blackboxIntercycleIndex = blackboxIteration % BLACKBOX_I_INTERVAL;
|
|
||||||
blackboxIntracycleIndex = blackboxIteration / BLACKBOX_I_INTERVAL;
|
|
||||||
|
|
||||||
if (blackboxIntercycleIndex == 0) {
|
// Write a keyframe every BLACKBOX_I_INTERVAL frames so we can resynchronise upon missing frames
|
||||||
|
if (blackboxPFrameIndex == 0) {
|
||||||
// Copy current system values into the blackbox
|
// Copy current system values into the blackbox
|
||||||
loadBlackboxState();
|
loadBlackboxState();
|
||||||
writeIntraframe();
|
writeIntraframe();
|
||||||
} else {
|
} else {
|
||||||
if ((blackboxIntercycleIndex + masterConfig.blackbox_rate_num - 1) % masterConfig.blackbox_rate_denom < masterConfig.blackbox_rate_num) {
|
if ((blackboxPFrameIndex + masterConfig.blackbox_rate_num - 1) % masterConfig.blackbox_rate_denom < masterConfig.blackbox_rate_num) {
|
||||||
loadBlackboxState();
|
loadBlackboxState();
|
||||||
writeInterframe();
|
writeInterframe();
|
||||||
}
|
}
|
||||||
|
|
@ -955,7 +1009,7 @@ void handleBlackbox(void)
|
||||||
* still be interpreted correctly.
|
* still be interpreted correctly.
|
||||||
*/
|
*/
|
||||||
if (GPS_home[0] != gpsHistory.GPS_home[0] || GPS_home[1] != gpsHistory.GPS_home[1]
|
if (GPS_home[0] != gpsHistory.GPS_home[0] || GPS_home[1] != gpsHistory.GPS_home[1]
|
||||||
|| (blackboxIntercycleIndex == BLACKBOX_I_INTERVAL / 2 && blackboxIntracycleIndex % 128 == 0)) {
|
|| (blackboxPFrameIndex == BLACKBOX_I_INTERVAL / 2 && blackboxIFrameIndex % 128 == 0)) {
|
||||||
|
|
||||||
writeGPSHomeFrame();
|
writeGPSHomeFrame();
|
||||||
writeGPSFrame();
|
writeGPSFrame();
|
||||||
|
|
@ -968,6 +1022,12 @@ void handleBlackbox(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
blackboxIteration++;
|
blackboxIteration++;
|
||||||
|
blackboxPFrameIndex++;
|
||||||
|
|
||||||
|
if (blackboxPFrameIndex == BLACKBOX_I_INTERVAL) {
|
||||||
|
blackboxPFrameIndex = 0;
|
||||||
|
blackboxIFrameIndex++;
|
||||||
|
}
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
break;
|
break;
|
||||||
|
|
@ -985,7 +1045,7 @@ static bool canUseBlackboxWithCurrentConfiguration(void)
|
||||||
void initBlackbox(void)
|
void initBlackbox(void)
|
||||||
{
|
{
|
||||||
if (canUseBlackboxWithCurrentConfiguration())
|
if (canUseBlackboxWithCurrentConfiguration())
|
||||||
blackboxState = BLACKBOX_STATE_STOPPED;
|
blackboxSetState(BLACKBOX_STATE_STOPPED);
|
||||||
else
|
else
|
||||||
blackboxState = BLACKBOX_STATE_DISABLED;
|
blackboxSetState(BLACKBOX_STATE_DISABLED);
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -1,52 +1,75 @@
|
||||||
/*
|
/*
|
||||||
* This file is part of Cleanflight.
|
* This file is part of Cleanflight.
|
||||||
*
|
*
|
||||||
* Cleanflight is free software: you can redistribute it and/or modify
|
* Cleanflight is free software: you can redistribute it and/or modify
|
||||||
* it under the terms of the GNU General Public License as published by
|
* it under the terms of the GNU General Public License as published by
|
||||||
* the Free Software Foundation, either version 3 of the License, or
|
* the Free Software Foundation, either version 3 of the License, or
|
||||||
* (at your option) any later version.
|
* (at your option) any later version.
|
||||||
*
|
*
|
||||||
* Cleanflight is distributed in the hope that it will be useful,
|
* Cleanflight is distributed in the hope that it will be useful,
|
||||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
* GNU General Public License for more details.
|
* GNU General Public License for more details.
|
||||||
*
|
*
|
||||||
* You should have received a copy of the GNU General Public License
|
* You should have received a copy of the GNU General Public License
|
||||||
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
//No prediction:
|
typedef enum FlightLogFieldCondition {
|
||||||
#define FLIGHT_LOG_FIELD_PREDICTOR_0 0
|
FLIGHT_LOG_FIELD_CONDITION_ALWAYS = 0,
|
||||||
|
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1,
|
||||||
//Predict that the field is the same as last frame:
|
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_2,
|
||||||
#define FLIGHT_LOG_FIELD_PREDICTOR_PREVIOUS 1
|
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_3,
|
||||||
|
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_4,
|
||||||
//Predict that the slope between this field and the previous item is the same as that between the past two history items:
|
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_5,
|
||||||
#define FLIGHT_LOG_FIELD_PREDICTOR_STRAIGHT_LINE 2
|
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_6,
|
||||||
|
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_7,
|
||||||
//Predict that this field is the same as the average of the last two history items:
|
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_8,
|
||||||
#define FLIGHT_LOG_FIELD_PREDICTOR_AVERAGE_2 3
|
FLIGHT_LOG_FIELD_CONDITION_TRICOPTER,
|
||||||
|
FLIGHT_LOG_FIELD_CONDITION_NEVER = 255,
|
||||||
//Predict that this field is minthrottle
|
} FlightLogFieldCondition;
|
||||||
#define FLIGHT_LOG_FIELD_PREDICTOR_MINTHROTTLE 4
|
|
||||||
|
typedef enum FlightLogFieldPredictor {
|
||||||
//Predict that this field is the same as motor 0
|
//No prediction:
|
||||||
#define FLIGHT_LOG_FIELD_PREDICTOR_MOTOR_0 5
|
FLIGHT_LOG_FIELD_PREDICTOR_0 = 0,
|
||||||
|
|
||||||
//This field always increments
|
//Predict that the field is the same as last frame:
|
||||||
#define FLIGHT_LOG_FIELD_PREDICTOR_INC 6
|
FLIGHT_LOG_FIELD_PREDICTOR_PREVIOUS = 1,
|
||||||
|
|
||||||
//Predict this GPS co-ordinate is the GPS home co-ordinate (or no prediction if that coordinate is not set)
|
//Predict that the slope between this field and the previous item is the same as that between the past two history items:
|
||||||
#define FLIGHT_LOG_FIELD_PREDICTOR_HOME_COORD 7
|
FLIGHT_LOG_FIELD_PREDICTOR_STRAIGHT_LINE = 2,
|
||||||
|
|
||||||
//Predict 1500
|
//Predict that this field is the same as the average of the last two history items:
|
||||||
#define FLIGHT_LOG_FIELD_PREDICTOR_1500 8
|
FLIGHT_LOG_FIELD_PREDICTOR_AVERAGE_2 = 3,
|
||||||
|
|
||||||
|
//Predict that this field is minthrottle
|
||||||
#define FLIGHT_LOG_FIELD_ENCODING_SIGNED_VB 0
|
FLIGHT_LOG_FIELD_PREDICTOR_MINTHROTTLE = 4,
|
||||||
#define FLIGHT_LOG_FIELD_ENCODING_UNSIGNED_VB 1
|
|
||||||
#define FLIGHT_LOG_FIELD_ENCODING_TAG2_3S32 7
|
//Predict that this field is the same as motor 0
|
||||||
#define FLIGHT_LOG_FIELD_ENCODING_TAG8_4S16 8
|
FLIGHT_LOG_FIELD_PREDICTOR_MOTOR_0 = 5,
|
||||||
#define FLIGHT_LOG_FIELD_ENCODING_NULL 9
|
|
||||||
|
//This field always increments
|
||||||
|
FLIGHT_LOG_FIELD_PREDICTOR_INC = 6,
|
||||||
|
|
||||||
|
//Predict this GPS co-ordinate is the GPS home co-ordinate (or no prediction if that coordinate is not set)
|
||||||
|
FLIGHT_LOG_FIELD_PREDICTOR_HOME_COORD = 7,
|
||||||
|
|
||||||
|
//Predict 1500
|
||||||
|
FLIGHT_LOG_FIELD_PREDICTOR_1500 = 8
|
||||||
|
} FlightLogFieldPredictor;
|
||||||
|
|
||||||
|
typedef enum FlightLogFieldEncoding {
|
||||||
|
FLIGHT_LOG_FIELD_ENCODING_SIGNED_VB = 0,
|
||||||
|
FLIGHT_LOG_FIELD_ENCODING_UNSIGNED_VB = 1,
|
||||||
|
FLIGHT_LOG_FIELD_ENCODING_TAG2_3S32 = 7,
|
||||||
|
FLIGHT_LOG_FIELD_ENCODING_TAG8_4S16 = 8,
|
||||||
|
FLIGHT_LOG_FIELD_ENCODING_NULL = 9
|
||||||
|
} FlightLogFieldEncoding;
|
||||||
|
|
||||||
|
typedef enum FlightLogFieldSign {
|
||||||
|
FLIGHT_LOG_FIELD_UNSIGNED = 0,
|
||||||
|
FLIGHT_LOG_FIELD_SIGNED = 1
|
||||||
|
} FlightLogFieldSign;
|
||||||
|
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue