Feat/precompute pathing

configs/dev.json

@@ -18,6 +18,7 @@
     },
     "pathing": {
         "laps": 2,
+        "upload_distance_buffer_m": 50.0,
         "dubins": {
             "turning_radius": 30.0,
             "point_separation": 20.0

configs/jetson.json

@@ -18,6 +18,7 @@
     },
     "pathing": {
         "laps": 10,
+        "upload_distance_buffer_m": 50.0,
         "dubins": {
             "turning_radius": 5.0,
             "point_separation": 20.0

include/core/mission_state.hpp

@@ -48,6 +48,9 @@ class MissionState {
     void setInitPath(const MissionPath& init_path);
     MissionPath getInitPath();
 
+    void setNextWaypointPath(const MissionPath& next_waypoint_path);
+    MissionPath getNextWaypointPath();
+
     void setCoveragePath(const MissionPath& coverage_path);
     MissionPath getCoveragePath();
 
@@ -139,6 +142,8 @@ class MissionState {
 
     std::mutex init_path_mut;  // for reading/writing the initial path
     MissionPath init_path;
+    std::mutex next_waypoint_path_mut;  // for reading/writing the next waypoint path
+    MissionPath next_waypoint_path;
     std::mutex coverage_path_mut;  // for reading/writing the coverage path
     MissionPath coverage_path;
     std::mutex airdrop_path_mut;

include/network/mavlink.hpp

@@ -83,6 +83,7 @@ class MavlinkClient {
     mavsdk::Telemetry::RcStatus get_conn_status();
     bool armAndHover(std::shared_ptr<MissionState> state);
     bool startMission();
+    bool clearMission();
 
     /*
      * Triggers a relay on the ArduPilot
@@ -92,10 +93,9 @@ class MavlinkClient {
      */
     bool triggerRelay(int relay_number, bool state);
 
+    // Safety Functions
     void KILL_THE_PLANE_DO_NOT_CALL_THIS_ACCIDENTALLY();
-
-    // rtl
-    void rtl();
+    void returnToLaunch();
 
  private:
     mavsdk::Mavsdk mavsdk;

include/pathing/static.hpp

@@ -302,11 +302,27 @@ class AirdropApproachPathing {
  */
 RRTPoint getCurrentLoc(std::shared_ptr<MissionState> state);
 
-MissionPath generateInitialPath(std::shared_ptr<MissionState> state);
+/**
+ * Calculates the approximate angle of exit out of a waypoint path
+ * 
+ * @param path  ==> the waypoint path
+ * @param state ==> mission state
+ * @return          angle of exit (+x-axis is 0 deg, ccw is positive)
+ */
+double calculateFinalAngle(
+    const MissionPath& path,
+    const std::optional<CartesianConverter<GPSProtoVec>>& cartesianConverter);
+
+std::vector<GPSCoord> generateInitialPath(std::shared_ptr<MissionState> state);
+
+std::vector<GPSCoord>
+generateNextWaypointPath(std::shared_ptr<MissionState> state, double start_angle);
 
-MissionPath generateSearchPath(std::shared_ptr<MissionState> state);
+std::vector<GPSCoord>
+generateSearchPath(std::shared_ptr<MissionState> state, double start_angle);
 
-MissionPath generateAirdropApproach(std::shared_ptr<MissionState> state, const GPSCoord &goal);
+std::vector<GPSCoord>
+generateAirdropApproach(std::shared_ptr<MissionState> state, const GPSCoord &goal);
 
 std::pair<double, double> estimateAreaCoveredAndPathLength(const std::vector<XYZCoord> &goals,
                                                            const Environment &airspace);

include/ticks/path_gen.hpp

@@ -27,8 +27,7 @@ class PathGenTick : public Tick {
     void init() override;
     Tick* tick() override;
  private:
-    std::future<MissionPath> init_path;
-    std::future<MissionPath> coverage_path;
+    std::future<void> paths_future;
 
     void startPathGeneration();
 };

include/utilities/obc_config.hpp

@@ -106,6 +106,7 @@ struct AirdropApproachConfig {
 
 struct PathingConfig {
     int laps;
+    double upload_distance_buffer_m;
     DubinsConfig dubins;
     RRTConfig rrt;
     AirdropCoverageConfig coverage;

src/core/mission_state.cpp

@@ -79,6 +79,16 @@ MissionPath MissionState::getInitPath() {
     return this->init_path;
 }
 
+void MissionState::setNextWaypointPath(const MissionPath& next_waypoint_path) {
+    Lock lock(this->next_waypoint_path_mut);
+    this->next_waypoint_path = next_waypoint_path;
+}
+
+MissionPath MissionState::getNextWaypointPath() {
+    Lock lock(this->next_waypoint_path_mut);
+    return this->next_waypoint_path;
+}
+
 void MissionState::setCoveragePath(const MissionPath& coverage_path) {
     Lock lock(this->coverage_path_mut);
     this->coverage_path = coverage_path;

src/network/gcs_routes.cpp

@@ -571,7 +571,7 @@ DEF_GCS_HANDLE(Post, camera, runpipeline) {
 
 DEF_GCS_HANDLE(Post, rtl) {
     LOG_REQUEST("POST", "/rtl");
-    state->getMav()->rtl();
+    state->getMav()->returnToLaunch();
     LOG_RESPONSE(INFO, "RTL activated", OK);
 }
 

src/network/mavlink.cpp

@@ -186,7 +186,7 @@ MavlinkClient::MavlinkClient(OBCConfig config)
     });
 
     this->telemetry->subscribe_heading([this](mavsdk::Telemetry::Heading heading) {
-        VLOG_F(DEBUG, "Heading: %d", heading.heading_deg);
+        VLOG_F(DEBUG, "Heading: %f deg", heading.heading_deg);
         Lock lock(this->data_mut);
         this->data.heading_deg = heading.heading_deg;
     });
@@ -511,12 +511,27 @@ bool MavlinkClient::startMission() {
     return true;
 }
 
+/**
+ * Clears the existng mission
+ */
+bool MavlinkClient::clearMission() {
+    LOG_F(INFO, "Sending clear mission command");
+    auto start_result = this->mission->clear_mission();
+    if (start_result != mavsdk::MissionRaw::Result::Success) {
+        LOG_S(ERROR) << "FAIL: Mission could not be cleared " << start_result;
+        return false;
+    }
+
+    LOG_F(INFO, "Mission Cleared!");
+    return true;
+}
+
 void MavlinkClient::KILL_THE_PLANE_DO_NOT_CALL_THIS_ACCIDENTALLY() {
     LOG_F(ERROR, "KILLING THE PLANE: SETTING AFS_TERMINATE TO 1");
     auto result = this->param->set_param_int("AFS_TERMINATE", 1);
     LOG_S(ERROR) << "KILL RESULT: " << result;
 }
 
-void MavlinkClient::rtl() {
+void MavlinkClient::returnToLaunch() {
     this->action->return_to_launch();
 }