using System; using System.Collections.Generic; using System.Linq; using System.Text; using Sharp3D.Math.Core; namespace ArdupilotMega.HIL { class FlashQuad { public ahrsc ahrs = new ahrsc() ;// :AHRS; public Vector3D loc = new Vector3D() ;// :Location; public param g = new param() ;// :Parameters; //public var apm_rc :APM_RC; //public var motor_filter_0 :AverageFilter; //public var motor_filter_1 :AverageFilter; //public var motor_filter_2 :AverageFilter; //public var motor_filter_3 :AverageFilter; public Vector3D drag = new Vector3D();// :Vector3D; // public Vector3D airspeed = new Vector3D();// :Vector3D; // //public var thrust :Vector3D; // public Vector3D position = new Vector3D();// :Vector3D; // public Vector3D velocity = new Vector3D();// :Vector3D; //public var velocity_old :Vector3D; //public var wind :Point; // public Vector3D rot_accel = new Vector3D();// :Vector3D; // public Vector3D angle3D = new Vector3D();// :Vector3D; // //public var windGenerator :Wind; // public double gravity = 980.5; public double thrust_scale = 0.4; public double throttle = 500; public double rotation_bias = 1; private double _jump_z = 0; private Vector3D v3 = new Vector3D(); public class ahrsc { public Matrix3D dcm = new Matrix3D(Matrix3D.Identity); public Vector3D gyro = new Vector3D(); public Vector3D omega = new Vector3D(); public Vector3D accel = new Vector3D(); public Vector3D rotation_speed = new Vector3D(); public double roll_sensor; public double pitch_sensor; public double yaw_sensor; } public class param { // --------------------------------------------- // Sim Details controls // --------------------------------------------- public int sim_iterations = 99999; public int sim_speed = 1; public double windSpeedMin = 150; public double windSpeedMax = 200; public double windPeriod = 30000; public double windDir = 45; public double airDensity = 1.184; //public var crossSection :Number = 0.015; public double crossSection = 0.012; public double dragCE = 0.20; public double speed_filter_size = 2; public double motor_kv = 1000; public double moment = 3; public double mass = 500; public int esc_delay = 12; // ----------------------------------------- // Inertial control // ----------------------------------------- public double speed_correction_z = 0.0350; public double xy_speed_correction = 0.030; public double xy_offset_correction = 0.00001; public double xy_pos_correction = 0.08; public double z_offset_correction = 0.00004; public double z_pos_correction = 0.2; public double accel_bias_x = .9; public double accel_bias_z = .9; public double accel_bias_y = .9; } double scale_rc(int sn, float servo, float min, float max) { float min_pwm = 1000; float max_pwm = 2000; //'''scale a PWM value''' # default to servo range of 1000 to 2000 if (MainV2.comPort.param.Count > 0) { min_pwm = int.Parse(MainV2.comPort.param["RC3_MIN"].ToString()); max_pwm = int.Parse(MainV2.comPort.param["RC3_MAX"].ToString()); } float p = (servo - min_pwm) / (max_pwm - min_pwm); float v = min + p * (max - min); if (v < min) v = min; if (v > max) v = max; return v * 1000; } public void update(ref double[] motor_output, double dt) { var _thrust = 0.0; rot_accel = new Vector3D(0,0,0); angle3D.X = angle3D.Y = 0; angle3D.Z = 1; // wind = windGenerator.read(); // ESC's moving average filter // var motor_output:Array = new Array(4); // motor_output[0] = motor_filter_0.apply(apm_rc.get_motor_output(0)); // motor_output[1] = motor_filter_1.apply(apm_rc.get_motor_output(1)); // motor_output[2] = motor_filter_2.apply(apm_rc.get_motor_output(2)); // motor_output[3] = motor_filter_3.apply(apm_rc.get_motor_output(3)); for (int i = 0; i < motor_output.Length; i++) { if (motor_output[i] <= 0.0) { motor_output[i] = 0; } else { motor_output[i] = scale_rc(i, (float)motor_output[i], 0.0f, 1.0f); //servos[i] = motor_speed[i]; } } /* 2 1 0 3 */ // setup motor rotations rot_accel.X -= g.motor_kv * motor_output[0]; // roll rot_accel.X += g.motor_kv * motor_output[1]; rot_accel.Y -= g.motor_kv * motor_output[3]; rot_accel.Y += g.motor_kv * motor_output[2]; rot_accel.Z += g.motor_kv * motor_output[0] * .08; // YAW rot_accel.Z += g.motor_kv * motor_output[1] * .08; rot_accel.Z -= g.motor_kv * motor_output[2] * .08; rot_accel.Z -= g.motor_kv * motor_output[3] * .08; rot_accel.X /= g.moment; rot_accel.Y /= g.moment; rot_accel.Z /= g.moment; //# rotational air resistance // Gyro is the rotation speed in deg/s // update rotational rates in body frame ahrs.gyro.X += rot_accel.X * dt; ahrs.gyro.Y += rot_accel.Y * dt; ahrs.gyro.Z += rot_accel.Z * dt; //ahrs.gyro.z += 200; ahrs.gyro.Z *= .995;// some drag // ahrs.dcm = Matrix3D.Identity; // move earth frame to body frame Vector3D tmp = Matrix3D.Transform(ahrs.dcm,ahrs.gyro) ;//ahrs.dcm.transformVector(ahrs.gyro); // update attitude: ahrs.dcm += new Matrix3D(new Vector3D((tmp.X/100) * dt,0,0),new Vector3D(0,(tmp.Y/100) * dt,0),new Vector3D(0,0,(tmp.Z/100) * dt)); //ahrs.dcm.appendRotation((tmp.X/100) * dt, Vector3D.X_AXIS); // ROLL //ahrs.dcm.appendRotation((tmp.Y/100) * dt, Vector3D.Y_AXIS); // PITCH //ahrs.dcm.appendRotation((tmp.Z/100) * dt, Vector3D.Z_AXIS); // YAW // ------------------------------------ // calc thrust // ------------------------------------ //get_motor_output returns 0 : 1000 _thrust += motor_output[0] * thrust_scale; _thrust += motor_output[1] * thrust_scale; _thrust += motor_output[2] * thrust_scale; _thrust += motor_output[3] * thrust_scale; Vector3D accel_body = new Vector3D(0, 0, (_thrust * -.9) / g.mass); //var accel_body:Vector3D = new Vector3D(0, 0, 0); Vector3D accel_earth = Matrix3D.Transform(ahrs.dcm,(accel_body)); angle3D = Matrix3D.Transform(ahrs.dcm,(angle3D)); //trace(ahrs.gyro.y, accel_earth.x); //trace(ahrs.gyro.x, ahrs.gyro.y, ahrs.gyro.z); // ------------------------------------ // calc copter velocity // ------------------------------------ // calc Drag drag.X = .5 * g.airDensity * airspeed.X * airspeed.X * g.dragCE * g.crossSection; drag.Y = .5 * g.airDensity * airspeed.Y * airspeed.Y * g.dragCE * g.crossSection; drag.Z = .5 * g.airDensity * airspeed.Z * airspeed.Z * g.dragCE * g.crossSection; ///* // this calulation includes wind if(airspeed.X >= 0) accel_earth.X -= drag.X; else accel_earth.X += drag.X; // Add in Drag if(airspeed.Y >= 0) accel_earth.Y -= drag.Y; else accel_earth.Y += drag.Y; if(airspeed.Z <= 0) accel_earth.Z -= drag.Z; else accel_earth.Z += drag.Z; //*/ // hacked vert disturbance accel_earth.Z += _jump_z * dt; _jump_z *= .999; // Add in Gravity accel_earth.Z += gravity; if(accel_earth.Z < 0) accel_earth.Z *=.9; if(position.Z <=.11 && accel_earth.Z > 0){ accel_earth.Z = 0; } velocity.X += (accel_earth.X * dt); // + : Forward (North) velocity.Y += (accel_earth.Y * dt); // + : Right (East) velocity.Z -= (accel_earth.Z * dt); // + : Up //trace(Math.floor(velocity.x),Math.floor(velocity.y),Math.floor(velocity.z)); // ------------------------------------ // calc inertia // ------------------------------------ // work out acceleration as seen by the accelerometers. It sees the kinematic // acceleration (ie. real movement), plus gravity Matrix3D dcm_t = ahrs.dcm.Clone(); dcm_t.Transpose(); double t = accel_earth.Z; accel_earth.Z -= gravity; ahrs.accel = Matrix3D.Transform(dcm_t,(accel_earth)); ahrs.accel *= 0.01; //ahrs.accel = accel_earth.clone(); ahrs.accel.X *= g.accel_bias_x; ahrs.accel.Y *= g.accel_bias_y; ahrs.accel.Z *= g.accel_bias_z; // ------------------------------------ // calc Position // ------------------------------------ position.Y += velocity.X * dt; position.X += velocity.Y * dt; position.Z += velocity.Z * dt; position.Z = Math.Min(position.Z, 4000); // XXX Force us to 3m above ground //position.z = 300; airspeed.X = (velocity.X);// - wind.x); airspeed.Y = (velocity.Y);// - wind.y); airspeed.Z = velocity.Z; // Altitude // -------- if(position.Z <=.1){ position.Z = .1; velocity.X = 0; velocity.Y = 0; velocity.Z = 0; airspeed.X = 0; airspeed.Y = 0; airspeed.Z = 0; //ahrs.init(); } // get omega - the simulated Gyro output ahrs.omega.X = radiansx100(ahrs.gyro.X); ahrs.omega.Y = radiansx100(ahrs.gyro.Y); ahrs.omega.Z = radiansx100(ahrs.gyro.Z); // get the Eulers output v3 = new Vector3D(Math.Atan2(-ahrs.dcm.M23, ahrs.dcm.M22), Math.Asin(ahrs.dcm.M21), Math.Atan2(-ahrs.dcm.M31, ahrs.dcm.M11));// ahrs.dcm.decompose(); ahrs.roll_sensor = Math.Floor(degrees(v3.X) * 100); ahrs.pitch_sensor = Math.Floor(degrees(v3.Y) * 100); ahrs.yaw_sensor = Math.Floor(degrees(v3.Z) * 100); // store the position for the GPS object loc.X = position.X; loc.Y = position.Y; loc.Z = position.Z; } public double constrain(double val, double min, double max) { val = Math.Max(val, min); val = Math.Min(val, max); return val; } public double wrap_180(int error) { if (error > 18000) error -= 36000; if (error < -18000) error += 36000; return error; } public int wrap_360(int error) { if (error > 36000) error -= 36000; if (error < 0) error += 36000; return error; } public double radiansx100(double n) { return 0.000174532925 * n; } public double degreesx100(double r) { return r * 5729.57795; } public double degrees(double r) { return r * 57.2957795; } public double radians(double n) { return 0.0174532925 * n; } } }