Fix: Pathing Inputs

configs/dev.json

@@ -33,7 +33,7 @@
         "coverage": {
             "altitude_m": 30.0,
             "camera_vision_m": 20.0,
-            "method": "hover",
+            "method": "forward",
             "hover": {
                 "pictures_per_stop": 1,
                 "hover_time_s": 1

include/core/mission_state.hpp

@@ -108,9 +108,14 @@ class MissionState {
     bool getMappingIsDone();
     void setMappingIsDone(bool isDone);
 
+    // Getter and Setter for laps
+    int getLapsRemaining();
+    void setLapsRemaining(int laps);
+    void decrementLapsRemaining();
+
     MissionParameters mission_params;  // has its own mutex
 
-    OBCConfig config;
+    const OBCConfig config;
 
     std::optional<airdrop_t> next_airdrop_to_drop;
 
@@ -131,6 +136,9 @@ class MissionState {
     std::mutex dropped_airdrops_mut;
     std::unordered_set<AirdropType> dropped_airdrops;
 
+    std::mutex laps_remaining_mut;
+    int laps_remaining;
+
     std::shared_ptr<MavlinkClient> mav;
     std::shared_ptr<AirdropClient> airdrop;
     std::shared_ptr<CVAggregator> cv;

include/pathing/static.hpp

@@ -294,6 +294,14 @@ class AirdropApproachPathing {
     XYZCoord wind;
 };
 
+/**
+ * Helper function to get the current location of the plane for pathing
+ * 
+ * @param state ==> the mission state to get the mavlink client from
+ * @return      ==> RRTPoint representing the current location
+ */
+RRTPoint getCurrentLoc(std::shared_ptr<MissionState> state);
+
 MissionPath generateInitialPath(std::shared_ptr<MissionState> state);
 
 MissionPath generateSearchPath(std::shared_ptr<MissionState> state);

include/ticks/fly_waypoints.hpp

@@ -23,6 +23,7 @@ class FlyWaypointsTick : public Tick {
     Tick* tick() override;
 
  private:
+    bool mission_started;
     Tick* next_tick;
     std::chrono::milliseconds last_photo_time;
 };

src/core/mission_state.cpp

@@ -129,4 +129,17 @@ bool MissionState::getMappingIsDone() { return this->mappingIsDone; }
 
 void MissionState::setMappingIsDone(bool isDone) { this->mappingIsDone = isDone; }
 
+int MissionState::getLapsRemaining() {
+    Lock lock(this->laps_remaining_mut);
+    return this->laps_remaining;
+}
+
+void MissionState::setLapsRemaining(int laps) {
+    Lock lock(this->laps_remaining_mut);
+    this->laps_remaining = laps;
+}
 
+void MissionState::decrementLapsRemaining() {
+    Lock lock(this->laps_remaining_mut);
+    this->laps_remaining--;
+}

src/core/obc.cpp

@@ -25,7 +25,7 @@ OBC::OBC(OBCConfig config) {
 
     this->state = std::make_shared<MissionState>(config);
     this->state->setTick(new MissionPrepTick(this->state));
-    this->state->config = config;
+    this->state->setLapsRemaining(config.pathing.laps);
     this->gcs_server = std::make_unique<GCSServer>(gcs_port, this->state);
 
     // Don't need to look at these futures at all because the connect functions

src/pathing/static.cpp

@@ -606,41 +606,39 @@ std::vector<std::vector<XYZCoord>> generateRankedNewGoalsList(const std::vector<
     return ranked_goals;
 }
 
-/* TODO - doesn't match compeition spec 
-
-    1. First waypoint is not home
-    2. we omit the first waypoint (FATAL) - this means we never tell the computer to hit it
-    3. We don't know where home location is - we rely on geofencing to not fly out of bounds
-*/
+RRTPoint getCurrentLoc(std::shared_ptr<MissionState> state) {
+    std::shared_ptr<MavlinkClient> mav = state->getMav();
+    std::pair<double, double> start_lat_long = mav->latlng_deg();
+
+    GPSCoord start_gps =
+        makeGPSCoord(start_lat_long.first, start_lat_long.second, mav->altitude_agl_m());
+
+    double angle_correction = (90 - mav->heading_deg()) * M_PI / 180.0;
+    double start_angle = (angle_correction < 0) ? (angle_correction + 2 * M_PI) : angle_correction;
+    XYZCoord start_xyz = state->getCartesianConverter().value().toXYZ(start_gps);
+    return RRTPoint(start_xyz, start_angle);
+}
+
 MissionPath generateInitialPath(std::shared_ptr<MissionState> state) {
     // first waypoint is start
 
     // the other waypoitns is the goals
-    if (state->mission_params.getWaypoints().size() < 2) {
+    if (state->mission_params.getWaypoints().size() < 1) {
         loguru::set_thread_name("Static Pathing");
-        LOG_F(ERROR, "Not enough waypoints to generate a path, required 2+, existing waypoints: %s",
+        LOG_F(ERROR, "Not enough waypoints to generate a path, requires >=1, num waypoints: %s",
               std::to_string(state->mission_params.getWaypoints().size()).c_str());
         return {};
     }
 
-    std::vector<XYZCoord> goals;
-
-    // Copy elements from the second element to the last element of source into
-    // destination all other methods of copying over crash???
-    for (int i = 1; i < state->mission_params.getWaypoints().size(); i++) {
-        goals.emplace_back(state->mission_params.getWaypoints()[i]);
-    }
+    std::vector<XYZCoord> goals = state->mission_params.getWaypoints();
 
     // update goals here
     if (state->config.pathing.rrt.generate_deviations) {
         Environment mapping_bounds(state->mission_params.getAirdropBoundary(), {}, {}, goals, {});
         goals = generateRankedNewGoalsList(goals, mapping_bounds)[0];
     }
 
-    double init_angle =
-        std::atan2(goals.front().y - state->mission_params.getWaypoints().front().y,
-                   goals.front().x - state->mission_params.getWaypoints().front().x);
-    RRTPoint start(state->mission_params.getWaypoints().front(), init_angle);
+    RRTPoint start = getCurrentLoc(state);
     start.coord.z = state->config.takeoff.altitude_m;
 
     RRT rrt(start, goals, SEARCH_RADIUS, state->mission_params.getFlightBoundary(), state->config,
@@ -651,10 +649,8 @@ MissionPath generateInitialPath(std::shared_ptr<MissionState> state) {
     std::vector<XYZCoord> path = rrt.getPointsToGoal();
 
     std::vector<GPSCoord> output_coords;
-    output_coords.push_back(
-        state->getCartesianConverter()->toLatLng(state->mission_params.getWaypoints().front()));
-    for (const XYZCoord &wpt : path) {
-        output_coords.push_back(state->getCartesianConverter()->toLatLng(wpt));
+    for (const XYZCoord &waypoint : path) {
+        output_coords.push_back(state->getCartesianConverter()->toLatLng(waypoint));
     }
 
     return MissionPath(MissionPath::Type::FORWARD, output_coords);
@@ -663,11 +659,9 @@ MissionPath generateInitialPath(std::shared_ptr<MissionState> state) {
 MissionPath generateSearchPath(std::shared_ptr<MissionState> state) {
     std::vector<GPSCoord> gps_coords;
     if (state->config.pathing.coverage.method == AirdropCoverageMethod::Enum::FORWARD) {
-        RRTPoint start(state->mission_params.getWaypoints().front(), 0);
-
-        // TODO , change the starting point to be something closer to loiter
-        // region
+        RRTPoint start(state->mission_params.getWaypoints().back(), 0);
         double scan_radius = state->config.pathing.coverage.camera_vision_m;
+
         ForwardCoveragePathing pathing(start, scan_radius,
                                        state->mission_params.getFlightBoundary(),
                                        state->mission_params.getAirdropBoundary(), state->config);
@@ -689,13 +683,7 @@ MissionPath generateSearchPath(std::shared_ptr<MissionState> state) {
 }
 
 MissionPath generateAirdropApproach(std::shared_ptr<MissionState> state, const GPSCoord &goal) {
-    // finds starting location
     std::shared_ptr<MavlinkClient> mav = state->getMav();
-    std::pair<double, double> start_lat_long = {38.315339, -76.548108};
-
-    GPSCoord start_gps =
-        makeGPSCoord(start_lat_long.first, start_lat_long.second, mav->altitude_agl_m());
-
     /*
         Note: this function was neutered right before we attempted to fly at the 2024 competition
         because we suddenly began running into an infinite loop during the execution of this
@@ -704,10 +692,7 @@ MissionPath generateAirdropApproach(std::shared_ptr<MissionState> state, const G
         instead of trying to formulate our own path.
     */
 
-    double start_angle = 90 - mav->heading_deg();
-    XYZCoord start_xyz = state->getCartesianConverter().value().toXYZ(start_gps);
-    RRTPoint start_rrt(start_xyz, start_angle);
-
+    RRTPoint start_rrt = getCurrentLoc(state);
     // pathing
     XYZCoord goal_xyz = state->getCartesianConverter().value().toXYZ(goal);
     AirdropApproachPathing airdrop_planner(start_rrt, goal_xyz, mav->wind(),

src/ticks/fly_waypoints.cpp

@@ -1,7 +1,11 @@
 #include "ticks/fly_waypoints.hpp"
-#include "ticks/fly_search.hpp"
+
+#include <chrono>
+#include <future>
 #include <memory>
 
+#include "pathing/static.hpp"
+#include "ticks/fly_search.hpp"
 #include "ticks/ids.hpp"
 #include "ticks/mav_upload.hpp"
 #include "utilities/constants.hpp"
@@ -11,18 +15,29 @@
 using namespace std::chrono_literals; // NOLINT
 
 FlyWaypointsTick::FlyWaypointsTick(std::shared_ptr<MissionState> state, Tick* next_tick)
-    :Tick(state, TickID::FlyWaypoints), next_tick(next_tick) {}
+    : Tick(state, TickID::FlyWaypoints), next_tick(next_tick) {}
 
 void FlyWaypointsTick::init() {
-    state->getMav()->startMission();
+    // note: I didn't get around to testing if 1 would be a better value than 0
+    // to see if the mission start can be forced.
+    if (!this->state->getMav()->setMissionItem(1)) {
+        LOG_F(ERROR, "Failed to reset Mission");
+    }
+
+    this->mission_started = this->state->getMav()->startMission();
     state->getCamera()->startStreaming();
     this->last_photo_time = getUnixTime_ms();
-}
 
-std::chrono::milliseconds FlyWaypointsTick::getWait() const {
-    return FLY_WAYPOINTS_TICK_WAIT;
+    // I have another one here because idk how startmIssion behaves exactly
+    if (!this->state->getMav()->setMissionItem(1)) {
+        LOG_F(ERROR, "Failed to reset Mission");
+    }
+
+    LOG_F(INFO, "Started FlyWaypointsTick, Laps Remaining: %d", state->getLapsRemaining());
 }
 
+std::chrono::milliseconds FlyWaypointsTick::getWait() const { return FLY_WAYPOINTS_TICK_WAIT; }
+
 Tick* FlyWaypointsTick::tick() {
     auto [lat_deg, lng_deg] = state->getMav()->latlng_deg();
     double altitude_agl_m = state->getMav()->altitude_agl_m();
@@ -60,15 +75,40 @@ Tick* FlyWaypointsTick::tick() {
     // TODO: Eventually implement dynamic avoidance so we dont crash brrr
     bool isMissionFinished = state->getMav()->isMissionFinished();
 
-    if (isMissionFinished) {
-        if (state->config.pathing.laps > 1) {
-            state->config.pathing.laps--;
-            state->getMav()->startMission();
+    if (!isMissionFinished) {
+        return nullptr;
+    }
+
+
+    if (state->getLapsRemaining() > 1) {
+        // regenerate path
+        std::future<MissionPath> init_path;
+        init_path = std::async(std::launch::async, generateInitialPath, this->state);
+        auto init_status = init_path.wait_for(std::chrono::milliseconds(0));
+        int count_ms = 2500;
+        const int wait_time_ms = 100;
+
+        while (init_status != std::future_status::ready && count_ms > 0) {
+            LOG_F(WARNING, "Waiting for path to be generated...");
+            std::this_thread::sleep_for(std::chrono::milliseconds(wait_time_ms));
+            init_status = init_path.wait_for(std::chrono::milliseconds(0));
+            count_ms -= wait_time_ms;
+        }
+
+        if (count_ms <= 0) {
+            LOG_F(ERROR, "Path generation took too long. Trying Again...");
             return nullptr;
         }
 
-        return next_tick;
+        state->decrementLapsRemaining();
+        state->setInitPath(init_path.get());
+
+        return new MavUploadTick(
+            this->state, new FlyWaypointsTick(this->state, new FlySearchTick(this->state)),
+            state->getInitPath(), false);
     }
 
-    return nullptr;
+    return new MavUploadTick(
+        this->state, new FlySearchTick(this->state),
+        state->getCoveragePath(), false);
 }