From 5318da9e0ea8079ec4a690471a01a417f20ae355 Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Wed, 25 Jul 2012 11:02:54 +0900 Subject: [PATCH] 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 --- ArduCopter/GCS_Mavlink.pde | 49 ++++++++++++++++-- ArduCopter/commands_logic.pde | 96 ++++++++++++++++++++++++++++++----- 2 files changed, 127 insertions(+), 18 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 57d276ffd4..ef56c79728 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -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; diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index f42af46ccf..164ea439bb 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -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(" 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(¤t_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(¤t_loc, &target_WP); +#endif +}