From 1906e06b6997b0c9ab9b9295f42076fe9070ba5b Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Wed, 13 Jun 2012 21:00:20 +0200 Subject: [PATCH] Added camera trigger functionality --- ArduPlane/ArduPlane.pde | 11 +- ArduPlane/GCS_Mavlink.pde | 14 ++ ArduPlane/Parameters.h | 15 +- ArduPlane/Parameters.pde | 4 + ArduPlane/commands_logic.pde | 11 ++ ArduPlane/config.h | 9 +- libraries/APO/AP_Var_keys.h | 1 + libraries/AP_Camera/AP_Camera.cpp | 176 ++++++++++++++++++++++++ libraries/AP_Camera/AP_Camera.h | 58 ++++++++ libraries/RC_Channel/RC_Channel_aux.cpp | 4 + libraries/RC_Channel/RC_Channel_aux.h | 2 + 11 files changed, 302 insertions(+), 3 deletions(-) create mode 100644 libraries/AP_Camera/AP_Camera.cpp create mode 100644 libraries/AP_Camera/AP_Camera.h diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 52a2bbb6f5..08b3166524 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -47,6 +47,7 @@ version 2.1 of the License, or (at your option) any later version. #include // Filter library #include // Mode Filter from Filter library #include // APM relay +#include // Photo or video camera #include // Configuration @@ -516,7 +517,8 @@ static long nav_pitch; // Waypoint distances //////////////////////////////////////////////////////////////////////////////// // Distance between plane and next waypoint. Meters -static long wp_distance; +// is not static because AP_Camera uses it +long wp_distance; // Distance between previous and next waypoint. Meters static long wp_totalDistance; @@ -640,6 +642,9 @@ static float load; AP_Mount camera_mount(¤t_loc, g_gps, &ahrs); #endif +#if CAMERA == ENABLED +//pinMode(camtrig, OUTPUT); // these are free pins PE3(5), PH3(15), PH6(18), PB4(23), PB5(24), PL1(36), PL3(38), PA6(72), PA7(71), PK0(89), PK1(88), PK2(87), PK3(86), PK4(83), PK5(84), PK6(83), PK7(82) +#endif //////////////////////////////////////////////////////////////////////////////// // Top-level logic @@ -769,6 +774,10 @@ static void medium_loop() camera_mount.update_mount_position(); #endif +#if CAMERA == ENABLED + g.camera.trigger_pic_cleanup(); +#endif + // This is the start of the medium (10 Hz) loop pieces // ----------------------------------------- switch(medium_loopCounter) { diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index eb47838cea..49b515d498 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -2058,6 +2058,20 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } #endif // HIL_MODE +#if CAMERA == ENABLED + case MAVLINK_MSG_ID_DIGICAM_CONFIGURE: + { + g.camera.configure_msg(msg); + break; + } + + case MAVLINK_MSG_ID_DIGICAM_CONTROL: + { + g.camera.control_msg(msg); + break; + } +#endif // CAMERA == ENABLED + #if MOUNT == ENABLED case MAVLINK_MSG_ID_MOUNT_CONFIGURE: { diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 7a71f86e35..d116fc1740 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -17,7 +17,7 @@ public: // The increment will prevent old parameters from being used incorrectly // by newer code. // - static const uint16_t k_format_version = 13; + static const uint16_t k_format_version = 14; // The parameter software_type is set up solely for ground station use // and identifies the software type (eg ArduPilotMega versus ArduCopterMega) @@ -100,6 +100,14 @@ public: k_param_inverted_flight_ch, k_param_min_gndspeed, + + // + // Camera parameters + // +#if CAMERA == ENABLED + k_param_camera, +#endif + // // 170: Radio settings // @@ -326,6 +334,11 @@ public: AP_Int8 flap_2_percent; AP_Int8 flap_2_speed; + // Camera +#if CAMERA == ENABLED + AP_Camera camera; +#endif + // RC channels RC_Channel channel_roll; RC_Channel channel_pitch; diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index c09ae4b490..eda4a70697 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -277,6 +277,10 @@ static const AP_Param::Info var_info[] PROGMEM = { // @User: Advanced GSCALAR(airspeed_use, "ARSPD_USE"), +#if CAMERA == ENABLED + GGROUP(camera, "CAM_", AP_Camera), +#endif + // RC channel //----------- GGROUP(channel_roll, "RC1_", RC_Channel), diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index e9e9de4a20..fcde508e03 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -102,6 +102,17 @@ static void handle_process_do_command() do_repeat_relay(); break; +#if CAMERA == ENABLED + case MAV_CMD_DO_CONTROL_VIDEO: // Control on-board camera capturing. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| + break; + + case MAV_CMD_DO_DIGICAM_CONFIGURE: // Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)| + break; + + case MAV_CMD_DO_DIGICAM_CONTROL: // Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty| + break; +#endif + #if MOUNT == ENABLED // Sets the region of interest (ROI) for a sensor set or the // vehicle itself. This can then be used by the vehicles control diff --git a/ArduPlane/config.h b/ArduPlane/config.h index 5166807f3c..c8456b6237 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -478,11 +478,18 @@ # define ELEVON_CH2_REVERSE DISABLED #endif +////////////////////////////////////////////////////////////////////////////// +// CAMERA TRIGGER AND CONTROL +// +#ifndef CAMERA +# define CAMERA DISABLED +#endif + ////////////////////////////////////////////////////////////////////////////// // MOUNT (ANTENNA OR CAMERA) // #ifndef MOUNT -# define MOUNT DISABLED +# define MOUNT ENABLED #endif #if defined( __AVR_ATmega1280__ ) && CAMERA == ENABLED diff --git a/libraries/APO/AP_Var_keys.h b/libraries/APO/AP_Var_keys.h index 01487c7141..f9c7c6dbb5 100644 --- a/libraries/APO/AP_Var_keys.h +++ b/libraries/APO/AP_Var_keys.h @@ -8,6 +8,7 @@ enum keys { k_cntrl, k_guide, k_sensorCalib, + k_camera, k_nav, k_radioChannelsStart=10, diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp new file mode 100644 index 0000000000..217bc095f8 --- /dev/null +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -0,0 +1,176 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +#include +#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 long wp_distance; +extern AP_Relay relay; + +// ------------------------------ +#define CAM_DEBUG DISABLED + +const AP_Param::GroupInfo AP_Camera::var_info[] PROGMEM = { + AP_GROUPINFO("TRIGG_TYPE", 0, AP_Camera, trigger_type), + AP_GROUPEND +}; + + +void +AP_Camera::servo_pic() // Servo operated camera +{ + 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 + } +} + +void +AP_Camera::relay_pic() // basic relay activation +{ + relay.on(); + keep_cam_trigg_active_cycles = 2; // leave a message that it should be active for two event loop cycles +} + +void +AP_Camera::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. +{ +// 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; +// g.channel_throttle.radio_out = g.throttle_cruise; + } + thr_pic++; +} + +void +AP_Camera::distance_pic() // pictures blurry? use this trigger. Turns off the throttle until closer to waypoint then takes the picture and re-enables the throttle. +{ +// 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 +// g.channel_throttle.radio_out = g.throttle_cruise; + } +} + +void +AP_Camera::NPN_pic() // hacked the circuit to run a transistor? use this trigger to send output. +{ + // 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; + } +} + +// 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; + } + } +} +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); +} + +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); +} + + diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h new file mode 100644 index 0000000000..ab9a9eaf68 --- /dev/null +++ b/libraries/AP_Camera/AP_Camera.h @@ -0,0 +1,58 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/// @file AP_Camera.h +/// @brief Photo or video camera manager, with EEPROM-backed storage of constants. + +#ifndef AP_CAMERA_H +#define AP_CAMERA_H + +#include +#include + +/// @class Camera +/// @brief Object managing a Photo or video camera +class AP_Camera{ + +public: + /// Constructor + /// + AP_Camera() : + trigger_type (0), + picture_time (0), // waypoint trigger variable + thr_pic (0), // timer variable for throttle_pic + camtrig (83), // PK6 chosen as it not near anything so safer for soldering + keep_cam_trigg_active_cycles (0), + wp_distance_min (10) + {} + + // single entry point to take pictures + void trigger_pic(); + + // de-activate the trigger after some delay, but without using a delay() function + void trigger_pic_cleanup(); + + // MAVLink methods + void configure_msg(mavlink_message_t* msg); + void control_msg(mavlink_message_t* msg); + + int picture_time; // waypoint trigger variable + long wp_distance_min; // take picture if distance to WP is smaller than this + + static const struct AP_Param::GroupInfo var_info[]; + +private: + uint8_t keep_cam_trigg_active_cycles; // event loop cycles to keep trigger active + int thr_pic; // timer variable for throttle_pic + int camtrig; // PK6 chosen as it not near anything so safer for soldering + + AP_Int8 trigger_type; // 0=Servo, 1=relay, 2=throttle_off time, 3=throttle_off waypoint 4=transistor + + void servo_pic(); // Servo operated camera + void relay_pic(); // basic relay activation + void 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. + void distance_pic(); // pictures blurry? use this trigger. Turns off the throttle until closer to waypoint then takes the picture and re-enables the throttle. + void NPN_pic(); // hacked the circuit to run a transistor? use this trigger to send output. + +}; + +#endif /* AP_CAMERA_H */ diff --git a/libraries/RC_Channel/RC_Channel_aux.cpp b/libraries/RC_Channel/RC_Channel_aux.cpp index d70672d773..1909f8f413 100644 --- a/libraries/RC_Channel/RC_Channel_aux.cpp +++ b/libraries/RC_Channel/RC_Channel_aux.cpp @@ -159,4 +159,8 @@ void update_aux_servo_function(RC_Channel_aux* rc_5, RC_Channel_aux* rc_6, RC_Ch g_rc_function[RC_Channel_aux::k_mount_roll]->angle_min / 10, g_rc_function[RC_Channel_aux::k_mount_roll]->angle_max / 10); G_RC_AUX(k_mount_open)->set_range(0,100); + G_RC_AUX(k_cam_trigger)->set_range( + g_rc_function[RC_Channel_aux::k_cam_trigger]->angle_min / 10, + g_rc_function[RC_Channel_aux::k_cam_trigger]->angle_max / 10); + G_RC_AUX(k_egg_drop)->set_range(0,100); } diff --git a/libraries/RC_Channel/RC_Channel_aux.h b/libraries/RC_Channel/RC_Channel_aux.h index 879fd041e9..08a49a2d97 100644 --- a/libraries/RC_Channel/RC_Channel_aux.h +++ b/libraries/RC_Channel/RC_Channel_aux.h @@ -42,6 +42,8 @@ public: k_mount_pitch = 7, // mount pitch (tilt) k_mount_roll = 8, // mount roll k_mount_open = 9, // mount open (deploy) / close (retract) + k_cam_trigger = 10, // camera trigger + k_egg_drop = 11, // egg drop k_nr_aux_servo_functions // This must be the last enum value (only add new values _before_ this one) } Aux_servo_function_t;