aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: DO_SET_ROI persists across waypoints

Browse Source 
Users should add a DO_SET_ROI command with zero lat, lon and alt to
restore the yaw control to the default control (which is normally
look-at-next-wp)
This commit is contained in:
Randy Mackay 2014-02-18 21:35:29 +09:00
parent 88afd40524
commit fdae812814
6 changed files with 50 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/ArduCopter.pde
View File

@ -634,8 +634,8 @@ static uint32_t throttle_integrator;
////////////////////////////////////////////////////////////////////////////////
// auto flight mode's yaw mode
static uint8_t auto_yaw_mode = AUTO_YAW_LOOK_AT_NEXT_WP;
// Yaw will point at this location if yaw_mode is set to YAW_LOOK_AT_LOCATION
static Vector3f yaw_look_at_WP;
// Yaw will point at this location if auto_yaw_mode is set to AUTO_YAW_ROI
static Vector3f roi_WP;
// bearing from current location to the yaw_look_at_WP
static float yaw_look_at_WP_bearing;
// yaw used for YAW_LOOK_AT_HEADING yaw_mode

14
ArduCopter/Attitude.pde
View File

@ -73,16 +73,16 @@ static float get_pilot_desired_yaw_rate(int16_t stick_angle)
* yaw controllers
*************************************************************/
// get_look_at_yaw - updates bearing to location held in look_at_yaw_WP and calls stabilize yaw controller
// get_roi_yaw - returns heading towards location held in roi_WP
// should be called at 100hz
static float get_look_at_yaw()
static float get_roi_yaw()
{
static uint8_t look_at_yaw_counter = 0; // used to reduce update rate to 10hz
static uint8_t roi_yaw_counter = 0; // used to reduce update rate to 10hz
look_at_yaw_counter++;
if (look_at_yaw_counter >= 10) {
look_at_yaw_counter = 0;
yaw_look_at_WP_bearing = pv_get_bearing_cd(inertial_nav.get_position(), yaw_look_at_WP);
roi_yaw_counter++;
if (roi_yaw_counter >= 10) {
roi_yaw_counter = 0;
yaw_look_at_WP_bearing = pv_get_bearing_cd(inertial_nav.get_position(), roi_WP);
}
return yaw_look_at_WP_bearing;

4
ArduCopter/GCS_Mavlink.pde
View File

@ -1365,8 +1365,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
param1 = tell_command.p1;
break;
case MAV_CMD_NAV_ROI:
case MAV_CMD_DO_SET_ROI:
case MAV_CMD_NAV_ROI: // 80
case MAV_CMD_DO_SET_ROI: // 201
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
x = tell_command.lat/1.0e7f; // local (x), global (latitude)
y = tell_command.lng/1.0e7f; // local (y), global (longitude)

41
ArduCopter/commands_logic.pde
View File

@ -658,26 +658,33 @@ static void do_set_home()
// TO-DO: add support for other features of MAV_CMD_DO_SET_ROI including pointing at a given waypoint
static void do_roi()
{
// if location is zero lat, lon and altitude turn off ROI
if (auto_yaw_mode == AUTO_YAW_ROI && (command_cond_queue.alt == 0 && command_cond_queue.lat == 0 && command_cond_queue.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));
// To-Do: switch off the camera tracking if 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 ) {
yaw_look_at_WP = pv_location_to_vector(command_cond_queue);
set_auto_yaw_mode(AUTO_YAW_LOOK_AT_LOCATION);
}
// send the command to the camera mount
camera_mount.set_roi_cmd(&command_cond_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 implemented)
// 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(command_cond_queue);
set_auto_yaw_mode(AUTO_YAW_ROI);
}
// send the command to the camera mount
camera_mount.set_roi_cmd(&command_cond_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 implemented)
#else
// if we have no camera mount aim the quad at the location
yaw_look_at_WP = pv_location_to_vector(command_cond_queue);
set_auto_yaw_mode(AUTO_YAW_LOOK_AT_LOCATION);
// if we have no camera mount aim the quad at the location
roi_WP = pv_location_to_vector(command_cond_queue);
set_auto_yaw_mode(AUTO_YAW_ROI);
#endif
}
}
// do_take_picture - take a picture with the camera library

20
ArduCopter/control_auto.pde
View File

@ -21,6 +21,11 @@
static bool auto_init(bool ignore_checks)
{
if ((GPS_ok() && inertial_nav.position_ok() && g.command_total > 1) || ignore_checks) {
// stop ROI from carrying over from previous runs of the mission
// To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check
if (auto_yaw_mode == AUTO_YAW_ROI) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
// clear the command queues. will be reloaded when "run_autopilot" calls "update_commands" function
init_commands();
return true;
@ -117,7 +122,10 @@ static void auto_wp_start(const Vector3f& destination)
wp_nav.set_wp_destination(destination);
// initialise yaw
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
// To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI
if (auto_yaw_mode != AUTO_YAW_ROI) {
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
}
}
// auto_wp_run - runs the auto waypoint controller
@ -296,10 +304,10 @@ void set_auto_yaw_mode(uint8_t yaw_mode)
switch (auto_yaw_mode) {
case AUTO_YAW_LOOK_AT_NEXT_WP:
// original_wp_bearing will be set by do_nav_wp or other nav command initialisatoin functions so no init required
// original_wp_bearing will be set by do_nav_wp or other nav command initialisation functions so no init required
break;
case AUTO_YAW_LOOK_AT_LOCATION:
case AUTO_YAW_ROI:
// point towards a location held in yaw_look_at_WP
yaw_look_at_WP_bearing = ahrs.yaw_sensor;
break;
@ -326,9 +334,9 @@ float get_auto_heading(void)
{
switch(auto_yaw_mode) {
case AUTO_YAW_LOOK_AT_LOCATION:
// point towards a location held in yaw_look_at_WP
return get_look_at_yaw();
case AUTO_YAW_ROI:
// point towards a location held in roi_WP
return get_roi_yaw();
break;
case AUTO_YAW_LOOK_AT_HEADING:

2
ArduCopter/defines.h
View File

@ -17,7 +17,7 @@
// ------------
#define AUTO_YAW_HOLD 0 // pilot controls the heading
#define AUTO_YAW_LOOK_AT_NEXT_WP 1 // point towards next waypoint (no pilot input accepted)
#define AUTO_YAW_LOOK_AT_LOCATION 2 // point towards a location held in yaw_look_at_WP (no pilot input accepted)
#define AUTO_YAW_ROI 2 // point towards a location held in roi_WP (no pilot input accepted)
#define AUTO_YAW_LOOK_AT_HEADING 3 // point towards a particular angle (not pilot input accepted)
#define AUTO_YAW_LOOK_AHEAD 4 // point in the direction the copter is moving
#define AUTO_YAW_RESETTOARMEDYAW 5 // point towards heading at time motors were armed