Ejemplo n.º 1
0
        public void Configure(Mode mode = Mode.Ndof, AccRange ar = AccRange._16g, GyroRange gr = GyroRange._2000dps)
        {
            bridge.sendCommand(new byte[] { (byte)SENSOR_FUSION, MODE, (byte)((byte)mode + 1), (byte)((byte)ar | (((byte)gr + 1) << 4)) });

            var accelerometer = bridge.GetModule <IAccelerometerBosch>();
            var gyro          = bridge.GetModule <IGyroBmi160>();
            var magnetometer  = bridge.GetModule <IMagnetometerBmm150>();

            this.mode = mode;
            switch (mode)
            {
            case Mode.Ndof:
                accelerometer.Configure(odr: 100f, range: AccelerometerBosch.RANGES[(int)ar]);
                gyro.Configure(odr: Sensor.GyroBmi160.OutputDataRate._100Hz, range: (Sensor.GyroBmi160.DataRange)gr);
                magnetometer.Configure(odr: Sensor.MagnetometerBmm150.OutputDataRate._25Hz);
                break;

            case Mode.ImuPlus:
                accelerometer.Configure(odr: 100f, range: AccelerometerBosch.RANGES[(int)ar]);
                gyro.Configure(odr: Sensor.GyroBmi160.OutputDataRate._100Hz, range: (Sensor.GyroBmi160.DataRange)gr);
                break;

            case Mode.Compass:
                accelerometer.Configure(odr: 25f, range: AccelerometerBosch.RANGES[(int)ar]);
                magnetometer.Configure(odr: Sensor.MagnetometerBmm150.OutputDataRate._25Hz);
                break;

            case Mode.M4g:
                accelerometer.Configure(odr: 50f, range: AccelerometerBosch.RANGES[(int)ar]);
                magnetometer.Configure(odr: Sensor.MagnetometerBmm150.OutputDataRate._25Hz);
                break;
            }
        }
        public void Configure(Mode mode         = Mode.Ndof, AccRange ar   = AccRange._16g, GyroRange gr = GyroRange._2000dps,
                              object[] accExtra = null, object[] gyroExtra = null)
        {
            bridge.sendCommand(new byte[] { (byte)SENSOR_FUSION, MODE, (byte)((byte)mode + 1), (byte)((byte)ar | (((byte)gr + 1) << 4)) });

            var accelerometer = bridge.GetModule <IAccelerometerBosch>();
            var gyro          = bridge.GetModule <IGyroBmi160>();
            var magnetometer  = bridge.GetModule <IMagnetometerBmm150>();

            void configAcc(Sensor.AccelerometerBmi160.OutputDataRate odr)
            {
                if (accelerometer is IAccelerometerBmi160)
                {
                    var casted = accelerometer as IAccelerometerBmi160;

                    Action <Sensor.AccelerometerBmi160.FilterMode> configure(Sensor.AccelerometerBosch.DataRange range) =>
                    filter => casted.Configure(odr: odr, range: range, filter: filter);

                    var partial = configure((Sensor.AccelerometerBosch.DataRange)ar);

                    if (accExtra != null)
                    {
                        foreach (Object it in accExtra)
                        {
                            if (it is Sensor.AccelerometerBmi160.FilterMode)
                            {
                                partial((Sensor.AccelerometerBmi160.FilterMode)it);
                                break;
                            }
                        }
                    }
                    else
                    {
                        partial(Sensor.AccelerometerBmi160.FilterMode.Normal);
                    }
                }
            }

            void configGyro()
            {
                Action <Sensor.GyroBmi160.FilterMode> configure(Sensor.GyroBmi160.DataRange range) =>
                filter => gyro.Configure(odr: Sensor.GyroBmi160.OutputDataRate._100Hz, range: range, filter: filter);

                var partial = configure((Sensor.GyroBmi160.DataRange)gr);

                if (gyroExtra != null)
                {
                    foreach (Object it in gyroExtra)
                    {
                        if (it is Sensor.GyroBmi160.FilterMode)
                        {
                            partial((Sensor.GyroBmi160.FilterMode)it);
                            break;
                        }
                    }
                }
                else
                {
                    partial(Sensor.GyroBmi160.FilterMode.Normal);
                }
            }

            this.mode = mode;
            switch (mode)
            {
            case Mode.Ndof:
                configAcc(Sensor.AccelerometerBmi160.OutputDataRate._100Hz);
                configGyro();
                magnetometer.Configure(odr: Sensor.MagnetometerBmm150.OutputDataRate._25Hz);
                break;

            case Mode.ImuPlus:
                configAcc(Sensor.AccelerometerBmi160.OutputDataRate._100Hz);
                configGyro();
                break;

            case Mode.Compass:
                configAcc(Sensor.AccelerometerBmi160.OutputDataRate._25Hz);
                magnetometer.Configure(odr: Sensor.MagnetometerBmm150.OutputDataRate._25Hz);
                break;

            case Mode.M4g:
                configAcc(Sensor.AccelerometerBmi160.OutputDataRate._50Hz);
                magnetometer.Configure(odr: Sensor.MagnetometerBmm150.OutputDataRate._25Hz);
                break;
            }
        }