diff --git a/lib/Espfc/src/Blackbox.h b/lib/Espfc/src/Blackbox.h index 3126b0fe..3afbdee1 100644 --- a/lib/Espfc/src/Blackbox.h +++ b/lib/Espfc/src/Blackbox.h @@ -4,6 +4,7 @@ #include "Model.h" #include "Hardware.h" #include "EscDriver.h" +#include "Math/Utils.h" extern "C" { #include "blackbox/blackbox.h" @@ -54,6 +55,11 @@ bool rxIsReceivingSignal(void) return ((*_model_ptr).state.inputLinkValid); } +bool isRssiConfigured(void) +{ + return false; +} + static uint32_t activeFeaturesLatch = 0; static uint32_t enabledSensors = 0; @@ -67,6 +73,15 @@ bool sensors(uint32_t mask) return enabledSensors & mask; } +float pidGetPreviousSetpoint(int axis) +{ + return Espfc::Math::toDeg(_model_ptr->state.desiredRate[axis]); +} + +float mixerGetThrottle(void) +{ + return (_model_ptr->state.output[Espfc::AXIS_THRUST] + 1.0f) * 0.5f; +} namespace Espfc { @@ -95,11 +110,13 @@ class Blackbox rp->rcRates[i] = _model.config.input.rate[i]; rp->rcExpo[i] = _model.config.input.expo[i]; rp->rates[i] = _model.config.input.superRate[i]; + rp->rate_limit[i] = 1998; } rp->thrMid8 = 50; rp->thrExpo8 = 0; rp->dynThrPID = _model.config.tpaScale; rp->tpa_breakpoint = _model.config.tpaBreakpoint; + rp->rates_type = 0; // betaflight pidProfile_s * cp = currentPidProfile = &_pidProfile; for(size_t i = 0; i < PID_ITEM_COUNT; i++) @@ -107,16 +124,24 @@ class Blackbox cp->pid[i].P = _model.config.pid[i].P; cp->pid[i].I = _model.config.pid[i].I; cp->pid[i].D = _model.config.pid[i].D; + cp->pid[i].F = _model.config.pid[i].F; } cp->pidAtMinThrottle = 1; cp->dterm_filter_type = _model.config.dtermFilter.type; cp->dterm_lowpass_hz = _model.config.dtermFilter.freq; cp->dterm_lowpass2_hz = _model.config.dtermFilter2.freq; + cp->dterm_filter2_type = _model.config.dtermFilter2.type; cp->dterm_notch_hz = _model.config.dtermNotchFilter.freq; cp->dterm_notch_cutoff = _model.config.dtermNotchFilter.cutoff; cp->yaw_lowpass_hz = _model.config.yawFilter.freq; cp->itermWindupPointPercent = _model.config.itermWindupPointPercent; cp->antiGravityMode = 0; + cp->pidSumLimit = 500; + cp->pidSumLimitYaw = 500; + cp->ff_boost = 0; + cp->dyn_lpf_dterm_min_hz = _model.config.dtermDynLpfFilter.cutoff; + cp->dyn_lpf_dterm_max_hz = _model.config.dtermDynLpfFilter.freq; + cp->feedForwardTransition = 0; rcControlsConfigMutable()->deadband = _model.config.input.deadband; rcControlsConfigMutable()->yaw_deadband = _model.config.input.deadband; @@ -130,6 +155,8 @@ class Blackbox gyroConfigMutable()->gyro_soft_notch_hz_1 = _model.config.gyroNotch1Filter.freq; gyroConfigMutable()->gyro_soft_notch_cutoff_2 = _model.config.gyroNotch2Filter.cutoff; gyroConfigMutable()->gyro_soft_notch_hz_2 = _model.config.gyroNotch2Filter.freq; + gyroConfigMutable()->dyn_lpf_gyro_min_hz = _model.config.gyroDynLpfFilter.cutoff; + gyroConfigMutable()->dyn_lpf_gyro_max_hz = _model.config.gyroDynLpfFilter.freq; accelerometerConfigMutable()->acc_lpf_hz = _model.config.accelFilter.freq; accelerometerConfigMutable()->acc_hardware = _model.config.accelDev; @@ -161,7 +188,9 @@ class Blackbox pidConfigMutable()->pid_process_denom = _model.config.loopSync; targetPidLooptime = _model.state.loopTimer.interval; - gyro.targetLooptime = _model.state.gyroTimer.interval; + //gyro.targetLooptime = _model.state.gyroTimer.interval; + gyro.sampleLooptime = _model.state.gyroTimer.interval; + activePidLoopDenom = _model.config.loopSync; featureConfigMutable()->enabledFeatures = _model.config.featureMask; @@ -195,21 +224,31 @@ class Blackbox for(size_t i = 0; i < 3; i++) { gyro.gyroADCf[i] = degrees(_model.state.gyro[i]); - acc.accADC[i] = _model.state.accel[i] * ACCEL_G_INV * acc.dev.acc_1G; pidData[i].P = _model.state.innerPid[i].pTerm * 1000.f; pidData[i].I = _model.state.innerPid[i].iTerm * 1000.f; pidData[i].D = _model.state.innerPid[i].dTerm * 1000.f; pidData[i].F = _model.state.innerPid[i].fTerm * 1000.f; rcCommand[i] = _model.state.input[i] * (i == AXIS_YAW ? -500.f : 500.f); + if(_model.accelActive()) { + acc.accADC[i] = _model.state.accel[i] * ACCEL_G_INV * acc.dev.acc_1G; + } + if(_model.magActive()) { + mag.magADC[i] = _model.state.mag[i]; + } + if(_model.baroActive()) { + baro.BaroAlt = lrintf(_model.state.baroAltitude * 100.f); // cm + } } rcCommand[AXIS_THRUST] = _model.state.input[AXIS_THRUST] * 500.f + 1500.f; for(size_t i = 0; i < 4; i++) { - motor[i] = constrain(_model.state.outputUs[i], 1000, 2000); + motor[i] = Math::clamp(_model.state.outputUs[i], (int16_t)1000, (int16_t)2000); if(_model.state.digitalOutput) { motor[i] = PWM_TO_DSHOT(motor[i]); } - debug[i] = _model.state.debug[i]; + if(_model.config.debugMode != DEBUG_NONE && _model.config.debugMode != DEBUG_BLACKBOX_OUTPUT) { + debug[i] = _model.state.debug[i]; + } } } diff --git a/lib/Espfc/src/Controller.h b/lib/Espfc/src/Controller.h index a4ffac9c..277e3469 100644 --- a/lib/Espfc/src/Controller.h +++ b/lib/Espfc/src/Controller.h @@ -2,6 +2,7 @@ #define _ESPFC_CONTROLLER_H_ #include "Model.h" +#include "Math/Utils.h" #define SETPOINT_RATE_LIMIT 1998.0f #define RC_RATE_INCREMENTAL 14.54f @@ -112,6 +113,9 @@ class Controller ); _model.state.desiredRate[AXIS_ROLL] = _model.state.outerPid[AXIS_ROLL].update(_model.state.desiredAngle[AXIS_ROLL], _model.state.angle[AXIS_ROLL]); _model.state.desiredRate[AXIS_PITCH] = _model.state.outerPid[AXIS_PITCH].update(_model.state.desiredAngle[AXIS_PITCH], _model.state.angle[AXIS_PITCH]); + // disable fterm in angle mode + _model.state.innerPid[AXIS_ROLL].fScale = 0.f; + _model.state.innerPid[AXIS_PITCH].fScale = 0.f; } else { @@ -136,7 +140,7 @@ class Controller for(size_t i = 0; i <= AXIS_YAW; ++i) { _model.state.output[i] = _model.state.innerPid[i].update(_model.state.desiredRate[i], _model.state.gyro[i]) * tpaFactor; - //_model.state.debug[i] = lrintf(_model.state.innerPid[i].iTerm * 1000); + //_model.state.debug[i] = lrintf(_model.state.innerPid[i].fTerm * 1000); } _model.state.output[AXIS_THRUST] = _model.state.desiredRate[AXIS_THRUST]; } @@ -144,7 +148,7 @@ class Controller float getTpaFactor() const { if(_model.config.tpaScale == 0) return 1.f; - float t = constrain(_model.state.inputUs[AXIS_THRUST], (float)_model.config.tpaBreakpoint, 2000.f); + float t = Math::clamp(_model.state.inputUs[AXIS_THRUST], (float)_model.config.tpaBreakpoint, 2000.f); return Math::map(t, (float)_model.config.tpaBreakpoint, 2000.f, 1.f, 1.f - ((float)_model.config.tpaScale * 0.01f)); } @@ -164,6 +168,8 @@ class Controller float calculateSetpointRate(int axis, float input) { + input = Math::clamp(input, -0.995f, 0.995f); // limit input + if(axis == AXIS_YAW) input *= -1.f; float rcRate = _model.config.input.rate[axis] / 100.0f; @@ -174,20 +180,20 @@ class Controller rcRate += RC_RATE_INCREMENTAL * (rcRate - 2.0f); } - const float inputAbs = abs(input); + const float inputAbs = fabsf(input); if(rcExpo) { - const float expof = rcExpo / 100.0f; + const float expof = rcExpo * 0.01f; input = input * power3(inputAbs) * expof + input * (1.f - expof); } float angleRate = 200.0f * rcRate * input; if (_model.config.input.superRate[axis]) { - const float rcSuperfactor = 1.0f / (constrain(1.0f - (inputAbs * (_model.config.input.superRate[axis] / 100.0f)), 0.01f, 1.00f)); + const float rcSuperfactor = 1.0f / (Math::clamp(1.0f - (inputAbs * (_model.config.input.superRate[axis] * 0.01f)), 0.01f, 1.00f)); angleRate *= rcSuperfactor; } - return radians(constrain(angleRate, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT)); // Rate limit protection (deg/sec) + return radians(Math::clamp(angleRate, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT)); // Rate limit protection (deg/sec) } private: diff --git a/lib/Espfc/src/Fusion.h b/lib/Espfc/src/Fusion.h index 9a289c18..8da0ecbb 100644 --- a/lib/Espfc/src/Fusion.h +++ b/lib/Espfc/src/Fusion.h @@ -110,7 +110,7 @@ class Fusion _model.state.pose = _model.state.accel.accelToEuler(); _model.state.angle.x = _model.state.pose.x; _model.state.angle.y = _model.state.pose.y; - _model.state.angle.z += _model.state.gyroTimer.getDelta() * _model.state.gyro.z; + _model.state.angle.z += _model.state.gyroTimer.intervalf * _model.state.gyro.z; if(_model.state.angle.z > PI) _model.state.angle.z -= TWO_PI; if(_model.state.angle.z < -PI) _model.state.angle.z += TWO_PI; } @@ -119,7 +119,7 @@ class Fusion { _model.state.pose = _model.state.accel.accelToEuler(); _model.state.pose.z = _model.state.angle.z; - const float dt = _model.state.gyroTimer.getDelta(); + const float dt = _model.state.gyroTimer.intervalf; for(size_t i = 0; i < 3; i++) { float angle = _model.state.kalman[i].getAngle(_model.state.pose.get(i), _model.state.gyro.get(i), dt); @@ -133,7 +133,7 @@ class Fusion { _model.state.pose = _model.state.accel.accelToEuler(); _model.state.pose.z = _model.state.angle.z; - const float dt = _model.state.gyroTimer.getDelta(); + const float dt = _model.state.gyroTimer.intervalf; const float alpha = 0.002f; for(size_t i = 0; i < 3; i++) { @@ -149,7 +149,7 @@ class Fusion void complementaryFusionOld() { const float alpha = 0.01f; - const float dt = _model.state.gyroTimer.getDelta(); + const float dt = _model.state.gyroTimer.intervalf; _model.state.pose = _model.state.accel.accelToEuler(); _model.state.pose.z = _model.state.angle.z; _model.state.angle = (_model.state.angle + _model.state.gyro * dt) * (1.f - alpha) + _model.state.pose * alpha; @@ -167,7 +167,7 @@ class Fusion return; } - float timeDelta = _model.state.gyroTimer.getDelta(); + float timeDelta = _model.state.gyroTimer.intervalf; Quaternion measuredQPose = _model.state.poseQ; Quaternion fusionQPose = _model.state.angleQ; VectorFloat fusionGyro = _model.state.gyro; @@ -295,7 +295,7 @@ class Fusion //_model.state.accelPose.eulerFromQuaternion(_model.state.accelPoseQ); // predict new state - Quaternion rotation = (_model.state.gyro * _model.state.gyroTimer.getDelta()).eulerToQuaternion(); + Quaternion rotation = (_model.state.gyro * _model.state.gyroTimer.intervalf).eulerToQuaternion(); _model.state.gyroPoseQ = (_model.state.gyroPoseQ * rotation).getNormalized(); // drift compensation diff --git a/lib/Espfc/src/Input.h b/lib/Espfc/src/Input.h index 01dafaf1..de2a5b1e 100644 --- a/lib/Espfc/src/Input.h +++ b/lib/Espfc/src/Input.h @@ -23,6 +23,10 @@ class Input { _device = Hardware::getInputDevice(_model); setFailsafe(); + for(size_t i = 0; i < INTERPOLETE_COUNT; i++) + { + _filter[i].begin(FilterConfig(FILTER_PT1, 30), _model.state.loopTimer.rate); + } return 1; } @@ -42,7 +46,7 @@ class Input if(!_device) return 0; static float step = 0; - static float inputDt = 0.02f; + static float inputDt = 0.02f; // default interpolation interval 20ms static uint32_t prevTm = 0; { @@ -64,24 +68,24 @@ class Input step = 0.f; switch(_model.config.input.interpolationMode) { - case INPUT_INTERPOLATION_AUTO: + case INPUT_INTERPOLATION_OFF: + for(size_t i = 0; i < INPUT_CHANNELS; ++i) { - uint32_t now = micros(); - inputDt = constrain(now - prevTm, (uint32_t)4000, (uint32_t)40000) * 0.000001f; - prevTm = now; + _model.state.inputUs[i] = (float)_get(i, 0); } break; - case INPUT_INTERPOLATION_MANUAL: - inputDt = _model.config.input.interpolationInterval * 0.001f; + case INPUT_INTERPOLATION_DEFAULT: + inputDt = 0.02f; // default interpolation interval 20ms break; - case INPUT_INTERPOLATION_OFF: - for(size_t i = 0; i < INPUT_CHANNELS; ++i) + case INPUT_INTERPOLATION_AUTO: { - _model.state.inputUs[i] = (float)_get(i, 0); + uint32_t now = micros(); + inputDt = Math::clamp(now - prevTm, (uint32_t)4000, (uint32_t)40000) * 0.000001f; // estimate real interval + prevTm = now; } break; - default: - inputDt = 0.02f; + case INPUT_INTERPOLATION_MANUAL: + inputDt = _model.config.input.interpolationInterval * 0.001f; // manual interval break; } } @@ -89,31 +93,33 @@ class Input { Stats::Measure filterMeasure(_model.state.stats, COUNTER_INPUT_FILTER); - if(_model.config.input.interpolationMode != INPUT_INTERPOLATION_OFF) + switch(_model.config.input.interpolationMode) { - const float loopDt = _model.state.loopTimer.getDelta(); - const float interpolationStep = loopDt / inputDt; - if(step < 1.f) - { - step += interpolationStep; - } - for(size_t i = 0; i < INPUT_CHANNELS; ++i) - { - float val = (float)_get(i, 0); - if(i < INTERPOLETE_COUNT) + case INPUT_INTERPOLATION_OFF: + break; + default: + const float loopDt = _model.state.loopTimer.intervalf; + const float interpolationStep = loopDt / inputDt; + if(step < 1.f) step += interpolationStep; + for(size_t i = 0; i < INPUT_CHANNELS; ++i) { - float prev = (float)_get(i, 1); - val =_interpolate(prev, val, step); + float val = (float)_get(i, 0); + if(i < INTERPOLETE_COUNT) + { + float prev = (float)_get(i, 1); + val =_interpolate(prev, val, step); + val = _filter[i].update(val); + } + _model.state.inputUs[i] = val; } - _model.state.inputUs[i] = val; - } - if(_model.config.debugMode == DEBUG_RC_INTERPOLATION) - { - _model.state.debug[0] = 1000 * inputDt; - _model.state.debug[1] = 10000 * loopDt; - _model.state.debug[2] = 1000 * interpolationStep; - _model.state.debug[3] = 1000 * step; - } + if(_model.config.debugMode == DEBUG_RC_INTERPOLATION) + { + _model.state.debug[0] = 1000 * inputDt; + _model.state.debug[1] = 10000 * loopDt; + _model.state.debug[2] = 1000 * interpolationStep; + _model.state.debug[3] = 1000 * step; + } + break; } for(size_t i = 0; i < INPUT_CHANNELS; ++i) @@ -164,7 +170,7 @@ class Input const InputChannelConfig& ich = _model.config.input.channel[c]; v -= _model.config.input.midRc - 1500; float t = Math::map3((float)v, (float)ich.min, (float)ich.neutral, (float)ich.max, 1000.f, 1500.f, 2000.f); - t = constrain(t, 800.f, 2200.f); + t = Math::clamp(t, 800.f, 2200.f); _buff[0][c] = _deadband(c, lrintf(t)); } @@ -180,6 +186,7 @@ class Input int16_t _buff[INPUT_BUFF_SIZE][INPUT_CHANNELS]; InputDevice * _device; static const size_t INTERPOLETE_COUNT = 4; + Filter _filter[INTERPOLETE_COUNT]; }; } diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index 45ad0716..e92b7e24 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -329,7 +329,7 @@ class Model state.gyroFilter2[i].begin(config.gyroFilter2, state.gyroTimer.rate); state.gyroFilter3[i].begin(config.gyroFilter3, state.gyroTimer.rate); state.accelFilter[i].begin(config.accelFilter, state.accelTimer.rate); - state.gyroFilterImu[i].begin(FilterConfig(FILTER_PT1_FIR2, state.accelTimer.rate / 2), state.gyroTimer.rate); + state.gyroFilterImu[i].begin(FilterConfig(FILTER_PT1, state.accelTimer.rate / 2), state.gyroTimer.rate); if(magActive()) { state.magFilter[i].begin(config.magFilter, state.magTimer.rate); @@ -370,7 +370,7 @@ class Model pid.dtermFilter.begin(config.dtermFilter, state.loopTimer.rate); } pid.dtermFilter2.begin(config.dtermFilter2, state.loopTimer.rate); - pid.ftermFilter.begin(FilterConfig(FILTER_PT1, 15), state.loopTimer.rate); + pid.ftermFilter.begin(FilterConfig(FILTER_PT1, 20), state.loopTimer.rate); if(i == AXIS_YAW) pid.ptermFilter.begin(config.yawFilter, state.loopTimer.rate); pid.begin(); } diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index eaab12e2..4030bc5e 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -1023,7 +1023,7 @@ class ModelConfig #if defined(ESP8266) debugMode = DEBUG_GYRO_SCALED; serial[SERIAL_UART_1].id = SERIAL_UART_1; - serial[SERIAL_UART_1].baudIndex = SERIAL_SPEED_INDEX_250000; + //serial[SERIAL_UART_1].baudIndex = SERIAL_SPEED_INDEX_250000; serial[SERIAL_UART_1].functionMask = SERIAL_FUNCTION_BLACKBOX; serial[SERIAL_UART_1].blackboxBaudIndex = SERIAL_SPEED_INDEX_250000; //serial[SERIAL_UART_1].functionMask = SERIAL_FUNCTION_TELEMETRY_FRSKY; diff --git a/lib/Espfc/src/Msp.h b/lib/Espfc/src/Msp.h index 9074f4a7..bb8e1b7b 100644 --- a/lib/Espfc/src/Msp.h +++ b/lib/Espfc/src/Msp.h @@ -158,7 +158,7 @@ class Msp case MSP_STATUS_EX: case MSP_STATUS: - r.writeU16(lrintf(_model.state.loopTimer.getDeltaReal() * 1000000.f)); + r.writeU16(_model.state.loopTimer.delta); r.writeU16(_model.state.i2cErrorCount); // i2c error count // acc, baro, mag, gps, sonar, gyro r.writeU16(_model.accelActive() | _model.baroActive() << 1 | _model.magActive() << 2 | 0 << 3 | 0 << 4 | _model.gyroActive() << 5); diff --git a/lib/Espfc/src/Pid.h b/lib/Espfc/src/Pid.h index 220d1911..5e7d9b22 100644 --- a/lib/Espfc/src/Pid.h +++ b/lib/Espfc/src/Pid.h @@ -8,7 +8,7 @@ #define PTERM_SCALE_BETAFLIGHT 0.032029f #define ITERM_SCALE_BETAFLIGHT 0.244381f #define DTERM_SCALE_BETAFLIGHT 0.000529f -#define FTERM_SCALE_BETAFLIGHT 0.013754f +#define FTERM_SCALE_BETAFLIGHT 0.00013754f #define PTERM_SCALE (PTERM_SCALE_BETAFLIGHT * RAD_TO_DEG * 0.001f) // ~ 0.00183 = 0.032029f * 57.29 / 1000 #define ITERM_SCALE (ITERM_SCALE_BETAFLIGHT * RAD_TO_DEG * 0.001f) // ~ 0.014f @@ -70,7 +70,7 @@ class Pid if(Kf > 0.f && fScale > 0.f) { - fTerm = Kf * fScale * ((prevSetpoint - setpoint) * rate); + fTerm = Kf * fScale * ((setpoint - prevSetpoint) * rate); fTerm = ftermFilter.update(fTerm); } else diff --git a/lib/Espfc/src/SerialManager.h b/lib/Espfc/src/SerialManager.h index 1fd024b5..18834d5f 100644 --- a/lib/Espfc/src/SerialManager.h +++ b/lib/Espfc/src/SerialManager.h @@ -49,10 +49,14 @@ class SerialManager break; } } + else if(spc.functionMask & SERIAL_FUNCTION_BLACKBOX) + { + sdc.baud = Hardware::fromIndex((SerialSpeedIndex)spc.blackboxBaudIndex, SERIAL_SPEED_115200); + } _model.logger.info().log(F("UART")).log(i).log(spc.id).log(spc.functionMask).log(sdc.baud).log(sdc.tx_pin).logln(sdc.rx_pin); port->flush(); - delay(20); + delay(10); port->begin(sdc); _model.state.serial[i].stream = port; } diff --git a/lib/Espfc/src/Timer.h b/lib/Espfc/src/Timer.h index d71abdac..9d498970 100644 --- a/lib/Espfc/src/Timer.h +++ b/lib/Espfc/src/Timer.h @@ -16,7 +16,8 @@ class Timer this->interval = interval; this->rate = 1000000UL / interval; this->denom = 1; - this->intervalf = this->delta = this->interval * 0.000001f; + this->delta = this->interval; + this->intervalf = this->interval * 0.000001f; iteration = 0; return 1; } @@ -26,7 +27,8 @@ class Timer this->rate = rate / denom; this->interval = 1000000UL / this->rate; this->denom = denom; - this->intervalf = this->delta = this->interval * 0.000001f; + this->delta = this->interval; + this->intervalf = this->interval * 0.000001f; iteration = 0; return 1; } @@ -69,16 +71,6 @@ class Timer return update(); } - float getDelta() const - { - return intervalf; - } - - float getDeltaReal() const - { - return delta * 0.000001f; - } - uint32_t interval; uint32_t rate; uint32_t denom; diff --git a/lib/betaflight/src/blackbox/blackbox.c b/lib/betaflight/src/blackbox/blackbox.c index 2593547d..33fe7f08 100644 --- a/lib/betaflight/src/blackbox/blackbox.c +++ b/lib/betaflight/src/blackbox/blackbox.c @@ -45,13 +45,15 @@ #include "config/feature.h" #include "pg/pg.h" #include "pg/pg_ids.h" +#include "pg/motor.h" #include "pg/rx.h" #include "drivers/compass/compass.h" #include "drivers/sensor.h" #include "drivers/time.h" -#include "fc/config.h" +#include "config/config.h" +#include "fc/board_info.h" #include "fc/controlrate_profile.h" #include "fc/rc.h" #include "fc/rc_controls.h" @@ -61,6 +63,7 @@ #include "flight/failsafe.h" #include "flight/mixer.h" #include "flight/pid.h" +#include "flight/rpm_filter.h" #include "flight/servos.h" #include "io/beeper.h" @@ -189,8 +192,13 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = { {"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)}, + {"rcCommand", 3, UNSIGNED, .Ipredict = PREDICT(0), .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 +299,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]; @@ -448,7 +457,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition) #endif case FLIGHT_LOG_FIELD_CONDITION_RSSI: - return rxConfig()->rssi_channel > 0 || featureIsEnabled(FEATURE_RSSI_ADC); + return isRssiConfigured(); case FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME: return blackboxConfig()->p_ratio != 1; @@ -541,10 +550,13 @@ static void writeIntraframe(void) blackboxWriteSigned16VBArray(blackboxCurrent->rcCommand, 3); /* - * Write the throttle separately from the rest of the RC data so we can apply a predictor to it. - * Throttle lies in range [minthrottle..maxthrottle]: + * Write the throttle separately from the rest of the RC data as it's unsigned. + * Throttle lies in range [PWM_RANGE_MIN..PWM_RANGE_MAX]: */ - blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[THROTTLE] - motorConfig()->minthrottle); + blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[THROTTLE]); + + // Write setpoint roll, pitch, yaw, and throttle + blackboxWriteSigned16VBArray(blackboxCurrent->setpoint, 4); if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) { /* @@ -648,6 +660,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 +691,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; @@ -1003,15 +1019,24 @@ static void loadMainState(timeUs_t currentTimeUs) blackboxCurrent->axisPID_D[i] = pidData[i].D; blackboxCurrent->axisPID_F[i] = pidData[i].F; blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]); +#if defined(USE_ACC) blackboxCurrent->accADC[i] = lrintf(acc.accADC[i]); +#endif #ifdef USE_MAG blackboxCurrent->magADC[i] = mag.magADC[i]; #endif } for (int i = 0; i < 4; i++) { - blackboxCurrent->rcCommand[i] = rcCommand[i]; + blackboxCurrent->rcCommand[i] = lrintf(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(mixerGetThrottle() * 1000); for (int i = 0; i < DEBUG16_VALUE_COUNT; i++) { blackboxCurrent->debug[i] = debug[i]; @@ -1164,15 +1189,15 @@ static bool sendFieldDefinition(char mainFrameChar, char deltaFrameChar, const v // Buf must be at least FORMATTED_DATE_TIME_BUFSIZE STATIC_UNIT_TESTED char *blackboxGetStartDateTime(char *buf) { - #ifdef USE_RTC_TIME +#ifdef USE_RTC_TIME dateTime_t dt; // rtcGetDateTime will fill dt with 0000-01-01T00:00:00 // when time is not known. rtcGetDateTime(&dt); dateTimeFormatLocal(buf, &dt); - #else +#else buf = "0000-01-01T00:00:00.000"; - #endif +#endif return buf; } @@ -1203,11 +1228,18 @@ static bool blackboxWriteSysinfo(void) char buf[FORMATTED_DATE_TIME_BUFSIZE]; +#ifdef USE_RC_SMOOTHING_FILTER + rcSmoothingFilter_t *rcSmoothingData = getRcSmoothingData(); +#endif + const controlRateConfig_t *currentControlRateProfile = controlRateProfiles(systemConfig()->activeRateProfile); switch (xmitState.headerIndex) { BLACKBOX_PRINT_HEADER_LINE("Firmware type", "%s", "Cleanflight"); BLACKBOX_PRINT_HEADER_LINE("Firmware revision", "%s %s (%s) %s", FC_FIRMWARE_NAME, FC_VERSION_STRING, shortGitRevision, targetName); BLACKBOX_PRINT_HEADER_LINE("Firmware date", "%s %s", buildDate, buildTime); +#ifdef USE_BOARD_INFO + BLACKBOX_PRINT_HEADER_LINE("Board information", "%s %s", getManufacturerId(), getBoardName()); +#endif BLACKBOX_PRINT_HEADER_LINE("Log start datetime", "%s", blackboxGetStartDateTime(buf)); BLACKBOX_PRINT_HEADER_LINE("Craft name", "%s", pilotConfig()->name); BLACKBOX_PRINT_HEADER_LINE("I interval", "%d", blackboxIInterval); @@ -1217,7 +1249,9 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE("maxthrottle", "%d", motorConfig()->maxthrottle); BLACKBOX_PRINT_HEADER_LINE("gyro_scale","0x%x", castFloatBytesToInt(1.0f)); BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d", motorOutputLowInt,motorOutputHighInt); +#if defined(USE_ACC) BLACKBOX_PRINT_HEADER_LINE("acc_1G", "%u", acc.dev.acc_1G); +#endif BLACKBOX_PRINT_HEADER_LINE_CUSTOM( if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) { @@ -1238,9 +1272,9 @@ static bool blackboxWriteSysinfo(void) } ); - BLACKBOX_PRINT_HEADER_LINE("looptime", "%d", gyro.targetLooptime); - BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom", "%d", gyroConfig()->gyro_sync_denom); - BLACKBOX_PRINT_HEADER_LINE("pid_process_denom", "%d", pidConfig()->pid_process_denom); + BLACKBOX_PRINT_HEADER_LINE("looptime", "%d", gyro.sampleLooptime); + BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom", "%d", 1); + BLACKBOX_PRINT_HEADER_LINE("pid_process_denom", "%d", activePidLoopDenom); BLACKBOX_PRINT_HEADER_LINE("thr_mid", "%d", currentControlRateProfile->thrMid8); BLACKBOX_PRINT_HEADER_LINE("thr_expo", "%d", currentControlRateProfile->thrExpo8); BLACKBOX_PRINT_HEADER_LINE("tpa_rate", "%d", currentControlRateProfile->dynThrPID); @@ -1254,6 +1288,9 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE("rates", "%d,%d,%d", currentControlRateProfile->rates[ROLL], currentControlRateProfile->rates[PITCH], currentControlRateProfile->rates[YAW]); + BLACKBOX_PRINT_HEADER_LINE("rate_limits", "%d,%d,%d", currentControlRateProfile->rate_limit[ROLL], + currentControlRateProfile->rate_limit[PITCH], + currentControlRateProfile->rate_limit[YAW]); BLACKBOX_PRINT_HEADER_LINE("rollPID", "%d,%d,%d", currentPidProfile->pid[PID_ROLL].P, currentPidProfile->pid[PID_ROLL].I, currentPidProfile->pid[PID_ROLL].D); @@ -1267,13 +1304,30 @@ static bool blackboxWriteSysinfo(void) currentPidProfile->pid[PID_LEVEL].I, currentPidProfile->pid[PID_LEVEL].D); BLACKBOX_PRINT_HEADER_LINE("magPID", "%d", currentPidProfile->pid[PID_MAG].P); +#ifdef USE_D_MIN + BLACKBOX_PRINT_HEADER_LINE("d_min", "%d,%d,%d", currentPidProfile->d_min[ROLL], + currentPidProfile->d_min[PITCH], + currentPidProfile->d_min[YAW]); + BLACKBOX_PRINT_HEADER_LINE("d_min_gain", "%d", currentPidProfile->d_min_gain); + BLACKBOX_PRINT_HEADER_LINE("d_min_advance", "%d", currentPidProfile->d_min_advance); +#endif BLACKBOX_PRINT_HEADER_LINE("dterm_filter_type", "%d", currentPidProfile->dterm_filter_type); BLACKBOX_PRINT_HEADER_LINE("dterm_lowpass_hz", "%d", currentPidProfile->dterm_lowpass_hz); +#ifdef USE_DYN_LPF + BLACKBOX_PRINT_HEADER_LINE("dterm_lowpass_dyn_hz", "%d,%d", currentPidProfile->dyn_lpf_dterm_min_hz, + currentPidProfile->dyn_lpf_dterm_max_hz); +#endif + BLACKBOX_PRINT_HEADER_LINE("dterm_filter2_type", "%d", currentPidProfile->dterm_filter2_type); BLACKBOX_PRINT_HEADER_LINE("dterm_lowpass2_hz", "%d", currentPidProfile->dterm_lowpass2_hz); BLACKBOX_PRINT_HEADER_LINE("yaw_lowpass_hz", "%d", currentPidProfile->yaw_lowpass_hz); BLACKBOX_PRINT_HEADER_LINE("dterm_notch_hz", "%d", currentPidProfile->dterm_notch_hz); BLACKBOX_PRINT_HEADER_LINE("dterm_notch_cutoff", "%d", currentPidProfile->dterm_notch_cutoff); BLACKBOX_PRINT_HEADER_LINE("iterm_windup", "%d", currentPidProfile->itermWindupPointPercent); +#if defined(USE_ITERM_RELAX) + BLACKBOX_PRINT_HEADER_LINE("iterm_relax", "%d", currentPidProfile->iterm_relax); + BLACKBOX_PRINT_HEADER_LINE("iterm_relax_type", "%d", currentPidProfile->iterm_relax_type); + BLACKBOX_PRINT_HEADER_LINE("iterm_relax_cutoff", "%d", currentPidProfile->iterm_relax_cutoff); +#endif BLACKBOX_PRINT_HEADER_LINE("vbat_pid_gain", "%d", currentPidProfile->vbatPidCompensation); BLACKBOX_PRINT_HEADER_LINE("pidAtMinThrottle", "%d", currentPidProfile->pidAtMinThrottle); @@ -1281,11 +1335,22 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE("anti_gravity_mode", "%d", currentPidProfile->antiGravityMode); BLACKBOX_PRINT_HEADER_LINE("anti_gravity_threshold", "%d", currentPidProfile->itermThrottleThreshold); BLACKBOX_PRINT_HEADER_LINE("anti_gravity_gain", "%d", currentPidProfile->itermAcceleratorGain); - +#ifdef USE_ABSOLUTE_CONTROL + BLACKBOX_PRINT_HEADER_LINE("abs_control_gain", "%d", currentPidProfile->abs_control_gain); +#endif +#ifdef USE_INTEGRATED_YAW_CONTROL + BLACKBOX_PRINT_HEADER_LINE("use_integrated_yaw", "%d", currentPidProfile->use_integrated_yaw); +#endif BLACKBOX_PRINT_HEADER_LINE("feedforward_transition", "%d", currentPidProfile->feedForwardTransition); BLACKBOX_PRINT_HEADER_LINE("feedforward_weight", "%d,%d,%d", currentPidProfile->pid[PID_ROLL].F, currentPidProfile->pid[PID_PITCH].F, currentPidProfile->pid[PID_YAW].F); +#ifdef USE_INTERPOLATED_SP + BLACKBOX_PRINT_HEADER_LINE("ff_interpolate_sp", "%d", currentPidProfile->ff_interpolate_sp); + BLACKBOX_PRINT_HEADER_LINE("ff_spike_limit", "%d", currentPidProfile->ff_spike_limit); + BLACKBOX_PRINT_HEADER_LINE("ff_max_rate_limit", "%d", currentPidProfile->ff_max_rate_limit); +#endif + BLACKBOX_PRINT_HEADER_LINE("ff_boost", "%d", currentPidProfile->ff_boost); BLACKBOX_PRINT_HEADER_LINE("acc_limit_yaw", "%d", currentPidProfile->yawRateAccelLimit); BLACKBOX_PRINT_HEADER_LINE("acc_limit", "%d", currentPidProfile->rateAccelLimit); @@ -1297,21 +1362,46 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE("yaw_deadband", "%d", rcControlsConfig()->yaw_deadband); BLACKBOX_PRINT_HEADER_LINE("gyro_hardware_lpf", "%d", gyroConfig()->gyro_hardware_lpf); -#ifdef USE_32K_CAPABLE_GYRO - BLACKBOX_PRINT_HEADER_LINE("gyro_32khz_hardware_lpf", "%d", gyroConfig()->gyro_32khz_hardware_lpf); -#endif BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_type", "%d", gyroConfig()->gyro_lowpass_type); BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz", "%d", gyroConfig()->gyro_lowpass_hz); +#ifdef USE_DYN_LPF + BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_dyn_hz", "%d,%d", gyroConfig()->dyn_lpf_gyro_min_hz, + gyroConfig()->dyn_lpf_gyro_max_hz); +#endif BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass2_type", "%d", gyroConfig()->gyro_lowpass2_type); BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass2_hz", "%d", gyroConfig()->gyro_lowpass2_hz); BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz", "%d,%d", gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_hz_2); BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1, gyroConfig()->gyro_soft_notch_cutoff_2); +#ifdef USE_GYRO_DATA_ANALYSE + BLACKBOX_PRINT_HEADER_LINE("dyn_notch_max_hz", "%d", gyroConfig()->dyn_notch_max_hz); + BLACKBOX_PRINT_HEADER_LINE("dyn_notch_width_percent", "%d", gyroConfig()->dyn_notch_width_percent); + BLACKBOX_PRINT_HEADER_LINE("dyn_notch_q", "%d", gyroConfig()->dyn_notch_q); + BLACKBOX_PRINT_HEADER_LINE("dyn_notch_min_hz", "%d", gyroConfig()->dyn_notch_min_hz); +#endif +#ifdef USE_DSHOT_TELEMETRY + BLACKBOX_PRINT_HEADER_LINE("dshot_bidir", "%d", motorConfig()->dev.useDshotTelemetry); +#endif +#ifdef USE_RPM_FILTER + BLACKBOX_PRINT_HEADER_LINE("gyro_rpm_notch_harmonics", "%d", rpmFilterConfig()->gyro_rpm_notch_harmonics); + BLACKBOX_PRINT_HEADER_LINE("gyro_rpm_notch_q", "%d", rpmFilterConfig()->gyro_rpm_notch_q); + BLACKBOX_PRINT_HEADER_LINE("gyro_rpm_notch_min", "%d", rpmFilterConfig()->gyro_rpm_notch_min); + BLACKBOX_PRINT_HEADER_LINE("dterm_rpm_notch_harmonics", "%d", rpmFilterConfig()->dterm_rpm_notch_harmonics); + BLACKBOX_PRINT_HEADER_LINE("dterm_rpm_notch_q", "%d", rpmFilterConfig()->dterm_rpm_notch_q); + BLACKBOX_PRINT_HEADER_LINE("dterm_rpm_notch_min", "%d", rpmFilterConfig()->dterm_rpm_notch_min); + BLACKBOX_PRINT_HEADER_LINE("rpm_notch_lpf", "%d", rpmFilterConfig()->rpm_lpf); +#endif +#if defined(USE_ACC) BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz", "%d", (int)(accelerometerConfig()->acc_lpf_hz * 100.0f)); BLACKBOX_PRINT_HEADER_LINE("acc_hardware", "%d", accelerometerConfig()->acc_hardware); +#endif +#ifdef USE_BARO BLACKBOX_PRINT_HEADER_LINE("baro_hardware", "%d", barometerConfig()->baro_hardware); +#endif +#ifdef USE_MAG BLACKBOX_PRINT_HEADER_LINE("mag_hardware", "%d", compassConfig()->mag_hardware); +#endif BLACKBOX_PRINT_HEADER_LINE("gyro_cal_on_first_arm", "%d", armingConfig()->gyro_cal_on_first_arm); BLACKBOX_PRINT_HEADER_LINE("rc_interpolation", "%d", rxConfig()->rcInterpolation); BLACKBOX_PRINT_HEADER_LINE("rc_interpolation_interval", "%d", rxConfig()->rcInterpolationInterval); @@ -1322,21 +1412,22 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE("motor_pwm_protocol", "%d", motorConfig()->dev.motorPwmProtocol); BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate", "%d", motorConfig()->dev.motorPwmRate); BLACKBOX_PRINT_HEADER_LINE("dshot_idle_value", "%d", motorConfig()->digitalIdleOffsetValue); - BLACKBOX_PRINT_HEADER_LINE("debug_mode", "%d", systemConfig()->debug_mode); + BLACKBOX_PRINT_HEADER_LINE("debug_mode", "%d", debugMode); BLACKBOX_PRINT_HEADER_LINE("features", "%d", featureConfig()->enabledFeatures); #ifdef USE_RC_SMOOTHING_FILTER BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_type", "%d", rxConfig()->rc_smoothing_type); - BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_debug_axis", "%d", rxConfig()->rc_smoothing_debug_axis); - BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_cutoffs", "%d, %d", rxConfig()->rc_smoothing_input_cutoff, - rxConfig()->rc_smoothing_derivative_cutoff); - BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_filter_type", "%d, %d", rxConfig()->rc_smoothing_input_type, - rxConfig()->rc_smoothing_derivative_type); - BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_active_cutoffs", "%d, %d", rcSmoothingGetValue(RC_SMOOTHING_VALUE_INPUT_ACTIVE), - rcSmoothingGetValue(RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE)); - BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_rx_average", "%d", rcSmoothingGetValue(RC_SMOOTHING_VALUE_AVERAGE_FRAME)); + BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_debug_axis", "%d", rcSmoothingData->debugAxis); + BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_cutoffs", "%d, %d", rcSmoothingData->inputCutoffSetting, + rcSmoothingData->derivativeCutoffSetting); + BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_auto_factor", "%d", rcSmoothingData->autoSmoothnessFactor); + BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_filter_type", "%d, %d", rcSmoothingData->inputFilterType, + rcSmoothingData->derivativeFilterType); + BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_active_cutoffs", "%d, %d", rcSmoothingData->inputCutoffFrequency, + rcSmoothingData->derivativeCutoffFrequency); + BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_rx_average", "%d", rcSmoothingData->averageFrameTimeUs); #endif // USE_RC_SMOOTHING_FILTER - + BLACKBOX_PRINT_HEADER_LINE("rates_type", "%d", currentControlRateProfile->rates_type); default: return true; @@ -1370,6 +1461,9 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data) blackboxWriteUnsignedVB(data->flightMode.flags); blackboxWriteUnsignedVB(data->flightMode.lastFlags); break; + case FLIGHT_LOG_EVENT_DISARM: + blackboxWriteUnsignedVB(data->disarm.reason); + break; case FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT: if (data->inflightAdjustment.floatFlag) { blackboxWrite(data->inflightAdjustment.adjustmentFunction + FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT_FUNCTION_FLOAT_VALUE_FLAG); @@ -1387,6 +1481,8 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data) blackboxWriteString("End of log"); blackboxWrite(0); break; + default: + break; } } @@ -1730,13 +1826,8 @@ void blackboxInit(void) // an I-frame is written every 32ms // blackboxUpdate() is run in synchronisation with the PID loop // targetPidLooptime is 1000 for 1kHz loop, 500 for 2kHz loop etc, targetPidLooptime is rounded for short looptimes - if (targetPidLooptime == 31) { // rounded from 31.25us - blackboxIInterval = 1024; - } else if (targetPidLooptime == 63) { // rounded from 62.5us - blackboxIInterval = 512; - } else { - blackboxIInterval = (uint16_t)(32 * 1000 / targetPidLooptime); - } + blackboxIInterval = (uint16_t)(32 * 1000 / targetPidLooptime); + // by default p_ratio is 32 and a P-frame is written every 1ms // if p_ratio is zero then no P-frames are logged if (blackboxConfig()->p_ratio == 0) { diff --git a/lib/betaflight/src/blackbox/blackbox.h b/lib/betaflight/src/blackbox/blackbox.h index b73b54ea..406318b5 100644 --- a/lib/betaflight/src/blackbox/blackbox.h +++ b/lib/betaflight/src/blackbox/blackbox.h @@ -27,12 +27,8 @@ typedef enum BlackboxDevice { BLACKBOX_DEVICE_NONE = 0, -#ifdef USE_FLASHFS BLACKBOX_DEVICE_FLASH = 1, -#endif -#ifdef USE_SDCARD BLACKBOX_DEVICE_SDCARD = 2, -#endif BLACKBOX_DEVICE_SERIAL = 3 } BlackboxDevice_e; @@ -44,8 +40,12 @@ typedef enum BlackboxMode { typedef enum FlightLogEvent { FLIGHT_LOG_EVENT_SYNC_BEEP = 0, + FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START = 10, // UNUSED + FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT = 11, // UNUSED + FLIGHT_LOG_EVENT_AUTOTUNE_TARGETS = 12, // UNUSED FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT = 13, FLIGHT_LOG_EVENT_LOGGING_RESUME = 14, + FLIGHT_LOG_EVENT_DISARM = 15, FLIGHT_LOG_EVENT_FLIGHTMODE = 30, // Add new event type for flight mode status. FLIGHT_LOG_EVENT_LOG_END = 255 } FlightLogEvent; diff --git a/lib/betaflight/src/blackbox/blackbox_fielddefs.h b/lib/betaflight/src/blackbox/blackbox_fielddefs.h index 7a5f54f0..e261fc7f 100644 --- a/lib/betaflight/src/blackbox/blackbox_fielddefs.h +++ b/lib/betaflight/src/blackbox/blackbox_fielddefs.h @@ -113,6 +113,10 @@ typedef struct flightLogEvent_syncBeep_s { uint32_t time; } flightLogEvent_syncBeep_t; +typedef struct flightLogEvent_disarm_s { + uint32_t reason; +} flightLogEvent_disarm_t; + typedef struct flightLogEvent_flightMode_s { // New Event Data type uint32_t flags; uint32_t lastFlags; @@ -135,6 +139,7 @@ typedef struct flightLogEvent_loggingResume_s { typedef union flightLogEventData_u { flightLogEvent_syncBeep_t syncBeep; flightLogEvent_flightMode_t flightMode; // New event data + flightLogEvent_disarm_t disarm; flightLogEvent_inflightAdjustment_t inflightAdjustment; flightLogEvent_loggingResume_t loggingResume; } flightLogEventData_t; diff --git a/lib/betaflight/src/blackbox/blackbox_io.c b/lib/betaflight/src/blackbox/blackbox_io.c index d0f7264b..da888647 100644 --- a/lib/betaflight/src/blackbox/blackbox_io.c +++ b/lib/betaflight/src/blackbox/blackbox_io.c @@ -28,6 +28,20 @@ #ifdef USE_BLACKBOX +#include "build/debug.h" + +// Debugging code that become useful when output bandwidth saturation is suspected. +// Set debug_mode = BLACKBOX_OUTPUT to see following debug values. +// +// 0: Average output bandwidth in last 100ms +// 1: Maximum hold of above. +// 2: Bytes dropped due to output buffer full. +// +// Note that bandwidth usage slightly increases when DEBUG_BB_OUTPUT is enabled, +// as output will include debug variables themselves. + +#define DEBUG_BB_OUTPUT + #include "blackbox.h" #include "blackbox_io.h" @@ -62,7 +76,7 @@ static struct { afatfsFilePtr_t logFile; afatfsFilePtr_t logDirectory; afatfsFinder_t logDirectoryFinder; - uint32_t largestLogFileNumber; + int32_t largestLogFileNumber; enum { BLACKBOX_SDCARD_INITIAL, @@ -87,8 +101,19 @@ void blackboxOpen(void) } } +#ifdef DEBUG_BB_OUTPUT +static uint32_t bbBits; +static timeMs_t bbLastclearMs; +static uint16_t bbRateMax; +static uint32_t bbDrops; +#endif + void blackboxWrite(uint8_t value) { +#ifdef DEBUG_BB_OUTPUT + bbBits += 8; +#endif + switch (blackboxConfig()->device) { #ifdef USE_FLASHFS case BLACKBOX_DEVICE_FLASH: @@ -102,9 +127,40 @@ void blackboxWrite(uint8_t value) #endif case BLACKBOX_DEVICE_SERIAL: default: - serialWrite(blackboxPort, value); + { + int txBytesFree = serialTxBytesFree(blackboxPort); + +#ifdef DEBUG_BB_OUTPUT + bbBits += 2; + DEBUG_SET(DEBUG_BLACKBOX_OUTPUT, 3, txBytesFree); +#endif + + if (txBytesFree == 0) { +#ifdef DEBUG_BB_OUTPUT + ++bbDrops; + DEBUG_SET(DEBUG_BLACKBOX_OUTPUT, 2, bbDrops); +#endif + return; + } + serialWrite(blackboxPort, value); + } break; } + +#ifdef DEBUG_BB_OUTPUT + timeMs_t now = millis(); + + if (now > bbLastclearMs + 100) { // Debug log every 100[msec] + uint16_t bbRate = ((bbBits * 10 + 5) / (now - bbLastclearMs)) / 10; // In unit of [Kbps] + DEBUG_SET(DEBUG_BLACKBOX_OUTPUT, 0, bbRate); + if (bbRate > bbRateMax) { + bbRateMax = bbRate; + DEBUG_SET(DEBUG_BLACKBOX_OUTPUT, 1, bbRateMax); + } + bbLastclearMs = now; + bbBits = 0; + } +#endif } // Print the null-terminated string 's' to the blackbox device and return the number of bytes written @@ -133,7 +189,7 @@ int blackboxWriteString(const char *s) default: pos = (uint8_t*) s; while (*pos) { - serialWrite(blackboxPort, *pos); + blackboxWrite(*pos); pos++; } @@ -205,7 +261,7 @@ bool blackboxDeviceOpen(void) switch (blackboxConfig()->device) { case BLACKBOX_DEVICE_SERIAL: { - serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_BLACKBOX); + const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_BLACKBOX); baudRate_e baudRateIndex; portOptions_e portOptions = SERIAL_PARITY_NO | SERIAL_NOT_INVERTED; @@ -384,7 +440,7 @@ static void blackboxLogFileCreated(afatfsFilePtr_t file) static void blackboxCreateLogFile(void) { - uint32_t remainder = blackboxSDCard.largestLogFileNumber + 1; + int32_t remainder = blackboxSDCard.largestLogFileNumber + 1; char filename[] = LOGFILE_PREFIX "00000." LOGFILE_SUFFIX; @@ -432,7 +488,7 @@ static bool blackboxSDCardBeginLog(void) memcpy(logSequenceNumberString, directoryEntry->filename + 3, 5); logSequenceNumberString[5] = '\0'; - blackboxSDCard.largestLogFileNumber = MAX((uint32_t) atoi(logSequenceNumberString), blackboxSDCard.largestLogFileNumber); + blackboxSDCard.largestLogFileNumber = MAX((int32_t)atoi(logSequenceNumberString), blackboxSDCard.largestLogFileNumber); } } else { // We're done checking all the files on the card, now we can create a new log file @@ -563,12 +619,17 @@ bool isBlackboxDeviceWorking(void) } } -unsigned int blackboxGetLogNumber(void) +int32_t blackboxGetLogNumber(void) { + switch (blackboxConfig()->device) { #ifdef USE_SDCARD - return blackboxSDCard.largestLogFileNumber; + case BLACKBOX_DEVICE_SDCARD: + return blackboxSDCard.largestLogFileNumber; #endif - return 0; + + default: + return -1; + } } /** diff --git a/lib/betaflight/src/blackbox/blackbox_io.h b/lib/betaflight/src/blackbox/blackbox_io.h index 72e1a604..52af9d5d 100644 --- a/lib/betaflight/src/blackbox/blackbox_io.h +++ b/lib/betaflight/src/blackbox/blackbox_io.h @@ -57,7 +57,7 @@ bool blackboxDeviceEndLog(bool retainLog); bool isBlackboxDeviceFull(void); bool isBlackboxDeviceWorking(void); -unsigned int blackboxGetLogNumber(void); +int32_t blackboxGetLogNumber(void); void blackboxReplenishHeaderBudget(void); blackboxBufferReserveStatus_e blackboxDeviceReserveBufferSpace(int32_t bytes); diff --git a/lib/betaflight/src/config/config.h b/lib/betaflight/src/config/config.h new file mode 100644 index 00000000..e69de29b diff --git a/lib/betaflight/src/drivers/sdcard.h b/lib/betaflight/src/drivers/sdcard.h new file mode 100644 index 00000000..e69de29b diff --git a/lib/betaflight/src/fc/board_info.h b/lib/betaflight/src/fc/board_info.h new file mode 100644 index 00000000..e69de29b diff --git a/lib/betaflight/src/flight/rpm_filter.h b/lib/betaflight/src/flight/rpm_filter.h new file mode 100644 index 00000000..e69de29b diff --git a/lib/betaflight/src/pg/motor.h b/lib/betaflight/src/pg/motor.h new file mode 100644 index 00000000..e69de29b diff --git a/lib/betaflight/src/platform.c b/lib/betaflight/src/platform.c index 7d1631cd..52bd09d4 100644 --- a/lib/betaflight/src/platform.c +++ b/lib/betaflight/src/platform.c @@ -48,12 +48,15 @@ uint8_t stateFlags; uint8_t armingFlags; uint8_t debugMode; int16_t debug[DEBUG16_VALUE_COUNT]; +uint8_t activePidLoopDenom = 1; gyro_t gyro; acc_t acc = { .dev = { .acc_1G = 2048 } }; +mag_t mag; +baro_t baro; uint16_t rssi; pidAxisData_t pidData[3]; float motor[MAX_SUPPORTED_MOTORS]; diff --git a/lib/betaflight/src/platform.h b/lib/betaflight/src/platform.h index a7cd06e0..333afe2c 100644 --- a/lib/betaflight/src/platform.h +++ b/lib/betaflight/src/platform.h @@ -1,6 +1,10 @@ #pragma once #define USE_BLACKBOX +#define USE_ACC +#define USE_MAG +#define USE_BARO +#define USE_DYN_LPF #include #include @@ -57,6 +61,7 @@ extern const char * const targetVersion; #define CONCAT4(_1,_2,_3,_4) CONCAT(CONCAT3(_1, _2, _3), _4) #define XYZ_AXIS_COUNT 3 #define DEBUG16_VALUE_COUNT 4 +#define DEBUG_SET(mode, index, value) {if (debugMode == (mode)) {debug[(index)] = (value);}} #ifdef UNIT_TEST #define STATIC_UNIT_TESTED @@ -375,43 +380,50 @@ bool areMotorsRunning(void); /* RCMODES START */ typedef enum { + // ARM flag BOXARM = 0, + // FLIGHT_MODE BOXANGLE, BOXHORIZON, - BOXBARO, - BOXANTIGRAVITY, BOXMAG, BOXHEADFREE, + BOXPASSTHRU, + BOXFAILSAFE, + BOXGPSRESCUE, + BOXID_FLIGHTMODE_LAST = BOXGPSRESCUE, + // RCMODE flags + BOXANTIGRAVITY, BOXHEADADJ, BOXCAMSTAB, - BOXCAMTRIG, - BOXGPSHOME, - BOXGPSHOLD, - BOXPASSTHRU, BOXBEEPERON, - BOXLEDMAX, BOXLEDLOW, - BOXLLIGHTS, BOXCALIB, - BOXGOV, BOXOSD, BOXTELEMETRY, - BOXGTUNE, - BOXSONAR, BOXSERVO1, BOXSERVO2, BOXSERVO3, BOXBLACKBOX, - BOXFAILSAFE, BOXAIRMODE, - BOX3DDISABLE, + BOX3D, BOXFPVANGLEMIX, BOXBLACKBOXERASE, BOXCAMERA1, BOXCAMERA2, BOXCAMERA3, - BOXDSHOTREVERSE, + BOXFLIPOVERAFTERCRASH, BOXPREARM, + BOXBEEPGPSCOUNT, + BOXVTXPITMODE, + BOXPARALYZE, + BOXUSER1, + BOXUSER2, + BOXUSER3, + BOXUSER4, + BOXPIDAUDIO, + BOXACROTRAINER, + BOXVTXCONTROLDISABLE, + BOXLAUNCHCONTROL, CHECKBOX_ITEM_COUNT } boxId_e; @@ -534,6 +546,7 @@ typedef struct controlRateConfig_s { uint8_t rcRates[3]; uint8_t rcExpo[3]; uint8_t rates[3]; + uint16_t rate_limit[3]; uint8_t dynThrPID; uint16_t tpa_breakpoint; // Breakpoint where TPA is activated uint8_t throttle_limit_type; // Sets the throttle limiting type - off, scale or clip @@ -590,7 +603,11 @@ typedef struct pidProfile_s { uint16_t yawRateAccelLimit; // yaw accel limiter for deg/sec/ms uint16_t rateAccelLimit; // accel limiter roll/pitch deg/sec/ms uint16_t itermLimit; - uint16_t dterm_lowpass2_hz; // Extra PT1 Filter on D in hz + uint16_t dterm_lowpass2_hz; // Extra Filter on D in hz + uint8_t dterm_filter2_type; // Extra Filter on D type + uint8_t ff_boost; + uint16_t dyn_lpf_dterm_min_hz; + uint16_t dyn_lpf_dterm_max_hz; } pidProfile_t; PG_DECLARE_ARRAY(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles); @@ -683,8 +700,10 @@ typedef enum { DEBUG_COUNT } debugType_e; +extern float rcCommand[4]; extern uint8_t debugMode; extern int16_t debug[DEBUG16_VALUE_COUNT]; +extern uint8_t activePidLoopDenom; /* DEBUG END */ /* SENSOR START */ @@ -702,10 +721,10 @@ typedef enum { typedef struct gyro_s { - uint32_t targetLooptime; + //uint32_t targetLooptime; + uint32_t sampleLooptime; float gyroADCf[XYZ_AXIS_COUNT]; } gyro_t; - extern gyro_t gyro; typedef struct accDev_s { @@ -716,9 +735,18 @@ typedef struct acc_s { accDev_t dev; int32_t accADC[XYZ_AXIS_COUNT]; } acc_t; - extern acc_t acc; +typedef struct mag_s { + float magADC[XYZ_AXIS_COUNT]; +} mag_t; +extern mag_t mag; + +typedef struct baro_s { + int32_t BaroAlt; +} baro_t; +extern baro_t baro; + typedef enum { VOLTAGE_METER_NONE = 0, VOLTAGE_METER_ADC, @@ -797,12 +825,13 @@ typedef struct gyroConfig_s { uint16_t gyro_lowpass_hz; uint8_t gyro_lowpass2_type; uint16_t gyro_lowpass2_hz; - bool gyro_use_32khz; uint8_t gyro_to_use; uint16_t gyro_soft_notch_hz_1; uint16_t gyro_soft_notch_cutoff_1; uint16_t gyro_soft_notch_hz_2; uint16_t gyro_soft_notch_cutoff_2; + uint16_t dyn_lpf_gyro_min_hz; + uint16_t dyn_lpf_gyro_max_hz; } gyroConfig_t; PG_DECLARE(gyroConfig_t, gyroConfig); @@ -832,11 +861,8 @@ typedef struct rxConfig_s { PG_DECLARE(rxConfig_t, rxConfig); -extern float rcCommand[4]; uint16_t getRssi(void); -/* RX START */ - /* FAILSAFE START */ typedef enum { FAILSAFE_IDLE = 0, @@ -850,4 +876,7 @@ typedef enum { failsafePhase_e failsafePhase(); bool rxIsReceivingSignal(void); bool rxAreFlightChannelsValid(void); +float pidGetPreviousSetpoint(int axis); +float mixerGetThrottle(void); +bool isRssiConfigured(void); /* FAILSAFE END */ diff --git a/test/fc/test_fc.cpp b/test/fc/test_fc.cpp index 043696b0..35f2b198 100644 --- a/test/fc/test_fc.cpp +++ b/test/fc/test_fc.cpp @@ -22,6 +22,8 @@ void test_timer_rate_100hz() timer.setRate(100); TEST_ASSERT_EQUAL_UINT32(100, timer.rate); TEST_ASSERT_EQUAL_UINT32(10000, timer.interval); + TEST_ASSERT_EQUAL_UINT32(10000, timer.delta); + TEST_ASSERT_FLOAT_WITHIN(0.000001f, 1.f / 100.f, timer.intervalf); } void test_timer_rate_100hz_div2() @@ -30,6 +32,8 @@ void test_timer_rate_100hz_div2() timer.setRate(100, 2); TEST_ASSERT_EQUAL_UINT32(50, timer.rate); TEST_ASSERT_EQUAL_UINT32(20000, timer.interval); + TEST_ASSERT_EQUAL_UINT32(20000, timer.delta); + TEST_ASSERT_FLOAT_WITHIN(0.000001f, 1.f / 50.f, timer.intervalf); } void test_timer_interval_10ms() @@ -38,6 +42,8 @@ void test_timer_interval_10ms() timer.setInterval(10000); TEST_ASSERT_EQUAL_UINT32(100, timer.rate); TEST_ASSERT_EQUAL_UINT32(10000, timer.interval); + TEST_ASSERT_EQUAL_UINT32(10000, timer.delta); + TEST_ASSERT_FLOAT_WITHIN(0.000001f, 1.f / 100.f, timer.intervalf); } void test_timer_check() @@ -50,21 +56,27 @@ void test_timer_check() TEST_ASSERT_TRUE( timer.check(1000)); TEST_ASSERT_EQUAL_UINT32(1, timer.iteration); + TEST_ASSERT_EQUAL_UINT32(1000, timer.delta); TEST_ASSERT_FALSE(timer.check(1500)); TEST_ASSERT_EQUAL_UINT32(1, timer.iteration); + TEST_ASSERT_EQUAL_UINT32(1000, timer.delta); TEST_ASSERT_TRUE( timer.check(2000)); TEST_ASSERT_EQUAL_UINT32(2, timer.iteration); + TEST_ASSERT_EQUAL_UINT32(1000, timer.delta); TEST_ASSERT_TRUE( timer.check(3000)); TEST_ASSERT_EQUAL_UINT32(3, timer.iteration); + TEST_ASSERT_EQUAL_UINT32(1000, timer.delta); TEST_ASSERT_FALSE(timer.check(3999)); TEST_ASSERT_EQUAL_UINT32(3, timer.iteration); + TEST_ASSERT_EQUAL_UINT32(1000, timer.delta); TEST_ASSERT_TRUE( timer.check(4050)); TEST_ASSERT_EQUAL_UINT32(4, timer.iteration); + TEST_ASSERT_EQUAL_UINT32(1050, timer.delta); } void test_timer_check_micros() @@ -143,19 +155,19 @@ void test_model_inner_pid_init() TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.1835f, model.state.innerPid[PID_ROLL].Kp); TEST_ASSERT_FLOAT_WITHIN(0.0001f, 1.4002f, model.state.innerPid[PID_ROLL].Ki); TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0030f, model.state.innerPid[PID_ROLL].Kd); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0788f, model.state.innerPid[PID_ROLL].Kf); + TEST_ASSERT_FLOAT_WITHIN(0.000001f, 0.000788f, model.state.innerPid[PID_ROLL].Kf); TEST_ASSERT_FLOAT_WITHIN( 0.1f, 1000.0f, model.state.innerPid[PID_PITCH].rate); TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.1835f, model.state.innerPid[PID_PITCH].Kp); TEST_ASSERT_FLOAT_WITHIN(0.0001f, 1.4002f, model.state.innerPid[PID_PITCH].Ki); TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0030f, model.state.innerPid[PID_PITCH].Kd); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0788f, model.state.innerPid[PID_PITCH].Kf); + TEST_ASSERT_FLOAT_WITHIN(0.000001f, 0.000788f, model.state.innerPid[PID_PITCH].Kf); TEST_ASSERT_FLOAT_WITHIN( 0.1f, 1000.0f, model.state.innerPid[PID_YAW].rate); TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.1835f, model.state.innerPid[PID_YAW].Kp); TEST_ASSERT_FLOAT_WITHIN(0.0001f, 1.4002f, model.state.innerPid[PID_YAW].Ki); TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0030f, model.state.innerPid[PID_YAW].Kd); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0788f, model.state.innerPid[PID_YAW].Kf); + TEST_ASSERT_FLOAT_WITHIN(0.000001f, 0.000788f, model.state.innerPid[PID_YAW].Kf); } void test_model_outer_pid_init() @@ -212,19 +224,19 @@ void test_controller_rates() TEST_ASSERT_FLOAT_WITHIN(0.01f, 2.04f, controller.calculateSetpointRate(AXIS_ROLL, 0.5f)); TEST_ASSERT_FLOAT_WITHIN(0.01f, 4.58f, controller.calculateSetpointRate(AXIS_ROLL, 0.75f)); TEST_ASSERT_FLOAT_WITHIN(0.01f, 6.49f, controller.calculateSetpointRate(AXIS_ROLL, 0.85f)); - TEST_ASSERT_FLOAT_WITHIN(0.01f, 12.22f, controller.calculateSetpointRate(AXIS_ROLL, 1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.01f, 22.40f, controller.calculateSetpointRate(AXIS_ROLL, 1.1f)); // !!! + TEST_ASSERT_FLOAT_WITHIN(0.01f, 11.92f, controller.calculateSetpointRate(AXIS_ROLL, 1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 11.92f, controller.calculateSetpointRate(AXIS_ROLL, 1.1f)); TEST_ASSERT_FLOAT_WITHIN(0.01f, 0.0f, controller.calculateSetpointRate(AXIS_PITCH, 0.0f)); TEST_ASSERT_FLOAT_WITHIN(0.01f, -2.04f, controller.calculateSetpointRate(AXIS_PITCH, -0.5f)); - TEST_ASSERT_FLOAT_WITHIN(0.01f, -12.22f, controller.calculateSetpointRate(AXIS_PITCH, -1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.01f, -11.92f, controller.calculateSetpointRate(AXIS_PITCH, -1.0f)); TEST_ASSERT_FLOAT_WITHIN(0.01f, 2.04f, controller.calculateSetpointRate(AXIS_PITCH, 0.5f)); - TEST_ASSERT_FLOAT_WITHIN(0.01f, 12.22f, controller.calculateSetpointRate(AXIS_PITCH, 1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 11.92f, controller.calculateSetpointRate(AXIS_PITCH, 1.0f)); TEST_ASSERT_FLOAT_WITHIN(0.01f, 0.0f, controller.calculateSetpointRate(AXIS_YAW, 0.0f)); TEST_ASSERT_FLOAT_WITHIN(0.01f, -1.48f, controller.calculateSetpointRate(AXIS_YAW, 0.3f)); TEST_ASSERT_FLOAT_WITHIN(0.01f, -3.59f, controller.calculateSetpointRate(AXIS_YAW, 0.6f)); - TEST_ASSERT_FLOAT_WITHIN(0.01f, -8.38f, controller.calculateSetpointRate(AXIS_YAW, 1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.01f, -8.29f, controller.calculateSetpointRate(AXIS_YAW, 1.0f)); } int main(int argc, char **argv) diff --git a/test/math/test_math.cpp b/test/math/test_math.cpp index a4c2287d..e52e7f41 100644 --- a/test/math/test_math.cpp +++ b/test/math/test_math.cpp @@ -432,8 +432,8 @@ void test_pid_update_f() TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.pTerm); TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.iTerm); TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.dTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.5f, pid.fTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.5f, result1); + TEST_ASSERT_FLOAT_WITHIN(0.001f, -0.5f, pid.fTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, -0.5f, result1); float result2 = pid.update(0.0f, 0.0f); @@ -442,15 +442,15 @@ void test_pid_update_f() TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.pTerm); TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.iTerm); TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.dTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, -0.5f, pid.fTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, -0.5f, result2); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.5f, pid.fTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.5f, result2); } void test_pid_update_sum() { Pid pid; ensure(pid); - gain(pid, 1, 100, 0.1f, 0.1f); + gain(pid, 1, 100, 0.1f, 0.01f); pid.begin(); float result = pid.update(0.1f, 0.f); @@ -460,25 +460,25 @@ void test_pid_update_sum() TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.1f, pid.pTerm); TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.1f, pid.iTerm); TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.dTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, -1.0f, pid.fTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, -0.8f, result); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.1f, pid.fTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.3f, result); } void test_pid_update_sum_limit() { Pid pid; ensure(pid); - gain(pid, 3, 100, 0.01f, 0.01f); + gain(pid, 1, 100, 0.01f, 0.01f); pid.begin(); float result = pid.update(0.5f, 0.f); TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.5f, pid.error); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.5f, pid.pTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.5f, pid.pTerm); TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.2f, pid.iTerm); TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.dTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, -0.5f, pid.fTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.5f, pid.fTerm); TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.0f, result); }