Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : /*
16 : Sailboat simulator class
17 :
18 : see explanation of lift and drag explained here: https://en.wikipedia.org/wiki/Forces_on_sails
19 :
20 : To-Do: add heel handling by calculating lateral force from wind vs gravity force from heel to arrive at roll rate or acceleration
21 : */
22 :
23 : #include "SIM_Sailboat.h"
24 : #include <AP_Math/AP_Math.h>
25 : #include <string.h>
26 : #include <stdio.h>
27 :
28 : extern const AP_HAL::HAL& hal;
29 :
30 : namespace SITL {
31 :
32 : #define STEERING_SERVO_CH 0 // steering controlled by servo output 1
33 : #define MAINSAIL_SERVO_CH 3 // main sail controlled by servo output 4
34 : #define THROTTLE_SERVO_CH 2 // throttle controlled by servo output 3
35 : #define MOTORLEFT_SERVO_CH 0 // skid-steering left motor controlled by servo output 1
36 : #define MOTORRIGHT_SERVO_CH 2 // skid-steering right motor controlled by servo output 3
37 : #define DIRECT_WING_SERVO_CH 4
38 :
39 : // very roughly sort of a stability factors for waves
40 : #define WAVE_ANGLE_GAIN 1
41 : #define WAVE_HEAVE_GAIN 1
42 :
43 7 : Sailboat::Sailboat(const char *frame_str) :
44 : Aircraft(frame_str),
45 7 : sail_area(1.0),
46 7 : steering_angle_max(35),
47 7 : turning_circle(1.8)
48 : {
49 7 : motor_connected = (strcmp(frame_str, "sailboat-motor") == 0);
50 7 : skid_steering = strstr(frame_str, "skid") != nullptr;
51 7 : lock_step_scheduled = true;
52 7 : }
53 :
54 : // calculate the lift and drag as values from 0 to 1
55 : // given an apparent wind speed in m/s and angle-of-attack in degrees
56 1068125 : void Sailboat::calc_lift_and_drag(float wind_speed, float angle_of_attack_deg, float& lift, float& drag) const
57 : {
58 1068125 : const uint16_t index_width_deg = 10;
59 1068125 : const uint8_t index_max = ARRAY_SIZE(lift_curve) - 1;
60 :
61 : // Convert to expected range
62 1068125 : angle_of_attack_deg = wrap_180(angle_of_attack_deg);
63 :
64 : // assume a symmetrical airfoil
65 1068125 : const float aoa = fabs(angle_of_attack_deg);
66 :
67 : // check extremes
68 1068125 : if (aoa <= 0.0f) {
69 838 : lift = lift_curve[0];
70 838 : drag = drag_curve[0];
71 1067287 : } else if (aoa >= index_max * index_width_deg) {
72 3813 : lift = lift_curve[index_max];
73 3813 : drag = drag_curve[index_max];
74 : } else {
75 1063474 : uint8_t index = constrain_int16(aoa / index_width_deg, 0, index_max);
76 1063474 : float remainder = aoa - (index * index_width_deg);
77 1063474 : lift = linear_interpolate(lift_curve[index], lift_curve[index+1], remainder, 0.0f, index_width_deg);
78 1063474 : drag = linear_interpolate(drag_curve[index], drag_curve[index+1], remainder, 0.0f, index_width_deg);
79 : }
80 :
81 : // apply scaling by wind speed
82 1068125 : lift *= wind_speed * sail_area;
83 1068125 : drag *= wind_speed * sail_area;
84 :
85 1068125 : if (is_negative(angle_of_attack_deg)) {
86 : // invert lift for negative aoa
87 703003 : lift *= -1;
88 : }
89 1068125 : }
90 :
91 : // return turning circle (diameter) in meters for steering angle proportion in the range -1 to +1
92 350466 : float Sailboat::get_turn_circle(float steering) const
93 : {
94 350466 : if (is_zero(steering)) {
95 0 : return 0;
96 : }
97 350466 : return turning_circle * sinf(radians(steering_angle_max)) / sinf(radians(steering * steering_angle_max));
98 : }
99 :
100 : // return yaw rate in deg/sec given a steering input (in the range -1 to +1) and speed in m/s
101 1068125 : float Sailboat::get_yaw_rate(float steering, float speed) const
102 : {
103 1068125 : if (skid_steering) {
104 344989 : return steering * M_PI * 5;
105 : }
106 :
107 723136 : float rate = 0.0f;
108 723136 : if (is_zero(steering) || is_zero(speed)) {
109 372670 : return rate;
110 : }
111 :
112 350466 : float d = get_turn_circle(steering);
113 350466 : float c = M_PI * d;
114 350466 : float t = c / speed;
115 350466 : rate = 360.0f / t;
116 :
117 350466 : return rate;
118 : }
119 :
120 : // return lateral acceleration in m/s/s given a steering input (in the range -1 to +1) and speed in m/s
121 0 : float Sailboat::get_lat_accel(float steering, float speed) const
122 : {
123 0 : float yaw_rate = get_yaw_rate(steering, speed);
124 0 : float accel = radians(yaw_rate) * speed;
125 0 : return accel;
126 : }
127 :
128 : // simulate basic waves / swell
129 1068125 : void Sailboat::update_wave(float delta_time)
130 : {
131 1068125 : const float wave_heading = sitl->wave.direction;
132 1068125 : const float wave_speed = sitl->wave.speed;
133 1068125 : const float wave_lenght = sitl->wave.length;
134 1068125 : const float wave_amp = sitl->wave.amp;
135 :
136 : // apply rate propositional to error between boat angle and water angle
137 : // this gives a 'stability' effect
138 : float r, p, y;
139 1068125 : dcm.to_euler(&r, &p, &y);
140 :
141 : // if not armed don't do waves, to allow gyro init
142 1068125 : if (sitl->wave.enable == 0 || !hal.util->get_soft_armed() || is_zero(wave_amp) ) {
143 765583 : wave_gyro = Vector3f(-r,-p,0.0f) * WAVE_ANGLE_GAIN;
144 765583 : wave_heave = -velocity_ef.z * WAVE_HEAVE_GAIN;
145 765583 : wave_phase = 0.0f;
146 765583 : return;
147 : }
148 :
149 : // calculate the sailboat speed in the direction of the wave
150 302542 : const float boat_speed = velocity_ef.x * sinf(radians(wave_heading)) + velocity_ef.y * cosf(radians(wave_heading));
151 :
152 : // update the wave phase
153 302542 : const float aprarent_wave_distance = (wave_speed - boat_speed) * delta_time;
154 302542 : const float apparent_wave_phase_change = (aprarent_wave_distance / wave_lenght) * M_2PI;
155 :
156 302542 : wave_phase += apparent_wave_phase_change;
157 302542 : wave_phase = wrap_2PI(wave_phase);
158 :
159 : // calculate the angles at this phase on the wave
160 : // use basic sine wave, dy/dx of sine = cosine
161 : // atan( cosine ) = wave angle
162 302542 : const float wave_slope = (wave_amp * 0.5f) * (M_2PI / wave_lenght) * cosf(wave_phase);
163 302542 : const float wave_angle = atanf(wave_slope);
164 :
165 : // convert wave angle to vehicle frame
166 302542 : const float heading_dif = wave_heading - y;
167 302542 : float angle_error_x = (sinf(heading_dif) * wave_angle) - r;
168 302542 : float angle_error_y = (cosf(heading_dif) * wave_angle) - p;
169 :
170 : // apply gain
171 302542 : wave_gyro.x = angle_error_x * WAVE_ANGLE_GAIN;
172 302542 : wave_gyro.y = angle_error_y * WAVE_ANGLE_GAIN;
173 302542 : wave_gyro.z = 0.0f;
174 :
175 : // calculate wave height (NED)
176 302542 : if (sitl->wave.enable == 2) {
177 0 : wave_heave = (wave_slope - velocity_ef.z) * WAVE_HEAVE_GAIN;
178 : } else {
179 302542 : wave_heave = 0.0f;
180 : }
181 : }
182 :
183 : /*
184 : update the sailboat simulation by one time step
185 : */
186 1068125 : void Sailboat::update(const struct sitl_input &input)
187 : {
188 : // update wind
189 1068125 : update_wind(input);
190 :
191 : // in sailboats the steering controls the rudder, the throttle controls the main sail position
192 1068125 : float steering = 0.0f;
193 1068125 : if (skid_steering) {
194 344989 : const float steering_left = input.servos[MOTORLEFT_SERVO_CH] ? normalise_servo_input(input.servos[MOTORLEFT_SERVO_CH]) : 0;
195 344989 : const float steering_right = input.servos[MOTORRIGHT_SERVO_CH] ? normalise_servo_input(input.servos[MOTORRIGHT_SERVO_CH]) : 0;
196 344989 : steering = steering_left - steering_right;
197 : } else {
198 : // invalid input (0us) centres the rudder, which is not great
199 723136 : steering = input.servos[STEERING_SERVO_CH] ? normalise_servo_input(input.servos[STEERING_SERVO_CH]) : 0;
200 : }
201 :
202 : // calculate apparent wind in earth-frame (this is the direction the wind is coming from)
203 : // Note than the SITL wind direction is defined as the direction the wind is travelling to
204 : // This is accounted for in these calculations
205 1068125 : Vector3f wind_apparent_ef = velocity_ef - wind_ef;
206 1068125 : const float wind_apparent_dir_ef = degrees(atan2f(wind_apparent_ef.y, wind_apparent_ef.x));
207 1068125 : const float wind_apparent_speed = safe_sqrt(sq(wind_apparent_ef.x)+sq(wind_apparent_ef.y));
208 :
209 : float roll, pitch, yaw;
210 1068125 : dcm.to_euler(&roll, &pitch, &yaw);
211 :
212 1068125 : const float wind_apparent_dir_bf = wrap_180(wind_apparent_dir_ef - degrees(yaw));
213 :
214 : // set RPM and airspeed from wind speed, allows to test RPM and Airspeed wind vane back end in SITL
215 1068125 : rpm[0] = wind_apparent_speed;
216 1068125 : airspeed_pitot = wind_apparent_speed;
217 :
218 1068125 : float aoa_deg = 0.0f;
219 1068125 : if (sitl->sail_type.get() == 1) {
220 : // directly actuated wing
221 0 : float wing_angle_bf = constrain_float((input.servos[DIRECT_WING_SERVO_CH]-1500)/500.0f * 90.0f, -90.0f, 90.0f);
222 :
223 0 : aoa_deg = wind_apparent_dir_bf - wing_angle_bf;
224 :
225 : } else {
226 : // mainsail with sheet
227 :
228 : // calculate mainsail angle from servo output 4, 0 to 90 degrees
229 1068125 : float mainsail_angle_bf = constrain_float((input.servos[MAINSAIL_SERVO_CH]-1000)/1000.0f * 90.0f, 0.0f, 90.0f);
230 :
231 : // calculate angle-of-attack from wind to mainsail, cannot have negative angle of attack, sheet would go slack
232 1068125 : aoa_deg = MAX(fabsf(wind_apparent_dir_bf) - mainsail_angle_bf, 0);
233 :
234 1068125 : if (is_negative(wind_apparent_dir_bf)) {
235 : // take into account the current tack
236 703530 : aoa_deg *= -1;
237 : }
238 :
239 : }
240 :
241 : // calculate Lift force (perpendicular to wind direction) and Drag force (parallel to wind direction)
242 : float lift_wf, drag_wf;
243 1068125 : calc_lift_and_drag(wind_apparent_speed, aoa_deg, lift_wf, drag_wf);
244 :
245 : // rotate lift and drag from wind frame into body frame
246 1068125 : const float sin_rot_rad = sinf(radians(wind_apparent_dir_bf));
247 1068125 : const float cos_rot_rad = cosf(radians(wind_apparent_dir_bf));
248 1068125 : const float force_fwd = (lift_wf * sin_rot_rad) - (drag_wf * cos_rot_rad);
249 :
250 : // how much time has passed?
251 1068125 : float delta_time = frame_time_us * 1.0e-6f;
252 :
253 : // speed in m/s in body frame
254 1068125 : Vector3f velocity_body = dcm.transposed() * velocity_ef_water;
255 :
256 : // speed along x axis, +ve is forward
257 1068125 : float speed = velocity_body.x;
258 :
259 : // yaw rate in degrees/s
260 1068125 : float yaw_rate = get_yaw_rate(steering, speed);
261 :
262 1068125 : gyro = Vector3f(0,0,radians(yaw_rate)) + wave_gyro;
263 :
264 : // update attitude
265 1068125 : dcm.rotate(gyro * delta_time);
266 1068125 : dcm.normalize();
267 :
268 : // hull drag
269 1068125 : float hull_drag = sq(speed) * 0.5f;
270 1068125 : if (!is_positive(speed)) {
271 381561 : hull_drag *= -1.0f;
272 : }
273 :
274 : // throttle force (for motor sailing)
275 : // gives throttle force == hull drag at 10m/s
276 1068125 : float throttle_force = 0.0f;
277 1068125 : if (motor_connected) {
278 589101 : if (skid_steering) {
279 344989 : const float throttle_left = input.servos[MOTORLEFT_SERVO_CH] ? normalise_servo_input(input.servos[MOTORLEFT_SERVO_CH]) : 0;
280 344989 : const float throttle_right = input.servos[MOTORRIGHT_SERVO_CH] ? normalise_servo_input(input.servos[MOTORRIGHT_SERVO_CH]) : 0;
281 344989 : throttle_force = (0.5f*(throttle_left + throttle_right)) * 50.0f;
282 : } else {
283 244112 : const float throttle_out = input.servos[THROTTLE_SERVO_CH] ? normalise_servo_input(input.servos[THROTTLE_SERVO_CH]) : 0;
284 244112 : throttle_force = throttle_out * 50.0f;
285 : }
286 : }
287 :
288 : // accel in body frame due acceleration from sail and deceleration from hull friction
289 1068125 : accel_body = Vector3f((throttle_force + force_fwd) - hull_drag, 0, 0);
290 1068125 : accel_body /= mass;
291 :
292 : // add in accel due to direction change
293 1068125 : accel_body.y += radians(yaw_rate) * speed;
294 :
295 : // now in earth frame
296 : // remove roll and pitch effects from waves
297 : float r, p, y;
298 1068125 : dcm.to_euler(&r, &p, &y);
299 1068125 : Matrix3f temp_dcm;
300 1068125 : temp_dcm.from_euler(0.0f, 0.0f, y);
301 1068125 : Vector3f accel_earth = temp_dcm * accel_body;
302 :
303 : // we are on the ground, so our vertical accel is zero
304 1068125 : accel_earth.z = 0 + wave_heave;
305 :
306 : // work out acceleration as seen by the accelerometers. It sees the kinematic
307 : // acceleration (ie. real movement), plus gravity
308 1068125 : accel_body = dcm.transposed() * (accel_earth + Vector3f(0, 0, -GRAVITY_MSS));
309 :
310 : // tide calcs
311 1068125 : Vector3f tide_velocity_ef;
312 1068125 : if (hal.util->get_soft_armed() && !is_zero(sitl->tide.speed) ) {
313 0 : tide_velocity_ef.x = -cosf(radians(sitl->tide.direction)) * sitl->tide.speed;
314 0 : tide_velocity_ef.y = -sinf(radians(sitl->tide.direction)) * sitl->tide.speed;
315 0 : tide_velocity_ef.z = 0.0f;
316 : }
317 :
318 : // new velocity vector
319 1068125 : velocity_ef_water += accel_earth * delta_time;
320 1068125 : velocity_ef = velocity_ef_water + tide_velocity_ef;
321 :
322 : // new position vector
323 1068125 : position += (velocity_ef * delta_time).todouble();
324 :
325 : // update lat/lon/altitude
326 1068125 : update_position();
327 1068125 : time_advance();
328 :
329 : // update magnetic field
330 1068125 : update_mag_field_bf();
331 :
332 : // update wave calculations
333 1068125 : update_wave(delta_time);
334 :
335 1068125 : update_battery();
336 1068125 : }
337 :
338 : } // namespace SITL
|
