Line data Source code
1 : #include "Rover.h"
2 :
3 : #define AR_CIRCLE_ACCEL_DEFAULT 1.0 // default acceleration in m/s/s if not specified by user
4 : #define AR_CIRCLE_RADIUS_MIN 0.1 // minimum radius in meters
5 :
6 : const AP_Param::GroupInfo ModeCircle::var_info[] = {
7 :
8 : // @Param: _RADIUS
9 : // @DisplayName: Circle Radius
10 : // @Description: Vehicle will circle the center at this distance
11 : // @Units: m
12 : // @Range: 0 100
13 : // @Increment: 1
14 : // @User: Standard
15 : AP_GROUPINFO("_RADIUS", 1, ModeCircle, radius, 20),
16 :
17 : // @Param: _SPEED
18 : // @DisplayName: Circle Speed
19 : // @Description: Vehicle will move at this speed around the circle. If set to zero WP_SPEED will be used
20 : // @Units: m/s
21 : // @Range: 0 10
22 : // @Increment: 0.1
23 : // @User: Standard
24 : AP_GROUPINFO("_SPEED", 2, ModeCircle, speed, 0),
25 :
26 : // @Param: _DIR
27 : // @DisplayName: Circle Direction
28 : // @Description: Circle Direction
29 : // @Values: 0:Clockwise, 1:Counter-Clockwise
30 : // @User: Standard
31 : AP_GROUPINFO("_DIR", 3, ModeCircle, direction, 0),
32 :
33 : AP_GROUPEND
34 : };
35 :
36 285 : ModeCircle::ModeCircle() : Mode()
37 : {
38 285 : AP_Param::setup_object_defaults(this, var_info);
39 285 : }
40 :
41 : // get the distance at which the vehicle is considered to be on track along the circle
42 3485 : float ModeCircle::get_reached_distance() const
43 : {
44 3485 : if (config.radius >= 0.5) {
45 3485 : return 0.2;
46 : }
47 0 : return 0.1;
48 : }
49 :
50 : // initialise with specific center location, radius (in meters) and direction
51 : // replaces use of _enter when initialised from within Auto mode
52 2 : bool ModeCircle::set_center(const Location& center_loc, float radius_m, bool dir_ccw)
53 : {
54 2 : Vector2f center_pos_cm;
55 2 : if (!center_loc.get_vector_xy_from_origin_NE_cm(center_pos_cm)) {
56 0 : return false;
57 : }
58 2 : if (!_enter()) {
59 0 : return false;
60 : }
61 :
62 : // convert center position from cm to m
63 2 : config.center_pos = center_pos_cm * 0.01;
64 :
65 : // set radius
66 2 : config.radius = MAX(fabsf(radius_m), AR_CIRCLE_RADIUS_MIN);
67 2 : check_config_radius();
68 :
69 : // set direction
70 2 : config.dir = dir_ccw ? Direction::CCW : Direction::CW;
71 :
72 : // set target yaw rad (target point on circle)
73 2 : init_target_yaw_rad();
74 :
75 : // record center as location (only used for reporting)
76 2 : config.center_loc = center_loc;
77 :
78 2 : return true;
79 : }
80 :
81 : // initialize circle mode from current position
82 2 : bool ModeCircle::_enter()
83 : {
84 : // capture starting point and yaw
85 2 : if (!AP::ahrs().get_relative_position_NE_origin_float(config.center_pos)) {
86 0 : return false;
87 : }
88 2 : config.radius = MAX(fabsf(radius), AR_CIRCLE_RADIUS_MIN);
89 2 : check_config_radius();
90 :
91 2 : config.dir = (direction == 1) ? Direction::CCW : Direction::CW;
92 2 : config.speed = is_positive(speed) ? speed : g2.wp_nav.get_default_speed();
93 2 : target.yaw_rad = AP::ahrs().get_yaw_rad();
94 2 : target.speed = 0;
95 :
96 : // record center as location (only used for reporting)
97 2 : config.center_loc = rover.current_loc;
98 :
99 : // check speed around circle does not lead to excessive lateral acceleration
100 2 : check_config_speed();
101 :
102 : // reset tracking_back
103 2 : tracking_back = false;
104 :
105 : // calculate speed, accel and jerk limits
106 : // otherwise the vehicle uses wp_nav default speed limit
107 2 : float atc_accel_max = MIN(g2.attitude_control.get_accel_max(), g2.attitude_control.get_decel_max());
108 2 : if (!is_positive(atc_accel_max)) {
109 0 : atc_accel_max = AR_CIRCLE_ACCEL_DEFAULT;
110 : }
111 2 : 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;
112 2 : const float jerk_max = is_positive(g2.wp_nav.get_default_jerk()) ? g2.wp_nav.get_default_jerk() : accel_max;
113 :
114 : // initialise position controller
115 2 : g2.pos_control.set_limits(config.speed, accel_max, g2.attitude_control.get_turn_lat_accel_max(), jerk_max);
116 2 : g2.pos_control.init();
117 :
118 : // initialise angles covered and reached_edge state
119 2 : angle_total_rad = 0;
120 2 : reached_edge = false;
121 2 : dist_to_edge_m = 0;
122 :
123 2 : return true;
124 : }
125 :
126 : // initialise target_yaw_rad using the vehicle's position and yaw
127 : // if there is no current position estimate target_yaw_rad is set to 0
128 2 : void ModeCircle::init_target_yaw_rad()
129 : {
130 : // if no position estimate use vehicle yaw
131 2 : Vector2f curr_pos_NE;
132 2 : if (!AP::ahrs().get_relative_position_NE_origin_float(curr_pos_NE)) {
133 0 : target.yaw_rad = AP::ahrs().get_yaw_rad();
134 0 : return;
135 : }
136 :
137 : // calc vector from circle center to vehicle
138 2 : Vector2f center_to_veh = (curr_pos_NE - config.center_pos);
139 2 : float dist_m = center_to_veh.length();
140 :
141 : // if current position is exactly at the center of the circle return vehicle yaw
142 2 : if (is_zero(dist_m)) {
143 0 : target.yaw_rad = AP::ahrs().get_yaw_rad();
144 : } else {
145 2 : target.yaw_rad = center_to_veh.angle();
146 : }
147 : }
148 :
149 1978 : void ModeCircle::update()
150 : {
151 : // get current position
152 1978 : Vector2f curr_pos;
153 1978 : const bool position_ok = AP::ahrs().get_relative_position_NE_origin_float(curr_pos);
154 :
155 : // if no position estimate stop vehicle
156 1978 : if (!position_ok) {
157 0 : stop_vehicle();
158 0 : return;
159 : }
160 :
161 : // Update distance to destination and distance to edge
162 1978 : const Vector2f center_to_veh = curr_pos - config.center_pos;
163 1978 : _distance_to_destination = (target.pos.tofloat() - curr_pos).length();
164 1978 : dist_to_edge_m = fabsf(center_to_veh.length() - config.radius);
165 :
166 : // Update depending on stage
167 1978 : if (!reached_edge) {
168 471 : update_drive_to_radius();
169 1507 : } else if (dist_to_edge_m > 2 * MAX(g2.turn_radius, get_reached_distance()) && !tracking_back) {
170 : // if more than 2* turn_radius outside circle radius, slow vehicle by 20%
171 0 : config.speed = 0.8 * config.speed;
172 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Circle: cannot keep up, slowing to %.1fm/s", config.speed);
173 0 : tracking_back = true;
174 1507 : } else if (dist_to_edge_m < MAX(g2.turn_radius, get_reached_distance()) && tracking_back) {
175 : // if within turn_radius, call the vehicle back on track
176 0 : tracking_back = false;
177 : } else {
178 1507 : update_circling();
179 : }
180 :
181 1978 : g2.pos_control.update(rover.G_Dt);
182 :
183 : // get desired speed and turn rate from pos_control
184 1978 : const float desired_speed = g2.pos_control.get_desired_speed();
185 1978 : const float desired_turn_rate = g2.pos_control.get_desired_turn_rate_rads();
186 :
187 : // run steering and throttle controllers
188 1978 : calc_steering_from_turn_rate(desired_turn_rate);
189 1978 : calc_throttle(desired_speed, true);
190 : }
191 :
192 471 : void ModeCircle::update_drive_to_radius()
193 : {
194 : // check if vehicle has reached edge of circle
195 471 : const float dist_thresh_m = MAX(g2.turn_radius, get_reached_distance());
196 471 : reached_edge |= dist_to_edge_m <= dist_thresh_m;
197 :
198 : // calculate target point's position, velocity and acceleration
199 471 : target.pos = config.center_pos.topostype();
200 471 : target.pos.offset_bearing(degrees(target.yaw_rad), config.radius);
201 :
202 471 : g2.pos_control.input_pos_target(target.pos, rover.G_Dt);
203 471 : }
204 :
205 : // Update position controller targets while circling
206 1507 : void ModeCircle::update_circling()
207 : {
208 :
209 : // accelerate speed up to desired speed
210 1507 : const float speed_change_max = (g2.pos_control.get_accel_max() * 0.5 * rover.G_Dt);
211 1507 : const float accel_fb = constrain_float(config.speed - target.speed, -speed_change_max, speed_change_max);
212 1507 : target.speed += accel_fb;
213 :
214 : // calculate angular rate and update target angle, if the vehicle is not trying to track back
215 1507 : if (!tracking_back) {
216 1507 : const float circumference = 2.0 * M_PI * config.radius;
217 1507 : const float angular_rate_rad = (target.speed / circumference) * M_2PI * (config.dir == Direction::CW ? 1.0 : -1.0);
218 1507 : const float angle_dt = angular_rate_rad * rover.G_Dt;
219 1507 : target.yaw_rad = wrap_PI(target.yaw_rad + angle_dt);
220 1507 : angle_total_rad += angle_dt;
221 : } else {
222 0 : init_target_yaw_rad();
223 : }
224 :
225 : // calculate target point's position, velocity and acceleration
226 1507 : target.pos = config.center_pos.topostype();
227 1507 : target.pos.offset_bearing(degrees(target.yaw_rad), config.radius);
228 :
229 : // velocity is perpendicular to angle from the circle's center to the target point on the edge of the circle
230 1507 : target.vel = Vector2f(target.speed, 0);
231 1507 : target.vel.rotate(target.yaw_rad + radians(90));
232 :
233 : // acceleration is towards center of circle and is speed^2 / radius
234 1507 : target.accel = Vector2f(-sq(target.speed) / config.radius, accel_fb / rover.G_Dt);
235 1507 : target.accel.rotate(target.yaw_rad);
236 :
237 1507 : g2.pos_control.set_pos_vel_accel_target(target.pos, target.vel, target.accel);
238 :
239 1507 : }
240 :
241 : // return desired heading (in degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)
242 828 : float ModeCircle::wp_bearing() const
243 : {
244 828 : Vector2f curr_pos_NE;
245 828 : if (!AP::ahrs().get_relative_position_NE_origin_float(curr_pos_NE)) {
246 0 : return 0;
247 : }
248 : // calc vector from circle center to vehicle
249 828 : Vector2f veh_to_center = (target.pos.tofloat() - curr_pos_NE);
250 828 : if (veh_to_center.is_zero()) {
251 0 : return 0;
252 : }
253 828 : return degrees(veh_to_center.angle());
254 : }
255 :
256 789 : float ModeCircle::nav_bearing() const
257 : {
258 : // get position error as a vector from the current position to the target position
259 789 : const Vector2p pos_error = g2.pos_control.get_pos_error();
260 789 : if (pos_error.is_zero()) {
261 79 : return 0;
262 : }
263 710 : return degrees(pos_error.angle());
264 : }
265 :
266 396 : float ModeCircle::get_desired_lat_accel() const
267 : {
268 396 : return g2.pos_control.get_desired_lat_accel();
269 : }
270 :
271 : // set desired speed in m/s
272 0 : bool ModeCircle::set_desired_speed(float speed_ms)
273 : {
274 0 : if (is_positive(speed_ms)) {
275 0 : config.speed = speed_ms;
276 :
277 : // check desired speed does not lead to excessive lateral acceleration
278 0 : check_config_speed();
279 :
280 : // update position controller limits if required
281 0 : if (config.speed > g2.pos_control.get_speed_max()) {
282 0 : g2.pos_control.set_limits(config.speed, g2.pos_control.get_accel_max(), g2.pos_control.get_lat_accel_max(), g2.pos_control.get_jerk_max());
283 : }
284 0 : return true;
285 : }
286 :
287 0 : return false;
288 : }
289 :
290 : // return desired location
291 393 : bool ModeCircle::get_desired_location(Location& destination) const
292 : {
293 393 : destination = config.center_loc;
294 393 : destination.offset_bearing(degrees(target.yaw_rad), config.radius);
295 393 : return true;
296 : }
297 :
298 : // limit config speed so that lateral acceleration is within limits
299 : // assumes that config.radius and attitude controller lat accel max have been set
300 : // outputs warning to user if speed is reduced
301 2 : void ModeCircle::check_config_speed()
302 : {
303 : // calculate maximum speed based on radius and max lateral acceleration max
304 2 : const float speed_max = MAX(safe_sqrt(g2.attitude_control.get_turn_lat_accel_max() * config.radius), 0);
305 :
306 2 : if (config.speed > speed_max) {
307 0 : config.speed = speed_max;
308 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Circle: max speed is %4.1f", (double)config.speed);
309 : }
310 2 : }
311 :
312 : // ensure config radius is no smaller then vehicle's TURN_RADIUS
313 : // assumes that config.radius has been set
314 : // radius is increased if necessary and warning is output to the user
315 4 : void ModeCircle::check_config_radius()
316 : {
317 : // ensure radius is at least as large as vehicle's turn radius
318 4 : if (config.radius < g2.turn_radius) {
319 0 : config.radius = g2.turn_radius;
320 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Circle: radius increased to TURN_RADIUS (%4.1f)", (double)g2.turn_radius);
321 : }
322 4 : }
|
