Line data Source code
1 : #include "Rover.h"
2 :
3 : #if MODE_DOCK_ENABLED
4 :
5 : const AP_Param::GroupInfo ModeDock::var_info[] = {
6 : // @Param: _SPEED
7 : // @DisplayName: Dock mode speed
8 : // @Description: Vehicle speed limit in dock mode
9 : // @Units: m/s
10 : // @Range: 0 100
11 : // @Increment: 0.1
12 : // @User: Standard
13 : AP_GROUPINFO("_SPEED", 1, ModeDock, speed, 0.0f),
14 :
15 : // @Param: _DIR
16 : // @DisplayName: Dock mode direction of approach
17 : // @Description: Compass direction in which vehicle should approach towards dock. -1 value represents unset parameter
18 : // @Units: deg
19 : // @Range: 0 360
20 : // @Increment: 1
21 : // @User: Advanced
22 : AP_GROUPINFO("_DIR", 2, ModeDock, desired_dir, -1.00f),
23 :
24 : // @Param: _HDG_CORR_EN
25 : // @DisplayName: Dock mode heading correction enable/disable
26 : // @Description: When enabled, the autopilot modifies the path to approach the target head-on along desired line of approach in dock mode
27 : // @Values: 0:Disabled,1:Enabled
28 : // @User: Advanced
29 : AP_GROUPINFO("_HDG_CORR_EN", 3, ModeDock, hdg_corr_enable, 0),
30 :
31 : // @Param: _HDG_CORR_WT
32 : // @DisplayName: Dock mode heading correction weight
33 : // @Description: This value describes how aggressively vehicle tries to correct its heading to be on desired line of approach
34 : // @Range: 0.00 0.90
35 : // @Increment: 0.05
36 : // @User: Advanced
37 : AP_GROUPINFO("_HDG_CORR_WT", 4, ModeDock, hdg_corr_weight, 0.75f),
38 :
39 : // @Param: _STOP_DIST
40 : // @DisplayName: Distance from docking target when we should stop
41 : // @Description: The vehicle starts stopping when it is this distance away from docking target
42 : // @Units: m
43 : // @Range: 0 2
44 : // @Increment: 0.01
45 : // @User: Standard
46 : AP_GROUPINFO("_STOP_DIST", 5, ModeDock, stopping_dist, 0.30f),
47 :
48 : AP_GROUPEND
49 : };
50 :
51 285 : ModeDock::ModeDock(void) : Mode()
52 : {
53 285 : AP_Param::setup_object_defaults(this, var_info);
54 285 : }
55 :
56 : #define AR_DOCK_ACCEL_MAX 20.0 // acceleration used when user has specified no acceleration limit
57 :
58 : // Return true if this mode is enabled
59 12529 : bool ModeDock::enabled() const
60 : {
61 : // Dock mode requires precland
62 12529 : return rover.precland.enabled();
63 : }
64 :
65 : // initialize dock mode
66 3 : bool ModeDock::_enter()
67 : {
68 : // refuse to enter the mode if dock is not in sight
69 3 : if (!enabled() || !rover.precland.target_acquired()) {
70 0 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Dock: target not acquired");
71 0 : return false;
72 : }
73 :
74 3 : if (hdg_corr_enable && is_negative(desired_dir)) {
75 : // DOCK_DIR is required for heading correction
76 0 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Dock: Set DOCK_DIR or disable heading correction");
77 0 : return false;
78 : }
79 :
80 : // set speed limit to dock_speed param if available
81 : // otherwise the vehicle uses wp_nav default speed limit
82 3 : const float speed_max = is_positive(speed) ? speed : g2.wp_nav.get_default_speed();
83 3 : float atc_accel_max = MIN(g2.attitude_control.get_accel_max(), g2.attitude_control.get_decel_max());
84 3 : if (!is_positive(atc_accel_max)) {
85 : // accel_max of zero means no limit so use maximum acceleration
86 0 : atc_accel_max = AR_DOCK_ACCEL_MAX;
87 : }
88 3 : const float accel_max = is_positive(g2.wp_nav.get_default_accel()) ? MIN(g2.wp_nav.get_default_accel(), atc_accel_max) : atc_accel_max;
89 3 : const float jerk_max = is_positive(g2.wp_nav.get_default_jerk()) ? g2.wp_nav.get_default_jerk() : accel_max;
90 :
91 : // initialise position controller
92 3 : g2.pos_control.set_limits(speed_max, accel_max, g2.attitude_control.get_turn_lat_accel_max(), jerk_max);
93 3 : g2.pos_control.init();
94 :
95 : // set the position controller reversed in case the camera is mounted on vehicle's back
96 3 : g2.pos_control.set_reversed(rover.precland.get_orient() == 4);
97 :
98 : // construct unit vector in the desired direction from where we want the vehicle to approach the dock
99 : // this helps to dock the vehicle head-on even when we enter the dock mode at an angle towards the dock
100 3 : _desired_heading_NE = Vector2f{cosf(radians(desired_dir)), sinf(radians(desired_dir))};
101 :
102 3 : _docking_complete = false;
103 :
104 3 : return true;
105 : }
106 :
107 6731 : void ModeDock::update()
108 : {
109 : // if docking is complete, rovers stop and boats loiter
110 6731 : if (_docking_complete) {
111 : // rovers stop, boats loiter
112 : // note that loiter update must be called after successful initialisation on mode loiter
113 0 : if (_loitering) {
114 : // mode loiter must be initialised before calling update method
115 0 : rover.mode_loiter.update();
116 : } else {
117 0 : stop_vehicle();
118 : }
119 0 : return;
120 : }
121 :
122 6731 : const bool real_dock_in_sight = rover.precland.get_target_position_m(_dock_pos_rel_origin_m);
123 6731 : Vector2f dock_pos_rel_vehicle_m;
124 6731 : if (!calc_dock_pos_rel_vehicle_NE_m(dock_pos_rel_vehicle_m)) {
125 0 : g2.motors.set_throttle(0.0f);
126 0 : g2.motors.set_steering(0.0f);
127 0 : return;
128 : }
129 :
130 6731 : _distance_to_destination = dock_pos_rel_vehicle_m.length();
131 :
132 : // we force the vehicle to use real dock as target when we are too close to the dock
133 : // note that heading correction does not work when we force real target
134 6731 : const bool force_real_target = _distance_to_destination < _force_real_target_limit_m;
135 :
136 : // if we are close enough to the dock or the target is not in sight when we strictly require it
137 : // we mark the docking to be completed so that the vehicle stops
138 6731 : if (_distance_to_destination <= stopping_dist || (force_real_target && !real_dock_in_sight)) {
139 0 : _docking_complete = true;
140 :
141 : // send a one time notification to GCS
142 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Dock: Docking complete");
143 :
144 : // initialise mode loiter if it is a boat
145 0 : if (rover.is_boat()) {
146 : // if we fail to enter, we set _loitering to false
147 0 : _loitering = rover.mode_loiter.enter();
148 : }
149 0 : return;
150 : }
151 :
152 6731 : Vector2f target_m = _dock_pos_rel_origin_m.tofloat();
153 :
154 : // ***** HEADING CORRECTION *****
155 : // to make to vehicle dock from a given direction we simulate a virtual moving target on the line of approach
156 : // this target is always at DOCK_HDG_COR_WT times the distance from dock to vehicle (along the line of approach)
157 : // For e.g., if the dock is 100 m away form dock, DOCK_HDG_COR_WT is 0.75
158 : // then the position target is 75 m from the dock, i.e., 25 m from the vehicle
159 : // as the vehicle tries to reach this target, this target appears to move towards the dock and at last it is sandwiched b/w dock and vehicle
160 : // since this target is moving along desired direction of approach, the vehicle also comes on that line while following it
161 6731 : if (!force_real_target && hdg_corr_enable) {
162 0 : const float correction_vec_mag = hdg_corr_weight * dock_pos_rel_vehicle_m.projected(_desired_heading_NE).length();
163 0 : target_m = _dock_pos_rel_origin_m.tofloat() - _desired_heading_NE * correction_vec_mag;
164 : }
165 :
166 6731 : const Vector2p target_pos { target_m.topostype() };
167 6731 : g2.pos_control.input_pos_target(target_pos, rover.G_Dt);
168 6731 : g2.pos_control.update(rover.G_Dt);
169 :
170 : // get desired speed and turn rate from pos_control
171 6731 : float desired_speed = g2.pos_control.get_desired_speed();
172 6731 : float desired_turn_rate = g2.pos_control.get_desired_turn_rate_rads();
173 :
174 : // slow down the vehicle as we approach the dock
175 6731 : desired_speed = apply_slowdown(desired_speed);
176 :
177 : // run steering and throttle controllers
178 6731 : calc_steering_from_turn_rate(desired_turn_rate);
179 6731 : calc_throttle(desired_speed, true);
180 :
181 : #if HAL_LOGGING_ENABLED
182 : // @LoggerMessage: DOCK
183 : // @Description: Dock mode target information
184 : // @Field: TimeUS: Time since system startup
185 : // @Field: DockX: Docking Station position, X-Axis
186 : // @Field: DockY: Docking Station position, Y-Axis
187 : // @Field: DockDist: Distance to docking station
188 : // @Field: TPosX: Current Position Target, X-Axis
189 : // @Field: TPosY: Current Position Target, Y-Axis
190 : // @Field: DSpd: Desired speed
191 : // @Field: DTrnRt: Desired Turn Rate
192 :
193 13462 : AP::logger().WriteStreaming(
194 : "DOCK",
195 : "TimeUS,DockX,DockY,DockDist,TPosX,TPosY,DSpd,DTrnRt",
196 : "smmmmmnE",
197 : "F0000000",
198 : "Qfffffff",
199 : AP_HAL::micros64(),
200 6731 : (float)_dock_pos_rel_origin_m.x,
201 6731 : (float)_dock_pos_rel_origin_m.y,
202 6731 : _distance_to_destination,
203 6731 : target_m.x,
204 6731 : target_m.y,
205 : desired_speed,
206 : desired_turn_rate);
207 : #endif
208 : }
209 :
210 6731 : float ModeDock::apply_slowdown(float desired_speed)
211 : {
212 6731 : const float dock_speed_slowdown_lmt = 0.5f;
213 :
214 : // no slowdown for speed below dock_speed_slowdown_lmt
215 6731 : if (fabsf(desired_speed) < dock_speed_slowdown_lmt) {
216 1385 : return desired_speed;
217 : }
218 :
219 5346 : Vector3f target_vec_rel_vehicle_ned_m;
220 5346 : if(!calc_dock_pos_rel_vehicle_NE_m(target_vec_rel_vehicle_ned_m.xy())) {
221 0 : return desired_speed;
222 : }
223 :
224 5346 : const Matrix3f &body_to_ned = AP::ahrs().get_rotation_body_to_ned();
225 5346 : Vector3f target_vec_body = body_to_ned.mul_transpose(target_vec_rel_vehicle_ned_m);
226 5346 : const float target_error_m = fabsf(target_vec_body.y);
227 5346 : float error_ratio = target_error_m / _acceptable_pos_error_m;
228 5346 : error_ratio = constrain_float(error_ratio, 0.0f, 1.0f);
229 :
230 5346 : const float dock_slow_dist_max_m = 15.0f;
231 5346 : const float dock_slow_dist_min_m = 5.0f;
232 : // approach slowdown is not applied when the vehicle is more than dock_slow_dist_max meters away
233 : // within dock_slow_dist_max and dock_slow_dist_min the weight of the slowdown increases linearly
234 : // once the vehicle reaches dock_slow_dist_min the slowdown weight becomes 1
235 5346 : float slowdown_weight = 1 - (target_vec_body.x * 0.01f - dock_slow_dist_min_m) / (dock_slow_dist_max_m - dock_slow_dist_min_m);
236 5346 : slowdown_weight = constrain_float(slowdown_weight, 0.0f, 1.0f);
237 :
238 5346 : desired_speed = MAX(dock_speed_slowdown_lmt, fabsf(desired_speed) * (1 - error_ratio * slowdown_weight));
239 :
240 : // restrict speed to avoid going beyond stopping distance
241 5346 : desired_speed = MIN(desired_speed, safe_sqrt(2 * fabsf(_distance_to_destination - stopping_dist) * g2.pos_control.get_accel_max()));
242 :
243 : // we worked on absolute value of speed before
244 : // make it negative again if reversed
245 5346 : if (g2.pos_control.get_reversed()) {
246 0 : desired_speed *= -1;
247 : }
248 :
249 5346 : return desired_speed;
250 : }
251 :
252 : // calculate position of dock relative to the vehicle
253 : // we need this method here because there can be a window during heading correction when we might lose the target
254 : // during that window precland won't be able to give us this vector
255 : // we can calculate it based on most recent value from precland because the dock is assumed stationary wrt ekf origin
256 12077 : bool ModeDock::calc_dock_pos_rel_vehicle_NE_m(Vector2f &dock_pos_rel_vehicle_m) const {
257 12077 : Vector2f current_pos_m;
258 12077 : if (!AP::ahrs().get_relative_position_NE_origin_float(current_pos_m)) {
259 0 : return false;
260 : }
261 :
262 12077 : dock_pos_rel_vehicle_m = _dock_pos_rel_origin_m.tofloat() - current_pos_m;
263 12077 : return true;
264 : }
265 : #endif // MODE_DOCK_ENABLED
|
