Hover Search

configs/config.json

@@ -27,11 +27,11 @@
  },
  "coverage": {
  "altitude_m": 30.0,
- "camera_vision_m": 20,
+ "camera_vision_m": 6.0,
  "method": "hover",
  "hover": {
  "pictures_per_stop": 1,
- "hover_time_s": 5
+ "hover_time_s": 1
  },
  "forward": {
  "optimize": true,
@@ -83,5 +83,8 @@
  "gain_auto_upper_limit": 10,
  "gain_auto_lower_limit": 1
  }
+ },
+ "mavlink_parameters": {
+ "Q_GUIDED_MODE": 1 // hover over loiter waypoints
  }
 }

configs/dev-config.json

@@ -27,11 +27,11 @@
  },
  "coverage": {
  "altitude_m": 30.0,
- "camera_vision_m": 20,
+ "camera_vision_m": 6.0,
  "method": "hover",
  "hover": {
  "pictures_per_stop": 1,
- "hover_time_s": 5
+ "hover_time_s": 1
  },
  "forward": {
  "optimize": true,
@@ -83,5 +83,8 @@
  "gain_auto_upper_limit": 10,
  "gain_auto_lower_limit": 1
  }
+ },
+ "mavlink_parameters": {
+ "Q_GUIDED_MODE": 1 // hover over loiter waypoints
  }
 }

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 setSearchPath(std::vector<GPSCoord> search_path);
- const std::vector<GPSCoord>& getSearchPath();
+ 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> search_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/gcs_routes.hpp

@@ -82,20 +82,21 @@ DEF_GCS_HANDLE(Post, airdrop);
 /*
  * GET /path/initial
  * 
- * TODO: reference protobuf class that encompasses the JSON
- * ---
- * Response is the cached initial path that hits all of the competition waypoints.
- * 
- * {
- * TODO: fill in the expected JSON output
- * }
- * 
  * 200 OK: path was previously generated and returned
  * 404 NOT FOUND: no path has been generated yet
  * TODO: determine if there are more errors we might encounter
  */
 DEF_GCS_HANDLE(Get, path, initial);
 
+/*
+ * GET /path/coverage
+ * 
+ * 200 OK: path was previously generated and returned
+ * 404 NOT FOUND: no path has been generated yet
+ * TODO: determine if there are more errors we might encounter
+ */
+DEF_GCS_HANDLE(Get, path, coverage);
+
 /*
  * GET /path/initial/new
  * 

include/network/mavlink.hpp

@@ -7,6 +7,7 @@
 #include <mavsdk/plugins/geofence/geofence.h>
 #include <mavsdk/plugins/action/action.h>
 #include <mavsdk/plugins/mavlink_passthrough/mavlink_passthrough.h>
+#include <mavsdk/plugins/param/param.h>
 #include <memory>
 #include <vector>
 #include <mutex>
@@ -19,8 +20,9 @@
 #include <utility>
 
 #include "protos/obc.pb.h"
-
 #include "utilities/datatypes.hpp"
+#include "pathing/mission_path.hpp"
+#include "utilities/obc_config.hpp"
 
 class MissionState;
 
@@ -32,7 +34,7 @@ class MavlinkClient {
  * example:
  * MavlinkClient("tcp://192.168.65.254:5762")
  */ 
- explicit MavlinkClient(std::string link);
+ explicit MavlinkClient(OBCConfig config);
 
  /*
  * BLOCKING. Continues to try to upload the mission based on the passed through MissionConfig
@@ -54,11 +56,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();
@@ -71,8 +73,7 @@ class MavlinkClient {
  double roll_deg();
  bool isArmed();
  mavsdk::Telemetry::FlightMode flight_mode();
- double angle2D(double x1, double y1, double x2, double y2);
- bool isPointInPolygon(std::pair<double, double> latlng, std::vector<XYZCoord> region);
+ int32_t curr_waypoint() const;
  bool isMissionFinished();
  mavsdk::Telemetry::RcStatus get_conn_status();
  bool armAndHover(std::shared_ptr<MissionState> state);
@@ -86,6 +87,7 @@ class MavlinkClient {
  std::unique_ptr<mavsdk::Geofence> geofence;
  std::unique_ptr<mavsdk::Action> action;
  std::unique_ptr<mavsdk::MavlinkPassthrough> passthrough;
+ std::unique_ptr<mavsdk::Param> param;
 
  struct Data {
  double lat_deg {};

include/pathing/environment.hpp

@@ -101,15 +101,20 @@ class Environment {
  * on the edge are counted as outside the polygon (to be more
  * conservative)
  *
- * Public ONLY for the sake of testing
+ * Public ONLY for the sake of testing <-- no (read below)
+ * 
+ * Making this static so that other parts of the code can access it
+ * but really this should just be a detacted helper function, should
+ * refactor this eventually, but for rn static is the easy thing to do
+ * - tyler
  *
  * @param point ==> given point
  * @return ==> whether or not the point is in this polygon object
  * @see ==> https://en.wikipedia.org/wiki/Point_in_polygon
  * [TODO] make a method to augment the polygon to get similar polygons
  * [TODO] something that increases cost based on time in the edge
  */
- bool isPointInPolygon(const Polygon& polygon, const XYZCoord& point) const;
+ static bool isPointInPolygon(const Polygon& polygon, const XYZCoord& point);
 
  /**
  * Checks wheter a line segment is in bounds or not, it must NOT intersect

include/pathing/mission_path.hpp

@@ -0,0 +1,47 @@
+#ifndef INCLUDE_PATHING_MISSION_PATH_HPP_
+#define INCLUDE_PATHING_MISSION_PATH_HPP_
+
+#include <mavsdk/mavsdk.h>
+#include <mavsdk/plugins/mission_raw/mission_raw.h>
+
+#include <vector>
+
+#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"
@@ -234,18 +235,22 @@ class ForwardCoveragePathing {
 /**
  * Class that performs coverage pathing over a given search area, given that the plane has
  * hovering capabilities and that we want to be taking pictures while hovering over the zone.
- *
+ * 
  * This outputs a series of XYZ Coordinates which represent a points at which the plane
  * should hover and take a picture.
+ * 
+ * Assumptions:
+ * - The drop zone has 4 points which form a rectangle larger than the vision of the camera
  */
 class HoverCoveragePathing {
  public:
- HoverCoveragePathing(Polygon flight_bounds, Polygon drop_zone);
+ explicit HoverCoveragePathing(std::shared_ptr<MissionState> state);
 
  std::vector<XYZCoord> run();
 
  private:
- Polygon flight_bounds;
+ std::shared_ptr<MissionState> state;
+ AirdropCoverageConfig config;
  Polygon drop_zone;
 };
 
@@ -276,8 +281,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/active_takeoff.hpp

@@ -19,11 +19,10 @@ class ActiveTakeoffTick: public Tick {
 
  std::chrono::milliseconds getWait() const override;
 
+ void init() override;
  Tick* tick() override;
 
  private:
- bool started;
-
  std::future<bool> takeoffResult;
 
  void armAndHover();

include/ticks/fly_search.hpp

@@ -19,9 +19,12 @@ class FlySearchTick : public Tick {
  explicit FlySearchTick(std::shared_ptr<MissionState> state);
  std::chrono::milliseconds getWait() const override;
 
+ void init() override;
  Tick* tick() override;
 
  private:
+ std::size_t curr_mission_item;
+ bool mission_started;
  Polygon airdrop_boundary;
  std::chrono::milliseconds last_photo_time;
 };

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,17 +22,18 @@
 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;
 
+ void init() override;
  Tick* tick() override;
 
  private:
  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,
@@ -23,9 +24,11 @@ class PathGenTick : public Tick {
 
  std::chrono::milliseconds getWait() const override;
 
+ void init() override;
  Tick* tick() override;
  private:
- std::future<std::vector<GPSCoord>> path;
+ std::future<MissionPath> init_path;
+ std::future<MissionPath> coverage_path;
 
  void startPathGeneration();
 };

include/ticks/tick.hpp

@@ -22,13 +22,23 @@ class Tick {
  // how long to wait between running each tick function
  virtual std::chrono::milliseconds getWait() const = 0;
 
- // function that is called every getWaitTimeMS() miliseconds
- // return nullptr if no state change should happen
- // return new implementation of Tick if state change should happen
+ /**
+ * function that is called every getWaitTimeMS() miliseconds
+ * return nullptr if no state change should happen
+ * return new implementation of Tick if state change should happen
+ */
  virtual Tick* tick() = 0;
 
+ /**
+ * Code that should be run upon entering the tick should be placed here, not in the constructor
+ * because the constructor can possibly be called before actually transitioning into the tick,
+ * as is the case for 
+ */
+ virtual void init() {}
+
  constexpr TickID getID() const { return this->id; }
  constexpr const char* getName() const { return TICK_ID_TO_STR(this->id); }
+
  protected:
  std::shared_ptr<MissionState> state;
  TickID id;