rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #1046 from fedorcomander/blackbox_inflight_adjustments 

Add logging of the inflight adjustment events to the blackbox
This commit is contained in:
Dominic Clifton 2015-07-13 18:05:08 +01:00
parent 9d60642cf2 00bff6485b
commit 57e1c70548
5 changed files with 137 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/main/blackbox/blackbox.c
View File

@ -1132,6 +1132,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);

10
src/main/blackbox/blackbox_fielddefs.h
View File

@ -106,6 +106,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;
@ -124,6 +125,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;
@ -138,12 +140,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

15
src/main/blackbox/blackbox_io.c
View File

@ -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.
*

2
src/main/blackbox/blackbox_io.h
View File

@ -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);

153
src/main/io/rc_controls.c
View File

@ -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;