| private Read ( int byteCount, byte &data ) : bool | ||
| byteCount | int | |
| data | byte | |
| return | bool | |
internal short[] ReadMagData() //TODO : When using the INT pin, this does not work at all.
{
byte[] rawData = { 0, 0, 0, 0, 0, 0 }; // x/y/z gyro register data stored here
//WriteByte(_ak8975A, AK8975A_CNTL, 0x01); // toggle enable data read from magnetometer, no continuous read mode!
//var w = WaitMs(20);
_mpu9150.Write(INT_PIN_CFG, 0x02);
var w = WaitMs(10);
_ak8975A.Write(0x0A, 0x01);
w = WaitMs(10);
//var rb = ReadByte(_ak8975A, AK8975A_ST1);
//Only accept a new magnetometer data read if the data ready bit is set and
// if there are no sensor overflow or data read errors
//if (ReadByte(_ak8975A, AK8975A_ST1) & 0x01) > 0)) //So the return for read byte should be a byte?
//{ // wait for magnetometer data ready bit to be set
_ak8975A.Read(6, AK8975A_XOUT_L, out rawData);
// Read the six raw data registers sequentially into data array
var magData = new short[3];
magData[0] = (short)((rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value
magData[1] = (short)((rawData[3] << 8) | rawData[2]);
magData[2] = (short)((rawData[5] << 8) | rawData[4]);
//}
return(magData);
}
internal double SetDesiredFrequency(double frequency)
{
frequency = Math.Min(frequency, _maxFrequency);
frequency = Math.Max(frequency, _minFrequency);
frequency *= 0.9f; // Correct for overshoot in the frequency setting (see issue #11).
var prescaleval = 25000000d;
prescaleval /= 4096;
prescaleval /= frequency;
prescaleval -= 1;
var prescale = (byte)Math.Floor(prescaleval + 0.5f);
byte[] readBuffer;
_pca9685.Read(1, (byte)Registers.MODE1, out readBuffer);
var oldmode = readBuffer[0];
var newmode = (byte)((oldmode & 0x7F) | 0x10); //sleep
_pca9685.Write((byte)Registers.MODE1, newmode);
_pca9685.Write((byte)Registers.PRESCALE, prescale);
_pca9685.Write((byte)Registers.MODE1, oldmode);
Task.Delay(5).Wait();
_pca9685.Write((byte)Registers.MODE1, (byte)(oldmode | 0xa1));
_actualFrequency = frequency;
return(_actualFrequency);
}
/// <summary>
/// Testing detecting vibration when a foot contacts an object.
/// </summary>
/// <param name="channel"></param>
internal void Start(int channel)
{
Task.Factory.StartNew(() =>
{
var avg = new double[10];
int i = 0;
while (true)
{
var config = Enums.Ads1115.Config.CQUE_NONE |
Enums.Ads1115.Config.CLAT_NONLAT |
Enums.Ads1115.Config.CPOL_ACTVLOW |
Enums.Ads1115.Config.CMODE_TRAD |
Enums.Ads1115.Config.DR_128SPS |
Enums.Ads1115.Config.MODE_SINGLE;
var newconfig = ((uint)config) | ((uint)Enums.Ads1115.Gain.Sixteen) | ((uint)Enums.Ads1115.Config.MUX_SINGLE_0) | ((uint)Enums.Ads1115.Config.OS_SINGLE);
var r = _ads1115.Write(new byte[] { 0x01, (byte)(newconfig >> 8), (byte)(newconfig & 0xff) });
_stopwatch.Restart();
while (_stopwatch.ElapsedMilliseconds < 4)
{
}
r = _ads1115.Write(new byte[] { 0x00, 0x00 });
byte[] toRead;
r = _ads1115.Read(2, out toRead);
var val = BitConverter.ToUInt16(toRead, 0);
if (val == 256)
{
val = 0;
}
avg[i] = val;
if (i == 9)
{
i = 0;
var outVal = avg.Sum() / 10;
if (outVal > 30000) //When a foot hits the floor, you get some value above this.
{
Debug.WriteLine("Value " + outVal);
}
continue;
}
i++;
}
}, TaskCreationOptions.LongRunning);
}
/// <summary>
/// return is ax, ay, az
/// </summary>
/// <returns></returns>
internal double[] ReadAccelData()
{
byte[] rawData = { 0, 0, 0, 0, 0, 0 }; // x/y/z accel register data stored here
_mpu9150.Read(6, ACCEL_XOUT_H, out rawData); // Read the six raw data registers into data array
if (rawData.Length == 1)
{
return(new double[2]);
}
var axAyAz = new double[3];
axAyAz[0] = Math.Round((((rawData[0] << 8) | rawData[1])) * GetAres, 2);
// Turn the MSB and LSB into a signed 16-bit value
axAyAz[1] = Math.Round((((rawData[2] << 8) | rawData[3])) * GetAres, 2);
axAyAz[2] = Math.Round((((rawData[4] << 8) | rawData[5])) * GetAres, 2);
return(axAyAz);
}
private int GetFifoCount(I2CDevice device)
{
if (!device.Write(new[] { FIFO_COUNTH }))
{
return(0);
}
byte[] buffer;
var r = device.Read(2, out buffer);
if (r)
{
return((buffer[0] << 8) | buffer[1]); //Get byte count
}
return(0);
}
internal bool WriteBit(I2CDevice device, byte regAddr, byte bitNum, byte data)
{
byte[] b;
device.Write(new[] { regAddr });
device.Read(1, out b);
if (data != 0)
{
b[0] = (byte)(1 << bitNum);
}
else
{
b[0] = (byte)(b[0] & (byte)(~(1 << bitNum)));
}
return(device.Write(new[] { regAddr, b[0] }));
}
private void Pin_ValueChanged(GpioPin sender, GpioPinValueChangedEventArgs args)
{
byte[] r;
if (!_i2CDevice.Read(2, out r))
{
return;
}
var touched = BitConverter.ToUInt16(r, 0);
for (var i = 0; i <= 15; i++)
{
if ((touched & (1 << i)) != 0)
{
Debug.WriteLine("Touched " + i);
}
}
}
internal bool WriteBits(I2CDevice device, byte regAddr, byte bitStart, byte length, byte data)
{
// 010 value to write
// 76543210 bit numbers
// xxx args: bitStart=4, length=3
// 00011100 mask byte
// 10101111 original value (sample)
// 10100011 original & ~mask
// 10101011 masked | value
byte[] b;
device.Write(new[] { regAddr });
if (device.Read(regAddr, out b))
{
var mask = (byte)(((1 << length) - 1) << (bitStart - length + 1));
data <<= (bitStart - length + 1); // shift data into correct position
data &= mask; // zero all non-important bits in data
b[0] &= (byte)(~(mask)); // zero all important bits in existing byte
b[0] |= data; // combine data with existing byte
return(device.Write(new[] { regAddr, b[0] }));
}
return(false);
}
private async void Interrupt(GpioPin sender, GpioPinValueChangedEventArgs args)
{
await Task.Delay(10);
if (_mpu9150 == null)
{
return;
}
int interruptStatus = _mpu9150.ReadRegisterSingle((byte)Mpu9150Setup.InterruptStatus);
if ((interruptStatus & 0x10) != 0)
{
_mpu9150.Write((byte)Mpu9150Setup.UserCtrl, 0x44);// reset - enable fifo
}
if ((interruptStatus & 0x1) == 0)
{
return;
}
var ea = new MpuSensorEventArgs();
ea.Status = (byte)interruptStatus;
ea.SamplePeriod = 0.02f;
var l = new List <MpuSensorValue>();
int count = _mpu9150.ReadUshort((byte)Mpu9150Setup.FifoCount);
while (count >= SensorBytes)
{
_mpu9150.Write((byte)Mpu9150Setup.FifoReadWrite);
byte[] buffer;
_mpu9150.Read(SensorBytes, out buffer);
count -= SensorBytes;
var xa = (short)(buffer[0] << 8 | buffer[1]);
var ya = (short)(buffer[2] << 8 | buffer[3]);
var za = (short)(buffer[4] << 8 | buffer[5]);
var xg = (short)(buffer[6] << 8 | buffer[7]);
var yg = (short)(buffer[8] << 8 | buffer[9]);
var zg = (short)(buffer[10] << 8 | buffer[11]);
var sv = new MpuSensorValue
{
AccelerationX = xa / 16384d,
AccelerationY = ya / 16384d,
AccelerationZ = za / 16384d,
GyroX = xg / 131d,
GyroY = yg / 131d,
GyroZ = zg / 131d
};
l.Add(sv);
var gain = 0.00875;
var xRotationPerSecond = sv.GyroX * gain; //xRotationPerSecond is the rate of rotation per second.
var loopPeriod = 0.015; //loop period - 0.02
_gyroXangle += xRotationPerSecond * loopPeriod;
var radToDeg = 57.29578;
var accXangle = (Math.Atan2(sv.AccelerationY, sv.AccelerationZ) + Math.PI) * radToDeg;
var complementaryFilterConstant = 0.98;
_cFangleX = complementaryFilterConstant * (_cFangleX + xRotationPerSecond * loopPeriod) + (1 - complementaryFilterConstant) * accXangle;
//Debug.WriteLine("X: " + sv.GyroX + ", Y: " + sv.GyroY + ", Z: " + sv.GyroZ);
//Debug.WriteLine("CFangleX: " + _cFangleX);
//Debug.WriteLine("AccelX: " + sv.AccelerationX + ", AccelY: " + sv.AccelerationY + ", AccelZ: " + sv.AccelerationZ);
}
ea.Values = l.ToArray();
if (SensorInterruptEvent == null)
{
return;
}
if (ea.Values.Length > 0)
{
SensorInterruptEvent(this, ea);
}
}
private int GetFifoCount(I2CDevice device)
{
if (!device.Write(new[] {FIFO_COUNTH}))
return 0;
byte[] buffer;
var r = device.Read(2, out buffer);
if (r)
return (buffer[0] << 8) | buffer[1]; //Get byte count
return 0;
}
internal bool WriteBits(I2CDevice device, byte regAddr, byte bitStart, byte length, byte data)
{
// 010 value to write
// 76543210 bit numbers
// xxx args: bitStart=4, length=3
// 00011100 mask byte
// 10101111 original value (sample)
// 10100011 original & ~mask
// 10101011 masked | value
byte[] b;
device.Write(new[] { regAddr });
if (device.Read(regAddr, out b))
{
var mask = (byte)(((1 << length) - 1) << (bitStart - length + 1));
data <<= (bitStart - length + 1); // shift data into correct position
data &= mask; // zero all non-important bits in data
b[0] &= (byte)(~(mask)); // zero all important bits in existing byte
b[0] |= data; // combine data with existing byte
return device.Write(new[] { regAddr, b[0] });
}
return false;
}
internal bool WriteBit(I2CDevice device, byte regAddr, byte bitNum, byte data)
{
byte[] b;
device.Write(new[] { regAddr });
device.Read(1, out b);
if (data != 0)
{
b[0] = (byte)(1 << bitNum);
}
else
{
b[0] = (byte)(b[0] & (byte)(~(1 << bitNum)));
}
return device.Write(new[] { regAddr, b[0] });
}
