Hover Search (#198)

* refactor config

* beautiful, beautiful macros

* fix lint & add camera vision config option

* log read in config options

* generate hover search path

* kind of working coverage path

* untested upload code

* minor refactor to make init method in tick & fix mission not starting for flysearch

* better coverage path & lots of small things

* fix lint

* load in mavlink parameters via config file

* json with comments :)

* fix merge conflict in integration test

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;