trying to fix merge with failsafe

.gitignore

@@ -19,9 +19,10 @@
 # Executables
 *.exe *.out *.app 
 
+.DS_Store
+
 .idea/
 .vscode/
-.DS_Store/
-Tools/*/build
+Tools/*/build/
 Debug/
 /.metadata/

.project

@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <projectDescription>
-	<name>ZeroPilot-SW-3.5</name>
+	<name>efs-zeropilot-3.5</name>
 	<comment></comment>
 	<projects>
 	</projects>

AttitudeManager/FlightModes/Inc/fbwa.hpp

@@ -0,0 +1,24 @@
+#ifndef ZPSW3_AM_FBWA_HPP
+#define ZPSW3_AM_FBWA_HPP
+
+#include "flightmode.hpp"
+
+namespace AM {
+
+class FBWA : public Flightmode {
+   public:
+    FBWA() = default;
+
+    AttitudeManagerInput run(const AttitudeManagerInput& input) override;
+    void updatePid() override;
+    void updatePidGains() override;
+    void updateControlLimits(ControlLimits_t limits) override;
+
+   private:
+    // TODO: FIXME to be a control limit class
+    ControlLimits_t fbwa_control_limits;
+};
+
+}  // namespace AM
+
+#endif  // ZPSW3_AM_FBWA_HPP

AttitudeManager/FlightModes/Inc/flightmode.hpp

@@ -0,0 +1,31 @@
+/*
+ * AM_ControlAlgorithm.hpp
+ *
+ * Attitude Manager Control Algorithm Interface
+ *
+ * Created on: Oct 22, 2022
+ * Author(s): Aidan Bowers
+ */
+#ifndef ZPSW3_AM_CONTROL_INTERFACE_HPP
+#define ZPSW3_AM_CONTROL_INTERFACE_HPP
+
+#include "CommonDataTypes.hpp"
+
+namespace AM {
+
+class Flightmode {
+   public:
+    virtual ~Flightmode() = default;
+
+    virtual AttitudeManagerInput run(const AttitudeManagerInput& input) = 0;
+    virtual void updatePid() = 0;
+    virtual void updatePidGains() = 0;
+    virtual void updateControlLimits(ControlLimits_t limits) = 0;
+
+   protected:
+    Flightmode() = default;
+};
+
+}  // namespace AM
+
+#endif  // ZPSW3_AM_CONTROL_INTERFACE_HPP

AttitudeManager/FlightModes/Inc/manual.hpp

@@ -0,0 +1,20 @@
+#ifndef ZPSW3_AM_MANUAL_HPP
+#define ZPSW3_AM_MANUAL_HPP
+
+#include "flightmode.hpp"
+
+namespace AM {
+
+class Manual : public Flightmode {
+   public:
+    Manual() = default;
+
+    AttitudeManagerInput run(const AttitudeManagerInput& input) override;
+    void updatePid() override;
+    void updatePidGains() override;
+    void updateControlLimits(ControlLimits_t limits) override;
+};
+
+}  // namespace AM
+
+#endif  // ZPSW3_AM_MANUAL_HPP

AttitudeManager/FlightModes/Src/fbwa.cpp

@@ -0,0 +1,51 @@
+#include "fbwa.hpp"
+#include "CommonDataTypes.hpp"
+#include "AM.hpp"
+#include <cassert>
+
+namespace AM {
+    AttitudeManagerInput FBWA::run(const AttitudeManagerInput& input) {
+
+        AttitudeManagerInput mappedOutputs;
+
+        // TODO: bound checking that doesn't throw an abort() and program exit
+        // Bounded checking to make sure input is within max and min
+        // assert(input.pitch <= AM::AttitudeManager::INPUT_MAX && input.pitch >= AM::AttitudeManager::INPUT_MIN);
+        // assert(input.roll <= AM::AttitudeManager::INPUT_MAX && input.roll >= AM::AttitudeManager::INPUT_MIN);
+        // assert(input.yaw <= AM::AttitudeManager::INPUT_MAX && input.yaw >= AM::AttitudeManager::INPUT_MIN);
+        // assert(input.throttle <= AM::AttitudeManager::INPUT_MAX && input.throttle >= AM::AttitudeManager::INPUT_MIN);
+
+        // Mapping inputs to outputs with ratios of min-max
+        // Mapping maintains 0->0 I/O
+        mappedOutputs.pitch =
+            input.pitch < 0
+                ? input.pitch * (fbwa_control_limits.pitchLimit.min / AM::AttitudeManager::INPUT_MIN)
+                : input.pitch * (fbwa_control_limits.pitchLimit.max / AM::AttitudeManager::INPUT_MAX);
+        mappedOutputs.roll =
+            input.roll < 0
+                ? input.roll * (fbwa_control_limits.rollLimit.min / AM::AttitudeManager::INPUT_MIN)
+                : input.roll * (fbwa_control_limits.rollLimit.max / AM::AttitudeManager::INPUT_MAX);
+        mappedOutputs.yaw =
+            input.yaw < 0
+                ? input.yaw * (fbwa_control_limits.yawLimit.min / AM::AttitudeManager::INPUT_MIN)
+                : input.yaw * (fbwa_control_limits.yawLimit.max / AM::AttitudeManager::INPUT_MAX);
+
+        // Throttle maps -100-100 to range 0-100
+        mappedOutputs.throttle =
+            fbwa_control_limits.throttleLimit.min +
+            (input.throttle - AM::AttitudeManager::INPUT_MIN) *
+            (fbwa_control_limits.throttleLimit.max - fbwa_control_limits.throttleLimit.min) /
+            (AM::AttitudeManager::INPUT_MAX - AM::AttitudeManager::INPUT_MIN);
+
+        return mappedOutputs;
+    }
+
+    void FBWA::updatePid() {} //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;
+    }
+}

AttitudeManager/FlightModes/Src/manual.cpp

@@ -0,0 +1,14 @@
+#include "manual.hpp"
+#include "CommonDataTypes.hpp"
+
+namespace AM {
+
+AttitudeManagerInput Manual::run(const AttitudeManagerInput& input) {
+    return input;
+}
+
+void Manual::updatePid() {}
+void Manual::updatePidGains() {}
+void Manual::updateControlLimits(ControlLimits_t limits) {}
+
+}  // namespace AM

AttitudeManager/Inc/AM.hpp

@@ -9,32 +9,59 @@
 #ifndef ZPSW3_AM_HPP
 #define ZPSW3_AM_HPP
 
-#include "AM_ControlAlgorithm.hpp"
 #include "CommonDataTypes.hpp"
-
+#include "config_foundation.hpp"
 #include "FreeRTOS.h"
+#include "flightmode.hpp"
 #include "semphr.h"
+#ifdef TESTING
+#include <gtest/gtest_prod.h>
+#endif
 
 namespace AM {
 
+typedef struct {
+    MotorChannel *motorInstance;
+    bool isInverted;
+} MotorInstance_t;
+
 class AttitudeManager {
    public:
+
+    //Constants used for mapping values
+    static constexpr float INPUT_MAX = 100;
+    static constexpr float INPUT_MIN = -100;
+
     static void setControlInputs(const AttitudeManagerInput& new_control_inputs);
 
     static AttitudeManagerInput getControlInputs();
 
-    AttitudeManager(Flightmode* control_algorithm) : control_algorithm(control_algorithm){};
+    AttitudeManager(Flightmode* controlAlgorithm,  MotorInstance_t *(&motorInstances)[], uint8_t (&numMotorsPerAxis)[]);
 
-    void runControlLoopIteration(const AttitudeManagerInput& instructions);
+    ~AttitudeManager();
+
+    void runControlLoopIteration();
 
    private:
+    #ifdef TESTING
+    FRIEND_TEST(AttitudeManager, MotorInitializationAndOutput); // Remove FRIEND_TEST when updating tests with setControlInputs
+    FRIEND_TEST(AttitudeManagerOutputToMotor, NoMotors);
+    FRIEND_TEST(AttitudeManagerOutputToMotor, MotorsOfSameAxis);
+    FRIEND_TEST(AttitudeManagerOutputToMotor, setOutputInRandomAxisOrder);
+    FRIEND_TEST(AttitudeManagerOutputToMotor, InvertedTest);
+    FRIEND_TEST(AttitudeManagerOutputToMotor, CombinedTest);
+    #endif
+
     AttitudeManager();
+    void outputToMotor(ControlAxis_t axis, uint8_t percent);
 
     static SemaphoreHandle_t control_inputs_mutex;
-
     static struct AttitudeManagerInput control_inputs;
 
-    Flightmode* control_algorithm;
+    Flightmode *controlAlgorithm_;
+    MotorInstance_t *(&motorInstances_)[];
+    uint8_t (&numMotorsPerAxis_)[];
+
 };
 
 }  // namespace AM

AttitudeManager/Src/AM.cpp

@@ -35,13 +35,41 @@ AttitudeManagerInput AttitudeManager::getControlInputs() {
     return temp;
 }
 
-void AttitudeManager::runControlLoopIteration(const AttitudeManagerInput& instructions) {
+AttitudeManager::AttitudeManager(Flightmode* controlAlgorithm,  MotorInstance_t *(&motorInstances)[], uint8_t (&numMotorsPerAxis)[]):
+    controlAlgorithm_(controlAlgorithm),
+    motorInstances_(motorInstances),
+    numMotorsPerAxis_(numMotorsPerAxis)
+{};
+
+AttitudeManager::~AttitudeManager()
+{}
+
+void AttitudeManager::runControlLoopIteration() {
     // Process Instructions
+    AttitudeManagerInput control_inputs = getControlInputs();
 
     // Run Control Algorithms
-    control_algorithm->run();
+    AttitudeManagerInput motor_outputs = controlAlgorithm_->run(control_inputs);
 
     // Write motor outputs
+    outputToMotor(yaw, static_cast<uint8_t>(motor_outputs.yaw));
+    outputToMotor(pitch, static_cast<uint8_t>(motor_outputs.pitch));
+    outputToMotor(roll, static_cast<uint8_t>(motor_outputs.roll));
+    outputToMotor(throttle, static_cast<uint8_t>(motor_outputs.throttle));
+
+}
+
+void AttitudeManager::outputToMotor(ControlAxis_t axis, uint8_t percent) {
+    // Move through the portion of the motorInstances array that matches the wanted axis.
+    // The motorReferences array holds references to the wanted positions in the motorInstances array
+    for (uint8_t motorCount{0}; motorCount < numMotorsPerAxis_[axis]; ++motorCount) {
+        if (motorInstances_[axis][motorCount].isInverted) {
+            motorInstances_[axis][motorCount].motorInstance->set(100-percent);
+        } else {
+            motorInstances_[axis][motorCount].motorInstance->set(percent);
+        }
+    }
+
 }
 
 }  // namespace AM