PX4 Firmware
PX4 Autopilot Software http://px4.io
follow_target.h
Go to the documentation of this file.
1 /***************************************************************************
2  *
3  * Copyright (c) 2016 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 /**
34  * @file followme.cpp
35  *
36  * Helper class to track and follow a given position
37  *
38  * @author Jimmy Johnson <catch22@fastmail.net>
39  */
40 
41 #pragma once
42 
43 #include "navigator_mode.h"
44 #include "mission_block.h"
45 
46 #include <mathlib/mathlib.h>
47 #include <matrix/math.hpp>
48 
49 #include <px4_platform_common/module_params.h>
50 #include <uORB/Subscription.hpp>
52 
53 class FollowTarget : public MissionBlock, public ModuleParams
54 {
55 
56 public:
58  ~FollowTarget() = default;
59 
60  void on_inactive() override;
61  void on_activation() override;
62  void on_active() override;
63 
64 private:
65 
66  static constexpr int TARGET_TIMEOUT_MS = 2500;
67  static constexpr int TARGET_ACCEPTANCE_RADIUS_M = 5;
68  static constexpr int INTERPOLATION_PNTS = 20;
69  static constexpr float FF_K = .25F;
70  static constexpr float OFFSET_M = 8;
71 
77  };
78 
79  enum {
84  };
85 
86  static constexpr float _follow_position_matricies[4][9] = {
87  { 1.0F, -1.0F, 0.0F, 1.0F, 1.0F, 0.0F, 0.0F, 0.0F, 1.0F}, // follow right
88  {-1.0F, 0.0F, 0.0F, 0.0F, -1.0F, 0.0F, 0.0F, 0.0F, 1.0F}, // follow behind
89  { 1.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 1.0F}, // follow front
90  { 1.0F, 1.0F, 0.0F, -1.0F, 1.0F, 0.0F, 0.0F, 0.0F, 1.0F} // follow left side
91  };
92 
94  (ParamFloat<px4::params::NAV_MIN_FT_HT>) _param_nav_min_ft_ht,
95  (ParamFloat<px4::params::NAV_FT_DST>) _param_nav_ft_dst,
96  (ParamInt<px4::params::NAV_FT_FS>) _param_nav_ft_fs,
97  (ParamFloat<px4::params::NAV_FT_RS>) _param_nav_ft_rs
98  )
99 
100  FollowTargetState _follow_target_state{SET_WAIT_FOR_TARGET_POSITION};
102 
104  float _step_time_in_ms{0.0f};
105  float _follow_offset{OFFSET_M};
106 
107  uint64_t _target_updates{0};
108  uint64_t _last_update_time{0};
109 
117 
120 
121  float _yaw_rate{0.0f};
122  float _responsiveness{0.0f};
123  float _yaw_angle{0.0f};
124 
125  // Mavlink defined motion reporting capabilities
126  enum {
127  POS = 0,
128  VEL = 1,
129  ACCEL = 2,
131  };
132 
134 
135  void track_target_position();
136  void track_target_velocity();
137  bool target_velocity_valid();
138  bool target_position_valid();
139  void reset_target_validity();
140  void update_position_sp(bool velocity_valid, bool position_valid, float yaw_rate);
141  void update_target_motion();
142  void update_target_velocity();
143 
144  /**
145  * Set follow_target item
146  */
147  void set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target, float yaw);
148 };
matrix::Dcmf _rot_matrix
Global position setpoint in WGS84 coordinates.
Definition: navigation.h:145
static constexpr int INTERPOLATION_PNTS
Definition: follow_target.h:68
static constexpr float FF_K
Definition: follow_target.h:69
matrix::Vector3f _target_position_offset
void update_target_motion()
bool target_velocity_valid()
void set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target, float yaw)
Set follow_target item.
Helper class to use mission items.
void on_active() override
This function is called while the mode is active.
FollowTarget(Navigator *navigator)
int _follow_target_position
void track_target_position()
matrix::Vector3f _filtered_target_position_delta
matrix::Vector3f _step_vel
void on_inactive() override
This function is called while the mode is inactive.
DEFINE_PARAMETERS((ParamFloat< px4::params::NAV_MIN_FT_HT >) _param_nav_min_ft_ht,(ParamFloat< px4::params::NAV_FT_DST >) _param_nav_ft_dst,(ParamInt< px4::params::NAV_FT_FS >) _param_nav_ft_fs,(ParamFloat< px4::params::NAV_FT_RS >) _param_nav_ft_rs) FollowTargetState _follow_target_state
Definition: follow_target.h:93
static constexpr float OFFSET_M
Definition: follow_target.h:70
uORB::Subscription _follow_target_sub
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
bool target_position_valid()
void update_target_velocity()
void track_target_velocity()
follow_target_s _current_target_motion
uint64_t _target_updates
void reset_target_validity()
void update_position_sp(bool velocity_valid, bool position_valid, float yaw_rate)
static constexpr int TARGET_TIMEOUT_MS
Definition: follow_target.h:66
float _follow_offset
matrix::Vector3f _current_vel
matrix::Vector3f _target_distance
void on_activation() override
This function is called one time when mode becomes active, pos_sp_triplet must be initialized here...
static constexpr int TARGET_ACCEPTANCE_RADIUS_M
Definition: follow_target.h:67
uint64_t _last_update_time
float _responsiveness
~FollowTarget()=default
matrix::Vector3f _target_position_delta
float _step_time_in_ms
matrix::Vector3f _est_target_vel
static constexpr float _follow_position_matricies[4][9]
Definition: follow_target.h:86
follow_target_s _previous_target_motion