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

@@ -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;
+    }
+}

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;

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);
 }
     
 /*

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
 };
     

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);