Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Camera: camera is responsible for taking distance-based-images and logging

This commit is contained in:
Peter Barker 2017-07-26 12:45:20 +10:00 committed by Francisco Ferreira
parent 7f59279670
commit 84ef63edc7
2 changed files with 109 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

102
libraries/AP_Camera/AP_Camera.cpp
View File

@ -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 &current_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);
}
}
}
}

57
libraries/AP_Camera/AP_Camera.h
View File

@ -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 &current_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 &current_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; }
};