ardupilot/APMrover2/mode_guided.cpp

296 lines
9.1 KiB
C++

#include "mode.h"
#include "Rover.h"
bool ModeGuided::_enter()
{
// set desired location to reasonable stopping point
if (!g2.wp_nav.set_desired_location_to_stopping_location()) {
return false;
}
_guided_mode = Guided_WP;
// initialise waypoint speed
g2.wp_nav.set_desired_speed_to_default();
sent_notification = false;
return true;
}
void ModeGuided::update()
{
switch (_guided_mode) {
case Guided_WP:
{
// check if we've reached the destination
if (!g2.wp_nav.reached_destination()) {
// update navigation controller
navigate_to_waypoint();
} else {
// send notification
if (!sent_notification) {
sent_notification = true;
rover.gcs().send_mission_item_reached_message(0);
}
// we have reached the destination so stay here
if (rover.is_boat()) {
if (!start_loiter()) {
stop_vehicle();
}
} else {
stop_vehicle();
}
// update distance to destination
_distance_to_destination = rover.current_loc.get_distance(g2.wp_nav.get_destination());
}
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);
calc_throttle(calc_speed_nudge(_desired_speed, is_negative(_desired_speed)), true);
} else {
// we have reached the destination so stay here
if (rover.is_boat()) {
if (!start_loiter()) {
stop_vehicle();
}
} 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);
set_steering(steering_out * 4500.0f);
calc_throttle(calc_speed_nudge(_desired_speed, is_negative(_desired_speed)), true);
} else {
// we have reached the destination so stay here
if (rover.is_boat()) {
if (!start_loiter()) {
stop_vehicle();
}
} else {
stop_vehicle();
}
}
break;
}
case Guided_Loiter:
{
rover.mode_loiter.update();
break;
}
default:
gcs().send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
break;
}
}
// return distance (in meters) to destination
float ModeGuided::get_distance_to_destination() const
{
switch (_guided_mode) {
case Guided_WP:
return _distance_to_destination;
case Guided_HeadingAndSpeed:
case Guided_TurnRateAndSpeed:
return 0.0f;
case Guided_Loiter:
return rover.mode_loiter.get_distance_to_destination();
}
// we should never reach here but just in case, return 0
return 0.0f;
}
// return true if vehicle has reached or even passed destination
bool ModeGuided::reached_destination() const
{
switch (_guided_mode) {
case Guided_WP:
return _reached_destination;
case Guided_HeadingAndSpeed:
case Guided_TurnRateAndSpeed:
case Guided_Loiter:
return true;
}
// we should never reach here but just in case, return true is the safer option
return true;
}
// set desired speed in m/s
bool ModeGuided::set_desired_speed(float speed)
{
switch (_guided_mode) {
case Guided_WP:
if (!is_negative(speed)) {
g2.wp_nav.set_desired_speed(speed);
return true;
}
return false;
case Guided_HeadingAndSpeed:
case Guided_TurnRateAndSpeed:
// speed is set from mavlink message
return false;
case Guided_Loiter:
return rover.mode_loiter.set_desired_speed(speed);
}
return false;
}
// get desired location
bool ModeGuided::get_desired_location(Location& destination) const
{
switch (_guided_mode) {
case Guided_WP:
if (g2.wp_nav.is_destination_valid()) {
destination = g2.wp_nav.get_oa_destination();
return true;
}
return false;
case Guided_HeadingAndSpeed:
case Guided_TurnRateAndSpeed:
// not supported in these submodes
return false;
case Guided_Loiter:
// get destination from loiter
return rover.mode_loiter.get_desired_location(destination);
}
// should never get here but just in case
return false;
}
// set desired location
bool ModeGuided::set_desired_location(const struct Location& destination,
float next_leg_bearing_cd)
{
if (g2.wp_nav.set_desired_location(destination, next_leg_bearing_cd)) {
// handle guided specific initialisation and logging
_guided_mode = ModeGuided::Guided_WP;
sent_notification = false;
rover.Log_Write_GuidedTarget(_guided_mode, Vector3f(destination.lat, destination.lng, 0), Vector3f(g2.wp_nav.get_desired_speed(), 0.0f, 0.0f));
return true;
}
return false;
}
// set desired attitude
void ModeGuided::set_desired_heading_and_speed(float yaw_angle_cd, float target_speed)
{
// initialisation and logging
_guided_mode = ModeGuided::Guided_HeadingAndSpeed;
_des_att_time_ms = AP_HAL::millis();
_reached_destination = false;
// record targets
_desired_yaw_cd = yaw_angle_cd;
_desired_speed = target_speed;
have_attitude_target = true;
// log new target
rover.Log_Write_GuidedTarget(_guided_mode, Vector3f(_desired_yaw_cd, 0.0f, 0.0f), Vector3f(_desired_speed, 0.0f, 0.0f));
}
void ModeGuided::set_desired_heading_delta_and_speed(float yaw_delta_cd, float target_speed)
{
// handle initialisation
if (_guided_mode != ModeGuided::Guided_HeadingAndSpeed) {
_guided_mode = ModeGuided::Guided_HeadingAndSpeed;
_desired_yaw_cd = ahrs.yaw_sensor;
}
set_desired_heading_and_speed(wrap_180_cd(_desired_yaw_cd + yaw_delta_cd), target_speed);
}
// set desired velocity
void ModeGuided::set_desired_turn_rate_and_speed(float turn_rate_cds, float target_speed)
{
// handle initialisation
_guided_mode = ModeGuided::Guided_TurnRateAndSpeed;
_des_att_time_ms = AP_HAL::millis();
_reached_destination = false;
// record targets
_desired_yaw_rate_cds = turn_rate_cds;
_desired_speed = target_speed;
have_attitude_target = true;
// log new target
rover.Log_Write_GuidedTarget(_guided_mode, Vector3f(_desired_yaw_rate_cds, 0.0f, 0.0f), Vector3f(_desired_speed, 0.0f, 0.0f));
}
bool ModeGuided::start_loiter()
{
if (rover.mode_loiter.enter()) {
_guided_mode = Guided_Loiter;
return true;
}
return false;
}
// set guided timeout and movement limits
void ModeGuided::limit_set(uint32_t timeout_ms, float horiz_max)
{
limit.timeout_ms = timeout_ms;
limit.horiz_max = horiz_max;
}
// clear/turn off guided limits
void ModeGuided::limit_clear()
{
limit.timeout_ms = 0;
limit.horiz_max = 0.0f;
}
// initialise guided start time and location as reference for limit checking
// only called from AUTO mode's start_guided method
void ModeGuided::limit_init_time_and_location()
{
limit.start_time_ms = AP_HAL::millis();
limit.start_loc = rover.current_loc;
}
// returns true if guided mode has breached a limit
bool ModeGuided::limit_breached() const
{
// check if we have passed the timeout
if ((limit.timeout_ms > 0) && (millis() - limit.start_time_ms >= limit.timeout_ms)) {
return true;
}
// check if we have gone beyond horizontal limit
if (is_positive(limit.horiz_max)) {
return (rover.current_loc.get_distance(limit.start_loc) > limit.horiz_max);
}
// if we got this far we must be within limits
return false;
}