SITL: added servo filtering

SIM_SERVO_SPEED is servo time constant in seconds

libraries/SITL/SIM_Aircraft.cpp

@@ -664,6 +664,42 @@ void Aircraft::smooth_sensors(void)
     smoothing.last_update_us = now;
     smoothing.enabled = true;
 }
+
+/*
+  return a filtered servo input as a value from -1 to 1
+  servo is assumed to be 1000 to 2000, trim at 1500
+ */
+float Aircraft::filtered_idx(float v, uint8_t idx)
+{
+    if (sitl->servo_speed <= 0) {
+        return v;
+    }
+    float cutoff = 1.0 / (2 * M_PI * sitl->servo_speed);
+    servo_filter[idx].set_cutoff_frequency(cutoff);
+    return servo_filter[idx].apply(v, frame_time_us*1.0e-6);
+}
+    
+
+/*
+  return a filtered servo input as a value from -1 to 1
+  servo is assumed to be 1000 to 2000, trim at 1500
+ */
+float Aircraft::filtered_servo_angle(const struct sitl_input &input, uint8_t idx)
+{
+    float v = (input.servos[idx]-1500)/500.0f;
+    return filtered_idx(v, idx);
+}
+
+/*
+  return a filtered servo input as a value from 0 to 1
+  servo is assumed to be 1000 to 2000
+ */
+float Aircraft::filtered_servo_range(const struct sitl_input &input, uint8_t idx)
+{
+    float v = (input.servos[idx]-1000)/1000.0f;
+    return filtered_idx(v, idx);
+}
     
 } // namespace SITL
 
+

libraries/SITL/SIM_Aircraft.h

@@ -204,6 +204,11 @@ protected:
      * Boolean returns false if latitude and longitude are outside the valid input range of +-60 latitude and +-180 longitude
     */
     bool get_mag_field_ef(float latitude_deg, float longitude_deg, float &intensity_gauss, float &declination_deg, float &inclination_deg);
+
+    // return filtered servo input as -1 to 1 range
+    float filtered_idx(float v, uint8_t idx);
+    float filtered_servo_angle(const struct sitl_input &input, uint8_t idx);
+    float filtered_servo_range(const struct sitl_input &input, uint8_t idx);
     
 private:
     uint64_t last_time_us = 0;
@@ -221,6 +226,8 @@ private:
         uint64_t last_update_us;
         Location location;
     } smoothing;
+
+    LowPassFilterFloat servo_filter[4];
     
     /* set this always to the sampling in degrees for the table below */
     #define SAMPLING_RES		10.0f

libraries/SITL/SIM_JSBSim.cpp

@@ -322,10 +322,10 @@ bool JSBSim::open_fdm_socket(void)
 void JSBSim::send_servos(const struct sitl_input &input)
 {
     char *buf = NULL;
-    float aileron  = (input.servos[0]-1500)/500.0f;
-    float elevator = (input.servos[1]-1500)/500.0f;
-    float throttle = (input.servos[2]-1000)/1000.0;
-    float rudder   = (input.servos[3]-1500)/500.0f;
+    float aileron  = filtered_servo_angle(input, 0);
+    float elevator = filtered_servo_angle(input, 1);
+    float throttle = filtered_servo_range(input, 2);
+    float rudder   = filtered_servo_angle(input, 3);
     if (frame == FRAME_ELEVON) {
         // fake an elevon plane
         float ch1 = aileron;

libraries/SITL/SIM_Plane.cpp

@@ -206,9 +206,9 @@ Vector3f Plane::getForce(float inputAileron, float inputElevator, float inputRud
 
 void Plane::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel)
 {
-    float aileron  = (input.servos[0]-1500)/500.0f;
-    float elevator = (input.servos[1]-1500)/500.0f;
-    float rudder   = (input.servos[3]-1500)/500.0f;
+    float aileron  = filtered_servo_angle(input, 0);
+    float elevator = filtered_servo_angle(input, 1);
+    float rudder   = filtered_servo_angle(input, 3);
     float throttle;
     if (reverse_elevator_rudder) {
         elevator = -elevator;
@@ -231,9 +231,9 @@ void Plane::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel
     }
 
     if (reverse_thrust) {
-        throttle = constrain_float((input.servos[2]-1500)/500.0f, -1, 1);
+        throttle = filtered_servo_angle(input, 2);
     } else {
-        throttle = constrain_float((input.servos[2]-1000)/1000.0f, 0, 1);
+        throttle = filtered_servo_range(input, 2);
     }
     
     float thrust     = throttle;

libraries/SITL/SIM_Plane.h

@@ -20,6 +20,7 @@
 
 #include "SIM_Aircraft.h"
 #include "SIM_ICEngine.h"
+#include <Filter/LowPassFilter.h>
 
 namespace SITL {
 

libraries/SITL/SITL.cpp

@@ -46,7 +46,7 @@ const AP_Param::GroupInfo SITL::var_info[] = {
     AP_GROUPINFO("GPS_BYTELOSS",  13, SITL,  gps_byteloss,  0),
     AP_GROUPINFO("GPS_NUMSATS",   14, SITL,  gps_numsats,   10),
     AP_GROUPINFO("MAG_ERROR",     15, SITL,  mag_error,  0),
-    AP_GROUPINFO("SERVO_RATE",    16, SITL,  servo_rate,  0),
+    AP_GROUPINFO("SERVO_SPEED",   16, SITL,  servo_speed,  0.14),
     AP_GROUPINFO("GPS_GLITCH",    17, SITL,  gps_glitch,  0),
     AP_GROUPINFO("GPS_HZ",        18, SITL,  gps_hertz,  5),
     AP_GROUPINFO("BATT_VOLTAGE",  19, SITL,  batt_voltage,  12.6f),

libraries/SITL/SITL.h

@@ -76,7 +76,7 @@ public:
     AP_Float mag_error;   // in degrees
     AP_Vector3f mag_mot;  // in mag units per amp
     AP_Vector3f mag_ofs;  // in mag units
-    AP_Float servo_rate;  // servo speed in degrees/second
+    AP_Float servo_speed; // servo speed in seconds
 
     AP_Float sonar_glitch;// probablility between 0-1 that any given sonar sample will read as max distance
     AP_Float sonar_noise; // in metres