From 6939a9fa91b2fd5a177720160aab42d195619e85 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Thu, 11 Apr 2019 11:55:04 +1000
Subject: [PATCH] SITL: add capability to twist the aircraft

This makes the EKF very, very unhappy, but is useful for compass
calibration testing
---
 libraries/SITL/SIM_Aircraft.cpp    | 29 +++++++++++++++++++++++++++++
 libraries/SITL/SIM_Aircraft.h      |  3 ++-
 libraries/SITL/SIM_Multicopter.cpp |  1 +
 libraries/SITL/SITL.cpp            |  7 +++++++
 libraries/SITL/SITL.h              | 11 +++++++++++
 5 files changed, 50 insertions(+), 1 deletion(-)

diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp
index bb18b9972f..b53ac88774 100644
--- a/libraries/SITL/SIM_Aircraft.cpp
+++ b/libraries/SITL/SIM_Aircraft.cpp
@@ -804,3 +804,32 @@ void Aircraft::add_shove_forces(Vector3f &rot_accel, Vector3f &body_accel)
         sitl->shove.t = 0;
     }
 }
+
+void Aircraft::add_twist_forces(Vector3f &rot_accel)
+{
+    if (sitl == nullptr) {
+        return;
+    }
+    if (sitl->gnd_behav != -1) {
+        ground_behavior = (GroundBehaviour)sitl->gnd_behav.get();
+    }
+    const uint32_t now = AP_HAL::millis();
+    if (sitl == nullptr) {
+        return;
+    }
+    if (sitl->twist.t == 0) {
+        return;
+    }
+    if (sitl->twist.start_ms == 0) {
+        sitl->twist.start_ms = now;
+    }
+    if (now - sitl->twist.start_ms < uint32_t(sitl->twist.t)) {
+        // FIXME: can we get a vector operation here instead?
+        rot_accel.x += sitl->twist.x;
+        rot_accel.y += sitl->twist.y;
+        rot_accel.z += sitl->twist.z;
+    } else {
+        sitl->twist.start_ms = 0;
+        sitl->twist.t = 0;
+    }
+}
diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h
index 736e8c5e34..abe3b38ddd 100644
--- a/libraries/SITL/SIM_Aircraft.h
+++ b/libraries/SITL/SIM_Aircraft.h
@@ -181,7 +181,7 @@ protected:
     // allow for AHRS_ORIENTATION
     AP_Int8 *ahrs_orientation;
 
-    enum {
+    enum GroundBehaviour {
         GROUND_BEHAVIOR_NONE = 0,
         GROUND_BEHAVIOR_NO_MOVEMENT,
         GROUND_BEHAVIOR_FWD_ONLY,
@@ -244,6 +244,7 @@ protected:
     void update_external_payload(const struct sitl_input &input);
 
     void add_shove_forces(Vector3f &rot_accel, Vector3f &body_accel);
+    void add_twist_forces(Vector3f &rot_accel);
 
 private:
     uint64_t last_time_us = 0;
diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp
index 6063cc7996..090a67ff14 100644
--- a/libraries/SITL/SIM_Multicopter.cpp
+++ b/libraries/SITL/SIM_Multicopter.cpp
@@ -50,6 +50,7 @@ void MultiCopter::calculate_forces(const struct sitl_input &input, Vector3f &rot
 {
     frame->calculate_forces(*this, input, rot_accel, body_accel);
     add_shove_forces(rot_accel, body_accel);
+    add_twist_forces(rot_accel);
 }
     
 /*
diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp
index b66a884aba..0da3770548 100644
--- a/libraries/SITL/SITL.cpp
+++ b/libraries/SITL/SITL.cpp
@@ -160,6 +160,13 @@ const AP_Param::GroupInfo SITL::var_info2[] = {
     AP_GROUPINFO("GYR_FAIL_MSK",   35, SITL,  gyro_fail_mask,  0),
     AP_GROUPINFO("ACC_FAIL_MSK",   36, SITL,  accel_fail_mask,  0),
 
+    AP_GROUPINFO("TWIST_X",     37, SITL,  twist.x, 0),
+    AP_GROUPINFO("TWIST_Y",     38, SITL,  twist.y, 0),
+    AP_GROUPINFO("TWIST_Z",     39, SITL,  twist.z, 0),
+    AP_GROUPINFO("TWIST_TIME",  40, SITL,  twist.t, 0),
+
+    AP_GROUPINFO("GND_BEHAV",   41, SITL,  gnd_behav, -1),
+
     AP_GROUPEND
 };
     
diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h
index 4c75a48401..da914abf1a 100644
--- a/libraries/SITL/SITL.h
+++ b/libraries/SITL/SITL.h
@@ -244,6 +244,17 @@ public:
         uint32_t start_ms;
     } shove;
 
+    struct {
+        AP_Float x;
+        AP_Float y;
+        AP_Float z;
+        AP_Int32 t;
+
+        uint32_t start_ms;
+    } twist;
+
+    AP_Int8 gnd_behav;
+
     uint16_t irlock_port;
 
     void simstate_send(mavlink_channel_t chan);