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

Copter: DO_SET_ROI accepted outside missions 

Moved ROI logic to new set_auto_yaw_roi function.
This commit is contained in:
Randy Mackay 2014-07-04 15:40:12 +09:00
parent ae8fb3f1c5
commit 81355d1adf
3 changed files with 53 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
ArduCopter/GCS_Mavlink.pde
View File

@ -1124,6 +1124,21 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
}
break;
case MAV_CMD_DO_SET_ROI:
// param1 : regional of interest mode (not supported)
// param2 : mission index/ target id (not supported)
// param3 : ROI index (not supported)
// param5 : x / lat
// param6 : y / lon
// param7 : z / alt
Location roi_loc;
roi_loc.lat = (int32_t)(packet.param5 * 1.0e7f);
roi_loc.lng = (int32_t)(packet.param6 * 1.0e7f);
roi_loc.alt = (int32_t)(packet.param7 * 100.0f);
set_auto_yaw_roi(roi_loc);
result = MAV_RESULT_ACCEPTED;
break;
case MAV_CMD_MISSION_START:
if (set_mode(AUTO)) {
result = MAV_RESULT_ACCEPTED;

34
ArduCopter/commands_logic.pde
View File

@ -896,42 +896,10 @@ static void do_set_home(const AP_Mission::Mission_Command& cmd)
// do_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_CMD_DO_SET_ROI including pointing at a given waypoint
static void do_roi(const AP_Mission::Mission_Command& cmd)
{
// if location is zero lat, lon and altitude turn off ROI
if (auto_yaw_mode == AUTO_YAW_ROI && (cmd.content.location.alt == 0 && cmd.content.location.lat == 0 && cmd.content.location.lng == 0)) {
// set auto yaw mode back to default assuming the active command is a waypoint command. A more sophisticated method is required to ensure we return to the proper yaw control for the active command
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
#if MOUNT == ENABLED
// switch off the camera tracking if enabled
if (camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) {
camera_mount.set_mode_to_default();
}
#endif // MOUNT == ENABLED
}else{
#if MOUNT == ENABLED
// check if mount type requires us to rotate the quad
if(camera_mount.get_mount_type() != AP_Mount::k_pan_tilt && camera_mount.get_mount_type() != AP_Mount::k_pan_tilt_roll) {
roi_WP = pv_location_to_vector(cmd.content.location);
set_auto_yaw_mode(AUTO_YAW_ROI);
}
// send the command to the camera mount
camera_mount.set_roi_cmd(&cmd.content.location);
// 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 implemented)
#else
// if we have no camera mount aim the quad at the location
roi_WP = pv_location_to_vector(cmd.content.location);
set_auto_yaw_mode(AUTO_YAW_ROI);
#endif // MOUNT == ENABLED
}
set_auto_yaw_roi(cmd.content.location);
}
// do_take_picture - take a picture with the camera library

37
ArduCopter/control_auto.pde
View File

@ -485,6 +485,43 @@ static void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, u
// TO-DO: restore support for clockwise and counter clockwise rotation held in cmd.content.yaw.direction. 1 = clockwise, -1 = counterclockwise
}
// set_auto_yaw_roi - sets the yaw to look at roi for auto mode
static void set_auto_yaw_roi(const Location &roi_location)
{
// if location is zero lat, lon and altitude turn off ROI
if (auto_yaw_mode == AUTO_YAW_ROI && (roi_location.alt == 0 && roi_location.lat == 0 && roi_location.lng == 0)) {
// set auto yaw mode back to default assuming the active command is a waypoint command. A more sophisticated method is required to ensure we return to the proper yaw control for the active command
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
#if MOUNT == ENABLED
// switch off the camera tracking if enabled
if (camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) {
camera_mount.set_mode_to_default();
}
#endif // MOUNT == ENABLED
}else{
#if MOUNT == ENABLED
// check if mount type requires us to rotate the quad
if(camera_mount.get_mount_type() != AP_Mount::k_pan_tilt && camera_mount.get_mount_type() != AP_Mount::k_pan_tilt_roll) {
roi_WP = pv_location_to_vector(roi_location);
set_auto_yaw_mode(AUTO_YAW_ROI);
}
// send the command to the camera mount
camera_mount.set_roi_cmd(&roi_location);
// 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 implemented)
#else
// if we have no camera mount aim the quad at the location
roi_WP = pv_location_to_vector(roi_location);
set_auto_yaw_mode(AUTO_YAW_ROI);
#endif // MOUNT == ENABLED
}
}
// get_auto_heading - returns target heading depending upon auto_yaw_mode
// 100hz update rate
float get_auto_heading(void)