diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index 0048b5d693..06dcd54667 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -4,21 +4,21 @@ #include #include <../RC_Channel/RC_Channel_aux.h> -extern RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function -extern int32_t wp_distance; +extern RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function +extern int32_t wp_distance; extern AP_Relay relay; // ------------------------------ #define CAM_DEBUG DISABLED const AP_Param::GroupInfo AP_Camera::var_info[] PROGMEM = { - // @Param: TRIGG_TYPE - // @DisplayName: Camera shutter (trigger) type - // @Description: how to trigger the camera to take a picture - // @Values: 0:Servo,1:relay,2:throttle_off_time,3:throttle_off_waypoint,4:transistor - // @User: Standard - AP_GROUPINFO("TRIGG_TYPE", 0, AP_Camera, trigger_type, 0), - AP_GROUPEND + // @Param: TRIGG_TYPE + // @DisplayName: Camera shutter (trigger) type + // @Description: how to trigger the camera to take a picture + // @Values: 0:Servo,1:relay,2:throttle_off_time,3:throttle_off_waypoint,4:transistor + // @User: Standard + AP_GROUPINFO("TRIGG_TYPE", 0, AP_Camera, trigger_type, 0), + AP_GROUPEND }; @@ -26,19 +26,19 @@ const AP_Param::GroupInfo AP_Camera::var_info[] PROGMEM = { void AP_Camera::servo_pic() { - if (g_rc_function[RC_Channel_aux::k_cam_trigger]) - { - g_rc_function[RC_Channel_aux::k_cam_trigger]->radio_out = g_rc_function[RC_Channel_aux::k_cam_trigger]->radio_max; - keep_cam_trigg_active_cycles = 2; // leave a message that it should be active for two event loop cycles - } + if (g_rc_function[RC_Channel_aux::k_cam_trigger]) + { + g_rc_function[RC_Channel_aux::k_cam_trigger]->radio_out = g_rc_function[RC_Channel_aux::k_cam_trigger]->radio_max; + keep_cam_trigg_active_cycles = 2; // leave a message that it should be active for two event loop cycles + } } /// basic relay activation void AP_Camera::relay_pic() { - relay.on(); - keep_cam_trigg_active_cycles = 2; // leave a message that it should be active for two event loop cycles + relay.on(); + keep_cam_trigg_active_cycles = 2; // leave a message that it should be active for two event loop cycles } /// pictures blurry? use this trigger. Turns off the throttle until for # of cycles of medium loop then takes the picture and re-enables the throttle. @@ -47,12 +47,12 @@ AP_Camera::throttle_pic() { // TODO find a way to do this without using the global parameter g // g.channel_throttle.radio_out = g.throttle_min; - if (thr_pic == 10){ - servo_pic(); // triggering method - thr_pic = 0; + if (thr_pic == 10) { + servo_pic(); // triggering method + thr_pic = 0; // g.channel_throttle.radio_out = g.throttle_cruise; - } - thr_pic++; + } + thr_pic++; } /// pictures blurry? use this trigger. Turns off the throttle until closer to waypoint then takes the picture and re-enables the throttle. @@ -61,129 +61,129 @@ AP_Camera::distance_pic() { // TODO find a way to do this without using the global parameter g // g.channel_throttle.radio_out = g.throttle_min; - if (wp_distance < 3){ - servo_pic(); // triggering method + if (wp_distance < 3) { + servo_pic(); // triggering method // g.channel_throttle.radio_out = g.throttle_cruise; - } + } } /// hacked the circuit to run a transistor? use this trigger to send output. void AP_Camera::NPN_pic() { - // TODO: Assign pin spare pin for output - digitalWrite(camtrig, HIGH); - keep_cam_trigg_active_cycles = 1; // leave a message that it should be active for two event loop cycles + // TODO: Assign pin spare pin for output + digitalWrite(camtrig, HIGH); + keep_cam_trigg_active_cycles = 1; // leave a message that it should be active for two event loop cycles } /// single entry point to take pictures void AP_Camera::trigger_pic() { - switch (trigger_type) - { - case 0: - servo_pic(); // Servo operated camera - break; - case 1: - relay_pic(); // basic relay activation - break; - case 2: - throttle_pic(); // pictures blurry? use this trigger. Turns off the throttle until for # of cycles of medium loop then takes the picture and re-enables the throttle. - break; - case 3: - distance_pic(); // pictures blurry? use this trigger. Turns off the throttle until closer to waypoint then takes the picture and re-enables the throttle. - break; - case 4: - NPN_pic(); // hacked the circuit to run a transistor? use this trigger to send output. - break; - } + switch (trigger_type) + { + case 0: + servo_pic(); // Servo operated camera + break; + case 1: + relay_pic(); // basic relay activation + break; + case 2: + throttle_pic(); // pictures blurry? use this trigger. Turns off the throttle until for # of cycles of medium loop then takes the picture and re-enables the throttle. + break; + case 3: + distance_pic(); // pictures blurry? use this trigger. Turns off the throttle until closer to waypoint then takes the picture and re-enables the throttle. + break; + case 4: + NPN_pic(); // hacked the circuit to run a transistor? use this trigger to send output. + break; + } } /// de-activate the trigger after some delay, but without using a delay() function void AP_Camera::trigger_pic_cleanup() { - if (keep_cam_trigg_active_cycles) - { - keep_cam_trigg_active_cycles --; - } - else - { - switch (trigger_type) - { - case 0: - case 2: - case 3: - G_RC_AUX(k_cam_trigger)->radio_out = g_rc_function[RC_Channel_aux::k_cam_trigger]->radio_min; - break; - case 1: - relay.off(); - break; - case 4: - digitalWrite(camtrig, LOW); - break; - } - } + if (keep_cam_trigg_active_cycles) + { + keep_cam_trigg_active_cycles--; + } + else + { + switch (trigger_type) + { + case 0: + case 2: + case 3: + G_RC_AUX(k_cam_trigger)->radio_out = g_rc_function[RC_Channel_aux::k_cam_trigger]->radio_min; + break; + case 1: + relay.off(); + break; + case 4: + digitalWrite(camtrig, LOW); + break; + } + } } /// decode MavLink that configures camera void AP_Camera::configure_msg(mavlink_message_t* msg) { - __mavlink_digicam_configure_t packet; - mavlink_msg_digicam_configure_decode(msg, &packet); - if (mavlink_check_target(packet.target_system, packet.target_component)) { - // not for us - return; - } - // TODO do something with these values - /* - packet.aperture - packet.command_id - packet.engine_cut_off - packet.exposure_type - packet.extra_param - packet.extra_value - packet.iso - packet.mode - packet.shutter_speed - */ - // echo the message to the ArduCam OSD camera control board - // for more info see: http://code.google.com/p/arducam-osd/ - // TODO is it connected to MAVLINK_COMM_3 ? - mavlink_msg_digicam_configure_send(MAVLINK_COMM_3, packet.target_system, packet.target_component, packet.mode, packet.shutter_speed, packet.aperture, packet.iso, packet.exposure_type, packet.command_id, packet.engine_cut_off, packet.extra_param, packet.extra_value); + __mavlink_digicam_configure_t packet; + mavlink_msg_digicam_configure_decode(msg, &packet); + if (mavlink_check_target(packet.target_system, packet.target_component)) { + // not for us + return; + } + // TODO do something with these values + /* + * packet.aperture + * packet.command_id + * packet.engine_cut_off + * packet.exposure_type + * packet.extra_param + * packet.extra_value + * packet.iso + * packet.mode + * packet.shutter_speed + */ + // echo the message to the ArduCam OSD camera control board + // for more info see: http://code.google.com/p/arducam-osd/ + // TODO is it connected to MAVLINK_COMM_3 ? + mavlink_msg_digicam_configure_send(MAVLINK_COMM_3, packet.target_system, packet.target_component, packet.mode, packet.shutter_speed, packet.aperture, packet.iso, packet.exposure_type, packet.command_id, packet.engine_cut_off, packet.extra_param, packet.extra_value); } /// decode MavLink that controls camera void AP_Camera::control_msg(mavlink_message_t* msg) { - __mavlink_digicam_control_t packet; - mavlink_msg_digicam_control_decode(msg, &packet); - if (mavlink_check_target(packet.target_system, packet.target_component)) { - // not for us - return; - } - // TODO do something with these values - /* - packet.command_id - packet.extra_param - packet.extra_value - packet.focus_lock - packet.session - packet.shot - packet.zoom_pos - packet.zoom_step - */ - if (packet.shot) - { - trigger_pic(); - } - // echo the message to the ArduCam OSD camera control board - // for more info see: http://code.google.com/p/arducam-osd/ - // TODO is it connected to MAVLINK_COMM_3 ? - mavlink_msg_digicam_control_send(MAVLINK_COMM_3, packet.target_system, packet.target_component, packet.session, packet.zoom_pos, packet.zoom_step, packet.focus_lock, packet.shot, packet.command_id, packet.extra_param, packet.extra_value); + __mavlink_digicam_control_t packet; + mavlink_msg_digicam_control_decode(msg, &packet); + if (mavlink_check_target(packet.target_system, packet.target_component)) { + // not for us + return; + } + // TODO do something with these values + /* + * packet.command_id + * packet.extra_param + * packet.extra_value + * packet.focus_lock + * packet.session + * packet.shot + * packet.zoom_pos + * packet.zoom_step + */ + if (packet.shot) + { + trigger_pic(); + } + // echo the message to the ArduCam OSD camera control board + // for more info see: http://code.google.com/p/arducam-osd/ + // TODO is it connected to MAVLINK_COMM_3 ? + mavlink_msg_digicam_control_send(MAVLINK_COMM_3, packet.target_system, packet.target_component, packet.session, packet.zoom_pos, packet.zoom_step, packet.focus_lock, packet.shot, packet.command_id, packet.extra_param, packet.extra_value); }