Rover: added full camera and mount support

APMrover2/APMrover2.pde

@@ -69,6 +69,7 @@ version 2.1 of the License, or (at your option) any later version.
 #include <AverageFilter.h>	// Mode Filter from Filter library
 #include <AP_Relay.h>       // APM relay
 #include <AP_Mount.h>		// Camera/Antenna mount
+#include <AP_Camera.h>		// Camera triggering
 #include <GCS_MAVLink.h>    // MAVLink GCS definitions
 #include <AP_Airspeed.h>    // needed for AHRS build
 #include <memcheck.h>
@@ -257,12 +258,24 @@ static AP_RangeFinder_analog sonar2;
 // relay support
 AP_Relay relay;
 
+// Camera
+#if CAMERA == ENABLED
+static AP_Camera camera(&relay);
+#endif
+
+// The rover's current location
+static struct 	Location current_loc;
+
+
 // Camera/Antenna mount tracking and stabilisation stuff
 // --------------------------------------
 #if MOUNT == ENABLED
-AP_Mount camera_mount(g_gps, &dcm);
+// 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, 0);
 #endif
 
+
 ////////////////////////////////////////////////////////////////////////////////
 // Global variables
 ////////////////////////////////////////////////////////////////////////////////
@@ -280,7 +293,7 @@ static bool usb_connected;
 			4   ---
 			5   Aux5
 			6   Aux6
-			7   Aux7
+			7   Aux7/learn
 			8   Aux8/Mode
 		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
@@ -487,8 +500,6 @@ static struct 	Location home;
 static bool	home_is_set;
 // The location of the previous waypoint.  Used for track following and altitude ramp calculations
 static struct 	Location prev_WP;
-// The rover's current location
-static struct 	Location current_loc;
 // The location of the current/active waypoint.  Used for track following
 static struct 	Location next_WP;
 // The location of the active waypoint in Guided mode.
@@ -672,6 +683,9 @@ static void mount_update(void)
 #if MOUNT == ENABLED
 	camera_mount.update_mount_position();
 #endif
+#if CAMERA == ENABLED
+    camera.trigger_pic_cleanup();
+#endif
 }
 
 /*
@@ -724,6 +738,26 @@ static void update_logging(void)
         Log_Write_Nav_Tuning();
 }
 
+
+/*
+  update aux servo mappings
+ */
+static void update_aux(void)
+{
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+    update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8, &g.rc_9, &g.rc_10, &g.rc_11, &g.rc_12);
+#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
+    update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8, &g.rc_10, &g.rc_11);
+#else
+    update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8);
+#endif
+    enable_aux_servos();
+        
+#if MOUNT == ENABLED
+    camera_mount.update_mount_type();
+#endif
+}
+
 /*
   once a second events
  */
@@ -740,8 +774,7 @@ static void one_second_loop(void)
     set_control_channels();
 
     // cope with changes to aux functions
-    update_aux_servo_function(&g.rc_2, &g.rc_4, &g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8);
+    update_aux();
-    enable_aux_servos();
 
 #if MOUNT == ENABLED
     camera_mount.update_mount_type();
@@ -809,6 +842,12 @@ static void update_GPS(void)
 			}
 		}
         ground_speed   = g_gps->ground_speed_cm * 0.01;
+
+#if CAMERA == ENABLED
+        if (camera.update_location(current_loc) == true) {
+            do_take_picture();
+        }
+#endif        
 	}
 }
 

APMrover2/GCS_Mavlink.pde

@@ -1704,6 +1704,20 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 		}
 #endif // HIL_MODE
 
+#if CAMERA == ENABLED
+    case MAVLINK_MSG_ID_DIGICAM_CONFIGURE:
+    {
+        camera.configure_msg(msg);
+        break;
+    }
+
+    case MAVLINK_MSG_ID_DIGICAM_CONTROL:
+    {
+        camera.control_msg(msg);
+        break;
+    }
+#endif // CAMERA == ENABLED
+
 #if MOUNT == ENABLED
     case MAVLINK_MSG_ID_MOUNT_CONFIGURE:
 		{

APMrover2/Log.pde

@@ -50,6 +50,7 @@ print_log_menu(void)
 		PLOG(CURRENT);
 		PLOG(SONAR);
 		PLOG(COMPASS);
+		PLOG(CAMERA);
 		#undef PLOG
 	}
 
@@ -141,6 +142,7 @@ select_logs(uint8_t argc, const Menu::arg *argv)
 		TARG(CURRENT);
 		TARG(SONAR);
 		TARG(COMPASS);
+		TARG(CAMERA);
 		#undef TARG
 	}
 
@@ -223,6 +225,33 @@ static void Log_Write_Cmd(uint8_t num, const struct Location *wp)
     DataFlash.WriteBlock(&pkt, sizeof(pkt));
 }
 
+struct PACKED log_Camera {
+    LOG_PACKET_HEADER;
+    uint32_t gps_time;
+    int32_t  latitude;
+    int32_t  longitude;
+    int16_t  roll;
+    int16_t  pitch;
+    uint16_t yaw;
+};
+
+// Write a Camera packet. Total length : 26 bytes
+static void Log_Write_Camera()
+{
+#if CAMERA == ENABLED
+    struct log_Camera pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_CAMERA_MSG),
+        gps_time    : g_gps->time,
+        latitude    : current_loc.lat,
+        longitude   : current_loc.lng,
+        roll        : (int16_t)ahrs.roll_sensor,
+        pitch       : (int16_t)ahrs.pitch_sensor,
+        yaw         : (uint16_t)ahrs.yaw_sensor
+    };
+    DataFlash.WriteBlock(&pkt, sizeof(pkt));
+#endif
+}
+
 struct PACKED log_Startup {
     LOG_PACKET_HEADER;
     uint8_t startup_type;
@@ -430,6 +459,8 @@ static const struct LogStructure log_structure[] PROGMEM = {
       "PM",  "IHhBBBhhhhB", "LTime,MLC,gDt,RNCnt,RNBl,GPScnt,GDx,GDy,GDz,PMT,I2CErr" },
     { LOG_CMD_MSG, sizeof(log_Cmd),                 
       "CMD", "BBBBBeLL",   "CTot,CNum,CId,COpt,Prm1,Alt,Lat,Lng" },
+    { LOG_CAMERA_MSG, sizeof(log_Camera),                 
+      "CAM", "ILLccC",   "GPSTime,Lat,Lng,Roll,Pitch,Yaw" },
     { LOG_STARTUP_MSG, sizeof(log_Startup),         
       "STRT", "BB",         "SType,CTot" },
     { LOG_CTUN_MSG, sizeof(log_Control_Tuning),     

APMrover2/Parameters.h

@@ -137,12 +137,21 @@ public:
         k_param_pidServoSteer,
         k_param_pidSpeedThrottle,
 
+        // high RC channels
+        k_param_rc_9 = 235,
+        k_param_rc_10,
+        k_param_rc_11,
+        k_param_rc_12,
+
         // other objects
         k_param_sitl = 240,
         k_param_ahrs,
         k_param_ins,
         k_param_compass,
         k_param_rcmap,
+        k_param_camera,
+        k_param_camera_mount,
+        k_param_camera_mount2,
 
         // 254,255: reserved
         };
@@ -199,6 +208,16 @@ public:
     RC_Channel_aux	rc_6;
     RC_Channel_aux	rc_7;
     RC_Channel_aux	rc_8;
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+    RC_Channel_aux rc_9;
+#endif
+#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 || CONFIG_HAL_BOARD == HAL_BOARD_PX4
+    RC_Channel_aux rc_10;
+    RC_Channel_aux rc_11;
+#endif
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+    RC_Channel_aux rc_12;
+#endif
 
     // Throttle
     //
@@ -256,6 +275,16 @@ public:
         rc_6(CH_6),
         rc_7(CH_7),
         rc_8(CH_8),
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+        rc_9                                    (CH_9),
+#endif
+#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 || CONFIG_HAL_BOARD == HAL_BOARD_PX4
+        rc_10                                   (CH_10),
+        rc_11                                   (CH_11),
+#endif
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+        rc_12                                   (CH_12),
+#endif
 
         // PID controller    initial P        initial I        initial D        initial imax
         //-----------------------------------------------------------------------------------

APMrover2/Parameters.pde

@@ -227,6 +227,28 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Path: ../libraries/RC_Channel/RC_Channel.cpp
 	GGROUP(rc_8,                    "RC8_", RC_Channel_aux),
 
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+    // @Group: RC9_
+    // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
+    GGROUP(rc_9,                    "RC9_", RC_Channel_aux),
+#endif
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 || CONFIG_HAL_BOARD == HAL_BOARD_PX4
+    // @Group: RC10_
+    // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
+    GGROUP(rc_10,                    "RC10_", RC_Channel_aux),
+
+    // @Group: RC11_
+    // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
+    GGROUP(rc_11,                    "RC11_", RC_Channel_aux),
+#endif
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+    // @Group: RC12_
+    // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
+    GGROUP(rc_12,                    "RC12_", RC_Channel_aux),
+#endif
+
     // @Param: THR_MIN
     // @DisplayName: Minimum Throttle
     // @Description: The minimum throttle setting to which the autopilot will apply. This is mostly useful for rovers with internal combustion motors, to prevent the motor from cutting out in auto mode.
@@ -461,6 +483,18 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Path: ../libraries/AP_AHRS/AP_AHRS.cpp
     GOBJECT(ahrs,                   "AHRS_",    AP_AHRS),
 
+#if CAMERA == ENABLED
+    // @Group: CAM_
+    // @Path: ../libraries/AP_Camera/AP_Camera.cpp
+    GOBJECT(camera,                  "CAM_", AP_Camera),
+#endif
+
+#if MOUNT == ENABLED
+    // @Group: MNT_
+    // @Path: ../libraries/AP_Mount/AP_Mount.cpp
+    GOBJECT(camera_mount,           "MNT_", AP_Mount),
+#endif
+
 	AP_VAREND
 };
 

APMrover2/Steering.pde

@@ -207,6 +207,16 @@ static void set_servos(void)
 	g.rc_6.output_ch(CH_6);
 	g.rc_7.output_ch(CH_7);
 	g.rc_8.output_ch(CH_8);
+ #if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+    g.rc_9.output_ch(CH_9);
+ #endif
+ #if CONFIG_HAL_BOARD == HAL_BOARD_APM2 || CONFIG_HAL_BOARD == HAL_BOARD_PX4
+    g.rc_10.output_ch(CH_10);
+    g.rc_11.output_ch(CH_11);
+ #endif
+ #if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+    g.rc_12.output_ch(CH_12);
+ #endif
 
 #endif
 }

APMrover2/commands_logic.pde

@@ -86,6 +86,18 @@ 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|
+        do_take_picture();
+        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
@@ -93,7 +105,10 @@ static void handle_process_do_command()
 		// 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:
+#if 0
+            // not supported yet
 			camera_mount.set_roi_cmd();
+#endif
 			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|
@@ -375,32 +390,11 @@ static void do_repeat_servo()
 	event_id = next_nonnav_command.p1 - 1;
 
 	if(next_nonnav_command.p1 >= CH_5 + 1 && next_nonnav_command.p1 <= CH_8 + 1) {
-
 		event_timer 	= 0;
 		event_delay 	= next_nonnav_command.lng * 500.0;	// /2 (half cycle time) * 1000 (convert to milliseconds)
 		event_repeat 	= next_nonnav_command.lat * 2;
 		event_value 	= next_nonnav_command.alt;
-
+        event_undo_value  = RC_Channel::rc_channel(next_nonnav_command.p1-1)->radio_trim;
-		switch(next_nonnav_command.p1) {
-			case CH_2:
-				event_undo_value = g.rc_2.radio_trim;
-				break;
-			case CH_4:
-				event_undo_value = g.rc_4.radio_trim;
-				break;
-			case CH_5:
-				event_undo_value = g.rc_5.radio_trim;
-				break;
-			case CH_6:
-				event_undo_value = g.rc_6.radio_trim;
-				break;
-			case CH_7:
-				event_undo_value = g.rc_7.radio_trim;
-				break;
-			case CH_8:
-				event_undo_value = g.rc_8.radio_trim;
-				break;
-		}
 		update_events();
 	}
 }
@@ -414,3 +408,14 @@ static void do_repeat_relay()
 	update_events();
 }
 
+
+// do_take_picture - take a picture with the camera library
+static void do_take_picture()
+{
+#if CAMERA == ENABLED
+    camera.trigger_pic();
+    if (g.log_bitmask & MASK_LOG_CAMERA) {
+        Log_Write_Camera();
+    }
+#endif
+}

APMrover2/config.h

@@ -36,9 +36,6 @@
  // default choices for a 1280. We can't fit everything in, so we 
  // make some popular choices by default
  #define LOGGING_ENABLED DISABLED
- #ifndef MOUNT2
- # define MOUNT2 DISABLED
- #endif
  #ifndef MOUNT
  # define MOUNT DISABLED
  #endif
@@ -286,7 +283,14 @@
 // MOUNT (ANTENNA OR CAMERA)
 //
 #ifndef MOUNT
-# define MOUNT		DISABLED
+# define MOUNT		ENABLED
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// CAMERA control
+//
+#ifndef CAMERA
+# define CAMERA ENABLED
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
@@ -368,12 +372,14 @@
     MASK_LOG_ATTITUDE_MED | \
     MASK_LOG_GPS | \
     MASK_LOG_PM | \
+    MASK_LOG_CTUN | \
     MASK_LOG_NTUN | \
     MASK_LOG_MODE | \
     MASK_LOG_CMD | \
     MASK_LOG_SONAR | \
     MASK_LOG_COMPASS | \
-    MASK_LOG_CURRENT
+    MASK_LOG_CURRENT | \
+    MASK_LOG_CAMERA
 
 
 

APMrover2/defines.h

@@ -123,6 +123,7 @@ enum ap_message {
 #define LOG_ATTITUDE_MSG        0x08
 #define LOG_MODE_MSG            0x09
 #define LOG_COMPASS_MSG         0x0A
+#define LOG_CAMERA_MSG          0x0B
 
 #define TYPE_AIRSTART_MSG		0x00
 #define TYPE_GROUNDSTART_MSG	0x01
@@ -140,6 +141,7 @@ enum ap_message {
 #define MASK_LOG_CURRENT		(1<<9)
 #define MASK_LOG_SONAR   		(1<<10)
 #define MASK_LOG_COMPASS   		(1<<11)
+#define MASK_LOG_CAMERA   		(1<<12)
 
 // Waypoint Modes
 // ----------------

APMrover2/radio.pde

@@ -21,7 +21,7 @@ static void init_rc_in()
 	channel_throttle->set_default_dead_zone(3);
 
 	//set auxiliary ranges
-    update_aux_servo_function(&g.rc_2, &g.rc_4, &g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8);
+    update_aux();
 }
 
 static void init_rc_out()
@@ -30,6 +30,17 @@ static void init_rc_out()
         RC_Channel::rc_channel(i)->enable_out();
         RC_Channel::rc_channel(i)->output_trim();
     }
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+    servo_write(CH_9,   g.rc_9.radio_trim);
+#endif
+#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 || CONFIG_HAL_BOARD == HAL_BOARD_PX4
+    servo_write(CH_10,  g.rc_10.radio_trim);
+    servo_write(CH_11,  g.rc_11.radio_trim);
+#endif
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+    servo_write(CH_12,  g.rc_12.radio_trim);
+#endif
 }
 
 static void read_radio()

APMrover2/system.pde

@@ -560,3 +560,20 @@ static uint8_t check_digital_pin(uint8_t pin)
 
     return hal.gpio->read(dpin);
 }
+
+/*
+  write to a servo
+ */
+static void servo_write(uint8_t ch, uint16_t pwm)
+{
+#if HIL_MODE != HIL_MODE_DISABLED
+    if (!g.hil_servos) {
+        if (ch < 8) {
+            RC_Channel::rc_channel(ch)->radio_out = pwm;
+        }
+        return;
+    }
+#endif
+    hal.rcout->enable_ch(ch);
+    hal.rcout->write(ch, pwm);
+}