From 851f23fe9c4f3ed2f61ea043abd3ad4963daf2f8 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 26 Jul 2017 12:45:20 +1000 Subject: [PATCH] AP_Camera: camera is responsible for taking distance-based-images and logging --- libraries/AP_Camera/AP_Camera.cpp | 102 +++++++++++++++++++++--------- libraries/AP_Camera/AP_Camera.h | 57 +++++++++++------ 2 files changed, 109 insertions(+), 50 deletions(-) diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index 56247bd5f8..71d53775a9 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -217,14 +217,12 @@ void AP_Camera::configure(float shooting_mode, float shutter_speed, float apertu GCS_MAVLINK::send_to_components(&msg); } -bool AP_Camera::control(float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id) +void AP_Camera::control(float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id) { - bool ret = false; - // take picture if (is_equal(shooting_cmd,1.0f)) { trigger_pic(false); - ret = true; + log_picture(); } mavlink_message_t msg; @@ -244,13 +242,12 @@ bool AP_Camera::control(float session, float zoom_pos, float zoom_step, float fo // send to all components GCS_MAVLINK::send_to_components(&msg); - return ret; } /* Send camera feedback to the GCS */ -void AP_Camera::send_feedback(mavlink_channel_t chan, AP_GPS &gps, const AP_AHRS &ahrs, const Location ¤t_loc) +void AP_Camera::send_feedback(mavlink_channel_t chan) { float altitude, altitude_rel; if (current_loc.flags.relative_alt) { @@ -271,42 +268,44 @@ void AP_Camera::send_feedback(mavlink_channel_t chan, AP_GPS &gps, const AP_AHRS } -/* update location, for triggering by GPS distance moved - This function returns true if a picture should be taken - 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 +/* update; triggers by distance moved */ -bool AP_Camera::update_location(const struct Location &loc, const AP_AHRS &ahrs) +void AP_Camera::update() { - if (is_zero(_trigg_dist)) { - return false; - } - if (_last_location.lat == 0 && _last_location.lng == 0) { - _last_location = loc; - return false; - } - if (_last_location.lat == loc.lat && _last_location.lng == loc.lng) { - // we haven't moved - this can happen as update_location() may - // be called without a new GPS fix - return false; + if (gps.status() < AP_GPS::GPS_OK_FIX_3D) { + return; } - if (get_distance(loc, _last_location) < _trigg_dist) { - return false; + if (is_zero(_trigg_dist)) { + return; + } + if (_last_location.lat == 0 && _last_location.lng == 0) { + _last_location = current_loc; + return; + } + if (_last_location.lat == current_loc.lat && _last_location.lng == current_loc.lng) { + // we haven't moved - this can happen as update() may + // be called without a new GPS fix + return; + } + + if (get_distance(current_loc, _last_location) < _trigg_dist) { + return; } if (_max_roll > 0 && labs(ahrs.roll_sensor/100) > _max_roll) { - return false; + return; } 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; + return; } + + take_picture(); + + _last_location = current_loc; + _last_photo_time = tnow; } /* @@ -398,3 +397,46 @@ failed: } _timer_installed = true; } + +// log_picture - log picture taken and send feedback to GCS +void AP_Camera::log_picture() +{ + DataFlash_Class *df = DataFlash_Class::instance(); + if (df == nullptr) { + return; + } + if (!using_feedback_pin()) { + gcs().send_message(MSG_CAMERA_FEEDBACK); + if (df->should_log(log_camera_bit)) { + df->Log_Write_Camera(ahrs, gps, current_loc); + } + } else { + if (df->should_log(log_camera_bit)) { + df->Log_Write_Trigger(ahrs, gps, current_loc); + } + } +} + +// take_picture - take a picture +void AP_Camera::take_picture() +{ + trigger_pic(true); + log_picture(); +} + +/* + update camera trigger - 50Hz + */ +void AP_Camera::update_trigger() +{ + trigger_pic_cleanup(); + if (check_trigger_pin()) { + gcs().send_message(MSG_CAMERA_FEEDBACK); + DataFlash_Class *df = DataFlash_Class::instance(); + if (df != nullptr) { + if (df->should_log(log_camera_bit)) { + df->Log_Write_Camera(ahrs, gps, current_loc); + } + } + } +} diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index c90eb8b3c9..0c8210aaa3 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -33,43 +33,38 @@ class AP_Camera { public: /// Constructor /// - AP_Camera(AP_Relay *obj_relay) : + AP_Camera(AP_Relay *obj_relay, uint32_t _log_camera_bit, const struct Location &_loc, const AP_GPS &_gps, const AP_AHRS &_ahrs) : _trigger_counter(0), // count of number of cycles shutter has been held open - _image_index(0) + _image_index(0), + log_camera_bit(_log_camera_bit), + current_loc(_loc), + gps(_gps), + ahrs(_ahrs) { AP_Param::setup_object_defaults(this, var_info); _apm_relay = obj_relay; } - // single entry point to take pictures - // set send_mavlink_msg to true to send DO_DIGICAM_CONTROL message to all components - void trigger_pic(bool send_mavlink_msg); - - // de-activate the trigger after some delay, but without using a delay() function - // should be called at 50hz from main program - void trigger_pic_cleanup(); - // MAVLink methods void control_msg(mavlink_message_t* msg); - void send_feedback(mavlink_channel_t chan, AP_GPS &gps, const AP_AHRS &ahrs, const Location ¤t_loc); + void send_feedback(mavlink_channel_t chan); // Command processing void configure(float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time); - // handle camera control. Return true if picture was triggered - bool control(float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id); + // handle camera control + void control(float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id); // set camera trigger distance in a mission 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, const AP_AHRS &ahrs); + void take_picture(); - // check if trigger pin has fired - bool check_trigger_pin(void); + // Update - to be called periodically @at least 10Hz + void update(); + + // update camera trigger - 50Hz + void update_trigger(); - // return true if we are using a feedback pin - bool using_feedback_pin(void) const { return _feedback_pin > 0; } - static const struct AP_Param::GroupInfo var_info[]; private: @@ -105,4 +100,26 @@ private: static volatile bool _camera_triggered; bool _timer_installed:1; uint8_t _last_pin_state; + + void log_picture(); + + uint32_t log_camera_bit; + const struct Location ¤t_loc; + const AP_GPS &gps; + const AP_AHRS &ahrs; + + // single entry point to take pictures + // set send_mavlink_msg to true to send DO_DIGICAM_CONTROL message to all components + void trigger_pic(bool send_mavlink_msg); + + // de-activate the trigger after some delay, but without using a delay() function + // should be called at 50hz from main program + void trigger_pic_cleanup(); + + // check if trigger pin has fired + bool check_trigger_pin(void); + + // return true if we are using a feedback pin + bool using_feedback_pin(void) const { return _feedback_pin > 0; } + };