Skip to content

Fix: Pathing Inputs #346

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
AskewParity wants to merge 2 commits into
base: main
Choose a base branch
from
+27 −13
Draft

Fix: Pathing Inputs #346

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
0847f03
fix: use current position for dynamic pathing
AskewParity Feb 26, 2026
b62a9f0
fix: lint
AskewParity Feb 26, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion configs/dev.json
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
8 changes: 8 additions & 0 deletions include/pathing/static.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
30 changes: 18 additions & 12 deletions src/pathing/static.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -606,6 +606,19 @@ std::vector<std::vector<XYZCoord>> generateRankedNewGoalsList(const std::vector<
return ranked_goals;
}

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);
}

/* TODO - doesn't match compeition spec

1. First waypoint is not home
Expand All @@ -618,7 +631,7 @@ MissionPath generateInitialPath(std::shared_ptr<MissionState> 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 {};
}
Expand Down Expand Up @@ -663,7 +676,7 @@ 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);
RRTPoint start(state->mission_params.getWaypoints().back(), 0);

// TODO , change the starting point to be something closer to loiter
// region
Expand All @@ -689,13 +702,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
Expand All @@ -704,10 +711,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(),
Expand Down Expand Up @@ -742,3 +746,5 @@ MissionPath generateAirdropApproach(std::shared_ptr<MissionState> state, const G

return MissionPath(MissionPath::Type::FORWARD, gps_path);
}