SITL: moved to common code for attitude/pos update

libraries/SITL/SIM_Aircraft.cpp

@@ -322,4 +322,50 @@ void Aircraft::set_speedup(float speedup)
     setup_frame_time(rate_hz, speedup);
 }
 
+/*
+  update the simulation attitude and relative position
+ */
+void Aircraft::update_dynamics(const Vector3f &rot_accel)
+{
+    float delta_time = frame_time_us * 1.0e-6f;
+
+    // update rotational rates in body frame
+    gyro += rot_accel * delta_time;
+
+    // update attitude
+    dcm.rotate(gyro * delta_time);
+    dcm.normalize();
+
+    Vector3f accel_earth = dcm * accel_body;
+    accel_earth += Vector3f(0, 0, GRAVITY_MSS);
+
+    // if we're on the ground, then our vertical acceleration is limited
+    // to zero. This effectively adds the force of the ground on the aircraft
+    if (on_ground(position) && accel_earth.z > 0) {
+        accel_earth.z = 0;
+    }
+
+    // work out acceleration as seen by the accelerometers. It sees the kinematic
+    // acceleration (ie. real movement), plus gravity
+    accel_body = dcm.transposed() * (accel_earth + Vector3f(0, 0, -GRAVITY_MSS));
+
+    // new velocity vector
+    velocity_ef += accel_earth * delta_time;
+
+    // new position vector
+    Vector3f old_position = position;
+    position += velocity_ef * delta_time;
+
+    // assume zero wind for now
+    airspeed = velocity_ef.length();
+
+    // constrain height to the ground
+    if (on_ground(position)) {
+        if (!on_ground(old_position)) {
+            printf("Hit ground at %f m/s\n", velocity_ef.z);
+            position.z = -(ground_level + frame_height - home.alt*0.01f);
+        }
+    }
+}
+    
 } // namespace SITL

libraries/SITL/SIM_Aircraft.h

@@ -152,6 +152,9 @@ protected:
     /* return wall clock time in microseconds since 1970 */
     uint64_t get_wall_time_us(void) const;
 
+    // update attitude and relative position
+    void update_dynamics(const Vector3f &rot_accel);
+
 private:
     uint64_t last_time_us = 0;
     uint32_t frame_counter = 0;

libraries/SITL/SIM_Balloon.cpp

@@ -44,18 +44,9 @@ void Balloon::update(const struct sitl_input &input)
         burst = true;
     }
 
-    float delta_time = frame_time_us * 1.0e-6f;
-
     // rotational air resistance
     Vector3f rot_accel = -gyro * radians(400) / terminal_rotation_rate;
 
-    // update rotational rates in body frame
-    gyro += rot_accel * delta_time;
-
-    // update attitude
-    dcm.rotate(gyro * delta_time);
-    dcm.normalize();
-
     // air resistance
     Vector3f air_resistance = -velocity_ef * (GRAVITY_MSS/terminal_velocity);
 
@@ -66,54 +57,15 @@ void Balloon::update(const struct sitl_input &input)
     }
 
     accel_body = Vector3f(0, 0, -lift_accel);
-    Vector3f accel_earth = dcm * accel_body;
+    accel_body += dcm * air_resistance;
-    accel_earth += Vector3f(0, 0, GRAVITY_MSS);
+    
-    accel_earth += air_resistance;
+    update_dynamics(rot_accel);
-
-    // if we're on the ground, then our vertical acceleration is limited
-    // to zero. This effectively adds the force of the ground on the aircraft
-    if (on_ground(position) && accel_earth.z > 0) {
-        accel_earth.z = 0;
-    }
-
-    // work out acceleration as seen by the accelerometers. It sees the kinematic
-    // acceleration (ie. real movement), plus gravity
-    accel_body = dcm.transposed() * (accel_earth + Vector3f(0, 0, -GRAVITY_MSS));
-
-    // add some noise
-    add_noise(velocity_ef.length() / terminal_velocity);
-
-    // new velocity vector
-    velocity_ef += accel_earth * delta_time;
-
-    // new position vector
-    Vector3f old_position = position;
-    position += velocity_ef * delta_time;
 
     if (position.z < -burst_altitude) {
         ::printf("Balloon burst at %.1f\n", -position.z);
         burst = true;
     }
-
+    
-    // assume zero wind for now
-    airspeed = velocity_ef.length();
-
-    // constrain height to the ground
-    if (on_ground(position)) {
-        if (!on_ground(old_position)) {
-            printf("Hit ground at %f m/s\n", velocity_ef.z);
-
-            velocity_ef.zero();
-
-            // zero roll/pitch, but keep yaw
-            float r, p, y;
-            dcm.to_euler(&r, &p, &y);
-            dcm.from_euler(0, 0, y);
-
-            position.z = -(ground_level + frame_height - home.alt*0.01f);
-        }
-    }
-
     // update lat/lon/altitude
     update_position();
 }

libraries/SITL/SIM_Helicopter.cpp

@@ -54,9 +54,6 @@ Helicopter::Helicopter(const char *home_str, const char *frame_str) :
  */
 void Helicopter::update(const struct sitl_input &input)
 {
-    // how much time has passed?
-    float delta_time = frame_time_us * 1.0e-6f;
-
     float rsc = (input.servos[7]-1000) / 1000.0f;
     // ignition only for gas helis
     bool ignition_enabled = gas_heli?(input.servos[5] > 1500):true;
@@ -147,13 +144,6 @@ void Helicopter::update(const struct sitl_input &input)
     // torque effect on tail
     rot_accel.z += torque_effect_accel;
 
-    // update rotational rates in body frame
-    gyro += rot_accel * delta_time;
-
-    // update attitude
-    dcm.rotate(gyro * delta_time);
-    dcm.normalize();
-
     // air resistance
     Vector3f air_resistance = -velocity_ef * (GRAVITY_MSS/terminal_velocity);
 
@@ -161,47 +151,18 @@ void Helicopter::update(const struct sitl_input &input)
     thrust *= thrust_scale;
 
     accel_body = Vector3f(lateral_x_thrust, lateral_y_thrust, -thrust / mass);
-    Vector3f accel_earth = dcm * accel_body;
+    accel_body += dcm * air_resistance;
-    accel_earth += Vector3f(0, 0, GRAVITY_MSS);
+    
-    accel_earth += air_resistance;
+    update_dynamics(rot_accel);
-
+    
-    // if we're on the ground, then our vertical acceleration is limited
-    // to zero. This effectively adds the force of the ground on the aircraft
-    if (on_ground(position) && accel_earth.z > 0) {
-        accel_earth.z = 0;
-    }
-
-    // work out acceleration as seen by the accelerometers. It sees the kinematic
-    // acceleration (ie. real movement), plus gravity
-    accel_body = dcm.transposed() * (accel_earth + Vector3f(0, 0, -GRAVITY_MSS));
-
-    // add some noise
-    add_noise(thrust / thrust_scale);
-
-    // new velocity vector
-    velocity_ef += accel_earth * delta_time;
-
-    // new position vector
-    Vector3f old_position = position;
-    position += velocity_ef * delta_time;
-
-    // assume zero wind for now
-    airspeed = velocity_ef.length();
-
     // constrain height to the ground
     if (on_ground(position)) {
-        if (!on_ground(old_position)) {
+        // zero roll/pitch, but keep yaw
-            printf("Hit ground at %f m/s\n", velocity_ef.z);
+        float r, p, y;
-
+        dcm.to_euler(&r, &p, &y);
-            velocity_ef.zero();
+        dcm.from_euler(0, 0, y);
-
+        
-            // zero roll/pitch, but keep yaw
+        position.z = -(ground_level + frame_height - home.alt*0.01f);
-            float r, p, y;
-            dcm.to_euler(&r, &p, &y);
-            dcm.from_euler(0, 0, y);
-
-            position.z = -(ground_level + frame_height - home.alt*0.01f);
-        }
     }
 
     // update lat/lon/altitude

libraries/SITL/SIM_Multicopter.cpp

@@ -197,58 +197,21 @@ void MultiCopter::calculate_forces(const struct sitl_input &input, Vector3f &rot
 void MultiCopter::update(const struct sitl_input &input)
 {
     // how much time has passed?
-    float delta_time = frame_time_us * 1.0e-6f;
     Vector3f rot_accel;
 
     calculate_forces(input, rot_accel, accel_body);
 
-    // update rotational rates in body frame
+    update_dynamics(rot_accel);
-    gyro += rot_accel * delta_time;
 
-    // update attitude
-    dcm.rotate(gyro * delta_time);
-    dcm.normalize();
-
-    Vector3f accel_earth = dcm * accel_body;
-
-    accel_earth += Vector3f(0, 0, GRAVITY_MSS);
-
-    // if we're on the ground, then our vertical acceleration is limited
-    // to zero. This effectively adds the force of the ground on the aircraft
-    if (on_ground(position) && accel_earth.z > 0) {
-        accel_earth.z = 0;
-    }
-
-    // work out acceleration as seen by the accelerometers. It sees the kinematic
-    // acceleration (ie. real movement), plus gravity
-    accel_body = dcm.transposed() * (accel_earth + Vector3f(0, 0, -GRAVITY_MSS));
-
-    // new velocity vector
-    velocity_ef += accel_earth * delta_time;
-
-    // new position vector
-    Vector3f old_position = position;
-    position += velocity_ef * delta_time;
-
-    // assume zero wind for now
-    airspeed = velocity_ef.length();
-
-    // constrain height to the ground
     if (on_ground(position)) {
-        if (!on_ground(old_position)) {
+        // zero roll/pitch, but keep yaw
-            printf("Hit ground at %f m/s\n", velocity_ef.z);
+        float r, p, y;
+        dcm.to_euler(&r, &p, &y);
+        dcm.from_euler(0, 0, y);
 
-            velocity_ef.zero();
+        position.z = -(ground_level + frame_height - home.alt*0.01f);
-
-            // zero roll/pitch, but keep yaw
-            float r, p, y;
-            dcm.to_euler(&r, &p, &y);
-            dcm.from_euler(0, 0, y);
-
-            position.z = -(ground_level + frame_height - home.alt*0.01f);
-        }
     }
-
+    
     // update lat/lon/altitude
     update_position();
 }

libraries/SITL/SIM_Plane.cpp

@@ -153,50 +153,12 @@ void Plane::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel
  */
 void Plane::update(const struct sitl_input &input)
 {
-    float delta_time = frame_time_us * 1.0e-6f;
-
     Vector3f rot_accel;
 
     calculate_forces(input, rot_accel, accel_body);
     
-    // update rotational rates in body frame
+    update_dynamics(rot_accel);
-    gyro += rot_accel * delta_time;
+    
-
-    // update attitude
-    dcm.rotate(gyro * delta_time);
-    dcm.normalize();
-
-    Vector3f accel_earth = dcm * accel_body;
-    accel_earth += Vector3f(0, 0, GRAVITY_MSS);
-
-    // if we're on the ground, then our vertical acceleration is limited
-    // to zero. This effectively adds the force of the ground on the aircraft
-    if (on_ground(position) && accel_earth.z > 0) {
-        accel_earth.z = 0;
-    }
-
-    // work out acceleration as seen by the accelerometers. It sees the kinematic
-    // acceleration (ie. real movement), plus gravity
-    accel_body = dcm.transposed() * (accel_earth + Vector3f(0, 0, -GRAVITY_MSS));
-
-    // new velocity vector
-    velocity_ef += accel_earth * delta_time;
-
-    // new position vector
-    Vector3f old_position = position;
-    position += velocity_ef * delta_time;
-
-    // assume zero wind for now
-    airspeed = velocity_ef.length();
-
-    // constrain height to the ground
-    if (on_ground(position)) {
-        if (!on_ground(old_position)) {
-            printf("Hit ground at %f m/s\n", velocity_ef.z);
-            position.z = -(ground_level + frame_height - home.alt*0.01f);
-        }
-    }
-
     // update lat/lon/altitude
     update_position();
 }