mirror of https://github.com/ArduPilot/ardupilot
Copter: integrate AP_Follow into chase mode
This commit is contained in:
Randy Mackay
parent
199455dc56
commit
053983eb70
|
|
@ -84,6 +84,7 @@
|
|||
#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"
|
||||
|
|
|
|||
|
|
@ -696,7 +696,8 @@ void GCS_MAVLINK_Copter::packetReceived(const mavlink_status_t &status,
|
|||
copter.avoidance_adsb.handle_msg(msg);
|
||||
}
|
||||
#endif
|
||||
copter.mode_chase.mavlink_packet_received(msg);
|
||||
// pass message to follow library
|
||||
copter.g2.follow.handle_msg(msg);
|
||||
GCS_MAVLINK::packetReceived(status, msg);
|
||||
}
|
||||
|
||||
|
|
@ -826,6 +827,14 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
|
|||
mavlink_msg_command_int_decode(msg, &packet);
|
||||
switch(packet.command)
|
||||
{
|
||||
case MAV_CMD_DO_FOLLOW:
|
||||
// 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;
|
||||
}
|
||||
break;
|
||||
|
||||
case MAV_CMD_DO_SET_HOME: {
|
||||
// assume failure
|
||||
result = MAV_RESULT_FAILED;
|
||||
|
|
@ -945,6 +954,14 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
|
|||
}
|
||||
break;
|
||||
|
||||
case MAV_CMD_DO_FOLLOW:
|
||||
// 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;
|
||||
}
|
||||
break;
|
||||
|
||||
case MAV_CMD_CONDITION_YAW:
|
||||
// param1 : target angle [0-360]
|
||||
// param2 : speed during change [deg per second]
|
||||
|
|
|
|||
|
|
@ -987,7 +987,11 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
|
|||
// @Path: mode_flowhold.cpp
|
||||
AP_SUBGROUPPTR(mode_flowhold_ptr, "FHLD", 26, ParametersG2, Copter::ModeFlowHold),
|
||||
#endif
|
||||
|
||||
|
||||
// @Group: FOLL
|
||||
// @Path: ../libraries/AP_Follow/AP_Follow.cpp
|
||||
AP_SUBGROUPINFO(follow, "FOLL", 27, ParametersG2, AP_Follow),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
|
|
@ -1012,6 +1016,7 @@ ParametersG2::ParametersG2(void)
|
|||
#if !HAL_MINIMIZE_FEATURES && OPTFLOW == ENABLED
|
||||
,mode_flowhold_ptr(&copter.mode_flowhold)
|
||||
#endif
|
||||
,follow(copter.ahrs)
|
||||
{
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -581,6 +581,9 @@ public:
|
|||
// we need a pointer to the mode for the G2 table
|
||||
void *mode_flowhold_ptr;
|
||||
#endif
|
||||
|
||||
// follow
|
||||
AP_Follow follow;
|
||||
};
|
||||
|
||||
extern const AP_Param::Info var_info[];
|
||||
|
|
|
|||
|
|
@ -1099,7 +1099,6 @@ public:
|
|||
|
||||
ModeChase(Copter &copter) :
|
||||
Copter::ModeGuided(copter) {
|
||||
target_loc.flags.relative_alt = true;
|
||||
}
|
||||
|
||||
bool init(bool ignore_checks) override;
|
||||
|
|
@ -1112,29 +1111,8 @@ public:
|
|||
|
||||
bool set_velocity(const Vector3f& velocity_neu);
|
||||
|
||||
void mavlink_packet_received(const mavlink_message_t &msg);
|
||||
|
||||
protected:
|
||||
|
||||
const char *name() const override { return "CHASE"; }
|
||||
const char *name4() const override { return "CHAS"; }
|
||||
|
||||
private:
|
||||
|
||||
uint8_t target_srcid = 255;
|
||||
|
||||
Location_Class target_loc;
|
||||
Vector3f target_vel;
|
||||
uint32_t target_last_update_ms;
|
||||
const uint16_t target_update_timeout_ms = 1000;
|
||||
|
||||
float sphere_radius_min = 5.0f; // no closer than this to target
|
||||
float sphere_radius_max = 50.0f; // give up when further than this
|
||||
|
||||
const float closure_speed = 10.0f; // metres/second
|
||||
const float distance_slop = 2.0f; // metres
|
||||
|
||||
void run_lonely_mode();
|
||||
Copter::Mode *lonely_mode;
|
||||
|
||||
};
|
||||
|
|
|
|||
|
|
@ -9,9 +9,10 @@
|
|||
* TODO: "channel 7 option" to lock onto "pointed at" target
|
||||
* TODO: do better in terms of loitering around the moving point; may need a PID? Maybe use loiter controller somehow?
|
||||
* TODO: extrapolate target vehicle position using its velocity and acceleration
|
||||
* TODO: ensure AC_AVOID_ENABLED is true because we rely on it velocity limiting functions
|
||||
*/
|
||||
|
||||
#if 1
|
||||
#if 0
|
||||
#define Debug(fmt, args ...) do {::fprintf(stderr, "%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
|
||||
#else
|
||||
#define Debug(fmt, args ...)
|
||||
|
|
@ -34,25 +35,6 @@ bool Copter::ModeChase::set_velocity(const Vector3f& velocity_neu)
|
|||
return true;
|
||||
}
|
||||
|
||||
void Copter::ModeChase::run_lonely_mode()
|
||||
{
|
||||
if (lonely_mode == nullptr) {
|
||||
if (copter.mode_loiter.init(false)) {
|
||||
lonely_mode = &copter.mode_loiter;
|
||||
} else if(copter.mode_rtl.init(false)) {
|
||||
lonely_mode = &copter.mode_rtl;
|
||||
} else {
|
||||
copter.mode_land.init(false);
|
||||
lonely_mode = &copter.mode_land;
|
||||
}
|
||||
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "Chase: Lonely; %s", lonely_mode->name());
|
||||
}
|
||||
|
||||
lonely_mode->run();
|
||||
}
|
||||
|
||||
|
||||
void Copter::ModeChase::run()
|
||||
{
|
||||
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
|
||||
|
|
@ -65,70 +47,112 @@ void Copter::ModeChase::run()
|
|||
// Note: this is safe from interference from GCSs and companion computer's whose guided mode
|
||||
// position and velocity requests will be ignored while the vehicle is not in guided mode
|
||||
|
||||
const uint32_t now = AP_HAL::millis();
|
||||
if (now - target_last_update_ms > target_update_timeout_ms) {
|
||||
return run_lonely_mode();
|
||||
}
|
||||
// variables to be sent to velocity controller
|
||||
Vector3f desired_velocity;
|
||||
bool use_yaw = false;
|
||||
float yaw_cd = 0.0f;
|
||||
|
||||
Vector3f to_vehicle = location_3d_diff_NED(_copter.current_loc, target_loc);
|
||||
Debug("to_vehicle: %f %f %f", to_vehicle.x, to_vehicle.y, to_vehicle.z);
|
||||
const float distance_to_vehicle = to_vehicle.length();
|
||||
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)) {
|
||||
// debug
|
||||
Debug("dist to veh: %f %f %f", (double)dist_vec.x, (double)dist_vec.y, (double)dist_vec.z);
|
||||
|
||||
if (distance_to_vehicle > sphere_radius_max) {
|
||||
return run_lonely_mode();
|
||||
}
|
||||
lonely_mode = nullptr;
|
||||
// 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 float distance_to_stop = pos_control->get_stopping_distance_xyz() * 0.01f;
|
||||
// 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();
|
||||
|
||||
// scale desired velocity to stay within horizontal speed limit
|
||||
float desired_speed_xy = safe_sqrt(sq(desired_velocity.x) + sq(desired_velocity.y));
|
||||
if (!is_zero(desired_speed_xy) && (desired_speed_xy > pos_control->get_speed_xy())) {
|
||||
const float scalar_xy = pos_control->get_speed_xy() / desired_speed_xy;
|
||||
desired_velocity.x *= scalar_xy;
|
||||
desired_velocity.y *= scalar_xy;
|
||||
desired_speed_xy = pos_control->get_speed_xy();
|
||||
}
|
||||
|
||||
// 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
|
||||
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;
|
||||
}
|
||||
|
||||
// create horizontal desired velocity vector (required for slow down calculations)
|
||||
Vector2f desired_velocity_xy(desired_velocity.x, desired_velocity.y);
|
||||
|
||||
// create horizontal unit vector towards target (required for slow down calculations)
|
||||
Vector2f dir_to_target_xy(desired_velocity_xy.x, desired_velocity_xy.y);
|
||||
if (!dir_to_target_xy.is_zero()) {
|
||||
dir_to_target_xy.normalize();
|
||||
}
|
||||
|
||||
// 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);
|
||||
|
||||
// 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);
|
||||
|
||||
// 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);
|
||||
desired_velocity.z = constrain_float(desired_velocity.z, -des_vel_z_max, des_vel_z_max);
|
||||
|
||||
// get avoidance adjusted climb rate
|
||||
desired_velocity.z = get_avoidance_adjusted_climbrate(desired_velocity.z);
|
||||
|
||||
// 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);
|
||||
if (dist_vec_xy.length() > 1.0f) {
|
||||
yaw_cd = get_bearing_cd(Vector3f(), dist_vec_xy);
|
||||
use_yaw = true;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case AP_Follow::YAW_BEHAVE_SAME_AS_LEAD_VEHICLE: {
|
||||
float target_hdg = 0.0f;;
|
||||
if (g2.follow.get_target_heading(target_hdg)) {
|
||||
yaw_cd = target_hdg * 100.0f;
|
||||
use_yaw = true;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case AP_Follow::YAW_BEHAVE_DIR_OF_FLIGHT: {
|
||||
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;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case AP_Follow::YAW_BEHAVE_NONE:
|
||||
default:
|
||||
// do nothing
|
||||
break;
|
||||
|
||||
const float distance_to_move = distance_to_vehicle - sphere_radius_min;
|
||||
Debug("distance_to_vehicle=%f move=%f stop=%f",
|
||||
distance_to_vehicle,
|
||||
distance_to_move,
|
||||
distance_to_stop);
|
||||
to_vehicle.normalize();
|
||||
if (fabsf(distance_to_move) > distance_to_stop) {
|
||||
to_vehicle *= closure_speed * 100; // m/s to cm/s (which set_velocity takes)
|
||||
to_vehicle.z = -to_vehicle.z; // translate to NEU
|
||||
if (distance_to_move < 0) {
|
||||
to_vehicle = -to_vehicle; // too close! back up!
|
||||
}
|
||||
} else {
|
||||
to_vehicle.x = 0;
|
||||
to_vehicle.y = 0;
|
||||
to_vehicle.z = 0;
|
||||
}
|
||||
to_vehicle += target_vel;
|
||||
|
||||
// re-use guided mode's velocity controller (takes NEU)
|
||||
Copter::ModeGuided::set_velocity(to_vehicle);
|
||||
Copter::ModeGuided::set_velocity(desired_velocity, use_yaw, yaw_cd);
|
||||
|
||||
Copter::ModeGuided::run();
|
||||
}
|
||||
|
||||
|
||||
|
||||
void Copter::ModeChase::mavlink_packet_received(const mavlink_message_t &msg)
|
||||
{
|
||||
if (copter.flightmode != &copter.mode_chase) {
|
||||
return;
|
||||
}
|
||||
if (msg.sysid != target_srcid) {
|
||||
return;
|
||||
}
|
||||
if (msg.msgid != MAVLINK_MSG_ID_GLOBAL_POSITION_INT) {
|
||||
// handle position only for now
|
||||
return;
|
||||
}
|
||||
|
||||
mavlink_global_position_int_t packet;
|
||||
mavlink_msg_global_position_int_decode(&msg, &packet);
|
||||
target_loc.lat = packet.lat;
|
||||
target_loc.lng = packet.lon;
|
||||
target_loc.alt = packet.relative_alt / 10; // mm -> cm
|
||||
target_vel.x = packet.vx/100.0f; // cm/s to m/s
|
||||
target_vel.y = packet.vy/100.0f; // cm/s to m/s
|
||||
target_vel.z = packet.vz/100.0f; // cm/s to m/s
|
||||
|
||||
target_last_update_ms = AP_HAL::millis();
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue