Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 12:55:19 +03:00
Code Issues Wiki Activity

Add setpoint to blackbox logging

Browse source 
Currently only rcCommand values are included in the log data and the configurator calculates the actual setpoint values based on rates values added to the blackbox header. The problem with this is that the rates information is only written at arming so if the rates change during the log (rateprofile change, in-flight adjustments, etc.) then the calculated setpoints will be incorrect. There's no way to tell from the log that this happened. This often causes confusion because it will suddenly make it appear in the log that the PID controller is not acheiving the requested rates when it's just a presentation error. Also the rates will be incorrectly calculated when the user selects Raceflight style rates as the rates type is not supplied in the log header (and the viewer doesn't have the forumla for them anyway).

This change adds the actual setpoint values for each axis as used by the PID controller, removing the necessity for the viewer to perform any calculations. In addition to showing any rate changes, it will also show any cases where other flight features have modified the setpoints from the user's input. These were invisible previously (examples include level modes, Acro Trainer, GPS Rescue, yaw spin recovery, etc.).

Also the throttle value used in the mixer is included in the throttle axis. This allow visualization of things that affect the commanded throttle like throttle boost, throttle limit, GPS Rescue, angle level strength, etc.
This commit is contained in:
Bruce Luckcuck 2019-01-22 16:21:52 -05:00
parent a4ce8b5600
commit 99685c21c6
5 changed files with 38 additions and 1 deletions
Download patch file Download diff file Expand all files Collapse all files

21
src/main/blackbox/blackbox.c
View file

@ -192,6 +192,12 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
/* 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)},
// setpoint - define 4 fields like rcCommand to use the same encoding. setpoint[4] contains the mixer throttle
{"setpoint", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
{"setpoint", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
{"setpoint", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
{"setpoint", 3, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
{"vbatLatest", -1, UNSIGNED, .Ipredict = PREDICT(VBATREF), .Iencode = ENCODING(NEG_14BIT), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_VBAT},
{"amperageLatest",-1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_AMPERAGE_ADC},
@ -291,6 +297,7 @@ typedef struct blackboxMainState_s {
int32_t axisPID_F[XYZ_AXIS_COUNT];
int16_t rcCommand[4];
int16_t setpoint[4];
int16_t gyroADC[XYZ_AXIS_COUNT];
int16_t accADC[XYZ_AXIS_COUNT];
int16_t debug[DEBUG16_VALUE_COUNT];
@ -546,6 +553,9 @@ static void writeIntraframe(void)
*/
blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[THROTTLE] - motorConfig()->minthrottle);
// Write setpoint roll, pitch, yaw, and throttle
blackboxWriteSigned16VBArray(blackboxCurrent->setpoint, 4);
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
/*
* Our voltage is expected to decrease over the course of the flight, so store our difference from
@ -648,6 +658,8 @@ static void writeInterframe(void)
blackboxWriteSignedVB((int32_t) (blackboxHistory[0]->time - 2 * blackboxHistory[1]->time + blackboxHistory[2]->time));
int32_t deltas[8];
int32_t setpointDeltas[4];
arraySubInt32(deltas, blackboxCurrent->axisPID_P, blackboxLast->axisPID_P, XYZ_AXIS_COUNT);
blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT);
@ -677,9 +689,11 @@ static void writeInterframe(void)
*/
for (int x = 0; x < 4; x++) {
deltas[x] = blackboxCurrent->rcCommand[x] - blackboxLast->rcCommand[x];
setpointDeltas[x] = blackboxCurrent->setpoint[x] - blackboxLast->setpoint[x];
}
blackboxWriteTag8_4S16(deltas);
blackboxWriteTag8_4S16(setpointDeltas);
//Check for sensors that are updated periodically (so deltas are normally zero)
int optionalFieldCount = 0;
@ -1013,6 +1027,13 @@ static void loadMainState(timeUs_t currentTimeUs)
blackboxCurrent->rcCommand[i] = rcCommand[i];
}
// log the currentPidSetpoint values applied to the PID controller
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
blackboxCurrent->setpoint[i] = lrintf(pidGetPreviousSetpoint(i));
}
// log the final throttle value used in the mixer
blackboxCurrent->setpoint[3] = lrintf(mixerGetLoggingThrottle() * 1000);
for (int i = 0; i < DEBUG16_VALUE_COUNT; i++) {
blackboxCurrent->debug[i] = debug[i];
}

7
src/main/flight/mixer.c
View file

@ -539,6 +539,7 @@ void stopPwmAllMotors(void)
}
static FAST_RAM_ZERO_INIT float throttle = 0;
static FAST_RAM_ZERO_INIT float loggingThrottle = 0;
static FAST_RAM_ZERO_INIT float motorOutputMin;
static FAST_RAM_ZERO_INIT float motorRangeMin;
static FAST_RAM_ZERO_INIT float motorRangeMax;
@ -915,6 +916,7 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa
}
#endif
loggingThrottle = throttle;
motorMixRange = motorMixMax - motorMixMin;
if (motorMixRange > 1.0f) {
for (int i = 0; i < motorCount; i++) {
@ -1007,3 +1009,8 @@ void mixerSetThrottleAngleCorrection(int correctionValue)
{
throttleAngleCorrection = correctionValue;
}
float mixerGetLoggingThrottle(void)
{
return loggingThrottle;
}

1
src/main/flight/mixer.h
View file

@ -131,4 +131,5 @@ uint16_t convertMotorToExternal(float motorValue);
bool mixerIsTricopter(void);
void mixerSetThrottleAngleCorrection(int correctionValue);
float mixerGetLoggingThrottle(void);

8
src/main/flight/pid.c
View file

@ -242,6 +242,8 @@ typedef union dtermLowpass_u {
biquadFilter_t biquadFilter;
} dtermLowpass_t;
static FAST_RAM_ZERO_INIT float previousPidSetpoint[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermNotchApplyFn;
static FAST_RAM_ZERO_INIT biquadFilter_t dtermNotch[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpassApplyFn;
@ -1151,7 +1153,6 @@ static float applyLaunchControl(int axis, const rollAndPitchTrims_t *angleTrim)
void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs)
{
static float previousGyroRateDterm[XYZ_AXIS_COUNT];
static float previousPidSetpoint[XYZ_AXIS_COUNT];
static timeUs_t levelModeStartTimeUs = 0;
static bool gpsRescuePreviousState = false;
@ -1458,3 +1459,8 @@ void pidSetItermReset(bool enabled)
{
zeroThrottleItermReset = enabled;
}
float pidGetPreviousSetpoint(int axis)
{
return previousPidSetpoint[axis];
}

2
src/main/flight/pid.h
View file

@ -233,3 +233,5 @@ float calcHorizonLevelStrength(void);
#endif
void dynLpfDTermUpdate(float throttle);
void pidSetItermReset(bool enabled);
float pidGetPreviousSetpoint(int axis);