diff --git a/stm32l552xx/boardfiles/drivers/motor/motor.cpp b/stm32l552xx/boardfiles/drivers/motor/motor.cpp index 843aea7a..91751f30 100644 --- a/stm32l552xx/boardfiles/drivers/motor/motor.cpp +++ b/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); } diff --git a/zeropilot3.5/include/attitude_manager/fbwa_mapping.hpp b/zeropilot3.5/include/attitude_manager/fbwa_mapping.hpp new file mode 100644 index 00000000..04d62204 --- /dev/null +++ b/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; +}; \ No newline at end of file diff --git a/zeropilot3.5/include/attitude_manager/pid.hpp b/zeropilot3.5/include/attitude_manager/pid.hpp new file mode 100644 index 00000000..c79f19be --- /dev/null +++ b/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; +}; diff --git a/zeropilot3.5/src/attitude_manager/attitude_manager.cpp b/zeropilot3.5/src/attitude_manager/attitude_manager.cpp index 46b8d879..6ed3b8b2 100644 --- a/zeropilot3.5/src/attitude_manager/attitude_manager.cpp +++ b/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); } } -} +} \ No newline at end of file diff --git a/zeropilot3.5/src/attitude_manager/fbwa_mapping.cpp b/zeropilot3.5/src/attitude_manager/fbwa_mapping.cpp new file mode 100644 index 00000000..8881ad7d --- /dev/null +++ b/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; +} + diff --git a/zeropilot3.5/src/attitude_manager/pid.cpp b/zeropilot3.5/src/attitude_manager/pid.cpp new file mode 100644 index 00000000..d1cfc161 --- /dev/null +++ b/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 +}