mirror of https://github.com/ArduPilot/ardupilot
135 lines
4.1 KiB
C++
135 lines
4.1 KiB
C++
#include "Copter.h"
|
|
|
|
/*
|
|
* mode_chase.cpp - chase 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
|
|
* 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
|
|
*/
|
|
|
|
#if 1
|
|
#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 ...)
|
|
#endif
|
|
|
|
// initialise avoid_adsb controller
|
|
bool Copter::ModeChase::init(const bool ignore_checks)
|
|
{
|
|
// re-use guided mode
|
|
return Copter::ModeGuided::init(ignore_checks);
|
|
}
|
|
|
|
bool Copter::ModeChase::set_velocity(const Vector3f& velocity_neu)
|
|
{
|
|
// check flight mode
|
|
if (_copter.flightmode != &_copter.mode_chase) {
|
|
return false;
|
|
}
|
|
|
|
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
|
|
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
|
|
zero_throttle_and_relax_ac();
|
|
return;
|
|
}
|
|
|
|
// re-use guided mode's velocity controller
|
|
// 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();
|
|
}
|
|
|
|
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();
|
|
|
|
if (distance_to_vehicle > sphere_radius_max) {
|
|
return run_lonely_mode();
|
|
}
|
|
lonely_mode = nullptr;
|
|
|
|
const float distance_to_stop = pos_control->get_stopping_distance_xyz() * 0.01f;
|
|
|
|
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::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();
|
|
}
|