mirror of https://github.com/ArduPilot/ardupilot
Rover: Loiter in AUTO mode for boat
This commit is contained in:
parent
5575eeddab
commit
1e033e616f
|
@ -33,7 +33,6 @@ Rover::Rover(void) :
|
|||
nav_controller(&L1_controller),
|
||||
control_mode(&mode_initializing),
|
||||
home(ahrs.get_home()),
|
||||
G_Dt(0.02f),
|
||||
mode_auto(mode_rtl)
|
||||
G_Dt(0.02f)
|
||||
{
|
||||
}
|
||||
|
|
|
@ -244,9 +244,6 @@ class ModeAuto : public Mode
|
|||
{
|
||||
public:
|
||||
|
||||
// constructor
|
||||
ModeAuto(ModeRTL& mode_rtl);
|
||||
|
||||
uint32_t mode_number() const override { return AUTO; }
|
||||
const char *name4() const override { return "AUTO"; }
|
||||
|
||||
|
@ -284,16 +281,14 @@ protected:
|
|||
enum AutoSubMode {
|
||||
Auto_WP, // drive to a given location
|
||||
Auto_HeadingAndSpeed, // turn to a given heading
|
||||
Auto_RTL // perform RTL within auto mode
|
||||
Auto_RTL, // perform RTL within auto mode
|
||||
Auto_Loiter // perform Loiter within auto mode
|
||||
} _submode;
|
||||
|
||||
private:
|
||||
|
||||
bool check_trigger(void);
|
||||
|
||||
// references
|
||||
ModeRTL& _mode_rtl;
|
||||
|
||||
// this is set to true when auto has been triggered to start
|
||||
bool auto_triggered;
|
||||
|
||||
|
@ -320,6 +315,8 @@ private:
|
|||
void do_set_home(const AP_Mission::Mission_Command& cmd);
|
||||
void do_set_reverse(const AP_Mission::Mission_Command& cmd);
|
||||
|
||||
bool start_loiter();
|
||||
|
||||
enum Mis_Done_Behave {
|
||||
MIS_DONE_BEHAVE_HOLD = 0,
|
||||
MIS_DONE_BEHAVE_LOITER = 1
|
||||
|
|
|
@ -1,12 +1,6 @@
|
|||
#include "mode.h"
|
||||
#include "Rover.h"
|
||||
|
||||
// constructor
|
||||
ModeAuto::ModeAuto(ModeRTL& mode_rtl) :
|
||||
_mode_rtl(mode_rtl)
|
||||
{
|
||||
}
|
||||
|
||||
bool ModeAuto::_enter()
|
||||
{
|
||||
// fail to enter auto if no mission commands
|
||||
|
@ -50,13 +44,19 @@ void ModeAuto::update()
|
|||
_reached_destination = true;
|
||||
}
|
||||
// determine if we should keep navigating
|
||||
if (!_reached_destination || (rover.is_boat() && !near_wp)) {
|
||||
if (!_reached_destination) {
|
||||
// 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 have reached the destination so stop
|
||||
stop_vehicle();
|
||||
// we have reached the destination so stay here
|
||||
if (rover.is_boat()) {
|
||||
if (!start_loiter()) {
|
||||
stop_vehicle();
|
||||
}
|
||||
} else {
|
||||
stop_vehicle();
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
@ -70,13 +70,24 @@ void ModeAuto::update()
|
|||
// check if we have reached within 5 degrees of target
|
||||
_reached_heading = (fabsf(_desired_yaw_cd - ahrs.yaw_sensor) < 500);
|
||||
} else {
|
||||
stop_vehicle();
|
||||
// we have reached the destination so stay here
|
||||
if (rover.is_boat()) {
|
||||
if (!start_loiter()) {
|
||||
stop_vehicle();
|
||||
}
|
||||
} else {
|
||||
stop_vehicle();
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case Auto_RTL:
|
||||
_mode_rtl.update();
|
||||
rover.mode_rtl.update();
|
||||
break;
|
||||
|
||||
case Auto_Loiter:
|
||||
rover.mode_loiter.update();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
@ -85,7 +96,7 @@ void ModeAuto::update()
|
|||
float ModeAuto::get_distance_to_destination() const
|
||||
{
|
||||
if (_submode == Auto_RTL) {
|
||||
return _mode_rtl.get_distance_to_destination();
|
||||
return rover.mode_rtl.get_distance_to_destination();
|
||||
}
|
||||
return _distance_to_destination;
|
||||
}
|
||||
|
@ -106,7 +117,7 @@ bool ModeAuto::reached_destination()
|
|||
return _reached_destination;
|
||||
}
|
||||
if (_submode == Auto_RTL) {
|
||||
return _mode_rtl.reached_destination();
|
||||
return rover.mode_rtl.reached_destination();
|
||||
}
|
||||
// we should never reach here but just in case, return true to allow missions to continue
|
||||
return true;
|
||||
|
@ -135,7 +146,7 @@ bool ModeAuto::reached_heading()
|
|||
// start RTL (within auto)
|
||||
void ModeAuto::start_RTL()
|
||||
{
|
||||
if (_mode_rtl.enter()) {
|
||||
if (rover.mode_rtl.enter()) {
|
||||
_submode = Auto_RTL;
|
||||
}
|
||||
}
|
||||
|
@ -194,3 +205,12 @@ void ModeAuto::calc_throttle(float target_speed, bool nudge_allowed, bool avoida
|
|||
}
|
||||
Mode::calc_throttle(target_speed, nudge_allowed, avoidance_enabled);
|
||||
}
|
||||
|
||||
bool ModeAuto::start_loiter()
|
||||
{
|
||||
if (rover.mode_loiter.enter()) {
|
||||
_submode = Auto_Loiter;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue