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

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

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;

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

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

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