示例#1
0
        public static IObservable <Quaternion> ToRotation(this IObservable <AngularVelocity3> source)
        {
            return(source
                   .TimeInterval()
                   .Select(angularVelocityTimed =>
            {
                AngularVelocity3 angularVelocity = angularVelocityTimed.Value;
                double dt = angularVelocityTimed.Interval.TotalSeconds;

                Vector3 w = angularVelocity.Normalize();
                double theta = AngleConvert.ToRadians(angularVelocity.Magnitude) * dt;

                return Quaternion.FromAxisAngle(w, -theta);
            }));
        }
示例#2
0
        public void Run()
        {
            var speedController = new SpeedController(Pca9685PwmController.Default);
            var aircraft        = new Quadcopter(speedController);

            WindowsPilot.Create(
                aircraft,
                L3gd20hGyroscope.Default,
                Lsm303dAccelerometer.Default,
                Lsm303dMagnetometer.Default,
                FlightController.FromGamepad(LogitechF710Gamepad.Default),
                thrustMax: 0.8d,
                angularPositionPitchMax: AngleConvert.ToRadians(15d),
                angularPositionRollMax: AngleConvert.ToRadians(15d),
                angularVelocityYawMax: AngleConvert.ToRadians(90 /*dps*/));
        }
示例#3
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);
        }