2017-07-18 23:17:45 -03:00
|
|
|
#include "mode.h"
|
|
|
|
#include "Rover.h"
|
|
|
|
|
|
|
|
bool ModeGuided::_enter()
|
|
|
|
{
|
2019-04-29 03:31:45 -03:00
|
|
|
// set desired location to reasonable stopping point
|
|
|
|
if (!g2.wp_nav.set_desired_location_to_stopping_location()) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
_guided_mode = Guided_WP;
|
|
|
|
|
2017-12-05 21:41:28 -04:00
|
|
|
// initialise waypoint speed
|
2019-04-29 03:31:45 -03:00
|
|
|
g2.wp_nav.set_desired_speed_to_default();
|
2017-12-05 21:41:28 -04:00
|
|
|
|
2019-04-29 03:31:45 -03:00
|
|
|
sent_notification = false;
|
2017-08-03 03:19:57 -03:00
|
|
|
|
2017-07-18 23:17:45 -03:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
void ModeGuided::update()
|
|
|
|
{
|
2017-08-03 05:14:16 -03:00
|
|
|
switch (_guided_mode) {
|
2017-07-18 23:17:45 -03:00
|
|
|
case Guided_WP:
|
2017-08-03 05:14:16 -03:00
|
|
|
{
|
2018-02-05 02:39:48 -04:00
|
|
|
// check if we've reached the destination
|
2019-04-29 03:31:45 -03:00
|
|
|
if (!g2.wp_nav.reached_destination()) {
|
|
|
|
// update navigation controller
|
|
|
|
navigate_to_waypoint();
|
2017-08-03 05:14:16 -03:00
|
|
|
} else {
|
2019-04-29 03:31:45 -03:00
|
|
|
// send notification
|
|
|
|
if (!sent_notification) {
|
|
|
|
sent_notification = true;
|
|
|
|
rover.gcs().send_mission_item_reached_message(0);
|
|
|
|
}
|
|
|
|
|
2019-01-10 06:46:43 -04:00
|
|
|
// we have reached the destination so stay here
|
|
|
|
if (rover.is_boat()) {
|
|
|
|
if (!start_loiter()) {
|
|
|
|
stop_vehicle();
|
|
|
|
}
|
2018-12-03 15:25:52 -04:00
|
|
|
} else {
|
|
|
|
stop_vehicle();
|
|
|
|
}
|
2019-04-29 03:31:45 -03:00
|
|
|
// update distance to destination
|
|
|
|
_distance_to_destination = rover.current_loc.get_distance(g2.wp_nav.get_destination());
|
2017-08-03 05:14:16 -03:00
|
|
|
}
|
|
|
|
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
|
2018-09-10 04:25:15 -03:00
|
|
|
calc_steering_to_heading(_desired_yaw_cd);
|
2019-05-04 00:09:24 -03:00
|
|
|
calc_throttle(calc_speed_nudge(_desired_speed, is_negative(_desired_speed)), true);
|
2017-07-18 23:17:45 -03:00
|
|
|
} else {
|
2019-01-10 06:46:43 -04:00
|
|
|
// we have reached the destination so stay here
|
|
|
|
if (rover.is_boat()) {
|
|
|
|
if (!start_loiter()) {
|
|
|
|
stop_vehicle();
|
|
|
|
}
|
2018-12-03 15:25:52 -04:00
|
|
|
} else {
|
|
|
|
stop_vehicle();
|
|
|
|
}
|
2017-07-18 23:17:45 -03:00
|
|
|
}
|
|
|
|
break;
|
2017-08-03 05:14:16 -03:00
|
|
|
}
|
2017-07-18 23:17:45 -03:00
|
|
|
|
2017-08-03 05:14:16 -03:00
|
|
|
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
|
2018-04-09 09:12:07 -03:00
|
|
|
float steering_out = attitude_control.get_steering_out_rate(radians(_desired_yaw_rate_cds / 100.0f),
|
|
|
|
g2.motors.limit.steer_left,
|
2018-05-21 22:05:20 -03:00
|
|
|
g2.motors.limit.steer_right,
|
|
|
|
rover.G_Dt);
|
2019-04-20 20:02:51 -03:00
|
|
|
set_steering(steering_out * 4500.0f);
|
2019-05-04 00:09:24 -03:00
|
|
|
calc_throttle(calc_speed_nudge(_desired_speed, is_negative(_desired_speed)), true);
|
2017-08-03 05:14:16 -03:00
|
|
|
} else {
|
2019-01-10 06:46:43 -04:00
|
|
|
// we have reached the destination so stay here
|
|
|
|
if (rover.is_boat()) {
|
|
|
|
if (!start_loiter()) {
|
|
|
|
stop_vehicle();
|
|
|
|
}
|
2018-12-03 15:25:52 -04:00
|
|
|
} else {
|
|
|
|
stop_vehicle();
|
|
|
|
}
|
2017-08-03 05:14:16 -03:00
|
|
|
}
|
2017-07-18 23:17:45 -03:00
|
|
|
break;
|
2017-08-03 05:14:16 -03:00
|
|
|
}
|
2017-07-18 23:17:45 -03:00
|
|
|
|
2018-12-03 15:25:52 -04:00
|
|
|
case Guided_Loiter:
|
|
|
|
{
|
|
|
|
rover.mode_loiter.update();
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
2017-07-18 23:17:45 -03:00
|
|
|
default:
|
|
|
|
gcs().send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2017-08-03 05:14:16 -03:00
|
|
|
// return distance (in meters) to destination
|
|
|
|
float ModeGuided::get_distance_to_destination() const
|
2017-07-18 23:17:45 -03:00
|
|
|
{
|
2019-04-29 03:31:45 -03:00
|
|
|
switch (_guided_mode) {
|
|
|
|
case Guided_WP:
|
|
|
|
return _distance_to_destination;
|
|
|
|
case Guided_HeadingAndSpeed:
|
|
|
|
case Guided_TurnRateAndSpeed:
|
2017-08-03 05:14:16 -03:00
|
|
|
return 0.0f;
|
2019-04-29 03:31:45 -03:00
|
|
|
case Guided_Loiter:
|
|
|
|
rover.mode_loiter.get_distance_to_destination();
|
|
|
|
break;
|
2017-07-18 23:17:45 -03:00
|
|
|
}
|
2019-04-29 03:31:45 -03:00
|
|
|
|
|
|
|
// we should never reach here but just in case, return 0
|
|
|
|
return 0.0f;
|
2017-08-03 05:14:16 -03:00
|
|
|
}
|
|
|
|
|
2019-03-08 01:41:50 -04:00
|
|
|
// return true if vehicle has reached or even passed destination
|
2019-03-15 01:26:01 -03:00
|
|
|
bool ModeGuided::reached_destination() const
|
2019-03-08 01:41:50 -04:00
|
|
|
{
|
|
|
|
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;
|
|
|
|
}
|
|
|
|
|
2017-08-03 05:14:16 -03:00
|
|
|
// set desired location
|
2018-11-07 07:08:18 -04:00
|
|
|
void ModeGuided::set_desired_location(const struct Location& destination,
|
|
|
|
float next_leg_bearing_cd)
|
2017-08-03 05:14:16 -03:00
|
|
|
{
|
2019-04-29 03:31:45 -03:00
|
|
|
if (g2.wp_nav.set_desired_location(destination, next_leg_bearing_cd)) {
|
2017-08-03 05:14:16 -03:00
|
|
|
|
2019-04-29 03:31:45 -03:00
|
|
|
// handle guided specific initialisation and logging
|
|
|
|
_guided_mode = ModeGuided::Guided_WP;
|
|
|
|
rover.Log_Write_GuidedTarget(_guided_mode, Vector3f(destination.lat, destination.lng, 0), Vector3f(g2.wp_nav.get_desired_speed(), 0.0f, 0.0f));
|
|
|
|
}
|
2017-08-03 05:14:16 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// set desired attitude
|
|
|
|
void ModeGuided::set_desired_heading_and_speed(float yaw_angle_cd, float target_speed)
|
|
|
|
{
|
|
|
|
// call parent
|
|
|
|
Mode::set_desired_heading_and_speed(yaw_angle_cd, target_speed);
|
|
|
|
|
|
|
|
// handle guided specific 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;
|
|
|
|
}
|
2017-08-03 06:41:47 -03:00
|
|
|
set_desired_heading_and_speed(wrap_180_cd(_desired_yaw_cd + yaw_delta_cd), target_speed);
|
2017-08-03 05:14:16 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// 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));
|
2017-07-18 23:17:45 -03:00
|
|
|
}
|
2018-12-03 15:25:52 -04:00
|
|
|
|
|
|
|
bool ModeGuided::start_loiter()
|
|
|
|
{
|
|
|
|
if (rover.mode_loiter.enter()) {
|
|
|
|
_guided_mode = Guided_Loiter;
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
return false;
|
|
|
|
}
|
2019-03-08 04:53:22 -04:00
|
|
|
|
|
|
|
// 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()
|
|
|
|
{
|
2019-03-15 03:00:31 -03:00
|
|
|
limit.start_time_ms = AP_HAL::millis();
|
2019-03-08 04:53:22 -04:00
|
|
|
limit.start_loc = rover.current_loc;
|
|
|
|
}
|
|
|
|
|
|
|
|
// returns true if guided mode has breached a limit
|
2019-03-15 01:22:36 -03:00
|
|
|
bool ModeGuided::limit_breached() const
|
2019-03-08 04:53:22 -04:00
|
|
|
{
|
|
|
|
// check if we have passed the timeout
|
2019-03-15 03:00:31 -03:00
|
|
|
if ((limit.timeout_ms > 0) && (millis() - limit.start_time_ms >= limit.timeout_ms)) {
|
2019-03-08 04:53:22 -04:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
// check if we have gone beyond horizontal limit
|
2019-03-15 01:22:36 -03:00
|
|
|
if (is_positive(limit.horiz_max)) {
|
2019-03-08 04:53:22 -04:00
|
|
|
return (rover.current_loc.get_distance(limit.start_loc) > limit.horiz_max);
|
|
|
|
}
|
|
|
|
|
|
|
|
// if we got this far we must be within limits
|
|
|
|
return false;
|
|
|
|
}
|