Merge the changes from APM_Camera branch into ArduCopter

Conflicts:

	ArduCopter/Camera.pde
	ArduCopter/Parameters.pde

ArduBoat/ArduBoat.pde

@@ -16,6 +16,10 @@
 #include <APO.h>
 #include <AP_AnalogSource.h>
 #include <AP_InertialSensor.h>
+#include <AP_Camera.h>
+#include <AP_Mount.h>
+#include <AP_Relay.h>
+#include <RC_Channel.h>
 
 // Vehicle Configuration
 //#include "BoatGeneric.h"

ArduCopter/ArduCopter.pde

@@ -96,6 +96,8 @@ http://code.google.com/p/ardupilot-mega/downloads/list
 #include <AverageFilter.h>	// Mode Filter from Filter library
 #include <AP_LeadFilter.h>	// GPS Lead filter
 #include <AP_Relay.h>		// APM relay
+#include <AP_Camera.h>		// Photo or video camera
+#include <AP_Mount.h>		// Camera/Antenna mount
 #include <memcheck.h>
 
 // Configuration
@@ -345,6 +347,8 @@ static const char* flight_mode_strings[] = {
 			6 	User assignable
 			7	trainer switch - sets throttle nominal (toggle switch), sets accels to Level (hold > 1 second)
 			8	TBD
+		Each Aux channel can be configured to have any of the available auxiliary functions assigned to it.
+		See libraries/RC_Channel/RC_Channel_aux.h for more information
 */
 
 //Documentation of GLobals:
@@ -716,7 +720,8 @@ static int32_t	home_to_copter_bearing;
 // distance between plane and home in cm
 static int32_t	home_distance;
 // distance between plane and next_WP in cm
-static int32_t	wp_distance;
+// is not static because AP_Camera uses it
+int32_t	wp_distance;
 
 ////////////////////////////////////////////////////////////////////////////////
 // 3D Location vectors
@@ -928,6 +933,18 @@ AP_Relay relay;
 #endif
 
 
+// Camera/Antenna mount tracking and stabilisation stuff
+// --------------------------------------
+#if MOUNT == ENABLED
+// current_loc uses the baro/gps soloution for altitude rather than gps only.
+// mabe one could use current_loc for lat/lon too and eliminate g_gps alltogether?
+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
 ////////////////////////////////////////////////////////////////////////////////
@@ -1215,6 +1232,13 @@ static void fifty_hz_loop()
 		gcs_send_message(MSG_RADIO_OUT);
 	#endif
 
+#if MOUNT == ENABLED
+	camera_mount.update_mount_position();
+#endif
+
+#if CAMERA == ENABLED
+	g.camera.trigger_pic_cleanup();
+#endif
 
 	# if HIL_MODE == HIL_MODE_DISABLED
 		if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST && motors.armed())
@@ -1224,9 +1248,6 @@ static void fifty_hz_loop()
 			Log_Write_Raw();
 	#endif
 
-
-	camera_stabilization();
-
 	// kick the GCS to process uplink data
 	gcs_update();
     gcs_data_stream_send();
@@ -1265,6 +1286,7 @@ static void slow_loop()
 			// -------------------------------
 			read_control_switch();
 
+			update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8);
 			// agmatthews - USERHOOKS
 			#ifdef USERHOOK_SLOWLOOP
 			   USERHOOK_SLOWLOOP

ArduCopter/Camera.pde

@@ -1,64 +0,0 @@
-/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
-
-//#if CAMERA_STABILIZER == ENABLED
-
-static void init_camera()
-{
-	APM_RC.enable_out(CH_CAM_PITCH);
-	APM_RC.enable_out(CH_CAM_ROLL);
-
-	// ch 6 high(right) is down.
-	g.rc_camera_pitch.set_angle(4500);
-	g.rc_camera_roll.set_angle(4500);
-
-	g.rc_camera_roll.set_type(RC_CHANNEL_ANGLE_RAW);
-	g.rc_camera_pitch.set_type(RC_CHANNEL_ANGLE_RAW);
-}
-
-static void
-camera_stabilization()
-{
-        int32_t p_sensor_value = g.camera_pitch_continuous ? (ahrs.get_gyro().y * 100) : ahrs.pitch_sensor;
-        int32_t r_sensor_value = g.camera_roll_continuous ? (ahrs.get_gyro().x * 100) : ahrs.roll_sensor;
-
-	// PITCH
-	// -----
-	// Allow user to control camera pitch with channel 6 (mixed with pitch DCM)
-	if(g.radio_tuning == 0) {
-		g.rc_camera_pitch.set_pwm(g.rc_6.radio_in);
-		g.rc_camera_pitch.servo_out = g.rc_camera_pitch.control_mix(p_sensor_value);
-	}else{
-		// unless channel 6 is already being used for tuning
-	    g.rc_camera_pitch.servo_out = p_sensor_value  * -1;
-	}
-	g.rc_camera_pitch.servo_out	= (float)g.rc_camera_pitch.servo_out * g.camera_pitch_gain;
-	
-	// limit
-	//g.rc_camera_pitch.servo_out = constrain(g.rc_camera_pitch.servo_out, -4500, 4500);
-
-
-	// ROLL
-	// -----
-	// allow control mixing
-	/*
-	g.rc_camera_roll.set_pwm(APM_RC.InputCh(CH_6)); // I'm using CH 6 input here.
-	g.rc_camera_roll.servo_out = g.rc_camera_roll.control_mix(-ahrs.roll_sensor);
-	*/
-
-	// dont allow control mixing
-	g.rc_camera_roll.servo_out	= (float)-r_sensor_value * g.camera_roll_gain;
-
-	// limit
-	//g.rc_camera_roll.servo_out = constrain(-ahrs.roll_sensor, -4500, 4500);
-
-	// Output
-	// ------
-	g.rc_camera_pitch.calc_pwm();
-	g.rc_camera_roll.calc_pwm();
-
-	APM_RC.OutputCh(CH_CAM_PITCH, g.rc_camera_pitch.radio_out);
-	APM_RC.OutputCh(CH_CAM_ROLL , g.rc_camera_roll.radio_out);
-	//Serial.printf("c:%d\n",  g.rc_camera_pitch.radio_out);
-}
-
-//#endif

ArduCopter/Parameters.h

@@ -115,6 +115,11 @@ public:
 	k_param_tilt_comp, //164
 
 
+	//
+	// Camera parameters
+	//
+	k_param_camera,
+
 	//
 	// 170: Radio settings
 	//
@@ -126,8 +131,8 @@ public:
 	k_param_rc_6,
 	k_param_rc_7,
 	k_param_rc_8,
-	k_param_rc_camera_pitch,// rc_9
-	k_param_rc_camera_roll, // rc_10
+	k_param_rc_9,
+	k_param_rc_10,
 	k_param_throttle_min,
 	k_param_throttle_max,
 	k_param_throttle_fs_enabled,
@@ -273,17 +278,22 @@ public:
 	RC_Channel	heli_servo_1, heli_servo_2, heli_servo_3, heli_servo_4;	// servos for swash plate and tail
 	#endif
 
+	// Camera
+#if CAMERA == ENABLED
+    AP_Camera		camera;
+#endif
+
 	// RC channels
 	RC_Channel	rc_1;
 	RC_Channel	rc_2;
 	RC_Channel	rc_3;
 	RC_Channel	rc_4;
-	RC_Channel	rc_5;
-	RC_Channel	rc_6;
-	RC_Channel	rc_7;
-	RC_Channel	rc_8;
-	RC_Channel	rc_camera_pitch;
-	RC_Channel	rc_camera_roll;
+	RC_Channel_aux	rc_5;
+	RC_Channel_aux	rc_6;
+	RC_Channel_aux	rc_7;
+	RC_Channel_aux	rc_8;
+	RC_Channel_aux	rc_9;
+	RC_Channel_aux	rc_10;
     AP_Int16    rc_speed; // speed of fast RC Channels in Hz
 
 	AP_Float	camera_pitch_gain;

ArduCopter/Parameters.pde

@@ -202,18 +202,42 @@ static const AP_Param::Info var_info[] PROGMEM = {
 	GGROUP(heli_servo_4,	"HS4_", RC_Channel),
 	#endif
 
+#if CAMERA == ENABLED
+	// @Group: CAM_
+	// @Path: ../libraries/AP_Camera/AP_Camera.cpp
+	GGROUP(camera,                  "CAM_",	AP_Camera),
+#endif
+
 	// RC channel
 	//-----------
 	GGROUP(rc_1,	"RC1_", RC_Channel),
 	GGROUP(rc_2,	"RC2_", RC_Channel),
 	GGROUP(rc_3,	"RC3_", RC_Channel),
 	GGROUP(rc_4,	"RC4_", RC_Channel),
-	GGROUP(rc_5,	"RC5_", RC_Channel),
-	GGROUP(rc_6,	"RC6_", RC_Channel),
-	GGROUP(rc_7,	"RC7_", RC_Channel),
-	GGROUP(rc_8,	"RC8_", RC_Channel),
-	GGROUP(rc_camera_pitch,	"CAM_P_", RC_Channel),
-	GGROUP(rc_camera_roll,	"CAM_R_", RC_Channel),
+
+	// @Group: RC5_
+	// @Path: ../libraries/RC_Channel/RC_Channel_aux.cpp
+	GGROUP(rc_5,                    "RC5_", RC_Channel_aux),
+
+	// @Group: RC6_
+	// @Path: ../libraries/RC_Channel/RC_Channel_aux.cpp
+	GGROUP(rc_6,                    "RC6_", RC_Channel_aux),
+
+	// @Group: RC7_
+	// @Path: ../libraries/RC_Channel/RC_Channel_aux.cpp
+	GGROUP(rc_7,                    "RC7_", RC_Channel_aux),
+
+	// @Group: RC8_
+	// @Path: ../libraries/RC_Channel/RC_Channel_aux.cpp
+	GGROUP(rc_8,                    "RC8_", RC_Channel_aux),
+
+	// @Group: RC9_
+	// @Path: ../libraries/RC_Channel/RC_Channel_aux.cpp
+	GGROUP(rc_9,                    "RC9_", RC_Channel_aux),
+
+	// @Group: RC10_
+	// @Path: ../libraries/RC_Channel/RC_Channel_aux.cpp
+	GGROUP(rc_10,                    "RC10_", RC_Channel_aux),
 
 	// @Param: RC_SPEED
 	// @DisplayName: ESC Update Speed

ArduCopter/commands_logic.pde

@@ -99,8 +99,36 @@ static void process_now_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
+
+		// Sets the region of interest (ROI) for a sensor set or the
+		// vehicle itself. This can then be used by the vehicles control
+		// system to control the vehicle attitude and the attitude of various
+		// devices such as cameras.
+		//    |Region of interest mode. (see MAV_ROI enum)| Waypoint index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple cameras etc.)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|
 		case MAV_CMD_DO_SET_ROI: // 201
 			do_target_yaw();
+#if MOUNT == ENABLED
+			camera_mount.set_roi_cmd();
+			break;
+
+		case MAV_CMD_DO_MOUNT_CONFIGURE:	// Mission command to configure a camera mount |Mount operation mode (see MAV_CONFIGURE_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty|
+			camera_mount.configure_cmd();
+			break;
+
+		case MAV_CMD_DO_MOUNT_CONTROL:		// Mission command to control a camera mount |pitch(deg*100) or lat, depending on mount mode.| roll(deg*100) or lon depending on mount mode| yaw(deg*100) or alt (in cm) depending on mount mode| Empty| Empty| Empty| Empty|
+			camera_mount.control_cmd();
+			break;
+#endif
 	}
 }
 

ArduCopter/config.h

@@ -482,6 +482,28 @@
 #endif
 
 
+//////////////////////////////////////////////////////////////////////////////
+// CAMERA TRIGGER AND CONTROL
+//
+#ifndef CAMERA
+# define CAMERA		ENABLED
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// MOUNT (ANTENNA OR CAMERA)
+//
+#ifndef MOUNT
+# define MOUNT		ENABLED
+#endif
+
+#if defined( __AVR_ATmega1280__ ) && MOUNT == ENABLED
+// The small ATmega1280 chip does not have enough memory for camera support
+// so disable CLI, this will allow camera support and other improvements to fit.
+// This should almost have no side effects, because the APM planner can now do a complete board setup.
+#define CLI_ENABLED DISABLED
+#endif
+
+
 //////////////////////////////////////////////////////////////////////////////
 // Attitude Control
 //

ArduCopter/radio.pde

@@ -38,10 +38,7 @@ static void init_rc_in()
 	g.rc_4.set_type(RC_CHANNEL_ANGLE_RAW);
 
 	//set auxiliary ranges
-	g.rc_5.set_range(0,1000);
-	g.rc_6.set_range(0,1000);
-	g.rc_7.set_range(0,1000);
-	g.rc_8.set_range(0,1000);
+	update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8);
 }
 
 static void init_rc_out()
@@ -57,9 +54,6 @@ static void init_rc_out()
 	motors.set_min_throttle(g.throttle_min);
 	motors.set_max_throttle(g.throttle_max);
 
-	// this is the camera pitch5 and roll6
-	APM_RC.OutputCh(CH_CAM_PITCH, 1500);
-	APM_RC.OutputCh(CH_CAM_ROLL, 1500);
 
 	for(byte i = 0; i < 5; i++){
 		delay(20);

ArduCopter/system.pde

@@ -202,8 +202,6 @@ static void init_ardupilot()
 	init_rc_in();		// sets up rc channels from radio
 	init_rc_out();		// sets up the timer libs
 
-	init_camera();
-
     timer_scheduler.init( &isr_registry );
 
     // initialise the analog port reader

ArduPlane/Parameters.pde

@@ -476,15 +476,19 @@ static const AP_Param::Info var_info[] PROGMEM = {
 	GGROUP(channel_pitch,           "RC2_", RC_Channel),
 	GGROUP(channel_throttle,        "RC3_", RC_Channel),
 	GGROUP(channel_rudder,          "RC4_", RC_Channel),
+
 	// @Group: RC5_
 	// @Path: ../libraries/RC_Channel/RC_Channel_aux.cpp
 	GGROUP(rc_5,                    "RC5_", RC_Channel_aux),
+
 	// @Group: RC6_
 	// @Path: ../libraries/RC_Channel/RC_Channel_aux.cpp
 	GGROUP(rc_6,                    "RC6_", RC_Channel_aux),
+
 	// @Group: RC7_
 	// @Path: ../libraries/RC_Channel/RC_Channel_aux.cpp
 	GGROUP(rc_7,                    "RC7_", RC_Channel_aux),
+
 	// @Group: RC8_
 	// @Path: ../libraries/RC_Channel/RC_Channel_aux.cpp
 	GGROUP(rc_8,                    "RC8_", RC_Channel_aux),