diff --git a/ArduBoat/ArduBoat.pde b/ArduBoat/ArduBoat.pde index 4e32cfdf72..9df8a1f8b9 100644 --- a/ArduBoat/ArduBoat.pde +++ b/ArduBoat/ArduBoat.pde @@ -16,6 +16,10 @@ #include #include #include +#include +#include +#include +#include // Vehicle Configuration //#include "BoatGeneric.h" diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 3448953362..5e4009b755 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -96,6 +96,8 @@ http://code.google.com/p/ardupilot-mega/downloads/list #include // Mode Filter from Filter library #include // GPS Lead filter #include // APM relay +#include // Photo or video camera +#include // Camera/Antenna mount #include // 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(¤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 //////////////////////////////////////////////////////////////////////////////// @@ -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 diff --git a/ArduCopter/Camera.pde b/ArduCopter/Camera.pde deleted file mode 100644 index 291a366846..0000000000 --- a/ArduCopter/Camera.pde +++ /dev/null @@ -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 diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 33de308932..7e17cb99d7 100644 --- a/ArduCopter/Parameters.h +++ b/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; diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index e8f370b422..43f0955dd2 100644 --- a/ArduCopter/Parameters.pde +++ b/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 diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 260d66df3d..3ab16ef5c5 100644 --- a/ArduCopter/commands_logic.pde +++ b/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 } } diff --git a/ArduCopter/config.h b/ArduCopter/config.h index ddfd44abed..4e8ef9f1ab 100644 --- a/ArduCopter/config.h +++ b/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 // diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index 571e5045e3..e36c5c9eab 100644 --- a/ArduCopter/radio.pde +++ b/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); diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 90e0acd9e7..ec2f8d4a45 100644 --- a/ArduCopter/system.pde +++ b/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 diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index 010e24c9e7..40ed539789 100644 --- a/ArduPlane/Parameters.pde +++ b/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),