From c0082271e63c8a557de1a9bf8d48a9a143dc2feb Mon Sep 17 00:00:00 2001 From: Ammarf <35584084+Ammarf@users.noreply.github.com> Date: Thu, 24 May 2018 13:47:07 +0900 Subject: [PATCH] Rover: implement Follow mode --- APMrover2/Parameters.cpp | 22 ++++++++------ APMrover2/Parameters.h | 3 ++ APMrover2/Rover.h | 3 ++ APMrover2/control_modes.cpp | 9 ++++++ APMrover2/defines.h | 3 +- APMrover2/make.inc | 1 + APMrover2/mode.h | 17 +++++++++++ APMrover2/mode_follow.cpp | 57 +++++++++++++++++++++++++++++++++++++ APMrover2/wscript | 1 + 9 files changed, 106 insertions(+), 10 deletions(-) create mode 100644 APMrover2/mode_follow.cpp diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index 7bdc54c028..cbf1a96cff 100644 --- a/APMrover2/Parameters.cpp +++ b/APMrover2/Parameters.cpp @@ -33,7 +33,7 @@ const AP_Param::Info Rover::var_info[] = { // @Param: INITIAL_MODE // @DisplayName: Initial driving mode // @Description: This selects the mode to start in on boot. This is useful for when you want to start in AUTO mode on boot without a receiver. Usually used in combination with when AUTO_TRIGGER_PIN or AUTO_KICKSTART. - // @Values: 0:MANUAL,1:ACRO,3:STEERING,4:HOLD,5:LOITER,10:AUTO,11:RTL,15:GUIDED + // @Values: 0:MANUAL,1:ACRO,3:STEERING,4:HOLD,5:LOITER,6:FOLLOW,10:AUTO,11:RTL,15:GUIDED // @User: Advanced GSCALAR(initial_mode, "INITIAL_MODE", Mode::Number::MANUAL), @@ -112,7 +112,7 @@ const AP_Param::Info Rover::var_info[] = { // @Param: CH7_OPTION // @DisplayName: Channel 7 option // @Description: What to do use channel 7 for - // @Values: 0:Nothing,1:SaveWaypoint,2:LearnCruiseSpeed,3:ArmDisarm,4:Manual,5:Acro,6:Steering,7:Hold,8:Auto,9:RTL,10:SmartRTL,11:Guided,12:Loiter + // @Values: 0:Nothing,1:SaveWaypoint,2:LearnCruiseSpeed,3:ArmDisarm,4:Manual,5:Acro,6:Steering,7:Hold,8:Auto,9:RTL,10:SmartRTL,11:Guided,12:Loiter,13:Follow // @User: Standard GSCALAR(ch7_option, "CH7_OPTION", CH7_OPTION), @@ -224,7 +224,7 @@ const AP_Param::Info Rover::var_info[] = { // @Param: MODE1 // @DisplayName: Mode1 - // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,10:Auto,11:RTL,12:SmartRTL,15:Guided + // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:FOLLOW,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", Mode::Number::MANUAL), @@ -232,35 +232,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,5:Loiter,10:Auto,11:RTL,12:SmartRTL,15:Guided + // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:FOLLOW,10:Auto,11:RTL,12:SmartRTL,15:Guided // @User: Standard GSCALAR(mode2, "MODE2", Mode::Number::MANUAL), // @Param: MODE3 // @DisplayName: Mode3 // @Description: Driving mode for switch position 3 (1361 to 1490) - // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,10:Auto,11:RTL,12:SmartRTL,15:Guided + // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:FOLLOW,10:Auto,11:RTL,12:SmartRTL,15:Guided // @User: Standard GSCALAR(mode3, "MODE3", Mode::Number::MANUAL), // @Param: MODE4 // @DisplayName: Mode4 // @Description: Driving mode for switch position 4 (1491 to 1620) - // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,10:Auto,11:RTL,12:SmartRTL,15:Guided + // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:FOLLOW,10:Auto,11:RTL,12:SmartRTL,15:Guided // @User: Standard GSCALAR(mode4, "MODE4", Mode::Number::MANUAL), // @Param: MODE5 // @DisplayName: Mode5 // @Description: Driving mode for switch position 5 (1621 to 1749) - // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,10:Auto,11:RTL,12:SmartRTL,15:Guided + // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:FOLLOW,10:Auto,11:RTL,12:SmartRTL,15:Guided // @User: Standard GSCALAR(mode5, "MODE5", Mode::Number::MANUAL), // @Param: MODE6 // @DisplayName: Mode6 // @Description: Driving mode for switch position 6 (1750 to 2049) - // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,10:Auto,11:RTL,12:SmartRTL,15:Guided + // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:FOLLOW,10:Auto,11:RTL,12:SmartRTL,15:Guided // @User: Standard GSCALAR(mode6, "MODE6", Mode::Number::MANUAL), @@ -565,6 +565,9 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Standard AP_GROUPINFO("CRASH_ANGLE", 22, ParametersG2, crash_angle, 0), + // @Group: FOLL + // @Path: ../libraries/AP_Follow/AP_Follow.cpp + AP_SUBGROUPINFO(follow, "FOLL", 23, ParametersG2, AP_Follow), AP_GROUPEND }; @@ -581,7 +584,8 @@ ParametersG2::ParametersG2(void) smart_rtl(), fence(rover.ahrs), proximity(rover.serial_manager), - avoid(rover.ahrs, fence, rover.g2.proximity, &rover.g2.beacon) + avoid(rover.ahrs, fence, rover.g2.proximity, &rover.g2.beacon), + follow() { AP_Param::setup_object_defaults(this, var_info); } diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index 8f55458352..ca2303b875 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -347,6 +347,9 @@ public: // pitch/roll angle for crash check AP_Int8 crash_angle; + + // follow mode library + AP_Follow follow; }; extern const AP_Param::Info var_info[]; diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index 6668390bb5..67542fb249 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -79,6 +79,7 @@ #include #include #include +#include #if CONFIG_HAL_BOARD == HAL_BOARD_SITL #include #endif @@ -117,6 +118,7 @@ public: friend class ModeManual; friend class ModeRTL; friend class ModeSmartRTL; + friend class ModeFollow; Rover(void); @@ -364,6 +366,7 @@ private: ModeSteering mode_steering; ModeRTL mode_rtl; ModeSmartRTL mode_smartrtl; + ModeFollow mode_follow; // cruise throttle and speed learning struct { diff --git a/APMrover2/control_modes.cpp b/APMrover2/control_modes.cpp index 11effb2b55..05075090f5 100644 --- a/APMrover2/control_modes.cpp +++ b/APMrover2/control_modes.cpp @@ -33,6 +33,9 @@ Mode *Rover::mode_from_mode_num(const enum Mode::Number num) case Mode::Number::GUIDED: ret = &mode_guided; break; + case Mode::Number::FOLLOW: + ret = &mode_follow; + break; case Mode::Number::INITIALISING: ret = &mode_initializing; break; @@ -263,5 +266,11 @@ void Rover::read_aux_switch() case CH7_LOITER: do_aux_function_change_mode(rover.mode_loiter, aux_ch7); break; + + // Set mode to Follow + case CH7_FOLLOW: + do_aux_function_change_mode(rover.mode_follow, aux_ch7); + break; + } } diff --git a/APMrover2/defines.h b/APMrover2/defines.h index e3a5080709..bdb0664069 100644 --- a/APMrover2/defines.h +++ b/APMrover2/defines.h @@ -27,7 +27,8 @@ enum ch7_option { CH7_RTL = 9, CH7_SMART_RTL = 10, CH7_GUIDED = 11, - CH7_LOITER = 12 + CH7_LOITER = 12, + CH7_FOLLOW = 13 }; // HIL enumerations diff --git a/APMrover2/make.inc b/APMrover2/make.inc index e6fc3c9e43..cc4ec05da5 100644 --- a/APMrover2/make.inc +++ b/APMrover2/make.inc @@ -50,3 +50,4 @@ LIBRARIES += AP_SmartRTL LIBRARIES += AC_Avoidance LIBRARIES += AC_AttitudeControl LIBRARIES += AP_Devo_Telem +LIBRARIES += AP_Follow diff --git a/APMrover2/mode.h b/APMrover2/mode.h index 78e5a4bde7..3b87bfa9fa 100644 --- a/APMrover2/mode.h +++ b/APMrover2/mode.h @@ -24,6 +24,7 @@ public: STEERING = 3, HOLD = 4, LOITER = 5, + FOLLOW = 6, AUTO = 10, RTL = 11, SMART_RTL = 12, @@ -465,3 +466,19 @@ public: bool has_manual_input() const override { return true; } bool attitude_stabilized() const override { return false; } }; + +class ModeFollow : public ModeGuided +{ +public: + + uint32_t mode_number() const override { return FOLLOW; } + const char *name4() const override { return "FOLL"; } + + // methods that affect movement of the vehicle in this mode + void update() override; + +protected: + + bool _enter() override; + uint32_t last_log_ms; // system time of last time desired velocity was logging +}; diff --git a/APMrover2/mode_follow.cpp b/APMrover2/mode_follow.cpp new file mode 100644 index 0000000000..a87d82aefe --- /dev/null +++ b/APMrover2/mode_follow.cpp @@ -0,0 +1,57 @@ +#include "mode.h" +#include "Rover.h" + +// initialize follow mode +bool ModeFollow::_enter() +{ + return ModeGuided::enter(); +} + +void ModeFollow::update() +{ + // variables to be sent to velocity controller + Vector3f desired_velocity_neu_cms; + float yaw_cd = 0.0f; + + Vector3f dist_vec; //vector to lead vehicle + Vector3f dist_vec_offs; // vector to lead vehicle + offset + Vector3f vel_of_target; // velocity of lead vehicle + + if (g2.follow.get_target_dist_and_vel_ned(dist_vec, dist_vec_offs, vel_of_target)) { + + // convert dist_vec_offs to cm in NEU + const Vector3f dist_vec_offs_neu(dist_vec_offs.x * 100.f, dist_vec_offs.y * 100.0f, -dist_vec_offs.z * 100.0f); + + // calculate desired velocity vector in cm/s in NEU + const float kp = g2.follow.get_pos_p().kP(); + desired_velocity_neu_cms.x = (vel_of_target.x * 100.0f) + (dist_vec_offs_neu.x * kp); + desired_velocity_neu_cms.y = (vel_of_target.y * 100.0f) + (dist_vec_offs_neu.y * kp); + + // scaled desired velocity to stay within horizontal speed limit + float desired_speed = safe_sqrt(sq(desired_velocity_neu_cms.x) + sq(desired_velocity_neu_cms.y)); + if (!is_zero(desired_speed) && (desired_speed > _desired_speed)) { + const float scalar_xy = _desired_speed / desired_speed; + desired_velocity_neu_cms.x *= scalar_xy; + desired_velocity_neu_cms.y *= scalar_xy; + desired_speed = _desired_speed; + } + + // unit vector towards target position (i.e. vector to lead vehicle + offset) + Vector3f dir_to_target_neu = dist_vec_offs_neu; + const float dir_to_target_neu_len = dir_to_target_neu.length(); + if (!is_zero(dir_to_target_neu_len)) { + dir_to_target_neu /= dir_to_target_neu_len; + } + + // calculate vehicle heading + const Vector3f dist_vec_xy(dist_vec_offs.x, dist_vec_offs.y, 0.0f); + if (dist_vec_xy.length() > 1.0f) { + yaw_cd = get_bearing_cd(Vector3f(), dist_vec_xy); + } + } + + // re-use guided mode's heading and speed controller + ModeGuided::set_desired_heading_and_speed(yaw_cd, safe_sqrt(sq(desired_velocity_neu_cms.x) + sq(desired_velocity_neu_cms.y))); + + ModeGuided::update(); +} diff --git a/APMrover2/wscript b/APMrover2/wscript index 457de019c5..a8721caa47 100644 --- a/APMrover2/wscript +++ b/APMrover2/wscript @@ -24,6 +24,7 @@ def build(bld): 'AC_Avoidance', 'AC_AttitudeControl', 'AP_Devo_Telem', + 'AP_Follow', ], )