Skip to content

Commit

Permalink
Merge pull request #12 from rtlopez/blackbox-42
Browse files Browse the repository at this point in the history 
blackbox 4.2 format
  • Loading branch information
@rtlopez
rtlopez authored Jun 26, 2020
2 parents c95cdf9 + 964e9dd commit cba182c
Show file tree
Hide file tree
Showing 24 changed files with 408 additions and 159 deletions.
47 changes: 43 additions & 4 deletions lib/Espfc/src/Blackbox.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
#include "Model.h"
#include "Hardware.h"
#include "EscDriver.h"
#include "Math/Utils.h"

extern "C" {
#include "blackbox/blackbox.h"
Expand Down Expand Up @@ -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;

Expand All @@ -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 {

Expand Down Expand Up @@ -95,28 +110,38 @@ 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++)
{
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;
Expand All @@ -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;
Expand Down Expand Up @@ -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;

Expand Down Expand Up @@ -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];
}
}
}

Expand Down
18 changes: 12 additions & 6 deletions lib/Espfc/src/Controller.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
{
Expand All @@ -136,15 +140,15 @@ 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];
}

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

Expand All @@ -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;
Expand All @@ -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:
Expand Down
12 changes: 6 additions & 6 deletions lib/Espfc/src/Fusion.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand All @@ -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);
Expand All @@ -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++)
{
Expand All @@ -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;
Expand All @@ -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;
Expand Down Expand Up @@ -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
Expand Down
77 changes: 42 additions & 35 deletions lib/Espfc/src/Input.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

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

{
Expand All @@ -64,56 +68,58 @@ 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;
}
}
}

{
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)
Expand Down Expand Up @@ -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));
}

Expand All @@ -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];
};

}
Expand Down
Loading

0 comments on commit cba182c

Please sign in to comment.