public void update(ref double[] servos, ArdupilotMega.GCSViews.Simulation.FGNetFDM fdm)
{
for (int i = 0; i < servos.Length; i++)
{
var servo = servos[(int)self.motors[i].servo - 1];
if (servo <= 0.0)
{
motor_speed[i] = 0;
}
else
{
motor_speed[i] = scale_rc(i, (float)servo, 0.0f, 1.0f);
//servos[i] = motor_speed[i];
}
}
double[] m = motor_speed;
//# how much time has passed?
DateTime t = DateTime.Now;
TimeSpan delta_time = t - last_time; // 0.02
last_time = t;
if (delta_time.TotalMilliseconds > 100) // somethings wrong / debug
{
delta_time = new TimeSpan(0, 0, 0, 0, 20);
}
// rotational acceleration, in degrees/s/s, in body frame
Vector3 rot_accel = new Vector3(0, 0, 0);
double thrust = 0.0;
foreach (var i in range((self.motors.Length)))
{
rot_accel.x += -radians(5000.0) * sin(radians(self.motors[i].angle)) * m[i];
rot_accel.y += radians(5000.0) * cos(radians(self.motors[i].angle)) * m[i];
if (self.motors[i].clockwise)
{
rot_accel.z -= m[i] * radians(400.0);
}
else
{
rot_accel.z += m[i] * radians(400.0);
}
thrust += m[i] * self.thrust_scale; // newtons
}
// rotational air resistance
rot_accel.x -= self.gyro.x * radians(5000.0) / self.terminal_rotation_rate;
rot_accel.y -= self.gyro.y * radians(5000.0) / self.terminal_rotation_rate;
rot_accel.z -= self.gyro.z * radians(400.0) / self.terminal_rotation_rate;
// Console.WriteLine("rot_accel " + rot_accel.ToString());
// update rotational rates in body frame
self.gyro += rot_accel * delta_time.TotalSeconds;
// Console.WriteLine("gyro " + gyro.ToString());
// update attitude
self.dcm.rotate(self.gyro * delta_time.TotalSeconds);
self.dcm.normalize();
// air resistance
Vector3 air_resistance = -self.velocity * (self.gravity / self.terminal_velocity);
accel_body = new Vector3(0, 0, -thrust / self.mass);
Vector3 accel_earth = self.dcm * accel_body;
accel_earth += new Vector3(0, 0, self.gravity);
accel_earth += air_resistance;
// add in some wind (turn force into accel by dividing by mass).
// accel_earth += self.wind.drag(self.velocity) / self.mass;
// 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 (self.on_ground() && 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
self.accel_body = self.dcm.transposed() * (accel_earth + new Vector3(0, 0, -self.gravity));
// new velocity vector
self.velocity += accel_earth * delta_time.TotalSeconds;
if (double.IsNaN(velocity.x) || double.IsNaN(velocity.y) || double.IsNaN(velocity.z))
{
velocity = new Vector3();
}
// new position vector
old_position = self.position.copy();
self.position += self.velocity * delta_time.TotalSeconds;
if (home_latitude == 0)
{
home_latitude = fdm.latitude * rad2deg;
home_longitude = fdm.longitude * rad2deg;
home_altitude = altitude;
}
// constrain height to the ground
if (self.on_ground())
{
if (!self.on_ground(old_position))
{
Console.WriteLine("Hit ground at {0} m/s", (self.velocity.z));
}
self.velocity = new Vector3(0, 0, 0);
// zero roll/pitch, but keep yaw
double r = 0;
double p = 0;
double y = 0;
self.dcm.to_euler(ref r, ref p, ref y);
self.dcm.from_euler(0, 0, y);
self.position = new Vector3(self.position.x, self.position.y,
-(self.ground_level + self.frame_height - self.home_altitude));
}
// update lat/lon/altitude
self.update_position(delta_time.TotalSeconds);
}
