dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Tests

This commit is contained in:
Jimmy Johnson 2016-03-02 20:27:07 -08:00 committed by Lorenz Meier
parent 0797c7fc21
commit 01e971b342
8 changed files with 124 additions and 93 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
msg/follow_target.msg
View File

@ -1,11 +1,11 @@
uint64 timestamp # timestamp, UNIX epoch (GPS synced)
uint8 est_capabilities # Flags to indicate which quantities the tracker knows about: (POS, VEL, ACCEL, ATT + RATES)
#uint8 est_capabilities # Flags to indicate which quantities the tracker knows about: (POS, VEL, ACCEL, ATT + RATES)
float64 lat # target position (deg * 1e7)
float64 lon # target position (deg * 1e7)
float32 alt # target position
float32[3] velocity # target velocity (AMSL, meters) # target position (0 0 0 for unknown)
float32[3] accel # target acceleration (linear) in earth frame.
float32[4] attitude_q # where the target is facing.
float32[3] rates # (0 0 0 for unknown)
float32[3] pos_cov # uncertainly in earth frame for X, Y and Z. We will need to agree on the exact format here -1 for unknown
uint64 custom_state # A custom vector, can be used to transmit e.g. button states or switches of a tracker device
#float32[3] velocity # target velocity (AMSL, meters) # target position (0 0 0 for unknown)
#float32[3] accel # target acceleration (linear) in earth frame.
#float32[4] attitude_q # where the target is facing.
#float32[3] rates # (0 0 0 for unknown)
#float32[3] pos_cov # uncertainly in earth frame for X, Y and Z. We will need to agree on the exact format here -1 for unknown
#uint64 custom_state # A custom vector, can be used to transmit e.g. button states or switches of a tracker device

28
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -1627,27 +1627,27 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
void MavlinkReceiver::handle_message_follow_target(mavlink_message_t *msg)
{
mavlink_follow_target_t follow_target_msg;
follow_target_s follow_target_topic = { };
// mavlink_follow_target_t follow_target_msg;
follow_target_s follow_target = {};
mavlink_msg_follow_target_decode(msg, &follow_target_msg);
//mavlink_msg_follow_target_decode(msg, &follow_target_msg);
follow_target_topic.timestamp = hrt_absolute_time();
follow_target.timestamp = hrt_absolute_time();
memcpy(follow_target_topic.accel, follow_target_msg.acc, sizeof(follow_target_topic.accel));
memcpy(follow_target_topic.velocity, follow_target_msg.vel, sizeof(follow_target_topic.velocity));
// memcpy(follow_target_topic.accel, follow_target_msg.acc, sizeof(follow_target_topic.accel));
//memcpy(follow_target_topic.velocity, follow_target_msg.vel, sizeof(follow_target_topic.velocity));
follow_target_topic.lat = follow_target_msg.lat*1e-7;
follow_target_topic.lon = follow_target_msg.lon*1e-7;
follow_target_topic.alt = follow_target_msg.alt;
// follow_target.lat = follow_target_msg.lat*1e-7;
// follow_target.lon = follow_target_msg.lon*1e-7;
// follow_target.alt = follow_target_msg.alt;
if (_follow_me_pub == nullptr) {
_follow_me_pub = orb_advertise(ORB_ID(follow_target), &follow_target_topic);
_follow_me_pub = orb_advertise(ORB_ID(follow_target), &follow_target);
} else {
warnx("publishing follow lat = %f lon = %f alt = %f", (double) follow_target_topic.lat,
(double) follow_target_topic.lon,
(double) follow_target_topic.alt);
orb_publish(ORB_ID(follow_target), _follow_me_pub, &follow_target_topic);
warnx("publishing follow lat = %f lon = %f alt = %f", (double) follow_target.lat,
(double) follow_target.lon,
(double) follow_target.alt);
orb_publish(ORB_ID(follow_target), _follow_me_pub, &follow_target);
}
}

4
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -1369,8 +1369,8 @@ MulticopterPositionControl::task_main()
} else {
/* run position & altitude controllers, if enabled (otherwise use already computed velocity setpoints) */
if (_run_pos_control) {
_vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0);
_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1);
_vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0) + _pos_sp_triplet.current.vx;
_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1) + _pos_sp_triplet.current.vy;
}
if (_run_alt_control) {

128
src/modules/navigator/follow_target.cpp
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);

7
src/modules/navigator/follow_target.h
View File

@ -63,7 +63,6 @@ public:
private:
Navigator *_navigator;
int _tracker_motion_position_sub; /**< tracker motion subscription */
control::BlockParamFloat _param_min_alt;
matrix::Vector2f pos_pair[2];
matrix::Vector2f gps_pair;
@ -83,5 +82,9 @@ private:
int wp_cnt;
matrix::Vector2f _current_vel;
matrix::Vector2f target_vel;
void convert_mission_item_to_sp(matrix::Vector2f & vel);
void update_position_sp(matrix::Vector2f & vel);
void loiter();
float target_dist_x;
float target_dist_y;
follow_target_s target;
};

2
src/modules/navigator/mission_block.cpp
View File

@ -453,7 +453,7 @@ MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clea
item->lon = target.lon;
item->altitude = target.alt;
if (min_clearance > 0.0f && item->altitude < _navigator->get_home_position()->alt + min_clearance) {
if (((min_clearance > 0.0f) && (item->altitude < _navigator->get_home_position()->alt + min_clearance)) || PX4_ISFINITE(target.alt)) {
item->altitude = _navigator->get_home_position()->alt + min_clearance;
}
}

12
src/modules/navigator/navigator.h
View File

@ -57,6 +57,7 @@
#include <uORB/topics/mission_result.h>
#include <uORB/topics/geofence_result.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/follow_target.h>
#include "navigator_mode.h"
#include "mission.h"
@ -74,7 +75,7 @@
/**
* Number of navigation modes that need on_active/on_inactive calls
*/
#define NAVIGATOR_MODE_ARRAY_SIZE 10
#define NAVIGATOR_MODE_ARRAY_SIZE 9
class Navigator : public control::SuperBlock
{
@ -182,7 +183,7 @@ public:
void increment_mission_instance_count() { _mission_instance_count++; }
void set_mission_failure(const char *reason);
int _tracker_motion_position_sub; /**< tracker motion subscription */
private:
bool _task_should_exit; /**< if true, sensor task should exit */
@ -202,6 +203,7 @@ private:
int _param_update_sub; /**< param update subscription */
int _vehicle_command_sub; /**< vehicle commands (onboard and offboard) */
orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */
orb_advert_t _mission_result_pub;
orb_advert_t _geofence_result_pub;
@ -218,7 +220,7 @@ private:
mission_item_s _mission_item; /**< current mission item */
navigation_capabilities_s _nav_caps; /**< navigation capabilities */
position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */
follow_target_s _follow_target_pk;
mission_result_s _mission_result;
geofence_result_s _geofence_result;
vehicle_attitude_setpoint_s _att_sp;
@ -251,7 +253,7 @@ private:
(FW only!) */
GpsFailure _gpsFailure; /**< class that handles the OBC gpsfailure loss mode */
FollowTarget _follow_target;
// FollowTarget _follow_target;
NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */
@ -299,7 +301,7 @@ private:
* Retrieve vehicle control mode
*/
void vehicle_control_mode_update();
void follow_target_update();
/**
* Update parameters
*/

22
src/modules/navigator/navigator_main.cpp
View File

@ -148,7 +148,7 @@ Navigator::Navigator() :
_dataLinkLoss(this, "DLL"),
_engineFailure(this, "EF"),
_gpsFailure(this, "GPSF"),
_follow_target(this, "TAR"),
// _follow_target(this, "TAR"),
_param_loiter_radius(this, "LOITER_RAD"),
_param_acceptance_radius(this, "ACC_RAD"),
_param_datalinkloss_obc(this, "DLL_OBC"),
@ -157,6 +157,7 @@ Navigator::Navigator() :
_param_cruising_speed_plane(this, "FW_AIRSPD_TRIM", false),
_mission_cruising_speed(-1.0f)
{
_tracker_motion_position_sub = -1;
/* Create a list of our possible navigation types */
_navigation_mode_array[0] = &_mission;
_navigation_mode_array[1] = &_loiter;
@ -167,7 +168,7 @@ Navigator::Navigator() :
_navigation_mode_array[6] = &_rcLoss;
_navigation_mode_array[7] = &_takeoff;
_navigation_mode_array[8] = &_land;
_navigation_mode_array[9] = &_follow_target;
//_navigation_mode_array[9] = nullptr;//&_follow_target;
updateParams();
}
@ -212,7 +213,13 @@ Navigator::gps_position_update()
void
Navigator::sensor_combined_update()
{
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
}
void
Navigator::follow_target_update()
{
orb_copy(ORB_ID(follow_target), _tracker_motion_position_sub, &_follow_target_pk);
}
void
@ -296,6 +303,7 @@ Navigator::task_main()
_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
_tracker_motion_position_sub = orb_subscribe(ORB_ID(follow_target));
/* copy all topics first time */
vehicle_status_update();
@ -386,6 +394,12 @@ Navigator::task_main()
home_position_update();
}
orb_check(_tracker_motion_position_sub, &updated);
if(updated) {
follow_target_update();
warnx("updated in nav");
}
orb_check(_vehicle_command_sub, &updated);
if (updated) {
vehicle_command_s cmd;
@ -503,7 +517,7 @@ Navigator::task_main()
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_follow_target;
// _navigation_mode = &_follow_target;
break;
default:
_navigation_mode = nullptr;