New Thread for Picture Taking

include/core/mission_state.hpp

@@ -56,6 +56,12 @@ class MissionState {
     void markAirdropAsDropped(AirdropType airdrop);
     std::unordered_set<AirdropType> getDroppedAirdrops();
 
+    void zoneHandler(const std::chrono::milliseconds& interval,
+                        std::shared_ptr<MavlinkClient> mavlinkClient);
+    void initThread(const std::chrono::milliseconds& interval,
+                        std::shared_ptr<MavlinkClient> mavlinkClient);
+    void stopThread();
+
     /*
      * Gets a locking reference to the underlying tick for the given tick subclass T.
      *
@@ -108,6 +114,9 @@ class MissionState {
     bool getMappingIsDone();
     void setMappingIsDone(bool isDone);
 
+
+
+
     MissionParameters mission_params;  // has its own mutex
 
     OBCConfig config;
@@ -136,13 +145,20 @@ class MissionState {
     std::shared_ptr<CVAggregator> cv;
     std::shared_ptr<CameraInterface> camera;
 
+    std::chrono::milliseconds most_recent_picture;
+
+    std::thread captureThread;
+
     std::mutex cv_mut;
     // Represents a single detected target used in pipeline
     std::vector<DetectedTarget> cv_detected_targets;
     // Gives an index into cv_detected_targets, and specifies that that bottle is matched
     // with the detected_target specified by the index
     std::array<size_t, NUM_AIRDROPS> cv_matches;
 
+    bool cameraThreadActive = true;
+
+
     bool mappingIsDone;
 
     void _setTick(Tick* newTick);  // does not acquire the tick_mut

src/core/mission_state.cpp

@@ -15,6 +15,11 @@
 #include "utilities/locks.hpp"
 #include "utilities/logging.hpp"
 #include "utilities/obc_config.hpp"
+#include "pathing/environment.hpp"
+#include "utilities/common.hpp"
+#include "ticks/ids.hpp"
+
+using namespace std::chrono_literals;
 
 MissionState::MissionState(OBCConfig config) : config(config) {}
 
@@ -94,6 +99,59 @@ void MissionState::setAirdropPath(const MissionPath& airdrop_path) {
     this->airdrop_path = airdrop_path;
 }
 
+
+
+void MissionState::zoneHandler(const std::chrono::milliseconds& interval,
+                        std::shared_ptr<MavlinkClient> mavlinkClient){
+    //TODO: find a way to handle stopping the while loop when stopThread hits
+    std::chrono::milliseconds last_photo_time = getUnixTime_ms();
+    while(this->cameraThreadActive){
+        auto [lat_deg, lng_deg] =this->getMav()->latlng_deg();
+        double altitude_agl_m = this->getMav()->altitude_agl_m();
+        GPSCoord current_pos = makeGPSCoord(lat_deg, lng_deg, altitude_agl_m);
+        // Get the CartesianConverter (which is already initialized from mission boundaries)
+        auto converter = this->getCartesianConverter();
+        if (converter) {
+            // Convert GPS to local XYZ coords
+            XYZCoord current_xyz = converter->toXYZ(current_pos);
+            // Get the airdrop boundary polygon
+            Polygon airdrop_boundary = this->mission_params.getAirdropBoundary();
+            // Check if we're inside the airdrop zone
+            bool in_zone = Environment::isPointInPolygon(airdrop_boundary, current_xyz);
+            if(in_zone){
+                auto now = getUnixTime_ms();
+                if ((now - last_photo_time) >= 300ms) {
+                    auto photo = this->getCamera()->takePicture(100ms, this->getMav());
+                    if (this->config.camera.save_images_to_file) {
+                        photo->saveToFile(this->config.camera.save_dir);
+                    }
+
+                    if (photo.has_value()&&((this->getTickID()==TickID::FlySearch)||
+                        (this->getTickID()==TickID::CVLoiter))) {
+                        // Update the last photo time
+                        // Run the pipeline on the photo
+                        this->getCV()->runPipeline(photo.value());
+                    }
+                    last_photo_time = getUnixTime_ms();
+                }
+            }
+        }
+        std::this_thread::sleep_for(interval);
+    }
+}
+void MissionState::initThread(const std::chrono::milliseconds& interval,
+    std::shared_ptr<MavlinkClient> mavlinkClient){
+    this->cameraThreadActive=true;
+    this->captureThread = std::thread([this, interval, mavlinkClient]() {
+        this->zoneHandler(interval, mavlinkClient);
+    });
+
+}
+void MissionState::stopThread(){
+    this->captureThread.join();
+    this->cameraThreadActive = false;
+}
+
 MissionPath MissionState::getAirdropPath() {
     Lock lock(this->airdrop_path_mut);
     return this->airdrop_path;

src/ticks/auto_landing.cpp

@@ -13,5 +13,6 @@ std::chrono::milliseconds AutoLandingTick::getWait() const {
 }
 
 Tick* AutoLandingTick::tick() {
+    this->state->stopThread();
     return nullptr;
 }

src/ticks/fly_search.cpp

@@ -43,6 +43,7 @@ void FlySearchTick::init() {
     LOG_F(INFO, "Total Waypoint #: %zu", this->state->getMav()->totalWaypoints());
 }
 
+
 Tick* FlySearchTick::tick() {
     if (!this->mission_started) {
         this->mission_started = this->state->getMav()->startMission();
@@ -56,32 +57,50 @@ Tick* FlySearchTick::tick() {
         // so we can just return a CVLoiterTick
         return new CVLoiterTick(this->state);
     }
+    auto [lat_deg, lng_deg] = state->getMav()->latlng_deg();
+    double altitude_agl_m = state->getMav()->altitude_agl_m();
+    GPSCoord current_pos = makeGPSCoord(lat_deg, lng_deg, altitude_agl_m);
+    auto converter = state->getCartesianConverter();
 
     // IMPORTANT: currently hardcoded to assume hover search pathing, so it
     // takes photos whenever it gets to a new waypoint (loiter position)
     // if we were doing forward pathing would probably want to make it
     // take photos at an interval but only when over the zone
-    auto curr_waypoint = this->state->getMav()->curr_waypoint();
-
-    if (this->curr_mission_item != curr_waypoint) {
-    LOG_F(INFO, "FlySearch Area reached (%zu, %d)", this->curr_mission_item, curr_waypoint);
-        for (int i = 0; i < this->state->config.pathing.coverage.hover.pictures_per_stop; i++) {
-        auto photo = this->state->getCamera()->takePicture(500ms, this->state->getMav());
-            if (state->config.camera.save_images_to_file) {
-                photo->saveToFile(state->config.camera.save_dir);
-            }
-
-            if (photo.has_value()) {
-                // Update the last photo time
-                this->last_photo_time = getUnixTime_ms();
-                // Run the pipeline on the photo
-                this->state->getCV()->runPipeline(photo.value());
+    // Convert GPS to local XYZ coords
+    if(converter){
+        XYZCoord current_xyz = converter->toXYZ(current_pos);
+        // Get the airdrop boundary polygon
+        Polygon airdrop_boundary = state->mission_params.getAirdropBoundary();
+        // Check if we're inside the airdrop zone
+        bool in_zone = Environment::isPointInPolygon(airdrop_boundary, current_xyz);
+
+        if(in_zone){
+            auto curr_waypoint = this->state->getMav()->curr_waypoint();
+
+            if (this->curr_mission_item != curr_waypoint) {
+                LOG_F(INFO, "FlySearch Area reached (%zu, %d)", this->curr_mission_item, curr_waypoint);
+                /*
+                for (int i = 0; i < this->state->config.pathing.coverage.hover.pictures_per_stop; i++) {
+                    auto photo = this->state->getCamera()->takePicture(500ms, this->state->getMav());
+                    if (state->config.camera.save_images_to_file) {
+                        photo->saveToFile(state->config.camera.save_dir);
+                    }
+
+                    if (photo.has_value()) {
+                        // Update the last photo time
+                        this->last_photo_time = getUnixTime_ms();
+                        // Run the pipeline on the photo
+                        this->state->getCV()->runPipeline(photo.value());
+                    }
+                }
+                */
+                this->curr_mission_item = curr_waypoint;
+
+                return nullptr;
             }
         }
-        this->curr_mission_item = curr_waypoint;
-
-        return nullptr;
     }
+
 
     return nullptr;
 }

src/ticks/fly_waypoints.cpp

@@ -24,6 +24,8 @@ std::chrono::milliseconds FlyWaypointsTick::getWait() const {
 }
 
 Tick* FlyWaypointsTick::tick() {
+
+    /*
     auto [lat_deg, lng_deg] = state->getMav()->latlng_deg();
     double altitude_agl_m = state->getMav()->altitude_agl_m();
     GPSCoord current_pos = makeGPSCoord(lat_deg, lng_deg, altitude_agl_m);
@@ -56,6 +58,7 @@ Tick* FlyWaypointsTick::tick() {
             // Stop camera/CV operations
         }
     }
+    */
 
     // TODO: Eventually implement dynamic avoidance so we dont crash brrr
     bool isMissionFinished = state->getMav()->isMissionFinished();
@@ -66,7 +69,6 @@ Tick* FlyWaypointsTick::tick() {
             state->getMav()->startMission();
             return nullptr;
         }
-
         return next_tick;
     }
 

src/ticks/manual_landing.cpp

@@ -15,6 +15,7 @@ std::chrono::milliseconds ManualLandingTick::getWait() const { return MANUAL_LAN
 
 Tick* ManualLandingTick::tick() {
     if (state->getDroppedAirdrops().size() >= NUM_AIRDROPS) {
+        state->stopThread();
         if (state->getMappingIsDone() == false) {
             // Currently does a two pass at the end
             // Probably should update for next year to optimize it.

src/ticks/takeoff.cpp

@@ -8,6 +8,9 @@
 #include "ticks/mav_upload.hpp"
 #include "ticks/fly_search.hpp"
 
+
+using namespace std::chrono_literals;
+
 TakeoffTick::TakeoffTick(std::shared_ptr<MissionState> state)
     :Tick(state, TickID::Takeoff) {}
 
@@ -18,6 +21,7 @@ std::chrono::milliseconds TakeoffTick::getWait() const {
 Tick* TakeoffTick::tick() {
     if (state->getMav()->flight_mode() == mavsdk::Telemetry::FlightMode::Mission) {
         LOG_F(INFO, "Plane has entered autonomous");
+        this->state->initThread(100ms,this->state->getMav());
 
         // NOTE: keep in sync with active_takeoff tick
         // transitions to flying waypoints tick, such that when the flying waypoints