Skip to content
This repository has been archived by the owner on Feb 5, 2024. It is now read-only.

Path manager restructure #157

Open
wants to merge 12 commits into
base: pathManRestructure
Choose a base branch
from
Open

Path manager restructure #157

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
12 commits
Select commit Hold shift + click to select a range
4002217
fix typo
Gongsta Nov 15, 2021
c11919f
change altitde to altitude
Gongsta Nov 15, 2021
b382551
add implementation of queue
Gongsta Nov 17, 2021
9a3d1d4
update cruisingmodestages with looping over instructions
Gongsta Nov 17, 2021
cdc1139
fix spacing
Gongsta Nov 17, 2021
500d287
rename functions and fix bugs from Dhruv's review
Gongsta Nov 19, 2021
b560747
start implementing testing
Gongsta Nov 30, 2021
093f631
add modeselectorqueue
Gongsta Nov 30, 2021
81f0545
remove inline unit testing inside stages files
Gongsta Dec 1, 2021
34a4d9a
test failing
Gongsta Dec 1, 2021
9c3f7a4
fix dequeuing
Gongsta Dec 1, 2021
5c18a1d
finalize tests for path manager revamp which implements instruction q…
Gongsta Jan 27, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions Autopilot/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -237,6 +237,7 @@ set(PATH_MANAGER_MODES_FSM_UNIT_TEST_SOURCES
${CMAKE_CURRENT_SOURCE_DIR}/Test/Src/PathManager/Test_CruisingMode_Fsm.cpp
${CMAKE_CURRENT_SOURCE_DIR}/Test/Src/PathManager/Test_LandingMode_Fsm.cpp
${CMAKE_CURRENT_SOURCE_DIR}/Test/Src/PathManager/Test_TakeoffMode_Fsm.cpp
${CMAKE_CURRENT_SOURCE_DIR}/Test/Src/PathManager/Test_ModeSelector_Queue.cpp
)

add_executable(pathManagerModesFsm ${PATH_MANAGER_MODES_FSM_SOURCES} ${PATH_MANAGER_MODES_FSM_UNIT_TEST_SOURCES} ${UNIT_TEST_MAIN})
Expand Down
47 changes: 45 additions & 2 deletions Autopilot/PathManager/Inc/pathModeSelector.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,12 @@
enum PathModeEnum {MODE_TAKEOFF = 0, MODE_CRUISING, MODE_LANDING, MODE_TAXIING};
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Later on we'll have to change this to account for the states more relevant to multicopters (hover, no taxiing, etc.). Do you think its easily changeable for that once its implemented?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Think that makes more sense to sit in my PR for landing and takeoff

#endif

//Implementated as a queue, where each editFlightPath instruction is a node in the queue
struct flightPathEditInstructionNode {
Telemetry_PIGO_t instruction;
flightPathEditInstructionNode* nextInstruction;
};

// ModeSelector in https://uwarg-docs.atlassian.net/wiki/spaces/ZP/pages/1866989569/Proposed+Redesign
class PathModeSelector {
public:
Expand All @@ -34,6 +40,28 @@ class PathModeSelector {
* @return none
*/
void execute(Telemetry_PIGO_t telemetry_in, SFOutput_t sensor_fusion_in, IMU_Data_t imu_data_in);

/**
* Checks whether the instruction queue is empty.
*
* @return true if the queue is empty. Else return false
*/
bool flightPathEditInstructionsIsEmpty();

/**
* Checks the length of the instruction queue.
*
* @return its length.
*/
int checkflightPathEditInstructionsLength();

/**
* Dequeues the first(a.k.a. most urgent) instruction from the instruction queue. Make sure that queue is not empty
* before dequeing by running instructionQueueIsEmpty() beforehand.
*
* @return telemetry data
*/
Telemetry_PIGO_t dequeueflightPathEditInstructions();

/**
* Returns a pointer to the current mode of flight. Note that a PathMode object is returned, which is the
Expand Down Expand Up @@ -75,7 +103,7 @@ class PathModeSelector {
*
* @return none
*/
void setAltitdeAirspeedInput(AltitudeAirspeedInput_t alt_airspeed_input);
void setAltitudeAirspeedInput(AltitudeAirspeedInput_t alt_airspeed_input);

/**
* get the altitude_airspeed_input parameter, which will be used by coordinateTurnElevation
Expand Down Expand Up @@ -162,9 +190,25 @@ class PathModeSelector {

bool getIsLanded() { return is_landed; }

void enqueueFlightPathEditInstructions(Telemetry_PIGO_t newInstruction);
//Make a pointer to the first instruction node, which will be the head of the flightPathEditInstructions (a queue).
flightPathEditInstructionNode* first_flight_path_edit_instr;

private:
PathModeSelector();
static PathModeSelector* singleton;


/**
* Function to enqueue a new instruction onto the instruction queue.
*
* @param newInstruction -> new instruciton to add onto the instruction queue
*
* @return none
*/
// void enqueueFlightPathEditInstructions(Telemetry_PIGO_t newInstruction);
// //Make a pointer to the first instruction node, which will be the head of the flightPathEditInstructions (a queue).
// flightPathEditInstructionNode* first_flight_path_edit_instr;

PathMode* current_mode;
PathModeEnum current_mode_enum;
Expand All @@ -187,4 +231,3 @@ class PathModeSelector {
};

#endif

19 changes: 16 additions & 3 deletions Autopilot/PathManager/Src/cruisingMode/cruisingModeStages.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,14 +17,28 @@ _CruisingState_Telemetry_Return CruisingFlight::_return_to_ground;
void CruisingFlight::execute(CruisingMode* cruise_mode) {
Telemetry_PIGO_t telem_data = cruise_mode->getTelemetryData();
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

all this will now be from the jetson instead of the plane. When FW-CV Comms gets merged into devel, you will have to re-adapt this restructure

SFOutput_t sf_data = cruise_mode->getSensorFusionData();


// Set waypoint manager input struct
_input_data.track = sf_data.track; // Gets track
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

where is _input_data defined?

_input_data.longitude = sf_data.longitude;
_input_data.latitude = sf_data.latitude;
_input_data.altitude = sf_data.altitude;

_ModifyFlightPathErrorCode edit_error = editFlightPath(&telem_data, cruising_state_manager, waypoint_id_array); // Edit flight path if applicable

Telemetry_PIGO_t instr;
_ModifyFlightPathErrorCode edit_error = MODIFY_CRUISING_SUCCESS; // Initialize to success, else if the instruction queue is empty, edit_error would not be initialized
//Go over all instructions on the instruction queue
while (!cruise_mode->getModeSelector()->flightPathEditInstructionsIsEmpty()) {
instr = cruise_mode->getModeSelector()->dequeueflightPathEditInstructions();

edit_error = editFlightPath(&instr, cruising_state_manager, waypoint_id_array); // Edit flight path if applicable

if (edit_error != MODIFY_CRUISING_SUCCESS) { //There has been an error, get out of the loop and handle the error first
break;
}
}

_GetNextDirectionsErrorCode path_error = pathFollow(&telem_data, cruising_state_manager, _input_data, &_output_data, going_home, in_hold); // Get next direction or modify flight behaviour pattern
setReturnValues(&_return_to_ground, cruising_state_manager, edit_error, path_error); // Set error codes

Expand All @@ -51,7 +65,7 @@ void CruisingFlight::execute(CruisingMode* cruise_mode) {
passby_control.throttlePassby = false;

cruise_mode->getModeSelector()->setCoordinatedTurnInput(coord_turn_input);
cruise_mode->getModeSelector()->setAltitdeAirspeedInput(alt_airspeed_input);
cruise_mode->getModeSelector()->setAltitudeAirspeedInput(alt_airspeed_input);
cruise_mode->getModeSelector()->setPassbyControl(passby_control);

// Set output data to be sent back to PM commsWithTelemetry state
Expand All @@ -75,4 +89,3 @@ CruisingModeStageManager& CruisingFlight::getInstance() {
static CruisingFlight singleton;
return singleton;
}

36 changes: 9 additions & 27 deletions Autopilot/PathManager/Src/landingMode/landingModeStages.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -46,11 +46,7 @@ void LandingTransitionStage::execute(LandingMode* land_mode) {
_input.telemetryData = &telem_data;

SFOutput_t sf_data;
#ifdef UNIT_TESTING
sf_data.track = 1;
#else
sf_data = land_mode->getSensorFusionData();
#endif
sf_data = land_mode->getSensorFusionData();
_input.sensorOutput = &sf_data;

//making sure landing points are only made once
Expand Down Expand Up @@ -118,7 +114,7 @@ void LandingTransitionStage::execute(LandingMode* land_mode) {
LandingTakeoffManager::translateLTSFCommandsToCoordTurns(*landing_takeoff_output, sf_data, imu_data, coord_turn_input, alt_airspeed_input);

land_mode->getModeSelector()->setCoordinatedTurnInput(coord_turn_input);
land_mode->getModeSelector()->setAltitdeAirspeedInput(alt_airspeed_input);
land_mode->getModeSelector()->setAltitudeAirspeedInput(alt_airspeed_input);
land_mode->getModeSelector()->setPassbyControl(_output.controlDetails);

// Set output telemetry data
Expand All @@ -144,11 +140,7 @@ void LandingSlopeStage::execute(LandingMode* land_mode) {
_input.telemetryData = &telem_data;

SFOutput_t sf_data;
#ifdef UNIT_TESTING
sf_data.altitude = 1;
#else
sf_data = land_mode->getSensorFusionData();
#endif
sf_data = land_mode->getSensorFusionData();
_input.sensorOutput = &sf_data;

if(_input.sensorOutput->altitude <= (FLARE_ALTITUDE + _input.telemetryData->stoppingAltitude)) { //if less than flare altitude
Expand Down Expand Up @@ -192,7 +184,7 @@ void LandingSlopeStage::execute(LandingMode* land_mode) {
LandingTakeoffManager::translateLTSFCommandsToCoordTurns(*landing_takeoff_output, sf_data, imu_data, coord_turn_input, alt_airspeed_input);

land_mode->getModeSelector()->setCoordinatedTurnInput(coord_turn_input);
land_mode->getModeSelector()->setAltitdeAirspeedInput(alt_airspeed_input);
land_mode->getModeSelector()->setAltitudeAirspeedInput(alt_airspeed_input);
land_mode->getModeSelector()->setPassbyControl(_output.controlDetails);

// Set output telemetry data
Expand All @@ -218,11 +210,7 @@ void LandingFlareStage::execute(LandingMode* land_mode) {
_input.telemetryData = &telem_data;

SFOutput_t sf_data;
#ifdef UNIT_TESTING
sf_data.altitude = 0.3;
#else
sf_data = land_mode->getSensorFusionData();
#endif
sf_data = land_mode->getSensorFusionData();
_input.sensorOutput = &sf_data;

if(_input.sensorOutput->altitude <= (DECRAB_ALTITUDE + _input.telemetryData->stoppingAltitude)) //altitude is below 70 cm
Expand Down Expand Up @@ -270,7 +258,7 @@ void LandingFlareStage::execute(LandingMode* land_mode) {
LandingTakeoffManager::translateLTSFCommandsToCoordTurns(*landing_takeoff_output, sf_data, imu_data, coord_turn_input, alt_airspeed_input);

land_mode->getModeSelector()->setCoordinatedTurnInput(coord_turn_input);
land_mode->getModeSelector()->setAltitdeAirspeedInput(alt_airspeed_input);
land_mode->getModeSelector()->setAltitudeAirspeedInput(alt_airspeed_input);
land_mode->getModeSelector()->setPassbyControl(_output.controlDetails);

// Set output telemetry data
Expand All @@ -296,11 +284,7 @@ void LandingDecrabStage::execute(LandingMode* land_mode) {
_input.telemetryData = &telem_data;

SFOutput_t sf_data;
#ifdef UNIT_TESTING
sf_data.altitude = 0.02;
#else
sf_data = land_mode->getSensorFusionData();
#endif
sf_data = land_mode->getSensorFusionData();
_input.sensorOutput = &sf_data;

if(_input.sensorOutput->altitude <= (TOUCHDOWN_ALTITUDE + _input.telemetryData->stoppingAltitude)) { //altitude is 5 cm or less/ultrasonic sensor sensed 5cm or less
Expand Down Expand Up @@ -332,7 +316,7 @@ void LandingDecrabStage::execute(LandingMode* land_mode) {
LandingTakeoffManager::translateLTSFCommandsToCoordTurns(*landing_takeoff_output, sf_data, imu_data, coord_turn_input, alt_airspeed_input);

land_mode->getModeSelector()->setCoordinatedTurnInput(coord_turn_input);
land_mode->getModeSelector()->setAltitdeAirspeedInput(alt_airspeed_input);
land_mode->getModeSelector()->setAltitudeAirspeedInput(alt_airspeed_input);
land_mode->getModeSelector()->setPassbyControl(_output.controlDetails);

land_mode->getModeSelector()->setIsError(false); // Set error value
Expand Down Expand Up @@ -387,7 +371,7 @@ void LandingTouchdownStage::execute(LandingMode* land_mode) {
LandingTakeoffManager::translateLTSFCommandsToCoordTurns(*landing_takeoff_output, sf_data, imu_data, coord_turn_input, alt_airspeed_input);

land_mode->getModeSelector()->setCoordinatedTurnInput(coord_turn_input);
land_mode->getModeSelector()->setAltitdeAirspeedInput(alt_airspeed_input);
land_mode->getModeSelector()->setAltitudeAirspeedInput(alt_airspeed_input);
land_mode->getModeSelector()->setPassbyControl(_output.controlDetails);

land_mode->getModeSelector()->setIsError(false); // Set error value
Expand Down Expand Up @@ -421,5 +405,3 @@ LandingModeStageManager& LandingTouchdownStage::getInstance() {
static LandingTouchdownStage singleton;
return singleton;
}


67 changes: 65 additions & 2 deletions Autopilot/PathManager/Src/pathModeSelector.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,21 +15,85 @@ PathModeSelector* PathModeSelector::getInstance() {

PathModeSelector::PathModeSelector() : current_mode_enum {MODE_TESTING_NONE} {
current_mode = nullptr;
first_flight_path_edit_instr = nullptr;
}

#else

PathModeSelector::PathModeSelector() : current_mode_enum {MODE_TAKEOFF} {
current_mode = &TakeoffMode::getInstance();
first_flight_path_edit_instr = nullptr;
}

#endif

void PathModeSelector::execute(Telemetry_PIGO_t telemetry_in, SFOutput_t sensor_fusion_in, IMU_Data_t imu_data_in) {

// Check whether the telemetry_in has an editflightPath command. If so, we enqueue it into our InstructionQueue
if (telemetry_in.waypointModifyFlightPathCommand != NO_FLIGHT_PATH_EDIT) {
enqueueFlightPathEditInstructions(telemetry_in);
}

current_mode->execute(telemetry_in, sensor_fusion_in, imu_data_in);
}

void PathModeSelector::setAltitdeAirspeedInput(AltitudeAirspeedInput_t alt_airspeed_input) {
bool PathModeSelector::flightPathEditInstructionsIsEmpty() {
if (first_flight_path_edit_instr == nullptr) {
return true;
} else {
return false;
}
}

int PathModeSelector::checkflightPathEditInstructionsLength() {
int count = 0;
if (flightPathEditInstructionsIsEmpty()) {
return count;
} else {
flightPathEditInstructionNode* latestInstruction = first_flight_path_edit_instr;

while (latestInstruction != nullptr) {
latestInstruction = latestInstruction->nextInstruction;
count += 1;
};
return count;
}

}

Telemetry_PIGO_t PathModeSelector::dequeueflightPathEditInstructions() {
// Assumes that Queue is non empty.
flightPathEditInstructionNode* old_first_instruction = first_flight_path_edit_instr;
Telemetry_PIGO_t instruction = old_first_instruction->instruction;
// old_first_instruction->nextInstruction = nullptr;
first_flight_path_edit_instr = first_flight_path_edit_instr->nextInstruction; //Update the head of the queue
delete old_first_instruction; //Delete the old node
return instruction;
};

void PathModeSelector::enqueueFlightPathEditInstructions(Telemetry_PIGO_t newInstruction) {
if (flightPathEditInstructionsIsEmpty()) { //If our Queue is empty (i.e. there are no editflightpath instructions), make it the first
first_flight_path_edit_instr = new flightPathEditInstructionNode{};
first_flight_path_edit_instr->instruction = newInstruction;
first_flight_path_edit_instr->nextInstruction = nullptr;
} else {
flightPathEditInstructionNode* latestInstruction = first_flight_path_edit_instr;

while (latestInstruction->nextInstruction != nullptr) {
latestInstruction = latestInstruction->nextInstruction;
};

// Create new instructionQueueNode
flightPathEditInstructionNode* newInstructionQueueNode = new flightPathEditInstructionNode{};
newInstructionQueueNode->instruction = newInstruction;
newInstructionQueueNode->nextInstruction = nullptr;

// Add new instructionQueueNode to the entire queue
latestInstruction->nextInstruction = newInstructionQueueNode;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Looks good. We should write two unit tests for this when the time comes tho :)

Question: What do you think the tests should cover?

}
};

void PathModeSelector::setAltitudeAirspeedInput(AltitudeAirspeedInput_t alt_airspeed_input) {
memcpy(&altitude_airspeed_input, &alt_airspeed_input, sizeof(AltitudeAirspeedInput_t));
}

Expand All @@ -40,4 +104,3 @@ void PathModeSelector::setCoordinatedTurnInput(CoordinatedTurnInput_t coord_turn
void PathModeSelector::setPassbyControl(_PassbyControl passby) {
memcpy(&passby_control_output, &passby, sizeof(_PassbyControl));
}

7 changes: 3 additions & 4 deletions Autopilot/PathManager/Src/pathStateClasses.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -111,13 +111,14 @@ void modeExecutor::execute(pathManager* pathMgr)
mode_selector->setCurrentMode(CruisingMode::getInstance());
}

Telemetry_PIGO_t telem_data;
Telemetry_PIGO_t telem_data {};

// These were added to ensure CruisingMode_Fsm tests pass.
// TODO: Ensure CruisingMode tests don't depend on this so we can delete this garbage
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Just commenting this TODO to keep it visible

telem_data.waypoints[0] = createTelemetryWaypoint(0.0, 0.0, 6, 0.0, 0);
telem_data.waypoints[1] = createTelemetryWaypoint(0.0, 0.0, 7, 0.0, 0);
telem_data.waypoints[2] = createTelemetryWaypoint(0.0, 0.0, 8, 0.0, 0);
telem_data.waypoints[3] = createTelemetryWaypoint(0.0, 0.0, 9, 0.0, 0);

telem_data.numWaypoints = 4;
telem_data.waypointModifyFlightPathCommand = INITIALIZE_FLIGHT_PATH;
telem_data.initializingHomeBase = 1;
Expand Down Expand Up @@ -220,5 +221,3 @@ pathManagerState& fatalFailureMode::getInstance()
static fatalFailureMode singleton;
return singleton;
}


Loading