diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp
index b95dadffef..5a9139ee09 100644
--- a/libraries/SITL/SIM_Aircraft.cpp
+++ b/libraries/SITL/SIM_Aircraft.cpp
@@ -532,6 +532,11 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel)
}
position.z = -(ground_level + frame_height - home.alt * 0.01f + ground_height_difference());
+ // get speed of ground movement (for ship takeoff/landing)
+ float yaw_rate = 0;
+ const Vector2f ship_movement = sitl->shipsim.get_ground_speed_adjustment(location, yaw_rate);
+ const Vector3f gnd_movement(ship_movement.x, ship_movement.y, 0);
+
switch (ground_behavior) {
case GROUND_BEHAVIOR_NONE:
break;
@@ -539,10 +544,11 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel)
// zero roll/pitch, but keep yaw
float r, p, y;
dcm.to_euler(&r, &p, &y);
+ y = y + yaw_rate * delta_time;
dcm.from_euler(0.0f, 0.0f, y);
- // no X or Y movement
- velocity_ef.x = 0.0f;
- velocity_ef.y = 0.0f;
+ // X, Y movement tracks ground movement
+ velocity_ef.x = gnd_movement.x;
+ velocity_ef.y = gnd_movement.y;
if (velocity_ef.z > 0.0f) {
velocity_ef.z = 0.0f;
}
@@ -561,6 +567,7 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel)
} else {
p = MAX(p, 0);
}
+ y = y + yaw_rate * delta_time;
dcm.from_euler(0.0f, p, y);
// only fwd movement
Vector3f v_bf = dcm.transposed() * velocity_ef;
@@ -568,11 +575,25 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel)
if (v_bf.x < 0.0f) {
v_bf.x = 0.0f;
}
+
+ Vector3f gnd_movement_bf = dcm.transposed() * gnd_movement;
+
+ // lateral speed equals ground movement
+ v_bf.y = gnd_movement_bf.y;
+
+ if (!gnd_movement_bf.is_zero()) {
+ // fwd speed slowly approaches ground movement to simulate wheel friction
+ const float tconst = 20; // seconds
+ const float alpha = delta_time/(delta_time+tconst/M_2PI);
+ v_bf.x += (gnd_movement.x - v_bf.x) * alpha;
+ }
+
velocity_ef = dcm * v_bf;
if (velocity_ef.z > 0.0f) {
velocity_ef.z = 0.0f;
}
gyro.zero();
+ gyro.z = yaw_rate;
use_smoothing = true;
break;
}
@@ -580,11 +601,15 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel)
// point straight up
float r, p, y;
dcm.to_euler(&r, &p, &y);
+ y = y + yaw_rate * delta_time;
dcm.from_euler(0.0f, radians(90), y);
// no movement
if (accel_earth.z > -1.1*GRAVITY_MSS) {
velocity_ef.zero();
}
+ // X, Y movement tracks ground movement
+ velocity_ef.x = gnd_movement.x;
+ velocity_ef.y = gnd_movement.y;
gyro.zero();
use_smoothing = true;
break;
@@ -816,6 +841,8 @@ void Aircraft::update_external_payload(const struct sitl_input &input)
if (richenpower) {
richenpower->update(input);
}
+
+ sitl->shipsim.update();
}
void Aircraft::add_shove_forces(Vector3f &rot_accel, Vector3f &body_accel)
diff --git a/libraries/SITL/SIM_QuadPlane.cpp b/libraries/SITL/SIM_QuadPlane.cpp
index ee91247bb0..9287a6fea5 100644
--- a/libraries/SITL/SIM_QuadPlane.cpp
+++ b/libraries/SITL/SIM_QuadPlane.cpp
@@ -127,6 +127,7 @@ void QuadPlane::update(const struct sitl_input &input)
accel_body += quad_accel_body;
update_dynamics(rot_accel);
+ update_external_payload(input);
// update lat/lon/altitude
update_position();
diff --git a/libraries/SITL/SIM_Ship.cpp b/libraries/SITL/SIM_Ship.cpp
new file mode 100644
index 0000000000..99aa38cafa
--- /dev/null
+++ b/libraries/SITL/SIM_Ship.cpp
@@ -0,0 +1,213 @@
+/*
+ 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 .
+ */
+/*
+ simulate ship takeoff/landing
+*/
+
+#include "SIM_Ship.h"
+#include "SITL.h"
+
+#include
+
+#include "SIM_Aircraft.h"
+#include
+
+// use a spare channel for send. This is static to avoid mavlink
+// header import in SIM_Ship.h
+static const mavlink_channel_t mavlink_ch = (mavlink_channel_t)(MAVLINK_COMM_0+6);
+
+using namespace SITL;
+
+// SITL Ship parameters
+const AP_Param::GroupInfo ShipSim::var_info[] = {
+ AP_GROUPINFO("ENABLE", 1, ShipSim, enable, 0),
+ AP_GROUPINFO("SPEED", 2, ShipSim, speed, 3),
+ AP_GROUPINFO("PSIZE", 3, ShipSim, path_size, 1000),
+ AP_GROUPINFO("SYSID", 4, ShipSim, sys_id, 17),
+ AP_GROUPINFO("DSIZE", 5, ShipSim, deck_size, 10),
+ AP_GROUPEND
+};
+
+/*
+ update a simulated vehicle
+ */
+void Ship::update(float delta_t)
+{
+ // acceletate over time to keep EKF happy
+ const float max_accel = 3.0;
+ const float dspeed_max = max_accel * delta_t;
+ speed = constrain_float(sim->speed.get(), speed-dspeed_max, speed+dspeed_max);
+
+ // calculate how far around the circle we go
+ float circumference = M_PI * sim->path_size.get();
+ float dist = delta_t * speed;
+ float dangle = (dist / circumference) * 360.0;
+
+ if (delta_t > 0) {
+ yaw_rate = radians(dangle) / delta_t;
+ }
+ heading_deg += dangle;
+ heading_deg = wrap_360(heading_deg);
+
+ Vector2f dpos(dist, 0);
+ dpos.rotate(radians(heading_deg));
+
+ position += dpos;
+}
+
+ShipSim::ShipSim()
+{
+ if (!valid_channel(mavlink_ch)) {
+ AP_HAL::panic("Invalid mavlink channel for ShipSim");
+ }
+ AP_Param::setup_object_defaults(this, var_info);
+}
+
+/*
+ get ground speed adjustment if we are landed on the ship
+ */
+Vector2f ShipSim::get_ground_speed_adjustment(const Location &loc, float &yaw_rate)
+{
+ if (!enable) {
+ yaw_rate = 0;
+ return Vector2f(0,0);
+ }
+ Location shiploc = home;
+ shiploc.offset(ship.position.x, ship.position.y);
+ if (loc.get_distance(shiploc) > deck_size) {
+ yaw_rate = 0;
+ return Vector2f(0,0);
+ }
+ Vector2f vel(ship.speed, 0);
+ vel.rotate(radians(ship.heading_deg));
+ yaw_rate = ship.yaw_rate;
+ return vel;
+}
+
+/*
+ update the ShipSim peripheral state
+*/
+void ShipSim::update(void)
+{
+ if (!enable) {
+ return;
+ }
+
+ auto *sitl = AP::sitl();
+ uint32_t now_us = AP_HAL::micros();
+
+ if (!initialised) {
+ home = sitl->state.home;
+ if (home.lat == 0 && home.lng == 0) {
+ return;
+ }
+ initialised = true;
+ ::printf("ShipSim home %f %f\n", home.lat*1.0e-7, home.lng*1.0e-7);
+ ship.sim = this;
+ last_update_us = now_us;
+ last_report_ms = AP_HAL::millis();
+ }
+
+ float dt = (now_us - last_update_us)*1.0e-6;
+ last_update_us = now_us;
+
+ ship.update(dt);
+
+ uint32_t now_ms = AP_HAL::millis();
+ if (now_ms - last_report_ms >= reporting_period_ms) {
+ last_report_ms = now_ms;
+ send_report();
+ }
+}
+
+/*
+ send a report to the vehicle control code over MAVLink
+*/
+void ShipSim::send_report(void)
+{
+ if (!mavlink_connected && mav_socket.connect(target_address, target_port)) {
+ ::printf("ShipSim connected to %s:%u\n", target_address, (unsigned)target_port);
+ mavlink_connected = true;
+ }
+ if (!mavlink_connected) {
+ return;
+ }
+
+ uint32_t now = AP_HAL::millis();
+ mavlink_message_t msg;
+ uint16_t len;
+ uint8_t buf[300];
+
+ const uint8_t component_id = MAV_COMP_ID_USER10;
+
+ if (now - last_heartbeat_ms >= 1000) {
+ last_heartbeat_ms = now;
+ mavlink_msg_heartbeat_pack_chan(sys_id.get(),
+ component_id,
+ mavlink_ch,
+ &msg,
+ MAV_TYPE_SURFACE_BOAT,
+ MAV_AUTOPILOT_INVALID,
+ 0,
+ 0,
+ 0);
+ len = mavlink_msg_to_send_buffer(buf, &msg);
+ mav_socket.send(buf, len);
+ }
+
+
+ /*
+ send a GLOBAL_POSITION_INT messages
+ */
+ Location loc = home;
+ loc.offset(ship.position.x, ship.position.y);
+
+ int32_t alt;
+ bool have_alt = false;
+
+#if AP_TERRAIN_AVAILABLE
+ auto &terrain = AP::terrain();
+ float height;
+ if (terrain.enabled() && terrain.height_amsl(loc, height, true)) {
+ alt = height * 1000;
+ have_alt = true;
+ }
+#endif
+ if (!have_alt) {
+ // assume home altitude
+ alt = home.alt;
+ }
+
+ Vector2f vel(ship.speed, 0);
+ vel.rotate(radians(ship.heading_deg));
+
+ mavlink_msg_global_position_int_pack_chan(sys_id,
+ component_id,
+ mavlink_ch,
+ &msg,
+ now,
+ loc.lat,
+ loc.lng,
+ alt,
+ 0,
+ vel.x*100,
+ vel.y*100,
+ 0,
+ ship.heading_deg*100);
+ len = mavlink_msg_to_send_buffer(buf, &msg);
+ if (len > 0) {
+ mav_socket.send(buf, len);
+ }
+}
diff --git a/libraries/SITL/SIM_Ship.h b/libraries/SITL/SIM_Ship.h
new file mode 100644
index 0000000000..5f98999f86
--- /dev/null
+++ b/libraries/SITL/SIM_Ship.h
@@ -0,0 +1,86 @@
+/*
+ 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 .
+ */
+/*
+ simulate ship takeoff and landing
+*/
+
+#pragma once
+
+#include
+#include
+#include
+
+namespace SITL {
+
+/*
+ a class for individual simulated vehicles
+*/
+
+class ShipSim;
+class Ship {
+ friend class ShipSim;
+
+private:
+ void update(float delta_t);
+
+ Vector2f position;
+ float heading_deg;
+ float yaw_rate;
+ float speed;
+ ShipSim *sim;
+};
+
+class ShipSim {
+public:
+ friend class Ship;
+ ShipSim();
+ void update(void);
+
+ static const struct AP_Param::GroupInfo var_info[];
+
+ /*
+ get a ground speed adjustment for a landed vehicle based on
+ whether it is on a ship
+ */
+ Vector2f get_ground_speed_adjustment(const Location &loc, float &yaw_rate);
+
+private:
+
+ AP_Int8 enable;
+ AP_Float speed;
+ AP_Float path_size;
+ AP_Float deck_size;
+ AP_Int8 sys_id;
+
+ Location home;
+ const char *target_address = "127.0.0.1";
+ const uint16_t target_port = 5762;
+
+ bool initialised;
+ Ship ship;
+ uint32_t last_update_us;
+
+ // reporting period in ms
+ const float reporting_period_ms = 200;
+ uint32_t last_report_ms;
+ uint32_t last_heartbeat_ms;
+
+ SocketAPM mav_socket { false };
+ bool mavlink_connected;
+
+ void send_report(void);
+};
+
+} // namespace SITL
diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp
index 4999360466..c19dce032c 100644
--- a/libraries/SITL/SITL.cpp
+++ b/libraries/SITL/SITL.cpp
@@ -77,6 +77,7 @@ const AP_Param::GroupInfo SITL::var_info[] = {
AP_GROUPINFO("FLOW_POS", 56, SITL, optflow_pos_offset, 0),
AP_GROUPINFO("ACC2_BIAS", 57, SITL, accel2_bias, 0),
AP_GROUPINFO("ENGINE_FAIL", 58, SITL, engine_fail, 0),
+ AP_SUBGROUPINFO(shipsim, "SHIP_", 59, SITL, ShipSim),
AP_SUBGROUPEXTENSION("", 60, SITL, var_mag),
AP_SUBGROUPEXTENSION("", 61, SITL, var_gps),
AP_SUBGROUPEXTENSION("", 62, SITL, var_info3),
diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h
index 4f2a282390..3224d8b5a9 100644
--- a/libraries/SITL/SITL.h
+++ b/libraries/SITL/SITL.h
@@ -18,6 +18,7 @@
#include "SIM_ToneAlarm.h"
#include "SIM_EFI_MegaSquirt.h"
#include "SIM_RichenPower.h"
+#include "SIM_Ship.h"
namespace SITL {
@@ -350,6 +351,9 @@ public:
Sprayer sprayer_sim;
+ // simulated ship takeoffs
+ ShipSim shipsim;
+
Gripper_Servo gripper_sim;
Gripper_EPM gripper_epm_sim;