Skip to content

Commit

Permalink
mag state refactor
Browse files Browse the repository at this point in the history
  • Loading branch information
@rtlopez
rtlopez committed Oct 28, 2024
1 parent 9c2db28 commit ef9416b
Show file tree
Hide file tree
Showing 10 changed files with 104 additions and 91 deletions.
2 changes: 1 addition & 1 deletion lib/Espfc/src/Blackbox/Blackbox.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -248,7 +248,7 @@ void FAST_CODE_ATTR Blackbox::updateData()
acc.accADC[i] = _model.state.accel[i] * ACCEL_G_INV * acc.dev.acc_1G;
}
if(_model.magActive()) {
mag.magADC[i] = _model.state.mag[i] * 1090;
mag.magADC[i] = _model.state.mag.adc[i] * 1090;
}
if(_model.baroActive()) {
baro.altitude = lrintf(_model.state.baroAltitude * 100.f); // cm
Expand Down
20 changes: 10 additions & 10 deletions lib/Espfc/src/Cli.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1058,17 +1058,17 @@ class Cli
s.print(_model.config.mag.offset[0]); s.print(' ');
s.print(_model.config.mag.offset[1]); s.print(' ');
s.print(_model.config.mag.offset[2]); s.print(F(" ["));
s.print(_model.state.magCalibrationOffset[0]); s.print(' ');
s.print(_model.state.magCalibrationOffset[1]); s.print(' ');
s.print(_model.state.magCalibrationOffset[2]); s.println(F("]"));
s.print(_model.state.mag.calibrationOffset[0]); s.print(' ');
s.print(_model.state.mag.calibrationOffset[1]); s.print(' ');
s.print(_model.state.mag.calibrationOffset[2]); s.println(F("]"));

s.print(F(" mag scale: "));
s.print(_model.config.mag.scale[0]); s.print(' ');
s.print(_model.config.mag.scale[1]); s.print(' ');
s.print(_model.config.mag.scale[2]); s.print(F(" ["));
s.print(_model.state.magCalibrationScale[0]); s.print(' ');
s.print(_model.state.magCalibrationScale[1]); s.print(' ');
s.print(_model.state.magCalibrationScale[2]); s.println(F("]"));
s.print(_model.state.mag.calibrationScale[0]); s.print(' ');
s.print(_model.state.mag.calibrationScale[1]); s.print(' ');
s.print(_model.state.mag.calibrationScale[2]); s.println(F("]"));
}
else if(strcmp_P(cmd.args[1], PSTR("gyro")) == 0)
{
Expand All @@ -1092,8 +1092,8 @@ class Cli
}
else if(strcmp_P(cmd.args[1], PSTR("reset_mag")) == 0 || strcmp_P(cmd.args[1], PSTR("reset_all")) == 0)
{
_model.state.magCalibrationOffset = VectorFloat();
_model.state.magCalibrationScale = VectorFloat(1.f, 1.f, 1.f);
_model.state.mag.calibrationOffset = VectorFloat();
_model.state.mag.calibrationScale = VectorFloat(1.f, 1.f, 1.f);
s.println(F("OK"));
}
}
Expand Down Expand Up @@ -1241,7 +1241,7 @@ class Cli

Device::GyroDevice * gyro = _model.state.gyroDev;
Device::BaroDevice * baro = _model.state.baroDev;
Device::MagDevice * mag = _model.state.magDev;
Device::MagDevice * mag = _model.state.mag.dev;
s.print(F(" devices: "));
if(gyro)
{
Expand Down Expand Up @@ -1543,7 +1543,7 @@ class Cli
s.println(F(" Hz"));

s.print(F(" mag rate: "));
s.print(_model.state.magTimer.rate);
s.print(_model.state.mag.timer.rate);
s.println(F(" Hz"));
}

Expand Down
2 changes: 1 addition & 1 deletion lib/Espfc/src/Device/MagAK8963.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -81,7 +81,7 @@ class MagAK8963: public MagDevice

const VectorFloat convert(const VectorInt16& v) const override
{
return (VectorFloat)v * scale;
return static_cast<VectorFloat>(v) * scale;
}

int getRate() const override
Expand Down
10 changes: 5 additions & 5 deletions lib/Espfc/src/Fusion.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -218,8 +218,8 @@ void Fusion::updatePoseFromAccelMag()
q.y = cosX2 * sinY2;
q.z = -sinX2 * sinY2;

VectorFloat m = _model.state.mag.getRotated(q);
_model.state.magPose = m;
VectorFloat m = _model.state.mag.adc.getRotated(q);
_model.state.mag.pose = m;
_model.state.pose.z = -atan2(m.y, m.x);
}
else
Expand Down Expand Up @@ -265,7 +265,7 @@ void Fusion::lerpFusion()
if(_model.magActive())
{
// use yaw from mag
VectorFloat mag = _model.state.mag.getRotated(_model.state.accelPoseQ);
VectorFloat mag = _model.state.mag.adc.getRotated(_model.state.accelPoseQ);
_model.state.accelPose.z = -atan2(mag.y, mag.x);
}
else
Expand Down Expand Up @@ -294,7 +294,7 @@ 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.mag.x, _model.state.mag.y, _model.state.mag.z
_model.state.mag.adc.x, _model.state.mag.adc.y, _model.state.mag.adc.z
);
}
else
Expand All @@ -315,7 +315,7 @@ 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.mag.x, _model.state.mag.y, _model.state.mag.z
_model.state.mag.adc.x, _model.state.mag.adc.y, _model.state.mag.adc.z
);
}
else
Expand Down
6 changes: 3 additions & 3 deletions lib/Espfc/src/Hardware.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -153,9 +153,9 @@ class Hardware
if(!detectedMag && detectDevice(qmc5883l, gyroSlaveBus)) detectedMag = &qmc5883l;

}
_model.state.magDev = detectedMag;
_model.state.magPresent = (bool)detectedMag;
_model.state.magRate = detectedMag ? detectedMag->getRate() : 0;
_model.state.mag.dev = detectedMag;
_model.state.mag.present = (bool)detectedMag;
_model.state.mag.rate = detectedMag ? detectedMag->getRate() : 0;
}

void detectBaro()
Expand Down
24 changes: 12 additions & 12 deletions lib/Espfc/src/Model.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -119,7 +119,7 @@ class Model

bool magActive() const
{
return state.magPresent && config.mag.dev != MAG_NONE;
return state.mag.present && config.mag.dev != MAG_NONE;
}

bool baroActive() const
Expand All @@ -129,7 +129,7 @@ class Model

bool calibrationActive() const
{
return state.accelCalibrationState != CALIBRATION_IDLE || state.gyroCalibrationState != CALIBRATION_IDLE || state.magCalibrationState != CALIBRATION_IDLE;
return state.accelCalibrationState != CALIBRATION_IDLE || state.gyroCalibrationState != CALIBRATION_IDLE || state.mag.calibrationState != CALIBRATION_IDLE;
}

void calibrateGyro()
Expand All @@ -143,7 +143,7 @@ class Model

void calibrateMag()
{
state.magCalibrationState = CALIBRATION_START;
state.mag.calibrationState = CALIBRATION_START;
}

void finishCalibration()
Expand All @@ -159,11 +159,11 @@ class Model
save();
logger.info().log(F("ACCEL BIAS")).log(state.accelBias.x).log(state.accelBias.y).logln(state.accelBias.z);
}
if(state.magCalibrationState == CALIBRATION_SAVE)
if(state.mag.calibrationState == CALIBRATION_SAVE)
{
save();
logger.info().log(F("MAG BIAS")).log(state.magCalibrationOffset.x).log(state.magCalibrationOffset.y).logln(state.magCalibrationOffset.z);
logger.info().log(F("MAG SCALE")).log(state.magCalibrationScale.x).log(state.magCalibrationScale.y).logln(state.magCalibrationScale.z);
logger.info().log(F("MAG BIAS")).log(state.mag.calibrationOffset.x).log(state.mag.calibrationOffset.y).logln(state.mag.calibrationOffset.z);
logger.info().log(F("MAG SCALE")).log(state.mag.calibrationScale.x).log(state.mag.calibrationScale.y).logln(state.mag.calibrationScale.z);
}
}

Expand Down Expand Up @@ -441,7 +441,7 @@ class Model
state.stats.timer.setRate(3);
if(magActive())
{
state.magTimer.setRate(state.magRate);
state.mag.timer.setRate(state.mag.rate);
}

state.boardAlignment.init(VectorFloat(Math::toRad(config.boardAlignment[0]), Math::toRad(config.boardAlignment[1]), Math::toRad(config.boardAlignment[2])));
Expand Down Expand Up @@ -486,7 +486,7 @@ class Model
}
if(magActive())
{
state.magFilter[i].begin(config.mag.filter, state.magTimer.rate);
state.mag.filter[i].begin(config.mag.filter, state.mag.timer.rate);
}
}

Expand Down Expand Up @@ -576,8 +576,8 @@ class Model
{
state.gyroBias.set(i, config.gyro.bias[i] / 1000.0f);
state.accelBias.set(i, config.accel.bias[i] / 1000.0f);
state.magCalibrationOffset.set(i, config.mag.offset[i] / 10.0f);
state.magCalibrationScale.set(i, config.mag.scale[i] / 1000.0f);
state.mag.calibrationOffset.set(i, config.mag.offset[i] / 10.0f);
state.mag.calibrationScale.set(i, config.mag.scale[i] / 1000.0f);
}
}

Expand All @@ -588,8 +588,8 @@ class Model
{
config.gyro.bias[i] = lrintf(state.gyroBias[i] * 1000.0f);
config.accel.bias[i] = lrintf(state.accelBias[i] * 1000.0f);
config.mag.offset[i] = lrintf(state.magCalibrationOffset[i] * 10.0f);
config.mag.scale[i] = lrintf(state.magCalibrationScale[i] * 1000.0f);
config.mag.offset[i] = lrintf(state.mag.calibrationOffset[i] * 10.0f);
config.mag.scale[i] = lrintf(state.mag.calibrationScale[i] * 1000.0f);
}
}

Expand Down
4 changes: 2 additions & 2 deletions lib/Espfc/src/ModelConfig.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -604,8 +604,8 @@ struct MagConfig
int8_t bus = BUS_AUTO;
int8_t dev = MAG_NONE;
int8_t align = ALIGN_DEFAULT;
int16_t scale[3] = { 0, 0, 0 };
int16_t offset[3] = { 1000, 1000, 1000 };
int16_t offset[3] = { 0, 0, 0 };
int16_t scale[3] = { 1000, 1000, 1000 };
FilterConfig filter{FILTER_BIQUAD, 10};
};

Expand Down
43 changes: 24 additions & 19 deletions lib/Espfc/src/ModelState.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -208,11 +208,34 @@ struct MixerState
EscDriver * escServo;
};

struct MagState
{
Device::MagDevice* dev;
bool present;
int rate;

VectorInt16 raw;
VectorFloat adc;
Filter filter[3];
Timer timer;

int calibrationSamples;
int calibrationState;
bool calibrationValid;
VectorFloat calibrationMin;
VectorFloat calibrationMax;
VectorFloat calibrationScale;
VectorFloat calibrationOffset;

VectorFloat pose;
};

// working data
struct ModelState
{
MagState mag;

Device::GyroDevice* gyroDev;
Device::MagDevice* magDev;
Device::BaroDevice* baroDev;

VectorInt16 gyroRaw;
Expand All @@ -222,18 +245,15 @@ struct ModelState
VectorFloat gyroImu;

VectorInt16 accelRaw;
VectorInt16 magRaw;

VectorFloat gyro;
VectorFloat accel;
VectorFloat mag;

VectorFloat gyroPose;
Quaternion gyroPoseQ;
VectorFloat accelPose;
VectorFloat accelPose2;
Quaternion accelPoseQ;
VectorFloat magPose;

bool imuUpdate;
bool loopUpdate;
Expand All @@ -254,7 +274,6 @@ struct ModelState
Filter gyroImuFilter[3];

Filter accelFilter[3];
Filter magFilter[3];
Filter rpmFreqFilter[RPM_FILTER_MOTOR_MAX];
Filter rpmFilter[RPM_FILTER_MOTOR_MAX][RPM_FILTER_HARMONICS_MAX][3];

Expand Down Expand Up @@ -304,19 +323,6 @@ struct ModelState
Timer loopTimer;

Timer actuatorTimer;

Timer magTimer;
int magRate;

int magCalibrationSamples;
int magCalibrationState;
bool magCalibrationValid;

VectorFloat magCalibrationMin;
VectorFloat magCalibrationMax;
VectorFloat magCalibrationScale;
VectorFloat magCalibrationOffset;

Timer telemetryTimer;

Stats stats;
Expand All @@ -343,7 +349,6 @@ struct ModelState

bool gyroPresent;
bool accelPresent;
bool magPresent;
bool baroPresent;

float baroTemperatureRaw;
Expand Down
2 changes: 1 addition & 1 deletion lib/Espfc/src/Msp/MspProcessor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1287,7 +1287,7 @@ class MspProcessor
}
for (int i = 0; i < 3; i++)
{
r.writeU16(lrintf(_model.state.mag[i] * 1090));
r.writeU16(lrintf(_model.state.mag.adc[i] * 1090));
}
break;

Expand Down
Loading

0 comments on commit ef9416b

Please sign in to comment.