From 5b88334dbb1c9bf9d23274340dca321c32efe43a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 15 Jul 2013 09:57:00 +1000 Subject: [PATCH] Rover: added full camera and mount support --- APMrover2/APMrover2.pde | 51 +++++++++++++++++++++++++++++++----- APMrover2/GCS_Mavlink.pde | 14 ++++++++++ APMrover2/Log.pde | 31 ++++++++++++++++++++++ APMrover2/Parameters.h | 29 ++++++++++++++++++++ APMrover2/Parameters.pde | 34 ++++++++++++++++++++++++ APMrover2/Steering.pde | 10 +++++++ APMrover2/commands_logic.pde | 49 ++++++++++++++++++---------------- APMrover2/config.h | 16 +++++++---- APMrover2/defines.h | 2 ++ APMrover2/radio.pde | 13 ++++++++- APMrover2/system.pde | 17 ++++++++++++ 11 files changed, 232 insertions(+), 34 deletions(-) diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index ec33e9992d..7185a3aef2 100644 --- a/APMrover2/APMrover2.pde +++ b/APMrover2/APMrover2.pde @@ -69,6 +69,7 @@ version 2.1 of the License, or (at your option) any later version. #include // Mode Filter from Filter library #include // APM relay #include // Camera/Antenna mount +#include // Camera triggering #include // MAVLink GCS definitions #include // needed for AHRS build #include @@ -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(¤t_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); - enable_aux_servos(); + update_aux(); #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 } } diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde index 8589911b0b..1a9be99652 100644 --- a/APMrover2/GCS_Mavlink.pde +++ b/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: { diff --git a/APMrover2/Log.pde b/APMrover2/Log.pde index 6f3be7f596..14cb16f9b8 100644 --- a/APMrover2/Log.pde +++ b/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), diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index e7775dfcc3..30fe37d508 100644 --- a/APMrover2/Parameters.h +++ b/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 //----------------------------------------------------------------------------------- diff --git a/APMrover2/Parameters.pde b/APMrover2/Parameters.pde index e729b937c1..8f457fd733 100644 --- a/APMrover2/Parameters.pde +++ b/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 }; diff --git a/APMrover2/Steering.pde b/APMrover2/Steering.pde index 9482df234d..6e5eeafc23 100644 --- a/APMrover2/Steering.pde +++ b/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 } diff --git a/APMrover2/commands_logic.pde b/APMrover2/commands_logic.pde index 7203b6fde1..a6e0a62571 100644 --- a/APMrover2/commands_logic.pde +++ b/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; - - 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; - } + event_undo_value = RC_Channel::rc_channel(next_nonnav_command.p1-1)->radio_trim; 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 +} diff --git a/APMrover2/config.h b/APMrover2/config.h index f6b9691f3e..242500f421 100644 --- a/APMrover2/config.h +++ b/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 diff --git a/APMrover2/defines.h b/APMrover2/defines.h index 2e91892ace..9d33fb4aed 100644 --- a/APMrover2/defines.h +++ b/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 // ---------------- diff --git a/APMrover2/radio.pde b/APMrover2/radio.pde index 16bc7ebc03..56223e6a81 100644 --- a/APMrover2/radio.pde +++ b/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() diff --git a/APMrover2/system.pde b/APMrover2/system.pde index 7de80b90df..0c585faf90 100644 --- a/APMrover2/system.pde +++ b/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); +}