zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Copter: follow mode fixups 

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
master
Peter Barker 7 years ago committed by Randy Mackay
parent
6ee101ca98
commit
af56826107
10 changed files with 81 additions and 42 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1
      ArduCopter/APM_Config.h
  2. 6
      ArduCopter/Copter.h
  3. 6
      ArduCopter/GCS_Mavlink.cpp
  4. 16
      ArduCopter/Parameters.cpp
  5. 3
      ArduCopter/Parameters.h
  6. 6
      ArduCopter/config.h
  7. 4
      ArduCopter/mode.cpp
  8. 11
      ArduCopter/mode.h
  9. 64
      ArduCopter/mode_follow.cpp
  10. 6
      ArduCopter/mode_guided.cpp

1
ArduCopter/APM_Config.h
Unescape View File

@ -29,6 +29,7 @@
//#define MODE_BRAKE_ENABLED DISABLED // disable brake mode support //#define MODE_BRAKE_ENABLED DISABLED // disable brake mode support
//#define MODE_CIRCLE_ENABLED DISABLED // disable circle mode support //#define MODE_CIRCLE_ENABLED DISABLED // disable circle mode support
//#define MODE_DRIFT_ENABLED DISABLED // disable drift 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_ENABLED DISABLED // disable guided mode support
//#define MODE_GUIDED_NOGPS_ENABLED DISABLED // disable guided/nogps mode support //#define MODE_GUIDED_NOGPS_ENABLED DISABLED // disable guided/nogps mode support
//#define MODE_LOITER_ENABLED DISABLED // disable loiter mode support //#define MODE_LOITER_ENABLED DISABLED // disable loiter mode support

6
ArduCopter/Copter.h
Unescape View File

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

6
ArduCopter/GCS_Mavlink.cpp
Unescape View File

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

16
ArduCopter/Parameters.cpp
Unescape View File

@ -297,42 +297,42 @@ const AP_Param::Info Copter::var_info[] = {
// @Param: FLTMODE1 // @Param: FLTMODE1
// @DisplayName: Flight Mode 1 // @DisplayName: Flight Mode 1
// @Description: Flight mode when Channel 5 pwm is <= 1230 // @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 // @User: Standard
GSCALAR(flight_mode1, "FLTMODE1", FLIGHT_MODE_1), GSCALAR(flight_mode1, "FLTMODE1", FLIGHT_MODE_1),
// @Param: FLTMODE2 // @Param: FLTMODE2
// @DisplayName: Flight Mode 2 // @DisplayName: Flight Mode 2
// @Description: Flight mode when Channel 5 pwm is >1230, <= 1360 // @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 // @User: Standard
GSCALAR(flight_mode2, "FLTMODE2", FLIGHT_MODE_2), GSCALAR(flight_mode2, "FLTMODE2", FLIGHT_MODE_2),
// @Param: FLTMODE3 // @Param: FLTMODE3
// @DisplayName: Flight Mode 3 // @DisplayName: Flight Mode 3
// @Description: Flight mode when Channel 5 pwm is >1360, <= 1490 // @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 // @User: Standard
GSCALAR(flight_mode3, "FLTMODE3", FLIGHT_MODE_3), GSCALAR(flight_mode3, "FLTMODE3", FLIGHT_MODE_3),
// @Param: FLTMODE4 // @Param: FLTMODE4
// @DisplayName: Flight Mode 4 // @DisplayName: Flight Mode 4
// @Description: Flight mode when Channel 5 pwm is >1490, <= 1620 // @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 // @User: Standard
GSCALAR(flight_mode4, "FLTMODE4", FLIGHT_MODE_4), GSCALAR(flight_mode4, "FLTMODE4", FLIGHT_MODE_4),
// @Param: FLTMODE5 // @Param: FLTMODE5
// @DisplayName: Flight Mode 5 // @DisplayName: Flight Mode 5
// @Description: Flight mode when Channel 5 pwm is >1620, <= 1749 // @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 // @User: Standard
GSCALAR(flight_mode5, "FLTMODE5", FLIGHT_MODE_5), GSCALAR(flight_mode5, "FLTMODE5", FLIGHT_MODE_5),
// @Param: FLTMODE6 // @Param: FLTMODE6
// @DisplayName: Flight Mode 6 // @DisplayName: Flight Mode 6
// @Description: Flight mode when Channel 5 pwm is >=1750 // @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 // @User: Standard
GSCALAR(flight_mode6, "FLTMODE6", FLIGHT_MODE_6), 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), AP_SUBGROUPPTR(mode_flowhold_ptr, "FHLD", 26, ParametersG2, Copter::ModeFlowHold),
#endif #endif
#if MODE_FOLLOW_ENABLED == ENABLED
// @Group: FOLL // @Group: FOLL
// @Path: ../libraries/AP_Follow/AP_Follow.cpp // @Path: ../libraries/AP_Follow/AP_Follow.cpp
AP_SUBGROUPINFO(follow, "FOLL", 27, ParametersG2, AP_Follow), AP_SUBGROUPINFO(follow, "FOLL", 27, ParametersG2, AP_Follow),
#endif
AP_GROUPEND AP_GROUPEND
}; };
@ -1016,7 +1018,9 @@ ParametersG2::ParametersG2(void)
#if !HAL_MINIMIZE_FEATURES && OPTFLOW == ENABLED #if !HAL_MINIMIZE_FEATURES && OPTFLOW == ENABLED
,mode_flowhold_ptr(&copter.mode_flowhold) ,mode_flowhold_ptr(&copter.mode_flowhold)
#endif #endif
#if MODE_FOLLOW_ENABLED == ENABLED
,follow(copter.ahrs) ,follow(copter.ahrs)
#endif
{ {
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
} }

3
ArduCopter/Parameters.h
Unescape View File

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

6
ArduCopter/config.h
Unescape View File

@ -291,6 +291,12 @@
# define MODE_DRIFT_ENABLED ENABLED # define MODE_DRIFT_ENABLED ENABLED
#endif #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 // Guided mode - control vehicle's position or angles from GCS
#ifndef MODE_GUIDED_ENABLED #ifndef MODE_GUIDED_ENABLED

4
ArduCopter/mode.cpp
Unescape View File

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

11
ArduCopter/mode.h
Unescape 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); 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 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); 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); 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(); void limit_clear();
@ -1098,9 +1098,8 @@ class ModeFollow : public ModeGuided {
public: public:
ModeFollow(Copter &copter) : // inherit constructor
Copter::ModeGuided(copter) { using Copter::ModeGuided::Mode;
}
bool init(bool ignore_checks) override; bool init(bool ignore_checks) override;
void run() override; void run() override;
@ -1110,10 +1109,10 @@ public:
bool allows_arming(bool from_gcs) const override { return false; } bool allows_arming(bool from_gcs) const override { return false; }
bool is_autopilot() const override { return true; } bool is_autopilot() const override { return true; }
bool set_velocity(const Vector3f& velocity_neu);
protected: protected:
const char *name() const override { return "FOLLOW"; } const char *name() const override { return "FOLLOW"; }
const char *name4() const override { return "FOLL"; } 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
Unescape View File

@ -1,9 +1,10 @@
#include "Copter.h" #include "Copter.h"
#if MODE_FOLLOW_ENABLED == ENABLED
/* /*
* mode_follow.cpp - follow another mavlink-enabled vehicle by system id * 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 move around on sphere
* TODO: stick control to change sphere diameter * TODO: stick control to change sphere diameter
* TODO: "channel 7 option" to lock onto "pointed at" target * 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 * TODO: ensure AC_AVOID_ENABLED is true because we rely on it velocity limiting functions
*/ */
#if 0 #if 1
#define Debug(fmt, args ...) do {::fprintf(stderr, "%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0) #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 #else
#define Debug(fmt, args ...) #define Debug(fmt, args ...)
#endif #endif
// initialise avoid_adsb controller // initialise follow mode
bool Copter::ModeFollow::init(const bool ignore_checks) bool Copter::ModeFollow::init(const bool ignore_checks)
{ {
// re-use guided mode // re-use guided mode
return Copter::ModeGuided::init(ignore_checks); gcs().send_text(MAV_SEVERITY_WARNING, "Follow-mode init");
} if (!g2.follow.enabled()) {
bool Copter::ModeFollow::set_velocity(const Vector3f& velocity_neu)
{
// check flight mode
if (_copter.flightmode != &_copter.mode_follow) {
return false; return false;
} }
return Copter::ModeGuided::init(ignore_checks);
return true;
} }
void Copter::ModeFollow::run() void Copter::ModeFollow::run()
@ -57,15 +57,21 @@ void Copter::ModeFollow::run()
Vector3f vel_of_target; // velocity of lead vehicle 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)) { if (g2.follow.get_target_dist_and_vel_ned(dist_vec, dist_vec_offs, vel_of_target)) {
// debug // 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 // 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); 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 // 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()); const float kp = g2.follow.get_pos_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.x = (vel_of_target.x * 100.0f) + (dist_vec_offs_neu.x * kp);
desired_velocity.z = (-vel_of_target.z * 100.0f) + dist_vec_offs_neu.z * pos_control->get_pos_z_p().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 // scale desired velocity to stay within horizontal speed limit
float desired_speed_xy = safe_sqrt(sq(desired_velocity.x) + sq(desired_velocity.y)); 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 // 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()); 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; Vector3f dir_to_target_neu = dist_vec_offs_neu;
const float dir_to_target_neu_len = dir_to_target_neu.length(); const float dir_to_target_neu_len = dir_to_target_neu.length();
if (!is_zero(dir_to_target_neu_len)) { 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) // 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(); 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 // 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 // copy horizontal velocity limits back to 3d vector
desired_velocity.x = desired_velocity_xy.x; desired_velocity.x = desired_velocity_xy.x;
desired_velocity.y = desired_velocity_xy.y; 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) // 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); desired_velocity.z = constrain_float(desired_velocity.z, -des_vel_z_max, des_vel_z_max);
// get avoidance adjusted climb rate // get avoidance adjusted climb rate
@ -116,7 +122,7 @@ void Copter::ModeFollow::run()
// calculate vehicle heading // calculate vehicle heading
switch (g2.follow.get_yaw_behave()) { switch (g2.follow.get_yaw_behave()) {
case AP_Follow::YAW_BEHAVE_FACE_LEAD_VEHICLE: { 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) { if (dist_vec_xy.length() > 1.0f) {
yaw_cd = get_bearing_cd(Vector3f(), dist_vec_xy); yaw_cd = get_bearing_cd(Vector3f(), dist_vec_xy);
use_yaw = true; use_yaw = true;
@ -125,7 +131,7 @@ void Copter::ModeFollow::run()
} }
case AP_Follow::YAW_BEHAVE_SAME_AS_LEAD_VEHICLE: { 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)) { if (g2.follow.get_target_heading(target_hdg)) {
yaw_cd = target_hdg * 100.0f; yaw_cd = target_hdg * 100.0f;
use_yaw = true; use_yaw = true;
@ -134,7 +140,7 @@ void Copter::ModeFollow::run()
} }
case AP_Follow::YAW_BEHAVE_DIR_OF_FLIGHT: { 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) { if (vel_vec.length() > 100.0f) {
yaw_cd = get_bearing_cd(Vector3f(), vel_vec); yaw_cd = get_bearing_cd(Vector3f(), vel_vec);
use_yaw = true; 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) // 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(); Copter::ModeGuided::run();
} }
#endif // MODE_FOLLOW_ENABLED == ENABLED

6
ArduCopter/mode_guided.cpp
Unescape 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 // 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 // check we are in velocity control mode
if (guided_mode != Guided_Velocity) { 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(); vel_update_time_ms = millis();
// log target // 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 // set guided mode posvel target

Write Preview
Loading…
Cancel
Save