Path manager restructure #157

Gongsta · 2021-11-17T02:47:04Z

This is a pull requests that implements an instructionQueue.

Currently, we can only modify the flight path when we are in cruising mode. The problem with that is that if we are in takeoff mode, and we receive instructions to modify the flight path, these instructions will simply be lost.

This problem has been fixed by adding an instruction queue (implemented as a linked list) that stores these instructions when we are not in cruising mode. Once we transition to cruising mode, we will check the queue first, and if the queue is not empty, execute those instructions first.

Pop0097

Left some comments. Nice work so far!

Also, I'd recommend looking over our Style Guide and fixing any inconsistencies you have with it :))

Pop0097 · 2021-11-17T20:28:43Z

Autopilot/PathManager/Inc/pathModeSelector.hpp

+         * 
+         * @return true if the queue is empty. Else return false
+         */
+        bool instructionQueueIsEmpty();


To communicate that this function is specific to cruising mode, maybe name the function something like:

cruisingEditPathInstrQueueIsEmpty() or something else if you have ideas

that way if we ever need to add new queues we can easily tell which queue we are calling.

Pop0097 · 2021-11-17T20:29:27Z

Autopilot/PathManager/Inc/pathModeSelector.hpp

+         * 
+         * @return telemetry data
+         */
+        Telemetry_PIGO_t dequeueInstruction();


cruisingEditPathInstrPop()?

Nice job with the comments :))

Autopilot/PathManager/Inc/pathModeSelector.hpp

Pop0097 · 2021-11-17T20:32:01Z

Autopilot/PathManager/Inc/pathModeSelector.hpp

+         * Function to enqueue a new instruction onto the instruction queue.
+         */
+        void enqueueInstruction(Telemetry_PIGO_t newInstruction);
+        instructionQueueNode* first_instr;


In your comment for the function here I'd write an @param field to explain what the parameter is.

Also, add a comment to the instructionQueueNode pointer so it's more clear what it is being used for. Maybe renaming it to cruising_edit_path_first_instr may be more clear (especially if we add more queues in the future)

Autopilot/PathManager/Src/pathModeSelector.cpp

Autopilot/PathManager/Src/cruisingMode/cruisingModeStages.cpp

Pop0097 · 2021-11-17T20:40:08Z

Autopilot/PathManager/Src/cruisingMode/cruisingModeStages.cpp

+    while (!cruise_mode->getModeSelector()->instructionQueueIsEmpty()) {
+        instr = cruise_mode->getModeSelector()->dequeueInstruction();
+
+        _ModifyFlightPathErrorCode edit_error = editFlightPath(&instr, cruising_state_manager, waypoint_id_array); // Edit flight path if applicable


Something to think about: We return an error code when we edit the flight path. What should we do if we get an error while updating the flight path?

Not sure about this, can we want to break out of the while loop (that dequeues the instructions) so we can run the setReturnValues function ASAP? We don't lose the flight path edit instructions that are stored on the queue, it's just going to be run later since handling the error code is a higher priority (though again I don't understand how the error is handled, since it doesn't seem to be directly handled inside cruisingModeStages.cpp

Autopilot/PathManager/Src/pathModeSelector.cpp

Pop0097 · 2021-11-17T20:45:15Z

Autopilot/PathManager/Src/pathModeSelector.cpp

+        newInstructionQueueNode->nextInstruction = nullptr;
+
+        //Add new instructionQueueNode to the entire queue
+        latestInstruction->nextInstruction = newInstructionQueueNode;


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

Question: What do you think the tests should cover?

Pop0097

Cool! Looks like we can look into testing now. We can discuss writing tests for this sometime later this week once assignments stop killing me lol

…ueues

upadhyaydhruv

Great work! I added a few comments and only have a few concerns:
1.) Considering that a lot of PM will be restructured (implementation of FW-CV comms instead of ground telemetry, usage of other states like hover), do you think its best to hold off on integrating this, or can you easily add more elements/add changes later on?

upadhyaydhruv · 2022-02-06T20:44:05Z

Autopilot/PathManager/Inc/pathModeSelector.hpp

@@ -14,6 +14,12 @@
    enum PathModeEnum {MODE_TAKEOFF = 0, MODE_CRUISING, MODE_LANDING, MODE_TAXIING};


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?

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

upadhyaydhruv · 2022-02-06T21:06:46Z

Autopilot/PathManager/Src/cruisingMode/cruisingModeStages.cpp

@@ -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();


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

upadhyaydhruv · 2022-02-06T21:11:56Z

Autopilot/PathManager/Src/cruisingMode/cruisingModeStages.cpp

@@ -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();
    SFOutput_t sf_data = cruise_mode->getSensorFusionData();
+

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


where is _input_data defined?

upadhyaydhruv · 2022-02-07T01:15:20Z

Autopilot/Test/Src/PathManager/Test_ModeSelector_Queue.cpp

@@ -0,0 +1,201 @@
+/*


Could you give a rundown of which tests you're running here (in the comments or in documentation) for future reference?

Gfcello · 2022-02-08T02:34:21Z

Autopilot/PathManager/Src/pathStateClasses.cpp


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


Just commenting this TODO to keep it visible

Gfcello · 2022-02-08T02:37:21Z

Autopilot/Test/Src/PathManager/Test_TakeoffMode_Fsm.cpp

@@ -98,7 +99,10 @@ TEST(TakeoffModeFSM, TakeoffClimbTransitionsToCruising) {
    Telemetry_PIGO_t telem_data;
 	telem_data.holdingAltitude = 20;

-	SFOutput_t sf_data {};
+	SFOutput_t sf_data;
+	sf_data.altitude = 50; // So we transition to cruising mode


Once Hover is implemented, would this make more sense to transition to hover?

Gongsta added 5 commits November 14, 2021 22:42

fix typo

4002217

change altitde to altitude

c11919f

add implementation of queue

b382551

update cruisingmodestages with looping over instructions

9a3d1d4

fix spacing

cdc1139

Gongsta requested a review from Pop0097 November 17, 2021 02:48

Pop0097 suggested changes Nov 17, 2021

View reviewed changes

rename functions and fix bugs from Dhruv's review

500d287

Pop0097 reviewed Nov 23, 2021

View reviewed changes

Gongsta added 6 commits November 29, 2021 20:59

start implementing testing

b560747

add modeselectorqueue

093f631

remove inline unit testing inside stages files

81f0545

test failing

34a4d9a

fix dequeuing

9c3f7a4

finalize tests for path manager revamp which implements instruction q…

5c18a1d

…ueues

Gongsta marked this pull request as ready for review January 27, 2022 15:50

upadhyaydhruv reviewed Feb 7, 2022

View reviewed changes

Gfcello reviewed Feb 8, 2022

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Path manager restructure #157

Path manager restructure #157

Gongsta commented Nov 17, 2021 •

edited

Loading

Pop0097 left a comment

Pop0097 Nov 17, 2021 •

edited

Loading

Pop0097 Nov 17, 2021 •

edited

Loading

Pop0097 Nov 17, 2021

Pop0097 Nov 17, 2021

Gongsta Nov 19, 2021

Pop0097 Nov 17, 2021

Pop0097 left a comment

upadhyaydhruv left a comment

upadhyaydhruv Feb 6, 2022

Gfcello Feb 8, 2022

upadhyaydhruv Feb 6, 2022

upadhyaydhruv Feb 6, 2022

upadhyaydhruv Feb 7, 2022

Gfcello Feb 8, 2022

Gfcello Feb 8, 2022

		@@ -14,6 +14,12 @@
		enum PathModeEnum {MODE_TAKEOFF = 0, MODE_CRUISING, MODE_LANDING, MODE_TAXIING};

		@@ -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();


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

Path manager restructure #157

Are you sure you want to change the base?

Path manager restructure #157

Conversation

Gongsta commented Nov 17, 2021 • edited Loading

Pop0097 left a comment

Choose a reason for hiding this comment

Pop0097 Nov 17, 2021 • edited Loading

Choose a reason for hiding this comment

Pop0097 Nov 17, 2021 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Pop0097 left a comment

Choose a reason for hiding this comment

upadhyaydhruv left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Gongsta commented Nov 17, 2021 •

edited

Loading

Pop0097 Nov 17, 2021 •

edited

Loading

Pop0097 Nov 17, 2021 •

edited

Loading