Cruising State Module

Cruising State is responsible for providing the coordinated turns module with a desired heading and altitude. It accomplishes this by making calls to the Waypoint Manager. The cruising state module is run by the Path Manager state machine when cruisingState::execute() is called.

This document concerns itself with cruisingState.hpp and cruisingState.cpp within the ./PathManager directory of ZeroPilot.

Responsibilities of Cruising State

Cruising State is responsible for two things: getting the plane’s desired direction and altitude during cruising flight; and updating the flight path if required. The module itself actually provides an API that the state machine can use to interact with the Waypoint Manager.

Sending Commands via Telemetry

In order to use the Cruising State module, data must be sent via telemetry. The following struct shows what needs to be sent in:

struct Telemetry_Waypoint_Data_t {
    long double latitude;
    long double longitude;
    int altitude;
    float turnRadius;
    int waypointType; // 0 = Path follow, 1 = Orbit, 2 = Hold
};

struct Telemetry_PIGO_t {

    ... other data sent by telemetry

    /* Parameters for the waypoint manager (crusingState) */
    int numWaypoints;
    
    char waypointModifyFlightPathCommand; // 0 = nothing, 1 = initialize flight path, 2 = append, 3 = insert, 4 = update, 5 = delete, 6 = nuke
    bool initializingHomeBase; // 0 = no, 1 = yes
    char waypointNextDirectionsCommand; // 0 = nothing, 1 = start/end holding, 2 = head home
    int holdingAltitude;
    int holdingTurnRadius;
    char holdingTurnDirection; // 0 = CW, 1 = CCW

    // When modifying the flight path.
    int nextId;
    int prevId;
    int modifyId;

    Telemetry_Waypoint_Data_t * waypoints[100]; // Somehow need to get PATH_BUFFER_SIZE here...
    Telemetry_Waypoint_Data_t homebase;
}

The first struct is a template for a waypoint. It contains the essential information and will be used to initialize waypoints in the Cruising State module.

The second struct is the general struct that includes the other data sent from telemetry (but, these values were excluded for obvious reasons).

• numWaypoints stores the number of waypoints in the waypoints array

• waypointNextDirectionsCommand is used to indicate if the plane should enter/exit holding patterns, start heading home, or just get the next direction and altitude as normal.

• Holding specific data

  • holdingAltitude is the altitude that the plane will hold when holding (in m)

  • holdingTurnRadius is the radius of the plane’s holding pattern (in m)

  • holdingTurnDirection is the direction that the plane will turn (when looking down from above)

• Modifying flight path

  • waypointModifyFlightPathCommand is used when editing the plane’s flight path in order to call the correct function in the Waypoint Manager

  • initilaizingHomeBase is used when initializing the flight path to indicate that we are also initializing the home base. If it is set to 0 and we are initializing the flight path, the home base will be set to nullptr.

  • nextId is used when inserting a waypoint and represents the id of the waypoint that will come after the inserted waypoint

  • prevId is used when inserting a waypoint and represents the id of the waypoint that will come before the inserted waypoint

  • modifyId is used when updating or deleting a waypoint and represents the id of the affected waypoint.

• Other

  • waypoints[100] stores waypoints that will be used to update or initialize the plane’s flight path

    • Length should be 1 when updating, appending, or inserting

    • Length should be greater than 1 when initializing

    • Length should be 0 when deleting, nuking, or doing nothing

  • homebase stores the home base when it is being initialized. Should be set to nullptr otherwise.

Functions Declared

Here is cruisingState.hpp:

#include "pathDatatypes.hpp"
#include "waypointManager.hpp"

#ifndef CRUISING_STATE
#define CRUISING_STATE

struct _CruisingState_Telemetry_Return {
    char editingFlightPathErrorCode; // 0 = success, 1 = error, 2 = incorrect telemetry command
    char pathFollowingErrorCode; // 0 = success, 1 = error, 2 = home base not initialized, 3 = incorrect telemetry command
    int currentWaypointId; 
    int currentWaypointIndex;
    bool homeBaseInitialized;
};

// The following functions are used to interact with the Waypoint Manager module
int editFlightPath(Telemetry_PIGO_t * telemetryData, WaypointManager& cruisingStateManager, int * idArray);

int pathFollow(Telemetry_PIGO_t * telemetryData, WaypointManager& cruisingStateManager, _WaypointManager_Data_In input, _WaypointManager_Data_Out * output, bool& goingHome, bool& inHold);

void setReturnValues(_CruisingState_Telemetry_Return * _returnToGround, WaypointManager& cruisingStateManager, int editErrorCode, int pathErrorCode);

// The following functions are used to update the ID array that is a part of the CruisingState class
void appendNewElement(int * idArray, int newId);
int indexOfDesiredId(int * idArray, int id);
void insertNewElement(int * idArray, int prevId, int newId);
void updateElement(int * idArray, int oldid, int newId);
void removeElement(int * idArray, int id);
void clearArray(int * idArray);

The struct, _CruisingState_Telemetry_Return will be used by the commsWithTelemetry state when preparing to send data to the ground. It contains data regarding the current state of the waypoint manager and whether any commands executed properly. Error codes, which are included below are also sent through this struct.

• Error Codes:

  • editingFlightPathErrorCode:

    • 0 = success

    • 1 = error

    • 2 = incorrect telemetry command

  • pathFollowingErrorCode

    • 0 = success

    • 1 = error

    • 2 = incorrect telemetry command

    • 3 = going home failed because home base is not initialized

The function editFlightPath() is called every time cruisingState::execute() is run. Using commands sent from telemetry, it will update the plane’s flight path by calling the appropriate Waypoint Manager API.

The function pathFollow() is called every time cruisingState::execute() is run. Using commands sent from telemetry, it will either get the plane’s next directions and altitude, put the plane in a holding pattern, or make the plane start heading home.

The function setReturnValues() populates an instance of the _CruisingState_Telemetry_Return with relevant data.

The functions on lines 23 through 28 are used to update an id array that is stored in the cruisingState class. The functions are called when needed by editFlightPath.

Testing

Unit tests were written in ./Test_CruisingState.cpp to test the functions declared in cruisingState.hpp. The unit tests tested the following:

1. Incorrect telemetry commands returns error codes

2. Initializing the flight path works

3. Nuking the flight path works

4. Appending a waypoint

   1. Case where it works

   2. Case where it fails

5. Inserting a waypoint

   1. Case where it works

   2. Case where it fails

6. Updating a waypoint

   1. Case where it works

   2. Case where it fails

7. Deleting a waypoint

   1. Case where it works

   2. Case where it fails

8. Getting next flight directions and altitude success

9. Entering a holding pattern successfully and returning correct next flight direction/altitude

10. Successfully start heading to home base

11. Fail to successfully start heading to home base