untested upload code

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<GPSCoord> init_path);
- const std::vector<GPSCoord>& getInitPath();
+ void setInitPath(const MissionPath& init_path);
+ MissionPath getInitPath();
 
- void setCoveragePath(std::vector<GPSCoord> coverage_path);
- const std::vector<GPSCoord>& 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> 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<GPSCoord> init_path;
- std::vector<GPSCoord> coverage_path;
+ MissionPath init_path;
+ std::mutex coverage_path_mut; // for reading/writing the coverage path
+ MissionPath coverage_path;
 
  std::shared_ptr<MavlinkClient> mav;
  std::shared_ptr<AirdropClient> airdrop;

include/network/mavlink.hpp

@@ -19,8 +19,8 @@
 #include <utility>
 
 #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<MissionState> state,
- bool upload_geofence, std::vector<GPSCoord> waypoints) const;
+ bool upload_geofence, const MissionPath& waypoints) const;
 
  bool uploadGeofenceUntilSuccess(std::shared_ptr<MissionState> state) const;
  bool uploadWaypointsUntilSuccess(std::shared_ptr<MissionState> state,
- std::vector<GPSCoord> waypoints) const;
+ const MissionPath& waypoints) const;
 
  std::pair<double, double> latlng_deg();
  double altitude_agl_m();

include/pathing/mission_path.hpp

@@ -0,0 +1,47 @@
+#ifndef INCLUDE_PATHING_MISSION_PATH_HPP_
+#define INCLUDE_PATHING_MISSION_PATH_HPP_
+
+#include <vector>
+
+#include <mavsdk/mavsdk.h>
+#include <mavsdk/plugins/mission_raw/mission_raw.h>
+
+#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<GPSCoord> path, int hover_wait_s = 5);
+ MissionPath() = default;
+
+ const std::vector<GPSCoord>& get() const;
+ const std::vector<mavsdk::MissionRaw::MissionItem> getCommands() const;
+
+ private:
+ Type type;
+ std::vector<GPSCoord> path;
+ std::vector<mavsdk::MissionRaw::MissionItem> path_mav;
+ int hover_wait_s;
+
+ void generateHoverCommands();
+ void generateForwardCommands();
+};
+
+#endif // INCLUDE_PATHING_MISSION_PATH_HPP_

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<GPSCoord> generateInitialPath(std::shared_ptr<MissionState> state);
+MissionPath generateInitialPath(std::shared_ptr<MissionState> state);
 
-std::vector<GPSCoord> generateSearchPath(std::shared_ptr<MissionState> state);
+MissionPath generateSearchPath(std::shared_ptr<MissionState> state);
 
 #endif // INCLUDE_PATHING_STATIC_HPP_

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<MissionState> state,
- Tick* next_tick, std::vector<GPSCoord> 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<bool> mav_uploaded;
 
  Tick* next_tick;
- std::vector<GPSCoord> waypoints;
+ MissionPath waypoints;
  bool upload_geofence;
 };
 

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<std::vector<GPSCoord>> init_path;
- std::future<std::vector<GPSCoord>> coverage_path;
+ std::future<MissionPath> init_path;
+ std::future<MissionPath> coverage_path;
 
  void startPathGeneration();
 };

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

src/core/mission_state.cpp

@@ -66,23 +66,23 @@ TickID MissionState::getTickID() {
  return this->tick->getID();
 }
 
-void MissionState::setInitPath(std::vector<GPSCoord> init_path) {
+void MissionState::setInitPath(const MissionPath& init_path) {
  Lock lock(this->init_path_mut);
  this->init_path = init_path;
 }
 
-const std::vector<GPSCoord>& MissionState::getInitPath() {
+MissionPath MissionState::getInitPath() {
  Lock lock(this->init_path_mut);
  return this->init_path;
 }
 
-void MissionState::setCoveragePath(std::vector<GPSCoord> 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<GPSCoord>& MissionState::getCoveragePath() {
- Lock lock(this->search_path_mut);
+MissionPath MissionState::getCoveragePath() {
+ Lock lock(this->coverage_path_mut);
  return this->coverage_path;
 }
 

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

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

src/network/mavlink.cpp

@@ -17,6 +17,7 @@
 #include <thread>
 
 #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<MissionState> state,
  bool upload_geofence,
- std::vector<GPSCoord> 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<MissionState> 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<MissionState> sta
 }
 
 bool MavlinkClient::uploadWaypointsUntilSuccess(std::shared_ptr<MissionState> state,
- std::vector<GPSCoord> waypoints) const {
+ const MissionPath& waypoints) const {
  LOG_SCOPE_F(INFO, "Uploading waypoints");
 
- // Parse the waypoint information
- std::vector<mavsdk::MissionRaw::MissionItem> 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<MissionState> st
  std::optional<mavsdk::MissionRaw::Result> 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();

src/pathing/CMakeLists.txt

@@ -6,6 +6,7 @@ set(FILES
  plotting.cpp
  static.cpp
  tree.cpp
+ mission_path.cpp
 )
 
 set (LIB_DEPS