コード例 #1
0
 public Ahrs(
     IObservable <AngularVelocity3> angularVelocity,
     IObservable <GForce3> acceleration,
     IObservable <MagneticFluxDensity3> magnetism,
     MagneticFluxDensity3 magnetismMinCalibration,
     MagneticFluxDensity3 magnetismMaxCalibration)
 {
     _angularVelocity         = angularVelocity;
     _acceleration            = acceleration;
     _magnetism               = magnetism;
     _magnetismMinCalibration = magnetismMinCalibration;
     _magnetismMaxCalibration = magnetismMaxCalibration;
 }
コード例 #2
0
        public static WindowsPilot Create(
            IAircraft aircraft,
            IGyroscope3 gyroscope,
            IAccelerometer3 accelerometer,
            IMagnetometer3 magnetometer,
            IFlightController controller,
            double thrustMax,
            double angularPositionPitchMax,
            double angularPositionRollMax,
            double angularVelocityYawMax)
        {
            var angularVelocityNoisy = new Subject <AngularVelocity3>();
            var accelerationNoisy    = new Subject <GForce3>();
            var magnetismNoisy       = new Subject <MagneticFluxDensity3>();

            IObservable <AngularVelocity3> angularVelocityOptimal =
                angularVelocityNoisy
                .Filter(
                    estimatedMeasurementNoiseCovariance: 0.0000025d,
                    estimatedProcessNoiseCovariance: 0d,
                    controlWeight: 0.5d * OutputFrequency / gyroscope.Frequency,
                    initialEstimate: 0d,
                    initialErrorCovariance: 1d)
                .Multiply(1.08d /*calibration*/);

            IObservable <GForce3> accelerationOptimal =
                accelerationNoisy
                .Filter(
                    estimatedMeasurementNoiseCovariance: 0.025d,
                    estimatedProcessNoiseCovariance: 0d,
                    controlWeight: 0.5d * OutputFrequency / accelerometer.Frequency,
                    initialEstimate: 0d,
                    initialErrorCovariance: 1d);

            IObservable <MagneticFluxDensity3> magnetismOptimal =
                magnetismNoisy
                .Filter(
                    estimatedMeasurementNoiseCovariance: 0.05d,
                    estimatedProcessNoiseCovariance: 0d,
                    controlWeight: 0.5d * OutputFrequency / magnetometer.Frequency,
                    initialEstimate: 0d,
                    initialErrorCovariance: 1d);

            var magnetismMinCalibration = new MagneticFluxDensity3(
                x: -0.273980716005263d,
                y: -0.245318477916743d,
                z: -0.237557110710227d);

            var magnetismMaxCalibration = new MagneticFluxDensity3(
                x: 0.148042531751317d,
                y: 0.177504483195312d,
                z: 0.155796981828076d);

            var ahrs = new Ahrs(angularVelocityOptimal, accelerationOptimal, magnetismOptimal, magnetismMinCalibration, magnetismMaxCalibration);

            var angularVelocityInterval = TimeSpan.FromTicks(Convert.ToInt64(TimeSpan.TicksPerSecond / gyroscope.Frequency));
            var accelerationInterval    = TimeSpan.FromTicks(Convert.ToInt64(TimeSpan.TicksPerSecond / accelerometer.Frequency));
            var magnetismInterval       = TimeSpan.FromTicks(Convert.ToInt64(TimeSpan.TicksPerSecond / magnetometer.Frequency));

            var yawTimer    = new Subject <object>();
            var yawInterval = TimeSpan.FromTicks(Convert.ToInt64(TimeSpan.TicksPerSecond / YawFrequency));

            var outputTimer    = new Subject <object>();
            var outputInterval = TimeSpan.FromTicks(Convert.ToInt64(TimeSpan.TicksPerSecond / OutputFrequency));

            SyncLoop.ProduceAsync(
                angularVelocityInterval, a => gyroscope.Read(), angularVelocityNoisy,
                accelerationInterval, a => accelerometer.Read(), accelerationNoisy,
                magnetismInterval, a => magnetometer.Read(), magnetismNoisy,
                yawInterval, a => null, yawTimer,
                outputInterval, a => null, outputTimer,
                WorkItemPriority.High).AsTask();

            IObservable <double> thrust =
                controller.Thrust.Select(value => value * thrustMax);

            IObservable <double> angularPositionPitchSetpoint =
                controller.Pitch.Select(value => value * angularPositionPitchMax);

            IObservable <double> angularPositionRollSetpoint =
                controller.Roll.Select(value => value * angularPositionRollMax);

            IObservable <double> angularPositionYawSetpoint =
                Extensions.ObservableExtensions
                .CombineLatest(
                    controller.Yaw,
                    yawTimer.TimeInterval(),
                    (yaw, timer, yawChanged, timerChanged) =>
                    new { Yaw = yaw, YawChanged = yawChanged, TimerElapsed = timerChanged, TimerInterval = timer.Interval })
                .Scan(
                    default(double),
                    (accu, t) =>
            {
                double yaw = accu;

                if (t.TimerElapsed)
                {
                    double dt = t.TimerInterval.TotalSeconds;
                    yaw      += t.Yaw * angularVelocityYawMax * dt;

                    if (yaw > AngleConvert.ToRadians(180d))
                    {
                        yaw -= AngleConvert.ToRadians(360d);
                    }
                    else if (yaw <= AngleConvert.ToRadians(-180d))
                    {
                        yaw += AngleConvert.ToRadians(360d);
                    }
                }

                return(yaw);
            });

            IObservable <EulerAngles> angularPositionSetpoint =
                Observable.CombineLatest(
                    angularPositionPitchSetpoint,
                    angularPositionRollSetpoint,
                    angularPositionYawSetpoint,
                    (pitch, roll, yaw) =>
                    new EulerAngles(pitch, roll, yaw));

            var pilot = new WindowsPilot(
                aircraft,
                thrust,
                angularPositionSetpoint,
                ahrs.AngularVelocityCalibrated,
                ahrs.AngularPosition);

            outputTimer.Subscribe(state => pilot.WriteOutput());
            return(pilot);
        }
コード例 #3
0
 public static IObservable <Vector3> ToDirection(this IObservable <MagneticFluxDensity3> source, MagneticFluxDensity3 min, MagneticFluxDensity3 max)
 {
     return(source
            .Select(magnetism =>
     {
         return new Vector3(
             x: 2d * (magnetism.X - min.X) / (max.X - min.X) - 1d,
             y: 2d * (magnetism.Y - min.Y) / (max.Y - min.Y) - 1d,
             z: 2d * (magnetism.Z - min.Z) / (max.Z - min.Z) - 1d).Normalize();
     }));
 }