From 0847f03fd5cc3efab7e6708853b7524d6df26170 Mon Sep 17 00:00:00 2001 From: AskewParity Date: Wed, 25 Feb 2026 18:15:48 -0800 Subject: [PATCH 1/2] 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 2/2] 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]); }