Added camera trigger functionality

ArduPlane/ArduPlane.pde

@@ -47,6 +47,7 @@ version 2.1 of the License, or (at your option) any later version.
 #include <Filter.h>			// Filter library
 #include <ModeFilter.h>		// Mode Filter from Filter library
 #include <AP_Relay.h>       // APM relay
+#include <AP_Camera.h>		// Photo or video camera
 #include <memcheck.h>
 
 // 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(&current_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) {

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:
 		{

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;

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),

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

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

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,

libraries/AP_Camera/AP_Camera.cpp

@@ -0,0 +1,176 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
+#include <AP_Camera.h>
+#include <AP_Relay.h>
+#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);
+}
+
+

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 <AP_Common.h>
+#include <GCS_MAVLink.h>
+
+/// @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 */

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);
 }

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;