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..c44fe6d7 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, num waypoints: %s", std::to_string(state->mission_params.getWaypoints().size()).c_str()); return {}; } @@ -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); } + +