// set_auto_yaw_roi - sets the yaw to look at roi for auto mode
void Copter::set_auto_yaw_roi(const Location &roi_location)
{
// if location is zero lat, lon and altitude turn off ROI
if (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.has_pan_control()) {
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_target(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
}
}
// rtl_return_start - initialise return to home
void Copter::rtl_return_start()
{
rtl_state = RTL_ReturnHome;
rtl_state_complete = false;
wp_nav.set_wp_destination(rtl_path.return_target);
// initialise yaw to point home (maybe)
set_auto_yaw_mode(get_default_auto_yaw_mode(true));
}
// guided_init - initialise guided controller
bool Sub::guided_init(bool ignore_checks)
{
if (!position_ok() && !ignore_checks) {
return false;
}
// initialise yaw
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
// start in position control mode
guided_pos_control_start();
return true;
}
// guided_init - initialise guided controller
bool Copter::guided_init(bool ignore_checks)
{
if (position_ok() || ignore_checks) {
// initialise yaw
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
// start in position control mode
guided_pos_control_start();
return true;
}else{
return false;
}
}
// auto_wp_start - initialises waypoint controller to implement flying to a particular destination
void Copter::auto_wp_start(const Vector3f& destination)
{
auto_mode = Auto_WP;
// initialise wpnav (no need to check return status because terrain data is not used)
wp_nav.set_wp_destination(destination, false);
// initialise yaw
// 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));
}
}
// rtl_loiterathome_start - initialise return to home
void Copter::rtl_loiterathome_start()
{
rtl_state = RTL_LoiterAtHome;
rtl_state_complete = false;
rtl_loiter_start_time = millis();
// yaw back to initial take-off heading yaw unless pilot has already overridden yaw
if(get_default_auto_yaw_mode(true) != AUTO_YAW_HOLD) {
set_auto_yaw_mode(AUTO_YAW_RESETTOARMEDYAW);
} else {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
}
// rtl_return_start - initialise return to home
void Copter::rtl_return_start()
{
rtl_state = RTL_ReturnHome;
rtl_state_complete = false;
if (!wp_nav.set_wp_destination(rtl_path.return_target)) {
// failure must be caused by missing terrain data, restart RTL
rtl_restart_without_terrain();
}
// initialise yaw to point home (maybe)
set_auto_yaw_mode(get_default_auto_yaw_mode(true));
}
// auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location
// we assume the caller has performed all required GPS_ok checks
void Copter::auto_circle_movetoedge_start(const Location_Class &circle_center, float radius_m)
{
// convert location to vector from ekf origin
Vector3f circle_center_neu;
if (!circle_center.get_vector_from_origin_NEU(circle_center_neu)) {
// default to current position and log error
circle_center_neu = inertial_nav.get_position();
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_CIRCLE_INIT);
}
circle_nav.set_center(circle_center_neu);
// set circle radius
if (!is_zero(radius_m)) {
circle_nav.set_radius(radius_m * 100.0f);
}
// check our distance from edge of circle
Vector3f circle_edge_neu;
circle_nav.get_closest_point_on_circle(circle_edge_neu);
float dist_to_edge = (inertial_nav.get_position() - circle_edge_neu).length();
// if more than 3m then fly to edge
if (dist_to_edge > 300.0f) {
// set the state to move to the edge of the circle
auto_mode = Auto_CircleMoveToEdge;
// convert circle_edge_neu to Location_Class
Location_Class circle_edge(circle_edge_neu);
// convert altitude to same as command
circle_edge.set_alt_cm(circle_center.alt, circle_center.get_alt_frame());
// initialise wpnav to move to edge of circle
if (!wp_nav.set_wp_destination(circle_edge)) {
// failure to set destination can only be because of missing terrain data
failsafe_terrain_on_event();
}
// if we are outside the circle, point at the edge, otherwise hold yaw
const Vector3f &curr_pos = inertial_nav.get_position();
float dist_to_center = norm(circle_center_neu.x - curr_pos.x, circle_center_neu.y - curr_pos.y);
if (dist_to_center > circle_nav.get_radius() && dist_to_center > 500) {
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
} else {
// vehicle is within circle so hold yaw to avoid spinning as we move to edge of circle
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
} else {
auto_circle_start();
}
}
// auto_spline_start - initialises waypoint controller to implement flying to a particular destination using the spline controller
// seg_end_type can be SEGMENT_END_STOP, SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE. If Straight or Spline the next_destination should be provided
void Copter::auto_spline_start(const Vector3f& destination, bool stopped_at_start,
AC_WPNav::spline_segment_end_type seg_end_type,
const Vector3f& next_destination)
{
auto_mode = Auto_Spline;
// initialise wpnav
wp_nav.set_spline_destination(destination, stopped_at_start, seg_end_type, next_destination);
// initialise yaw
// 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_start - initialises waypoint controller to implement flying to a particular destination
void Copter::auto_wp_start(const Location_Class& dest_loc)
{
auto_mode = Auto_WP;
// send target to waypoint controller
if (!wp_nav.set_wp_destination(dest_loc)) {
// failure to set destination can only be because of missing terrain data
failsafe_terrain_on_event();
}
// initialise yaw
// 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_spline_start - initialises waypoint controller to implement flying to a particular destination using the spline controller
// seg_end_type can be SEGMENT_END_STOP, SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE. If Straight or Spline the next_destination should be provided
void Copter::auto_spline_start(const Location_Class& destination, bool stopped_at_start,
AC_WPNav::spline_segment_end_type seg_end_type,
const Location_Class& next_destination)
{
auto_mode = Auto_Spline;
// initialise wpnav
if (!wp_nav.set_spline_destination(destination, stopped_at_start, seg_end_type, next_destination)) {
// failure to set destination can only be because of missing terrain data
failsafe_terrain_on_event();
return;
}
// initialise yaw
// 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_spline_start - initialises waypoint controller to implement flying to a particular destination using the spline controller
// seg_end_type can be SEGMENT_END_STOP, SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE. If Straight or Spline the next_destination should be provided
void Copter::auto_spline_start(const Location_Class& destination, bool stopped_at_start,
AC_WPNav::spline_segment_end_type seg_end_type,
const Location_Class& next_destination)
{
auto_mode = Auto_Spline;
// initialise wpnav
if (!wp_nav.set_spline_destination(destination, stopped_at_start, seg_end_type, next_destination)) {
// failure to set destination (likely because of missing terrain data)
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
// To-Do: handle failure
}
// initialise yaw
// 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));
}
}
// initialise guided mode's position controller
void Sub::guided_pos_control_start()
{
// set to position control mode
guided_mode = Guided_WP;
// initialise waypoint and spline controller
wp_nav.wp_and_spline_init();
// initialise wpnav to stopping point at current altitude
// To-Do: set to current location if disarmed?
// To-Do: set to stopping point altitude?
Vector3f stopping_point;
stopping_point.z = inertial_nav.get_altitude();
wp_nav.get_wp_stopping_point_xy(stopping_point);
// no need to check return status because terrain data is not used
wp_nav.set_wp_destination(stopping_point, false);
// initialise yaw
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
}
// rtl_return_start - initialise return to home
void Copter::rtl_return_start()
{
rtl_state = RTL_ReturnHome;
rtl_state_complete = false;
// set target to above home/rally point
#if AC_RALLY == ENABLED
// rally_point will be the nearest rally point or home. uses absolute altitudes
Location rally_point = rally.calc_best_rally_or_home_location(current_loc, rtl_alt+ahrs.get_home().alt);
rally_point.alt -= ahrs.get_home().alt; // convert to altitude above home
rally_point.alt = max(rally_point.alt, current_loc.alt); // ensure we do not descend before reaching home
Vector3f destination = pv_location_to_vector(rally_point);
#else
Vector3f destination = pv_location_to_vector(ahrs.get_home());
destination.z = pv_alt_above_origin(rtl_alt));
#endif
wp_nav.set_wp_destination(destination);
// initialise yaw to point home (maybe)
set_auto_yaw_mode(get_default_auto_yaw_mode(true));
}
// auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location
// we assume the caller has set the circle's circle with circle_nav.set_center()
// we assume the caller has performed all required GPS_ok checks
void Copter::auto_circle_movetoedge_start()
{
// check our distance from edge of circle
Vector3f circle_edge;
circle_nav.get_closest_point_on_circle(circle_edge);
// set the state to move to the edge of the circle
auto_mode = Auto_CircleMoveToEdge;
// initialise wpnav to move to edge of circle
wp_nav.set_wp_destination(circle_edge);
// if we are outside the circle, point at the edge, otherwise hold yaw
const Vector3f &curr_pos = inertial_nav.get_position();
const Vector3f &circle_center = circle_nav.get_center();
float dist_to_center = pythagorous2(circle_center.x - curr_pos.x, circle_center.y - curr_pos.y);
if (dist_to_center > circle_nav.get_radius() && dist_to_center > 500) {
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
} else {
// vehicle is within circle so hold yaw to avoid spinning as we move to edge of circle
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
}