From e580a10d803c440c22d65f99582c01d292ec8760 Mon Sep 17 00:00:00 2001 From: JJKSweaty Date: Wed, 28 May 2025 01:45:34 -0400 Subject: [PATCH 01/24] added rudder mixing --- .../include/attitude_manager/attitude_manager.hpp | 5 +++++ .../src/attitude_manager/attitude_manager.cpp | 12 ++++++++++++ 2 files changed, 17 insertions(+) diff --git a/zeropilot3.5/include/attitude_manager/attitude_manager.hpp b/zeropilot3.5/include/attitude_manager/attitude_manager.hpp index 3a2784e1..4ff473e8 100644 --- a/zeropilot3.5/include/attitude_manager/attitude_manager.hpp +++ b/zeropilot3.5/include/attitude_manager/attitude_manager.hpp @@ -7,6 +7,7 @@ #include "motor_datatype.hpp" #define AM_MAIN_DELAY 50 +#define ADVERSE_YAW_COEFFICIENT 1.5f typedef enum { YAW = 0, @@ -48,6 +49,10 @@ class AttitudeManager { MotorGroupInstance_t *flapMotors; MotorGroupInstance_t *steeringMotors; + + float adverseYaw = 0.0f; + float signedYaw = 0.0f; + bool getControlInputs(RCMotorControlMessage_t *pControlMsg); void outputToMotor(ControlAxis_e axis, uint8_t percent); diff --git a/zeropilot3.5/src/attitude_manager/attitude_manager.cpp b/zeropilot3.5/src/attitude_manager/attitude_manager.cpp index 46b8d879..93ff228a 100644 --- a/zeropilot3.5/src/attitude_manager/attitude_manager.cpp +++ b/zeropilot3.5/src/attitude_manager/attitude_manager.cpp @@ -1,6 +1,7 @@ #include "attitude_manager.hpp" #include "rc_motor_control.hpp" + AttitudeManager::AttitudeManager( IMessageQueue *amQueue, IMessageQueue *smLoggerQueue, @@ -65,6 +66,17 @@ void AttitudeManager::runControlLoopIteration() { RCMotorControlMessage_t motorOutputs = controlAlgorithm->runControl(controlMsg); + signedYaw = motorOutputs.roll-50; + adverseYaw = signedYaw * ADVERSE_YAW_COEFFICIENT; + motorOutputs.yaw +=adverseYaw; + // limit yaw to 100 + if (motorOutputs.yaw>100){ + motorOutputs.yaw = 100; + //limit yaw to 0 + } else if (motorOutputs.yaw < 0) { + motorOutputs.yaw = 0; + } + outputToMotor(YAW, motorOutputs.yaw); outputToMotor(PITCH, motorOutputs.pitch); outputToMotor(ROLL, motorOutputs.roll); From d9ce32bba8a587b3514903ca57162a880701aa16 Mon Sep 17 00:00:00 2001 From: Bill Lu <80836399+William-Lu-Jia-Rong@users.noreply.github.com> Date: Sun, 1 Jun 2025 15:32:03 -0400 Subject: [PATCH 02/24] software trim added --- zeropilot3.5/include/attitude_manager/attitude_manager.hpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/zeropilot3.5/include/attitude_manager/attitude_manager.hpp b/zeropilot3.5/include/attitude_manager/attitude_manager.hpp index 4ff473e8..0b194c88 100644 --- a/zeropilot3.5/include/attitude_manager/attitude_manager.hpp +++ b/zeropilot3.5/include/attitude_manager/attitude_manager.hpp @@ -9,6 +9,13 @@ #define AM_MAIN_DELAY 50 #define ADVERSE_YAW_COEFFICIENT 1.5f +#define ROLLMOTORS_TRIM 0 +#define PITCHMOTORS_TRIM 0 +#define YAWMOTORS_TRIM 0 +#define THROTTLEMOTORS_TRIM 0 +#define FLAPMOTORS_TRIM 0 +#define STEERINGMOTORS_TRIM 0 + typedef enum { YAW = 0, PITCH, From a244319fcfb42c0f720f15dec8ba7e257db2e568 Mon Sep 17 00:00:00 2001 From: Bill Lu <80836399+William-Lu-Jia-Rong@users.noreply.github.com> Date: Sun, 1 Jun 2025 15:36:29 -0400 Subject: [PATCH 03/24] Make sure the precent of motor do not go below 0 --- stm32l552xx/boardfiles/drivers/motor/motor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/stm32l552xx/boardfiles/drivers/motor/motor.cpp b/stm32l552xx/boardfiles/drivers/motor/motor.cpp index 843aea7a..fa0f2523 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 < 0) ? 0 : (percent > 100 ? 100 : percent); uint32_t ticks = ((percent / 100.0) * (maxCCR - minCCR)) + minCCR; __HAL_TIM_SET_COMPARE(timer, timerChannel, ticks); } From 7e54198529b5542b80f514438f716567a60288d4 Mon Sep 17 00:00:00 2001 From: Bill Lu <80836399+William-Lu-Jia-Rong@users.noreply.github.com> Date: Sun, 1 Jun 2025 15:38:03 -0400 Subject: [PATCH 04/24] Add offset value to each motor --- .../src/attitude_manager/attitude_manager.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/zeropilot3.5/src/attitude_manager/attitude_manager.cpp b/zeropilot3.5/src/attitude_manager/attitude_manager.cpp index 93ff228a..7397fc02 100644 --- a/zeropilot3.5/src/attitude_manager/attitude_manager.cpp +++ b/zeropilot3.5/src/attitude_manager/attitude_manager.cpp @@ -77,12 +77,12 @@ void AttitudeManager::runControlLoopIteration() { motorOutputs.yaw = 0; } - outputToMotor(YAW, motorOutputs.yaw); - outputToMotor(PITCH, motorOutputs.pitch); - outputToMotor(ROLL, motorOutputs.roll); - outputToMotor(THROTTLE, motorOutputs.throttle); - outputToMotor(FLAP_ANGLE, motorOutputs.flapAngle); - outputToMotor(STEERING, motorOutputs.yaw); + outputToMotor(YAW, motorOutputs.yaw+YAWMOTORS_TRIM); + outputToMotor(PITCH, motorOutputs.pitch+PITCHMOTORS_TRIM); + outputToMotor(ROLL, motorOutputs.roll+ROLLMOTORS_TRIM); + outputToMotor(THROTTLE, motorOutputs.throttle+THROTTLEMOTORS_TRIM); + outputToMotor(FLAP_ANGLE, motorOutputs.flapAngle+FLAPMOTORS_TRIM); + outputToMotor(STEERING, motorOutputs.yaw+STEERINGMOTORS_TRIM); } bool AttitudeManager::getControlInputs(RCMotorControlMessage_t *pControlMsg) { From 615f11ce55c04e53cd8fe90d3eaa470ba5a7c9d6 Mon Sep 17 00:00:00 2001 From: Eeman Aleem Date: Sun, 1 Jun 2025 16:29:52 -0400 Subject: [PATCH 05/24] Added PID header & cpp files (framework) --- zeropilot3.5/include/attitude_manager/PID_mapping.hpp | 10 ++++++++++ zeropilot3.5/src/attitude_manager/pid_mapping.cpp | 5 +++++ 2 files changed, 15 insertions(+) create mode 100644 zeropilot3.5/include/attitude_manager/PID_mapping.hpp create mode 100644 zeropilot3.5/src/attitude_manager/pid_mapping.cpp diff --git a/zeropilot3.5/include/attitude_manager/PID_mapping.hpp b/zeropilot3.5/include/attitude_manager/PID_mapping.hpp new file mode 100644 index 00000000..be3bb1f0 --- /dev/null +++ b/zeropilot3.5/include/attitude_manager/PID_mapping.hpp @@ -0,0 +1,10 @@ +#pragma once + +#include "flightmode.hpp" + +class PIDMapping : public Flightmode { + public: + PIDMapping() = default; + + RCMotorControlMessage_t runControl(RCMotorControlMessage_t controlInput) override; +}; diff --git a/zeropilot3.5/src/attitude_manager/pid_mapping.cpp b/zeropilot3.5/src/attitude_manager/pid_mapping.cpp new file mode 100644 index 00000000..44204e0b --- /dev/null +++ b/zeropilot3.5/src/attitude_manager/pid_mapping.cpp @@ -0,0 +1,5 @@ +#include "PID_mapping.hpp" + +RCMotorControlMessage_t PIDMapping::runControl(RCMotorControlMessage_t controlInputs){ + return controlInputs; +} \ No newline at end of file From ef37963ba2a9897410db2d4bc4fced75d7c91874 Mon Sep 17 00:00:00 2001 From: Bill Lu <80836399+William-Lu-Jia-Rong@users.noreply.github.com> Date: Thu, 5 Jun 2025 17:35:35 -0400 Subject: [PATCH 06/24] set to private member --- .../attitude_manager/attitude_manager.hpp | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/zeropilot3.5/include/attitude_manager/attitude_manager.hpp b/zeropilot3.5/include/attitude_manager/attitude_manager.hpp index 0b194c88..802ed908 100644 --- a/zeropilot3.5/include/attitude_manager/attitude_manager.hpp +++ b/zeropilot3.5/include/attitude_manager/attitude_manager.hpp @@ -9,12 +9,6 @@ #define AM_MAIN_DELAY 50 #define ADVERSE_YAW_COEFFICIENT 1.5f -#define ROLLMOTORS_TRIM 0 -#define PITCHMOTORS_TRIM 0 -#define YAWMOTORS_TRIM 0 -#define THROTTLEMOTORS_TRIM 0 -#define FLAPMOTORS_TRIM 0 -#define STEERINGMOTORS_TRIM 0 typedef enum { YAW = 0, @@ -56,6 +50,19 @@ class AttitudeManager { MotorGroupInstance_t *flapMotors; MotorGroupInstance_t *steeringMotors; + bool if_rollMotors_invert=0; + bool if_pitchMotors_invert=0; + bool if_yawMotors_invert=0; + bool if_throttleMotors_invert=0; + bool if_flapMotors_invert=0; + bool if_steeringMotor_invert=0; + + float ROLLMOTORS_TRIM; + float PITCHMOTORS_TRIM; + float YAWMOTORS_TRIM; + float THROTTLEMOTORS_TRIM; + float FLAPMOTORS_TRIM; + float STEERINGMOTORS_TRIM; float adverseYaw = 0.0f; float signedYaw = 0.0f; From be21d7f9aa750a8f32fd89e776f919bb2bd5d8e0 Mon Sep 17 00:00:00 2001 From: Bill Lu <80836399+William-Lu-Jia-Rong@users.noreply.github.com> Date: Thu, 5 Jun 2025 17:36:19 -0400 Subject: [PATCH 07/24] Update attitude_manager.cpp --- .../src/attitude_manager/attitude_manager.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/zeropilot3.5/src/attitude_manager/attitude_manager.cpp b/zeropilot3.5/src/attitude_manager/attitude_manager.cpp index 7397fc02..a61d0509 100644 --- a/zeropilot3.5/src/attitude_manager/attitude_manager.cpp +++ b/zeropilot3.5/src/attitude_manager/attitude_manager.cpp @@ -77,11 +77,11 @@ void AttitudeManager::runControlLoopIteration() { motorOutputs.yaw = 0; } - outputToMotor(YAW, motorOutputs.yaw+YAWMOTORS_TRIM); - outputToMotor(PITCH, motorOutputs.pitch+PITCHMOTORS_TRIM); - outputToMotor(ROLL, motorOutputs.roll+ROLLMOTORS_TRIM); - outputToMotor(THROTTLE, motorOutputs.throttle+THROTTLEMOTORS_TRIM); - outputToMotor(FLAP_ANGLE, motorOutputs.flapAngle+FLAPMOTORS_TRIM); + outputToMotor(YAW, (100-motorOutputs.yaw)*if_yawMotors_invert+motorOutputs.yaw*(if_yawMotors_invert-1)+YAWMOTORS_TRIM); + outputToMotor(PITCH, (100-motorOutputs.pitch)*if_pitchMotors_invert+motorOutputs.pitch*(if_pitchMotors_invert-1)+PITCHMOTORS_TRIM); + outputToMotor(ROLL, (100-motorOutputs.roll)*if_rollMotors_invert+motorOutputs.roll*(if_rollMotors_invert-1)+ROLLMOTORS_TRIM); + outputToMotor(THROTTLE, (100-motorOutputs.throttle)*if_throttleMotors_invert+motorOutputs.throttle*(if_throttleMotors_invert-1)+THROTTLEMOTORS_TRIM); + outputToMotor(FLAP_ANGLE, (100-motorOutputs.flapAngle)*if_flapMotors_invert+motorOutputs.flapAngle*(if_flapMotors_invert-1)+FLAPMOTORS_TRIM); outputToMotor(STEERING, motorOutputs.yaw+STEERINGMOTORS_TRIM); } @@ -131,3 +131,4 @@ void AttitudeManager::outputToMotor(ControlAxis_e axis, uint8_t percent) { } } } + From d301507982e366b07620504ad8e4e1d587b8326f Mon Sep 17 00:00:00 2001 From: JJKSweaty Date: Sun, 8 Jun 2025 02:03:06 -0400 Subject: [PATCH 08/24] formating --- zeropilot3.5/include/attitude_manager/attitude_manager.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/zeropilot3.5/include/attitude_manager/attitude_manager.hpp b/zeropilot3.5/include/attitude_manager/attitude_manager.hpp index 802ed908..96a9fa4f 100644 --- a/zeropilot3.5/include/attitude_manager/attitude_manager.hpp +++ b/zeropilot3.5/include/attitude_manager/attitude_manager.hpp @@ -7,7 +7,6 @@ #include "motor_datatype.hpp" #define AM_MAIN_DELAY 50 -#define ADVERSE_YAW_COEFFICIENT 1.5f typedef enum { @@ -56,7 +55,7 @@ class AttitudeManager { bool if_throttleMotors_invert=0; bool if_flapMotors_invert=0; bool if_steeringMotor_invert=0; - + float ROLLMOTORS_TRIM; float PITCHMOTORS_TRIM; float YAWMOTORS_TRIM; @@ -64,6 +63,7 @@ class AttitudeManager { float FLAPMOTORS_TRIM; float STEERINGMOTORS_TRIM; + float const ADVERSE_YAW_COEFFICIENT=1.5f; float adverseYaw = 0.0f; float signedYaw = 0.0f; From b908ea986ebbba98b32eb185644c5edcdde22ca4 Mon Sep 17 00:00:00 2001 From: Eeman Aleem Date: Mon, 30 Jun 2025 01:44:21 -0400 Subject: [PATCH 09/24] Added pitch controls to runControls func in PID_mapping hpp & cpp --- .../include/attitude_manager/PID_mapping.hpp | 10 +++++++++ .../src/attitude_manager/attitude_manager.cpp | 2 +- .../src/attitude_manager/pid_mapping.cpp | 21 +++++++++++++++++++ 3 files changed, 32 insertions(+), 1 deletion(-) diff --git a/zeropilot3.5/include/attitude_manager/PID_mapping.hpp b/zeropilot3.5/include/attitude_manager/PID_mapping.hpp index be3bb1f0..c09ea3b4 100644 --- a/zeropilot3.5/include/attitude_manager/PID_mapping.hpp +++ b/zeropilot3.5/include/attitude_manager/PID_mapping.hpp @@ -7,4 +7,14 @@ class PIDMapping : public Flightmode { PIDMapping() = default; RCMotorControlMessage_t runControl(RCMotorControlMessage_t controlInput) override; + private: + // Pitch controls' PID constants + float pitch_Kp = 0.0f; + static float pitch_Ki = 0.0f; + float pitch_Kd = 0.0f; + + // PID state variables + float previousPitchError = 0.0f; + float pitchIntegral = 0.0f; + float pitchSampleTime = 0.001f; // Sample time in seconds (100ms) }; diff --git a/zeropilot3.5/src/attitude_manager/attitude_manager.cpp b/zeropilot3.5/src/attitude_manager/attitude_manager.cpp index 7397fc02..7bba06b8 100644 --- a/zeropilot3.5/src/attitude_manager/attitude_manager.cpp +++ b/zeropilot3.5/src/attitude_manager/attitude_manager.cpp @@ -130,4 +130,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/pid_mapping.cpp b/zeropilot3.5/src/attitude_manager/pid_mapping.cpp index 44204e0b..57edebfc 100644 --- a/zeropilot3.5/src/attitude_manager/pid_mapping.cpp +++ b/zeropilot3.5/src/attitude_manager/pid_mapping.cpp @@ -1,5 +1,26 @@ #include "PID_mapping.hpp" RCMotorControlMessage_t PIDMapping::runControl(RCMotorControlMessage_t controlInputs){ + // Current State + float pitchMeasured = controlInputs.pitch; // What is the band of values this can take? Ask limit of pitch + float pitchSetpoint = 50.0f; + + // Error Calculation + float pitchError = pitchSetpoint - pitchMeasured; + float pitchDerivative = (pitchError - previousPitchError) / pitchSampleTime; + pitchIntegral += pitchError * pitchSampleTime; + + // PID Computation + float pitchControlEffort = (pitch_Kp * pitchError) + (pitch_Kd * pitchDerivative) + (pitch_Ki * pitchIntegral); + previousPitchError = pitchError; + + float pitchOutput = pitchMeasured + pitchControlEffort; + + // Clamp output. 100 is a placeholder. + if (pitchOutput > 100.0f) pitchOutput = 100.0f; + if (pitchOutput < 0.0f) pitchOutput = 0.0f; + + // Set controlInput + controlInputs.pitch = pitchOutput; return controlInputs; } \ No newline at end of file From dfc7ed48120849ab97f98f53e2125846185e232f Mon Sep 17 00:00:00 2001 From: Eeman Aleem Date: Sun, 6 Jul 2025 15:53:45 -0400 Subject: [PATCH 10/24] Cleaned up code Var names, formatting, file names, etc. --- .../attitude_manager/AM_PID_mapping.hpp | 38 +++++++++ zeropilot3.5/include/attitude_manager/PID.hpp | 37 --------- .../src/attitude_manager/AM_PID_mapping.cpp | 63 ++++++++++++++ zeropilot3.5/src/attitude_manager/PID.cpp | 82 ------------------- 4 files changed, 101 insertions(+), 119 deletions(-) create mode 100644 zeropilot3.5/include/attitude_manager/AM_PID_mapping.hpp delete mode 100644 zeropilot3.5/include/attitude_manager/PID.hpp create mode 100644 zeropilot3.5/src/attitude_manager/AM_PID_mapping.cpp delete mode 100644 zeropilot3.5/src/attitude_manager/PID.cpp diff --git a/zeropilot3.5/include/attitude_manager/AM_PID_mapping.hpp b/zeropilot3.5/include/attitude_manager/AM_PID_mapping.hpp new file mode 100644 index 00000000..507aa8b8 --- /dev/null +++ b/zeropilot3.5/include/attitude_manager/AM_PID_mapping.hpp @@ -0,0 +1,38 @@ +#pragma once + +#include "flightmode.hpp" + +class PID: public Flightmode { + public: + RCMotorControlMessage_t runControl(RCMotorControlMessage_t controlInput) override; + + // PID object constructor + PID(float KP, float KI, float KD, float tau, float limMin, float limMax, + float integral_min_lim, float integral_max_lim, float T); + + // PID object's state var initialized (or reset) + void PIDInitState(); + + // + float PIDOutput(float setpoint, float measurement); + + private: + // Gains + float KP, KI, KD; // PID constants - May choose these to be optimized real-time dep. on optimization alg. chosen + float tau; // Derivative low-pass filter constant + float T; // Sample time (set to AM_MAIN_DELAY) + + // Output and Integral Limits + float output_min_lim, output_max_lim; // Output limits + float integral_min_lim, integral_max_lim; // integral limits + + // State variables + float pid_derivative, pid_integral; + float prev_error, prev_measurement; + + // Control effort var - redundancy added for clarity + float pid_control_effort; + + // Output var + float pid_output; // Directly into motor control +}; diff --git a/zeropilot3.5/include/attitude_manager/PID.hpp b/zeropilot3.5/include/attitude_manager/PID.hpp deleted file mode 100644 index 72048069..00000000 --- a/zeropilot3.5/include/attitude_manager/PID.hpp +++ /dev/null @@ -1,37 +0,0 @@ -#pragma once - -#include "flightmode.hpp" - -class PID: public Flightmode { - public: - RCMotorControlMessage_t runControl(RCMotorControlMessage_t controlInput) override; - - - PID(float Kp, float Ki, float Kd, - float tau, - float limMin, float limMax, - float limMinInt, float limMaxInt, - float T); - void init(); - float update(float setpoint, float measurement); - private: - //gains - float Kp, Ki, Kd; // Gains - float tau; // Derivative low-pass filter constant - float T; // Sample time - - //limits - float limMinOutput, limMaxOutput; // Output limits - float limMinInt, limMaxInt; // Integrator limits - float integrator; - float prevError; - float differentiator; - float prevMeasurement; - - - // Output variable - float out; - - //Im gonna use Phils Lab iteration for the PID controller - -}; diff --git a/zeropilot3.5/src/attitude_manager/AM_PID_mapping.cpp b/zeropilot3.5/src/attitude_manager/AM_PID_mapping.cpp new file mode 100644 index 00000000..e122090a --- /dev/null +++ b/zeropilot3.5/src/attitude_manager/AM_PID_mapping.cpp @@ -0,0 +1,63 @@ +#include "PID.hpp" +#include "attitude_manager.hpp" + +// Constructor +PID::PID(float KP, float KI, float KD, + float tau, + float output_min_lim, float output_max_lim, + float integral_min_lim, float integral_max_lim, + float T): + KP(KP), KI(KI), KD(KD), tau(tau), T(AM_MAIN_DELAY), + output_min_lim(output_min_lim), output_max_lim(output_max_lim), + integral_min_lim(integral_min_lim), integral_max_lim(integral_max_lim) +{} + +// Initialization method - Can be used as resetter +void PID::PIDInitState() { + pid_integral = 0.0f; + prev_error = 0.0f; + pid_derivative = 0.0f; + prev_measurement = 0.0f; + pid_control_effort = 0.0f; + pid_output = 0.0f; +} + +// Update method +float PID::PIDOutput(float setpoint, float measurement) { + // Calculate error + float error = setpoint - measurement; + + // PID Proportional + float pid_proportional = KP * error; + + // PID Integral + pid_integral += (0.5f * KI) * (error + prev_error); + + // Anti-integral windup + if (pid_integral > integral_max_lim) { pid_integral = integral_max_lim; } + if (pid_integral < integral_min_lim) { pid_integral = integral_min_lim; } + + // PID Derivative with low-pass filter + pid_derivative = ((-1 * 2.0f * KD * (measurement - prev_measurement)) + ((2 * tau - T) * pid_derivative)) / ((2.0f * tau) + T); + + // PID control effort + output calc. + pid_control_effort = pid_proportional + pid_integral + pid_derivative; + pid_output = measurement + pid_control_effort; + + // Clamp output + if (out > output_max_lim) { pid_output = output_max_lim; } + if (out < output_min_lim) { pid_output = output_min_lim; } + + // Update previous values + prev_error = error; + prev_measurement = measurement; + + return pid_output; // Must go directly into motor control +} + +RCMotorControlMessage_t PIDMapping::runControl(RCMotorControlMessage_t controlInputs){ + +} + + + diff --git a/zeropilot3.5/src/attitude_manager/PID.cpp b/zeropilot3.5/src/attitude_manager/PID.cpp deleted file mode 100644 index 4e5ab333..00000000 --- a/zeropilot3.5/src/attitude_manager/PID.cpp +++ /dev/null @@ -1,82 +0,0 @@ -#include "PID.hpp" -#include "attitude_manager.hpp" -// RCMotorControlMessage_t PID::runControl(RCMotorControlMessage_t controlInputs){ -// // Current State -// float pitchMeasured = controlInputs.pitch; // What is the band of values this can take? Ask limit of pitch -// float pitchSetpoint = 50.0f; - -// // Error Calculation -// float pitchError = pitchSetpoint - pitchMeasured; -// float pitchDerivative = (pitchError - previousPitchError) / pitchSampleTime; -// pitchIntegral += pitchError * pitchSampleTime; - -// // PID Computation -// float pitchControlEffort = (pitch_Kp * pitchError) + (pitch_Kd * pitchDerivative) + (pitch_Ki * pitchIntegral); -// previousPitchError = pitchError; - -// float pitchOutput = pitchMeasured + pitchControlEffort; - -// // Clamp output. 100 is a placeholder. -// if (pitchOutput > 100.0f) pitchOutput = 100.0f; -// if (pitchOutput < 0.0f) pitchOutput = 0.0f; - -// // Set controlInput -// controlInputs.pitch = pitchOutput; - - //starting Phil code here (Template) - -// Implementation of PID methods (move class definition to header file PID.hpp) - -// Constructor -PID::PID(float Kp, float Ki, float Kd, - float tau, - float limMinOutput, float limMaxOutput, - float limMinInt, float limMaxInt, - float T) - : Kp(Kp), Ki(Ki), Kd(Kd), tau(tau), T(AM_MAIN_DELAY), - limMinOutput(limMinOutput), limMaxOutput(limMaxOutput), - limMinInt(limMinInt), limMaxInt(limMaxInt) -{} - -// Initialization method -void PID::init() { - integrator = 0.0f; - prevError = 0.0f; - differentiator = 0.0f; - prevMeasurement = 0.0f; - out = 0.0f; -} - -// Update method -float PID::update(float setpoint, float measurement) { - // Calculate error - float error = setpoint - measurement; - - float proportional = Kp * error; - - // Integral - integrator = integrator + (0.5f * Ki) * (error + prevError); - - // Anti integral windup - if (integrator > limMaxInt) integrator = limMaxInt; - if (integrator < limMinInt) integrator = limMinInt; - - // Calculate differentiator with low-pass filter - differentiator = (-(2.0f * Kd * (measurement - prevMeasurement)) + ((2 * tau - T) * differentiator)) /((2.0f * tau) + T); - - - // PID output calculation - out = proportional + integrator + differentiator; - - - // Clamp output - if (out > limMaxOutput) out = limMaxOutput; - if (out < limMinOutput) out = limMinOutput; - - // Update previous values - prevError = error; - prevMeasurement = measurement; - - return out; -} - From 9e46757aa12903ea458f8ff59c96b365e159f29a Mon Sep 17 00:00:00 2001 From: Eeman Aleem Date: Sun, 6 Jul 2025 15:56:14 -0400 Subject: [PATCH 11/24] Fixed include --- zeropilot3.5/src/attitude_manager/AM_PID_mapping.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/zeropilot3.5/src/attitude_manager/AM_PID_mapping.cpp b/zeropilot3.5/src/attitude_manager/AM_PID_mapping.cpp index e122090a..87fe8492 100644 --- a/zeropilot3.5/src/attitude_manager/AM_PID_mapping.cpp +++ b/zeropilot3.5/src/attitude_manager/AM_PID_mapping.cpp @@ -1,4 +1,4 @@ -#include "PID.hpp" +#include "AM_PID_mapping.hpp" #include "attitude_manager.hpp" // Constructor From aa0e7c0635857f309860859261bdd04fcd54517c Mon Sep 17 00:00:00 2001 From: Eeman Aleem Date: Sun, 6 Jul 2025 16:12:52 -0400 Subject: [PATCH 12/24] Added separate pid_mapping files Keeping pid calc files separate to allow general use of pid. --- .../{AM_PID_mapping.hpp => AM_pid.hpp} | 0 .../include/attitude_manager/pid_mapping.hpp | 20 +++++++++++++++++++ .../{AM_PID_mapping.cpp => AM_pid.cpp} | 6 +++--- .../src/attitude_manager/pid_mapping.cpp | 0 4 files changed, 23 insertions(+), 3 deletions(-) rename zeropilot3.5/include/attitude_manager/{AM_PID_mapping.hpp => AM_pid.hpp} (100%) create mode 100644 zeropilot3.5/include/attitude_manager/pid_mapping.hpp rename zeropilot3.5/src/attitude_manager/{AM_PID_mapping.cpp => AM_pid.cpp} (91%) create mode 100644 zeropilot3.5/src/attitude_manager/pid_mapping.cpp diff --git a/zeropilot3.5/include/attitude_manager/AM_PID_mapping.hpp b/zeropilot3.5/include/attitude_manager/AM_pid.hpp similarity index 100% rename from zeropilot3.5/include/attitude_manager/AM_PID_mapping.hpp rename to zeropilot3.5/include/attitude_manager/AM_pid.hpp diff --git a/zeropilot3.5/include/attitude_manager/pid_mapping.hpp b/zeropilot3.5/include/attitude_manager/pid_mapping.hpp new file mode 100644 index 00000000..e745d3d9 --- /dev/null +++ b/zeropilot3.5/include/attitude_manager/pid_mapping.hpp @@ -0,0 +1,20 @@ +#pragma once + +#include "flightmode.hpp" + +class PIDMapping : public Flightmode { + public: + PIDMapping() = default; + + RCMotorControlMessage_t runControl(RCMotorControlMessage_t controlInput) override; + private: + // Roll controls' PID constants + float roll_Kp = 0.0f; + static float roll_Ki = 0.0f; + float roll_Kd = 0.0f; + + // Pitch controls' PID constants + float pitch_Kp = 0.0f; + static float pitch_Ki = 0.0f; + float pitch_Kd = 0.0f; +}; \ No newline at end of file diff --git a/zeropilot3.5/src/attitude_manager/AM_PID_mapping.cpp b/zeropilot3.5/src/attitude_manager/AM_pid.cpp similarity index 91% rename from zeropilot3.5/src/attitude_manager/AM_PID_mapping.cpp rename to zeropilot3.5/src/attitude_manager/AM_pid.cpp index 87fe8492..3dbe1ce0 100644 --- a/zeropilot3.5/src/attitude_manager/AM_PID_mapping.cpp +++ b/zeropilot3.5/src/attitude_manager/AM_pid.cpp @@ -1,4 +1,4 @@ -#include "AM_PID_mapping.hpp" +#include "AM_pid.hpp" #include "attitude_manager.hpp" // Constructor @@ -45,8 +45,8 @@ float PID::PIDOutput(float setpoint, float measurement) { pid_output = measurement + pid_control_effort; // Clamp output - if (out > output_max_lim) { pid_output = output_max_lim; } - if (out < output_min_lim) { pid_output = output_min_lim; } + if (pid_output > output_max_lim) { pid_output = output_max_lim; } + if (pid_output < output_min_lim) { pid_output = output_min_lim; } // Update previous values prev_error = error; diff --git a/zeropilot3.5/src/attitude_manager/pid_mapping.cpp b/zeropilot3.5/src/attitude_manager/pid_mapping.cpp new file mode 100644 index 00000000..e69de29b From d718ecb9358d38215e657dd406b95bd4a7afdb25 Mon Sep 17 00:00:00 2001 From: JJKSweaty Date: Sun, 6 Jul 2025 19:53:19 -0400 Subject: [PATCH 13/24] Updated PID --- .../include/attitude_manager/AM_pid.hpp | 10 +++-- .../attitude_manager/pid_controller.hpp | 23 ----------- .../include/attitude_manager/pid_mapping.hpp | 40 +++++++++++++++++-- zeropilot3.5/src/attitude_manager/AM_pid.cpp | 11 ++--- .../src/attitude_manager/pid_mapping.cpp | 32 +++++++++++++++ 5 files changed, 78 insertions(+), 38 deletions(-) delete mode 100644 zeropilot3.5/include/attitude_manager/pid_controller.hpp diff --git a/zeropilot3.5/include/attitude_manager/AM_pid.hpp b/zeropilot3.5/include/attitude_manager/AM_pid.hpp index 507aa8b8..c6d17b95 100644 --- a/zeropilot3.5/include/attitude_manager/AM_pid.hpp +++ b/zeropilot3.5/include/attitude_manager/AM_pid.hpp @@ -1,13 +1,13 @@ #pragma once -#include "flightmode.hpp" -class PID: public Flightmode { +class PID { public: - RCMotorControlMessage_t runControl(RCMotorControlMessage_t controlInput) override; + // RCMotorControlMessage_t runControl(RCMotorControlMessage_t controlInput) override; // PID object constructor - PID(float KP, float KI, float KD, float tau, float limMin, float limMax, + PID(float KP, float KI, float KD, + float tau, float output_min_lim, float output_max_lim, float integral_min_lim, float integral_max_lim, float T); // PID object's state var initialized (or reset) @@ -16,6 +16,8 @@ class PID: public Flightmode { // float PIDOutput(float setpoint, float measurement); + ~ PID(); + private: // Gains float KP, KI, KD; // PID constants - May choose these to be optimized real-time dep. on optimization alg. chosen diff --git a/zeropilot3.5/include/attitude_manager/pid_controller.hpp b/zeropilot3.5/include/attitude_manager/pid_controller.hpp deleted file mode 100644 index e79f31bd..00000000 --- a/zeropilot3.5/include/attitude_manager/pid_controller.hpp +++ /dev/null @@ -1,23 +0,0 @@ -#pragma once -class PIDController { -public: - PIDController(float kp, float ki, float kd, float tau, float T, - float limMin, float limMax, - float limMinInt, float limMaxInt); - - void init(); - float update(float setpoint, float measurement, float feedforward = 0.0f); - -private: - float Kp, Ki, Kd; // Gains - float tau; // Derivative low-pass filter constant - float T; // Sample time - float limMinOutput, limMaxOutput; // Output limits - float limMinInt, limMaxInt; // Integrator limits - - float integrator; - float prevError; - float differentiator; - float prevMeasurement; - float out; -}; \ No newline at end of file diff --git a/zeropilot3.5/include/attitude_manager/pid_mapping.hpp b/zeropilot3.5/include/attitude_manager/pid_mapping.hpp index e745d3d9..3a76597e 100644 --- a/zeropilot3.5/include/attitude_manager/pid_mapping.hpp +++ b/zeropilot3.5/include/attitude_manager/pid_mapping.hpp @@ -1,20 +1,54 @@ #pragma once #include "flightmode.hpp" +#include "attitude_manager.hpp" +#include "AM_pid.hpp" class PIDMapping : public Flightmode { public: PIDMapping() = default; RCMotorControlMessage_t runControl(RCMotorControlMessage_t controlInput) override; + + void setRollPIDConstants(float Kp, float Ki, float Kd, float tau) { + roll_Kp = Kp; + roll_Ki = Ki; + roll_Kd = Kd; + roll_tau = tau; + + } + void setPitchPIDConstants(float Kp, float Ki, float Kd, float tau) { + pitch_Kp = Kp; + pitch_Ki = Ki; + pitch_Kd = Kd; + pitch_tau = tau; + } + private: // Roll controls' PID constants float roll_Kp = 0.0f; - static float roll_Ki = 0.0f; + float roll_Ki = 0.0f; float roll_Kd = 0.0f; - + // Roll integral limits + const float roll_integral_min_lim = 0.0f; + float roll_integral_max_lim = 0.0f; + // Pitch controls' PID constants float pitch_Kp = 0.0f; - static float pitch_Ki = 0.0f; + float pitch_Ki = 0.0f; float pitch_Kd = 0.0f; + // Pitch integral limits + float pitch_integral_min_lim; + float pitch_integral_max_lim; + + // Output limits + const float output_min = 0.0f; + const float output_max = 0.0f; + + // Tau constants + float roll_tau = 0.0f; + float pitch_tau = 0.0f; + + PID rollPID; + PID pitchPID; }; \ No newline at end of file diff --git a/zeropilot3.5/src/attitude_manager/AM_pid.cpp b/zeropilot3.5/src/attitude_manager/AM_pid.cpp index 3dbe1ce0..65a0f720 100644 --- a/zeropilot3.5/src/attitude_manager/AM_pid.cpp +++ b/zeropilot3.5/src/attitude_manager/AM_pid.cpp @@ -1,13 +1,10 @@ #include "AM_pid.hpp" -#include "attitude_manager.hpp" // Constructor PID::PID(float KP, float KI, float KD, - float tau, - float output_min_lim, float output_max_lim, - float integral_min_lim, float integral_max_lim, - float T): - KP(KP), KI(KI), KD(KD), tau(tau), T(AM_MAIN_DELAY), + float tau, float output_min_lim, float output_max_lim, + float integral_min_lim, float integral_max_lim, float T): + KP(KP), KI(KI), KD(KD), tau(tau), T(T), output_min_lim(output_min_lim), output_max_lim(output_max_lim), integral_min_lim(integral_min_lim), integral_max_lim(integral_max_lim) {} @@ -55,9 +52,7 @@ float PID::PIDOutput(float setpoint, float measurement) { return pid_output; // Must go directly into motor control } -RCMotorControlMessage_t PIDMapping::runControl(RCMotorControlMessage_t controlInputs){ -} diff --git a/zeropilot3.5/src/attitude_manager/pid_mapping.cpp b/zeropilot3.5/src/attitude_manager/pid_mapping.cpp index e69de29b..ada21285 100644 --- a/zeropilot3.5/src/attitude_manager/pid_mapping.cpp +++ b/zeropilot3.5/src/attitude_manager/pid_mapping.cpp @@ -0,0 +1,32 @@ +#include "PID_mapping.hpp" + +PIDMapping::PIDMapping(): + rollPID(roll_Kp, roll_Ki, roll_Kd, + roll_tau, output_min, output_max, + roll_integral_min_lim, roll_integral_max_lim, AM_MAIN_DELAY), + + pitchPID(pitch_Kp, pitch_Ki, pitch_Kd, + pitch_tau, output_min, output_max, + pitch_integral_min_lim, pitch_integral_max_lim, AM_MAIN_DELAY) +{ + rollPID.PIDInitState(); + pitchPID.PIDInitState(); +} + +RCMotorControlMessage_t PIDMapping::runControl(RCMotorControlMessage_t controlInputs){ + float roll_setpoint = controlInputs.roll; + float pitch_setpoint = controlInputs.pitch; + + // Get from sensor fusion + float roll_measured = 50.0; + float pitch_measured = 50.0; + + float roll_output = rollPID.PIDOutput(roll_setpoint, roll_measured); + float pitch_output = pitchPID.PIDOutput(pitch_setpoint, pitch_measured); + + controlInputs.roll = roll_output; // setting desired roll angle + controlInputs.pitch = pitch_output; // setting desired pitch angle + + return controlInputs; +} + From aa1bfc51a3824d773feb6ce9332d8acaa2a7bafb Mon Sep 17 00:00:00 2001 From: Eeman Aleem Date: Mon, 7 Jul 2025 08:20:11 -0400 Subject: [PATCH 14/24] Correct pid_control_effort calcs - Corrected mistake in accidentally clamping absolute position (pid_output) instead of pid_control_effort. - Added some comments. --- zeropilot3.5/include/attitude_manager/AM_pid.hpp | 7 +++---- .../include/attitude_manager/pid_mapping.hpp | 6 +++++- zeropilot3.5/src/attitude_manager/AM_pid.cpp | 12 +++++++----- zeropilot3.5/src/attitude_manager/pid_mapping.cpp | 3 ++- 4 files changed, 17 insertions(+), 11 deletions(-) diff --git a/zeropilot3.5/include/attitude_manager/AM_pid.hpp b/zeropilot3.5/include/attitude_manager/AM_pid.hpp index c6d17b95..81431eb3 100644 --- a/zeropilot3.5/include/attitude_manager/AM_pid.hpp +++ b/zeropilot3.5/include/attitude_manager/AM_pid.hpp @@ -13,10 +13,9 @@ class PID { // PID object's state var initialized (or reset) void PIDInitState(); - // + // Computes PID for a measurement with its desired setpoint passed in float PIDOutput(float setpoint, float measurement); - ~ PID(); private: // Gains @@ -32,9 +31,9 @@ class PID { float pid_derivative, pid_integral; float prev_error, prev_measurement; - // Control effort var - redundancy added for clarity + // Control effort var float pid_control_effort; - // Output var + // Output var, absolute position currently float pid_output; // Directly into motor control }; diff --git a/zeropilot3.5/include/attitude_manager/pid_mapping.hpp b/zeropilot3.5/include/attitude_manager/pid_mapping.hpp index 3a76597e..11aacce3 100644 --- a/zeropilot3.5/include/attitude_manager/pid_mapping.hpp +++ b/zeropilot3.5/include/attitude_manager/pid_mapping.hpp @@ -10,6 +10,7 @@ class PIDMapping : public Flightmode { RCMotorControlMessage_t runControl(RCMotorControlMessage_t controlInput) override; + // Setter *roll* for PID consts + time constant tau for the derivative low-pass filter void setRollPIDConstants(float Kp, float Ki, float Kd, float tau) { roll_Kp = Kp; roll_Ki = Ki; @@ -17,6 +18,8 @@ class PIDMapping : public Flightmode { roll_tau = tau; } + + // Setter for *pitch* PID consts + time constant tau for the derivative low-pass filter void setPitchPIDConstants(float Kp, float Ki, float Kd, float tau) { pitch_Kp = Kp; pitch_Ki = Ki; @@ -41,7 +44,7 @@ class PIDMapping : public Flightmode { float pitch_integral_min_lim; float pitch_integral_max_lim; - // Output limits + // Output limits (for control effort) const float output_min = 0.0f; const float output_max = 0.0f; @@ -49,6 +52,7 @@ class PIDMapping : public Flightmode { float roll_tau = 0.0f; float pitch_tau = 0.0f; + // Roll and Pitch PID class objects PID rollPID; PID pitchPID; }; \ No newline at end of file diff --git a/zeropilot3.5/src/attitude_manager/AM_pid.cpp b/zeropilot3.5/src/attitude_manager/AM_pid.cpp index 65a0f720..119ef4a3 100644 --- a/zeropilot3.5/src/attitude_manager/AM_pid.cpp +++ b/zeropilot3.5/src/attitude_manager/AM_pid.cpp @@ -37,18 +37,20 @@ float PID::PIDOutput(float setpoint, float measurement) { // PID Derivative with low-pass filter pid_derivative = ((-1 * 2.0f * KD * (measurement - prev_measurement)) + ((2 * tau - T) * pid_derivative)) / ((2.0f * tau) + T); - // PID control effort + output calc. + // PID control effort pid_control_effort = pid_proportional + pid_integral + pid_derivative; - pid_output = measurement + pid_control_effort; - // Clamp output - if (pid_output > output_max_lim) { pid_output = output_max_lim; } - if (pid_output < output_min_lim) { pid_output = output_min_lim; } + // Clamp control effort output + if (pid_control_effort > output_max_lim) { pid_control_effort = output_max_lim; } + if (pid_control_effort < output_min_lim) { pid_control_effort = output_min_lim; } // Update previous values prev_error = error; prev_measurement = measurement; + // Absolute motor control position + pid_output = measurement + pid_control_effort; + return pid_output; // Must go directly into motor control } diff --git a/zeropilot3.5/src/attitude_manager/pid_mapping.cpp b/zeropilot3.5/src/attitude_manager/pid_mapping.cpp index ada21285..b007fee7 100644 --- a/zeropilot3.5/src/attitude_manager/pid_mapping.cpp +++ b/zeropilot3.5/src/attitude_manager/pid_mapping.cpp @@ -17,10 +17,11 @@ RCMotorControlMessage_t PIDMapping::runControl(RCMotorControlMessage_t controlIn float roll_setpoint = controlInputs.roll; float pitch_setpoint = controlInputs.pitch; - // Get from sensor fusion + // Get from sensor fusion -- make sure to update include files when we get this float roll_measured = 50.0; float pitch_measured = 50.0; + // Currently, roll & pitch outputs receive absolute roll & pitch angles, not relative to current position. float roll_output = rollPID.PIDOutput(roll_setpoint, roll_measured); float pitch_output = pitchPID.PIDOutput(pitch_setpoint, pitch_measured); From ea2ad688a8aca286951055f200a76831ccb7dc63 Mon Sep 17 00:00:00 2001 From: Eeman Aleem Date: Thu, 10 Jul 2025 18:17:44 -0400 Subject: [PATCH 15/24] Update pid_mapping.cpp --- zeropilot3.5/src/attitude_manager/pid_mapping.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/zeropilot3.5/src/attitude_manager/pid_mapping.cpp b/zeropilot3.5/src/attitude_manager/pid_mapping.cpp index b007fee7..1b165801 100644 --- a/zeropilot3.5/src/attitude_manager/pid_mapping.cpp +++ b/zeropilot3.5/src/attitude_manager/pid_mapping.cpp @@ -1,4 +1,4 @@ -#include "PID_mapping.hpp" +#include "pid_mapping.hpp" PIDMapping::PIDMapping(): rollPID(roll_Kp, roll_Ki, roll_Kd, From 3df16bc2e889421e90c05f5d9682d09748cf86aa Mon Sep 17 00:00:00 2001 From: Eeman Aleem Date: Thu, 10 Jul 2025 18:58:55 -0400 Subject: [PATCH 16/24] Implemented lint check format stuff Issue with constructor (?) tryna fix. --- .../include/attitude_manager/AM_pid.hpp | 26 +++++----- .../attitude_manager/attitude_manager.hpp | 24 ++++----- .../include/attitude_manager/pid_mapping.hpp | 52 +++++++++---------- zeropilot3.5/src/attitude_manager/AM_pid.cpp | 52 +++++++++---------- .../src/attitude_manager/pid_mapping.cpp | 32 ++++++------ 5 files changed, 93 insertions(+), 93 deletions(-) diff --git a/zeropilot3.5/include/attitude_manager/AM_pid.hpp b/zeropilot3.5/include/attitude_manager/AM_pid.hpp index 81431eb3..9e21ce6b 100644 --- a/zeropilot3.5/include/attitude_manager/AM_pid.hpp +++ b/zeropilot3.5/include/attitude_manager/AM_pid.hpp @@ -6,34 +6,34 @@ class PID { // RCMotorControlMessage_t runControl(RCMotorControlMessage_t controlInput) override; // PID object constructor - PID(float KP, float KI, float KD, - float tau, float output_min_lim, float output_max_lim, - float integral_min_lim, float integral_max_lim, float T); + PID(float kp, float ki, float kd, + float tau, float outputMinLim, float outputMaxLim, + float integralMinLim, float integralMaxLim, float t); // PID object's state var initialized (or reset) - void PIDInitState(); + void pidInitState(); // Computes PID for a measurement with its desired setpoint passed in - float PIDOutput(float setpoint, float measurement); + float pidOutput(float setpoint, float measurement); private: // Gains - float KP, KI, KD; // PID constants - May choose these to be optimized real-time dep. on optimization alg. chosen + float kp, ki, kd; // PID constants - May choose these to be optimized real-time dep. on optimization alg. chosen float tau; // Derivative low-pass filter constant - float T; // Sample time (set to AM_MAIN_DELAY) + float t; // Sample time (set to AM_MAIN_DELAY) // Output and Integral Limits - float output_min_lim, output_max_lim; // Output limits - float integral_min_lim, integral_max_lim; // integral limits + float outputMinLim, outputMaxLim; // Output limits + float integralMinLim, integralMaxLim; // integral limits // State variables - float pid_derivative, pid_integral; - float prev_error, prev_measurement; + float pidDerivative, pidIntegral; + float prevError, prevMeasurement; // Control effort var - float pid_control_effort; + float pidControlEffort; // Output var, absolute position currently - float pid_output; // Directly into motor control + float pidOutput; // Directly into motor control }; diff --git a/zeropilot3.5/include/attitude_manager/attitude_manager.hpp b/zeropilot3.5/include/attitude_manager/attitude_manager.hpp index 96a9fa4f..5ac0f2e7 100644 --- a/zeropilot3.5/include/attitude_manager/attitude_manager.hpp +++ b/zeropilot3.5/include/attitude_manager/attitude_manager.hpp @@ -49,19 +49,19 @@ class AttitudeManager { MotorGroupInstance_t *flapMotors; MotorGroupInstance_t *steeringMotors; - bool if_rollMotors_invert=0; - bool if_pitchMotors_invert=0; - bool if_yawMotors_invert=0; - bool if_throttleMotors_invert=0; - bool if_flapMotors_invert=0; - bool if_steeringMotor_invert=0; + bool ifRollMotorsInvert=0; + bool ifPitchMotorsInvert=0; + bool ifYawMotorsInvert=0; + bool ifThrottleMotorsInvert=0; + bool ifFlapMotorsInvert=0; + bool ifSteeringMotorInvert=0; - float ROLLMOTORS_TRIM; - float PITCHMOTORS_TRIM; - float YAWMOTORS_TRIM; - float THROTTLEMOTORS_TRIM; - float FLAPMOTORS_TRIM; - float STEERINGMOTORS_TRIM; + float rollmotorsTrim; + float pitchmotorsTrim; + float yawmotorsTrim; + float throttlemotorsTrim; + float flapmotorsTrim; + float steeringmotorsTrim; float const ADVERSE_YAW_COEFFICIENT=1.5f; float adverseYaw = 0.0f; diff --git a/zeropilot3.5/include/attitude_manager/pid_mapping.hpp b/zeropilot3.5/include/attitude_manager/pid_mapping.hpp index 11aacce3..5c251690 100644 --- a/zeropilot3.5/include/attitude_manager/pid_mapping.hpp +++ b/zeropilot3.5/include/attitude_manager/pid_mapping.hpp @@ -6,51 +6,51 @@ class PIDMapping : public Flightmode { public: - PIDMapping() = default; + PIDMapping(); RCMotorControlMessage_t runControl(RCMotorControlMessage_t controlInput) override; // Setter *roll* for PID consts + time constant tau for the derivative low-pass filter - void setRollPIDConstants(float Kp, float Ki, float Kd, float tau) { - roll_Kp = Kp; - roll_Ki = Ki; - roll_Kd = Kd; - roll_tau = tau; + void setRollPIDConstants(float kp, float ki, float kd, float tau) { + roll_kp = kp; + roll_ki = ki; + roll_kd = kd; + rollTau = tau; } // Setter for *pitch* PID consts + time constant tau for the derivative low-pass filter - void setPitchPIDConstants(float Kp, float Ki, float Kd, float tau) { - pitch_Kp = Kp; - pitch_Ki = Ki; - pitch_Kd = Kd; - pitch_tau = tau; + void setPitchPIDConstants(float kp, float ki, float kd, float tau) { + pitch_kp = kp; + pitch_ki = ki; + pitch_kd = kd; + pitchTau = tau; } private: // Roll controls' PID constants - float roll_Kp = 0.0f; - float roll_Ki = 0.0f; - float roll_Kd = 0.0f; + float roll_kp = 0.0f; + float roll_ki = 0.0f; + float roll_kd = 0.0f; // Roll integral limits - const float roll_integral_min_lim = 0.0f; - float roll_integral_max_lim = 0.0f; + const float ROLL_INTEGRAL_MIN_LIM = 0.0f; + const float ROLL_INTEGRAL_MAX_LIM = 0.0f; // Pitch controls' PID constants - float pitch_Kp = 0.0f; - float pitch_Ki = 0.0f; - float pitch_Kd = 0.0f; + float pitch_kp = 0.0f; + float pitch_ki = 0.0f; + float pitch_kd = 0.0f; // Pitch integral limits - float pitch_integral_min_lim; - float pitch_integral_max_lim; + const float PITCH_INTEGRAL_MIN_LIM; + const float PITCH_INTEGRAL_MAX_LIM; // Output limits (for control effort) - const float output_min = 0.0f; - const float output_max = 0.0f; + const float OUTPUT_MIN = 0.0f; + const float OUTPUT_MAX = 0.0f; - // Tau constants - float roll_tau = 0.0f; - float pitch_tau = 0.0f; + // tau constants + float rollTau = 0.0f; + float pitchTau = 0.0f; // Roll and Pitch PID class objects PID rollPID; diff --git a/zeropilot3.5/src/attitude_manager/AM_pid.cpp b/zeropilot3.5/src/attitude_manager/AM_pid.cpp index 119ef4a3..89f150f4 100644 --- a/zeropilot3.5/src/attitude_manager/AM_pid.cpp +++ b/zeropilot3.5/src/attitude_manager/AM_pid.cpp @@ -1,57 +1,57 @@ #include "AM_pid.hpp" // Constructor -PID::PID(float KP, float KI, float KD, - float tau, float output_min_lim, float output_max_lim, - float integral_min_lim, float integral_max_lim, float T): - KP(KP), KI(KI), KD(KD), tau(tau), T(T), - output_min_lim(output_min_lim), output_max_lim(output_max_lim), - integral_min_lim(integral_min_lim), integral_max_lim(integral_max_lim) +PID::PID(float kp, float ki, float kd, + float tau, float outputMinLim, float outputMaxLim, + float integralMinLim, float integralMaxLim, float t): + 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() { - pid_integral = 0.0f; - prev_error = 0.0f; - pid_derivative = 0.0f; - prev_measurement = 0.0f; - pid_control_effort = 0.0f; - pid_output = 0.0f; +void PID::pidInitState() { + pidIntegral = 0.0f; + prevError = 0.0f; + pidDerivative = 0.0f; + prevMeasurement = 0.0f; + pidControlEffort = 0.0f; + pidOutput = 0.0f; } // Update method -float PID::PIDOutput(float setpoint, float measurement) { +float PID::pidOutput(float setpoint, float measurement) { // Calculate error float error = setpoint - measurement; // PID Proportional - float pid_proportional = KP * error; + float pidProportional = kp * error; // PID Integral - pid_integral += (0.5f * KI) * (error + prev_error); + pidIntegral += (0.5f * ki) * (error + prevError); // Anti-integral windup - if (pid_integral > integral_max_lim) { pid_integral = integral_max_lim; } - if (pid_integral < integral_min_lim) { pid_integral = integral_min_lim; } + if (pidIntegral > integralMaxLim) { pidIntegral = integralMaxLim; } + if (pidIntegral < integralMinLim) { pidIntegral = integralMinLim; } // PID Derivative with low-pass filter - pid_derivative = ((-1 * 2.0f * KD * (measurement - prev_measurement)) + ((2 * tau - T) * pid_derivative)) / ((2.0f * tau) + T); + pidDerivative = ((-1 * 2.0f * kd * (measurement - prevMeasurement)) + ((2 * tau - t) * pidDerivative)) / ((2.0f * tau) + t); // PID control effort - pid_control_effort = pid_proportional + pid_integral + pid_derivative; + pidControlEffort = pidProportional + pidIntegral + pidDerivative; // Clamp control effort output - if (pid_control_effort > output_max_lim) { pid_control_effort = output_max_lim; } - if (pid_control_effort < output_min_lim) { pid_control_effort = output_min_lim; } + if (pidControlEffort > outputMaxLim) { pidControlEffort = outputMaxLim; } + if (pidControlEffort < outputMinLim) { pidControlEffort = outputMinLim; } // Update previous values - prev_error = error; - prev_measurement = measurement; + prevError = error; + prevMeasurement = measurement; // Absolute motor control position - pid_output = measurement + pid_control_effort; + pidOutput = measurement + pidControlEffort; - return pid_output; // Must go directly into motor control + return pidOutput; // Must go directly into motor control } diff --git a/zeropilot3.5/src/attitude_manager/pid_mapping.cpp b/zeropilot3.5/src/attitude_manager/pid_mapping.cpp index 1b165801..5fe105ef 100644 --- a/zeropilot3.5/src/attitude_manager/pid_mapping.cpp +++ b/zeropilot3.5/src/attitude_manager/pid_mapping.cpp @@ -1,32 +1,32 @@ #include "pid_mapping.hpp" PIDMapping::PIDMapping(): - rollPID(roll_Kp, roll_Ki, roll_Kd, - roll_tau, output_min, output_max, - roll_integral_min_lim, roll_integral_max_lim, AM_MAIN_DELAY), + rollPID(roll_kp, roll_ki, roll_kd, + rollTau, OUTPUT_MIN, OUTPUT_MAX, + ROLL_INTEGRAL_MIN_LIM, ROLL_INTEGRAL_MAX_LIM, AM_MAIN_DELAY), - pitchPID(pitch_Kp, pitch_Ki, pitch_Kd, - pitch_tau, output_min, output_max, - pitch_integral_min_lim, pitch_integral_max_lim, AM_MAIN_DELAY) + pitchPID(pitch_kp, pitch_ki, pitch_kd, + pitchTau, OUTPUT_MIN, OUTPUT_MAX, + PITCH_INTEGRAL_MIN_LIM, PITCH_INTEGRAL_MAX_LIM, AM_MAIN_DELAY) { - rollPID.PIDInitState(); - pitchPID.PIDInitState(); + rollPID.pidInitState(); + pitchPID.pidInitState(); } RCMotorControlMessage_t PIDMapping::runControl(RCMotorControlMessage_t controlInputs){ - float roll_setpoint = controlInputs.roll; - float pitch_setpoint = controlInputs.pitch; + float rollSetpoint = controlInputs.roll; + float pitchSetpoint = controlInputs.pitch; // Get from sensor fusion -- make sure to update include files when we get this - float roll_measured = 50.0; - float pitch_measured = 50.0; + float rollMeasured = 50.0; + float pitchMeasured = 50.0; // Currently, roll & pitch outputs receive absolute roll & pitch angles, not relative to current position. - float roll_output = rollPID.PIDOutput(roll_setpoint, roll_measured); - float pitch_output = pitchPID.PIDOutput(pitch_setpoint, pitch_measured); + float rollOutput = rollPID.pidOutput(rollSetpoint, rollMeasured); + float pitchOutput = pitchPID.pidOutput(pitchSetpoint, pitchMeasured); - controlInputs.roll = roll_output; // setting desired roll angle - controlInputs.pitch = pitch_output; // setting desired pitch angle + controlInputs.roll = rollOutput; // setting desired roll angle + controlInputs.pitch = pitchOutput; // setting desired pitch angle return controlInputs; } From c16d7241f16e678e2500ed92eccc11b7e187e11a Mon Sep 17 00:00:00 2001 From: Eeman Aleem Date: Thu, 10 Jul 2025 19:06:45 -0400 Subject: [PATCH 17/24] Update attitude_manager.cpp --- .../src/attitude_manager/attitude_manager.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/zeropilot3.5/src/attitude_manager/attitude_manager.cpp b/zeropilot3.5/src/attitude_manager/attitude_manager.cpp index a61d0509..d92fd05d 100644 --- a/zeropilot3.5/src/attitude_manager/attitude_manager.cpp +++ b/zeropilot3.5/src/attitude_manager/attitude_manager.cpp @@ -77,12 +77,12 @@ void AttitudeManager::runControlLoopIteration() { motorOutputs.yaw = 0; } - outputToMotor(YAW, (100-motorOutputs.yaw)*if_yawMotors_invert+motorOutputs.yaw*(if_yawMotors_invert-1)+YAWMOTORS_TRIM); - outputToMotor(PITCH, (100-motorOutputs.pitch)*if_pitchMotors_invert+motorOutputs.pitch*(if_pitchMotors_invert-1)+PITCHMOTORS_TRIM); - outputToMotor(ROLL, (100-motorOutputs.roll)*if_rollMotors_invert+motorOutputs.roll*(if_rollMotors_invert-1)+ROLLMOTORS_TRIM); - outputToMotor(THROTTLE, (100-motorOutputs.throttle)*if_throttleMotors_invert+motorOutputs.throttle*(if_throttleMotors_invert-1)+THROTTLEMOTORS_TRIM); - outputToMotor(FLAP_ANGLE, (100-motorOutputs.flapAngle)*if_flapMotors_invert+motorOutputs.flapAngle*(if_flapMotors_invert-1)+FLAPMOTORS_TRIM); - outputToMotor(STEERING, motorOutputs.yaw+STEERINGMOTORS_TRIM); + outputToMotor(YAW, (100-motorOutputs.yaw)*ifYawMotorsInvert+motorOutputs.yaw*(ifYawMotorsInvert-1)+yawmotorsTrim); + outputToMotor(PITCH, (100-motorOutputs.pitch)*ifPitchMotorsInvert+motorOutputs.pitch*(ifPitchMotorsInvert-1)+pitchmotorsTrim); + outputToMotor(ROLL, (100-motorOutputs.roll)*ifRollMotorsInvert+motorOutputs.roll*(ifRollMotorsInvert-1)+rollmotorsTrim); + outputToMotor(THROTTLE, (100-motorOutputs.throttle)*ifThrottleMotorsInvert+motorOutputs.throttle*(ifThrottleMotorsInvert-1)+throttlemotorsTrim); + outputToMotor(FLAP_ANGLE, (100-motorOutputs.flapAngle)*ifFlapMotorsInvert+motorOutputs.flapAngle*(ifFlapMotorsInvert-1)+flapmotorsTrim); + outputToMotor(STEERING, motorOutputs.yaw+steeringmotorsTrim); } bool AttitudeManager::getControlInputs(RCMotorControlMessage_t *pControlMsg) { From 47001d7c7b6b4184beac34200eb0003b3d7a939b Mon Sep 17 00:00:00 2001 From: Eeman Aleem Date: Thu, 10 Jul 2025 19:35:35 -0400 Subject: [PATCH 18/24] Lint format + rollPID & pitchPID object functionality fixed - Fixed lint check's case style errors - Made rollPID & pitchPID into ptrs and added ptr stuff wherever they're used. --- .../{pid_mapping.hpp => fwba_mapping.hpp} | 31 ++++++++++--------- .../attitude_manager/{AM_pid.hpp => pid.hpp} | 6 ++-- .../{pid_mapping.cpp => fwba_mapping.cpp} | 14 ++++----- .../attitude_manager/{AM_pid.cpp => pid.cpp} | 8 ++--- 4 files changed, 29 insertions(+), 30 deletions(-) rename zeropilot3.5/include/attitude_manager/{pid_mapping.hpp => fwba_mapping.hpp} (71%) rename zeropilot3.5/include/attitude_manager/{AM_pid.hpp => pid.hpp} (78%) rename zeropilot3.5/src/attitude_manager/{pid_mapping.cpp => fwba_mapping.cpp} (72%) rename zeropilot3.5/src/attitude_manager/{AM_pid.cpp => pid.cpp} (91%) diff --git a/zeropilot3.5/include/attitude_manager/pid_mapping.hpp b/zeropilot3.5/include/attitude_manager/fwba_mapping.hpp similarity index 71% rename from zeropilot3.5/include/attitude_manager/pid_mapping.hpp rename to zeropilot3.5/include/attitude_manager/fwba_mapping.hpp index 5c251690..c12f2579 100644 --- a/zeropilot3.5/include/attitude_manager/pid_mapping.hpp +++ b/zeropilot3.5/include/attitude_manager/fwba_mapping.hpp @@ -2,7 +2,7 @@ #include "flightmode.hpp" #include "attitude_manager.hpp" -#include "AM_pid.hpp" +#include "pid.hpp" class PIDMapping : public Flightmode { public: @@ -12,34 +12,35 @@ class PIDMapping : public Flightmode { // Setter *roll* for PID consts + time constant tau for the derivative low-pass filter void setRollPIDConstants(float kp, float ki, float kd, float tau) { - roll_kp = kp; - roll_ki = ki; - roll_kd = kd; + rollKp = kp; + rollKi = ki; + rollKd = kd; rollTau = tau; } // Setter for *pitch* PID consts + time constant tau for the derivative low-pass filter void setPitchPIDConstants(float kp, float ki, float kd, float tau) { - pitch_kp = kp; - pitch_ki = ki; - pitch_kd = kd; + pitchKp = kp; + pitchKi = ki; + pitchKd = kd; pitchTau = tau; } private: + // For the PID roll & pitch consts -> may choose these to be optimized real-time dep. on optimization alg. chosen // Roll controls' PID constants - float roll_kp = 0.0f; - float roll_ki = 0.0f; - float roll_kd = 0.0f; + float rollKp = 0.0f; + float rollKi = 0.0f; + float rollKd = 0.0f; // Roll integral limits const float ROLL_INTEGRAL_MIN_LIM = 0.0f; const float ROLL_INTEGRAL_MAX_LIM = 0.0f; // Pitch controls' PID constants - float pitch_kp = 0.0f; - float pitch_ki = 0.0f; - float pitch_kd = 0.0f; + float pitchKp = 0.0f; + float pitchKi = 0.0f; + float pitchKd = 0.0f; // Pitch integral limits const float PITCH_INTEGRAL_MIN_LIM; const float PITCH_INTEGRAL_MAX_LIM; @@ -53,6 +54,6 @@ class PIDMapping : public Flightmode { float pitchTau = 0.0f; // Roll and Pitch PID class objects - PID rollPID; - PID pitchPID; + PID *rollPID; + PID *pitchPID; }; \ No newline at end of file diff --git a/zeropilot3.5/include/attitude_manager/AM_pid.hpp b/zeropilot3.5/include/attitude_manager/pid.hpp similarity index 78% rename from zeropilot3.5/include/attitude_manager/AM_pid.hpp rename to zeropilot3.5/include/attitude_manager/pid.hpp index 9e21ce6b..14c190ad 100644 --- a/zeropilot3.5/include/attitude_manager/AM_pid.hpp +++ b/zeropilot3.5/include/attitude_manager/pid.hpp @@ -3,8 +3,6 @@ class PID { public: - // RCMotorControlMessage_t runControl(RCMotorControlMessage_t controlInput) override; - // PID object constructor PID(float kp, float ki, float kd, float tau, float outputMinLim, float outputMaxLim, @@ -19,7 +17,7 @@ class PID { private: // Gains - float kp, ki, kd; // PID constants - May choose these to be optimized real-time dep. on optimization alg. chosen + float kp, ki, kd; // PID constants float tau; // Derivative low-pass filter constant float t; // Sample time (set to AM_MAIN_DELAY) @@ -35,5 +33,5 @@ class PID { float pidControlEffort; // Output var, absolute position currently - float pidOutput; // Directly into motor control + float pidOut; // Directly into motor control }; diff --git a/zeropilot3.5/src/attitude_manager/pid_mapping.cpp b/zeropilot3.5/src/attitude_manager/fwba_mapping.cpp similarity index 72% rename from zeropilot3.5/src/attitude_manager/pid_mapping.cpp rename to zeropilot3.5/src/attitude_manager/fwba_mapping.cpp index 5fe105ef..6e6bf285 100644 --- a/zeropilot3.5/src/attitude_manager/pid_mapping.cpp +++ b/zeropilot3.5/src/attitude_manager/fwba_mapping.cpp @@ -1,16 +1,16 @@ -#include "pid_mapping.hpp" +#include "fwba_mapping.hpp" PIDMapping::PIDMapping(): - rollPID(roll_kp, roll_ki, roll_kd, + rollPID = new PID(rollKp, rollKi, rollKd, rollTau, OUTPUT_MIN, OUTPUT_MAX, ROLL_INTEGRAL_MIN_LIM, ROLL_INTEGRAL_MAX_LIM, AM_MAIN_DELAY), - pitchPID(pitch_kp, pitch_ki, pitch_kd, + pitchPID = new PID(pitchKp, pitchKi, pitchKd, pitchTau, OUTPUT_MIN, OUTPUT_MAX, PITCH_INTEGRAL_MIN_LIM, PITCH_INTEGRAL_MAX_LIM, AM_MAIN_DELAY) { - rollPID.pidInitState(); - pitchPID.pidInitState(); + rollPID->pidInitState(); + pitchPID->pidInitState(); } RCMotorControlMessage_t PIDMapping::runControl(RCMotorControlMessage_t controlInputs){ @@ -22,8 +22,8 @@ RCMotorControlMessage_t PIDMapping::runControl(RCMotorControlMessage_t controlIn 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); + 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 diff --git a/zeropilot3.5/src/attitude_manager/AM_pid.cpp b/zeropilot3.5/src/attitude_manager/pid.cpp similarity index 91% rename from zeropilot3.5/src/attitude_manager/AM_pid.cpp rename to zeropilot3.5/src/attitude_manager/pid.cpp index 89f150f4..714a0c8d 100644 --- a/zeropilot3.5/src/attitude_manager/AM_pid.cpp +++ b/zeropilot3.5/src/attitude_manager/pid.cpp @@ -1,4 +1,4 @@ -#include "AM_pid.hpp" +#include "pid.hpp" // Constructor PID::PID(float kp, float ki, float kd, @@ -16,7 +16,7 @@ void PID::pidInitState() { pidDerivative = 0.0f; prevMeasurement = 0.0f; pidControlEffort = 0.0f; - pidOutput = 0.0f; + pidOut = 0.0f; } // Update method @@ -49,9 +49,9 @@ float PID::pidOutput(float setpoint, float measurement) { prevMeasurement = measurement; // Absolute motor control position - pidOutput = measurement + pidControlEffort; + pidOut = measurement + pidControlEffort; - return pidOutput; // Must go directly into motor control + return pidOut; // Must go directly into motor control } From 9821b58d525b45806f4d04417ddbf0bff7f485ec Mon Sep 17 00:00:00 2001 From: Eeman Aleem Date: Thu, 10 Jul 2025 20:23:24 -0400 Subject: [PATCH 19/24] More rollPID & pitchPID adjustments - Removed redundant vars for roll & pitch i.e. PID consts and tau. Note: tau is updated in an alg. whenever KD is. - Fixed constructor for roll and pitch PID. - Linked setters for roll & pitch to setter in pid class. - Added destructors. --- .../include/attitude_manager/fwba_mapping.hpp | 40 ++++++------------- zeropilot3.5/include/attitude_manager/pid.hpp | 3 ++ .../src/attitude_manager/fwba_mapping.cpp | 20 ++++++---- zeropilot3.5/src/attitude_manager/pid.cpp | 12 +++--- 4 files changed, 34 insertions(+), 41 deletions(-) diff --git a/zeropilot3.5/include/attitude_manager/fwba_mapping.hpp b/zeropilot3.5/include/attitude_manager/fwba_mapping.hpp index c12f2579..5617eec9 100644 --- a/zeropilot3.5/include/attitude_manager/fwba_mapping.hpp +++ b/zeropilot3.5/include/attitude_manager/fwba_mapping.hpp @@ -10,49 +10,33 @@ class PIDMapping : public Flightmode { RCMotorControlMessage_t runControl(RCMotorControlMessage_t controlInput) override; - // Setter *roll* for PID consts + time constant tau for the derivative low-pass filter - void setRollPIDConstants(float kp, float ki, float kd, float tau) { - rollKp = kp; - rollKi = ki; - rollKd = kd; - rollTau = tau; - + // Setter *roll* for PID consts + void setRollPIDConstants(float newKp, float newKi, float newKd, float newTau) { + rollPID->setConstants(newKp, newKi, newKd, newTau); } - // Setter for *pitch* PID consts + time constant tau for the derivative low-pass filter - void setPitchPIDConstants(float kp, float ki, float kd, float tau) { - pitchKp = kp; - pitchKi = ki; - pitchKd = kd; - pitchTau = tau; + // Setter for *pitch* PID consts + void setPitchPIDConstants(float newKp, float newKi, float newKd, float newTau) { + pitchPID->setConstants(newKp, newKi, newKd, newTau); } + + // Destructor + ~PIDMapping() override; private: - // For the PID roll & pitch consts -> may choose these to be optimized real-time dep. on optimization alg. chosen - // Roll controls' PID constants - float rollKp = 0.0f; - float rollKi = 0.0f; - float rollKd = 0.0f; + // Roll integral limits const float ROLL_INTEGRAL_MIN_LIM = 0.0f; const float ROLL_INTEGRAL_MAX_LIM = 0.0f; - // Pitch controls' PID constants - float pitchKp = 0.0f; - float pitchKi = 0.0f; - float pitchKd = 0.0f; // Pitch integral limits - const float PITCH_INTEGRAL_MIN_LIM; - const float PITCH_INTEGRAL_MAX_LIM; + const float PITCH_INTEGRAL_MIN_LIM = 0.0f; + const float PITCH_INTEGRAL_MAX_LIM = 0.0f; // Output limits (for control effort) const float OUTPUT_MIN = 0.0f; const float OUTPUT_MAX = 0.0f; - // tau constants - float rollTau = 0.0f; - float pitchTau = 0.0f; - // Roll and Pitch PID class objects PID *rollPID; PID *pitchPID; diff --git a/zeropilot3.5/include/attitude_manager/pid.hpp b/zeropilot3.5/include/attitude_manager/pid.hpp index 14c190ad..607e659a 100644 --- a/zeropilot3.5/include/attitude_manager/pid.hpp +++ b/zeropilot3.5/include/attitude_manager/pid.hpp @@ -7,6 +7,9 @@ class PID { PID(float kp, float ki, float kd, float tau, float outputMinLim, float outputMaxLim, float integralMinLim, float integralMaxLim, float t); + + // 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); // PID object's state var initialized (or reset) void pidInitState(); diff --git a/zeropilot3.5/src/attitude_manager/fwba_mapping.cpp b/zeropilot3.5/src/attitude_manager/fwba_mapping.cpp index 6e6bf285..c6556f4f 100644 --- a/zeropilot3.5/src/attitude_manager/fwba_mapping.cpp +++ b/zeropilot3.5/src/attitude_manager/fwba_mapping.cpp @@ -1,18 +1,22 @@ #include "fwba_mapping.hpp" -PIDMapping::PIDMapping(): - rollPID = new PID(rollKp, rollKi, rollKd, - rollTau, OUTPUT_MIN, OUTPUT_MAX, - ROLL_INTEGRAL_MIN_LIM, ROLL_INTEGRAL_MAX_LIM, AM_MAIN_DELAY), +PIDMapping::PIDMapping() { + rollPID = new PID(0.0f, 0.0f, 0.0f, + 0.0f, OUTPUT_MIN, OUTPUT_MAX, + ROLL_INTEGRAL_MIN_LIM, ROLL_INTEGRAL_MAX_LIM, AM_MAIN_DELAY); - pitchPID = new PID(pitchKp, pitchKi, pitchKd, - pitchTau, OUTPUT_MIN, OUTPUT_MAX, - PITCH_INTEGRAL_MIN_LIM, PITCH_INTEGRAL_MAX_LIM, AM_MAIN_DELAY) -{ + pitchPID = new PID(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(); } +PIDMapping::~PIDMapping() { + delete rollPID; + delete pitchPID; +} + RCMotorControlMessage_t PIDMapping::runControl(RCMotorControlMessage_t controlInputs){ float rollSetpoint = controlInputs.roll; float pitchSetpoint = controlInputs.pitch; diff --git a/zeropilot3.5/src/attitude_manager/pid.cpp b/zeropilot3.5/src/attitude_manager/pid.cpp index 714a0c8d..d7acfa9e 100644 --- a/zeropilot3.5/src/attitude_manager/pid.cpp +++ b/zeropilot3.5/src/attitude_manager/pid.cpp @@ -19,6 +19,13 @@ void PID::pidInitState() { pidOut = 0.0f; } +void PID::setConstants(float newKp, float newKi, float newKd, float newTau) { + kp = newKp; + ki = newKi; + kd = newKd; + tau = newTau; +} + // Update method float PID::pidOutput(float setpoint, float measurement) { // Calculate error @@ -53,8 +60,3 @@ float PID::pidOutput(float setpoint, float measurement) { return pidOut; // Must go directly into motor control } - - - - - From bd2ac4930887ee7cdde7a754c9db0194ece5d6bb Mon Sep 17 00:00:00 2001 From: eGuitar999 Date: Fri, 18 Jul 2025 01:07:17 -0400 Subject: [PATCH 20/24] remove rudder mixing branch changes --- .../attitude_manager/attitude_manager.hpp | 19 ------------- .../src/attitude_manager/attitude_manager.cpp | 27 +++++-------------- 2 files changed, 7 insertions(+), 39 deletions(-) diff --git a/zeropilot3.5/include/attitude_manager/attitude_manager.hpp b/zeropilot3.5/include/attitude_manager/attitude_manager.hpp index 5ac0f2e7..3a2784e1 100644 --- a/zeropilot3.5/include/attitude_manager/attitude_manager.hpp +++ b/zeropilot3.5/include/attitude_manager/attitude_manager.hpp @@ -8,7 +8,6 @@ #define AM_MAIN_DELAY 50 - typedef enum { YAW = 0, PITCH, @@ -49,24 +48,6 @@ class AttitudeManager { MotorGroupInstance_t *flapMotors; MotorGroupInstance_t *steeringMotors; - bool ifRollMotorsInvert=0; - bool ifPitchMotorsInvert=0; - bool ifYawMotorsInvert=0; - bool ifThrottleMotorsInvert=0; - bool ifFlapMotorsInvert=0; - bool ifSteeringMotorInvert=0; - - float rollmotorsTrim; - float pitchmotorsTrim; - float yawmotorsTrim; - float throttlemotorsTrim; - float flapmotorsTrim; - float steeringmotorsTrim; - - float const ADVERSE_YAW_COEFFICIENT=1.5f; - float adverseYaw = 0.0f; - float signedYaw = 0.0f; - bool getControlInputs(RCMotorControlMessage_t *pControlMsg); void outputToMotor(ControlAxis_e axis, uint8_t percent); diff --git a/zeropilot3.5/src/attitude_manager/attitude_manager.cpp b/zeropilot3.5/src/attitude_manager/attitude_manager.cpp index d92fd05d..6ed3b8b2 100644 --- a/zeropilot3.5/src/attitude_manager/attitude_manager.cpp +++ b/zeropilot3.5/src/attitude_manager/attitude_manager.cpp @@ -1,7 +1,6 @@ #include "attitude_manager.hpp" #include "rc_motor_control.hpp" - AttitudeManager::AttitudeManager( IMessageQueue *amQueue, IMessageQueue *smLoggerQueue, @@ -66,23 +65,12 @@ void AttitudeManager::runControlLoopIteration() { RCMotorControlMessage_t motorOutputs = controlAlgorithm->runControl(controlMsg); - signedYaw = motorOutputs.roll-50; - adverseYaw = signedYaw * ADVERSE_YAW_COEFFICIENT; - motorOutputs.yaw +=adverseYaw; - // limit yaw to 100 - if (motorOutputs.yaw>100){ - motorOutputs.yaw = 100; - //limit yaw to 0 - } else if (motorOutputs.yaw < 0) { - motorOutputs.yaw = 0; - } - - outputToMotor(YAW, (100-motorOutputs.yaw)*ifYawMotorsInvert+motorOutputs.yaw*(ifYawMotorsInvert-1)+yawmotorsTrim); - outputToMotor(PITCH, (100-motorOutputs.pitch)*ifPitchMotorsInvert+motorOutputs.pitch*(ifPitchMotorsInvert-1)+pitchmotorsTrim); - outputToMotor(ROLL, (100-motorOutputs.roll)*ifRollMotorsInvert+motorOutputs.roll*(ifRollMotorsInvert-1)+rollmotorsTrim); - outputToMotor(THROTTLE, (100-motorOutputs.throttle)*ifThrottleMotorsInvert+motorOutputs.throttle*(ifThrottleMotorsInvert-1)+throttlemotorsTrim); - outputToMotor(FLAP_ANGLE, (100-motorOutputs.flapAngle)*ifFlapMotorsInvert+motorOutputs.flapAngle*(ifFlapMotorsInvert-1)+flapmotorsTrim); - outputToMotor(STEERING, motorOutputs.yaw+steeringmotorsTrim); + outputToMotor(YAW, motorOutputs.yaw); + outputToMotor(PITCH, motorOutputs.pitch); + outputToMotor(ROLL, motorOutputs.roll); + outputToMotor(THROTTLE, motorOutputs.throttle); + outputToMotor(FLAP_ANGLE, motorOutputs.flapAngle); + outputToMotor(STEERING, motorOutputs.yaw); } bool AttitudeManager::getControlInputs(RCMotorControlMessage_t *pControlMsg) { @@ -130,5 +118,4 @@ void AttitudeManager::outputToMotor(ControlAxis_e axis, uint8_t percent) { motor->motorInstance->set(percent); } } -} - +} \ No newline at end of file From 1f22ab05a579e0dba1e18c2e14feaf97777ba4f4 Mon Sep 17 00:00:00 2001 From: Eeman Aleem Date: Mon, 21 Jul 2025 00:07:51 -0400 Subject: [PATCH 21/24] Removed pidOut and pidControlEffort as class members --- zeropilot3.5/include/attitude_manager/pid.hpp | 6 ------ zeropilot3.5/src/attitude_manager/pid.cpp | 6 ++---- 2 files changed, 2 insertions(+), 10 deletions(-) diff --git a/zeropilot3.5/include/attitude_manager/pid.hpp b/zeropilot3.5/include/attitude_manager/pid.hpp index 607e659a..44c70d63 100644 --- a/zeropilot3.5/include/attitude_manager/pid.hpp +++ b/zeropilot3.5/include/attitude_manager/pid.hpp @@ -31,10 +31,4 @@ class PID { // State variables float pidDerivative, pidIntegral; float prevError, prevMeasurement; - - // Control effort var - float pidControlEffort; - - // Output var, absolute position currently - float pidOut; // Directly into motor control }; diff --git a/zeropilot3.5/src/attitude_manager/pid.cpp b/zeropilot3.5/src/attitude_manager/pid.cpp index d7acfa9e..e8410279 100644 --- a/zeropilot3.5/src/attitude_manager/pid.cpp +++ b/zeropilot3.5/src/attitude_manager/pid.cpp @@ -15,8 +15,6 @@ void PID::pidInitState() { prevError = 0.0f; pidDerivative = 0.0f; prevMeasurement = 0.0f; - pidControlEffort = 0.0f; - pidOut = 0.0f; } void PID::setConstants(float newKp, float newKi, float newKd, float newTau) { @@ -45,7 +43,7 @@ float PID::pidOutput(float setpoint, float measurement) { pidDerivative = ((-1 * 2.0f * kd * (measurement - prevMeasurement)) + ((2 * tau - t) * pidDerivative)) / ((2.0f * tau) + t); // PID control effort - pidControlEffort = pidProportional + pidIntegral + pidDerivative; + float pidControlEffort = pidProportional + pidIntegral + pidDerivative; // Clamp control effort output if (pidControlEffort > outputMaxLim) { pidControlEffort = outputMaxLim; } @@ -56,7 +54,7 @@ float PID::pidOutput(float setpoint, float measurement) { prevMeasurement = measurement; // Absolute motor control position - pidOut = measurement + pidControlEffort; + float pidOut = measurement + pidControlEffort; // Directly into motor control return pidOut; // Must go directly into motor control } From 2b97a460a8cbce563a8daf15137eec705ab11161 Mon Sep 17 00:00:00 2001 From: Eeman Aleem Date: Sat, 20 Sep 2025 00:58:22 -0400 Subject: [PATCH 22/24] Review changes + sample time addition to pidIntegral calc. - Integral calculation in pid.cpp was missing multiplication with time. Corrected now. - Removed dynamic alloc - Changed file names to correct misspelling (lol) - Changed class names for clarity - Moved function bodies to .cpp files - Roll integral, pitch integral, and output limits changed to const static var - Added noexcept keyword to some functions that don't throw exceptions for optimization. --- .../include/attitude_manager/fbwa_mapping.hpp | 39 ++++++++++++++++ .../include/attitude_manager/fwba_mapping.hpp | 43 ------------------ zeropilot3.5/include/attitude_manager/pid.hpp | 10 ++--- .../src/attitude_manager/fbwa_mapping.cpp | 44 +++++++++++++++++++ .../src/attitude_manager/fwba_mapping.cpp | 37 ---------------- zeropilot3.5/src/attitude_manager/pid.cpp | 10 ++--- 6 files changed, 93 insertions(+), 90 deletions(-) create mode 100644 zeropilot3.5/include/attitude_manager/fbwa_mapping.hpp delete mode 100644 zeropilot3.5/include/attitude_manager/fwba_mapping.hpp create mode 100644 zeropilot3.5/src/attitude_manager/fbwa_mapping.cpp delete mode 100644 zeropilot3.5/src/attitude_manager/fwba_mapping.cpp 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/fwba_mapping.hpp b/zeropilot3.5/include/attitude_manager/fwba_mapping.hpp deleted file mode 100644 index 5617eec9..00000000 --- a/zeropilot3.5/include/attitude_manager/fwba_mapping.hpp +++ /dev/null @@ -1,43 +0,0 @@ -#pragma once - -#include "flightmode.hpp" -#include "attitude_manager.hpp" -#include "pid.hpp" - -class PIDMapping : public Flightmode { - public: - PIDMapping(); - - RCMotorControlMessage_t runControl(RCMotorControlMessage_t controlInput) override; - - // Setter *roll* for PID consts - void setRollPIDConstants(float newKp, float newKi, float newKd, float newTau) { - rollPID->setConstants(newKp, newKi, newKd, newTau); - } - - // Setter for *pitch* PID consts - void setPitchPIDConstants(float newKp, float newKi, float newKd, float newTau) { - pitchPID->setConstants(newKp, newKi, newKd, newTau); - } - - // Destructor - ~PIDMapping() override; - - private: - - // Roll integral limits - const float ROLL_INTEGRAL_MIN_LIM = 0.0f; - const float ROLL_INTEGRAL_MAX_LIM = 0.0f; - - // Pitch integral limits - const float PITCH_INTEGRAL_MIN_LIM = 0.0f; - const float PITCH_INTEGRAL_MAX_LIM = 0.0f; - - // Output limits (for control effort) - const float OUTPUT_MIN = 0.0f; - const 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 index 44c70d63..c79f19be 100644 --- a/zeropilot3.5/include/attitude_manager/pid.hpp +++ b/zeropilot3.5/include/attitude_manager/pid.hpp @@ -6,13 +6,13 @@ class PID { // PID object constructor PID(float kp, float ki, float kd, float tau, float outputMinLim, float outputMaxLim, - float integralMinLim, float integralMaxLim, float t); + float integralMinLim, float integralMaxLim, float t) 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); - // PID object's state var initialized (or reset) - void pidInitState(); + 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); 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..156683b9 --- /dev/null +++ b/zeropilot3.5/src/attitude_manager/fbwa_mapping.cpp @@ -0,0 +1,44 @@ +#include "fbwa_mapping.hpp" + +FBWAMapping::FBWAMapping() : + 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) noexcept +{ + 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/fwba_mapping.cpp b/zeropilot3.5/src/attitude_manager/fwba_mapping.cpp deleted file mode 100644 index c6556f4f..00000000 --- a/zeropilot3.5/src/attitude_manager/fwba_mapping.cpp +++ /dev/null @@ -1,37 +0,0 @@ -#include "fwba_mapping.hpp" - -PIDMapping::PIDMapping() { - rollPID = new PID(0.0f, 0.0f, 0.0f, - 0.0f, OUTPUT_MIN, OUTPUT_MAX, - ROLL_INTEGRAL_MIN_LIM, ROLL_INTEGRAL_MAX_LIM, AM_MAIN_DELAY); - - pitchPID = new PID(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(); -} - -PIDMapping::~PIDMapping() { - delete rollPID; - delete pitchPID; -} - -RCMotorControlMessage_t PIDMapping::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 index e8410279..d1dfa537 100644 --- a/zeropilot3.5/src/attitude_manager/pid.cpp +++ b/zeropilot3.5/src/attitude_manager/pid.cpp @@ -3,21 +3,21 @@ // Constructor PID::PID(float kp, float ki, float kd, float tau, float outputMinLim, float outputMaxLim, - float integralMinLim, float integralMaxLim, float t): + float integralMinLim, float integralMaxLim, float t) : kp(kp), ki(ki), kd(kd), tau(tau), t(t), outputMinLim(outputMinLim), outputMaxLim(outputMaxLim), - integralMinLim(integralMinLim), integralMaxLim(integralMaxLim) + integralMinLim(integralMinLim), integralMaxLim(integralMaxLim) noexcept {} // Initialization method - Can be used as resetter -void PID::pidInitState() { +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) { +void PID::setConstants(float newKp, float newKi, float newKd, float newTau) noexcept { kp = newKp; ki = newKi; kd = newKd; @@ -33,7 +33,7 @@ float PID::pidOutput(float setpoint, float measurement) { float pidProportional = kp * error; // PID Integral - pidIntegral += (0.5f * ki) * (error + prevError); + pidIntegral += (0.5f * ki * t) * (error + prevError); // Anti-integral windup if (pidIntegral > integralMaxLim) { pidIntegral = integralMaxLim; } From 2058aa02963a04d949417b6480fbbbc34a37ed09 Mon Sep 17 00:00:00 2001 From: Eeman Aleem Date: Sat, 20 Sep 2025 01:06:59 -0400 Subject: [PATCH 23/24] Fixed noexcept placement in fbwa_mapping and pid cpp files --- zeropilot3.5/src/attitude_manager/fbwa_mapping.cpp | 4 ++-- zeropilot3.5/src/attitude_manager/pid.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/zeropilot3.5/src/attitude_manager/fbwa_mapping.cpp b/zeropilot3.5/src/attitude_manager/fbwa_mapping.cpp index 156683b9..8881ad7d 100644 --- a/zeropilot3.5/src/attitude_manager/fbwa_mapping.cpp +++ b/zeropilot3.5/src/attitude_manager/fbwa_mapping.cpp @@ -1,12 +1,12 @@ #include "fbwa_mapping.hpp" -FBWAMapping::FBWAMapping() : +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) noexcept + PITCH_INTEGRAL_MIN_LIM, PITCH_INTEGRAL_MAX_LIM, AM_MAIN_DELAY) { rollPID.pidInitState(); pitchPID.pidInitState(); diff --git a/zeropilot3.5/src/attitude_manager/pid.cpp b/zeropilot3.5/src/attitude_manager/pid.cpp index d1dfa537..d1cfc161 100644 --- a/zeropilot3.5/src/attitude_manager/pid.cpp +++ b/zeropilot3.5/src/attitude_manager/pid.cpp @@ -3,10 +3,10 @@ // Constructor PID::PID(float kp, float ki, float kd, float tau, float outputMinLim, float outputMaxLim, - float integralMinLim, float integralMaxLim, float t) : + 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) noexcept + integralMinLim(integralMinLim), integralMaxLim(integralMaxLim) {} // Initialization method - Can be used as resetter From 3e8cf21e613eb1d282ac97408f8c469854b40f49 Mon Sep 17 00:00:00 2001 From: JJKSweaty Date: Sun, 21 Sep 2025 15:56:45 -0400 Subject: [PATCH 24/24] removed incorrect perecnet logic --- stm32l552xx/boardfiles/drivers/motor/motor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/stm32l552xx/boardfiles/drivers/motor/motor.cpp b/stm32l552xx/boardfiles/drivers/motor/motor.cpp index fa0f2523..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 < 0) ? 0 : (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); }