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

ArduCopter: add support for MAVLink's MAV_CMD_NAV_ROI command 

do_nav_roi and verify_nav_roi functions added to turn the copter and/or mount in response to a MAV_CMD_NAV_ROI way point
This commit is contained in:
rmackay9 2012-07-25 11:02:54 +09:00
parent 960c2f08af
commit 5318da9e0e
2 changed files with 127 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
ArduCopter/GCS_Mavlink.pde
View File

@ -1190,7 +1190,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
z = tell_command.alt/1.0e2; // local (z), global/relative (altitude)
}
// Switch to map APM command fields inot MAVLink command fields
// Switch to map APM command fields into MAVLink command fields
switch (tell_command.id) {
case MAV_CMD_NAV_LOITER_TURNS:
@ -1199,6 +1199,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
param1 = tell_command.p1;
break;
case MAV_CMD_NAV_ROI:
param1 = tell_command.p1; // MAV_ROI (aka roi mode) is held in wp's parameter but we actually do nothing with it because we only support pointing at a specific location provided by x,y and z parameters
break;
case MAV_CMD_CONDITION_YAW:
param3 = tell_command.p1;
param1 = tell_command.alt;
@ -1415,15 +1419,18 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
tell_command.lat = 1.0e7 * packet.x; // in as DD converted to * t7
tell_command.lng = 1.0e7 * packet.y; // in as DD converted to * t7
tell_command.alt = packet.z * 1.0e2;
tell_command.options = 1; // store altitude relative!! Always!!
tell_command.options = 1; // store altitude relative to home alt!! Always!!
switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields
switch (tell_command.id) { // Switch to map APM command fields into MAVLink command fields
case MAV_CMD_NAV_LOITER_TURNS:
case MAV_CMD_DO_SET_HOME:
case MAV_CMD_DO_SET_ROI:
tell_command.p1 = packet.param1;
break;
case MAV_CMD_NAV_ROI:
tell_command.p1 = packet.param1; // MAV_ROI (aka roi mode) is held in wp's parameter but we actually do nothing with it because we only support pointing at a specific location provided by x,y and z parameters
break;
case MAV_CMD_CONDITION_YAW:
tell_command.p1 = packet.param3;
tell_command.alt = packet.param1;
@ -1761,6 +1768,40 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
}
#endif // HIL_MODE
#if CAMERA == ENABLED
case MAVLINK_MSG_ID_DIGICAM_CONFIGURE:
{
g.camera.configure_msg(msg);
break;
}
case MAVLINK_MSG_ID_DIGICAM_CONTROL:
{
g.camera.control_msg(msg);
break;
}
#endif // CAMERA == ENABLED
#if MOUNT == ENABLED
case MAVLINK_MSG_ID_MOUNT_CONFIGURE:
{
camera_mount.configure_msg(msg);
break;
}
case MAVLINK_MSG_ID_MOUNT_CONTROL:
{
camera_mount.control_msg(msg);
break;
}
case MAVLINK_MSG_ID_MOUNT_STATUS:
{
camera_mount.status_msg(msg);
break;
}
#endif // MOUNT == ENABLED
case MAVLINK_MSG_ID_RADIO:
{
mavlink_radio_t packet;

96
ArduCopter/commands_logic.pde
View File

@ -36,6 +36,11 @@ static void process_nav_command()
do_RTL();
break;
// point the copter and camera at a region of interest (ROI)
case MAV_CMD_NAV_ROI: // 80
do_nav_roi();
break;
default:
break;
}
@ -110,17 +115,7 @@ static void process_now_command()
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;
@ -129,6 +124,10 @@ static void process_now_command()
camera_mount.control_cmd();
break;
#endif
default:
// do nothing with unrecognized MAVLink messages
break;
}
}
@ -144,6 +143,10 @@ static bool verify_must()
return verify_takeoff();
break;
case MAV_CMD_NAV_WAYPOINT:
return verify_nav_wp();
break;
case MAV_CMD_NAV_LAND:
if(g.sonar_enabled == true){
return verify_land_sonar();
@ -152,10 +155,6 @@ static bool verify_must()
}
break;
case MAV_CMD_NAV_WAYPOINT:
return verify_nav_wp();
break;
case MAV_CMD_NAV_LOITER_UNLIM:
return verify_loiter_unlimited();
break;
@ -172,6 +171,10 @@ static bool verify_must()
return verify_RTL();
break;
case MAV_CMD_NAV_ROI: // 80
return verify_nav_roi();
break;
default:
//gcs_send_text_P(SEVERITY_HIGH,PSTR("<verify_must: default> No current Must commands"));
return false;
@ -695,6 +698,39 @@ static bool verify_yaw()
}
}
// verify_nav_roi - verifies that actions required by MAV_CMD_NAV_ROI have completed
// we assume the camera command has been successfully implemented by the do_nav_roi command
// so all we need to check is whether we needed to yaw the copter (due to the mount type) and
// whether that yaw has completed
// TO-DO: add support for other features of MAV_NAV_ROI including pointing at a given waypoint
static bool verify_nav_roi()
{
#if MOUNT == ENABLED
// check if mount type requires us to rotate the quad
if( camera_mount.get_mount_type() == AP_Mount::k_tilt_roll ) {
// ensure yaw has gotten to within 2 degrees of the target
if( abs(wrap_180(ahrs.yaw_sensor-auto_yaw)) <= 200 ) {
nav_yaw = auto_yaw; // ensure target yaw for YAW_HOLD is our desired yaw
return true;
}else{
return false;
}
}else{
// if no rotation required, assume the camera instruction was implemented immediately
return true;
}
#else
// if we have no camera mount simply check we've reached the desired yaw
// ensure yaw has gotten to within 2 degrees of the target
if( abs(wrap_180(ahrs.yaw_sensor-auto_yaw)) <= 200 ) {
nav_yaw = auto_yaw; // ensure target yaw for YAW_HOLD is our desired yaw
return true;
}else{
return false;
}
#endif
}
/********************************************************************************/
// Do (Now) commands
/********************************************************************************/
@ -817,3 +853,35 @@ static void do_repeat_relay()
event_repeat = command_cond_queue.alt * 2;
update_events();
}
// do_nav_roi - starts actions required by MAV_CMD_NAV_ROI
// this involves either moving the camera to point at the ROI (region of interest)
// and possibly rotating the copter to point at the ROI if our mount type does not support a yaw feature
// Note: the ROI should already be in the command_nav_queue global variable
// TO-DO: add support for other features of MAV_NAV_ROI including pointing at a given waypoint
static void do_nav_roi()
{
#if MOUNT == ENABLED
// check if mount type requires us to rotate the quad
if( camera_mount.get_mount_type() == AP_Mount::k_tilt_roll ) {
yaw_tracking = MAV_ROI_LOCATION;
target_WP = command_nav_queue;
auto_yaw = get_bearing(&current_loc, &target_WP);
}
// send the command to the camera mount
camera_mount.set_roi_cmd(&command_nav_queue);
// TO-DO: expand handling of the do_nav_roi to support all modes of the MAVLink. Currently we only handle mode 4 (see below)
// 0: do nothing
// 1: point at next waypoint
// 2: point at a waypoint taken from WP# parameter (2nd parameter?)
// 3: point at a location given by alt, lon, lat parameters
// 4: point at a target given a target id (can't be implmented)
#else
// if we have no camera mount simply rotate the quad
yaw_tracking = MAV_ROI_LOCATION;
target_WP = command_nav_queue;
auto_yaw = get_bearing(&current_loc, &target_WP);
#endif
}