PM Takeoff/Landing Implementation

CMakeLists.txt

@@ -7,13 +7,16 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
 
 
 set(ATTITUDE_MANAGER_DIR "${CMAKE_CURRENT_SOURCE_DIR}/AttitudeManager")
+set(PATH_MANAGER_DIR "${CMAKE_CURRENT_SOURCE_DIR}/PathManager")
 set(LAMINAR_OS_DIR "${CMAKE_CURRENT_SOURCE_DIR}/LaminarOS")
 
 include_directories(
     ## ZP Software Includes
     ${CMAKE_CURRENT_SOURCE_DIR}/AttitudeManager/Inc
     ${CMAKE_CURRENT_SOURCE_DIR}/AttitudeManager/ControlAlgorithms/Inc
 
+    ${CMAKE_CURRENT_SOURCE_DIR}/PathManager/Inc
+
     ${CMAKE_CURRENT_SOURCE_DIR}/SystemManager/Inc
 
     ${CMAKE_CURRENT_SOURCE_DIR}/Common/Inc
@@ -48,6 +51,10 @@ set(ZP_AM_C_SOURCES "${ZP_AM}/Src/*.c"
 set(ZP_AM_CXX_SOURCES "${ZP_AM}/Src/*.cpp"
                       "${ZP_AM}/ControlAlgorithms/Src/*.cpp")
 
+set(ZP_PM ${CMAKE_CURRENT_SOURCE_DIR}/PathManager)
+set(ZP_PM_C_SOURCES "${ZP_PM}/Src/*.c")
+set(ZP_PM_CXX_SOURCES "${ZP_PM}/Src/*.cpp")
+
 set(HAL_DRIVERS_DIR ${CMAKE_CURRENT_SOURCE_DIR}/LaminarOS/boardfiles/${FOLDER_NAME}/Drivers)
 set(HAL_DRIVERS_C_SOURCES "${HAL_DRIVERS_DIR}/${FAMILY_NAME}_HAL_Driver/Src/*.c")
 
@@ -80,14 +87,16 @@ file(GLOB_RECURSE C_SOURCES ${HAL_DRIVERS_C_SOURCES}
                             ${CORE_C_SOURCES}
                             ${FREE_RTOS_C_SOURCES}
                             ${ZP_SM_C_SOURCES}
-                            ${ZP_AM_C_SOURCES})
+                            ${ZP_AM_C_SOURCES}
+                            ${ZP_PM_C_SOURCES})
 message("MESSAGE: ${C_SOURCES}")
 file(GLOB_RECURSE CXX_SOURCES ${HAL_CORE_CXX_SOURCES}
                               ${INTERFACE_CXX_SOURCES}
                               ${DRIVERS_CXX_SOURCES}
                               ${CORE_CXX_SOURCES}
                               ${ZP_SM_CXX_SOURCES}
-                              ${ZP_AM_CXX_SOURCES})
+                              ${ZP_AM_CXX_SOURCES}
+                              ${ZP_PM_CXX_SOURCES})
 
 set(STARTUP_ASM_FILE ${CMAKE_CURRENT_SOURCE_DIR}/LaminarOS/${STARTUP_ASM})
 set(LINKER_SCRIPT_FILE ${CMAKE_CURRENT_SOURCE_DIR}/LaminarOS/${LINKER_SCRIPT})

Common/Inc/CommonDataTypes.hpp

@@ -6,6 +6,7 @@
 #define ZPSW3_COMMON_DATATYPES_HPP
 
 #include <stdint.h>
+#include "AM_DataTypes.hpp"
 
 namespace LOS {
 // Struct copied from old code

PathManager/Inc/PM.hpp

@@ -0,0 +1,70 @@
+#ifndef ZPSW3_PM_HPP
+#define ZPSW3_PM_HPP
+
+#include <stdint.h>
+#include "cmsis_os.h"
+
+#include "PM_StateManager.hpp"
+#include "CommonDataTypes.hpp"
+#include "PM_DataTypes.hpp"
+#include "PM_LandingTakeoffManager.hpp"
+
+namespace PM
+{
+    class PathManagerState;
+
+    // Gives status of Path Manager so we know when it has completed a cycle, or entered failure mode.
+    enum Path_Manager_Cycle_Status {COMPLETED_CYCLE = 0, IN_CYCLE, FAILURE_MODE};
+
+    class PathManager
+    {
+        public:
+            PathManager();
+            inline PathManagerState* getCurrentState() const {return currentState;}
+            void execute();
+            void setState(PathManagerState& newState);
+            Path_Manager_Cycle_Status getStatus() {return status;}
+
+            //used to determine the stage of the landing sequence
+            LandingTakeoffManager vtol_manager;
+            FlightStage flight_stage;
+            bool isError;
+
+            void storeSmPmQueue(osMessageQId queueId);
+            osMessageQId getSmPmQueue();
+
+            void storePmAmQueue(osMessageQId queueId);
+            osMessageQId getPmAmQueue();
+
+        // void storeUsePmFlag();
+
+            //GlobalFlag getUsePmFlag(); 
+
+            void setSmStruct(const SM_PM_Commands &from_sm_data); 
+            SM_PM_Commands getSmStruct(); 
+
+            AM::AttitudeManagerInput getAmStruct(); 
+            void setAmStruct(const AM::AttitudeManagerInput &am_instructions); 
+
+
+
+
+
+        private:
+            PathManagerState* currentState;
+            Path_Manager_Cycle_Status status;
+
+            // Message Q
+            osMessageQId SM_to_PM_queue;
+            osMessageQId PM_to_AM_queue;
+
+            SM_PM_Commands sm_instructions; 
+            AM::AttitudeManagerInput am_data; 
+    };
+
+}
+
+
+
+#endif
+

PathManager/Inc/PM_DataTypes.hpp

@@ -0,0 +1,69 @@
+#ifndef PM_DATATYPES_HPP
+#define PM_DATATYPES_HPP
+
+#include <cstdint>
+
+
+/********************************************************************
+* Enums
+********************************************************************/
+
+
+// Used by PM to determine the stage of flight
+enum FlightStage{PREFLIGHT = 0, TAKEOFF, CRUISING, LANDING, LANDED, DISARMED, TELEOP}; 
+
+
+// Used to specify the type of output
+// Where TELEOP_MODE is used by SM to indicate to PM to pass data directly to AM 
+enum WaypointType {PATH_FOLLOW = 0, ORBIT_FOLLOW, HOVER_WAYPOINT, TAKEOFF_WAYPOINT, LANDING_WAYPOINT, TRANSITION_WAYPOINT, TELEOP_MODE};
+
+enum ModifyFlightPathCommand { NO_FLIGHT_PATH_EDIT = 0, INITIALIZE_FLIGHT_PATH, APPEND, INSERT, UPDATE, DELETE, NUKE }; // Used by cruisingState
+enum GetNextDirectionsCommand { REGULAR_PATH_FOLLOWING = 0, TOGGLE_HOLDING, TOGGLE_HEAD_HOME }; // Used by cruisingState
+
+
+/********************************************************************
+* Important Structs
+********************************************************************/
+
+
+/**
+* Structure stores information about the waypoints along our path to the destination and back.
+*/
+struct WaypointData {
+    int waypointId;                   // Id of the waypoint
+    WaypointData * next;                 // Next waypoint
+    WaypointData * previous;             // Previous waypoint
+    long double latitude;             // Latitude of waypoint
+    long double longitude;            // Longitude of waypoint
+    int altitude;                     // Altitude of waypoint
+    WaypointType waypoint_type; 
+    double velocity_target; 
+};
+
+// Used in Cruising State 
+struct WaypointManager_Data_In {
+    long double latitude;
+    long double longitude;
+    int altitude;
+    double track;
+};
+
+struct WaypointManager_Data_Out{
+    float desiredTrack;              // Desired track to stay on path
+    int desiredAltitude;                // Desired altitude at next waypoint
+    long double distanceToNextWaypoint; // Distance to the next waypoint (helps with airspeed PID)
+    float distanceX, distanceY, distanceZ;
+    float rotation; 
+    //WaypointStatus errorCode;          // Contains error codes
+    bool isDataNew;                     // Notifies PID modules if the data in this structure is new
+    int desiredAirspeed;                // FUN FACT WE NEED THIS 
+    uint32_t timeOfData;                // The time that the data in this structure was collected
+    WaypointType waypoint_type;       // Output type (determines which parameters are defined)
+};
+
+
+
+
+
+#endif
+

PathManager/Inc/PM_Interface.h

@@ -0,0 +1,18 @@
+#ifndef ZPSW3_PM_INTERFACE_HPP
+#define ZPSW3_PM_INTERFACE_HPP
+/***This interface exists so that freeRTOS can call C-Style functions while not messing up the fact that most of our development is in C++***/
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void PathManagerInterfaceInit(void);
+
+bool PathManagerInterfaceExecute(void);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif 

PathManager/Inc/PM_LandingTakeoffManager.hpp

@@ -0,0 +1,108 @@
+
+/* Related Confluence Pages
+    https://uwarg-docs.atlassian.net/wiki/spaces/ZP/pages/1607630921/Landing+and+Takeoff
+*/
+
+#ifndef ZPSW3_LANDING_TAKEOFF_MANAGER_HPP
+#define ZPSW3_LANDING_TAKEOFF_MANAGER_HPP
+#include "CommonDataTypes.hpp"
+#include "PM_DataTypes.hpp"
+
+/********************************************************************
+* Constants
+********************************************************************/
+
+const double TAKEOFF_TARGET = 4.0;
+const double LANDING_SPEED = 0.2;
+const double MAX_SPEED = 1.0;
+
+class LandingTakeoffManager{
+
+    public:
+        /******************
+         * EXTERNAL FUNCTIONS
+         ******************/
+
+        /*
+            This function sets the ground height if it is called for the first time
+            This function returns the takeoff altitude target (ground height + TAKEOFF_TARGET)
+
+            @param double currentAltitude - this variable holds the current altitude value of the aircraft
+
+            @return double - this structure holds the altitude point
+        */
+        double getTakeoffAltitudeTarget(double currentAltitude);
+
+        /*
+            This function returns the ground height that was set during takeoff
+
+            @param double currentAltitude - this variable holds the current altitude value of the aircraft
+
+            @return double - this structure holds the altitude point
+        */
+        double getLandingAltitudeTarget(double currentAltitude);
+
+        /******************
+         * LANDING FUNCTIONS
+         ******************/
+
+        /*
+            This function returns a waypoint that the drone should follow
+            This waypoint does not have a horizontal component yet, this will come as corrections from the CV team
+
+            @param const SFOutput_t & input - this variable holds the sensor fusion module output
+
+            @return WaypointData - this structure holds the climb point, which the plane will follow until it exits below a certain altitude
+        */
+        AM::AttitudeManagerInput createLandingWaypoint(const LOS::LosSFData & input);
+
+        /******************
+         * TAKEOFF FUNCTIONS
+         *****************/
+        /*
+
+        /*
+            This function returns a waypoint that the drone should follow
+            This waypoint does not have a horizontal component yet, this will come as corrections from the CV team
+
+            @param const SFOutput_t & input - this variable holds the sensor fusion module output
+
+            @return WaypointData - this structure holds the climb point, which the plane will follow until it exits at a certain altitude
+        */
+        AM::AttitudeManagerInput createTakeoffWaypoint(const LOS::LosSFData & input);
+
+        /***********************************
+        COMMON LANDING AND TAKEOFF FUNCTIONS
+        ************************************/
+
+        /*
+            This function returns the desired climb/descend speed given the current altitude
+            Because the takeoff and landing target velocities 
+
+            @param double currentAltitude - this double holds the current aircraft altitude
+
+            @param double rangeConstant - this double holds the range constant for the path
+
+            @return double - this function will return the desired climb speed for the aircraft
+        */
+        double getSpeedTarget(double currentAltitude, double rangeConstant);
+
+        /*
+            This function returns a waypoint that the drone should follow
+            This waypoint does not have a horizontal component yet, this will come as corrections from the CV team
+
+            @param double midpointAltitude - this variable holds the midpoint of the altitude range.
+
+            @return double - this structure holds the range constant to determine the velocity based on altitude.
+        */
+        double getRangeConstant(double midpointAltitude);
+
+    private:
+        double groundHeight = -1; // Set by takeoff and used by landing
+        double takeoffRangeConstant = 0; // Value set based on the drone altitude when transitioning to takeoff
+        double landingRangeConstant = 0; // Value set based on the drone altitude when transitioning to landing
+        // TODO: Reset these constants to 0 in case landing is transitioned into multiple times in a flight
+        // Note: a range constant value of 0 means a velocity of 0, so drone only moves once value set.
+};
+
+#endif

PathManager/Inc/PM_StateManager.hpp

@@ -0,0 +1,28 @@
+/**
+ *  Path Manager State Machine Header
+ */
+
+#ifndef ZPSW3_PM_STATE_MANAGER_HPP
+#define ZPSW3_PM_STATE_MANAGER_HPP
+
+#include "PM.hpp"
+
+namespace PM {
+class PathManager;
+
+class PathManagerState
+{
+    public:
+        virtual void enter(PathManager* pathMgr) = 0;
+        virtual void execute(PathManager* pathMgr) = 0;
+        virtual void exit(PathManager* pathMgr) = 0;
+
+        bool operator==(const PathManagerState& rhs) const {return (this == &rhs);} 
+
+        virtual ~PathManagerState() {}
+
+};
+}
+
+#endif
+