// sets guided mode's target from a Location object // returns false if destination could not be set (probably caused by missing terrain data) // or if the fence is enabled and guided waypoint is outside the fence bool Sub::guided_set_destination(const Location_Class& dest_loc) { // ensure we are in position control mode if (guided_mode != Guided_WP) { guided_pos_control_start(); } #if AC_FENCE == ENABLED // reject destination outside the fence. // Note: there is a danger that a target specified as a terrain altitude might not be checked if the conversion to alt-above-home fails if (!fence.check_destination_within_fence(dest_loc)) { Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE); // failure is propagated to GCS with NAK return false; } #endif if (!wp_nav.set_wp_destination(dest_loc)) { // failure to set destination can only be because of missing terrain data Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION); // failure is propagated to GCS with NAK return false; } // log target Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f()); return 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; } }
// guided_set_destination - sets guided mode's target destination // Returns true if the fence is enabled and guided waypoint is within the fence // else return false if the waypoint is outside the fence bool Copter::guided_set_destination(const Vector3f& destination) { // ensure we are in position control mode if (guided_mode != Guided_WP) { guided_pos_control_start(); } #if AC_FENCE == ENABLED // reject destination if outside the fence if (!fence.check_destination_within_fence(pv_alt_above_home(destination.z)*0.01f, pv_distance_to_home_cm(destination)*0.01f)) { Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE); // failure is propagated to GCS with NAK return false; } #endif // no need to check return status because terrain data is not used wp_nav.set_wp_destination(destination, false); // log target Log_Write_GuidedTarget(guided_mode, destination, Vector3f()); return true; }