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

Rover: Loiter mode for boats

This commit is contained in:
Randy Mackay 2018-04-28 15:31:31 +09:00
parent ce44326ba8
commit c6638f66de
6 changed files with 94 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
APMrover2/Parameters.cpp
View File

@ -241,7 +241,7 @@ const AP_Param::Info Rover::var_info[] = {
// @Param: MODE1
// @DisplayName: Mode1
// @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,12:SmartRTL,15:Guided
// @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,10:Auto,11:RTL,12:SmartRTL,15:Guided
// @User: Standard
// @Description: Driving mode for switch position 1 (910 to 1230 and above 2049)
GSCALAR(mode1, "MODE1", MANUAL),
@ -249,35 +249,35 @@ const AP_Param::Info Rover::var_info[] = {
// @Param: MODE2
// @DisplayName: Mode2
// @Description: Driving mode for switch position 2 (1231 to 1360)
// @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,12:SmartRTL,15:Guided
// @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,10:Auto,11:RTL,12:SmartRTL,15:Guided
// @User: Standard
GSCALAR(mode2, "MODE2", MANUAL),
// @Param: MODE3
// @DisplayName: Mode3
// @Description: Driving mode for switch position 3 (1361 to 1490)
// @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,12:SmartRTL,15:Guided
// @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,10:Auto,11:RTL,12:SmartRTL,15:Guided
// @User: Standard
GSCALAR(mode3, "MODE3", MANUAL),
// @Param: MODE4
// @DisplayName: Mode4
// @Description: Driving mode for switch position 4 (1491 to 1620)
// @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,12:SmartRTL,15:Guided
// @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,10:Auto,11:RTL,12:SmartRTL,15:Guided
// @User: Standard
GSCALAR(mode4, "MODE4", MANUAL),
// @Param: MODE5
// @DisplayName: Mode5
// @Description: Driving mode for switch position 5 (1621 to 1749)
// @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,12:SmartRTL,15:Guided
// @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,10:Auto,11:RTL,12:SmartRTL,15:Guided
// @User: Standard
GSCALAR(mode5, "MODE5", MANUAL),
// @Param: MODE6
// @DisplayName: Mode6
// @Description: Driving mode for switch position 6 (1750 to 2049)
// @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,12:SmartRTL,15:Guided
// @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,10:Auto,11:RTL,12:SmartRTL,15:Guided
// @User: Standard
GSCALAR(mode6, "MODE6", MANUAL),

2
APMrover2/Rover.h
View File

@ -113,6 +113,7 @@ public:
friend class ModeAuto;
friend class ModeGuided;
friend class ModeHold;
friend class ModeLoiter;
friend class ModeSteering;
friend class ModeManual;
friend class ModeRTL;
@ -372,6 +373,7 @@ private:
ModeAcro mode_acro;
ModeGuided mode_guided;
ModeAuto mode_auto;
ModeLoiter mode_loiter;
ModeSteering mode_steering;
ModeRTL mode_rtl;
ModeSmartRTL mode_smartrtl;

3
APMrover2/control_modes.cpp
View File

@ -18,6 +18,9 @@ Mode *Rover::mode_from_mode_num(const enum mode num)
case HOLD:
ret = &mode_hold;
break;
case LOITER:
ret = &mode_loiter;
break;
case AUTO:
ret = &mode_auto;
break;

1
APMrover2/defines.h
View File

@ -40,6 +40,7 @@ enum mode {
ACRO = 1,
STEERING = 3,
HOLD = 4,
LOITER = 5,
AUTO = 10,
RTL = 11,
SMART_RTL = 12,

17
APMrover2/mode.h
View File

@ -303,6 +303,23 @@ public:
bool requires_velocity() const override { return false; }
};
class ModeLoiter : public Mode
{
public:
uint32_t mode_number() const override { return LOITER; }
const char *name4() const override { return "LOIT"; }
// methods that affect movement of the vehicle in this mode
void update() override;
// return distance (in meters) to destination
float get_distance_to_destination() const override { return _distance_to_destination; }
protected:
bool _enter() override;
};
class ModeManual : public Mode
{

65
APMrover2/mode_loiter.cpp Normal file
View File

@ -0,0 +1,65 @@
#include "mode.h"
#include "Rover.h"
bool ModeLoiter::_enter()
{
// set _destination to reasonable stopping point
calc_stopping_location(_destination);
// initialise desired speed to current speed
if (!attitude_control.get_forward_speed(_desired_speed)) {
_desired_speed = 0.0f;
}
// initialise heading to current heading
_desired_yaw_cd = ahrs.yaw_sensor;
_yaw_error_cd = 0.0f;
// set reversed based on speed
rover.set_reverse(is_negative(_desired_speed));
return true;
}
void ModeLoiter::update()
{
// get distance (in meters) to destination
_distance_to_destination = get_distance(rover.current_loc, _destination);
// if within waypoint radius slew desired speed towards zero and use existing desired heading
if (_distance_to_destination <= g.waypoint_radius) {
if (is_negative(_desired_speed)) {
_desired_speed = MIN(_desired_speed + attitude_control.get_accel_max() * rover.G_Dt, 0.0f);
} else {
_desired_speed = MAX(_desired_speed - attitude_control.get_accel_max() * rover.G_Dt, 0.0f);
}
_yaw_error_cd = 0.0f;
} else {
// P controller with hard-coded gain to convert distance to desired speed
// To-Do: make gain configurable or calculate from attitude controller's maximum accelearation
_desired_speed = MIN((_distance_to_destination - g.waypoint_radius) * 0.5f, g.speed_cruise);
// calculate bearing to destination
_desired_yaw_cd = get_bearing_cd(rover.current_loc, _destination);
_yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
// if destination is behind vehicle, reverse towards it
if (fabsf(_yaw_error_cd) > 9000) {
_desired_yaw_cd = wrap_180_cd(_desired_yaw_cd + 18000);
_yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
_desired_speed = -_desired_speed;
}
// reduce desired speed if yaw_error is large
// note: copied from calc_reduced_speed_for_turn_or_distance
float yaw_error_ratio = 1.0f - constrain_float(fabsf(_yaw_error_cd / 9000.0f), 0.0f, 1.0f) * ((100.0f - g.speed_turn_gain) * 0.01f);
_desired_speed *= yaw_error_ratio;
}
// run steering and throttle controllers
calc_steering_to_heading(_desired_yaw_cd, _desired_speed < 0);
calc_throttle(_desired_speed, false, true);
// mark us as in_reverse when using a negative throttle
// To-Do: only in reverse if vehicle is actually travelling backwards?
rover.set_reverse(_desired_speed < 0);
}