setpoint state refactor

rtlopez · Oct 29, 2024 · 3f3f6f5 · 3f3f6f5
1 parent d947406
commit 3f3f6f5
Show file tree

Hide file tree

Showing 3 changed files with 35 additions and 38 deletions.
diff --git a/lib/Espfc/src/Blackbox/BlackboxBridge.cpp b/lib/Espfc/src/Blackbox/BlackboxBridge.cpp
@@ -65,7 +65,7 @@ bool sensors(uint32_t mask)
 
 float pidGetPreviousSetpoint(int axis)
 {
-  return Espfc::Math::toDeg(_model_ptr->state.desiredRate[axis]);
+  return Espfc::Math::toDeg(_model_ptr->state.setpoint.rate[axis]);
 }
 
 float mixerGetThrottle(void)

diff --git a/lib/Espfc/src/Controller.cpp b/lib/Espfc/src/Controller.cpp
@@ -69,8 +69,8 @@ void Controller::outerLoopRobot()
   {
     angle = _model.state.outerPid[AXIS_PITCH].update(_model.state.input.ch[AXIS_PITCH], speed) * Math::toRad(_model.config.level.rateLimit);
   }
-  _model.state.desiredAngle.set(AXIS_PITCH, angle);
-  _model.state.desiredRate[AXIS_YAW] = _model.state.input.ch[AXIS_YAW] * Math::toRad(_model.config.level.rateLimit);
+  _model.state.setpoint.angle.set(AXIS_PITCH, angle);
+  _model.state.setpoint.rate[AXIS_YAW] = _model.state.input.ch[AXIS_YAW] * Math::toRad(_model.config.level.rateLimit);
 
   if(_model.config.debug.mode == DEBUG_ANGLERATE)
   {
@@ -89,8 +89,8 @@ void Controller::innerLoopRobot()
   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.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]);
+    _model.state.output.ch[AXIS_PITCH] = _model.state.innerPid[AXIS_PITCH].update(_model.state.setpoint.angle[AXIS_PITCH], _model.state.attitude.euler[AXIS_PITCH]);
+    _model.state.output.ch[AXIS_YAW]   = _model.state.innerPid[AXIS_YAW].update(_model.state.setpoint.rate[AXIS_YAW], _model.state.gyro.adc[AXIS_YAW]);
   }
   else
   {
@@ -110,30 +110,30 @@ void FAST_CODE_ATTR Controller::outerLoop()
 {
   if(_model.isActive(MODE_ANGLE))
   {
-    _model.state.desiredAngle = VectorFloat(
+    _model.state.setpoint.angle = 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.attitude.euler[AXIS_YAW]
     );
-    _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]);
+    _model.state.setpoint.rate[AXIS_ROLL]  = _model.state.outerPid[AXIS_ROLL].update(_model.state.setpoint.angle[AXIS_ROLL], _model.state.attitude.euler[AXIS_ROLL]);
+    _model.state.setpoint.rate[AXIS_PITCH] = _model.state.outerPid[AXIS_PITCH].update(_model.state.setpoint.angle[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;
   }
   else
   {
-    _model.state.desiredRate[AXIS_ROLL] = calculateSetpointRate(AXIS_ROLL, _model.state.input.ch[AXIS_ROLL]);
-    _model.state.desiredRate[AXIS_PITCH] = calculateSetpointRate(AXIS_PITCH, _model.state.input.ch[AXIS_PITCH]);
+    _model.state.setpoint.rate[AXIS_ROLL] = calculateSetpointRate(AXIS_ROLL, _model.state.input.ch[AXIS_ROLL]);
+    _model.state.setpoint.rate[AXIS_PITCH] = calculateSetpointRate(AXIS_PITCH, _model.state.input.ch[AXIS_PITCH]);
   }
-  _model.state.desiredRate[AXIS_YAW] = calculateSetpointRate(AXIS_YAW, _model.state.input.ch[AXIS_YAW]);
-  _model.state.desiredRate[AXIS_THRUST] = _model.state.input.ch[AXIS_THRUST];
+  _model.state.setpoint.rate[AXIS_YAW] = calculateSetpointRate(AXIS_YAW, _model.state.input.ch[AXIS_YAW]);
+  _model.state.setpoint.rate[AXIS_THRUST] = _model.state.input.ch[AXIS_THRUST];
 
   if(_model.config.debug.mode == DEBUG_ANGLERATE)
   {
     for(size_t i = 0; i < 3; ++i)
     {
-      _model.state.debug[i] = lrintf(Math::toDeg(_model.state.desiredRate[i]));
+      _model.state.debug[i] = lrintf(Math::toDeg(_model.state.setpoint.rate[i]));
     }
   }
 }
@@ -143,10 +143,10 @@ 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.adc[i]) * tpaFactor;
+    _model.state.output.ch[i] = _model.state.innerPid[i].update(_model.state.setpoint.rate[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];
+  _model.state.output.ch[AXIS_THRUST] = _model.state.setpoint.rate[AXIS_THRUST];
 
   if(_model.config.debug.mode == DEBUG_ITERM_RELAX)
   {

diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h
@@ -304,6 +304,12 @@ struct AttitudeState
   Quaternion quaternion;
 };
 
+struct SetpointState
+{
+  VectorFloat angle;
+  float rate[AXES];
+};
+
 // working data
 struct ModelState
 {
@@ -312,38 +318,19 @@ struct ModelState
   MagState mag;
   BaroState baro;
 
-  AttitudeState attitude;
-
-  VectorFloat gyroPose;
-  Quaternion gyroPoseQ;
-  VectorFloat accelPose;
-  VectorFloat accelPose2;
-  Quaternion accelPoseQ;
-  VectorFloat pose;
-  Quaternion poseQ;
+  InputState input;
+  FailsafeState failsafe;
 
+  AttitudeState attitude;
   RotationMatrixFloat boardAlignment;
 
-  VectorFloat velocity;
-  VectorFloat desiredVelocity;
-
-  VectorFloat desiredAngle;
-  Quaternion desiredAngleQ;
-
-  float desiredRate[AXES];
-
+  SetpointState setpoint;
   Control::Pid innerPid[AXES];
   Control::Pid outerPid[AXES];
 
-  InputState input;
-  FailsafeState failsafe;
-
   MixerState mixer;
   OutputState output;
 
-  // other state
-  Kalman kalman[AXES];
-
   int32_t loopRate;
   Timer loopTimer;
 
@@ -380,6 +367,16 @@ struct ModelState
   Timer serialTimer;
 
   Target::Queue appQueue;
+
+  // other state
+  Kalman kalman[AXES];
+  VectorFloat gyroPose;
+  Quaternion gyroPoseQ;
+  VectorFloat accelPose;
+  VectorFloat accelPose2;
+  Quaternion accelPoseQ;
+  VectorFloat pose;
+  Quaternion poseQ;
 };
 
 }