aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

SITL: sailboat add tide

Browse Source
This commit is contained in:
Peter Hall 2019-05-28 00:52:11 +01:00 committed by Randy Mackay
parent 90bf224e7f
commit 2a3676003a
4 changed files with 21 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/SITL/SIM_Sailboat.cpp
View File

@ -198,7 +198,7 @@ void Sailboat::update(const struct sitl_input &input)
float delta_time = frame_time_us * 1.0e-6f;
// speed in m/s in body frame
Vector3f velocity_body = dcm.transposed() * velocity_ef;
Vector3f velocity_body = dcm.transposed() * velocity_ef_water;
// speed along x axis, +ve is forward
float speed = velocity_body.x;
@ -240,8 +240,17 @@ void Sailboat::update(const struct sitl_input &input)
// acceleration (ie. real movement), plus gravity
accel_body = dcm.transposed() * (accel_earth + Vector3f(0, 0, -GRAVITY_MSS));
// tide calcs
Vector3f tide_velocity_ef;
if (hal.util->get_soft_armed() && !is_zero(sitl->tide.speed) ) {
tide_velocity_ef.x = -cosf(radians(sitl->tide.direction)) * sitl->tide.speed;
tide_velocity_ef.y = -sinf(radians(sitl->tide.direction)) * sitl->tide.speed;
tide_velocity_ef.z = 0.0f;
}
// new velocity vector
velocity_ef += accel_earth * delta_time;
velocity_ef_water += accel_earth * delta_time;
velocity_ef = velocity_ef_water + tide_velocity_ef;
// new position vector
position += velocity_ef * delta_time;

2
libraries/SITL/SIM_Sailboat.h
View File

@ -62,6 +62,8 @@ private:
const float mass = 2.0f;
Vector3f velocity_ef_water; // m/s
// simulate basic waves / swell
void update_wave(float delta_time);
Vector3f wave_gyro; // rad/s

4
libraries/SITL/SITL.cpp
View File

@ -170,12 +170,14 @@ const AP_Param::GroupInfo SITL::var_info2[] = {
AP_GROUPINFO("GPS_HDG", 43, SITL, gps_hdg_enabled, 0),
// sailboat wave simulation parameters
// sailboat wave and tide simulation parameters
AP_GROUPINFO("WAVE_ENABLE", 44, SITL, wave.enable, 0.0f),
AP_GROUPINFO("WAVE_LENGTH", 45, SITL, wave.length, 10.0f),
AP_GROUPINFO("WAVE_AMP", 46, SITL, wave.amp, 0.5f),
AP_GROUPINFO("WAVE_DIR", 47, SITL, wave.direction, 0.0f),
AP_GROUPINFO("WAVE_SPEED", 48, SITL, wave.speed, 0.5f),
AP_GROUPINFO("TIDE_DIR", 49, SITL, tide.direction, 0.0f),
AP_GROUPINFO("TIDE_SPEED", 50, SITL, tide.speed, 0.0f),
AP_GROUPEND

5
libraries/SITL/SITL.h
View File

@ -263,6 +263,11 @@ public:
AP_Float speed; // m/s
} wave;
struct {
AP_Float direction; // deg (direction tide is coming from)
AP_Float speed; // m/s
} tide;
uint16_t irlock_port;
void simstate_send(mavlink_channel_t chan);