Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 20:48:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: Loiter in AUTO mode for boat

Browse Source
This commit is contained in:
TsuyoshiKawamura 2019-01-07 03:21:57 +09:00 committed by Randy Mackay
parent 5575eeddab
commit 1e033e616f
3 changed files with 39 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
APMrover2/Rover.cpp
View File

@ -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)
{
}

11
APMrover2/mode.h
View File

@ -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

48
APMrover2/mode_auto.cpp
View File

@ -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;
}