Changes made based on comments, fixed updatePidGains

AttitudeManager/FlightModes/Inc/fbwa.hpp

@@ -11,12 +11,15 @@ class FBWA : public Flightmode {
 
  AttitudeManagerInput run(const AttitudeManagerInput& input) override;
  void updatePid() override;
- void updatePidGains(PIDController _axis, GainTerm whichGain, float desiredGain) override;
- void updateControlLimits() override;
+ void updatePidGains(PidAxis pid_axis, GainTerm pid_gain_term, float desired_gain) override;
+ void updateControlLimits(ControlLimits_t limits) override;
 
  private:
  // TODO: FIXME to be a control limit class
  ControlLimits_t fbwa_control_limits;
+ PIDController pidRoll;
+ PIDController pidPitch;
+ PIDController pidYaw;
 };
 
 } // namespace AM

AttitudeManager/FlightModes/Inc/flightmode.hpp

@@ -10,16 +10,16 @@
 #define ZPSW3_AM_CONTROL_INTERFACE_HPP
 
 #include "CommonDataTypes.hpp"
-#include <PID.hpp>
+#include "PID.hpp"
 namespace AM {
 
 class Flightmode {
  public:
  virtual ~Flightmode() = default;
 
- virtual AttitudeManagerInput run(const AttitudeManagerInput& input, AxisPIDs) = 0;
+ virtual AttitudeManagerInput run(const AttitudeManagerInput& input) = 0;
  virtual void updatePid() = 0;
- virtual void updatePidGains(PIDController _axis, GainTerm whichGain, float desiredGain) = 0;
+ virtual void updatePidGains(PidAxis pid_axis, GainTerm pid_gain_term, float desired_gain) = 0;
  virtual void updateControlLimits(ControlLimits_t limits) = 0;
 
  protected:

AttitudeManager/FlightModes/Inc/manual.hpp

@@ -9,10 +9,16 @@ class Manual : public Flightmode {
  public:
  Manual() = default;
 
- AttitudeManagerInput run(const AttitudeManagerInput& input, AxisPIDs) override;
+ AttitudeManagerInput run(const AttitudeManagerInput& input) override;
  void updatePid() override;
- void updatePidGains(PIDController _axis, GainTerm whichGain, float desiredGain) override;
+ void updatePidGains(PidAxis pid_axis, GainTerm pid_gain_term, float desired_gain) override;
  void updateControlLimits(ControlLimits_t limits) override;
+
+ private:
+ PIDController pitchPID;
+ PIDController rollPID;
+ PIDController yawPID;
+ PIDController throttlePID;
 };
 
 } // namespace AM

AttitudeManager/FlightModes/Src/fbwa.cpp

@@ -2,9 +2,10 @@
 #include "CommonDataTypes.hpp"
 #include "AM.hpp"
 #include <cassert>
+#include <stdexcept>
 
 namespace AM {
-AttitudeManagerInput FBWA::run(const AttitudeManagerInput& input, AxisPIDs _pids) {
+AttitudeManagerInput FBWA::run(const AttitudeManagerInput& input) {
  AttitudeManagerInput mappedOutputs;
 
  // TODO: bound checking that doesn't throw an abort() and program exit
@@ -42,58 +43,38 @@ AttitudeManagerInput FBWA::run(const AttitudeManagerInput& input, AxisPIDs _pids
  _pids.pitch.setDesired(mappedOutputs.pitch);
  _pids.roll.setDesired(mappedOutputs.roll);
  _pids.yaw.setDesired(mappedOutputs.yaw);
+ // Need to get actual and actualRate data from IMU filter
+ // pidRoll.execute(mappedOutputs.roll, actual, actualRate);
+ // pidPitch.execute(mappedOutputs.pitch, actual, actualRate);
+ // pidYaw.execute(mappedOutputs.yaw, actual, actualRate);
 
 
  return mappedOutputs;
 
 void FBWA::updatePid() {} // Needs to be implemented
 
-void FBWA::updatePidGains(PIDController _axis, GainTerm whichGain, float desiredGain) {
- if (isnan(desiredGain)) {
+void FBWA::updatePidGains(PidAxis pid_axis, GainTerm pid_gain_term, float desired_gain) {
+ if ((std::isnan(pid_gain_term)) || (desired_gain < 0) || (desired_gain > 1)) {
  return;
  }
 
- void FBWA::updatePid() {} //Needs to be implemented
-
- void FBWA::updatePidGains(PIDController _axis, GainTerm whichGain, float desiredGain) {
- if ( isnan(desiredGain)){
- return;
- }
-
- switch (whichGain){
- case proportional:
- _axis.setKp(desiredGain);
- break;
- case integral:
- _axis.setKi(desiredGain);
- break;
- case derivative:
- _axis.setKd(desiredGain);
- break;
- }
- } //Needs to be implemented
-
- void FBWA::updatePidGains(PIDController _axis, GainTerm whichGain, float desiredGain) {
- switch (whichGain){
- case proportional:
- _axis.setKp(desiredGain);
- break;
- case integral:
- _axis.setKi(desiredGain);
- break;
- case derivative:
- _axis.setKd(desiredGain);
- break;
- }
- } //Needs to be implemented
-
- void FBWA::updatePidGains() {} //Needs to be implemented
-
- void FBWA::updateControlLimits(ControlLimits_t limits) {
- // TODO: make this better than a straight copy
- fbwa_control_limits = limits;
- }
-} // Needs to be implemented
+ switch (pid_axis) {
+ case PidAxis::Pitch:
+ pidPitch.setGainTerm(pid_gain_term, desired_gain);
+ break;
+ case PidAxis::Roll:
+ pidRoll.setGainTerm(pid_gain_term, desired_gain);
+ break;
+ case PidAxis::Yaw:
+ pidYaw.setGainTerm(pid_gain_term, desired_gain);
+ break;
+ case PidAxis::Throttle:
+ break;
+ default:
+ throw std::invalid_argument("Invalid pid_axis value.");
+ break; 
+ } 
+}
 
 void FBWA::updateControlLimits(ControlLimits_t limits) {
  // TODO: make this better than a straight copy

AttitudeManager/FlightModes/Src/manual.cpp

@@ -1,28 +1,35 @@
 #include "manual.hpp"
 #include "CommonDataTypes.hpp"
-
+#include <stdexcept>
 namespace AM {
 
-AttitudeManagerInput Manual::run(const AttitudeManagerInput& input, AxisPIDs _pids) {
+AttitudeManagerInput Manual::run(const AttitudeManagerInput& input) {
  return input;
 }
 
 void Manual::updatePid() {}
-void Manual::updatePidGains(PIDController _axis, GainTerm whichGain, float desiredGain) {
- if (isnan(desiredGain)) {
+void Manual::updatePidGains(PidAxis pid_axis, GainTerm pid_gain_term, float desired_gain) {
+ if ((isnan(pid_gain_term)) || (desired_gain < 0) || (desired_gain > 1)) {
  return;
  }
 
- switch (whichGain) {
- case proportional:
- _axis.setKp(desiredGain);
+ switch (pid_axis) {
+ case PidAxis::Pitch:
+ pitchPID.setGainTerm(pid_gain_term, desired_gain);
+ break;
+ case PidAxis::Roll:
+ rollPID.setGainTerm(pid_gain_term, desired_gain);
  break;
- case integral:
- _axis.setKi(desiredGain);
+ case PidAxis::Yaw:
+ yawPID.setGainTerm(pid_gain_term, desired_gain);
  break;
- case derivative:
- _axis.setKd(desiredGain);
+ case PidAxis::Throttle:
+ throttlePID.setGainTerm(pid_gain_term, desired_gain);
  break;
+ default:
+ throw std::invalid_argument("Invalid pid_axis value.");
+ break;
+
  }
 }
 void Manual::updateControlLimits(ControlLimits_t limits) {}

AttitudeManager/Inc/PID.hpp

@@ -26,12 +26,18 @@ struct PIDGains{
  float kd;
 };
 
-typedef enum {
- proportional,
- integral,
- derivative
-} GainTerm;
+enum GainTerm {
+ Proportional,
+ Integral,
+ Derivative
+};
 
+enum PidAxis{
+ Pitch,
+ Roll,
+ Yaw,
+ Throttle
+};
 
  /**
  * Initialises the Pid object.
@@ -105,6 +111,7 @@ class PIDController {
  void setKi(float _ki) {pid.ki = _ki;};
  void setKp(float _kp) {pid.kp = _kp;};
  void setKd(float _kd) {pid.kd = _kd;};
+ void setGainTerm(GainTerm _gainTerm, float _desired_gain);
  void setDesired(float _desired) {pid.desired = _desired;};
  void setActual(float _actual) {pid.actual = _actual;};
  void setActualRate(float _actualRate) {pid.actualRate = _actualRate;};
@@ -135,13 +142,4 @@ class PIDController {
  // if necessary
 };
 
-struct AxisPIDs{
- PIDController pitch;
- PIDController roll;
- PIDController yaw;
- PIDController throttle;
-};
-
-
-
 #endif /* PID_HPP_ */

AttitudeManager/Src/PID.cpp

@@ -87,4 +87,23 @@ float PIDController::execute_d_back(float _desired, float _actual, float _actual
 
 float PIDController::map(float x, float in_min, float in_max, float out_min, float out_max) {
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+
+}
+
+void PIDController::setGainTerm(GainTerm _gainTerm, float _desired_gain) {
+ if (std::isnan(_desired_gain) || _desired_gain < 0 || _desired_gain > 1) {
+ return;
+ }
+
+ switch (_gainTerm) {
+ case Proportional:
+ pid.kp = _desired_gain;
+ break;
+ case Derivative:
+ pid.kd = _desired_gain;
+ break;
+ case Integral:
+ pid.ki = _desired_gain;
+ break;
+ }
 }