diff --git a/include/core/mission_state.hpp b/include/core/mission_state.hpp index e18fa4a8..c3ddf370 100644 --- a/include/core/mission_state.hpp +++ b/include/core/mission_state.hpp @@ -17,6 +17,7 @@ #include "network/airdrop_client.hpp" #include "network/mavlink.hpp" #include "pathing/cartesian.hpp" +#include "pathing/mission_path.hpp" #include "protos/obc.pb.h" #include "ticks/ids.hpp" #include "utilities/constants.hpp" @@ -41,11 +42,11 @@ class MissionState { void setTick(Tick* newTick); - void setInitPath(std::vector init_path); - const std::vector& getInitPath(); + void setInitPath(const MissionPath& init_path); + MissionPath getInitPath(); - void setCoveragePath(std::vector coverage_path); - const std::vector& getCoveragePath(); + void setCoveragePath(const MissionPath& coverage_path); + MissionPath getCoveragePath(); /* * Gets a locking reference to the underlying tick for the given tick subclass T. @@ -107,9 +108,9 @@ class MissionState { std::shared_ptr tick; std::mutex init_path_mut; // for reading/writing the initial path - std::mutex search_path_mut; // for reading/writing the search path - std::vector init_path; - std::vector coverage_path; + MissionPath init_path; + std::mutex coverage_path_mut; // for reading/writing the coverage path + MissionPath coverage_path; std::shared_ptr mav; std::shared_ptr airdrop; diff --git a/include/network/mavlink.hpp b/include/network/mavlink.hpp index 104bafd0..1270415f 100644 --- a/include/network/mavlink.hpp +++ b/include/network/mavlink.hpp @@ -19,8 +19,8 @@ #include #include "protos/obc.pb.h" - #include "utilities/datatypes.hpp" +#include "pathing/mission_path.hpp" class MissionState; @@ -54,11 +54,11 @@ class MavlinkClient { * should never happen due to how the state machine is set up, but it is there just in case. */ bool uploadMissionUntilSuccess(std::shared_ptr state, - bool upload_geofence, std::vector waypoints) const; + bool upload_geofence, const MissionPath& waypoints) const; bool uploadGeofenceUntilSuccess(std::shared_ptr state) const; bool uploadWaypointsUntilSuccess(std::shared_ptr state, - std::vector waypoints) const; + const MissionPath& waypoints) const; std::pair latlng_deg(); double altitude_agl_m(); diff --git a/include/pathing/mission_path.hpp b/include/pathing/mission_path.hpp new file mode 100644 index 00000000..2d3bb7cc --- /dev/null +++ b/include/pathing/mission_path.hpp @@ -0,0 +1,47 @@ +#ifndef INCLUDE_PATHING_MISSION_PATH_HPP_ +#define INCLUDE_PATHING_MISSION_PATH_HPP_ + +#include + +#include +#include + +#include "utilities/datatypes.hpp" +#include "protos/obc.pb.h" + +/** + * Class which abstracts around a waypoint mission that we will upload via mavlink. + * + * There are two kinds of mission paths: forward paths and hover paths + * + * Forward paths will just be normal paths where you fly forward through all waypoints + * + * Hover paths will be where you hover at each waypoint for a specified duration + * + * It may make sense in the future to allow mixing these, so a mission path that contains + * both hovering and forward flight, but at this time it makes more sense to keep it + * simple with a binary choice on the entire path. + */ +class MissionPath { + public: + enum class Type { + FORWARD, HOVER + }; + + MissionPath(Type type, std::vector path, int hover_wait_s = 5); + MissionPath() = default; + + const std::vector& get() const; + const std::vector getCommands() const; + + private: + Type type; + std::vector path; + std::vector path_mav; + int hover_wait_s; + + void generateHoverCommands(); + void generateForwardCommands(); +}; + +#endif // INCLUDE_PATHING_MISSION_PATH_HPP_ diff --git a/include/pathing/static.hpp b/include/pathing/static.hpp index 30c26fd8..bcd70a01 100644 --- a/include/pathing/static.hpp +++ b/include/pathing/static.hpp @@ -15,6 +15,7 @@ #include "pathing/environment.hpp" #include "pathing/plotting.hpp" #include "pathing/tree.hpp" +#include "pathing/mission_path.hpp" #include "udp_squared/internal/enum.h" #include "utilities/constants.hpp" #include "utilities/datatypes.hpp" @@ -279,8 +280,8 @@ class AirdropApproachPathing { RRTPoint wind; }; -std::vector generateInitialPath(std::shared_ptr state); +MissionPath generateInitialPath(std::shared_ptr state); -std::vector generateSearchPath(std::shared_ptr state); +MissionPath generateSearchPath(std::shared_ptr state); #endif // INCLUDE_PATHING_STATIC_HPP_ diff --git a/include/ticks/mav_upload.hpp b/include/ticks/mav_upload.hpp index 1d68ce38..57101db4 100644 --- a/include/ticks/mav_upload.hpp +++ b/include/ticks/mav_upload.hpp @@ -11,6 +11,7 @@ #include "ticks/tick.hpp" #include "ticks/ids.hpp" #include "protos/obc.pb.h" +#include "pathing/mission_path.hpp" /* * Handles uploading waypoint mission to the Pixhawk flight @@ -21,7 +22,7 @@ class MavUploadTick: public Tick { public: MavUploadTick(std::shared_ptr state, - Tick* next_tick, std::vector waypoints, bool upload_geofence); + Tick* next_tick, const MissionPath& waypoints, bool upload_geofence); std::chrono::milliseconds getWait() const override; @@ -31,7 +32,7 @@ class MavUploadTick: public Tick { std::future mav_uploaded; Tick* next_tick; - std::vector waypoints; + MissionPath waypoints; bool upload_geofence; }; diff --git a/include/ticks/path_gen.hpp b/include/ticks/path_gen.hpp index 825783ca..0ff082b2 100644 --- a/include/ticks/path_gen.hpp +++ b/include/ticks/path_gen.hpp @@ -10,6 +10,7 @@ #include "ticks/tick.hpp" #include "core/mission_state.hpp" #include "protos/obc.pb.h" +#include "pathing/mission_path.hpp" /* * Generates a path, caches the path in the mission state, @@ -25,8 +26,8 @@ class PathGenTick : public Tick { Tick* tick() override; private: - std::future> init_path; - std::future> coverage_path; + std::future init_path; + std::future coverage_path; void startPathGeneration(); }; diff --git a/include/utilities/serialize.hpp b/include/utilities/serialize.hpp index 7d9f8f48..8df64950 100644 --- a/include/utilities/serialize.hpp +++ b/include/utilities/serialize.hpp @@ -24,7 +24,7 @@ std::string messagesToJson(Iterator begin, Iterator end) { std::string json = "["; while (it != end) { - google::protobuf::Message& message = *it; + const google::protobuf::Message& message = *it; std::string message_json; google::protobuf::util::MessageToJsonString(message, &message_json); diff --git a/src/core/mission_state.cpp b/src/core/mission_state.cpp index 9a7858c8..cfc5ca64 100644 --- a/src/core/mission_state.cpp +++ b/src/core/mission_state.cpp @@ -66,23 +66,23 @@ TickID MissionState::getTickID() { return this->tick->getID(); } -void MissionState::setInitPath(std::vector init_path) { +void MissionState::setInitPath(const MissionPath& init_path) { Lock lock(this->init_path_mut); this->init_path = init_path; } -const std::vector& MissionState::getInitPath() { +MissionPath MissionState::getInitPath() { Lock lock(this->init_path_mut); return this->init_path; } -void MissionState::setCoveragePath(std::vector coverage_path) { - Lock lock(this->search_path_mut); +void MissionState::setCoveragePath(const MissionPath& coverage_path) { + Lock lock(this->coverage_path_mut); this->coverage_path = coverage_path; } -const std::vector& MissionState::getCoveragePath() { - Lock lock(this->search_path_mut); +MissionPath MissionState::getCoveragePath() { + Lock lock(this->coverage_path_mut); return this->coverage_path; } diff --git a/src/network/airdrop_client.cpp b/src/network/airdrop_client.cpp index 6641fe41..ffbd9e77 100644 --- a/src/network/airdrop_client.cpp +++ b/src/network/airdrop_client.cpp @@ -49,7 +49,7 @@ void AirdropClient::_establishConnection() { while (true) { LOG_F(INFO, "Sending reset packets to all bottles..."); send_ad_packet(this->socket, makeResetPacket(UDP2_ALL)); - std::this_thread::sleep_for(2s); + std::this_thread::sleep_for(10s); if (stop) { return; } diff --git a/src/network/gcs_routes.cpp b/src/network/gcs_routes.cpp index 30fc2a0a..fc3aeb09 100644 --- a/src/network/gcs_routes.cpp +++ b/src/network/gcs_routes.cpp @@ -12,6 +12,7 @@ #include "utilities/serialize.hpp" #include "utilities/logging.hpp" #include "utilities/http.hpp" +#include "pathing/mission_path.hpp" #include "network/gcs_macros.hpp" #include "ticks/tick.hpp" #include "ticks/path_gen.hpp" @@ -128,10 +129,10 @@ DEF_GCS_HANDLE(Get, path, initial) { LOG_REQUEST("GET", "/path/initial"); auto path = state->getInitPath(); - if (path.empty()) { + if (path.get().empty()) { LOG_RESPONSE(WARNING, "No initial path generated", BAD_REQUEST); } else { - std::string json = messagesToJson(path.begin(), path.end()); + std::string json = messagesToJson(path.get().begin(), path.get().end()); LOG_RESPONSE(INFO, "Got initial path", OK, json.c_str(), mime::json); } } @@ -140,10 +141,10 @@ DEF_GCS_HANDLE(Get, path, coverage) { LOG_REQUEST("GET", "/path/coverage"); auto path = state->getCoveragePath(); - if (path.empty()) { + if (path.get().empty()) { LOG_RESPONSE(WARNING, "No coverage path generated", BAD_REQUEST); } else { - std::string json = messagesToJson(path.begin(), path.end()); + std::string json = messagesToJson(path.get().begin(), path.get().end()); LOG_RESPONSE(INFO, "Got initial path", OK, json.c_str(), mime::json); } } @@ -164,7 +165,7 @@ DEF_GCS_HANDLE(Get, path, initial, new) { DEF_GCS_HANDLE(Post, path, initial, validate) { LOG_REQUEST("POST", "/path/initial/validate"); - if (state->getInitPath().empty()) { + if (state->getInitPath().get().empty()) { LOG_RESPONSE(WARNING, "No initial path generated", BAD_REQUEST); return; } diff --git a/src/network/mavlink.cpp b/src/network/mavlink.cpp index 4aa4ab72..b73214e1 100644 --- a/src/network/mavlink.cpp +++ b/src/network/mavlink.cpp @@ -17,6 +17,7 @@ #include #include "core/mission_state.hpp" +#include "pathing/mission_path.hpp" #include "utilities/locks.hpp" #include "utilities/logging.hpp" @@ -131,14 +132,14 @@ MavlinkClient::MavlinkClient(std::string link) bool MavlinkClient::uploadMissionUntilSuccess(std::shared_ptr state, bool upload_geofence, - std::vector waypoints) const { + const MissionPath& path) const { if (upload_geofence) { if (!this->uploadGeofenceUntilSuccess(state)) { return false; } } - if (waypoints.size() > 0) { - if (!this->uploadWaypointsUntilSuccess(state, waypoints)) { + if (path.get().size() > 0) { + if (!this->uploadWaypointsUntilSuccess(state, path)) { return false; } } @@ -154,7 +155,7 @@ bool MavlinkClient::uploadGeofenceUntilSuccess(std::shared_ptr sta LOG_F(ERROR, "Upload failed - no mission"); return false; // todo determine return type } - if (state->getInitPath().empty()) { + if (state->getInitPath().get().empty()) { LOG_F(ERROR, "Upload failed - no initial path"); return false; } @@ -204,30 +205,9 @@ bool MavlinkClient::uploadGeofenceUntilSuccess(std::shared_ptr sta } bool MavlinkClient::uploadWaypointsUntilSuccess(std::shared_ptr state, - std::vector waypoints) const { + const MissionPath& waypoints) const { LOG_SCOPE_F(INFO, "Uploading waypoints"); - // Parse the waypoint information - std::vector mission_items; - int i = 0; - for (const auto& coord : waypoints) { - mavsdk::MissionRaw::MissionItem new_raw_item_nav{}; - new_raw_item_nav.seq = i; - new_raw_item_nav.frame = 3; // MAV_FRAME_GLOBAL_RELATIVE_ALT - new_raw_item_nav.command = 16; // MAV_CMD_NAV_WAYPOINT - new_raw_item_nav.current = (i == 0) ? 1 : 0; - new_raw_item_nav.autocontinue = 1; - new_raw_item_nav.param1 = 0.0; // Hold - new_raw_item_nav.param2 = 7.0; // Accept Radius 7.0m close to 25ft - new_raw_item_nav.param3 = 0.0; // Pass Radius - new_raw_item_nav.param4 = NAN; // Yaw - new_raw_item_nav.x = int32_t(std::round(coord.latitude() * 1e7)); - new_raw_item_nav.y = int32_t(std::round(coord.longitude() * 1e7)); - new_raw_item_nav.z = coord.altitude(); - new_raw_item_nav.mission_type = 0; // MAV_MISSION_TYPE_MISSION - mission_items.push_back(new_raw_item_nav); - i++; - } while (true) { LOG_F(INFO, "Sending waypoint information..."); @@ -236,7 +216,7 @@ bool MavlinkClient::uploadWaypointsUntilSuccess(std::shared_ptr st std::optional result{}; this->mission->upload_mission_async( - mission_items, [&result, &resultMut](const mavsdk::MissionRaw::Result& res) { + waypoints.getCommands(), [&result, &resultMut](const mavsdk::MissionRaw::Result& res) { resultMut.lock(); result = res; resultMut.unlock(); diff --git a/src/pathing/CMakeLists.txt b/src/pathing/CMakeLists.txt index deeb4f6d..cf467c65 100644 --- a/src/pathing/CMakeLists.txt +++ b/src/pathing/CMakeLists.txt @@ -6,6 +6,7 @@ set(FILES plotting.cpp static.cpp tree.cpp + mission_path.cpp ) set (LIB_DEPS diff --git a/src/pathing/mission_path.cpp b/src/pathing/mission_path.cpp new file mode 100644 index 00000000..5ed7c95a --- /dev/null +++ b/src/pathing/mission_path.cpp @@ -0,0 +1,79 @@ +#include "pathing/mission_path.hpp" + +#include + +#include "protos/obc.pb.h" +#include "utilities/logging.hpp" + +MissionPath::MissionPath(MissionPath::Type type, std::vector path, int hover_wait_s): + type(type), path(path), hover_wait_s(hover_wait_s) +{ + switch (type) { + case Type::HOVER: + generateHoverCommands(); + break; + default: + LOG_F(WARNING, "Unknown MissionPath type %d, defaulting to forward", static_cast(type)); + case Type::FORWARD: + generateForwardCommands(); + break; + } +} + +const std::vector& MissionPath::get() const { + return this->path; +} + +const std::vector MissionPath::getCommands() const { + return this->path_mav; +} + +void MissionPath::generateForwardCommands() { + this->path_mav.clear(); // incase this gets called multiple times, but it shouldnt + + // Parse the waypoint information + int i = 0; + for (const auto& coord : this->path) { + mavsdk::MissionRaw::MissionItem item{}; + item.seq = i; + item.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; + item.command = MAV_CMD_NAV_WAYPOINT; + item.current = (i == 0) ? 1 : 0; + item.autocontinue = 1; + item.param1 = 0.0; // Hold + item.param2 = 7.0; // Accept Radius 7.0m close to 25ft + item.param3 = 0.0; // Pass Radius + item.param4 = NAN; // Yaw + item.x = int32_t(std::round(coord.latitude() * 1e7)); + item.y = int32_t(std::round(coord.longitude() * 1e7)); + item.z = coord.altitude(); + item.mission_type = MAV_MISSION_TYPE_MISSION; + this->path_mav.push_back(item); + i++; + } +} + +void MissionPath::generateHoverCommands() { + this->path_mav.clear(); + + // https://mavlink.io/en/messages/common.html#MAV_CMD_NAV_LOITER_TIME + int i = 0; + for (const auto& coord : this->path) { + mavsdk::MissionRaw::MissionItem item{}; + item.seq = i; + item.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; + item.command = MAV_CMD_NAV_LOITER_TIME; + item.current = (i == 0) ? 1 : 0; + item.autocontinue = 1; + item.param1 = static_cast(this->hover_wait_s); + item.param2 = 0.0f; // 0 => dont need to point heading at next waypoint + item.param3 = 0.0f; // loiter radius, which shouldn't matter for hover quadplane + item.param4 = NAN; // xtrack, something for forward flight planes which we arent here + item.x = int32_t(std::round(coord.latitude() * 1e7)); + item.y = int32_t(std::round(coord.longitude() * 1e7)); + item.z = coord.altitude(); + item.mission_type = MAV_MISSION_TYPE_MISSION; + this->path_mav.push_back(item); + i++; + } +} diff --git a/src/pathing/static.cpp b/src/pathing/static.cpp index df48e100..8a989f2b 100644 --- a/src/pathing/static.cpp +++ b/src/pathing/static.cpp @@ -425,10 +425,10 @@ std::vector HoverCoveragePathing::run() { double vision = this->config.camera_vision_m; double altitude = this->config.altitude_m; - double start_y = top_left.y - (vision / 2.0); - double stop_y = bottom_left.y - (vision / 2.0); - double start_x = top_left.x + (vision / 2.0); - double stop_x = top_right.x + (vision / 2.0); + double start_y = std::max(top_left.y, top_right.y) - (vision / 2.0); + double stop_y = std::min(bottom_left.y, bottom_right.y) - (vision / 2.0); + double start_x = std::min(top_left.x, bottom_left.x) + (vision / 2.0); + double stop_x = std::max(top_right.x, bottom_right.x) + (vision / 2.0); bool right = true; // start going from right to left for (double y = start_y; y > stop_y; y -= vision) { @@ -489,7 +489,7 @@ RRTPoint AirdropApproachPathing::getDropLocation() const { return RRTPoint(drop_location, angle); } -std::vector generateInitialPath(std::shared_ptr state) { +MissionPath generateInitialPath(std::shared_ptr state) { // first waypoint is start // the other waypoitns is the goals @@ -527,19 +527,20 @@ std::vector generateInitialPath(std::shared_ptr state) { output_coords.push_back(state->getCartesianConverter()->toLatLng(wpt)); } - return output_coords; + return MissionPath(MissionPath::Type::FORWARD, output_coords); } -std::vector generateSearchPath(std::shared_ptr state) { +MissionPath generateSearchPath(std::shared_ptr state) { if (state->config.pathing.coverage.method == AirdropCoverageMethod::Enum::FORWARD) { LOG_F(FATAL, "Forward search path not fully integrated yet."); - return {}; + return MissionPath(MissionPath::Type::FORWARD, {}); } else { // hover HoverCoveragePathing pathing(state->mission_params.getAirdropBoundary(), state->config); std::vector coords; for (const XYZCoord &coord : pathing.run()) { coords.push_back(state->getCartesianConverter()->toLatLng(coord)); } - return coords; + return MissionPath(MissionPath::Type::HOVER, coords, + state->config.pathing.coverage.hover.hover_time_s); } } diff --git a/src/ticks/mav_upload.cpp b/src/ticks/mav_upload.cpp index b55fab62..3efebfcc 100644 --- a/src/ticks/mav_upload.cpp +++ b/src/ticks/mav_upload.cpp @@ -13,7 +13,7 @@ #include "protos/obc.pb.h" MavUploadTick::MavUploadTick(std::shared_ptr state, - Tick* next_tick, std::vector waypoints, bool upload_geofence) + Tick* next_tick, const MissionPath& waypoints, bool upload_geofence) :Tick(state, TickID::MavUpload), next_tick{next_tick}, waypoints{waypoints}, upload_geofence{upload_geofence} { this->mav_uploaded = std::async(std::launch::async,