SITL: add simulated tie-down clamp

Author: peterbarker

libraries/SITL/SIM_Aircraft.cpp

@@ -958,6 +958,45 @@ void Aircraft::extrapolate_sensors(float delta_time)
     velocity_air_bf = dcm.transposed() * velocity_air_ef;
 }
 
+bool Aircraft::Clamp::clamped(Aircraft &aircraft, const struct sitl_input &input)
+{
+    const auto clamp_ch = AP::sitl()->clamp_ch;
+    if (clamp_ch < 1) {
+        return false;
+    }
+    const uint32_t clamp_idx = clamp_ch - 1;
+    if (clamp_idx > ARRAY_SIZE(input.servos)) {
+        return false;
+    }
+    const uint16_t servo_pos = input.servos[clamp_idx];
+    bool new_clamped = currently_clamped;
+    if (servo_pos < 1200) {
+        if (currently_clamped) {
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SITL: Clamp: released vehicle");
+            new_clamped = false;
+        }
+        grab_attempted = false;
+    } else {
+        // re-clamp if < 10cm from home
+        if (servo_pos > 1800 && !grab_attempted) {
+            const Vector3d pos = aircraft.get_position_relhome();
+            const float distance_from_home = pos.length();
+            // GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SITL: Clamp: dist=%f", distance_from_home);
+            if (distance_from_home < 0.5) {
+                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SITL: Clamp: grabbed vehicle");
+                new_clamped = true;
+            } else if (!grab_attempted) {
+                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SITL: Clamp: missed vehicle");
+            }
+            grab_attempted = true;
+        }
+    }
+
+    currently_clamped = new_clamped;
+
+    return currently_clamped;
+}
+
 void Aircraft::update_external_payload(const struct sitl_input &input)
 {
     external_payload_mass = 0;

libraries/SITL/SIM_Aircraft.h

@@ -317,6 +317,15 @@ class Aircraft {
     // get local thermal updraft
     float get_local_updraft(const Vector3d &currentPos);
 
+    // clamp support
+    class Clamp {
+    public:
+        bool clamped(class Aircraft&, const struct sitl_input &input);  // true if the vehicle is currently clamped down
+    private:
+        bool currently_clamped;
+        bool grab_attempted;  // avoid warning multiple times about missed grab
+    } clamp;
+
 private:
     uint64_t last_time_us;
     uint32_t frame_counter;

libraries/SITL/SIM_Multicopter.cpp

@@ -61,6 +61,11 @@ void MultiCopter::update(const struct sitl_input &input)
     Vector3f rot_accel;
 
     calculate_forces(input, rot_accel, accel_body);
+    // simulated clamp holding vehicle down
+    if (clamp.clamped(*this, input)) {
+        rot_accel.zero();
+        accel_body.zero();
+    }
 
     // estimate voltage and current
     frame->current_and_voltage(battery_voltage, battery_current);

libraries/SITL/SITL.cpp

@@ -1083,6 +1083,11 @@ const AP_Param::GroupInfo SIM::var_ins[] = {
     // @Description: Allow relay output operation when running SIM-on-hardware
     AP_GROUPINFO("OH_RELAY_MSK",     48, SIM, on_hardware_relay_enable_mask, SIM_DEFAULT_ENABLED_RELAY_CHANNELS),
 
+    // @Param: CLAMP_CH
+    // @DisplayName: Simulated Clamp Channel
+    // @Description: If non-zero the vehicle will be clamped in position until the value on this servo channel passes 1800PWM
+    AP_GROUPINFO("CLAMP_CH",     49, SIM, clamp_ch, 0),
+
     // the IMUT parameters must be last due to the enable parameters
 #if HAL_INS_TEMPERATURE_CAL_ENABLE
     AP_SUBGROUPINFO(imu_tcal[0], "IMUT1_", 61, SIM, AP_InertialSensor_TCal),

libraries/SITL/SITL.h

@@ -527,6 +527,9 @@ class SIM {
     // Master instance to use servos from with slave instances
     AP_Int8 ride_along_master;
 
+    // clamp simulation - servo channel starting at offset 1 (usually ailerons)
+    AP_Int8 clamp_ch;
+
 #if AP_SIM_INS_FILE_ENABLED
     enum INSFileMode {
         INS_FILE_NONE = 0,