void ModeSmartRTL::update() { switch (smart_rtl_state) { case SmartRTL_WaitForPathCleanup: // check if return path is computed and if yes, begin journey home if (g2.smart_rtl.request_thorough_cleanup()) { smart_rtl_state = SmartRTL_PathFollow; _load_point = true; } // Note: this may lead to an unnecessary 20ms slow down of the vehicle (but it is unlikely) stop_vehicle(); break; case SmartRTL_PathFollow: // load point if required if (_load_point) { Vector3f next_point; if (!g2.smart_rtl.pop_point(next_point)) { // if not more points, we have reached home gcs().send_text(MAV_SEVERITY_INFO, "Reached destination"); smart_rtl_state = SmartRTL_StopAtHome; break; } _load_point = false; // set target destination to new point if (!set_desired_location_NED(next_point)) { // this failure should never happen but we add it just in case gcs().send_text(MAV_SEVERITY_INFO, "SmartRTL: failed to set destination"); smart_rtl_state = SmartRTL_Failure; } } // check if we've reached the next point _distance_to_destination = get_distance(rover.current_loc, _destination); if (_distance_to_destination <= rover.g.waypoint_radius || location_passed_point(rover.current_loc, _origin, _destination)) { _load_point = true; } // continue driving towards destination calc_steering_to_waypoint(_origin, _destination); calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true); break; case SmartRTL_StopAtHome: case SmartRTL_Failure: _reached_destination = true; if (rover.is_boat()) { // boats attempt to hold position at home calc_steering_to_waypoint(rover.current_loc, _destination); calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true); } else { // rovers stop stop_vehicle(); } break; } }
void ModeRTL::update() { if (!_reached_destination || rover.is_boat()) { // calculate distance to home _distance_to_destination = get_distance(rover.current_loc, _destination); // check if we've reached the destination if (!_reached_destination && (_distance_to_destination <= rover.g.waypoint_radius || location_passed_point(rover.current_loc, _origin, _destination))) { // trigger reached _reached_destination = true; gcs().send_text(MAV_SEVERITY_INFO, "Reached destination"); } // continue driving towards destination calc_steering_to_waypoint(_reached_destination ? rover.current_loc :_origin, _destination); calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true); } else { // we've reached destination so stop stop_vehicle(); _desired_lat_accel = 0.0f; } }
void ModeRTL::update() { // calculate distance to home _distance_to_destination = rover.current_loc.get_distance(_destination); const bool near_wp = _distance_to_destination <= rover.g.waypoint_radius; // check if we've reached the destination if (!_reached_destination && (near_wp || location_passed_point(rover.current_loc, _origin, _destination))) { // trigger reached _reached_destination = true; gcs().send_text(MAV_SEVERITY_INFO, "Reached destination"); } // determine if we should keep navigating if (!_reached_destination || (rover.is_boat() && !near_wp)) { // continue driving towards destination calc_steering_to_waypoint(_reached_destination ? rover.current_loc :_origin, _destination, _reversed); calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, true); } else { // we've reached destination so stop stop_vehicle(); } }
void ModeGuided::update() { switch (_guided_mode) { case Guided_WP: { if (!_reached_destination || rover.is_boat()) { // check if we've reached the destination _distance_to_destination = get_distance(rover.current_loc, _destination); if (!_reached_destination && (_distance_to_destination <= rover.g.waypoint_radius || location_passed_point(rover.current_loc, _origin, _destination))) { _reached_destination = true; rover.gcs().send_mission_item_reached_message(0); } // drive towards destination calc_steering_to_waypoint(_reached_destination ? rover.current_loc : _origin, _destination); calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true); } else { stop_vehicle(); } break; } case Guided_HeadingAndSpeed: { // stop vehicle if target not updated within 3 seconds if (have_attitude_target && (millis() - _des_att_time_ms) > 3000) { gcs().send_text(MAV_SEVERITY_WARNING, "target not received last 3secs, stopping"); have_attitude_target = false; } if (have_attitude_target) { // run steering and throttle controllers const float yaw_error = wrap_PI(radians((_desired_yaw_cd - ahrs.yaw_sensor) * 0.01f)); const float steering_out = attitude_control.get_steering_out_angle_error(yaw_error, g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right, _desired_speed < 0); g2.motors.set_steering(steering_out * 4500.0f); calc_throttle(_desired_speed, true); } else { stop_vehicle(); g2.motors.set_steering(0.0f); } break; } case Guided_TurnRateAndSpeed: { // stop vehicle if target not updated within 3 seconds if (have_attitude_target && (millis() - _des_att_time_ms) > 3000) { gcs().send_text(MAV_SEVERITY_WARNING, "target not received last 3secs, stopping"); have_attitude_target = false; } if (have_attitude_target) { // run steering and throttle controllers float steering_out = attitude_control.get_steering_out_rate(radians(_desired_yaw_rate_cds / 100.0f), g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right, _desired_speed < 0); g2.motors.set_steering(steering_out * 4500.0f); calc_throttle(_desired_speed, true); } else { stop_vehicle(); g2.motors.set_steering(0.0f); } break; } default: gcs().send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode"); break; } }
void ModeGuided::update() { switch (_guided_mode) { case Guided_WP: { _distance_to_destination = get_distance(rover.current_loc, _destination); const bool near_wp = _distance_to_destination <= rover.g.waypoint_radius; // check if we've reached the destination if (!_reached_destination && (near_wp || location_passed_point(rover.current_loc, _origin, _destination))) { _reached_destination = true; rover.gcs().send_mission_item_reached_message(0); } // determine if we should keep navigating if (!_reached_destination || (rover.is_boat() && !near_wp)) { // drive towards destination calc_steering_to_waypoint(_reached_destination ? rover.current_loc : _origin, _destination, _reversed); calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, true); } else { stop_vehicle(); } break; } case Guided_HeadingAndSpeed: { // stop vehicle if target not updated within 3 seconds if (have_attitude_target && (millis() - _des_att_time_ms) > 3000) { gcs().send_text(MAV_SEVERITY_WARNING, "target not received last 3secs, stopping"); have_attitude_target = false; } if (have_attitude_target) { // run steering and throttle controllers calc_steering_to_heading(_desired_yaw_cd, _desired_speed < 0); calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true, true); } else { stop_vehicle(); } break; } case Guided_TurnRateAndSpeed: { // stop vehicle if target not updated within 3 seconds if (have_attitude_target && (millis() - _des_att_time_ms) > 3000) { gcs().send_text(MAV_SEVERITY_WARNING, "target not received last 3secs, stopping"); have_attitude_target = false; } if (have_attitude_target) { // run steering and throttle controllers float steering_out = attitude_control.get_steering_out_rate(radians(_desired_yaw_rate_cds / 100.0f), g2.motors.limit.steer_left, g2.motors.limit.steer_right, rover.G_Dt); g2.motors.set_steering(steering_out * 4500.0f); calc_throttle(_desired_speed, true, true); } else { stop_vehicle(); } break; } default: gcs().send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode"); break; } }