From c15d7299fa9f96fa1c16bab89a01c1372587e997 Mon Sep 17 00:00:00 2001 From: Rustom Jehangir Date: Wed, 3 Feb 2016 14:56:39 -0800 Subject: [PATCH] Sub: Sub changes to match newest Copter changes --- ArduSub/ArduSub.cpp | 16 ++++++++++++++-- ArduSub/GCS_Mavlink.cpp | 7 ++++--- ArduSub/Log.cpp | 36 +++++++++++++++++++++--------------- ArduSub/Sub.h | 1 + ArduSub/commands_logic.cpp | 21 ++++++++++++++------- 5 files changed, 54 insertions(+), 27 deletions(-) diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index 5fd267d8fd..952c85a131 100644 --- a/ArduSub/ArduSub.cpp +++ b/ArduSub/ArduSub.cpp @@ -127,6 +127,7 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = { SCHED_TASK(gcs_send_deferred, 50, 550), SCHED_TASK(gcs_data_stream_send, 50, 550), SCHED_TASK(update_mount, 50, 75), + SCHED_TASK(update_trigger, 50, 75), SCHED_TASK(ten_hz_logging_loop, 10, 350), SCHED_TASK(fifty_hz_logging_loop, 50, 110), SCHED_TASK(full_rate_logging_loop,400, 100), @@ -343,9 +344,20 @@ void Sub::update_mount() // update camera mount's position camera_mount.update(); #endif +} -#if CAMERA == ENABLED - camera.trigger_pic_cleanup(); + +// update camera trigger +void Sub::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); + } + } #endif } diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 180f98a41d..827ffe48b1 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -1301,13 +1301,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; case MAV_CMD_DO_DIGICAM_CONTROL: - sub.camera.control(packet.param1, + if (sub.camera.control(packet.param1, packet.param2, packet.param3, packet.param4, packet.param5, - packet.param6); - + packet.param6)) { + sub.log_picture(); + } result = MAV_RESULT_ACCEPTED; break; #endif // CAMERA == ENABLED diff --git a/ArduSub/Log.cpp b/ArduSub/Log.cpp index fe0ad64f23..301d2308e4 100644 --- a/ArduSub/Log.cpp +++ b/ArduSub/Log.cpp @@ -30,21 +30,27 @@ bool Sub::print_log_menu(void) if (0 == g.log_bitmask) { cliSerial->printf("none"); }else{ - if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) cliSerial->printf(" ATTITUDE_FAST"); - if (g.log_bitmask & MASK_LOG_ATTITUDE_MED) cliSerial->printf(" ATTITUDE_MED"); - if (g.log_bitmask & MASK_LOG_GPS) cliSerial->printf(" GPS"); - if (g.log_bitmask & MASK_LOG_PM) cliSerial->printf(" PM"); - if (g.log_bitmask & MASK_LOG_CTUN) cliSerial->printf(" CTUN"); - if (g.log_bitmask & MASK_LOG_NTUN) cliSerial->printf(" NTUN"); - if (g.log_bitmask & MASK_LOG_RCIN) cliSerial->printf(" RCIN"); - if (g.log_bitmask & MASK_LOG_IMU) cliSerial->printf(" IMU"); - if (g.log_bitmask & MASK_LOG_CMD) cliSerial->printf(" CMD"); - if (g.log_bitmask & MASK_LOG_CURRENT) cliSerial->printf(" CURRENT"); - if (g.log_bitmask & MASK_LOG_RCOUT) cliSerial->printf(" RCOUT"); - if (g.log_bitmask & MASK_LOG_OPTFLOW) cliSerial->printf(" OPTFLOW"); - if (g.log_bitmask & MASK_LOG_COMPASS) cliSerial->printf(" COMPASS"); - if (g.log_bitmask & MASK_LOG_CAMERA) cliSerial->printf(" CAMERA"); - if (g.log_bitmask & MASK_LOG_PID) cliSerial->printf(" PID"); + // Macro to make the following code a bit easier on the eye. + // Pass it the capitalised name of the log option, as defined + // in defines.h but without the LOG_ prefix. It will check for + // the bit being set and print the name of the log option to suit. +#define PLOG(_s) if (g.log_bitmask & MASK_LOG_ ## _s) cliSerial->printf(" %s", # _s) + PLOG(ATTITUDE_FAST); + PLOG(ATTITUDE_MED); + PLOG(GPS); + PLOG(PM); + PLOG(CTUN); + PLOG(NTUN); + PLOG(RCIN); + PLOG(IMU); + PLOG(CMD); + PLOG(CURRENT); + PLOG(RCOUT); + PLOG(OPTFLOW); + PLOG(COMPASS); + PLOG(CAMERA); + PLOG(PID); +#undef PLOG } cliSerial->println(); diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 3e7d3bd6d4..a2fe6c1e83 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -572,6 +572,7 @@ private: void rc_loop(); void throttle_loop(); void update_mount(); + void update_trigger(); void update_batt_compass(void); void ten_hz_logging_loop(); void fifty_hz_logging_loop(); diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp index b915888141..eb00361442 100644 --- a/ArduSub/commands_logic.cpp +++ b/ArduSub/commands_logic.cpp @@ -873,13 +873,14 @@ void Sub::do_digicam_configure(const AP_Mission::Mission_Command& cmd) void Sub::do_digicam_control(const AP_Mission::Mission_Command& cmd) { #if CAMERA == ENABLED - camera.control(cmd.content.digicam_control.session, + 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(); + cmd.content.digicam_control.cmd_id)) { + log_picture(); + } #endif } @@ -895,10 +896,16 @@ void Sub::do_take_picture() // log_picture - log picture taken and send feedback to GCS void Sub::log_picture() { - gcs_send_message(MSG_CAMERA_FEEDBACK); - if (should_log(MASK_LOG_CAMERA)) { - DataFlash.Log_Write_Camera(ahrs, gps, current_loc); - } + 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); + } + } } // point the camera to a specified angle