diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index fb993d246..c98390f1c 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1164,8 +1164,8 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data) } case FLIGHT_LOG_EVENT_GTUNE_RESULT: blackboxWrite(data->gtuneCycleResult.gtuneAxis); - blackboxWrite(data->gtuneCycleResult.gtuneGyroAVG); - blackboxWrite(data->gtuneCycleResult.gtuneNewP); + blackboxWriteSignedVB(data->gtuneCycleResult.gtuneGyroAVG); + blackboxWriteS16(data->gtuneCycleResult.gtuneNewP); break; case FLIGHT_LOG_EVENT_LOGGING_RESUME: blackboxWriteUnsignedVB(data->loggingResume.logIteration); diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h index f74e0bb52..fcd087584 100644 --- a/src/main/blackbox/blackbox_fielddefs.h +++ b/src/main/blackbox/blackbox_fielddefs.h @@ -157,8 +157,8 @@ typedef struct flightLogEvent_loggingResume_t { ======= typedef struct flightLogEvent_gtuneCycleResult_t { uint8_t gtuneAxis; - uint32_t gtuneGyroAVG; - uint8_t gtuneNewP; + int32_t gtuneGyroAVG; + int16_t gtuneNewP; } flightLogEvent_gtuneCycleResult_t; >>>>>>> 6f60c52 Add BlackBox recording for G-Tune diff --git a/src/main/flight/gtune.c b/src/main/flight/gtune.c index d228c4aa8..bb3af9b3d 100644 --- a/src/main/flight/gtune.c +++ b/src/main/flight/gtune.c @@ -159,7 +159,8 @@ void calculate_Gtune(uint8_t axis) eventData.gtuneAxis = axis; eventData.gtuneGyroAVG = AvgGyro[axis]; - eventData.gtuneNewP = newP; + if (floatPID) eventData.gtuneNewP = newP / 10; + else eventData.gtuneNewP = newP; blackboxLogEvent(FLIGHT_LOG_EVENT_GTUNE_RESULT, (flightLogEventData_t*)&eventData); } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index bb5026e71..b7f97c85e 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -456,11 +456,6 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control axisPID[FD_YAW] = PTerm + ITerm; - if (motorCount >= 4) { // prevent "yaw jump" during yaw correction - int16_t limit = ABS(rcCommand[FD_YAW]) + 100; - axisPID[FD_YAW] = (float)constrain((int32_t)axisPID[FD_YAW], -limit, +limit); - } - #ifdef GTUNE if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { calculate_Gtune(FD_YAW); @@ -594,11 +589,6 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con axisPID[FD_YAW] = PTerm + ITerm; - if (motorCount >= 4) { // prevent "yaw jump" during yaw correction - int16_t limit = ABS(rcCommand[FD_YAW]) + 100; - axisPID[FD_YAW] = (float)constrain((int32_t)axisPID[FD_YAW], -limit, +limit); - } - #ifdef GTUNE if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { calculate_Gtune(FD_YAW); @@ -749,11 +739,6 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) axisPID[FD_YAW] = PTerm + ITerm; axisPID[FD_YAW] = lrintf(axisPID[FD_YAW]); // Round up result. - if (motorCount >= 4) { // prevent "yaw jump" during yaw correction - int16_t limit = ABS(rcCommand[FD_YAW]) + 100; - axisPID[FD_YAW] = (float)constrain((int32_t)axisPID[FD_YAW], -limit, +limit); - } - #ifdef GTUNE if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { calculate_Gtune(FD_YAW);