Line data Source code
1 : #include "Rover.h"
2 :
3 : #define SAILBOAT_AUTO_TACKING_TIMEOUT_MS 5000 // tacks in auto mode timeout if not successfully completed within this many milliseconds
4 : #define SAILBOAT_TACKING_ACCURACY_DEG 10 // tack is considered complete when vehicle is within this many degrees of target tack angle
5 : #define SAILBOAT_NOGO_PAD 10 // deg, the no go zone is padded by this much when deciding if we should use the Sailboat heading controller
6 : #define TACK_RETRY_TIME_MS 5000 // Can only try another auto mode tack this many milliseconds after the last is cleared (either competed or timed-out)
7 : /*
8 : To Do List
9 : - Improve tacking in light winds and bearing away in strong wings
10 : - consider drag vs lift sailing differences, ie upwind sail is like wing, dead down wind sail is like parachute
11 : - max speed parameter and controller, for mapping you may not want to go too fast
12 : - mavlink sailing messages
13 : - smart decision making, ie tack on windshifts, what to do if stuck head to wind
14 : - some sailing codes track waves to try and 'surf' and to allow tacking on a flat bit, not sure if there is much gain to be had here
15 : - add some sort of pitch monitoring to prevent nose diving in heavy weather
16 : - pitch PID for hydrofoils
17 : - more advanced sail control, ie twist
18 : - independent sheeting for main and jib
19 : - tack on depth sounder info to stop sailing into shallow water on indirect sailing routes
20 : - add option to do proper tacks, ie tacking on flat spot in the waves, or only try once at a certain speed, or some better method than just changing the desired heading suddenly
21 : */
22 :
23 : const AP_Param::GroupInfo Sailboat::var_info[] = {
24 :
25 : // @Param: ENABLE
26 : // @DisplayName: Enable Sailboat
27 : // @Description: This enables Sailboat functionality
28 : // @Values: 0:Disable,1:Enable
29 : // @User: Standard
30 : // @RebootRequired: True
31 : AP_GROUPINFO_FLAGS("ENABLE", 1, Sailboat, enable, 0, AP_PARAM_FLAG_ENABLE),
32 :
33 : // @Param: ANGLE_MIN
34 : // @DisplayName: Sail min angle
35 : // @Description: Mainsheet tight, angle between centerline and boom
36 : // @Units: deg
37 : // @Range: 0 90
38 : // @Increment: 1
39 : // @User: Standard
40 : AP_GROUPINFO("ANGLE_MIN", 2, Sailboat, sail_angle_min, 0),
41 :
42 : // @Param: ANGLE_MAX
43 : // @DisplayName: Sail max angle
44 : // @Description: Mainsheet loose, angle between centerline and boom. For direct-control rotating masts, the rotation angle at SERVOx_MAX/_MIN; for rotating masts, this value can exceed 90 degrees if the linkages can physically rotate the mast past that angle.
45 : // @Units: deg
46 : // @Range: 0 90
47 : // @Increment: 1
48 : // @User: Standard
49 : AP_GROUPINFO("ANGLE_MAX", 3, Sailboat, sail_angle_max, 90),
50 :
51 : // @Param: ANGLE_IDEAL
52 : // @DisplayName: Sail ideal angle
53 : // @Description: Ideal angle between sail and apparent wind
54 : // @Units: deg
55 : // @Range: 0 90
56 : // @Increment: 1
57 : // @User: Standard
58 : AP_GROUPINFO("ANGLE_IDEAL", 4, Sailboat, sail_angle_ideal, 25),
59 :
60 : // @Param: HEEL_MAX
61 : // @DisplayName: Sailing maximum heel angle
62 : // @Description: When in auto sail trim modes the heel will be limited to this value using PID control
63 : // @Units: deg
64 : // @Range: 0 90
65 : // @Increment: 1
66 : // @User: Standard
67 : AP_GROUPINFO("HEEL_MAX", 5, Sailboat, sail_heel_angle_max, 15),
68 :
69 : // @Param: NO_GO_ANGLE
70 : // @DisplayName: Sailing no go zone angle
71 : // @Description: The typical closest angle to the wind the vehicle will sail at. the vehicle will sail at this angle when going upwind
72 : // @Units: deg
73 : // @Range: 0 90
74 : // @Increment: 1
75 : // @User: Standard
76 : AP_GROUPINFO("NO_GO_ANGLE", 6, Sailboat, sail_no_go, 45),
77 :
78 : // @Param: WNDSPD_MIN
79 : // @DisplayName: Sailboat minimum wind speed to sail in
80 : // @Description: Sailboat minimum wind speed to continue sail in, at lower wind speeds the sailboat will motor if one is fitted
81 : // @Units: m/s
82 : // @Range: 0 5
83 : // @Increment: 0.1
84 : // @User: Standard
85 : AP_GROUPINFO("WNDSPD_MIN", 7, Sailboat, sail_windspeed_min, 0),
86 :
87 : // @Param: XTRACK_MAX
88 : // @DisplayName: Sailing vehicle max cross track error
89 : // @Description: The sail boat will tack when it reaches this cross track error, defines a corridor of 2 times this value wide, 0 disables
90 : // @Units: m
91 : // @Range: 5 25
92 : // @Increment: 1
93 : // @User: Standard
94 : AP_GROUPINFO("XTRACK_MAX", 8, Sailboat, xtrack_max, 10),
95 :
96 : // @Param: LOIT_RADIUS
97 : // @DisplayName: Loiter radius
98 : // @Description: When in sailing modes the vehicle will keep moving within this loiter radius
99 : // @Units: m
100 : // @Range: 0 20
101 : // @Increment: 1
102 : // @User: Standard
103 : AP_GROUPINFO("LOIT_RADIUS", 9, Sailboat, loit_radius, 5),
104 :
105 : AP_GROUPEND
106 : };
107 :
108 :
109 : /*
110 : constructor
111 : */
112 285 : Sailboat::Sailboat()
113 : {
114 285 : AP_Param::setup_object_defaults(this, var_info);
115 285 : }
116 :
117 : // true if sailboat navigation (aka tacking) is enabled
118 457514 : bool Sailboat::tack_enabled() const
119 : {
120 : // tacking disabled if not a sailboat
121 457514 : if (!sail_enabled()) {
122 414537 : return false;
123 : }
124 :
125 : // tacking disabled if motor is always on
126 42977 : if (motor_state == UseMotor::USE_MOTOR_ALWAYS) {
127 0 : return false;
128 : }
129 :
130 : // disable tacking if motor is available and wind is below cutoff
131 42977 : if (motor_assist_low_wind()) {
132 0 : return false;
133 : }
134 :
135 : // otherwise tacking is enabled
136 42977 : return true;
137 : }
138 :
139 283 : void Sailboat::init()
140 : {
141 : // sailboat defaults
142 283 : if (sail_enabled()) {
143 4 : rover.g2.crash_angle.set_default(0);
144 :
145 : // sailboats without motors may travel faster than WP_SPEED so allow waypoint navigation to
146 : // speedup to catch the vehicle instead of asking the vehicle to slow down
147 4 : rover.g2.wp_nav.enable_overspeed(motor_state != UseMotor::USE_MOTOR_ALWAYS);
148 : }
149 :
150 283 : if (tack_enabled()) {
151 4 : rover.g2.loit_type.set_default(1);
152 : }
153 :
154 : // initialise motor state to USE_MOTOR_ASSIST
155 : // this will silently fail if there is no motor attached
156 283 : set_motor_state(UseMotor::USE_MOTOR_ASSIST, false);
157 283 : }
158 :
159 : // initialise rc input (channel_mainsail), may be called intermittently
160 12811 : void Sailboat::init_rc_in()
161 : {
162 : // get auxiliary throttle value
163 12811 : RC_Channel *rc_ptr = rc().find_channel_for_option(RC_Channel::AUX_FUNC::MAINSAIL);
164 12811 : if (rc_ptr != nullptr) {
165 : // use aux as sail input if defined
166 0 : channel_mainsail = rc_ptr;
167 0 : channel_mainsail->set_angle(100);
168 0 : channel_mainsail->set_default_dead_zone(30);
169 : } else {
170 : // use throttle channel
171 12811 : channel_mainsail = rover.channel_throttle;
172 : }
173 12811 : }
174 :
175 : /// @brief decode pilot mainsail input in manual modes and update the various
176 : /// sail actuator values for different sail types ready for SRV_Channel output.
177 362133 : void Sailboat::set_pilot_desired_mainsail()
178 : {
179 : // no RC input means mainsail is moved to trim
180 362133 : if ((rover.failsafe.bits & FAILSAFE_EVENT_THROTTLE) || (channel_mainsail == nullptr)) {
181 217 : relax_sails();
182 : } else {
183 361916 : rover.g2.motors.set_mainsail(constrain_float(channel_mainsail->get_control_in(), 0.0f, 100.0f));
184 361916 : rover.g2.motors.set_wingsail(constrain_float(channel_mainsail->get_control_in(), -100.0f, 100.0f));
185 361916 : rover.g2.motors.set_mast_rotation(constrain_float(channel_mainsail->get_control_in(), -100.0f, 100.0f));
186 : }
187 362133 : }
188 :
189 : /// @brief Set mainsail in auto modes
190 : /// @param[in] desired_speed desired speed (in m/s) only used to detect desired direction
191 17468 : void Sailboat::set_auto_mainsail(float desired_speed)
192 : {
193 : // use PID controller to sheet out, this number is expected approximately in the 0 to 100 range (with default PIDs)
194 17468 : const float pid_offset = rover.g2.attitude_control.get_sail_out_from_heel(radians(sail_heel_angle_max), rover.G_Dt) * 100.0f;
195 :
196 : // get apparent wind, + is wind over starboard side, - is wind over port side
197 17468 : const float wind_dir_apparent = degrees(rover.g2.windvane.get_apparent_wind_direction_rad());
198 17468 : const float wind_dir_apparent_abs = fabsf(wind_dir_apparent);
199 17468 : const float wind_dir_apparent_sign = is_negative(wind_dir_apparent) ? -1.0f : 1.0f;
200 :
201 : //
202 : // mainsail control.
203 : //
204 : // mainsail_out represents a range from 0 to 100
205 17468 : float mainsail_out = 100.0f;
206 : // main sails cannot be used to reverse
207 17468 : if (is_positive(desired_speed)) {
208 : // Sails are sheeted the same on each side use abs wind direction
209 : // set the main sail to the ideal angle to the wind
210 : const float mainsail_angle =
211 17460 : constrain_float(wind_dir_apparent_abs - sail_angle_ideal,sail_angle_min, sail_angle_max);
212 :
213 : // linear interpolate mainsail value (0 to 100) from wind angle mainsail_angle
214 17460 : const float mainsail_base = linear_interpolate(0.0f, 100.0f, mainsail_angle,sail_angle_min,sail_angle_max);
215 :
216 17460 : mainsail_out = constrain_float((mainsail_base + pid_offset), 0.0f ,100.0f);
217 : }
218 17468 : rover.g2.motors.set_mainsail(mainsail_out);
219 : //
220 : // wingsail control
221 : // wing sails auto trim, we only need to reduce power if we are tipping over, must also be trimmed for correct tack
222 : // dont allow to reduce power to less than 0, ie not backwinding the sail to self-right
223 : // wing sails can be used to go backwards, probably not recommended though
224 17468 : const float wing_sail_out_sign = is_negative(desired_speed) ? -1.0f : 1.0f;
225 17468 : const float wingsail_out = (100.0f - MIN(pid_offset,100.0f)) * wind_dir_apparent_sign * wing_sail_out_sign;
226 17468 : rover.g2.motors.set_wingsail(wingsail_out);
227 : //
228 : // direct mast rotation control
229 : //
230 17468 : float mast_rotation_out = 0.0f;
231 17468 : if (is_positive(desired_speed)) {
232 : // rotating sails can be used to reverse, but not in this version
233 17460 : if (wind_dir_apparent_abs < sail_angle_ideal) {
234 : // in irons, center the sail.
235 1238 : mast_rotation_out = 0.0f;
236 : } else {
237 : float mast_rotation_angle;
238 16222 : if (wind_dir_apparent_abs < (90.0f + sail_angle_ideal)) {
239 : // use sail as a lift device, at ideal angle of attack, but depower to prevent excessive heel
240 : // multiply pid_offset by 0.01 to keep the scaling in the same range as the other sail outputs
241 : // this means the default PIDs should apply reasonably well to all sail types
242 15194 : mast_rotation_angle = wind_dir_apparent_abs - sail_angle_ideal * MAX(1.0f - pid_offset*0.01f,0.0f);
243 :
244 : // restore sign
245 15194 : mast_rotation_angle *= wind_dir_apparent_sign;
246 :
247 : } else {
248 : // use sail as drag device, but avoid wagging the sail as the wind oscillates
249 : // between 180 and -180 degrees
250 1028 : mast_rotation_angle = 90.0f;
251 1028 : if (wind_dir_apparent_abs > 135.0f) {
252 : // wind is almost directly behind, keep wing on current tack
253 764 : if (is_negative(SRV_Channels::get_output_scaled(SRV_Channel::k_mast_rotation))) {
254 152 : mast_rotation_angle *= -1.0f;
255 : }
256 : } else {
257 : // set the wing on the correct tack, so that is can be sheeted in if required
258 264 : mast_rotation_angle *= wind_dir_apparent_sign;
259 : }
260 : }
261 : // linear interpolate servo displacement (-100 to 100) from mast rotation angle and restore sign
262 16222 : mast_rotation_out = linear_interpolate(-100.0f, 100.0f, mast_rotation_angle, -sail_angle_max, sail_angle_max);
263 : }
264 : }
265 17468 : rover.g2.motors.set_mast_rotation(mast_rotation_out);
266 17468 : }
267 :
268 47521 : void Sailboat::relax_sails()
269 : {
270 47521 : rover.g2.motors.set_mainsail(100.0f);
271 47521 : rover.g2.motors.set_wingsail(0.0f);
272 47521 : rover.g2.motors.set_mast_rotation(0.0f);
273 47521 : }
274 :
275 : // calculate throttle and mainsail angle required to attain desired speed (in m/s)
276 17468 : void Sailboat::get_throttle_and_set_mainsail(float desired_speed, float &throttle_out)
277 : {
278 17468 : throttle_out = 0.0f;
279 17468 : if (!sail_enabled()) {
280 0 : relax_sails();
281 0 : return;
282 : }
283 :
284 : // run speed controller if motor is forced on or motor assistance is required for low speeds or tacking
285 17468 : if ((motor_state == UseMotor::USE_MOTOR_ALWAYS) ||
286 34936 : motor_assist_tack() ||
287 17468 : motor_assist_low_wind()) {
288 : // run speed controller - duplicate of calls found in mode::calc_throttle();
289 0 : throttle_out = 100.0f * rover.g2.attitude_control.get_throttle_out_speed(desired_speed,
290 0 : rover.g2.motors.limit.throttle_lower,
291 0 : rover.g2.motors.limit.throttle_upper,
292 : rover.g.speed_cruise,
293 0 : rover.g.throttle_cruise * 0.01f,
294 : rover.G_Dt);
295 : }
296 :
297 17468 : if (motor_state == UseMotor::USE_MOTOR_ALWAYS) {
298 0 : relax_sails();
299 : } else {
300 17468 : set_auto_mainsail(desired_speed);
301 : }
302 : }
303 :
304 : // Velocity Made Good, this is the speed we are traveling towards the desired destination
305 : // only for logging at this stage
306 : // https://en.wikipedia.org/wiki/Velocity_made_good
307 20419 : float Sailboat::get_VMG() const
308 : {
309 : // return zero if we don't have a valid speed
310 : float speed;
311 20419 : if (!rover.g2.attitude_control.get_forward_speed(speed)) {
312 237 : return 0.0f;
313 : }
314 :
315 : // return speed if not heading towards a waypoint
316 20182 : if (!rover.control_mode->is_autopilot_mode()) {
317 2671 : return speed;
318 : }
319 :
320 17511 : return (speed * cosf(wrap_PI(radians(rover.g2.wp_nav.wp_bearing_cd() * 0.01f) - rover.ahrs.get_yaw_rad())));
321 : }
322 :
323 : // handle user initiated tack while in acro mode
324 0 : void Sailboat::handle_tack_request_acro()
325 : {
326 0 : if (!tack_enabled() || currently_tacking) {
327 0 : return;
328 : }
329 : // set tacking heading target to the current angle relative to the true wind but on the new tack
330 0 : currently_tacking = true;
331 0 : tack_heading_rad = wrap_2PI(rover.ahrs.get_yaw_rad() + 2.0f * wrap_PI((rover.g2.windvane.get_true_wind_direction_rad() - rover.ahrs.get_yaw_rad())));
332 :
333 0 : tack_request_ms = AP_HAL::millis();
334 : }
335 :
336 : // return target heading in radians when tacking (only used in acro)
337 0 : float Sailboat::get_tack_heading_rad()
338 : {
339 0 : if (fabsf(wrap_PI(tack_heading_rad - rover.ahrs.get_yaw_rad())) < radians(SAILBOAT_TACKING_ACCURACY_DEG) ||
340 0 : ((AP_HAL::millis() - tack_request_ms) > SAILBOAT_AUTO_TACKING_TIMEOUT_MS)) {
341 0 : clear_tack();
342 : }
343 :
344 0 : return tack_heading_rad;
345 : }
346 :
347 : // handle user initiated tack while in autonomous modes (Auto, Guided, RTL, SmartRTL, etc)
348 0 : void Sailboat::handle_tack_request_auto()
349 : {
350 0 : if (!tack_enabled() || currently_tacking) {
351 0 : return;
352 : }
353 :
354 : // record time of request for tack. This will be processed asynchronously by sailboat_calc_heading
355 0 : tack_request_ms = AP_HAL::millis();
356 : }
357 :
358 : // clear tacking state variables
359 567 : void Sailboat::clear_tack()
360 : {
361 567 : currently_tacking = false;
362 567 : tack_assist = false;
363 567 : tack_request_ms = 0;
364 567 : tack_clear_ms = AP_HAL::millis();
365 567 : }
366 :
367 : // returns true if boat is currently tacking
368 7017 : bool Sailboat::tacking() const
369 : {
370 7017 : return tack_enabled() && currently_tacking;
371 : }
372 :
373 : // returns true if sailboat should take a indirect navigation route to go upwind
374 : // desired_heading should be in centi-degrees
375 192980 : bool Sailboat::use_indirect_route(float desired_heading_cd) const
376 : {
377 192980 : if (!tack_enabled()) {
378 175512 : return false;
379 : }
380 :
381 : // use sailboat controller until tack is completed
382 17468 : if (currently_tacking) {
383 783 : return true;
384 : }
385 :
386 : // convert desired heading to radians
387 16685 : const float desired_heading_rad = radians(desired_heading_cd * 0.01f);
388 :
389 : // check if desired heading is in the no go zone, if it is we can't go direct
390 : // pad no go zone, this allows use of heading controller rather than L1 when close to the wind
391 16685 : return fabsf(wrap_PI(rover.g2.windvane.get_true_wind_direction_rad() - desired_heading_rad)) <= radians(sail_no_go + SAILBOAT_NOGO_PAD);
392 : }
393 :
394 : // if we can't sail on the desired heading then we should pick the best heading that we can sail on
395 : // this function assumes the caller has already checked sailboat_use_indirect_route(desired_heading_cd) returned true
396 3028 : float Sailboat::calc_heading(float desired_heading_cd)
397 : {
398 3028 : if (!tack_enabled()) {
399 0 : return desired_heading_cd;
400 : }
401 3028 : bool should_tack = false;
402 :
403 : // find which tack we are on
404 3028 : const AP_WindVane::Sailboat_Tack current_tack = rover.g2.windvane.get_current_tack();
405 :
406 : // convert desired heading to radians
407 3028 : const float desired_heading_rad = radians(desired_heading_cd * 0.01f);
408 :
409 : // if the desired heading is outside the no go zone there is no need to change it
410 : // this allows use of heading controller rather than L1 when desired
411 : // this is used in the 'SAILBOAT_NOGO_PAD' region
412 3028 : const float true_wind_rad = rover.g2.windvane.get_true_wind_direction_rad();
413 3028 : if (fabsf(wrap_PI(true_wind_rad - desired_heading_rad)) > radians(sail_no_go) && !currently_tacking) {
414 :
415 : // calculate the tack the new heading would be on
416 495 : const float new_heading_apparent_angle = wrap_PI(true_wind_rad - desired_heading_rad);
417 : AP_WindVane::Sailboat_Tack new_tack;
418 495 : if (is_negative(new_heading_apparent_angle)) {
419 131 : new_tack = AP_WindVane::Sailboat_Tack::TACK_PORT;
420 : } else {
421 364 : new_tack = AP_WindVane::Sailboat_Tack::TACK_STARBOARD;
422 : }
423 :
424 : // if the new tack is not the same as the current tack we need might need to tack
425 495 : if (new_tack != current_tack) {
426 : // see if it would be a tack, the front of the boat going through the wind
427 : // or a gybe, the back of the boat going through the wind
428 241 : const float app_wind_rad = rover.g2.windvane.get_apparent_wind_direction_rad();
429 241 : if (fabsf(app_wind_rad) + fabsf(new_heading_apparent_angle) < M_PI) {
430 201 : should_tack = true;
431 : }
432 : }
433 :
434 495 : if (!should_tack) {
435 294 : return desired_heading_cd;
436 : }
437 : }
438 :
439 : // check for user requested tack
440 2734 : uint32_t now = AP_HAL::millis();
441 2734 : if (tack_request_ms != 0 && !should_tack && !currently_tacking) {
442 : // set should_tack flag is user requested tack within last 0.5 sec
443 0 : should_tack = ((now - tack_request_ms) < 500);
444 0 : tack_request_ms = 0;
445 : }
446 :
447 : // trigger tack if cross track error larger than xtrack_max parameter
448 : // this effectively defines a 'corridor' of width 2*xtrack_max that the boat will stay within
449 2734 : const float cross_track_error = rover.g2.wp_nav.crosstrack_error_m();
450 2734 : if ((fabsf(cross_track_error) >= xtrack_max) && !is_zero(xtrack_max) && !should_tack && !currently_tacking) {
451 : // make sure the new tack will reduce the cross track error
452 : // if were on starboard tack we are traveling towards the left hand boundary
453 0 : if (is_positive(cross_track_error) && (current_tack == AP_WindVane::Sailboat_Tack::TACK_STARBOARD)) {
454 0 : should_tack = true;
455 : }
456 : // if were on port tack we are traveling towards the right hand boundary
457 0 : if (is_negative(cross_track_error) && (current_tack == AP_WindVane::Sailboat_Tack::TACK_PORT)) {
458 0 : should_tack = true;
459 : }
460 : }
461 :
462 : // calculate left and right no go headings looking upwind, Port tack heading is left no-go, STBD tack is right of no-go
463 2734 : const float left_no_go_heading_rad = wrap_2PI(true_wind_rad + radians(sail_no_go));
464 2734 : const float right_no_go_heading_rad = wrap_2PI(true_wind_rad - radians(sail_no_go));
465 :
466 : // if tack triggered, calculate target heading
467 2734 : if (should_tack && (now - tack_clear_ms) > TACK_RETRY_TIME_MS) {
468 11 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Sailboat: Tacking");
469 : // calculate target heading for the new tack
470 11 : switch (current_tack) {
471 4 : case AP_WindVane::Sailboat_Tack::TACK_PORT:
472 4 : tack_heading_rad = right_no_go_heading_rad;
473 4 : break;
474 7 : case AP_WindVane::Sailboat_Tack::TACK_STARBOARD:
475 7 : tack_heading_rad = left_no_go_heading_rad;
476 7 : break;
477 : }
478 11 : currently_tacking = true;
479 11 : auto_tack_start_ms = now;
480 : }
481 :
482 : // if we are tacking we maintain the target heading until the tack completes or times out
483 2734 : if (currently_tacking) {
484 : // check if we have reached target
485 794 : if (fabsf(wrap_PI(tack_heading_rad - rover.ahrs.get_yaw_rad())) <= radians(SAILBOAT_TACKING_ACCURACY_DEG)) {
486 8 : clear_tack();
487 786 : } else if ((now - auto_tack_start_ms) > SAILBOAT_AUTO_TACKING_TIMEOUT_MS) {
488 : // tack has taken too long
489 0 : if ((motor_state == UseMotor::USE_MOTOR_ASSIST) && (now - auto_tack_start_ms) < (3.0f * SAILBOAT_AUTO_TACKING_TIMEOUT_MS)) {
490 : // if we have throttle available use it for another two time periods to get the tack done
491 0 : tack_assist = true;
492 : } else {
493 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Sailboat: Tacking timed out");
494 0 : clear_tack();
495 : }
496 : }
497 : // return tack target heading
498 794 : return degrees(tack_heading_rad) * 100.0f;
499 : }
500 :
501 : // return the correct heading for our current tack
502 1940 : if (current_tack == AP_WindVane::Sailboat_Tack::TACK_PORT) {
503 700 : return degrees(left_no_go_heading_rad) * 100.0f;
504 : } else {
505 1240 : return degrees(right_no_go_heading_rad) * 100.0f;
506 : }
507 : }
508 :
509 : // set state of motor
510 285 : void Sailboat::set_motor_state(UseMotor state, bool report_failure)
511 : {
512 : // always allow motor to be disabled
513 285 : if (state == UseMotor::USE_MOTOR_NEVER) {
514 1 : motor_state = state;
515 1 : return;
516 : }
517 :
518 : // enable assistance or always on if a motor is defined
519 532 : if (rover.g2.motors.have_skid_steering() ||
520 532 : SRV_Channels::function_assigned(SRV_Channel::k_throttle) ||
521 0 : rover.get_frame_type() != rover.g2.motors.frame_type::FRAME_TYPE_UNDEFINED) {
522 284 : motor_state = state;
523 0 : } else if (report_failure) {
524 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Sailboat: failed to enable motor");
525 : }
526 : }
527 :
528 : // true if motor is on to assist with slow tack
529 17468 : bool Sailboat::motor_assist_tack() const
530 : {
531 : // throttle is assist is disabled
532 17468 : if (motor_state != UseMotor::USE_MOTOR_ASSIST) {
533 0 : return false;
534 : }
535 :
536 : // assist with a tack because it is taking too long
537 17468 : return tack_assist;
538 : }
539 :
540 : // true if motor should be on to assist with low wind
541 60445 : bool Sailboat::motor_assist_low_wind() const
542 : {
543 : // motor assist is disabled
544 60445 : if (motor_state != UseMotor::USE_MOTOR_ASSIST) {
545 4 : return false;
546 : }
547 :
548 : // assist if wind speed is below cutoff
549 60441 : return (is_positive(sail_windspeed_min) &&
550 60441 : rover.g2.windvane.wind_speed_enabled() &&
551 60441 : (rover.g2.windvane.get_true_wind_speed() < sail_windspeed_min));
552 : }
|
