From 0847f03fd5cc3efab7e6708853b7524d6df26170 Mon Sep 17 00:00:00 2001 From: AskewParity Date: Wed, 25 Feb 2026 18:15:48 -0800 Subject: [PATCH 01/12] fix: use current position for dynamic pathing --- configs/dev.json | 2 +- include/pathing/static.hpp | 8 ++++++++ src/pathing/static.cpp | 32 +++++++++++++++++++------------- 3 files changed, 28 insertions(+), 14 deletions(-) diff --git a/configs/dev.json b/configs/dev.json index e55ddd1f..a98895f3 100644 --- a/configs/dev.json +++ b/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 diff --git a/include/pathing/static.hpp b/include/pathing/static.hpp index 657f2328..da9bb561 100644 --- a/include/pathing/static.hpp +++ b/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 state); + MissionPath generateInitialPath(std::shared_ptr state); MissionPath generateSearchPath(std::shared_ptr state); diff --git a/src/pathing/static.cpp b/src/pathing/static.cpp index edaf2b1b..4da33c58 100644 --- a/src/pathing/static.cpp +++ b/src/pathing/static.cpp @@ -606,6 +606,19 @@ std::vector> generateRankedNewGoalsList(const std::vector< return ranked_goals; } +RRTPoint getCurrentLoc(std::shared_ptr state) { + std::shared_ptr mav = state->getMav(); + std::pair 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); +} + /* TODO - doesn't match compeition spec 1. First waypoint is not home @@ -618,7 +631,7 @@ MissionPath generateInitialPath(std::shared_ptr state) { // the other waypoitns is the goals if (state->mission_params.getWaypoints().size() < 2) { 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 >=2, existing waypoints: %s", std::to_string(state->mission_params.getWaypoints().size()).c_str()); return {}; } @@ -627,7 +640,7 @@ MissionPath generateInitialPath(std::shared_ptr state) { // 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++) { + for (int i = 1; i < state->mission_params.getWaypoints().size(); i++) { goals.emplace_back(state->mission_params.getWaypoints()[i]); } @@ -663,7 +676,7 @@ MissionPath generateInitialPath(std::shared_ptr state) { MissionPath generateSearchPath(std::shared_ptr state) { std::vector gps_coords; if (state->config.pathing.coverage.method == AirdropCoverageMethod::Enum::FORWARD) { - RRTPoint start(state->mission_params.getWaypoints().front(), 0); + RRTPoint start(state->mission_params.getWaypoints().back(), 0); // TODO , change the starting point to be something closer to loiter // region @@ -689,13 +702,7 @@ MissionPath generateSearchPath(std::shared_ptr state) { } MissionPath generateAirdropApproach(std::shared_ptr state, const GPSCoord &goal) { - // finds starting location std::shared_ptr mav = state->getMav(); - std::pair 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 +711,7 @@ MissionPath generateAirdropApproach(std::shared_ptr 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(), @@ -742,3 +746,5 @@ MissionPath generateAirdropApproach(std::shared_ptr state, const G return MissionPath(MissionPath::Type::FORWARD, gps_path); } + + From b62a9f07843426f148c4fd56170caac3fe8c8329 Mon Sep 17 00:00:00 2001 From: AskewParity Date: Wed, 25 Feb 2026 18:18:58 -0800 Subject: [PATCH 02/12] fix: lint --- src/pathing/static.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/pathing/static.cpp b/src/pathing/static.cpp index 4da33c58..c44fe6d7 100644 --- a/src/pathing/static.cpp +++ b/src/pathing/static.cpp @@ -631,7 +631,7 @@ MissionPath generateInitialPath(std::shared_ptr state) { // the other waypoitns is the goals if (state->mission_params.getWaypoints().size() < 2) { loguru::set_thread_name("Static Pathing"); - LOG_F(ERROR, "Not enough waypoints to generate a path, requires >=2, existing waypoints: %s", + LOG_F(ERROR, "Not enough waypoints to generate a path, requires >=2, num waypoints: %s", std::to_string(state->mission_params.getWaypoints().size()).c_str()); return {}; } @@ -640,7 +640,7 @@ MissionPath generateInitialPath(std::shared_ptr state) { // 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++) { + for (int i = 1; i < state->mission_params.getWaypoints().size(); i++) { goals.emplace_back(state->mission_params.getWaypoints()[i]); } From e28ed59b60e8911772b5bb95e2ceab7a7f9f4b3d Mon Sep 17 00:00:00 2001 From: AskewParity Date: Fri, 27 Feb 2026 12:11:59 -0800 Subject: [PATCH 03/12] feat: use heading --- src/pathing/static.cpp | 18 +++++++----------- 1 file changed, 7 insertions(+), 11 deletions(-) diff --git a/src/pathing/static.cpp b/src/pathing/static.cpp index c44fe6d7..3a1b8d49 100644 --- a/src/pathing/static.cpp +++ b/src/pathing/static.cpp @@ -629,20 +629,16 @@ MissionPath generateInitialPath(std::shared_ptr 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, requires >=2, num 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 goals; - + std::vector goals = state->mission_params.getWaypoints(); // 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]); - } // update goals here if (state->config.pathing.rrt.generate_deviations) { @@ -650,10 +646,11 @@ MissionPath generateInitialPath(std::shared_ptr state) { goals = generateRankedNewGoalsList(goals, mapping_bounds)[0]; } + RRTPoint start = getCurrentLoc(state); 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); + std::atan2(goals.front().y - start.coord.y, + goals.front().x - start.coord.x); + start.psi = init_angle; start.coord.z = state->config.takeoff.altitude_m; RRT rrt(start, goals, SEARCH_RADIUS, state->mission_params.getFlightBoundary(), state->config, @@ -677,7 +674,6 @@ MissionPath generateSearchPath(std::shared_ptr state) { std::vector gps_coords; if (state->config.pathing.coverage.method == AirdropCoverageMethod::Enum::FORWARD) { RRTPoint start(state->mission_params.getWaypoints().back(), 0); - // TODO , change the starting point to be something closer to loiter // region double scan_radius = state->config.pathing.coverage.camera_vision_m; From 8efef91a4fa2a4f21ceda756f8f9c7891c7b08bc Mon Sep 17 00:00:00 2001 From: AskewParity Date: Fri, 27 Feb 2026 12:12:21 -0800 Subject: [PATCH 04/12] feat: laps now generate dynamicslly --- include/ticks/fly_waypoints.hpp | 1 + src/ticks/fly_waypoints.cpp | 61 ++++++++++++++++++++++++++------- 2 files changed, 50 insertions(+), 12 deletions(-) diff --git a/include/ticks/fly_waypoints.hpp b/include/ticks/fly_waypoints.hpp index 7bf76fb9..3ccd8d54 100644 --- a/include/ticks/fly_waypoints.hpp +++ b/include/ticks/fly_waypoints.hpp @@ -23,6 +23,7 @@ class FlyWaypointsTick : public Tick { Tick* tick() override; private: + bool mission_started; Tick* next_tick; }; diff --git a/src/ticks/fly_waypoints.cpp b/src/ticks/fly_waypoints.cpp index dc06a3bd..92944ef3 100644 --- a/src/ticks/fly_waypoints.cpp +++ b/src/ticks/fly_waypoints.cpp @@ -1,35 +1,72 @@ #include "ticks/fly_waypoints.hpp" -#include "ticks/fly_search.hpp" + +#include +#include #include +#include "pathing/static.hpp" +#include "ticks/fly_search.hpp" #include "ticks/ids.hpp" #include "ticks/mav_upload.hpp" #include "utilities/constants.hpp" FlyWaypointsTick::FlyWaypointsTick(std::shared_ptr 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(); -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"); + } } +std::chrono::milliseconds FlyWaypointsTick::getWait() const { return FLY_WAYPOINTS_TICK_WAIT; } + 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; + } + + state->config.pathing.laps--; + if (state->config.pathing.laps > 0) { + // regenerate path + std::future 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->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); } From e8a4ff8c58566102a41089cc4ce63b2c4e11a787 Mon Sep 17 00:00:00 2001 From: AskewParity Date: Fri, 27 Feb 2026 13:06:31 -0800 Subject: [PATCH 05/12] fix: remove incorrect angle and waypoint plaement for init path --- src/pathing/static.cpp | 19 ++++--------------- 1 file changed, 4 insertions(+), 15 deletions(-) diff --git a/src/pathing/static.cpp b/src/pathing/static.cpp index 3a1b8d49..300c4261 100644 --- a/src/pathing/static.cpp +++ b/src/pathing/static.cpp @@ -637,8 +637,6 @@ MissionPath generateInitialPath(std::shared_ptr state) { } std::vector goals = state->mission_params.getWaypoints(); - // Copy elements from the second element to the last element of source into - // destination all other methods of copying over crash??? // update goals here if (state->config.pathing.rrt.generate_deviations) { @@ -647,10 +645,6 @@ MissionPath generateInitialPath(std::shared_ptr state) { } RRTPoint start = getCurrentLoc(state); - double init_angle = - std::atan2(goals.front().y - start.coord.y, - goals.front().x - start.coord.x); - start.psi = init_angle; start.coord.z = state->config.takeoff.altitude_m; RRT rrt(start, goals, SEARCH_RADIUS, state->mission_params.getFlightBoundary(), state->config, @@ -661,10 +655,8 @@ MissionPath generateInitialPath(std::shared_ptr state) { std::vector path = rrt.getPointsToGoal(); std::vector 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); @@ -674,9 +666,8 @@ MissionPath generateSearchPath(std::shared_ptr state) { std::vector gps_coords; if (state->config.pathing.coverage.method == AirdropCoverageMethod::Enum::FORWARD) { RRTPoint start(state->mission_params.getWaypoints().back(), 0); - // TODO , change the starting point to be something closer to loiter - // region 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); @@ -741,6 +732,4 @@ MissionPath generateAirdropApproach(std::shared_ptr state, const G // gps_path.push_back(goal); return MissionPath(MissionPath::Type::FORWARD, gps_path); -} - - +} \ No newline at end of file From 2fb1739102f8d26098e046f93ea7e50eac30b70a Mon Sep 17 00:00:00 2001 From: AskewParity Date: Fri, 27 Feb 2026 13:15:00 -0800 Subject: [PATCH 06/12] fix: lint --- src/pathing/static.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pathing/static.cpp b/src/pathing/static.cpp index 300c4261..cd300b0b 100644 --- a/src/pathing/static.cpp +++ b/src/pathing/static.cpp @@ -732,4 +732,4 @@ MissionPath generateAirdropApproach(std::shared_ptr state, const G // gps_path.push_back(goal); return MissionPath(MissionPath::Type::FORWARD, gps_path); -} \ No newline at end of file +} From 90585355f813373954e2d2f78396501e51465a82 Mon Sep 17 00:00:00 2001 From: AskewParity Date: Tue, 3 Mar 2026 18:52:02 -0800 Subject: [PATCH 07/12] fix: hardcoded timeout will trigger --- src/ticks/fly_waypoints.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ticks/fly_waypoints.cpp b/src/ticks/fly_waypoints.cpp index ad478b8c..ea5e954a 100644 --- a/src/ticks/fly_waypoints.cpp +++ b/src/ticks/fly_waypoints.cpp @@ -93,7 +93,7 @@ Tick* FlyWaypointsTick::tick() { count_ms -= wait_time_ms; } - if (count_ms < 0) { + if (count_ms <= 0) { LOG_F(ERROR, "Path generation took too long. Trying Again..."); return nullptr; } From 6ff2d82a8401fb0fc34403dcc5e8abae8d0e80f6 Mon Sep 17 00:00:00 2001 From: AskewParity Date: Thu, 5 Mar 2026 15:45:42 -0800 Subject: [PATCH 08/12] feat: change mission state to modify laps --- include/core/mission_state.hpp | 8 ++++++++ src/core/mission_state.cpp | 13 +++++++++++++ src/core/obc.cpp | 1 + src/ticks/fly_waypoints.cpp | 5 ++++- 4 files changed, 26 insertions(+), 1 deletion(-) diff --git a/include/core/mission_state.hpp b/include/core/mission_state.hpp index f97de9b8..6199dbc1 100644 --- a/include/core/mission_state.hpp +++ b/include/core/mission_state.hpp @@ -108,6 +108,11 @@ 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; @@ -131,6 +136,9 @@ class MissionState { std::mutex dropped_airdrops_mut; std::unordered_set dropped_airdrops; + std::mutex laps_remaining_mut; + int laps_remaining; + std::shared_ptr mav; std::shared_ptr airdrop; std::shared_ptr cv; diff --git a/src/core/mission_state.cpp b/src/core/mission_state.cpp index bde2b23e..09411c0c 100644 --- a/src/core/mission_state.cpp +++ b/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--; +} \ No newline at end of file diff --git a/src/core/obc.cpp b/src/core/obc.cpp index ada75b00..acc2ef8a 100644 --- a/src/core/obc.cpp +++ b/src/core/obc.cpp @@ -26,6 +26,7 @@ OBC::OBC(OBCConfig config) { this->state = std::make_shared(config); this->state->setTick(new MissionPrepTick(this->state)); this->state->config = config; + this->state->setLapsRemaining(config.pathing.laps); this->gcs_server = std::make_unique(gcs_port, this->state); // Don't need to look at these futures at all because the connect functions diff --git a/src/ticks/fly_waypoints.cpp b/src/ticks/fly_waypoints.cpp index ea5e954a..633b1504 100644 --- a/src/ticks/fly_waypoints.cpp +++ b/src/ticks/fly_waypoints.cpp @@ -32,6 +32,8 @@ void FlyWaypointsTick::init() { 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; } @@ -77,7 +79,8 @@ Tick* FlyWaypointsTick::tick() { return nullptr; } - state->config.pathing.laps--; + state->decrementLapsRemaining(); + if (state->config.pathing.laps > 0) { // regenerate path std::future init_path; From 069736138d9f8b6097c81785e558c74bf7273c13 Mon Sep 17 00:00:00 2001 From: AskewParity Date: Thu, 5 Mar 2026 16:05:47 -0800 Subject: [PATCH 09/12] fix: fly_waypoints uses dynamic missionstate instead of static config, chore: made config const --- include/core/mission_state.hpp | 2 +- src/core/obc.cpp | 1 - src/ticks/fly_waypoints.cpp | 2 +- 3 files changed, 2 insertions(+), 3 deletions(-) diff --git a/include/core/mission_state.hpp b/include/core/mission_state.hpp index 6199dbc1..b1d555cd 100644 --- a/include/core/mission_state.hpp +++ b/include/core/mission_state.hpp @@ -115,7 +115,7 @@ class MissionState { MissionParameters mission_params; // has its own mutex - OBCConfig config; + const OBCConfig config; std::optional next_airdrop_to_drop; diff --git a/src/core/obc.cpp b/src/core/obc.cpp index acc2ef8a..7a945fab 100644 --- a/src/core/obc.cpp +++ b/src/core/obc.cpp @@ -25,7 +25,6 @@ OBC::OBC(OBCConfig config) { this->state = std::make_shared(config); this->state->setTick(new MissionPrepTick(this->state)); - this->state->config = config; this->state->setLapsRemaining(config.pathing.laps); this->gcs_server = std::make_unique(gcs_port, this->state); diff --git a/src/ticks/fly_waypoints.cpp b/src/ticks/fly_waypoints.cpp index 633b1504..2a0b9eb0 100644 --- a/src/ticks/fly_waypoints.cpp +++ b/src/ticks/fly_waypoints.cpp @@ -81,7 +81,7 @@ Tick* FlyWaypointsTick::tick() { state->decrementLapsRemaining(); - if (state->config.pathing.laps > 0) { + if (state->getLapsRemaining() > 0) { // regenerate path std::future init_path; init_path = std::async(std::launch::async, generateInitialPath, this->state); From de1a4a78db39b7106fae49584755f84204a83a9e Mon Sep 17 00:00:00 2001 From: AskewParity Date: Thu, 5 Mar 2026 16:27:34 -0800 Subject: [PATCH 10/12] chore: lint --- src/core/mission_state.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/core/mission_state.cpp b/src/core/mission_state.cpp index 09411c0c..5bee1d7d 100644 --- a/src/core/mission_state.cpp +++ b/src/core/mission_state.cpp @@ -142,4 +142,4 @@ void MissionState::setLapsRemaining(int laps) { void MissionState::decrementLapsRemaining() { Lock lock(this->laps_remaining_mut); this->laps_remaining--; -} \ No newline at end of file +} From 066d128413c8491f3546ad53daf1de80fb6444a7 Mon Sep 17 00:00:00 2001 From: AskewParity Date: Thu, 5 Mar 2026 16:36:03 -0800 Subject: [PATCH 11/12] chore: remove outdated todo --- src/pathing/static.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/pathing/static.cpp b/src/pathing/static.cpp index cd300b0b..83cd6949 100644 --- a/src/pathing/static.cpp +++ b/src/pathing/static.cpp @@ -619,12 +619,6 @@ RRTPoint getCurrentLoc(std::shared_ptr state) { return RRTPoint(start_xyz, start_angle); } -/* 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 -*/ MissionPath generateInitialPath(std::shared_ptr state) { // first waypoint is start From 1d8051e6cbe91d8483d34fb155c668eb7a682385 Mon Sep 17 00:00:00 2001 From: AskewParity Date: Thu, 5 Mar 2026 22:41:29 -0800 Subject: [PATCH 12/12] fix: prevent premature lap count --- src/ticks/fly_waypoints.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/ticks/fly_waypoints.cpp b/src/ticks/fly_waypoints.cpp index 2a0b9eb0..5fc9d53d 100644 --- a/src/ticks/fly_waypoints.cpp +++ b/src/ticks/fly_waypoints.cpp @@ -79,9 +79,8 @@ Tick* FlyWaypointsTick::tick() { return nullptr; } - state->decrementLapsRemaining(); - if (state->getLapsRemaining() > 0) { + if (state->getLapsRemaining() > 1) { // regenerate path std::future init_path; init_path = std::async(std::launch::async, generateInitialPath, this->state); @@ -101,6 +100,7 @@ Tick* FlyWaypointsTick::tick() { return nullptr; } + state->decrementLapsRemaining(); state->setInitPath(init_path.get()); return new MavUploadTick(