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

follow me working

This commit is contained in:
Jimmy Johnson 2016-03-09 15:31:50 -08:00 committed by Lorenz Meier
parent 01e971b342
commit 26cb645ee0
7 changed files with 34 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
msg/follow_target.msg
View File

@ -1,11 +1,4 @@
uint64 timestamp # timestamp, UNIX epoch (GPS synced)
#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

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

@ -127,6 +127,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_manual_pub(nullptr),
_land_detector_pub(nullptr),
_time_offset_pub(nullptr),
_follow_me_pub(nullptr),
_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
_hil_frames(0),
_old_timestamp(0),
@ -1627,27 +1628,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 = {};
mavlink_follow_target_t follow_target_msg;
follow_target_s follow_target_topic = { };
//mavlink_msg_follow_target_decode(msg, &follow_target_msg);
mavlink_msg_follow_target_decode(msg, &follow_target_msg);
follow_target.timestamp = hrt_absolute_time();
follow_target_topic.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));
// 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;
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;
if (_follow_me_pub == nullptr) {
_follow_me_pub = orb_advertise(ORB_ID(follow_target), &follow_target);
_follow_me_pub = orb_advertise(ORB_ID(follow_target), &follow_target_topic);
} else {
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);
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);
}
}

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

@ -51,6 +51,7 @@
#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"
@ -59,7 +60,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),
@ -92,6 +93,10 @@ 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());
@ -121,11 +126,10 @@ FollowTarget::on_active() {
_current_vel(1) = _navigator->get_global_position()->vel_e;
}
orb_check(_navigator->_tracker_motion_position_sub, &updated);
orb_check(_tracker_motion_position_sub, &updated);
if (updated) {
warnx("UPDASTD ");
if (orb_copy(ORB_ID(follow_target), _navigator->_tracker_motion_position_sub, &target) == OK) {
if (orb_copy(ORB_ID(follow_target), _tracker_motion_position_sub, &target) == OK) {
float dt = ((double)(current_time - _last_message_time) * 1e-6);
@ -173,14 +177,8 @@ FollowTarget::on_active() {
sqrtf(target_vel(0)*target_vel(0) + target_vel(1)*target_vel(1))))) / (double) (dt*10);
}
}
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);
}
} else if (((double)(current_time - _last_message_time) * 1e-6) > 10) {
on_activation();
}
if ((((double) (current_time - _last_publish_time) * 1e-3) >= 100) && (follow_target_reached == true)) {

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

@ -63,6 +63,7 @@ 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;

1
src/modules/navigator/loiter.cpp
View File

@ -79,6 +79,7 @@ Loiter::on_activation()
/* convert mission item to current setpoint */
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->current.vx = pos_sp_triplet->current.vy = pos_sp_triplet->current.vz = 0;
pos_sp_triplet->previous.valid = false;
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;

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

@ -57,7 +57,6 @@
#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"
@ -75,7 +74,7 @@
/**
* Number of navigation modes that need on_active/on_inactive calls
*/
#define NAVIGATOR_MODE_ARRAY_SIZE 9
#define NAVIGATOR_MODE_ARRAY_SIZE 10
class Navigator : public control::SuperBlock
{
@ -183,7 +182,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 */
@ -203,7 +202,6 @@ 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;
@ -220,7 +218,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;
@ -253,7 +251,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 */
@ -301,7 +299,7 @@ private:
* Retrieve vehicle control mode
*/
void vehicle_control_mode_update();
void follow_target_update();
/**
* Update parameters
*/

20
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,7 +157,6 @@ 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;
@ -168,7 +167,7 @@ Navigator::Navigator() :
_navigation_mode_array[6] = &_rcLoss;
_navigation_mode_array[7] = &_takeoff;
_navigation_mode_array[8] = &_land;
//_navigation_mode_array[9] = nullptr;//&_follow_target;
_navigation_mode_array[9] = &_follow_target;
updateParams();
}
@ -216,12 +215,6 @@ Navigator::sensor_combined_update()
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
Navigator::home_position_update(bool force)
{
@ -303,7 +296,6 @@ 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();
@ -394,12 +386,6 @@ 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;
@ -517,7 +503,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;