Feat/airdrop prep

configs/dev-config.json

@@ -27,7 +27,7 @@
  },
  "coverage": {
  "altitude_m": 30.0,
- "camera_vision_m": 6.0,
+ "camera_vision_m": 5.0,
  "method": "hover",
  "hover": {
  "pictures_per_stop": 1,
@@ -42,7 +42,7 @@
  "approach": {
  "drop_method": "unguided",
  "drop_angle_rad": 2.52134317,
- "drop_altitude_m": 26.0,
+ "drop_altitude_m": 35.0,
  "guided_drop_distance_m": 50.0,
  "unguided_drop_distance_m": 50.0
  }

include/core/mission_parameters.hpp

@@ -26,10 +26,11 @@ class MissionParameters {
 
  // Getters for singular value
  // Use when only need to read one value
+
  Polygon getFlightBoundary();
  Polygon getAirdropBoundary();
  Polyline getWaypoints();
- const std::vector<Bottle>& getAirdropBottles();
+ std::vector<Bottle> getAirdropBottles();
 
  // Getters for multiple values
  // Use when need to get multiple values

include/core/mission_state.hpp

@@ -8,6 +8,7 @@
 #include <mutex>
 #include <optional>
 #include <queue>
+#include <unordered_set>
 #include <vector>
 
 #include "camera/interface.hpp"
@@ -48,6 +49,12 @@ class MissionState {
  void setCoveragePath(const MissionPath& coverage_path);
  MissionPath getCoveragePath();
 
+ void setAirdropPath(const MissionPath& airdrop_path);
+ MissionPath getAirdropPath();
+
+ void markBottleAsDropped(BottleDropIndex bottle);
+ std::unordered_set<BottleDropIndex> getDroppedBottles();
+
  /*
  * Gets a locking reference to the underlying tick for the given tick subclass T.
  *
@@ -98,6 +105,7 @@ class MissionState {
 
  MissionParameters mission_params; // has its own mutex
 
+
  OBCConfig config;
 
  private:
@@ -111,6 +119,11 @@ class MissionState {
  MissionPath init_path;
  std::mutex coverage_path_mut; // for reading/writing the coverage path
  MissionPath coverage_path;
+ std::mutex airdrop_path_mut;
+ MissionPath airdrop_path;
+
+ std::mutex dropped_bottles_mut;
+ std::unordered_set<BottleDropIndex> dropped_bottles;
 
  std::shared_ptr<MavlinkClient> mav;
  std::shared_ptr<AirdropClient> airdrop;

include/cv/utilities.hpp

@@ -8,15 +8,6 @@
 #include "utilities/constants.hpp"
 #include "protos/obc.pb.h"
 
-struct DetectedTarget {
- DetectedTarget(GPSCoord coord, BottleDropIndex index, double match_distance)
- :coord{coord}, likely_bottle{index}, match_distance{match_distance} {}
-
- GPSCoord coord;
- BottleDropIndex likely_bottle;
- double match_distance;
-};
-
 class Bbox {
  public:
  int x1;
@@ -34,6 +25,16 @@ struct CroppedTarget {
  bool isMannikin;
 };
 
+struct DetectedTarget {
+ DetectedTarget(GPSCoord coord, BottleDropIndex index, double match_distance, CroppedTarget crop)
+ :coord{coord}, likely_bottle{index}, match_distance{match_distance}, crop{crop} {}
+
+ GPSCoord coord;
+ BottleDropIndex likely_bottle;
+ double match_distance;
+ CroppedTarget crop;
+};
+
 cv::Mat crop(cv::Mat original, Bbox bbox);
 
 #endif // INCLUDE_CV_UTILITIES_HPP_

include/network/gcs_routes.hpp

@@ -65,19 +65,11 @@ DEF_GCS_HANDLE(Get, mission);
 DEF_GCS_HANDLE(Post, mission);
 
 /*
- * POST /airdrop
- * 
- * {
- * TODO: fill in the expected JSON format
- * }
- * 
- * TODO: reference protobuf class that encompasses the JSON
+ * POST /targets/locations
  * ---
- * Response is plain text that says whether posting was successful or not
- * 200 OK: waypoints were in correct format and uploaded to server
- * 400 BAD REQUEST: waypoints were not in correct format; ignored
+ * Upload the bottle locations to the OBC
  */
-DEF_GCS_HANDLE(Post, airdrop);
+DEF_GCS_HANDLE(Post, targets, locations);
 
 /*
  * GET /path/initial
@@ -153,13 +145,36 @@ DEF_GCS_HANDLE(Post, takeoff, autonomous);
 /**
  * POST /targets/validate
  * ---
+ * validate the current set of matched targets and go onto airdrop mode
  */
 DEF_GCS_HANDLE(Post, targets, validate);
 
 /**
  * POST /targets/reject
  * ---
+ * tell the plane to do the search path again because we missed targets
  */
 DEF_GCS_HANDLE(Post, targets, reject);
 
+/**
+ * GET /targets/all
+ * ---
+ * get all of the targets that have been identified & send to the GCS
+ */
+DEF_GCS_HANDLE(Get, targets, all);
+
+/**
+ * GET /targets/matched
+ * ---
+ * get the current mappings between identified targets and bottles
+ */
+DEF_GCS_HANDLE(Get, targets, matched);
+
+/**
+ * POST /targets/matched
+ * ---
+ * manual override for mappings between targets
+ */
+DEF_GCS_HANDLE(Post, targets, matched);
+
 #endif // INCLUDE_NETWORK_GCS_ROUTES_HPP_

include/network/mavlink.hpp

@@ -2,22 +2,23 @@
 #define INCLUDE_NETWORK_MAVLINK_HPP_
 
 #include <mavsdk/mavsdk.h>
-#include <mavsdk/plugins/telemetry/telemetry.h>
-#include <mavsdk/plugins/mission_raw/mission_raw.h>
-#include <mavsdk/plugins/geofence/geofence.h>
 #include <mavsdk/plugins/action/action.h>
+#include <mavsdk/plugins/geofence/geofence.h>
 #include <mavsdk/plugins/mavlink_passthrough/mavlink_passthrough.h>
+#include <mavsdk/plugins/mission_raw/mission_raw.h>
+#include <mavsdk/plugins/telemetry/telemetry.h>
 #include <mavsdk/plugins/param/param.h>
+
+#include <chrono>
+#include <cmath>
+#include <iostream>
 #include <memory>
-#include <vector>
 #include <mutex>
+#include <optional>
 #include <string>
-#include <chrono>
-#include <iostream>
 #include <thread>
-#include <optional>
-#include <cmath>
 #include <utility>
+#include <vector>
 
 #include "protos/obc.pb.h"
 #include "utilities/datatypes.hpp"
@@ -39,18 +40,18 @@ class MavlinkClient {
  /*
  * BLOCKING. Continues to try to upload the mission based on the passed through MissionConfig
  * until it is successfully received by the plane.
- * 
+ *
  * Uploading the mission takes two steps:
  * 1. uploading the geofence data (the flight boundary)
  * 2. uploading the waypoints data (this is what actually is called the "mission" in
  * the mavlink terms)
- * 
+ *
  * This function will continue to try and upload these two items until it has successfully
  * uploaded both. Since it is blocking, this should usually be called inside of a separate
  * thread if async behavior is desired. TODO: consider if it would be better to have this
  * function only attempt one mission upload, and have the retrying behavior start from the
  * outside.
- * 
+ *
  * The only way this function fails is if there is no cached mission inside
  * of the state, or if the initial path is empty, which will make it return false. This
  * should never happen due to how the state machine is set up, but it is there just in case.
@@ -71,6 +72,7 @@ class MavlinkClient {
  double yaw_deg();
  double pitch_deg();
  double roll_deg();
+ XYZCoord wind();
  bool isArmed();
  mavsdk::Telemetry::FlightMode flight_mode();
  int32_t curr_waypoint() const;
@@ -90,18 +92,19 @@ class MavlinkClient {
  std::unique_ptr<mavsdk::Param> param;
 
  struct Data {
- double lat_deg {};
- double lng_deg {};
- double altitude_agl_m {};
- double altitude_msl_m {};
- double groundspeed_m_s {};
- double airspeed_m_s {};
- double heading_deg {};
- double yaw_deg {};
- double pitch_deg {};
- double roll_deg {};
- mavsdk::Telemetry::FlightMode flight_mode {};
- bool armed {};
+ double lat_deg{};
+ double lng_deg{};
+ double altitude_agl_m{};
+ double altitude_msl_m{};
+ double groundspeed_m_s{};
+ double airspeed_m_s{};
+ double heading_deg{};
+ double yaw_deg{};
+ double pitch_deg{};
+ double roll_deg{};
+ XYZCoord wind{0, 0, 0};
+ mavsdk::Telemetry::FlightMode flight_mode{};
+ bool armed{};
  } data;
  std::mutex data_mut;
 };