2018-09-24 04:16:23 -03:00
|
|
|
/*
|
|
|
|
This program is free software: you can redistribute it and/or modify
|
|
|
|
it under the terms of the GNU General Public License as published by
|
|
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
|
|
(at your option) any later version.
|
|
|
|
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
|
|
GNU General Public License for more details.
|
|
|
|
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
|
|
*/
|
|
|
|
/*
|
|
|
|
Sailboat simulator class
|
|
|
|
|
|
|
|
see explanation of lift and drag explained here: https://en.wikipedia.org/wiki/Forces_on_sails
|
|
|
|
|
|
|
|
To-Do: add heel handling by calculating lateral force from wind vs gravity force from heel to arrive at roll rate or acceleration
|
|
|
|
*/
|
|
|
|
|
|
|
|
#include "SIM_Sailboat.h"
|
|
|
|
#include <AP_Math/AP_Math.h>
|
2019-06-26 23:37:34 -03:00
|
|
|
#include <AP_AHRS/AP_AHRS.h>
|
2018-09-24 04:16:23 -03:00
|
|
|
#include <string.h>
|
|
|
|
#include <stdio.h>
|
|
|
|
|
|
|
|
namespace SITL {
|
|
|
|
|
2018-09-25 03:02:51 -03:00
|
|
|
#define STEERING_SERVO_CH 0 // steering controlled by servo output 1
|
|
|
|
#define MAINSAIL_SERVO_CH 3 // main sail controlled by servo output 4
|
|
|
|
|
2018-09-24 04:16:23 -03:00
|
|
|
Sailboat::Sailboat(const char *home_str, const char *frame_str) :
|
|
|
|
Aircraft(home_str, frame_str),
|
2018-09-25 03:02:51 -03:00
|
|
|
steering_angle_max(35),
|
2018-09-24 04:16:23 -03:00
|
|
|
turning_circle(1.8)
|
|
|
|
{
|
|
|
|
}
|
|
|
|
|
|
|
|
// calculate the lift and drag as values from 0 to 1
|
|
|
|
// given an apparent wind speed in m/s and angle-of-attack in degrees
|
2018-09-25 03:02:51 -03:00
|
|
|
void Sailboat::calc_lift_and_drag(float wind_speed, float angle_of_attack_deg, float& lift, float& drag) const
|
2018-09-24 04:16:23 -03:00
|
|
|
{
|
2018-09-25 03:02:51 -03:00
|
|
|
const uint16_t index_width_deg = 10;
|
|
|
|
const uint8_t index_max = ARRAY_SIZE(lift_curve) - 1;
|
|
|
|
|
2018-09-24 04:16:23 -03:00
|
|
|
// check extremes
|
|
|
|
if (angle_of_attack_deg <= 0.0f) {
|
|
|
|
lift = lift_curve[0];
|
|
|
|
drag = drag_curve[0];
|
2018-10-05 19:22:44 -03:00
|
|
|
} else if (angle_of_attack_deg >= index_max * index_width_deg) {
|
2018-09-25 03:02:51 -03:00
|
|
|
lift = lift_curve[index_max];
|
|
|
|
drag = drag_curve[index_max];
|
2018-10-05 19:22:44 -03:00
|
|
|
} else {
|
|
|
|
uint8_t index = constrain_int16(angle_of_attack_deg / index_width_deg, 0, index_max);
|
|
|
|
float remainder = angle_of_attack_deg - (index * index_width_deg);
|
|
|
|
lift = linear_interpolate(lift_curve[index], lift_curve[index+1], remainder, 0.0f, index_width_deg);
|
|
|
|
drag = linear_interpolate(drag_curve[index], drag_curve[index+1], remainder, 0.0f, index_width_deg);
|
2018-09-24 04:16:23 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// apply scaling by wind speed
|
|
|
|
lift *= wind_speed;
|
|
|
|
drag *= wind_speed;
|
|
|
|
}
|
|
|
|
|
2018-09-25 03:02:51 -03:00
|
|
|
// return turning circle (diameter) in meters for steering angle proportion in the range -1 to +1
|
|
|
|
float Sailboat::get_turn_circle(float steering) const
|
2018-09-24 04:16:23 -03:00
|
|
|
{
|
2018-09-25 03:02:51 -03:00
|
|
|
if (is_zero(steering)) {
|
2018-09-24 04:16:23 -03:00
|
|
|
return 0;
|
|
|
|
}
|
2018-09-25 03:02:51 -03:00
|
|
|
return turning_circle * sinf(radians(steering_angle_max)) / sinf(radians(steering * steering_angle_max));
|
2018-09-24 04:16:23 -03:00
|
|
|
}
|
|
|
|
|
2018-09-25 03:02:51 -03:00
|
|
|
// return yaw rate in deg/sec given a steering input (in the range -1 to +1) and speed in m/s
|
|
|
|
float Sailboat::get_yaw_rate(float steering, float speed) const
|
2018-09-24 04:16:23 -03:00
|
|
|
{
|
2018-09-25 03:02:51 -03:00
|
|
|
if (is_zero(steering) || is_zero(speed)) {
|
2018-09-24 04:16:23 -03:00
|
|
|
return 0;
|
|
|
|
}
|
2018-09-25 03:02:51 -03:00
|
|
|
float d = get_turn_circle(steering);
|
2018-09-24 04:16:23 -03:00
|
|
|
float c = M_PI * d;
|
|
|
|
float t = c / speed;
|
|
|
|
float rate = 360.0f / t;
|
|
|
|
return rate;
|
|
|
|
}
|
|
|
|
|
2018-09-25 03:02:51 -03:00
|
|
|
// return lateral acceleration in m/s/s given a steering input (in the range -1 to +1) and speed in m/s
|
|
|
|
float Sailboat::get_lat_accel(float steering, float speed) const
|
2018-09-24 04:16:23 -03:00
|
|
|
{
|
2018-09-25 03:02:51 -03:00
|
|
|
float yaw_rate = get_yaw_rate(steering, speed);
|
2018-09-24 04:16:23 -03:00
|
|
|
float accel = radians(yaw_rate) * speed;
|
|
|
|
return accel;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
update the sailboat simulation by one time step
|
|
|
|
*/
|
|
|
|
void Sailboat::update(const struct sitl_input &input)
|
|
|
|
{
|
|
|
|
// update wind
|
|
|
|
update_wind(input);
|
|
|
|
|
|
|
|
// in sailboats the steering controls the rudder, the throttle controls the main sail position
|
2018-09-25 03:02:51 -03:00
|
|
|
float steering = 2*((input.servos[STEERING_SERVO_CH]-1000)/1000.0f - 0.5f);
|
2018-09-24 04:16:23 -03:00
|
|
|
|
|
|
|
// calculate mainsail angle from servo output 4, 0 to 90 degrees
|
2018-09-25 03:02:51 -03:00
|
|
|
float mainsail_angle_bf = constrain_float((input.servos[MAINSAIL_SERVO_CH]-1000)/1000.0f * 90.0f, 0.0f, 90.0f);
|
2018-09-24 04:16:23 -03:00
|
|
|
|
|
|
|
// calculate apparent wind in earth-frame (this is the direction the wind is coming from)
|
2018-10-05 19:22:44 -03:00
|
|
|
// Note than the SITL wind direction is defined as the direction the wind is travelling to
|
|
|
|
// This is accounted for in these calculations
|
2018-09-24 04:16:23 -03:00
|
|
|
Vector3f wind_apparent_ef = wind_ef + velocity_ef;
|
|
|
|
const float wind_apparent_dir_ef = degrees(atan2f(wind_apparent_ef.y, wind_apparent_ef.x));
|
|
|
|
const float wind_apparent_speed = safe_sqrt(sq(wind_apparent_ef.x)+sq(wind_apparent_ef.y));
|
|
|
|
|
2018-10-05 19:22:44 -03:00
|
|
|
const float wind_apparent_dir_bf = wrap_180(wind_apparent_dir_ef - degrees(AP::ahrs().yaw));
|
|
|
|
|
2019-05-15 20:05:32 -03:00
|
|
|
// set RPM and airspeed from wind speed, allows to test RPM and Airspeed wind vane back end in SITL
|
|
|
|
rpm1 = wind_apparent_speed;
|
|
|
|
airspeed_pitot = wind_apparent_speed;
|
|
|
|
|
2018-09-24 04:16:23 -03:00
|
|
|
// calculate angle-of-attack from wind to mainsail
|
2018-10-05 19:22:44 -03:00
|
|
|
float aoa_deg = MAX(fabsf(wind_apparent_dir_bf) - mainsail_angle_bf, 0);
|
2018-09-24 04:16:23 -03:00
|
|
|
|
|
|
|
// calculate Lift force (perpendicular to wind direction) and Drag force (parallel to wind direction)
|
|
|
|
float lift_wf, drag_wf;
|
|
|
|
calc_lift_and_drag(wind_apparent_speed, aoa_deg, lift_wf, drag_wf);
|
|
|
|
|
|
|
|
// rotate lift and drag from wind frame into body frame
|
2018-10-05 19:22:44 -03:00
|
|
|
const float sin_rot_rad = sinf(radians(wind_apparent_dir_bf));
|
|
|
|
const float cos_rot_rad = cosf(radians(wind_apparent_dir_bf));
|
|
|
|
const float force_fwd = fabsf(lift_wf * sin_rot_rad) - (drag_wf * cos_rot_rad);
|
2018-09-24 04:16:23 -03:00
|
|
|
|
|
|
|
// how much time has passed?
|
|
|
|
float delta_time = frame_time_us * 1.0e-6f;
|
|
|
|
|
|
|
|
// speed in m/s in body frame
|
|
|
|
Vector3f velocity_body = dcm.transposed() * velocity_ef;
|
|
|
|
|
|
|
|
// speed along x axis, +ve is forward
|
|
|
|
float speed = velocity_body.x;
|
|
|
|
|
|
|
|
// yaw rate in degrees/s
|
2018-09-25 03:02:51 -03:00
|
|
|
float yaw_rate = get_yaw_rate(steering, speed);
|
2018-09-24 04:16:23 -03:00
|
|
|
|
|
|
|
gyro = Vector3f(0,0,radians(yaw_rate));
|
|
|
|
|
|
|
|
// update attitude
|
|
|
|
dcm.rotate(gyro * delta_time);
|
|
|
|
dcm.normalize();
|
|
|
|
|
2018-10-05 19:22:44 -03:00
|
|
|
// hull drag
|
|
|
|
float hull_drag = sq(speed) * 0.5f;
|
|
|
|
if (!is_positive(speed)) {
|
|
|
|
hull_drag *= -1.0f;
|
|
|
|
}
|
|
|
|
|
2018-09-24 04:16:23 -03:00
|
|
|
// accel in body frame due acceleration from sail and deceleration from hull friction
|
2018-10-05 19:22:44 -03:00
|
|
|
accel_body = Vector3f(force_fwd - hull_drag, 0, 0);
|
|
|
|
accel_body /= mass;
|
2018-09-24 04:16:23 -03:00
|
|
|
|
|
|
|
// add in accel due to direction change
|
|
|
|
accel_body.y += radians(yaw_rate) * speed;
|
|
|
|
|
|
|
|
// now in earth frame
|
|
|
|
Vector3f accel_earth = dcm * accel_body;
|
|
|
|
|
|
|
|
// we are on the ground, so our vertical accel is zero
|
|
|
|
accel_earth.z = 0;
|
|
|
|
|
|
|
|
// work out acceleration as seen by the accelerometers. It sees the kinematic
|
|
|
|
// acceleration (ie. real movement), plus gravity
|
|
|
|
accel_body = dcm.transposed() * (accel_earth + Vector3f(0, 0, -GRAVITY_MSS));
|
|
|
|
|
|
|
|
// new velocity vector
|
|
|
|
velocity_ef += accel_earth * delta_time;
|
|
|
|
|
|
|
|
// new position vector
|
|
|
|
position += velocity_ef * delta_time;
|
|
|
|
|
|
|
|
// update lat/lon/altitude
|
|
|
|
update_position();
|
|
|
|
time_advance();
|
|
|
|
|
|
|
|
// update magnetic field
|
|
|
|
update_mag_field_bf();
|
|
|
|
}
|
|
|
|
|
|
|
|
} // namespace SITL
|