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/core/mission_state.hpp b/include/core/mission_state.hpp index f97de9b8..b1d555cd 100644 --- a/include/core/mission_state.hpp +++ b/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 next_airdrop_to_drop; @@ -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/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/include/ticks/fly_waypoints.hpp b/include/ticks/fly_waypoints.hpp index a352cc7b..6e1bfc68 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; std::chrono::milliseconds last_photo_time; }; diff --git a/src/core/mission_state.cpp b/src/core/mission_state.cpp index bde2b23e..5bee1d7d 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--; +} diff --git a/src/core/obc.cpp b/src/core/obc.cpp index ada75b00..7a945fab 100644 --- a/src/core/obc.cpp +++ b/src/core/obc.cpp @@ -25,7 +25,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/pathing/static.cpp b/src/pathing/static.cpp index edaf2b1b..83cd6949 100644 --- a/src/pathing/static.cpp +++ b/src/pathing/static.cpp @@ -606,30 +606,31 @@ std::vector> 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 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); +} + 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, 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 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 goals = state->mission_params.getWaypoints(); // update goals here if (state->config.pathing.rrt.generate_deviations) { @@ -637,10 +638,7 @@ MissionPath generateInitialPath(std::shared_ptr state) { 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 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); @@ -663,11 +659,9 @@ 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); - - // 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 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 +692,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(), diff --git a/src/ticks/fly_waypoints.cpp b/src/ticks/fly_waypoints.cpp index dc66b1a2..5fc9d53d 100644 --- a/src/ticks/fly_waypoints.cpp +++ b/src/ticks/fly_waypoints.cpp @@ -1,7 +1,11 @@ #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" @@ -11,18 +15,29 @@ using namespace std::chrono_literals; // NOLINT 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(); 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 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); }