diff --git a/.gitignore b/.gitignore index 259148fa..147800c0 100644 --- a/.gitignore +++ b/.gitignore @@ -30,3 +30,5 @@ *.exe *.out *.app + +.idea/ diff --git a/AttitudeManager/Inc/AM.hpp b/AttitudeManager/Inc/AM.hpp new file mode 100644 index 00000000..c2ba0852 --- /dev/null +++ b/AttitudeManager/Inc/AM.hpp @@ -0,0 +1,38 @@ +/* + * AM.hpp + * + * Attitude Manager Header + * + * Created on: Oct 12, 2022 + * Author(s): Anthony (anni) Luo; Dhruv Upadhyay + */ +#ifndef ZPSW3_AM_HPP +#define ZPSW3_AM_HPP + +#include "LOS_Link.hpp" +#include "LOS_Actuators.hpp" +#include "AM_StateManager.hpp" + +class AttitudeState; + +namespace AM { + // Gives status of attitude manager so we know when it has completed a cycle (its state is FetchInstructionsMode) or entered failure mode + enum _Attitude_Manager_Cycle_Status {COMPLETED_CYCLE = 0, IN_CYCLE, FAILURE_MODE}; +} + +class AttitudeManager { +public: + AttitudeManager(LOS_Link *link, LOS_Actuators *output); + inline AttitudeState* getCurrentState() const {return currentState;} + void execute(); + void setState(AttitudeState& newState); + AM::_Attitude_Manager_Cycle_Status getStatus() {return status;} + LOS_Link *link; + LOS_Actuators *output; +private: + AttitudeState* currentState; + AM::_Attitude_Manager_Cycle_Status status; + AttitudeManager(); +}; + +#endif //ZPSW3_AM_HPP diff --git a/AttitudeManager/Inc/AM_DataTypes.hpp b/AttitudeManager/Inc/AM_DataTypes.hpp new file mode 100644 index 00000000..462019f5 --- /dev/null +++ b/AttitudeManager/Inc/AM_DataTypes.hpp @@ -0,0 +1,21 @@ +// +// Created by Anthony Luo on 2022-10-13. +// + +#ifndef ZPSW3_AM_DATATYPES_HPP +#define ZPSW3_AM_DATATYPES_HPP + +typedef struct { + int some_data_here; +} Position_t; + + +enum flight_mode: int8_t { + fm_fatal_failure = -1, + fm_limp = 0, + fm_stabilize = 1, + fm_gps = 2, + fm_autonomous = 3 +}; + +#endif //ZPSW3_AM_DATATYPES_HPP diff --git a/AttitudeManager/Inc/AM_Interface.h b/AttitudeManager/Inc/AM_Interface.h new file mode 100644 index 00000000..dd267715 --- /dev/null +++ b/AttitudeManager/Inc/AM_Interface.h @@ -0,0 +1,24 @@ +/* + * AM_Interface.h + * + * Interface so that freeRTOS can call C-style functions while dev is in c++ + * + * Created on: Oct 12, 2022 + * Author(s): Anthony (anni) Luo; Dhruv Upadhyay + */ + +#ifndef ZPSW3_AM_INTERFACE_H +#define ZPSW3_AM_INTERFACE_H + +#ifdef __cplusplus +extern "C" { +#endif + +void AM_InterfaceInit(void); +void AM_InterfaceExecute(void); + +#ifdef __cplusplus +} +#endif + +#endif //ZPSW3_AM_INTERFACE_H diff --git a/AttitudeManager/Inc/AM_StateManager.hpp b/AttitudeManager/Inc/AM_StateManager.hpp new file mode 100644 index 00000000..127d64a7 --- /dev/null +++ b/AttitudeManager/Inc/AM_StateManager.hpp @@ -0,0 +1,27 @@ +/* + * AM_StateManager.hpp + * + * Attitude State-Machine header + * + * Created on: Oct 12, 2022 + * Author(s): Anthony (anni) Luo; Dhruv Upadhyay + */ + +#ifndef ZPSW3_AM_STATEMANAGER_HPP +#define ZPSW3_AM_STATEMANAGER_HPP + +#include "AM.hpp" + +class AttitudeManager; + +class AttitudeState { +public: + virtual void enter(AttitudeManager* attitudeMgr) = 0; + virtual void execute(AttitudeManager* attitudeMgr) = 0; + virtual void exit(AttitudeManager* attitudeMgr) = 0; + + bool operator==(const AttitudeState& rhs) const {return (this == &rhs);} + + virtual ~AttitudeState() {} +}; +#endif //ZPSW3_AM_STATEMANAGER_HPP diff --git a/AttitudeManager/Inc/AM_States.hpp b/AttitudeManager/Inc/AM_States.hpp new file mode 100644 index 00000000..b4d81e97 --- /dev/null +++ b/AttitudeManager/Inc/AM_States.hpp @@ -0,0 +1,149 @@ +/* + * AM_States.hpp + * + * Attitude Manager State-Machine header file + * + * Describes the states of AM: + * + * Init / Disarm / FetchInstructionsMode (all in one rn) + * ControlLoopMode [ run our control loops ] + * OutputMixingMode [ where we mix outputs ] ? does this include telemetry + * + */ + +#ifndef ZPSW3_AM_STATES_HPP +#define ZPSW3_AM_STATES_HPP + +#include "AM_StateManager.hpp" +#include "AM_DataTypes.hpp" + + +/* not being used at the moment + * will be brought it after things start working incrementally + **/ +class DisarmMode: public AttitudeState { +public: + void enter(AttitudeManager *att_man) { + (void) att_man; + } + void execute(AttitudeManager *att_man); + void exit(AttitudeManager *att_man) { + (void) att_man; + } + static AttitudeState& getInstance(); +private: + DisarmMode() { + } + DisarmMode(const DisarmMode &other); + DisarmMode& operator =(const DisarmMode &other); + static bool receieveArmDisarmInstruction(AttitudeManager *att_man); + static bool isArmed(); + static uint8_t m_arm_disarm_ppm_val; + static uint8_t m_arm_disarm_timeout_count; +}; + +class FetchInstructionsMode : public AttitudeState { +public: + void enter(AttitudeManager *att_man) { + (void) att_man; + } + + void execute(AttitudeManager *att_man); + + void exit(AttitudeManager *att_man) { + (void) att_man; + } + + static AttitudeState &getInstance(); + + static flight_mode getFlightMode(void) { + return FetchInstructionsMode::m_flight_mode; + } + + static Teleop_Instructions_t *getTeleopInstructions(void) { + return &m_teleop_instructions; + } + +private: + FetchInstructionsMode() { + // TODO: to be implemented + } + + FetchInstructionsMode(const FetchInstructionsMode &other); + + FetchInstructionsMode &operator=(const FetchInstructionsMode &other); + + static bool receiveTeleopInstructions(AttitudeManager *att_man); + + static bool isArmed(); + + static Teleop_Instructions_t m_teleop_instructions; + static flight_mode m_flight_mode; + static uint8_t m_teleop_timeout_count; + static Position_t m_pos; +}; + +class ControlLoopMode : public AttitudeState { +public: + void enter(AttitudeManager *att_man) { + (void) att_man; + } + void execute(AttitudeManager *att_man); + void exit(AttitudeManager *att_man) { + (void) att_man; + } + static AttitudeState& getInstance(); + static Controls_Output_t* getControlsOutput(void) { + return m_controls_output + } +private: + ControlLoopMode() { + + } + ControlLoopMode(const ControlLoopMode &other); + ControlLoopMode& operator = (const ControlLoopMode &other); + static Controls_Output_t *m_controls_output +}; + +class OutputMixingMode : public AttitudeState { +public: + void enter(AttitudeManager *att_man) { + (void) att_man; + } + void execute(AttitudeManager *att_man) { + (void) att_man; + } + void exit(AttitudeManager *att_man) { + (void) att_man; + } + static AttitudeState& getInstance(); + static float* getChannelOut(void) { + return m_channel_out; + } +private: + OutputMixingMode() { + + } + OutputMixingMode(const OutputMixingMode &other); + OutputMixingMode& operator = (const OutputMixingMode &other); + static float m_channel_out[5]; //FIXME: make this a data struct +}; + +class FatalFailureMode: public AttitudeState { +public: + void enter(AttitudeManager *att_man) { + (void) att_man; + } + void execute(AttitudeManager *att_man); + void exit(AttitudeManager *att_man) { + (void) att_man; + } + static AttitudeState& getInstance(); +private: + FatalFailureMode() { + } + FatalFailureMode(const FatalFailureMode &other); + FatalFailureMode& operator =(const FatalFailureMode &other); +}; + +#endif //ZPSW3_AM_STATES_HPP diff --git a/AttitudeManager/Src/AM.cpp b/AttitudeManager/Src/AM.cpp new file mode 100644 index 00000000..9a681d59 --- /dev/null +++ b/AttitudeManager/Src/AM.cpp @@ -0,0 +1,3 @@ +// +// Created by Anthony Luo on 2022-10-12. +// diff --git a/AttitudeManager/Src/AM_Interface.cpp b/AttitudeManager/Src/AM_Interface.cpp new file mode 100644 index 00000000..63246dbe --- /dev/null +++ b/AttitudeManager/Src/AM_Interface.cpp @@ -0,0 +1,5 @@ +// +// Created by Anthony Luo on 2022-10-12. +// + +#include "../Inc/AM_Interface.h" diff --git a/AttitudeManager/Src/AM_States.cpp b/AttitudeManager/Src/AM_States.cpp new file mode 100644 index 00000000..1f50c240 --- /dev/null +++ b/AttitudeManager/Src/AM_States.cpp @@ -0,0 +1,159 @@ +/* + * AM_States.cpp + * + * Attitude Manager State-Machine classes + * + * Created on: Oct 12, 2022 + * Author(s): Anthony (anni) Luo; Dhruv Upadhyay + */ + + +#include "../Inc/AM_States.hpp" +#include "LOS_Link.hpp" +#include "LOS_Actuators.hpp" + +/******************** + * DisArm Mode + ********************/ +void DisarmMode::execute(AttitudeManager *att_man) { + // set all pwm to 0 + const uint8_t TIMEOUT_THRESHOLD = 2; //Max cycles without data until connection is considered broken + + //Get Arm Disarm instruction + if (ReceiveArmDisarmInstruction(attitudeMgr)) { + armDisarmTimeoutCount = 0; + } else { + if (armDisarmTimeoutCount < TIMEOUT_THRESHOLD) + armDisarmTimeoutCount++; + } + + + /* + 3 possibilities: + 1. Go into FatalFailureMode bec of timeout + 2. Go into fetchInstructionsMode bec "Arm" instruction was sent + 3. Do nothing, stay in the disarm state + */ + if (armDisarmTimeoutCount > TIMEOUT_THRESHOLD && CommsFailed()) { + //Abort due to timeout failures + attitudeMgr->setState(FatalFailureMode::getInstance()); + return; + } else if (isArmed()) { + attitudeMgr->setState(fetchInstructionsMode::getInstance()); + } else { + //Do nothing, stay in this state + //attitudeMgr->setState(DisarmMode::getInstance()); + } +} + +AttitudeState& DisarmMode::getInstance() { + static DisarmMode singleton; + return singleton; +} + +bool DisarmMode::receiveArmDisarmInstruction(AttitudeManager* att_man) { + // if am dc; return false + // if connected, return arm channel value +} + +bool DisarmMode::isArmed() { + // check arm channel value > min arm value + +} + + +/******************** + * FetchInstructions Mode + ********************/ +void FetchInstructionsMode::execute(AttitudeManager *att_man) { + const uint8_t TIMEOUT_THRESH = 2; + + FetchInstructionsMode::m_flight_mode = fm_stabilize; + if (receiveTeleopInstructions(att_man)) { + m_teleop_timeout_count = 0; + } else { + if (m_teleop_timeout_count < TIMEOUT_THRESH) { + m_teleop_timeout_count++; + } + } + + // TODO: add section to move to disarmed state + + if (m_teleop_timeout_count < TIMEOUT_THRESH) { + // TODO: add comms failed cehck + FetchInstructionsMode::m_flight_mode = fm_stabilize; + } else { + att_man->setState(FatalFailureMode::getInstance()); + } + + /* at this point, we can run our actual instructions processing */ + // TODO: get position data + // TODO: move flight mode as needed +} + +AttitudeState &FetchInstructionsMode::getInstance() { + static FetchInstructionsMode singleton; + return singleton; +} + +bool FetchInstructionsMode::receiveTeleopInstructions(AttitudeManager *att_man) { + bool is_dc{true}; + if (is_dc) { + return false; + } + + // !TODO: get information from the link + // (and somehow process it) +} + +bool FetchInstructionsMode::isArmed() { + bool retval = false; + if (m_teleop_instructions.is_armed >= MIN_ARM_VALUE) { + retval = true; + } + return retval; +} + +/******************** + * ControlLoop Mode + ********************/ + +void ControlLoopMode::execute(AttitudeManager *att_man) { + Controls_Output_t *ctrl_out = nullptr; + Teleop_Instructions_t *m_teleop_instructions = FetchInstructionsMode::getTeleopInstructions(); + if (FetchInstructionsMode::getFlightMode() == fm_autonomous) { + // run certain modes + } else if (FetchInstructionsMode::getFlightMode() == fm_gps) { + + } else if (FetchInstructionsMode::getFlightMode() == fm_stabilize) { + + } else if (FetchInstructionsMode::getFlightMode() == fm_limp) { + // limp mode + } else { + // fatal failure mode + } +} + +/******************** + * OutputMixing Mode + ********************/ + +void OutputMixingMode::execute(AttitudeManager *att_man) { + Controls_Output_t *ctrl_out = ControlLoopMode::getControlsOutput(); + + // match types? + att_man->output->set(ctrl_out); +} + +/******************** + * FatalFailure Mode + ********************/ + +void FatalFailureMode::execute(AttitudeManager *att_man) { + att_man->setState(FatalFailureMode::getInstance()); +} + +AttitudeState &FatalFailureMode::getInstance() { + static FatalFailureMode singleton; + return singleton; +} diff --git a/AttitudeManager/tests/todo.txt b/AttitudeManager/tests/todo.txt new file mode 100644 index 00000000..3072fd13 --- /dev/null +++ b/AttitudeManager/tests/todo.txt @@ -0,0 +1 @@ +Somehow, create tests for AM on a system level. HOW?!?!?!? \ No newline at end of file