Skip to content

Commit

Permalink
attitude state refactor
Browse files Browse the repository at this point in the history
  • Loading branch information
@rtlopez
rtlopez committed Oct 29, 2024
1 parent eefcb15 commit d947406
Show file tree
Hide file tree
Showing 7 changed files with 63 additions and 61 deletions.
14 changes: 7 additions & 7 deletions lib/Espfc/src/Controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -82,14 +82,14 @@ void Controller::outerLoopRobot()
void Controller::innerLoopRobot()
{
//VectorFloat v(0.f, 0.f, 1.f);
//v.rotate(_model.state.angleQ);
//v.rotate(_model.state.attitude.quaternion);
//const float angle = acos(v.z);
const float angle = std::max(abs(_model.state.angle[AXIS_PITCH]), abs(_model.state.angle[AXIS_ROLL]));
const float angle = std::max(abs(_model.state.attitude.euler[AXIS_PITCH]), abs(_model.state.attitude.euler[AXIS_ROLL]));

const bool stabilize = angle < Math::toRad(_model.config.level.angleLimit);
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_PITCH] = _model.state.innerPid[AXIS_PITCH].update(_model.state.desiredAngle[AXIS_PITCH], _model.state.attitude.euler[AXIS_PITCH]);
_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(Math::toDeg(_model.state.angle[AXIS_PITCH]) * 10);
_model.state.debug[2] = lrintf(Math::toDeg(_model.state.attitude.euler[AXIS_PITCH]) * 10);
_model.state.debug[3] = lrintf(_model.state.output.ch[AXIS_PITCH] * 1000);
}
}
Expand All @@ -113,10 +113,10 @@ void FAST_CODE_ATTR Controller::outerLoop()
_model.state.desiredAngle = VectorFloat(
_model.state.input.ch[AXIS_ROLL] * Math::toRad(_model.config.level.angleLimit),
_model.state.input.ch[AXIS_PITCH] * Math::toRad(_model.config.level.angleLimit),
_model.state.angle[AXIS_YAW]
_model.state.attitude.euler[AXIS_YAW]
);
_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]);
_model.state.desiredRate[AXIS_ROLL] = _model.state.outerPid[AXIS_ROLL].update(_model.state.desiredAngle[AXIS_ROLL], _model.state.attitude.euler[AXIS_ROLL]);
_model.state.desiredRate[AXIS_PITCH] = _model.state.outerPid[AXIS_PITCH].update(_model.state.desiredAngle[AXIS_PITCH], _model.state.attitude.euler[AXIS_PITCH]);
// disable fterm in angle mode
_model.state.innerPid[AXIS_ROLL].fScale = 0.f;
_model.state.innerPid[AXIS_PITCH].fScale = 0.f;
Expand Down
74 changes: 37 additions & 37 deletions lib/Espfc/src/Fusion.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,9 +72,9 @@ int FAST_CODE_ATTR Fusion::update()

if(_model.config.debug.mode == DEBUG_ALTITUDE)
{
_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);
_model.state.debug[0] = lrintf(Math::toDeg(_model.state.attitude.euler[0]) * 10);
_model.state.debug[1] = lrintf(Math::toDeg(_model.state.attitude.euler[1]) * 10);
_model.state.debug[2] = lrintf(Math::toDeg(_model.state.attitude.euler[2]) * 10);
}
return 1;
}
Expand All @@ -83,75 +83,75 @@ void Fusion::experimentalFusion()
{
// Experiment: workaround for 90 deg limit on pitch[y] axis
Quaternion r = Quaternion::lerp(Quaternion(), _model.state.accel.adc.accelToQuaternion(), 0.5);
_model.state.angle.eulerFromQuaternion(r);
_model.state.angle *= 2.f;
_model.state.attitude.euler.eulerFromQuaternion(r);
_model.state.attitude.euler *= 2.f;
}

void Fusion::simpleFusion()
{
_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.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;
_model.state.attitude.euler.x = _model.state.pose.x;
_model.state.attitude.euler.y = _model.state.pose.y;
_model.state.attitude.euler.z += _model.state.gyro.timer.intervalf * _model.state.gyro.adc.z;
if(_model.state.attitude.euler.z > PI) _model.state.attitude.euler.z -= TWO_PI;
if(_model.state.attitude.euler.z < -PI) _model.state.attitude.euler.z += TWO_PI;
}

void Fusion::kalmanFusion()
{
_model.state.pose = _model.state.accel.adc.accelToEuler();
_model.state.pose.z = _model.state.angle.z;
_model.state.pose.z = _model.state.attitude.euler.z;
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.adc.get(i), dt);
_model.state.angle.set(i, angle);
_model.state.attitude.euler.set(i, angle);
//_model.state.rate.set(i, _model.state.kalman[i].getRate());
}
_model.state.angleQ = _model.state.angle.eulerToQuaternion();
_model.state.attitude.quaternion = _model.state.attitude.euler.eulerToQuaternion();
}

void Fusion::complementaryFusion()
{
_model.state.pose = _model.state.accel.adc.accelToEuler();
_model.state.pose.z = _model.state.angle.z;
_model.state.pose.z = _model.state.attitude.euler.z;
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.adc[i] * dt) * (1.f - alpha) + _model.state.pose[i] * alpha;
float angle = (_model.state.attitude.euler[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);
_model.state.attitude.euler.set(i, angle);
}
_model.state.angleQ = _model.state.angle.eulerToQuaternion();
//_model.state.angle.eulerFromQuaternion(_model.state.angleQ); // causes NaN
_model.state.attitude.quaternion = _model.state.attitude.euler.eulerToQuaternion();
//_model.state.attitude.euler.eulerFromQuaternion(_model.state.attitude.quaternion); // causes NaN
}

void Fusion::complementaryFusionOld()
{
const float alpha = 0.01f;
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.adc * dt) * (1.f - alpha) + _model.state.pose * alpha;
_model.state.angleQ = _model.state.angle.eulerToQuaternion();
_model.state.pose.z = _model.state.attitude.euler.z;
_model.state.attitude.euler = (_model.state.attitude.euler + _model.state.gyro.adc * dt) * (1.f - alpha) + _model.state.pose * alpha;
_model.state.attitude.quaternion = _model.state.attitude.euler.eulerToQuaternion();
}

void Fusion::rtqfFusion()
{
float slerpPower = 0.001f;
if(_first)
{
_model.state.angle = _model.state.pose;
_model.state.angleQ = _model.state.poseQ;
_model.state.attitude.euler = _model.state.pose;
_model.state.attitude.quaternion = _model.state.poseQ;
_first = false;
return;
}

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

float qs, qx, qy, qz;
Expand Down Expand Up @@ -194,8 +194,8 @@ void Fusion::rtqfFusion()
fusionQPose = fusionQPose * rotationPower;
fusionQPose.normalize();

_model.state.angleQ = fusionQPose;
_model.state.angle.eulerFromQuaternion(fusionQPose);
_model.state.attitude.quaternion = fusionQPose;
_model.state.attitude.euler.eulerFromQuaternion(fusionQPose);
}

void Fusion::updatePoseFromAccelMag()
Expand Down Expand Up @@ -224,7 +224,7 @@ void Fusion::updatePoseFromAccelMag()
}
else
{
_model.state.pose.z = _model.state.angle.z;
_model.state.pose.z = _model.state.attitude.euler.z;
}
_model.state.poseQ = _model.state.pose.eulerToQuaternion();
_model.state.pose.eulerFromQuaternion(_model.state.poseQ);
Expand All @@ -244,8 +244,8 @@ void Fusion::updatePoseFromAccelMag()

// if the biggest component has a different sign in the measured and kalman poses,
// change the sign of the measured pose to match.
if(((_model.state.poseQ.get(maxIndex) < 0) && (_model.state.angleQ.get(maxIndex) > 0)) ||
((_model.state.poseQ.get(maxIndex) > 0) && (_model.state.angleQ.get(maxIndex) < 0))) {
if(((_model.state.poseQ.get(maxIndex) < 0) && (_model.state.attitude.quaternion.get(maxIndex) > 0)) ||
((_model.state.poseQ.get(maxIndex) > 0) && (_model.state.attitude.quaternion.get(maxIndex) < 0))) {
_model.state.poseQ.w = -_model.state.poseQ.w;
_model.state.poseQ.x = -_model.state.poseQ.x;
_model.state.poseQ.y = -_model.state.poseQ.y;
Expand Down Expand Up @@ -292,41 +292,41 @@ void Fusion::madgwickFusion()
if(false && _model.magActive())
{
_madgwick.update(
_model.state.gyroImu.x, _model.state.gyroImu.y, _model.state.gyroImu.z,
_model.state.attitude.rate.x, _model.state.attitude.rate.y, _model.state.attitude.rate.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.attitude.rate.x, _model.state.attitude.rate.y, _model.state.attitude.rate.z,
_model.state.accel.adc.x, _model.state.accel.adc.y, _model.state.accel.adc.z
);
}
_model.state.angleQ = _madgwick.getQuaternion();
_model.state.angle = _madgwick.getEuler();
_model.state.attitude.quaternion = _madgwick.getQuaternion();
_model.state.attitude.euler = _madgwick.getEuler();
}

void FAST_CODE_ATTR Fusion::mahonyFusion()
{
if(false && _model.magActive())
{
_mahony.update(
_model.state.gyroImu.x, _model.state.gyroImu.y, _model.state.gyroImu.z,
_model.state.attitude.rate.x, _model.state.attitude.rate.y, _model.state.attitude.rate.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.attitude.rate.x, _model.state.attitude.rate.y, _model.state.attitude.rate.z,
_model.state.accel.adc.x, _model.state.accel.adc.y, _model.state.accel.adc.z
);
}
_model.state.angleQ = _mahony.getQuaternion();
_model.state.angle = _mahony.getEuler();
_model.state.attitude.quaternion = _mahony.getQuaternion();
_model.state.attitude.euler = _mahony.getEuler();
}

}
2 changes: 1 addition & 1 deletion lib/Espfc/src/Model.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -474,7 +474,7 @@ class Model
state.gyro.filter2[i].begin(config.gyro.filter2, gyroFilterRate);
state.gyro.filter3[i].begin(config.gyro.filter3, gyroPreFilterRate);
state.accel.filter[i].begin(config.accel.filter, gyroFilterRate);
state.gyroImuFilter[i].begin(FilterConfig(FILTER_PT1, state.accel.timer.rate / 3), gyroFilterRate);
state.attitude.filter[i].begin(FilterConfig(FILTER_PT1, state.accel.timer.rate / 3), gyroFilterRate);
for(size_t m = 0; m < RPM_FILTER_MOTOR_MAX; m++)
{
state.gyro.rpmFreqFilter[m].begin(FilterConfig(FILTER_PT1, config.gyro.rpmFilter.freqLpf), gyroFilterRate);
Expand Down
20 changes: 11 additions & 9 deletions lib/Espfc/src/ModelState.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -286,7 +286,7 @@ struct AccelState
VectorInt16 raw;
VectorFloat adc;
VectorFloat prev;
Filter filter[3];
Filter filter[AXIS_COUNT_RPY];
Timer timer;

float scale;
Expand All @@ -296,6 +296,14 @@ struct AccelState
int calibrationState;
};

struct AttitudeState
{
VectorFloat rate;
Filter filter[AXIS_COUNT_RPY];
VectorFloat euler;
Quaternion quaternion;
};

// working data
struct ModelState
{
Expand All @@ -304,24 +312,18 @@ struct ModelState
MagState mag;
BaroState baro;

AttitudeState attitude;

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

bool loopUpdate;
VectorFloat pose;
Quaternion poseQ;
VectorFloat angle;
Quaternion angleQ;

RotationMatrixFloat boardAlignment;

bool imuUpdate;
VectorFloat gyroImu;
Filter gyroImuFilter[3];

VectorFloat velocity;
VectorFloat desiredVelocity;

Expand Down
6 changes: 3 additions & 3 deletions lib/Espfc/src/Msp/MspProcessor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -679,9 +679,9 @@ class MspProcessor
break;

case MSP_ATTITUDE:
r.writeU16(lrintf(Math::toDeg(_model.state.angle.x) * 10.f)); // roll [decidegrees]
r.writeU16(lrintf(Math::toDeg(_model.state.angle.y) * 10.f)); // pitch [decidegrees]
r.writeU16(lrintf(Math::toDeg(-_model.state.angle.z))); // yaw [degrees]
r.writeU16(lrintf(Math::toDeg(_model.state.attitude.euler.x) * 10.f)); // roll [decidegrees]
r.writeU16(lrintf(Math::toDeg(_model.state.attitude.euler.y) * 10.f)); // pitch [decidegrees]
r.writeU16(lrintf(Math::toDeg(-_model.state.attitude.euler.z))); // yaw [degrees]
break;

case MSP_ALTITUDE:
Expand Down
2 changes: 1 addition & 1 deletion lib/Espfc/src/Sensor/GyroSensor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -146,7 +146,7 @@ int FAST_CODE_ATTR GyroSensor::filter()

if (_model.accelActive())
{
_model.state.gyroImu = Utils::applyFilter(_model.state.gyroImuFilter, _model.state.gyro.adc);
_model.state.attitude.rate = Utils::applyFilter(_model.state.attitude.filter, _model.state.gyro.adc);
}

return 1;
Expand Down
6 changes: 3 additions & 3 deletions lib/Espfc/src/Telemetry/TelemetryCRSF.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -100,9 +100,9 @@ class TelemetryCRSF
{
msg.prepare(Rc::CRSF_FRAMETYPE_ATTITUDE);

int16_t r = toAngle(_model.state.angle.x);
int16_t p = toAngle(_model.state.angle.y);
int16_t y = toAngle(_model.state.angle.z);
int16_t r = toAngle(_model.state.attitude.euler.x);
int16_t p = toAngle(_model.state.attitude.euler.y);
int16_t y = toAngle(_model.state.attitude.euler.z);

msg.writeU16(Math::toBigEndian16(r));
msg.writeU16(Math::toBigEndian16(p));
Expand Down

0 comments on commit d947406

Please sign in to comment.