Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: guided starts in stop submode

This commit is contained in:
Randy Mackay 2021-12-02 11:34:51 +09:00
parent a0ef7a1476
commit dd6ce1fa73
2 changed files with 27 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
Rover/mode.h
View File

@ -407,6 +407,9 @@ public:
// vehicle start loiter // vehicle start loiter
bool start_loiter(); bool start_loiter();
// start stopping
void start_stop();
// guided limits // guided limits
void limit_set(uint32_t timeout_ms, float horiz_max); void limit_set(uint32_t timeout_ms, float horiz_max);
void limit_clear(); void limit_clear();
@ -420,7 +423,8 @@ protected:
Guided_HeadingAndSpeed, Guided_HeadingAndSpeed,
Guided_TurnRateAndSpeed, Guided_TurnRateAndSpeed,
Guided_Loiter, Guided_Loiter,
Guided_SteeringAndThrottle Guided_SteeringAndThrottle,
Guided_Stop
}; };
bool _enter() override; bool _enter() override;

26
Rover/mode_guided.cpp
View File

@ -3,11 +3,14 @@
bool ModeGuided::_enter() bool ModeGuided::_enter()
{ {
// set desired location to reasonable stopping point // initialise submode to stop or loiter
if (!g2.wp_nav.set_desired_location_to_stopping_location()) { if (rover.is_boat()) {
return false; if (!start_loiter()) {
start_stop();
}
} else {
start_stop();
} }
_guided_mode = Guided_WP;
// initialise waypoint speed // initialise waypoint speed
g2.wp_nav.set_desired_speed_to_default(); g2.wp_nav.set_desired_speed_to_default();
@ -129,6 +132,10 @@ void ModeGuided::update()
break; break;
} }
case Guided_Stop:
stop_vehicle();
break;
default: default:
gcs().send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode"); gcs().send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
break; break;
@ -147,6 +154,7 @@ float ModeGuided::get_distance_to_destination() const
case Guided_Loiter: case Guided_Loiter:
return rover.mode_loiter.get_distance_to_destination(); return rover.mode_loiter.get_distance_to_destination();
case Guided_SteeringAndThrottle: case Guided_SteeringAndThrottle:
case Guided_Stop:
return 0.0f; return 0.0f;
} }
@ -164,6 +172,7 @@ bool ModeGuided::reached_destination() const
case Guided_TurnRateAndSpeed: case Guided_TurnRateAndSpeed:
case Guided_Loiter: case Guided_Loiter:
case Guided_SteeringAndThrottle: case Guided_SteeringAndThrottle:
case Guided_Stop:
return true; return true;
} }
@ -188,6 +197,7 @@ bool ModeGuided::set_desired_speed(float speed)
case Guided_Loiter: case Guided_Loiter:
return rover.mode_loiter.set_desired_speed(speed); return rover.mode_loiter.set_desired_speed(speed);
case Guided_SteeringAndThrottle: case Guided_SteeringAndThrottle:
case Guided_Stop:
// no speed control // no speed control
return false; return false;
} }
@ -212,6 +222,7 @@ bool ModeGuided::get_desired_location(Location& destination) const
// get destination from loiter // get destination from loiter
return rover.mode_loiter.get_desired_location(destination); return rover.mode_loiter.get_desired_location(destination);
case Guided_SteeringAndThrottle: case Guided_SteeringAndThrottle:
case Guided_Stop:
// no desired location in this submode // no desired location in this submode
break; break;
} }
@ -298,6 +309,13 @@ bool ModeGuided::start_loiter()
return false; return false;
} }
// start stopping vehicle as quickly as possible
void ModeGuided::start_stop()
{
_guided_mode = Guided_Stop;
}
// set guided timeout and movement limits // set guided timeout and movement limits
void ModeGuided::limit_set(uint32_t timeout_ms, float horiz_max) void ModeGuided::limit_set(uint32_t timeout_ms, float horiz_max)
{ {