fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 19:13:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Tests

Browse Source
This commit is contained in:
Jimmy Johnson
2016-03-27 12:10:57 +02:00
committed by Lorenz Meier
parent 0797c7fc21
commit 01e971b342
8 changed files with 124 additions and 93 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/follow_target.cpp
+70 -58
View File
@@ -51,7 +51,6 @@
#include <uORB/uORB.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/follow_target.h>
#include <lib/geo/geo.h>
#include "navigator.h"
@@ -60,7 +59,7 @@ using namespace matrix;
FollowTarget::FollowTarget(Navigator *navigator, const char *name) :
MissionBlock(navigator, name),
_navigator(navigator),
_tracker_motion_position_sub(-1),
//_tracker_motion_position_sub(-1),
_param_min_alt(this, "MIS_TAKEOFF_ALT", false),
gps_valid(false),
_last_message_time(0),
@@ -73,6 +72,8 @@ FollowTarget::FollowTarget(Navigator *navigator, const char *name) :
pos_pair[1].setZero();
_current_vel.setZero();
_steps = 0.0f;
target_dist_x = target_dist_y = 0.0f;
target = {};
}
FollowTarget::~FollowTarget()
@@ -91,31 +92,42 @@ FollowTarget::on_activation()
Vector2f vel;
vel.setZero();
if(_tracker_motion_position_sub < 0) {
_tracker_motion_position_sub = orb_subscribe(ORB_ID(follow_target));
}
// inital set point is same as loiter sp
set_loiter_item(&_mission_item, _param_min_alt.get());
convert_mission_item_to_sp(vel);
update_position_sp(vel);
target.lat = _navigator->get_global_position()->lat;
target.lon = _navigator->get_global_position()->lon;
}
void
FollowTarget::on_active() {
follow_target_s target;
bool updated;
struct map_projection_reference_s target_ref;
float target_dist_x,target_dist_y;
struct map_projection_reference_s target_ref;
uint64_t current_time = hrt_absolute_time();
// get distance to target
map_projection_init(&target_ref, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
map_projection_project(&target_ref, target.lat, target.lon, &target_dist_x, &target_dist_y);
orb_check(_tracker_motion_position_sub, &updated);
follow_target_reached = (sqrtf(target_dist_x * target_dist_x + target_dist_y * target_dist_y) < 5);
if(follow_target_reached == false) {
_current_vel(0) = _navigator->get_global_position()->vel_n;
_current_vel(1) = _navigator->get_global_position()->vel_e;
}
orb_check(_navigator->_tracker_motion_position_sub, &updated);
if (updated) {
if (orb_copy(ORB_ID(follow_target), _tracker_motion_position_sub, &target) == OK) {
warnx("UPDASTD ");
if (orb_copy(ORB_ID(follow_target), _navigator->_tracker_motion_position_sub, &target) == OK) {
float dt = ((double)(current_time - _last_message_time) * 1e-6);
if ((gps_valid == false) ) {
gps_pair(0) = target.lat;
@@ -125,34 +137,15 @@ FollowTarget::on_active() {
return;
}
// get distance to target
map_projection_init(&target_ref, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
map_projection_project(&target_ref, target.lat, target.lon, &target_dist_x, &target_dist_y);
follow_target_reached = (sqrtf(target_dist_x * target_dist_x + target_dist_y * target_dist_y) < 5);
if(follow_target_reached == false && gps_valid == true) {
Vector2f go_to_vel;
warnx("WP not reached lat %f (%f) lon %f (%f), %f", target.lat, (double)_navigator->get_global_position()->lat, (double)_navigator->get_global_position()->lon, target.lon, (double)sqrtf(target_dist_x * target_dist_x + target_dist_y * target_dist_y));
set_follow_target_item(&_mission_item, _param_min_alt.get(), target);
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
convert_mission_item_to_sp(go_to_vel);
_navigator->get_position_setpoint_triplet()->previous.valid = false;
_navigator->set_position_setpoint_triplet_updated();
//target_vel
return;
}
// get last gps reference
map_projection_init(&target_ref, gps_pair(0), gps_pair(1));
map_projection_project(&target_ref, target.lat, target.lon, &(pos_pair[1](0)), &(pos_pair[1](1)));
target_vel = pos_pair[1]/((double)(current_time - _last_message_time) * 1e-6);
target_vel = pos_pair[1]/(dt);
target_vel(0) += target_dist_x*.1f;
target_vel(1) += target_dist_y*.1f;
warnx("tracker x %f y %f m, x %f m/s, y %f m/s gs = %f m/s dis = %f m",
(double) pos_pair[1](0),
@@ -169,44 +162,63 @@ FollowTarget::on_active() {
_last_message_time = current_time;
if(follow_target_reached == false) {
warnx("WP not reached lat %f (%f) lon %f (%f), %f", target.lat, (double)_navigator->get_global_position()->lat, (double)_navigator->get_global_position()->lon, target.lon, (double)sqrtf(target_dist_x * target_dist_x + target_dist_y * target_dist_y));
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
update_position_sp(target_vel);
return;
}
_steps = fabs((double)((sqrtf(_current_vel(0)*_current_vel(0) + _current_vel(1)*_current_vel(1)) -
sqrtf(target_vel(0)*target_vel(0) + target_vel(1)*target_vel(1))))) / (double)10.0f;
//pos_pair[0] = pos_pair[1];
sqrtf(target_vel(0)*target_vel(0) + target_vel(1)*target_vel(1))))) / (double) (dt*10);
}
}
if ((((double) (current_time - _last_publish_time) * 1e-3) > 100) && (follow_target_reached == true)) {
if(_current_vel(0) <= target_vel(0)) {
_current_vel(0) += _steps;
}
if(_current_vel(0) >= target_vel(0)) {
_current_vel(0) -= _steps;
}
if(_current_vel(1) <= target_vel(1)) {
_current_vel(1) += _steps;
}
if(_current_vel(1) >= target_vel(1)) {
_current_vel(1) -= _steps;
}
convert_mission_item_to_sp(_current_vel);
_navigator->set_position_setpoint_triplet_updated();
warnx("updating x %f y %f %f", (double)_current_vel(0), (double)_current_vel(1), (double) _steps);
_last_publish_time = current_time;
if ((current_time - _last_message_time) / (1000*1000) > 5) {
// on_activation();
static int gg = 0;
if(!(gg++%100)) {
warnx("timed out loitering %llu %d", (current_time - _last_message_time) / (1000*1000), _navigator->_tracker_motion_position_sub);
}
}
if ((((double) (current_time - _last_publish_time) * 1e-3) >= 100) && (follow_target_reached == true)) {
if (_current_vel(0) <= target_vel(0)) {
_current_vel(0) += (_steps);
}
if (_current_vel(0) >= target_vel(0)) {
_current_vel(0) -= (_steps);
}
if (_current_vel(1) <= target_vel(1)) {
_current_vel(1) += (_steps);
}
if (_current_vel(1) >= target_vel(1)) {
_current_vel(1) -= (_steps);
}
update_position_sp(_current_vel);
warnx("updating x %f m y %f m x %f m/s y %f m/s %f ( uav gs %f)", (double )target_dist_x, (double )target_dist_y,
(double )_current_vel(0), (double )_current_vel(1), (double ) _steps,
(double ) sqrtf(_navigator->get_global_position()->vel_e * _navigator->get_global_position()->vel_e + _navigator->get_global_position()->vel_n * _navigator->get_global_position()->vel_n));
_last_publish_time = current_time;
}
}
void
FollowTarget::convert_mission_item_to_sp(Vector2f & vel) {
FollowTarget::update_position_sp(Vector2f & vel) {
/* convert mission item to current setpoint */
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
// activate line following in pos control
pos_sp_triplet->previous.valid = false;
pos_sp_triplet->previous.valid = true;
pos_sp_triplet->previous = pos_sp_triplet->current;
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->current.vx = vel(0);