From 22be1bbe8a3b4eb8c8c69447af7224c829959501 Mon Sep 17 00:00:00 2001 From: Vidyuth2 Date: Thu, 5 Mar 2026 15:42:00 -0800 Subject: [PATCH 1/5] take pictures over zone in fly-waypoints --- src/ticks/fly_search.cpp | 53 ++++++++++++++++++++++++------------- src/ticks/fly_waypoints.cpp | 1 - 2 files changed, 35 insertions(+), 19 deletions(-) diff --git a/src/ticks/fly_search.cpp b/src/ticks/fly_search.cpp index 685f0bff..79101bfe 100644 --- a/src/ticks/fly_search.cpp +++ b/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,48 @@ 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; } diff --git a/src/ticks/fly_waypoints.cpp b/src/ticks/fly_waypoints.cpp index dc66b1a2..34a82a94 100644 --- a/src/ticks/fly_waypoints.cpp +++ b/src/ticks/fly_waypoints.cpp @@ -66,7 +66,6 @@ Tick* FlyWaypointsTick::tick() { state->getMav()->startMission(); return nullptr; } - return next_tick; } From c0ca7c9488fd35e104e248d2e5c3c2e3719b1fae Mon Sep 17 00:00:00 2001 From: Vidyuth2 Date: Tue, 28 Apr 2026 21:07:43 +0000 Subject: [PATCH 2/5] added camera thread --- include/core/mission_state.hpp | 12 ++++++++ src/core/mission_state.cpp | 53 ++++++++++++++++++++++++++++++++++ src/ticks/auto_landing.cpp | 1 + src/ticks/takeoff.cpp | 4 +++ 4 files changed, 70 insertions(+) diff --git a/include/core/mission_state.hpp b/include/core/mission_state.hpp index f97de9b8..fa7affa9 100644 --- a/include/core/mission_state.hpp +++ b/include/core/mission_state.hpp @@ -56,6 +56,12 @@ class MissionState { void markAirdropAsDropped(AirdropType airdrop); std::unordered_set getDroppedAirdrops(); + void zoneHandler(const std::chrono::milliseconds& interval, + std::shared_ptr mavlinkClient); + void initThread(const std::chrono::milliseconds& interval, + std::shared_ptr mavlinkClient); + void stopThread(); + /* * Gets a locking reference to the underlying tick for the given tick subclass T. * @@ -108,6 +114,7 @@ class MissionState { bool getMappingIsDone(); void setMappingIsDone(bool isDone); + MissionParameters mission_params; // has its own mutex OBCConfig config; @@ -136,6 +143,8 @@ class MissionState { std::shared_ptr cv; std::shared_ptr camera; + std::thread captureThread; + std::mutex cv_mut; // Represents a single detected target used in pipeline std::vector cv_detected_targets; @@ -143,6 +152,9 @@ class MissionState { // with the detected_target specified by the index std::array cv_matches; + bool cameraThreadActive = true; + + bool mappingIsDone; void _setTick(Tick* newTick); // does not acquire the tick_mut diff --git a/src/core/mission_state.cpp b/src/core/mission_state.cpp index bde2b23e..b22a23a5 100644 --- a/src/core/mission_state.cpp +++ b/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,54 @@ void MissionState::setAirdropPath(const MissionPath& airdrop_path) { this->airdrop_path = airdrop_path; } +void MissionState::zoneHandler(const std::chrono::milliseconds& interval, + std::shared_ptr 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(); + } + } + } + } +} +void MissionState::initThread(const std::chrono::milliseconds& interval, + std::shared_ptr mavlinkClient){ + this->cameraThreadActive=true; + this->captureThread = std::thread(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; diff --git a/src/ticks/auto_landing.cpp b/src/ticks/auto_landing.cpp index be96bbd1..6bc9414c 100644 --- a/src/ticks/auto_landing.cpp +++ b/src/ticks/auto_landing.cpp @@ -13,5 +13,6 @@ std::chrono::milliseconds AutoLandingTick::getWait() const { } Tick* AutoLandingTick::tick() { + this->state->stopThread(); return nullptr; } diff --git a/src/ticks/takeoff.cpp b/src/ticks/takeoff.cpp index 4cfe0560..a729851a 100644 --- a/src/ticks/takeoff.cpp +++ b/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 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 From 0b0554226b3b843b88e74d835c330d982ce88b2a Mon Sep 17 00:00:00 2001 From: Vidyuth2 Date: Thu, 14 May 2026 23:51:02 +0000 Subject: [PATCH 3/5] adding initial changes to take pictures whenever we're in search zone --- include/core/mission_state.hpp | 4 ++++ src/core/mission_state.cpp | 9 +++++++-- src/ticks/fly_search.cpp | 4 +++- src/ticks/fly_waypoints.cpp | 3 +++ src/ticks/manual_landing.cpp | 1 + 5 files changed, 18 insertions(+), 3 deletions(-) diff --git a/include/core/mission_state.hpp b/include/core/mission_state.hpp index fa7affa9..3cfafeb7 100644 --- a/include/core/mission_state.hpp +++ b/include/core/mission_state.hpp @@ -115,6 +115,8 @@ class MissionState { void setMappingIsDone(bool isDone); + + MissionParameters mission_params; // has its own mutex OBCConfig config; @@ -143,6 +145,8 @@ class MissionState { std::shared_ptr cv; std::shared_ptr camera; + std::chrono::milliseconds most_recent_picture; + std::thread captureThread; std::mutex cv_mut; diff --git a/src/core/mission_state.cpp b/src/core/mission_state.cpp index b22a23a5..475a3203 100644 --- a/src/core/mission_state.cpp +++ b/src/core/mission_state.cpp @@ -99,6 +99,8 @@ void MissionState::setAirdropPath(const MissionPath& airdrop_path) { this->airdrop_path = airdrop_path; } + + void MissionState::zoneHandler(const std::chrono::milliseconds& interval, std::shared_ptr mavlinkClient){ //TODO: find a way to handle stopping the while loop when stopThread hits @@ -134,13 +136,16 @@ void MissionState::zoneHandler(const std::chrono::milliseconds& interval, } } } + std::this_thread::sleep_for(interval); } } void MissionState::initThread(const std::chrono::milliseconds& interval, std::shared_ptr mavlinkClient){ this->cameraThreadActive=true; - this->captureThread = std::thread(zoneHandler, interval, - mavlinkClient); + this->captureThread = std::thread([this, interval, mavlinkClient]() { + this->zoneHandler(interval, mavlinkClient); + }); + } void MissionState::stopThread(){ this->captureThread.join(); diff --git a/src/ticks/fly_search.cpp b/src/ticks/fly_search.cpp index 79101bfe..b6121e01 100644 --- a/src/ticks/fly_search.cpp +++ b/src/ticks/fly_search.cpp @@ -79,8 +79,9 @@ Tick* FlySearchTick::tick() { 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()); + 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); } @@ -92,6 +93,7 @@ Tick* FlySearchTick::tick() { this->state->getCV()->runPipeline(photo.value()); } } + */ this->curr_mission_item = curr_waypoint; return nullptr; diff --git a/src/ticks/fly_waypoints.cpp b/src/ticks/fly_waypoints.cpp index 34a82a94..b4de4770 100644 --- a/src/ticks/fly_waypoints.cpp +++ b/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(); diff --git a/src/ticks/manual_landing.cpp b/src/ticks/manual_landing.cpp index 110a3aa0..b8a48766 100644 --- a/src/ticks/manual_landing.cpp +++ b/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. From 1f8461345b73982b2bb54b85bf67730040b085dd Mon Sep 17 00:00:00 2001 From: Vidyuth2 Date: Fri, 15 May 2026 00:22:28 +0000 Subject: [PATCH 4/5] fix lint --- src/core/mission_state.cpp | 21 ++++++++++----------- src/ticks/fly_search.cpp | 9 ++++----- src/ticks/fly_waypoints.cpp | 1 - src/ticks/takeoff.cpp | 3 +-- 4 files changed, 15 insertions(+), 19 deletions(-) diff --git a/src/core/mission_state.cpp b/src/core/mission_state.cpp index 475a3203..0a8feee4 100644 --- a/src/core/mission_state.cpp +++ b/src/core/mission_state.cpp @@ -102,11 +102,11 @@ void MissionState::setAirdropPath(const MissionPath& airdrop_path) { void MissionState::zoneHandler(const std::chrono::milliseconds& interval, - std::shared_ptr mavlinkClient){ - //TODO: find a way to handle stopping the while loop when stopThread hits + std::shared_ptr 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(); + 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) @@ -118,7 +118,7 @@ void MissionState::zoneHandler(const std::chrono::milliseconds& interval, 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){ + if (in_zone) { auto now = getUnixTime_ms(); if ((now - last_photo_time) >= 300ms) { auto photo = this->getCamera()->takePicture(100ms, this->getMav()); @@ -126,8 +126,8 @@ void MissionState::zoneHandler(const std::chrono::milliseconds& interval, photo->saveToFile(this->config.camera.save_dir); } - if (photo.has_value()&&((this->getTickID()==TickID::FlySearch)|| - (this->getTickID()==TickID::CVLoiter))) { + 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()); @@ -140,14 +140,13 @@ void MissionState::zoneHandler(const std::chrono::milliseconds& interval, } } void MissionState::initThread(const std::chrono::milliseconds& interval, - std::shared_ptr mavlinkClient){ - this->cameraThreadActive=true; + std::shared_ptr mavlinkClient) { + this->cameraThreadActive = true; this->captureThread = std::thread([this, interval, mavlinkClient]() { this->zoneHandler(interval, mavlinkClient); }); - } -void MissionState::stopThread(){ +void MissionState::stopThread() { this->captureThread.join(); this->cameraThreadActive = false; } diff --git a/src/ticks/fly_search.cpp b/src/ticks/fly_search.cpp index b6121e01..6160a6de 100644 --- a/src/ticks/fly_search.cpp +++ b/src/ticks/fly_search.cpp @@ -67,18 +67,18 @@ Tick* FlySearchTick::tick() { // if we were doing forward pathing would probably want to make it // take photos at an interval but only when over the zone // Convert GPS to local XYZ coords - if(converter){ + 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){ + 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); + 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()); @@ -100,7 +100,6 @@ Tick* FlySearchTick::tick() { } } } - return nullptr; } diff --git a/src/ticks/fly_waypoints.cpp b/src/ticks/fly_waypoints.cpp index b4de4770..93fb9b5e 100644 --- a/src/ticks/fly_waypoints.cpp +++ b/src/ticks/fly_waypoints.cpp @@ -24,7 +24,6 @@ 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(); diff --git a/src/ticks/takeoff.cpp b/src/ticks/takeoff.cpp index a729851a..37c045b0 100644 --- a/src/ticks/takeoff.cpp +++ b/src/ticks/takeoff.cpp @@ -21,8 +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()); - + 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 // tick is done it transitions to uploading the coverage path From b248aed85c7f6b7fbb4a1f250ad9cca643111c03 Mon Sep 17 00:00:00 2001 From: Vidyuth2 Date: Tue, 26 May 2026 21:01:07 +0000 Subject: [PATCH 5/5] fix pull request --- configs/dev.json | 5 +++ include/camera/picamera.hpp | 2 + include/core/mission_state.hpp | 7 ++-- include/utilities/obc_config.hpp | 2 + src/core/mission_state.cpp | 65 ++++++++++++++++++++------------ src/ticks/auto_landing.cpp | 2 +- src/ticks/fly_search.cpp | 8 ++-- src/ticks/manual_landing.cpp | 2 +- src/ticks/takeoff.cpp | 2 +- 9 files changed, 59 insertions(+), 36 deletions(-) diff --git a/configs/dev.json b/configs/dev.json index e55ddd1f..ea7b4db2 100644 --- a/configs/dev.json +++ b/configs/dev.json @@ -30,6 +30,10 @@ "allowed_to_skip_waypoints": false, "generate_deviations": false }, + "camera timing":{ + "timeout":100, + "new_photo_time":300 + }, "coverage": { "altitude_m": 30.0, "camera_vision_m": 20.0, @@ -62,6 +66,7 @@ "_comment": "See CameraConfig struct in datatypes.hpp for detailed explanations", "type": "mock", "save_dir": "/workspaces/obcpp/images/mock", + "photo_delay":300, "save_images_to_file": true, "mock": { "not_stolen_port": 6060, diff --git a/include/camera/picamera.hpp b/include/camera/picamera.hpp index 5e31877d..20b5dcf1 100644 --- a/include/camera/picamera.hpp +++ b/include/camera/picamera.hpp @@ -56,6 +56,8 @@ class PiCamera : public CameraInterface { std::shared_ptr mavlinkClient) override; void startStreaming() override; + bool getIsTakingPictures(); + private: /** * Takes an image and sleeps for the specified interval before diff --git a/include/core/mission_state.hpp b/include/core/mission_state.hpp index 3cfafeb7..3298a1f1 100644 --- a/include/core/mission_state.hpp +++ b/include/core/mission_state.hpp @@ -58,9 +58,9 @@ class MissionState { void zoneHandler(const std::chrono::milliseconds& interval, std::shared_ptr mavlinkClient); - void initThread(const std::chrono::milliseconds& interval, + void initCameraThread(const std::chrono::milliseconds& interval, std::shared_ptr mavlinkClient); - void stopThread(); + void stopCameraThread(); /* * Gets a locking reference to the underlying tick for the given tick subclass T. @@ -145,9 +145,8 @@ class MissionState { std::shared_ptr cv; std::shared_ptr camera; - std::chrono::milliseconds most_recent_picture; - std::thread captureThread; + std::size_t curr_mission_item; std::mutex cv_mut; // Represents a single detected target used in pipeline diff --git a/include/utilities/obc_config.hpp b/include/utilities/obc_config.hpp index 186555a1..b7e0a022 100644 --- a/include/utilities/obc_config.hpp +++ b/include/utilities/obc_config.hpp @@ -117,6 +117,8 @@ struct CameraConfig { std::string type; // directory to save images to std::string save_dir; + // time to wait before taking another picture + std::chrono::milliseconds photo_delay; // whether or not to save to save_dir bool save_images_to_file; struct { diff --git a/src/core/mission_state.cpp b/src/core/mission_state.cpp index 0a8feee4..1108b3b4 100644 --- a/src/core/mission_state.cpp +++ b/src/core/mission_state.cpp @@ -103,50 +103,65 @@ void MissionState::setAirdropPath(const MissionPath& airdrop_path) { void MissionState::zoneHandler(const std::chrono::milliseconds& interval, std::shared_ptr 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()); - } + if (!converter) { + std::this_thread::sleep_for(interval); + continue; + } + XYZCoord current_xyz = converter->toXYZ(current_pos); + Polygon airdrop_boundary = this->mission_params.getAirdropBoundary(); + bool in_zone = Environment::isPointInPolygon(airdrop_boundary, current_xyz); + if (!in_zone) { + std::this_thread::sleep_for(interval); + continue; + } + auto curr_waypoint = this->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->config.pathing.coverage.hover.pictures_per_stop; i++) { + auto photo = this->getCamera()->takePicture(500ms, this->getMav()); + if (this->config.camera.save_images_to_file) { + photo->saveToFile(this->config.camera.save_dir); + } + + if (photo.has_value()) { last_photo_time = getUnixTime_ms(); + this->getCV()->runPipeline(photo.value()); } } + this->curr_mission_item = curr_waypoint; + } + auto now = getUnixTime_ms(); + if ((now - last_photo_time) >= std::chrono::milliseconds(this->config.camera.photo_delay)) { + 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))) { + this->getCV()->runPipeline(photo.value()); + } + last_photo_time = getUnixTime_ms(); } std::this_thread::sleep_for(interval); } } -void MissionState::initThread(const std::chrono::milliseconds& interval, +void MissionState::initCameraThread(const std::chrono::milliseconds& interval, std::shared_ptr mavlinkClient) { this->cameraThreadActive = true; this->captureThread = std::thread([this, interval, mavlinkClient]() { this->zoneHandler(interval, mavlinkClient); }); + this->curr_mission_item = 1; } -void MissionState::stopThread() { +void MissionState::stopCameraThread() { this->captureThread.join(); this->cameraThreadActive = false; } diff --git a/src/ticks/auto_landing.cpp b/src/ticks/auto_landing.cpp index 6bc9414c..60722471 100644 --- a/src/ticks/auto_landing.cpp +++ b/src/ticks/auto_landing.cpp @@ -13,6 +13,6 @@ std::chrono::milliseconds AutoLandingTick::getWait() const { } Tick* AutoLandingTick::tick() { - this->state->stopThread(); + this->state->stopCameraThread(); return nullptr; } diff --git a/src/ticks/fly_search.cpp b/src/ticks/fly_search.cpp index 6160a6de..04d90e51 100644 --- a/src/ticks/fly_search.cpp +++ b/src/ticks/fly_search.cpp @@ -57,6 +57,7 @@ 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); @@ -75,11 +76,10 @@ Tick* FlySearchTick::tick() { 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) { @@ -93,13 +93,13 @@ Tick* FlySearchTick::tick() { this->state->getCV()->runPipeline(photo.value()); } } - */ + this->curr_mission_item = curr_waypoint; return nullptr; } } } - + */ return nullptr; } diff --git a/src/ticks/manual_landing.cpp b/src/ticks/manual_landing.cpp index b8a48766..a45fb182 100644 --- a/src/ticks/manual_landing.cpp +++ b/src/ticks/manual_landing.cpp @@ -15,7 +15,7 @@ std::chrono::milliseconds ManualLandingTick::getWait() const { return MANUAL_LAN Tick* ManualLandingTick::tick() { if (state->getDroppedAirdrops().size() >= NUM_AIRDROPS) { - state->stopThread(); + state->stopCameraThread(); if (state->getMappingIsDone() == false) { // Currently does a two pass at the end // Probably should update for next year to optimize it. diff --git a/src/ticks/takeoff.cpp b/src/ticks/takeoff.cpp index 37c045b0..cf484f63 100644 --- a/src/ticks/takeoff.cpp +++ b/src/ticks/takeoff.cpp @@ -21,7 +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()); + this->state->initCameraThread(100ms, this->state->getMav()); // NOTE: keep in sync with active_takeoff tick // transitions to flying waypoints tick, such that when the flying waypoints // tick is done it transitions to uploading the coverage path