From 4db011f530d8fd1ba704bda6465ca9fd508f3921 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Mon, 27 Nov 2017 21:45:23 +1100
Subject: [PATCH] SITL: add SIM_SHOVE_* options to shove the simulated vehicle

The parameter values are time in milliseconds and body-frame
acceleration in m/s/s

So to test throw mode:
 mode throw
 param set SIM_SHOVE_Z -30
 arm throttle
 param set SIM_SHOVE_TIME 500
---
 libraries/SITL/SIM_Aircraft.cpp    | 21 +++++++++++++++++++++
 libraries/SITL/SIM_Aircraft.h      |  2 ++
 libraries/SITL/SIM_Multicopter.cpp |  1 +
 libraries/SITL/SITL.cpp            |  4 ++++
 libraries/SITL/SITL.h              | 11 ++++++++++-
 5 files changed, 38 insertions(+), 1 deletion(-)

diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp
index b6a97d6729..2bb099fa0c 100644
--- a/libraries/SITL/SIM_Aircraft.cpp
+++ b/libraries/SITL/SIM_Aircraft.cpp
@@ -781,3 +781,24 @@ void Aircraft::update_external_payload(const struct sitl_input &input)
         precland->update(get_location(), get_position());
     }
 }
+
+void Aircraft::add_shove_forces(Vector3f &rot_accel, Vector3f &body_accel)
+{
+    const uint32_t now = AP_HAL::millis();
+
+    if (sitl->shove.t == 0) {
+        return;
+    }
+    if (sitl->shove.start_ms == 0) {
+        sitl->shove.start_ms = now;
+    }
+    if (now - sitl->shove.start_ms < uint32_t(sitl->shove.t)) {
+        // FIXME: can we get a vector operation here instead?
+        body_accel.x += sitl->shove.x;
+        body_accel.y += sitl->shove.y;
+        body_accel.z += sitl->shove.z;
+    } else {
+        sitl->shove.start_ms = 0;
+        sitl->shove.t = 0;
+    }
+}
diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h
index c77687dd92..736e8c5e34 100644
--- a/libraries/SITL/SIM_Aircraft.h
+++ b/libraries/SITL/SIM_Aircraft.h
@@ -243,6 +243,8 @@ protected:
     // update external payload/sensor dynamic
     void update_external_payload(const struct sitl_input &input);
 
+    void add_shove_forces(Vector3f &rot_accel, Vector3f &body_accel);
+
 private:
     uint64_t last_time_us = 0;
     uint32_t frame_counter = 0;
diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp
index 877aa83216..6063cc7996 100644
--- a/libraries/SITL/SIM_Multicopter.cpp
+++ b/libraries/SITL/SIM_Multicopter.cpp
@@ -49,6 +49,7 @@ MultiCopter::MultiCopter(const char *home_str, const char *frame_str) :
 void MultiCopter::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel)
 {
     frame->calculate_forces(*this, input, rot_accel, body_accel);
+    add_shove_forces(rot_accel, body_accel);
 }
     
 /*
diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp
index 097b155c85..c17e8ffa9a 100644
--- a/libraries/SITL/SITL.cpp
+++ b/libraries/SITL/SITL.cpp
@@ -148,6 +148,10 @@ const AP_Param::GroupInfo SITL::var_info2[] = {
     // @Path: ./SIM_Precland.cpp
     AP_SUBGROUPINFO(precland_sim, "PLD_", 29, SITL, SIM_Precland),
 
+    AP_GROUPINFO("SHOVE_X",     30, SITL,  shove.x, 0),
+    AP_GROUPINFO("SHOVE_Y",     31, SITL,  shove.y, 0),
+    AP_GROUPINFO("SHOVE_Z",     32, SITL,  shove.z, 0),
+    AP_GROUPINFO("SHOVE_TIME",  33, SITL,  shove.t, 0),
     AP_GROUPEND
 };
     
diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h
index 29c6b6ab9c..4011b0f1a1 100644
--- a/libraries/SITL/SITL.h
+++ b/libraries/SITL/SITL.h
@@ -229,7 +229,16 @@ public:
 
     // vibration frequencies in Hz on each axis
     AP_Vector3f vibe_freq;
-    
+
+    struct {
+        AP_Float x;
+        AP_Float y;
+        AP_Float z;
+        AP_Int32 t;
+
+        uint32_t start_ms;
+    } shove;
+
     uint16_t irlock_port;
 
     void simstate_send(mavlink_channel_t chan);