From 6da7e76990306a633e632353c35aafcf95935ee3 Mon Sep 17 00:00:00 2001 From: Andre Kjellstrup Date: Fri, 18 Dec 2015 18:16:11 +1100 Subject: [PATCH] AP_Camera: Camera options for better camera control All on one because they would not pass autotest if split up. --- libraries/AP_Camera/AP_Camera.cpp | 37 +++++++++++++++++++++++++++---- libraries/AP_Camera/AP_Camera.h | 5 ++++- 2 files changed, 37 insertions(+), 5 deletions(-) diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index 0f6c0cbfa5..577deaab98 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -55,10 +55,27 @@ const AP_Param::GroupInfo AP_Camera::var_info[] = { // @Values: 0:Low,1:High // @User: Standard AP_GROUPINFO("RELAY_ON", 5, AP_Camera, _relay_on, 1), - + + // @Param: MIN_INTERVAL + // @DisplayName: Minimum time between photos + // @Description: Postpone shooting if previous picture was taken less than preset time(ms) ago. + // @User: Standard + // @Units: milliseconds + // @Range: 0 10000 + AP_GROUPINFO("MIN_INTERVAL", 6, AP_Camera, _min_interval, 0), + + // @Param: MAX_ROLL + // @DisplayName: Maximum photo roll angle. + // @Description: Postpone shooting if roll is greater than limit. (0=Disable, will shoot regardless of roll). + // @User: Standard + // @Units: Degrees + // @Range: 0 180 + AP_GROUPINFO("MAX_ROLL", 7, AP_Camera, _max_roll, 0), + AP_GROUPEND }; +extern const AP_HAL::HAL& hal; /// Servo operated camera void @@ -228,7 +245,7 @@ void AP_Camera::send_feedback(mavlink_channel_t chan, AP_GPS &gps, const AP_AHRS The caller is responsible for taking the picture based on the return value of this function. The caller is also responsible for logging the details about the photo */ -bool AP_Camera::update_location(const struct Location &loc) +bool AP_Camera::update_location(const struct Location &loc, const AP_AHRS &ahrs) { if (is_zero(_trigg_dist)) { return false; @@ -242,9 +259,21 @@ bool AP_Camera::update_location(const struct Location &loc) // be called without a new GPS fix return false; } + if (get_distance(loc, _last_location) < _trigg_dist) { return false; } - _last_location = loc; - return true; + + if (_max_roll > 0 && labs(ahrs.roll_sensor/100) > _max_roll) { + return false; + } + + uint32_t tnow = AP_HAL::millis(); + if (tnow - _last_photo_time < (unsigned) _min_interval) { + return false; + } else { + _last_location = loc; + _last_photo_time = tnow; + return true; + } } diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index f05ae91ddd..29dda115af 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -60,7 +60,7 @@ public: void set_trigger_distance(uint32_t distance_m) { _trigg_dist.set(distance_m); } // Update location of vehicle and return true if a picture should be taken - bool update_location(const struct Location &loc); + bool update_location(const struct Location &loc, const AP_AHRS &ahrs); static const struct AP_Param::GroupInfo var_info[]; @@ -77,6 +77,9 @@ private: void relay_pic(); // basic relay activation AP_Float _trigg_dist; // distance between trigger points (meters) + AP_Int16 _min_interval; // Minimum time between shots required by camera + AP_Int16 _max_roll; // Maximum acceptable roll angle when trigging camera + uint32_t _last_photo_time; // last time a photo was taken struct Location _last_location; uint16_t _image_index; // number of pictures taken since boot