diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index 5f5d292aa1..59c73da27d 100644 --- a/APMrover2/Parameters.cpp +++ b/APMrover2/Parameters.cpp @@ -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), diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index 79a275582a..7bce6fc466 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -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; diff --git a/APMrover2/control_modes.cpp b/APMrover2/control_modes.cpp index bd8393c64a..2f9a21e349 100644 --- a/APMrover2/control_modes.cpp +++ b/APMrover2/control_modes.cpp @@ -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; diff --git a/APMrover2/defines.h b/APMrover2/defines.h index 75fd6b5ebb..5e3a44ad1a 100644 --- a/APMrover2/defines.h +++ b/APMrover2/defines.h @@ -40,6 +40,7 @@ enum mode { ACRO = 1, STEERING = 3, HOLD = 4, + LOITER = 5, AUTO = 10, RTL = 11, SMART_RTL = 12, diff --git a/APMrover2/mode.h b/APMrover2/mode.h index c88ddfe36d..59ce3d29bf 100644 --- a/APMrover2/mode.h +++ b/APMrover2/mode.h @@ -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 { diff --git a/APMrover2/mode_loiter.cpp b/APMrover2/mode_loiter.cpp new file mode 100644 index 0000000000..afa3fb10b8 --- /dev/null +++ b/APMrover2/mode_loiter.cpp @@ -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); +}