From 00bff6485ba9aa4e3e4348364df6b5c9a0b9fe14 Mon Sep 17 00:00:00 2001 From: Alexander Fedorov Date: Tue, 23 Jun 2015 15:20:22 +0200 Subject: [PATCH] Add logging of the inflight adjustment events to the blackbox --- src/main/blackbox/blackbox.c | 9 ++ src/main/blackbox/blackbox_fielddefs.h | 10 ++ src/main/blackbox/blackbox_io.c | 15 +++ src/main/blackbox/blackbox_io.h | 2 + src/main/io/rc_controls.c | 153 ++++++++++++++++--------- 5 files changed, 137 insertions(+), 52 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 5b2dc2cef..24a29d490 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1012,6 +1012,15 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data) blackboxWriteS16(data->autotuneTargets.firstPeakAngle); blackboxWriteS16(data->autotuneTargets.secondPeakAngle); break; + case FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT: + if (data->inflightAdjustment.floatFlag) { + blackboxWrite(data->inflightAdjustment.adjustmentFunction + FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT_FUNCTION_FLOAT_VALUE_FLAG); + blackboxWriteFloat(data->inflightAdjustment.newFloatValue); + } else { + blackboxWrite(data->inflightAdjustment.adjustmentFunction); + blackboxWriteSignedVB(data->inflightAdjustment.newValue); + } + break; case FLIGHT_LOG_EVENT_LOG_END: blackboxPrint("End of log"); blackboxWrite(0); diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h index 9b1ad46eb..315183e02 100644 --- a/src/main/blackbox/blackbox_fielddefs.h +++ b/src/main/blackbox/blackbox_fielddefs.h @@ -103,6 +103,7 @@ typedef enum FlightLogEvent { FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START = 10, FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT = 11, FLIGHT_LOG_EVENT_AUTOTUNE_TARGETS = 12, + FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT = 13, FLIGHT_LOG_EVENT_LOG_END = 255 } FlightLogEvent; @@ -121,6 +122,7 @@ typedef struct flightLogEvent_autotuneCycleStart_t { #define FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_OVERSHOT 1 #define FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_TIMEDOUT 2 +#define FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT_FUNCTION_FLOAT_VALUE_FLAG 128 typedef struct flightLogEvent_autotuneCycleResult_t { uint8_t flags; @@ -135,12 +137,20 @@ typedef struct flightLogEvent_autotuneTargets_t { uint16_t firstPeakAngle, secondPeakAngle; } flightLogEvent_autotuneTargets_t; +typedef struct flightLogEvent_inflightAdjustment_t { + uint8_t adjustmentFunction; + bool floatFlag; + int32_t newValue; + float newFloatValue; +} flightLogEvent_inflightAdjustment_t; + typedef union flightLogEventData_t { flightLogEvent_syncBeep_t syncBeep; flightLogEvent_autotuneCycleStart_t autotuneCycleStart; flightLogEvent_autotuneCycleResult_t autotuneCycleResult; flightLogEvent_autotuneTargets_t autotuneTargets; + flightLogEvent_inflightAdjustment_t inflightAdjustment; } flightLogEventData_t; typedef struct flightLogEvent_t diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index ffc1df7f6..fb0df826f 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -407,6 +407,21 @@ void blackboxWriteTag8_8SVB(int32_t *values, int valueCount) } } +/** Write unsigned integer **/ +void blackboxWriteU32(int32_t value) +{ + blackboxWrite(value & 0xFF); + blackboxWrite((value >> 8) & 0xFF); + blackboxWrite((value >> 16) & 0xFF); + blackboxWrite((value >> 24) & 0xFF); +} + +/** Write float value in the integer form **/ +void blackboxWriteFloat(float value) +{ + blackboxWriteU32(castFloatBytesToInt(value)); +} + /** * If there is data waiting to be written to the blackbox device, attempt to write (a portion of) that now. * diff --git a/src/main/blackbox/blackbox_io.h b/src/main/blackbox/blackbox_io.h index 50166dc55..89a1ad253 100644 --- a/src/main/blackbox/blackbox_io.h +++ b/src/main/blackbox/blackbox_io.h @@ -45,6 +45,8 @@ void blackboxWriteS16(int16_t value); void blackboxWriteTag2_3S32(int32_t *values); void blackboxWriteTag8_4S16(int32_t *values); void blackboxWriteTag8_8SVB(int32_t *values, int valueCount); +void blackboxWriteU32(int32_t value); +void blackboxWriteFloat(float value); bool blackboxDeviceFlush(void); bool blackboxDeviceOpen(void); diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 4ac3b7f16..0801842f6 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -53,6 +53,8 @@ #include "flight/navigation.h" #include "flight/failsafe.h" +#include "blackbox/blackbox.h" + #include "mw.h" @@ -67,6 +69,30 @@ int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+ uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e +void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue) { +#ifdef BLACKBOX + if (feature(FEATURE_BLACKBOX)) { + flightLogEvent_inflightAdjustment_t eventData; + eventData.adjustmentFunction = adjustmentFunction; + eventData.newValue = newValue; + eventData.floatFlag = false; + blackboxLogEvent(FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT, (flightLogEventData_t*)&eventData); + } +#endif +} + +void blackboxLogInflightAdjustmentEventFloat(adjustmentFunction_e adjustmentFunction, float newFloatValue) { +#ifdef BLACKBOX + if (feature(FEATURE_BLACKBOX)) { + flightLogEvent_inflightAdjustment_t eventData; + eventData.adjustmentFunction = adjustmentFunction; + eventData.newFloatValue = newFloatValue; + eventData.floatFlag = true; + blackboxLogEvent(FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT, (flightLogEventData_t*)&eventData); + } +#endif +} + bool isUsingSticksForArming(void) { return isUsingSticksToArm; @@ -88,8 +114,6 @@ throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband return THROTTLE_HIGH; } - - void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool retarded_arm, bool disarm_kill_switch) { static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors @@ -443,44 +467,52 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm } switch(adjustmentFunction) { case ADJUSTMENT_RC_RATE: - newValue = (int)controlRateConfig->rcRate8 + delta; - controlRateConfig->rcRate8 = constrain(newValue, 0, 250); // FIXME magic numbers repeated in serial_cli.c + newValue = constrain((int)controlRateConfig->rcRate8 + delta, 0, 250); // FIXME magic numbers repeated in serial_cli.c + controlRateConfig->rcRate8 = newValue; generatePitchRollCurve(controlRateConfig); + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE, newValue); break; case ADJUSTMENT_RC_EXPO: - newValue = (int)controlRateConfig->rcExpo8 + delta; - controlRateConfig->rcExpo8 = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newValue = constrain((int)controlRateConfig->rcExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c + controlRateConfig->rcExpo8 = newValue; generatePitchRollCurve(controlRateConfig); - break; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_EXPO, newValue); + break; case ADJUSTMENT_THROTTLE_EXPO: - newValue = (int)controlRateConfig->thrExpo8 + delta; - controlRateConfig->thrExpo8 = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newValue = constrain((int)controlRateConfig->thrExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c + controlRateConfig->thrExpo8 = newValue; generateThrottleCurve(controlRateConfig, escAndServoConfig); - break; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO, newValue); + break; case ADJUSTMENT_PITCH_ROLL_RATE: case ADJUSTMENT_PITCH_RATE: - newValue = (int)controlRateConfig->rates[FD_PITCH] + delta; - controlRateConfig->rates[FD_PITCH] = constrain(newValue, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); + newValue = constrain((int)controlRateConfig->rates[FD_PITCH] + delta, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); + controlRateConfig->rates[FD_PITCH] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RATE, newValue); if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) { break; } // follow though for combined ADJUSTMENT_PITCH_ROLL_RATE case ADJUSTMENT_ROLL_RATE: - newValue = (int)controlRateConfig->rates[FD_ROLL] + delta; - controlRateConfig->rates[FD_ROLL] = constrain(newValue, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); + newValue = constrain((int)controlRateConfig->rates[FD_ROLL] + delta, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); + controlRateConfig->rates[FD_ROLL] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RATE, newValue); break; case ADJUSTMENT_YAW_RATE: - newValue = (int)controlRateConfig->rates[FD_YAW] + delta; - controlRateConfig->rates[FD_YAW] = constrain(newValue, 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX); + newValue = constrain((int)controlRateConfig->rates[FD_YAW] + delta, 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX); + controlRateConfig->rates[FD_YAW] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_RATE, newValue); break; case ADJUSTMENT_PITCH_ROLL_P: case ADJUSTMENT_PITCH_P: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = pidProfile->P_f[PIDPITCH] + (float)(delta / 10.0f); - pidProfile->P_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = constrainf(pidProfile->P_f[PIDPITCH] + (float)(delta / 10.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c + pidProfile->P_f[PIDPITCH] = newFloatValue; + blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_PITCH_P, newFloatValue); } else { - newValue = (int)pidProfile->P8[PIDPITCH] + delta; - pidProfile->P8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + newValue = constrain((int)pidProfile->P8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->P8[PIDPITCH] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_P, newValue); } if (adjustmentFunction == ADJUSTMENT_PITCH_P) { break; @@ -488,21 +520,25 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm // follow though for combined ADJUSTMENT_PITCH_ROLL_P case ADJUSTMENT_ROLL_P: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = pidProfile->P_f[PIDROLL] + (float)(delta / 10.0f); - pidProfile->P_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = constrainf(pidProfile->P_f[PIDROLL] + (float)(delta / 10.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c + pidProfile->P_f[PIDROLL] = newFloatValue; + blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_ROLL_P, newFloatValue); } else { - newValue = (int)pidProfile->P8[PIDROLL] + delta; - pidProfile->P8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + newValue = constrain((int)pidProfile->P8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->P8[PIDROLL] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_P, newValue); } break; case ADJUSTMENT_PITCH_ROLL_I: case ADJUSTMENT_PITCH_I: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = pidProfile->I_f[PIDPITCH] + (float)(delta / 100.0f); - pidProfile->I_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = constrainf(pidProfile->I_f[PIDPITCH] + (float)(delta / 100.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c + pidProfile->I_f[PIDPITCH] = newFloatValue; + blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_PITCH_I, newFloatValue); } else { - newValue = (int)pidProfile->I8[PIDPITCH] + delta; - pidProfile->I8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + newValue = constrain((int)pidProfile->I8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->I8[PIDPITCH] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_I, newValue); } if (adjustmentFunction == ADJUSTMENT_PITCH_I) { break; @@ -510,21 +546,25 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm // follow though for combined ADJUSTMENT_PITCH_ROLL_I case ADJUSTMENT_ROLL_I: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = pidProfile->I_f[PIDROLL] + (float)(delta / 100.0f); - pidProfile->I_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = constrainf(pidProfile->I_f[PIDROLL] + (float)(delta / 100.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c + pidProfile->I_f[PIDROLL] = newFloatValue; + blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_ROLL_I, newFloatValue); } else { - newValue = (int)pidProfile->I8[PIDROLL] + delta; - pidProfile->I8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + newValue = constrain((int)pidProfile->I8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->I8[PIDROLL] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_I, newValue); } break; case ADJUSTMENT_PITCH_ROLL_D: case ADJUSTMENT_PITCH_D: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = pidProfile->D_f[PIDPITCH] + (float)(delta / 1000.0f); - pidProfile->D_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = constrainf(pidProfile->D_f[PIDPITCH] + (float)(delta / 1000.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c + pidProfile->D_f[PIDPITCH] = newFloatValue; + blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_PITCH_D, newFloatValue); } else { - newValue = (int)pidProfile->D8[PIDPITCH] + delta; - pidProfile->D8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + newValue = constrain((int)pidProfile->D8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->D8[PIDPITCH] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_D, newValue); } if (adjustmentFunction == ADJUSTMENT_PITCH_D) { break; @@ -532,38 +572,46 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm // follow though for combined ADJUSTMENT_PITCH_ROLL_D case ADJUSTMENT_ROLL_D: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = pidProfile->D_f[PIDROLL] + (float)(delta / 1000.0f); - pidProfile->D_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = constrainf(pidProfile->D_f[PIDROLL] + (float)(delta / 1000.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c + pidProfile->D_f[PIDROLL] = newFloatValue; + blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_ROLL_D, newFloatValue); } else { - newValue = (int)pidProfile->D8[PIDROLL] + delta; - pidProfile->D8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + newValue = constrain((int)pidProfile->D8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->D8[PIDROLL] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue); } break; case ADJUSTMENT_YAW_P: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = pidProfile->P_f[PIDYAW] + (float)(delta / 10.0f); - pidProfile->P_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = constrainf(pidProfile->P_f[PIDYAW] + (float)(delta / 10.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c + pidProfile->P_f[PIDYAW] = newFloatValue; + blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_YAW_P, newFloatValue); } else { - newValue = (int)pidProfile->P8[PIDYAW] + delta; - pidProfile->P8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + newValue = constrain((int)pidProfile->P8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->P8[PIDYAW] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_P, newValue); } break; case ADJUSTMENT_YAW_I: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = pidProfile->I_f[PIDYAW] + (float)(delta / 100.0f); - pidProfile->I_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = constrainf(pidProfile->I_f[PIDYAW] + (float)(delta / 100.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c + pidProfile->I_f[PIDYAW] = newFloatValue; + blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_YAW_I, newFloatValue); } else { - newValue = (int)pidProfile->I8[PIDYAW] + delta; - pidProfile->I8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + newValue = constrain((int)pidProfile->I8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->I8[PIDYAW] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_I, newValue); } break; case ADJUSTMENT_YAW_D: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = pidProfile->D_f[PIDYAW] + (float)(delta / 1000.0f); - pidProfile->D_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = constrainf(pidProfile->D_f[PIDYAW] + (float)(delta / 1000.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c + pidProfile->D_f[PIDYAW] = newFloatValue; + blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_YAW_D, newFloatValue); } else { - newValue = (int)pidProfile->D8[PIDYAW] + delta; - pidProfile->D8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + newValue = constrain((int)pidProfile->D8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->D8[PIDYAW] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue); } break; default: @@ -581,6 +629,7 @@ void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position) case ADJUSTMENT_RATE_PROFILE: if (getCurrentControlRateProfile() != position) { changeControlRateProfile(position); + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RATE_PROFILE, position); applied = true; } break;