Skip to content

Commit

Permalink
gyro accel state refactor
Browse files Browse the repository at this point in the history
  • Loading branch information
@rtlopez
rtlopez committed Oct 29, 2024
1 parent 7b183ad commit eefcb15
Show file tree
Hide file tree
Showing 19 changed files with 267 additions and 262 deletions.
4 changes: 2 additions & 2 deletions lib/Espfc/src/Actuator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -98,7 +98,7 @@ class Actuator
int errors = _model.state.i2cErrorDelta;
_model.state.i2cErrorDelta = 0;

_model.setArmingDisabled(ARMING_DISABLED_NO_GYRO, !_model.state.gyroPresent || errors);
_model.setArmingDisabled(ARMING_DISABLED_NO_GYRO, !_model.state.gyro.present || errors);
_model.setArmingDisabled(ARMING_DISABLED_FAILSAFE, _model.state.failsafe.phase != FC_FAILSAFE_IDLE);
_model.setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE, _model.state.input.rxLoss || _model.state.input.rxFailSafe);
_model.setArmingDisabled(ARMING_DISABLED_THROTTLE, !_model.isThrottleLow());
Expand Down Expand Up @@ -224,7 +224,7 @@ class Actuator
if(_model.config.gyro.dynLpfFilter.cutoff > 0) {
int gyroFreq = Math::map(scale, 1000, 2000, _model.config.gyro.dynLpfFilter.cutoff, _model.config.gyro.dynLpfFilter.freq);
for(size_t i = 0; i < AXIS_COUNT_RPY; i++) {
_model.state.gyroFilter[i].reconfigure(gyroFreq);
_model.state.gyro.filter[i].reconfigure(gyroFreq);
}
}
if(_model.config.dterm.dynLpfFilter.cutoff > 0) {
Expand Down
8 changes: 4 additions & 4 deletions lib/Espfc/src/Blackbox/Blackbox.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -152,7 +152,7 @@ int Blackbox::begin()
if(_model.magActive()) sensorsSet(SENSOR_MAG);
if(_model.baroActive()) sensorsSet(SENSOR_BARO);

gyro.sampleLooptime = _model.state.gyroTimer.interval;
gyro.sampleLooptime = _model.state.gyro.timer.interval;
targetPidLooptime = _model.state.loopTimer.interval;
activePidLoopDenom = _model.config.loopSync;

Expand Down Expand Up @@ -237,15 +237,15 @@ void FAST_CODE_ATTR Blackbox::updateData()
{
for(size_t i = 0; i < 3; i++)
{
gyro.gyroADCf[i] = degrees(_model.state.gyro[i]);
gyro.gyroADC[i] = degrees(_model.state.gyroScaled[i]);
gyro.gyroADCf[i] = Math::toDeg(_model.state.gyro.adc[i]);
gyro.gyroADC[i] = Math::toDeg(_model.state.gyro.scaled[i]);
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.buffer[i] - 1500) * (i == AXIS_YAW ? -1 : 1);
if(_model.accelActive()) {
acc.accADC[i] = _model.state.accel[i] * ACCEL_G_INV * acc.dev.acc_1G;
acc.accADC[i] = _model.state.accel.adc[i] * ACCEL_G_INV * acc.dev.acc_1G;
}
if(_model.magActive()) {
mag.magADC[i] = _model.state.mag.adc[i] * 1090;
Expand Down
24 changes: 12 additions & 12 deletions lib/Espfc/src/Cli.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1042,17 +1042,17 @@ class Cli
s.print(_model.config.gyro.bias[0]); s.print(' ');
s.print(_model.config.gyro.bias[1]); s.print(' ');
s.print(_model.config.gyro.bias[2]); s.print(F(" ["));
s.print(Math::toDeg(_model.state.gyroBias[0])); s.print(' ');
s.print(Math::toDeg(_model.state.gyroBias[1])); s.print(' ');
s.print(Math::toDeg(_model.state.gyroBias[2])); s.println(F("]"));
s.print(Math::toDeg(_model.state.gyro.bias[0])); s.print(' ');
s.print(Math::toDeg(_model.state.gyro.bias[1])); s.print(' ');
s.print(Math::toDeg(_model.state.gyro.bias[2])); s.println(F("]"));

s.print(F("accel offset: "));
s.print(_model.config.accel.bias[0]); s.print(' ');
s.print(_model.config.accel.bias[1]); s.print(' ');
s.print(_model.config.accel.bias[2]); s.print(F(" ["));
s.print(_model.state.accelBias[0]); s.print(' ');
s.print(_model.state.accelBias[1]); s.print(' ');
s.print(_model.state.accelBias[2]); s.println(F("]"));
s.print(_model.state.accel.bias[0]); s.print(' ');
s.print(_model.state.accel.bias[1]); s.print(' ');
s.print(_model.state.accel.bias[2]); s.println(F("]"));

s.print(F(" mag offset: "));
s.print(_model.config.mag.offset[0]); s.print(' ');
Expand Down Expand Up @@ -1082,12 +1082,12 @@ class Cli
}
else if(strcmp_P(cmd.args[1], PSTR("reset_accel")) == 0 || strcmp_P(cmd.args[1], PSTR("reset_all")) == 0)
{
_model.state.accelBias = VectorFloat();
_model.state.accel.bias = VectorFloat();
s.println(F("OK"));
}
else if(strcmp_P(cmd.args[1], PSTR("reset_gyro")) == 0 || strcmp_P(cmd.args[1], PSTR("reset_all")) == 0)
{
_model.state.gyroBias = VectorFloat();
_model.state.gyro.bias = VectorFloat();
s.println(F("OK"));
}
else if(strcmp_P(cmd.args[1], PSTR("reset_mag")) == 0 || strcmp_P(cmd.args[1], PSTR("reset_all")) == 0)
Expand Down Expand Up @@ -1239,7 +1239,7 @@ class Cli
printStats(s);
s.println();

Device::GyroDevice * gyro = _model.state.gyroDev;
Device::GyroDevice * gyro = _model.state.gyro.dev;
Device::BaroDevice * baro = _model.state.baro.dev;
Device::MagDevice * mag = _model.state.mag.dev;
s.print(F(" devices: "));
Expand Down Expand Up @@ -1519,11 +1519,11 @@ class Cli
s.println(F(" MHz"));

s.print(F(" gyro clock: "));
s.print(_model.state.gyroClock);
s.print(_model.state.gyro.clock);
s.println(F(" Hz"));

s.print(F(" gyro rate: "));
s.print(_model.state.gyroTimer.rate);
s.print(_model.state.gyro.timer.rate);
s.println(F(" Hz"));

s.print(F(" loop rate: "));
Expand All @@ -1535,7 +1535,7 @@ class Cli
s.println(F(" Hz"));

s.print(F(" accel rate: "));
s.print(_model.state.accelTimer.rate);
s.print(_model.state.accel.timer.rate);
s.println(F(" Hz"));

s.print(F(" baro rate: "));
Expand Down
12 changes: 6 additions & 6 deletions lib/Espfc/src/Controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,7 @@ void Controller::outerLoopRobot()
{
const float speedScale = 2.f;
const float gyroScale = 0.1f;
const float speed = _speedFilter.update(_model.state.output.ch[AXIS_PITCH] * speedScale + _model.state.gyro[AXIS_PITCH] * gyroScale);
const float speed = _speedFilter.update(_model.state.output.ch[AXIS_PITCH] * speedScale + _model.state.gyro.adc[AXIS_PITCH] * gyroScale);
float angle = 0;

if(true || _model.isActive(MODE_ANGLE))
Expand All @@ -75,7 +75,7 @@ void Controller::outerLoopRobot()
if(_model.config.debug.mode == DEBUG_ANGLERATE)
{
_model.state.debug[0] = speed * 1000;
_model.state.debug[1] = lrintf(degrees(angle) * 10);
_model.state.debug[1] = lrintf(Math::toDeg(angle) * 10);
}
}

Expand All @@ -90,7 +90,7 @@ void Controller::innerLoopRobot()
if(stabilize)
{
_model.state.output.ch[AXIS_PITCH] = _model.state.innerPid[AXIS_PITCH].update(_model.state.desiredAngle[AXIS_PITCH], _model.state.angle[AXIS_PITCH]);
_model.state.output.ch[AXIS_YAW] = _model.state.innerPid[AXIS_YAW].update(_model.state.desiredRate[AXIS_YAW], _model.state.gyro[AXIS_YAW]);
_model.state.output.ch[AXIS_YAW] = _model.state.innerPid[AXIS_YAW].update(_model.state.desiredRate[AXIS_YAW], _model.state.gyro.adc[AXIS_YAW]);
}
else
{
Expand All @@ -101,7 +101,7 @@ void Controller::innerLoopRobot()

if(_model.config.debug.mode == DEBUG_ANGLERATE)
{
_model.state.debug[2] = lrintf(degrees(_model.state.angle[AXIS_PITCH]) * 10);
_model.state.debug[2] = lrintf(Math::toDeg(_model.state.angle[AXIS_PITCH]) * 10);
_model.state.debug[3] = lrintf(_model.state.output.ch[AXIS_PITCH] * 1000);
}
}
Expand Down Expand Up @@ -133,7 +133,7 @@ void FAST_CODE_ATTR Controller::outerLoop()
{
for(size_t i = 0; i < 3; ++i)
{
_model.state.debug[i] = lrintf(degrees(_model.state.desiredRate[i]));
_model.state.debug[i] = lrintf(Math::toDeg(_model.state.desiredRate[i]));
}
}
}
Expand All @@ -143,7 +143,7 @@ void FAST_CODE_ATTR Controller::innerLoop()
const float tpaFactor = getTpaFactor();
for(size_t i = 0; i < AXIS_COUNT_RPY; ++i)
{
_model.state.output.ch[i] = _model.state.innerPid[i].update(_model.state.desiredRate[i], _model.state.gyro[i]) * tpaFactor;
_model.state.output.ch[i] = _model.state.innerPid[i].update(_model.state.desiredRate[i], _model.state.gyro.adc[i]) * tpaFactor;
//_model.state.debug[i] = lrintf(_model.state.innerPid[i].fTerm * 1000);
}
_model.state.output.ch[AXIS_THRUST] = _model.state.desiredRate[AXIS_THRUST];
Expand Down
10 changes: 5 additions & 5 deletions lib/Espfc/src/Espfc.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,18 +38,18 @@ int FAST_CODE_ATTR Espfc::update(bool externalTrigger)
{
if(externalTrigger)
{
_model.state.gyroTimer.update();
_model.state.gyro.timer.update();
}
else
{
if(!_model.state.gyroTimer.check()) return 0;
if(!_model.state.gyro.timer.check()) return 0;
}
Stats::Measure measure(_model.state.stats, COUNTER_CPU_0);

#if defined(ESPFC_MULTI_CORE)

_sensor.read();
if(_model.state.input.timer.syncTo(_model.state.gyroTimer, 1u))
if(_model.state.input.timer.syncTo(_model.state.gyro.timer, 1u))
{
_input.update();
}
Expand All @@ -65,15 +65,15 @@ int FAST_CODE_ATTR Espfc::update(bool externalTrigger)
#else

_sensor.update();
if(_model.state.loopTimer.syncTo(_model.state.gyroTimer))
if(_model.state.loopTimer.syncTo(_model.state.gyro.timer))
{
_controller.update();
if(_model.state.mixer.timer.syncTo(_model.state.loopTimer))
{
_mixer.update();
}
_blackbox.update();
if(_model.state.input.timer.syncTo(_model.state.gyroTimer, 1u))
if(_model.state.input.timer.syncTo(_model.state.gyro.timer, 1u))
{
_input.update();
}
Expand Down
2 changes: 1 addition & 1 deletion lib/Espfc/src/Espfc.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,7 @@ class Espfc

int getGyroInterval() const
{
return _model.state.gyroTimer.interval;
return _model.state.gyro.timer.interval;
}

private:
Expand Down
52 changes: 26 additions & 26 deletions lib/Espfc/src/Fusion.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,10 +9,10 @@ int Fusion::begin()
{
_model.state.gyroPoseQ = Quaternion();

_madgwick.begin(_model.state.accelTimer.rate);
_madgwick.begin(_model.state.accel.timer.rate);
_madgwick.setKp(_model.config.fusion.gain * 0.05f);

_mahony.begin(_model.state.accelTimer.rate);
_mahony.begin(_model.state.accel.timer.rate);
_mahony.setKp(_model.config.fusion.gain * 0.1f);
_mahony.setKi(_model.config.fusion.gainI * 0.1f);

Expand Down Expand Up @@ -72,39 +72,39 @@ int FAST_CODE_ATTR Fusion::update()

if(_model.config.debug.mode == DEBUG_ALTITUDE)
{
_model.state.debug[0] = lrintf(degrees(_model.state.angle[0]) * 10);
_model.state.debug[1] = lrintf(degrees(_model.state.angle[1]) * 10);
_model.state.debug[2] = lrintf(degrees(_model.state.angle[2]) * 10);
_model.state.debug[0] = lrintf(Math::toDeg(_model.state.angle[0]) * 10);
_model.state.debug[1] = lrintf(Math::toDeg(_model.state.angle[1]) * 10);
_model.state.debug[2] = lrintf(Math::toDeg(_model.state.angle[2]) * 10);
}
return 1;
}

void Fusion::experimentalFusion()
{
// Experiment: workaround for 90 deg limit on pitch[y] axis
Quaternion r = Quaternion::lerp(Quaternion(), _model.state.accel.accelToQuaternion(), 0.5);
Quaternion r = Quaternion::lerp(Quaternion(), _model.state.accel.adc.accelToQuaternion(), 0.5);
_model.state.angle.eulerFromQuaternion(r);
_model.state.angle *= 2.f;
}

void Fusion::simpleFusion()
{
_model.state.pose = _model.state.accel.accelToEuler();
_model.state.pose = _model.state.accel.adc.accelToEuler();
_model.state.angle.x = _model.state.pose.x;
_model.state.angle.y = _model.state.pose.y;
_model.state.angle.z += _model.state.gyroTimer.intervalf * _model.state.gyro.z;
_model.state.angle.z += _model.state.gyro.timer.intervalf * _model.state.gyro.adc.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;
}

void Fusion::kalmanFusion()
{
_model.state.pose = _model.state.accel.accelToEuler();
_model.state.pose = _model.state.accel.adc.accelToEuler();
_model.state.pose.z = _model.state.angle.z;
const float dt = _model.state.gyroTimer.intervalf;
const float dt = _model.state.gyro.timer.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);
float angle = _model.state.kalman[i].getAngle(_model.state.pose.get(i), _model.state.gyro.adc.get(i), dt);
_model.state.angle.set(i, angle);
//_model.state.rate.set(i, _model.state.kalman[i].getRate());
}
Expand All @@ -113,13 +113,13 @@ void Fusion::kalmanFusion()

void Fusion::complementaryFusion()
{
_model.state.pose = _model.state.accel.accelToEuler();
_model.state.pose = _model.state.accel.adc.accelToEuler();
_model.state.pose.z = _model.state.angle.z;
const float dt = _model.state.gyroTimer.intervalf;
const float dt = _model.state.gyro.timer.intervalf;
const float alpha = 0.002f;
for(size_t i = 0; i < 3; i++)
{
float angle = (_model.state.angle[i] + _model.state.gyro[i] * dt) * (1.f - alpha) + _model.state.pose[i] * alpha;
float angle = (_model.state.angle[i] + _model.state.gyro.adc[i] * dt) * (1.f - alpha) + _model.state.pose[i] * alpha;
if(angle > PI) angle -= TWO_PI;
if(angle < -PI) angle += TWO_PI;
_model.state.angle.set(i, angle);
Expand All @@ -131,10 +131,10 @@ void Fusion::complementaryFusion()
void Fusion::complementaryFusionOld()
{
const float alpha = 0.01f;
const float dt = _model.state.gyroTimer.intervalf;
_model.state.pose = _model.state.accel.accelToEuler();
const float dt = _model.state.gyro.timer.intervalf;
_model.state.pose = _model.state.accel.adc.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;
_model.state.angle = (_model.state.angle + _model.state.gyro.adc * dt) * (1.f - alpha) + _model.state.pose * alpha;
_model.state.angleQ = _model.state.angle.eulerToQuaternion();
}

Expand All @@ -149,10 +149,10 @@ void Fusion::rtqfFusion()
return;
}

float timeDelta = _model.state.gyroTimer.intervalf;
float timeDelta = _model.state.gyro.timer.intervalf;
Quaternion measuredQPose = _model.state.poseQ;
Quaternion fusionQPose = _model.state.angleQ;
VectorFloat fusionGyro = _model.state.gyro;
VectorFloat fusionGyro = _model.state.gyro.adc;

float qs, qx, qy, qz;
qs = fusionQPose.w;
Expand Down Expand Up @@ -200,7 +200,7 @@ void Fusion::rtqfFusion()

void Fusion::updatePoseFromAccelMag()
{
_model.state.pose = _model.state.accel.accelToEuler();
_model.state.pose = _model.state.accel.adc.accelToEuler();
//_model.state.accelPose = _model.state.pose;

if(_model.magActive())
Expand Down Expand Up @@ -259,7 +259,7 @@ void Fusion::lerpFusion()
{
float correctionAlpha = 0.001f; // 0 - 1 => gyro - accel

_model.state.accelPose = _model.state.accel.accelToEuler();
_model.state.accelPose = _model.state.accel.adc.accelToEuler();
_model.state.accelPoseQ = _model.state.accelPose.eulerToQuaternion();

if(_model.magActive())
Expand All @@ -277,7 +277,7 @@ void Fusion::lerpFusion()
//_model.state.accelPose.eulerFromQuaternion(_model.state.accelPoseQ);

// predict new state
Quaternion rotation = (_model.state.gyro * _model.state.gyroTimer.intervalf).eulerToQuaternion();
Quaternion rotation = (_model.state.gyro.adc * _model.state.gyro.timer.intervalf).eulerToQuaternion();
_model.state.gyroPoseQ = (_model.state.gyroPoseQ * rotation).getNormalized();

// drift compensation
Expand All @@ -293,15 +293,15 @@ void Fusion::madgwickFusion()
{
_madgwick.update(
_model.state.gyroImu.x, _model.state.gyroImu.y, _model.state.gyroImu.z,
_model.state.accel.x, _model.state.accel.y, _model.state.accel.z,
_model.state.accel.adc.x, _model.state.accel.adc.y, _model.state.accel.adc.z,
_model.state.mag.adc.x, _model.state.mag.adc.y, _model.state.mag.adc.z
);
}
else
{
_madgwick.update(
_model.state.gyroImu.x, _model.state.gyroImu.y, _model.state.gyroImu.z,
_model.state.accel.x, _model.state.accel.y, _model.state.accel.z
_model.state.accel.adc.x, _model.state.accel.adc.y, _model.state.accel.adc.z
);
}
_model.state.angleQ = _madgwick.getQuaternion();
Expand All @@ -314,15 +314,15 @@ void FAST_CODE_ATTR Fusion::mahonyFusion()
{
_mahony.update(
_model.state.gyroImu.x, _model.state.gyroImu.y, _model.state.gyroImu.z,
_model.state.accel.x, _model.state.accel.y, _model.state.accel.z,
_model.state.accel.adc.x, _model.state.accel.adc.y, _model.state.accel.adc.z,
_model.state.mag.adc.x, _model.state.mag.adc.y, _model.state.mag.adc.z
);
}
else
{
_mahony.update(
_model.state.gyroImu.x, _model.state.gyroImu.y, _model.state.gyroImu.z,
_model.state.accel.x, _model.state.accel.y, _model.state.accel.z
_model.state.accel.adc.x, _model.state.accel.adc.y, _model.state.accel.adc.z
);
}
_model.state.angleQ = _mahony.getQuaternion();
Expand Down
Loading

0 comments on commit eefcb15

Please sign in to comment.