Add PID and FWBA Control

stm32l552xx/boardfiles/drivers/motor/motor.cpp

@@ -9,7 +9,7 @@ MotorControl::MotorControl(TIM_HandleTypeDef *timer, uint32_t timerChannel, uint
 }
 
 void MotorControl::set(uint32_t percent) {
-    percent = percent > 100 ? 100 : percent;
+    percent =(percent > 100 ? 100 : percent);
     uint32_t ticks = ((percent / 100.0) * (maxCCR - minCCR)) + minCCR;
     __HAL_TIM_SET_COMPARE(timer, timerChannel, ticks);
 }

zeropilot3.5/include/attitude_manager/fbwa_mapping.hpp

@@ -0,0 +1,39 @@
+#pragma once
+
+#include "flightmode.hpp"
+#include "attitude_manager.hpp"
+#include "pid.hpp"
+
+class FBWAMapping : public Flightmode {
+    public:
+        FBWAMapping() noexcept;
+
+        RCMotorControlMessage_t runControl(RCMotorControlMessage_t controlInput) override;
+
+        // Setter *roll* for PID consts
+        void setRollPIDConstants(float newKp, float newKi, float newKd, float newTau) noexcept;
+
+        // Setter for *pitch* PID consts
+        void setPitchPIDConstants(float newKp, float newKi, float newKd, float newTau) noexcept;
+
+        // Destructor
+        ~FBWAMapping() noexcept override;
+
+    private:
+
+        // Roll integral limits ******
+        inline const static float ROLL_INTEGRAL_MIN_LIM = 0.0f;
+        inline const static float ROLL_INTEGRAL_MAX_LIM = 0.0f;
+
+        // Pitch integral limits ******
+        inline const static float PITCH_INTEGRAL_MIN_LIM = 0.0f;
+        inline const static float PITCH_INTEGRAL_MAX_LIM = 0.0f;
+
+        // Output limits (for control effort)
+        inline const static float OUTPUT_MIN = 0.0f;
+        inline const static float OUTPUT_MAX = 0.0f;
+
+        // Roll and Pitch PID class objects
+        PID rollPID;
+        PID pitchPID;
+};

zeropilot3.5/include/attitude_manager/pid.hpp

@@ -0,0 +1,34 @@
+#pragma once
+
+
+class PID {
+    public:
+        // PID object constructor
+        PID(float kp, float ki, float kd, 
+            float tau, float outputMinLim, float outputMaxLim,
+            float integralMinLim, float integralMaxLim, float t) noexcept;
+
+        // PID object's state var initialized (or reset)
+        void pidInitState() noexcept;
+
+        // For the PID roll & pitch consts -> may choose these to be optimized real-time dep. on optimization alg. chosen
+        void setConstants(float newKp, float newKi, float newKd, float newTau) noexcept;
+
+        // Computes PID for a measurement with its desired setpoint passed in
+        float pidOutput(float setpoint, float measurement);
+
+
+    private:
+        // Gains
+        float kp, ki, kd;      // PID constants
+        float tau;             // Derivative low-pass filter constant
+        float t;               // Sample time (set to AM_MAIN_DELAY)
+
+        // Output and Integral Limits
+        float outputMinLim, outputMaxLim;       // Output limits
+        float integralMinLim, integralMaxLim;   // integral limits
+
+        // State variables
+        float pidDerivative, pidIntegral;
+        float prevError, prevMeasurement;
+};

zeropilot3.5/src/attitude_manager/attitude_manager.cpp

@@ -118,4 +118,4 @@ void AttitudeManager::outputToMotor(ControlAxis_e axis, uint8_t percent) {
             motor->motorInstance->set(percent);
         }
     }
-}
+}

zeropilot3.5/src/attitude_manager/fbwa_mapping.cpp

@@ -0,0 +1,44 @@
+#include "fbwa_mapping.hpp"
+
+FBWAMapping::FBWAMapping() noexcept :
+    rollPID(0.0f, 0.0f, 0.0f,
+        0.0f, OUTPUT_MIN, OUTPUT_MAX,
+        ROLL_INTEGRAL_MIN_LIM, ROLL_INTEGRAL_MAX_LIM, AM_MAIN_DELAY),
+    pitchPID(0.0f, 0.0f, 0.0f,
+        0.0f, OUTPUT_MIN, OUTPUT_MAX,
+        PITCH_INTEGRAL_MIN_LIM, PITCH_INTEGRAL_MAX_LIM, AM_MAIN_DELAY)
+{
+    rollPID.pidInitState();
+    pitchPID.pidInitState();
+}
+
+FBWAMapping::~FBWAMapping() noexcept {}
+
+// Setter *roll* for PID consts
+void FBWAMapping::setRollPIDConstants(float newKp, float newKi, float newKd, float newTau) noexcept {
+    rollPID.setConstants(newKp, newKi, newKd, newTau);
+}
+
+// Setter for *pitch* PID consts
+void FBWAMapping::setPitchPIDConstants(float newKp, float newKi, float newKd, float newTau) noexcept {
+    pitchPID.setConstants(newKp, newKi, newKd, newTau);
+}
+
+RCMotorControlMessage_t FBWAMapping::runControl(RCMotorControlMessage_t controlInputs){
+    float rollSetpoint = controlInputs.roll;
+    float pitchSetpoint = controlInputs.pitch;
+
+    // Get from sensor fusion -- make sure to update include files when we get this ********
+    float rollMeasured = 50.0;
+    float pitchMeasured = 50.0;
+
+    // Currently, roll & pitch outputs receive absolute roll & pitch angles, not relative to current position.
+    float rollOutput = rollPID.pidOutput(rollSetpoint, rollMeasured);
+    float pitchOutput = pitchPID.pidOutput(pitchSetpoint, pitchMeasured);
+
+    controlInputs.roll = rollOutput; // setting desired roll angle
+    controlInputs.pitch = pitchOutput; // setting desired pitch angle
+
+    return controlInputs;
+}
+

zeropilot3.5/src/attitude_manager/pid.cpp

@@ -0,0 +1,60 @@
+#include "pid.hpp"
+
+// Constructor
+PID::PID(float kp, float ki, float kd,
+         float tau, float outputMinLim, float outputMaxLim,
+         float integralMinLim, float integralMaxLim, float t) noexcept : 
+            kp(kp), ki(ki), kd(kd), tau(tau), t(t),
+            outputMinLim(outputMinLim), outputMaxLim(outputMaxLim),
+            integralMinLim(integralMinLim), integralMaxLim(integralMaxLim)
+{}
+
+// Initialization method - Can be used as resetter
+void PID::pidInitState() noexcept {
+    pidIntegral = 0.0f;
+    prevError = 0.0f;
+    pidDerivative = 0.0f;
+    prevMeasurement = 0.0f;
+}
+
+void PID::setConstants(float newKp, float newKi, float newKd, float newTau) noexcept {
+    kp = newKp;
+    ki = newKi;
+    kd = newKd;
+    tau = newTau;
+}
+
+// Update method
+float PID::pidOutput(float setpoint, float measurement) {
+    // Calculate error
+    float error = setpoint - measurement;
+
+    // PID Proportional
+    float pidProportional = kp * error;
+
+    // PID Integral 
+    pidIntegral += (0.5f * ki * t) * (error + prevError);
+
+    // Anti-integral windup
+    if (pidIntegral > integralMaxLim) { pidIntegral = integralMaxLim; }
+    if (pidIntegral < integralMinLim) { pidIntegral = integralMinLim; }
+
+    // PID Derivative with low-pass filter
+    pidDerivative = ((-1 * 2.0f * kd * (measurement - prevMeasurement)) + ((2 * tau - t) * pidDerivative)) / ((2.0f * tau) + t);
+
+    // PID control effort
+    float pidControlEffort = pidProportional + pidIntegral + pidDerivative;
+
+    // Clamp control effort output
+    if (pidControlEffort > outputMaxLim) { pidControlEffort = outputMaxLim; }
+    if (pidControlEffort < outputMinLim) { pidControlEffort = outputMinLim; }
+
+    // Update previous values
+    prevError = error;
+    prevMeasurement = measurement;
+
+    // Absolute motor control position
+    float pidOut = measurement + pidControlEffort; // Directly into motor control
+
+    return pidOut; // Must go directly into motor control
+}