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

Rover: implement Follow mode

This commit is contained in:
Ammarf 2018-05-24 13:47:07 +09:00 committed by Randy Mackay
parent 465564ba99
commit c0082271e6
9 changed files with 106 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
APMrover2/Parameters.cpp
View File

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

3
APMrover2/Parameters.h
View File

@ -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[];

3
APMrover2/Rover.h
View File

@ -79,6 +79,7 @@
#include <AC_Fence/AC_Fence.h>
#include <AP_Proximity/AP_Proximity.h>
#include <AC_Avoidance/AC_Avoid.h>
#include <AP_Follow/AP_Follow.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include <SITL/SITL.h>
#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 {

9
APMrover2/control_modes.cpp
View File

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

3
APMrover2/defines.h
View File

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

1
APMrover2/make.inc
View File

@ -50,3 +50,4 @@ LIBRARIES += AP_SmartRTL
LIBRARIES += AC_Avoidance
LIBRARIES += AC_AttitudeControl
LIBRARIES += AP_Devo_Telem
LIBRARIES += AP_Follow

17
APMrover2/mode.h
View File

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

57
APMrover2/mode_follow.cpp Normal file
View File

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

1
APMrover2/wscript
View File

@ -24,6 +24,7 @@ def build(bld):
'AC_Avoidance',
'AC_AttitudeControl',
'AP_Devo_Telem',
'AP_Follow',
],
)