From cdf9ebcb39f9324a7470951599b26a58bdbef8c1 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 26 Jul 2017 13:33:45 +1000 Subject: [PATCH] Copter: camera is responsible for taking distance-based-images and logging --- ArduCopter/ArduCopter.cpp | 16 ++-------------- ArduCopter/Copter.cpp | 2 +- ArduCopter/Copter.h | 2 -- ArduCopter/GCS_Mavlink.cpp | 17 +++++++---------- ArduCopter/commands_logic.cpp | 36 ++++++----------------------------- ArduCopter/switches.cpp | 2 +- 6 files changed, 17 insertions(+), 58 deletions(-) diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index 95c88d8c5f..59e45a36e4 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -338,13 +338,7 @@ void Copter::update_mount() void Copter::update_trigger(void) { #if CAMERA == ENABLED - camera.trigger_pic_cleanup(); - if (camera.check_trigger_pin()) { - gcs().send_message(MSG_CAMERA_FEEDBACK); - if (should_log(MASK_LOG_CAMERA)) { - DataFlash.Log_Write_Camera(ahrs, gps, current_loc); - } - } + camera.update_trigger(); #endif } @@ -533,15 +527,9 @@ void Copter::update_GPS(void) // set system time if necessary set_system_time_from_GPS(); - // checks to initialise home and take location based pictures - if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) { - #if CAMERA == ENABLED - if (camera.update_location(current_loc, copter.ahrs) == true) { - do_take_picture(); - } + camera.update(); #endif - } } } diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index cbcf1ce325..90909334a5 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -71,7 +71,7 @@ Copter::Copter(void) : auto_trim_counter(0), ServoRelayEvents(relay), #if CAMERA == ENABLED - camera(&relay), + camera(&relay, MASK_LOG_CAMERA, current_loc, gps, ahrs), #endif #if MOUNT == ENABLED camera_mount(ahrs, current_loc), diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index b0b8e6cced..cb021e4b98 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -779,8 +779,6 @@ private: bool verify_wait_delay(); bool verify_within_distance(); bool verify_yaw(); - void do_take_picture(); - void log_picture(); MAV_RESULT mavlink_compassmot(mavlink_channel_t chan); void delay(uint32_t ms); bool acro_init(bool ignore_checks); diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 9af0353598..f7879c2018 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -435,7 +435,7 @@ bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id) case MSG_CAMERA_FEEDBACK: #if CAMERA == ENABLED CHECK_PAYLOAD_SIZE(CAMERA_FEEDBACK); - copter.camera.send_feedback(chan, copter.gps, copter.ahrs, copter.current_loc); + copter.camera.send_feedback(chan); #endif break; @@ -1076,14 +1076,12 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) break; case MAV_CMD_DO_DIGICAM_CONTROL: - if (copter.camera.control(packet.param1, - packet.param2, - packet.param3, - packet.param4, - packet.param5, - packet.param6)) { - copter.log_picture(); - } + copter.camera.control(packet.param1, + packet.param2, + packet.param3, + packet.param4, + packet.param5, + packet.param6); result = MAV_RESULT_ACCEPTED; break; @@ -1705,7 +1703,6 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) //deprecated. Use MAV_CMD_DO_DIGICAM_CONTROL case MAVLINK_MSG_ID_DIGICAM_CONTROL: copter.camera.control_msg(msg); - copter.log_picture(); break; #endif // CAMERA == ENABLED diff --git a/ArduCopter/commands_logic.cpp b/ArduCopter/commands_logic.cpp index f25d356f5e..073b0f58be 100644 --- a/ArduCopter/commands_logic.cpp +++ b/ArduCopter/commands_logic.cpp @@ -1134,36 +1134,12 @@ void Copter::do_digicam_configure(const AP_Mission::Mission_Command& cmd) // do_digicam_control Send Digicam Control message with the camera library void Copter::do_digicam_control(const AP_Mission::Mission_Command& cmd) { - if (camera.control(cmd.content.digicam_control.session, - cmd.content.digicam_control.zoom_pos, - cmd.content.digicam_control.zoom_step, - cmd.content.digicam_control.focus_lock, - cmd.content.digicam_control.shooting_cmd, - cmd.content.digicam_control.cmd_id)) { - log_picture(); - } -} - -// do_take_picture - take a picture with the camera library -void Copter::do_take_picture() -{ - camera.trigger_pic(true); - log_picture(); -} - -// log_picture - log picture taken and send feedback to GCS -void Copter::log_picture() -{ - if (!camera.using_feedback_pin()) { - gcs().send_message(MSG_CAMERA_FEEDBACK); - if (should_log(MASK_LOG_CAMERA)) { - DataFlash.Log_Write_Camera(ahrs, gps, current_loc); - } - } else { - if (should_log(MASK_LOG_CAMERA)) { - DataFlash.Log_Write_Trigger(ahrs, gps, current_loc); - } - } + camera.control(cmd.content.digicam_control.session, + cmd.content.digicam_control.zoom_pos, + cmd.content.digicam_control.zoom_step, + cmd.content.digicam_control.focus_lock, + cmd.content.digicam_control.shooting_cmd, + cmd.content.digicam_control.cmd_id); } #endif diff --git a/ArduCopter/switches.cpp b/ArduCopter/switches.cpp index b7f9e92828..c423d008e5 100644 --- a/ArduCopter/switches.cpp +++ b/ArduCopter/switches.cpp @@ -298,7 +298,7 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) case AUXSW_CAMERA_TRIGGER: #if CAMERA == ENABLED if (ch_flag == AUX_SWITCH_HIGH) { - do_take_picture(); + camera.take_picture(); } #endif break;