Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-19 14:23:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: follow mode fixups

Browse Source 
add follow to FLTMODEx param descriptions
remove follow's set_velocity method
10hz logging of desired velocity in follow mode
follow mode uses pos error P gain
add send-debug-via-mavlink option
don't enter follow if follow lib is disabled
follow debug slowed to 1hz
disable follow on min-features builds
This commit is contained in:
Peter Barker 2018-02-06 15:16:09 +09:00 committed by Randy Mackay
parent 6ee101ca98
commit af56826107
10 changed files with 81 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/APM_Config.h
View File

@ -29,6 +29,7 @@
//#define MODE_BRAKE_ENABLED DISABLED // disable brake mode support
//#define MODE_CIRCLE_ENABLED DISABLED // disable circle mode support
//#define MODE_DRIFT_ENABLED DISABLED // disable drift mode support
//#define MODE_FOLLOW_ENABLED DISABLED // disable follow mode support
//#define MODE_GUIDED_ENABLED DISABLED // disable guided mode support
//#define MODE_GUIDED_NOGPS_ENABLED DISABLED // disable guided/nogps mode support
//#define MODE_LOITER_ENABLED DISABLED // disable loiter mode support

6
ArduCopter/Copter.h
View File

@ -84,7 +84,6 @@
#include <AP_Arming/AP_Arming.h>
#include <AP_SmartRTL/AP_SmartRTL.h>
#include <AP_TempCalibration/AP_TempCalibration.h>
#include <AP_Follow/AP_Follow.h>
// Configuration
#include "defines.h"
@ -121,6 +120,9 @@
#if ADSB_ENABLED == ENABLED
# include <AP_ADSB/AP_ADSB.h>
#endif
#if MODE_FOLLOW_ENABLED == ENABLED
# include <AP_Follow/AP_Follow.h>
#endif
#if AC_FENCE == ENABLED
# include <AC_Fence/AC_Fence.h>
#endif
@ -980,7 +982,9 @@ private:
ModeDrift mode_drift;
#endif
ModeFlip mode_flip;
#if MODE_FOLLOW_ENABLED == ENABLED
ModeFollow mode_follow;
#endif
#if MODE_GUIDED_ENABLED == ENABLED
ModeGuided mode_guided;
#endif

6
ArduCopter/GCS_Mavlink.cpp
View File

@ -696,8 +696,10 @@ void GCS_MAVLINK_Copter::packetReceived(const mavlink_status_t &status,
copter.avoidance_adsb.handle_msg(msg);
}
#endif
#if MODE_FOLLOW_ENABLED == ENABLED
// pass message to follow library
copter.g2.follow.handle_msg(msg);
#endif
GCS_MAVLINK::packetReceived(status, msg);
}
@ -828,11 +830,13 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
switch(packet.command)
{
case MAV_CMD_DO_FOLLOW:
#if MODE_FOLLOW_ENABLED == ENABLED
// param1: sysid of target to follow
if ((packet.param1 > 0) && (packet.param1 <= 255)) {
copter.g2.follow.set_target_sysid((uint8_t)packet.param1);
result = MAV_RESULT_ACCEPTED;
}
#endif
break;
case MAV_CMD_DO_SET_HOME: {
@ -955,11 +959,13 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
break;
case MAV_CMD_DO_FOLLOW:
#if MODE_FOLLOW_ENABLED == ENABLED
// param1: sysid of target to follow
if ((packet.param1 > 0) && (packet.param1 <= 255)) {
copter.g2.follow.set_target_sysid((uint8_t)packet.param1);
result = MAV_RESULT_ACCEPTED;
}
#endif
break;
case MAV_CMD_CONDITION_YAW:

16
ArduCopter/Parameters.cpp
View File

@ -297,42 +297,42 @@ const AP_Param::Info Copter::var_info[] = {
// @Param: FLTMODE1
// @DisplayName: Flight Mode 1
// @Description: Flight mode when Channel 5 pwm is <= 1230
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:Follow
// @User: Standard
GSCALAR(flight_mode1, "FLTMODE1", FLIGHT_MODE_1),
// @Param: FLTMODE2
// @DisplayName: Flight Mode 2
// @Description: Flight mode when Channel 5 pwm is >1230, <= 1360
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:Follow
// @User: Standard
GSCALAR(flight_mode2, "FLTMODE2", FLIGHT_MODE_2),
// @Param: FLTMODE3
// @DisplayName: Flight Mode 3
// @Description: Flight mode when Channel 5 pwm is >1360, <= 1490
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:Follow
// @User: Standard
GSCALAR(flight_mode3, "FLTMODE3", FLIGHT_MODE_3),
// @Param: FLTMODE4
// @DisplayName: Flight Mode 4
// @Description: Flight mode when Channel 5 pwm is >1490, <= 1620
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:Follow
// @User: Standard
GSCALAR(flight_mode4, "FLTMODE4", FLIGHT_MODE_4),
// @Param: FLTMODE5
// @DisplayName: Flight Mode 5
// @Description: Flight mode when Channel 5 pwm is >1620, <= 1749
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:Follow
// @User: Standard
GSCALAR(flight_mode5, "FLTMODE5", FLIGHT_MODE_5),
// @Param: FLTMODE6
// @DisplayName: Flight Mode 6
// @Description: Flight mode when Channel 5 pwm is >=1750
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:Follow
// @User: Standard
GSCALAR(flight_mode6, "FLTMODE6", FLIGHT_MODE_6),
@ -988,9 +988,11 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
AP_SUBGROUPPTR(mode_flowhold_ptr, "FHLD", 26, ParametersG2, Copter::ModeFlowHold),
#endif
#if MODE_FOLLOW_ENABLED == ENABLED
// @Group: FOLL
// @Path: ../libraries/AP_Follow/AP_Follow.cpp
AP_SUBGROUPINFO(follow, "FOLL", 27, ParametersG2, AP_Follow),
#endif
AP_GROUPEND
};
@ -1016,7 +1018,9 @@ ParametersG2::ParametersG2(void)
#if !HAL_MINIMIZE_FEATURES && OPTFLOW == ENABLED
,mode_flowhold_ptr(&copter.mode_flowhold)
#endif
#if MODE_FOLLOW_ENABLED == ENABLED
,follow(copter.ahrs)
#endif
{
AP_Param::setup_object_defaults(this, var_info);
}

3
ArduCopter/Parameters.h
View File

@ -581,9 +581,10 @@ public:
// we need a pointer to the mode for the G2 table
void *mode_flowhold_ptr;
#endif
#if MODE_FOLLOW_ENABLED == ENABLED
// follow
AP_Follow follow;
#endif
};
extern const AP_Param::Info var_info[];

6
ArduCopter/config.h
View File

@ -291,6 +291,12 @@
# define MODE_DRIFT_ENABLED ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// Follow - follow another vehicle or GCS
#ifndef MODE_FOLLOW_ENABLED
# define MODE_FOLLOW_ENABLED !HAL_MINIMIZE_FEATURES
#endif
//////////////////////////////////////////////////////////////////////////////
// Guided mode - control vehicle's position or angles from GCS
#ifndef MODE_GUIDED_ENABLED

4
ArduCopter/mode.cpp
View File

@ -145,10 +145,12 @@ Copter::Mode *Copter::mode_from_mode_num(const uint8_t mode)
ret = (Copter::Mode *)g2.mode_flowhold_ptr;
break;
#endif
#if MODE_FOLLOW_ENABLED == ENABLED
case FOLLOW:
ret = &mode_follow;
break;
#endif
default:
break;

11
ArduCopter/mode.h
View File

@ -690,7 +690,7 @@ public:
void set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads);
bool set_destination(const Vector3f& destination, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
bool set_destination(const Location_Class& dest_loc, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
void set_velocity(const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
void set_velocity(const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false, bool log_request = true);
bool set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
void limit_clear();
@ -1098,9 +1098,8 @@ class ModeFollow : public ModeGuided {
public:
ModeFollow(Copter &copter) :
Copter::ModeGuided(copter) {
}
// inherit constructor
using Copter::ModeGuided::Mode;
bool init(bool ignore_checks) override;
void run() override;
@ -1110,10 +1109,10 @@ public:
bool allows_arming(bool from_gcs) const override { return false; }
bool is_autopilot() const override { return true; }
bool set_velocity(const Vector3f& velocity_neu);
protected:
const char *name() const override { return "FOLLOW"; }
const char *name4() const override { return "FOLL"; }
uint32_t last_log_ms; // system time of last time desired velocity was logging
};

64
ArduCopter/mode_follow.cpp
View File

@ -1,9 +1,10 @@
#include "Copter.h"
#if MODE_FOLLOW_ENABLED == ENABLED
/*
* mode_follow.cpp - follow another mavlink-enabled vehicle by system id
*
* TODO: set ROI yaw mode / point camera at target
* TODO: stick control to move around on sphere
* TODO: stick control to change sphere diameter
* TODO: "channel 7 option" to lock onto "pointed at" target
@ -12,27 +13,26 @@
* TODO: ensure AC_AVOID_ENABLED is true because we rely on it velocity limiting functions
*/
#if 0
#define Debug(fmt, args ...) do {::fprintf(stderr, "%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
#if 1
#define Debug(fmt, args ...) do { \
gcs().send_text(MAV_SEVERITY_WARNING, fmt, ## args); \
} while(0)
#elif 0
#define Debug(fmt, args ...) do { \
::fprintf(stderr, "%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args);
#else
#define Debug(fmt, args ...)
#endif
// initialise avoid_adsb controller
// initialise follow mode
bool Copter::ModeFollow::init(const bool ignore_checks)
{
// re-use guided mode
return Copter::ModeGuided::init(ignore_checks);
}
bool Copter::ModeFollow::set_velocity(const Vector3f& velocity_neu)
{
// check flight mode
if (_copter.flightmode != &_copter.mode_follow) {
gcs().send_text(MAV_SEVERITY_WARNING, "Follow-mode init");
if (!g2.follow.enabled()) {
return false;
}
return true;
return Copter::ModeGuided::init(ignore_checks);
}
void Copter::ModeFollow::run()
@ -57,15 +57,21 @@ void Copter::ModeFollow::run()
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)) {
// debug
Debug("dist to veh: %f %f %f", (double)dist_vec.x, (double)dist_vec.y, (double)dist_vec.z);
static uint16_t counter = 0;
counter++;
if (counter >= 400) {
counter = 0;
Debug("dist to veh: %f %f %f", (double)dist_vec.x, (double)dist_vec.y, (double)dist_vec.z);
}
// convert dist_vec_offs to cm in NEU
const Vector3f dist_vec_offs_neu(dist_vec_offs.x * 100.0f, dist_vec_offs.y * 100.0f, -dist_vec_offs.z * 100.0f);
// calculate desired velocity vector in cm/s in NEU
desired_velocity.x = (vel_of_target.x * 100.0f) + (dist_vec_offs_neu.x * pos_control->get_pos_xy_p().kP());
desired_velocity.y = (vel_of_target.y * 100.0f) + dist_vec_offs_neu.y * pos_control->get_pos_xy_p().kP();
desired_velocity.z = (-vel_of_target.z * 100.0f) + dist_vec_offs_neu.z * pos_control->get_pos_z_p().kP();
const float kp = g2.follow.get_pos_p().kP();
desired_velocity.x = (vel_of_target.x * 100.0f) + (dist_vec_offs_neu.x * kp);
desired_velocity.y = (vel_of_target.y * 100.0f) + (dist_vec_offs_neu.y * kp);
desired_velocity.z = (-vel_of_target.z * 100.0f) + (dist_vec_offs_neu.z * kp);
// scale desired velocity to stay within horizontal speed limit
float desired_speed_xy = safe_sqrt(sq(desired_velocity.x) + sq(desired_velocity.y));
@ -79,7 +85,7 @@ void Copter::ModeFollow::run()
// limit desired velocity to be between maximum climb and descent rates
desired_velocity.z = constrain_float(desired_velocity.z, -fabsf(pos_control->get_speed_down()), pos_control->get_speed_up());
// unit vector towards target position
// 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)) {
@ -97,17 +103,17 @@ void Copter::ModeFollow::run()
// slow down horizontally as we approach target (use 1/2 of maximum deceleration for gentle slow down)
const float dist_to_target_xy = Vector2f(dist_vec_offs_neu.x, dist_vec_offs_neu.y).length();
_copter.avoid.limit_velocity(pos_control->get_pos_xy_p().kP().get(), pos_control->get_accel_xy() / 2.0f, desired_velocity_xy, dir_to_target_xy, dist_to_target_xy, _copter.G_Dt);
copter.avoid.limit_velocity(pos_control->get_pos_xy_p().kP().get(), pos_control->get_accel_xy() * 0.5f, desired_velocity_xy, dir_to_target_xy, dist_to_target_xy, copter.G_Dt);
// limit the horizontal velocity to prevent fence violations
_copter.avoid.adjust_velocity(pos_control->get_pos_xy_p().kP().get(), pos_control->get_accel_xy(), desired_velocity_xy, G_Dt);
copter.avoid.adjust_velocity(pos_control->get_pos_xy_p().kP().get(), pos_control->get_accel_xy(), desired_velocity_xy, G_Dt);
// copy horizontal velocity limits back to 3d vector
desired_velocity.x = desired_velocity_xy.x;
desired_velocity.y = desired_velocity_xy.y;
// limit vertical desired_velocity to slow as we approach target (we use 1/2 of maximum deceleration for gentle slow down)
const float des_vel_z_max = _copter.avoid.get_max_speed(pos_control->get_pos_z_p().kP().get(), pos_control->get_accel_z() / 2.0f, fabsf(dist_vec_offs_neu.z), _copter.G_Dt);
const float des_vel_z_max = copter.avoid.get_max_speed(pos_control->get_pos_z_p().kP().get(), pos_control->get_accel_z() * 0.5f, fabsf(dist_vec_offs_neu.z), copter.G_Dt);
desired_velocity.z = constrain_float(desired_velocity.z, -des_vel_z_max, des_vel_z_max);
// get avoidance adjusted climb rate
@ -116,7 +122,7 @@ void Copter::ModeFollow::run()
// calculate vehicle heading
switch (g2.follow.get_yaw_behave()) {
case AP_Follow::YAW_BEHAVE_FACE_LEAD_VEHICLE: {
Vector3f dist_vec_xy(dist_vec.x, dist_vec.y, 0.0f);
const Vector3f dist_vec_xy(dist_vec.x, dist_vec.y, 0.0f);
if (dist_vec_xy.length() > 1.0f) {
yaw_cd = get_bearing_cd(Vector3f(), dist_vec_xy);
use_yaw = true;
@ -125,7 +131,7 @@ void Copter::ModeFollow::run()
}
case AP_Follow::YAW_BEHAVE_SAME_AS_LEAD_VEHICLE: {
float target_hdg = 0.0f;;
float target_hdg = 0.0f;
if (g2.follow.get_target_heading(target_hdg)) {
yaw_cd = target_hdg * 100.0f;
use_yaw = true;
@ -134,7 +140,7 @@ void Copter::ModeFollow::run()
}
case AP_Follow::YAW_BEHAVE_DIR_OF_FLIGHT: {
Vector3f vel_vec(desired_velocity.x, desired_velocity.y, 0.0f);
const Vector3f vel_vec(desired_velocity.x, desired_velocity.y, 0.0f);
if (vel_vec.length() > 100.0f) {
yaw_cd = get_bearing_cd(Vector3f(), vel_vec);
use_yaw = true;
@ -150,9 +156,17 @@ void Copter::ModeFollow::run()
}
}
// log output at 10hz
uint32_t now = AP_HAL::millis();
bool log_request = false;
if ((now - last_log_ms >= 100) || (last_log_ms == 0)) {
log_request = true;
last_log_ms = now;
}
// re-use guided mode's velocity controller (takes NEU)
Copter::ModeGuided::set_velocity(desired_velocity, use_yaw, yaw_cd);
Copter::ModeGuided::set_velocity(desired_velocity, use_yaw, yaw_cd, false, 0.0f, false, log_request);
Copter::ModeGuided::run();
}
#endif // MODE_FOLLOW_ENABLED == ENABLED

6
ArduCopter/mode_guided.cpp
View File

@ -241,7 +241,7 @@ bool Copter::ModeGuided::set_destination(const Location_Class& dest_loc, bool us
}
// guided_set_velocity - sets guided mode's target velocity
void Copter::ModeGuided::set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
void Copter::ModeGuided::set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool log_request)
{
// check we are in velocity control mode
if (guided_mode != Guided_Velocity) {
@ -256,7 +256,9 @@ void Copter::ModeGuided::set_velocity(const Vector3f& velocity, bool use_yaw, fl
vel_update_time_ms = millis();
// log target
copter.Log_Write_GuidedTarget(guided_mode, Vector3f(), velocity);
if (log_request) {
copter.Log_Write_GuidedTarget(guided_mode, Vector3f(), velocity);
}
}
// set guided mode posvel target