Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

SITL: sailboat fixes from peer review

This commit is contained in:
Randy Mackay 2018-09-25 15:02:51 +09:00
parent ada073fbdc
commit b7c88da67c
2 changed files with 46 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
libraries/SITL/SIM_Sailboat.cpp
View File

@ -27,71 +27,71 @@
namespace SITL { namespace SITL {
#define STEERING_SERVO_CH 0 // steering controlled by servo output 1
#define MAINSAIL_SERVO_CH 3 // main sail controlled by servo output 4
Sailboat::Sailboat(const char *home_str, const char *frame_str) : Sailboat::Sailboat(const char *home_str, const char *frame_str) :
Aircraft(home_str, frame_str), Aircraft(home_str, frame_str),
max_wheel_turn(35), steering_angle_max(35),
turning_circle(1.8) turning_circle(1.8)
{ {
} }
// calculate the lift and drag as values from 0 to 1 // 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 // given an apparent wind speed in m/s and angle-of-attack in degrees
void Sailboat::calc_lift_and_drag(float wind_speed, float angle_of_attack_deg, float& lift, float& drag) void Sailboat::calc_lift_and_drag(float wind_speed, float angle_of_attack_deg, float& lift, float& drag) const
{ {
const uint16_t index_width_deg = 10;
const uint8_t index_max = ARRAY_SIZE(lift_curve) - 1;
// check extremes // check extremes
if (angle_of_attack_deg <= 0.0f) { if (angle_of_attack_deg <= 0.0f) {
lift = lift_curve[0]; lift = lift_curve[0];
drag = drag_curve[0]; drag = drag_curve[0];
return; return;
} }
if (angle_of_attack_deg >= 170.0f) { if (angle_of_attack_deg >= index_max * index_width_deg) {
lift = lift_curve[17]; lift = lift_curve[index_max];
drag = drag_curve[17]; drag = drag_curve[index_max];
return; return;
} }
uint8_t index = constrain_int16(angle_of_attack_deg / 10, 0, 17); uint8_t index = constrain_int16(angle_of_attack_deg / index_width_deg, 0, index_max);
float remainder = angle_of_attack_deg - (index * 10.0f); float remainder = angle_of_attack_deg - (index * index_width_deg);
lift = linear_interpolate(lift_curve[index], lift_curve[index+1], remainder, 0.0f, 10.0f); 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, 10.0f); drag = linear_interpolate(drag_curve[index], drag_curve[index+1], remainder, 0.0f, index_width_deg);
// apply scaling by wind speed // apply scaling by wind speed
lift *= wind_speed; lift *= wind_speed;
drag *= wind_speed; drag *= wind_speed;
} }
/* // return turning circle (diameter) in meters for steering angle proportion in the range -1 to +1
return turning circle (diameter) in meters for steering angle proportion in degrees float Sailboat::get_turn_circle(float steering) const
*/
float Sailboat::turn_circle(float steering)
{ {
if (fabsf(steering) < 1.0e-6) { if (is_zero(steering)) {
return 0; return 0;
} }
return turning_circle * sinf(radians(max_wheel_turn)) / sinf(radians(steering*max_wheel_turn)); return turning_circle * sinf(radians(steering_angle_max)) / sinf(radians(steering * steering_angle_max));
} }
/* // return yaw rate in deg/sec given a steering input (in the range -1 to +1) and speed in m/s
return yaw rate in degrees/second given steering_angle and speed float Sailboat::get_yaw_rate(float steering, float speed) const
*/
float Sailboat::calc_yaw_rate(float steering, float speed)
{ {
if (fabsf(steering) < 1.0e-6 or fabsf(speed) < 1.0e-6) { if (is_zero(steering) || is_zero(speed)) {
return 0; return 0;
} }
float d = turn_circle(steering); float d = get_turn_circle(steering);
float c = M_PI * d; float c = M_PI * d;
float t = c / speed; float t = c / speed;
float rate = 360.0f / t; float rate = 360.0f / t;
return rate; return rate;
} }
/* // return lateral acceleration in m/s/s given a steering input (in the range -1 to +1) and speed in m/s
return lateral acceleration in m/s/s float Sailboat::get_lat_accel(float steering, float speed) const
*/
float Sailboat::calc_lat_accel(float steering_angle, float speed)
{ {
float yaw_rate = calc_yaw_rate(steering_angle, speed); float yaw_rate = get_yaw_rate(steering, speed);
float accel = radians(yaw_rate) * speed; float accel = radians(yaw_rate) * speed;
return accel; return accel;
} }
@ -105,10 +105,10 @@ void Sailboat::update(const struct sitl_input &input)
update_wind(input); update_wind(input);
// in sailboats the steering controls the rudder, the throttle controls the main sail position // in sailboats the steering controls the rudder, the throttle controls the main sail position
float steering = 2*((input.servos[0]-1000)/1000.0f - 0.5f); float steering = 2*((input.servos[STEERING_SERVO_CH]-1000)/1000.0f - 0.5f);
// calculate mainsail angle from servo output 4, 0 to 90 degrees // calculate mainsail angle from servo output 4, 0 to 90 degrees
float mainsail_angle_bf = constrain_float((input.servos[3]-1000)/1000.0f * 90.0f, 0.0f, 90.0f); float mainsail_angle_bf = constrain_float((input.servos[MAINSAIL_SERVO_CH]-1000)/1000.0f * 90.0f, 0.0f, 90.0f);
// calculate apparent wind in earth-frame (this is the direction the wind is coming from) // calculate apparent wind in earth-frame (this is the direction the wind is coming from)
Vector3f wind_apparent_ef = wind_ef + velocity_ef; Vector3f wind_apparent_ef = wind_ef + velocity_ef;
@ -138,7 +138,7 @@ void Sailboat::update(const struct sitl_input &input)
float speed = velocity_body.x; float speed = velocity_body.x;
// yaw rate in degrees/s // yaw rate in degrees/s
float yaw_rate = calc_yaw_rate(steering, speed); float yaw_rate = get_yaw_rate(steering, speed);
gyro = Vector3f(0,0,radians(yaw_rate)); gyro = Vector3f(0,0,radians(yaw_rate));
@ -154,7 +154,6 @@ void Sailboat::update(const struct sitl_input &input)
// now in earth frame // now in earth frame
Vector3f accel_earth = dcm * accel_body; Vector3f accel_earth = dcm * accel_body;
accel_earth += Vector3f(0, 0, GRAVITY_MSS);
// we are on the ground, so our vertical accel is zero // we are on the ground, so our vertical accel is zero
accel_earth.z = 0; accel_earth.z = 0;

27
libraries/SITL/SIM_Sailboat.h
View File

@ -39,18 +39,25 @@ public:
private: private:
void calc_lift_and_drag(float wind_speed, float angle_of_attack_deg, float& lift, float& drag); // 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
float turn_circle(float steering); void calc_lift_and_drag(float wind_speed, float angle_of_attack_deg, float& lift, float& drag) const;
float calc_yaw_rate(float steering, float speed);
float calc_lat_accel(float steering_angle, float speed);
float max_wheel_turn; // return turning circle (diameter) in meters for steering angle proportion in the range -1 to +1
float turning_circle; float get_turn_circle(float steering) const;
// 10 point curves for lift and drag. index is angle/10deg // return yaw rate in deg/sec given a steering input (in the range -1 to +1) and speed in m/s
// angle-of-attack 0 10 20 30 40 50 60 70 80 90 100 110 120 130 140 150 160 170+ float get_yaw_rate(float steering, float speed) const;
float lift_curve[18] = {0.00f, 0.00f, 0.80f, 1.00f, 0.95f, 0.75f, 0.60f, 0.40f, 0.20f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f};
float drag_curve[18] = {0.10f, 0.10f, 0.12f, 0.15f, 0.20f, 0.27f, 0.35f, 0.50f, 0.70f, 1.00f, 0.70f, 0.50f, 0.35f, 0.27f, 0.20f, 0.15f, 0.12f, 0.10f}; // return lateral acceleration in m/s/s given a steering input (in the range -1 to +1) and speed in m/s
float get_lat_accel(float steering, float speed) const;
float steering_angle_max; // vehicle steering mechanism's max angle in degrees
float turning_circle; // vehicle minimum turning circle diameter in meters
// lift and drag curves. index is angle/10deg
// angle-of-attack 0 10 20 30 40 50 60 70 80 90 100 110 120 130 140 150 160 170+
const float lift_curve[18] = {0.00f, 0.00f, 0.80f, 1.00f, 0.95f, 0.75f, 0.60f, 0.40f, 0.20f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f};
const float drag_curve[18] = {0.10f, 0.10f, 0.12f, 0.15f, 0.20f, 0.27f, 0.35f, 0.50f, 0.70f, 1.00f, 0.70f, 0.50f, 0.35f, 0.27f, 0.20f, 0.15f, 0.12f, 0.10f};
}; };
} // namespace SITL } // namespace SITL