Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: added full camera and mount support

This commit is contained in:
Andrew Tridgell 2013-07-15 09:57:00 +10:00
parent 9a159a5e23
commit 5b88334dbb
11 changed files with 232 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
APMrover2/APMrover2.pde
View File

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

14
APMrover2/GCS_Mavlink.pde
View File

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

31
APMrover2/Log.pde
View File

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

29
APMrover2/Parameters.h
View File

@ -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
//-----------------------------------------------------------------------------------

34
APMrover2/Parameters.pde
View File

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

10
APMrover2/Steering.pde
View File

@ -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
}

49
APMrover2/commands_logic.pde
View File

@ -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
}

16
APMrover2/config.h
View File

@ -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

2
APMrover2/defines.h
View File

@ -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
// ----------------

13
APMrover2/radio.pde
View File

@ -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()

17
APMrover2/system.pde
View File

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