From 7fb244e9ae5c5625e1db4e4c0ac9554dd723e622 Mon Sep 17 00:00:00 2001 From: techyian Date: Mon, 10 Feb 2020 20:21:33 +0000 Subject: [PATCH] #47. Probing sensor now working. --- src/MMALSharp/Config/ModeDefs/ModeDef.cs | 4 +- src/MMALSharp/Config/SensorDefs/SensorDef.cs | 18 +-- src/MMALSharp/MMALRawcam.cs | 161 +++++++++++-------- src/MMALSharp/Native/MMALI2C.cs | 32 ++-- 4 files changed, 123 insertions(+), 92 deletions(-) diff --git a/src/MMALSharp/Config/ModeDefs/ModeDef.cs b/src/MMALSharp/Config/ModeDefs/ModeDef.cs index 6468030d..926237c8 100644 --- a/src/MMALSharp/Config/ModeDefs/ModeDef.cs +++ b/src/MMALSharp/Config/ModeDefs/ModeDef.cs @@ -12,8 +12,8 @@ public class ModeDef public int Encoding { get; set; } public BayerOrder Order { get; set; } public int NativeBitDepth { get; set; } - public int ImageId { get; set; } - public int DataLanes { get; set; } + public byte ImageId { get; set; } + public byte DataLanes { get; set; } public int MinVts { get; set; } public int LineTimeNs { get; set; } public int[] Timing { get; set; } diff --git a/src/MMALSharp/Config/SensorDefs/SensorDef.cs b/src/MMALSharp/Config/SensorDefs/SensorDef.cs index 68cdcfb2..a34a8df1 100644 --- a/src/MMALSharp/Config/SensorDefs/SensorDef.cs +++ b/src/MMALSharp/Config/SensorDefs/SensorDef.cs @@ -11,27 +11,27 @@ public class SensorDef public List StopReg { get; set; } public int NumStopRegs { get; set; } - public int I2CAddr { get; set; } + public byte I2CAddr { get; set; } public int I2CAddressing { get; set; } public int I2CDataSize { get; set; } - public int I2CIdentLength { get; set; } - public int I2CIdentReg { get; set; } - public int I2CIdentValue { get; set; } + public ushort I2CIdentLength { get; set; } + public ushort I2CIdentReg { get; set; } + public ushort I2CIdentValue { get; set; } - public int VFlipReg { get; set; } + public ushort VFlipReg { get; set; } public int VFlipRegBit { get; set; } - public int HFlipReg { get; set; } + public ushort HFlipReg { get; set; } public int HFlipRegBit { get; set; } public int FlipsDontChangeBayerOrder { get; set; } - public int ExposureReg { get; set; } + public ushort ExposureReg { get; set; } public int ExposureRegNumBits { get; set; } - public int VtsReg { get; set; } + public ushort VtsReg { get; set; } public int VtsRegNumBits { get; set; } - public int GainReg { get; set; } + public ushort GainReg { get; set; } public int GainRegNumBits { get; set; } } } diff --git a/src/MMALSharp/MMALRawcam.cs b/src/MMALSharp/MMALRawcam.cs index 24f294f8..d3d21358 100644 --- a/src/MMALSharp/MMALRawcam.cs +++ b/src/MMALSharp/MMALRawcam.cs @@ -19,9 +19,10 @@ public sealed class MMALRawcam private const int OPEN_READ_WRITE = 2; private const int I2C_M_RD = 0x0001; + private const int I2C_SLAVE = 0x0703; private const int I2C_SLAVE_FORCE = 0x0706; - private const int I2C_RDWR = 0x0707; - + private const int I2C_RDWR = 0x0707; + /// /// Gets the singleton instance of the MMAL Raw Camera. Call to initialise the camera for first use. /// @@ -54,6 +55,8 @@ private static List SensorDefs private ModeDef ModeDef { get; set; } + private int Mode { get; set; } + private string I2CDeviceName { get; set; } private MMALRawcam() @@ -68,16 +71,14 @@ private MMALRawcam() /// /// The component reference. /// The component reference. - /// The sensor definition config object. - /// The mode definition config object. + /// The selected sensor mode. /// The I2C device name. - public void ConfigureRawcamPipeline(MMALRawcamComponent rawcamComponent, MMALIspComponent ispComponent, SensorDef sensorDef, ModeDef modeDef, string i2cDeviceName) + public void ConfigureRawcamPipeline(MMALRawcamComponent rawcamComponent, MMALIspComponent ispComponent, int mode, string i2cDeviceName) { this.RawcamComponent = rawcamComponent; this.IspComponent = ispComponent; - this.SensorDef = sensorDef; - this.ModeDef = modeDef; this.I2CDeviceName = i2cDeviceName; + this.Mode = mode; } /// @@ -90,7 +91,7 @@ public void ConfigureRawcamPipeline(MMALRawcamComponent rawcamComponent, MMALIsp { var tasks = new List(); - MMALLog.Logger.LogInformation("Attemping to enable rawcam components..."); + MMALLog.Logger.LogInformation("Attempting to enable rawcam components..."); var sensor = this.ProbeSensor(this.I2CDeviceName); @@ -109,15 +110,15 @@ public void ConfigureRawcamPipeline(MMALRawcamComponent rawcamComponent, MMALIsp tasks.Add(this.IspComponent.Outputs[0].Trigger.Task); tasks.Add(this.RawcamComponent.Outputs[0].Trigger.Task); - MMALLog.Logger.LogDebug("Attemping to start rawcam streaming..."); + MMALLog.Logger.LogDebug("Attempting to start rawcam streaming..."); - await this.StartCapture(this.SensorDef, this.ModeDef, this.I2CDeviceName); + await this.StartCapture(sensor, sensor.Modes[this.Mode], this.I2CDeviceName); if (cancellationToken == CancellationToken.None) { await Task.WhenAll(tasks).ConfigureAwait(false); - await this.StopCapture(this.SensorDef, this.I2CDeviceName); + await this.StopCapture(sensor, this.I2CDeviceName); } else { @@ -126,7 +127,7 @@ public void ConfigureRawcamPipeline(MMALRawcamComponent rawcamComponent, MMALIsp this.IspComponent.ForceStopProcessing = true; this.RawcamComponent.ForceStopProcessing = true; - await this.StopCapture(this.SensorDef, this.I2CDeviceName); + await this.StopCapture(sensor, this.I2CDeviceName); await Task.WhenAll(tasks).ConfigureAwait(false); } @@ -179,11 +180,19 @@ private async Task StartCapture(SensorDef sensorDef, ModeDef modeDef, string i2c throw new MMALIOException("Couldn't open I2C device."); } - if (MMALI2C.Ioctl(fd, I2C_SLAVE_FORCE, (IntPtr)sensorDef.I2CAddr) < 0) + var addrArr = new byte[1] { sensorDef.I2CAddr }; + var ptr = Marshal.AllocHGlobal(addrArr.Length); + + Marshal.Copy(addrArr, 0, ptr, addrArr.Length); + + if (MMALI2C.Ioctl(fd, I2C_SLAVE_FORCE, ptr) < 0) { + Marshal.FreeHGlobal(ptr); throw new MMALIOException("Failed to set I2C address."); } + Marshal.FreeHGlobal(ptr); + await this.SendRegs(fd, sensorDef, modeDef.Regs); MMALI2C.Close(fd); MMALLog.Logger.LogInformation("Now streaming..."); @@ -205,11 +214,19 @@ private async Task StopCapture(SensorDef sensorDef, string i2cDeviceName) throw new MMALIOException("Couldn't open I2C device."); } - if (MMALI2C.Ioctl(fd, I2C_SLAVE_FORCE, (IntPtr)sensorDef.I2CAddr) < 0) + var addrArr = new byte[1] { sensorDef.I2CAddr }; + var ptr = Marshal.AllocHGlobal(addrArr.Length); + + Marshal.Copy(addrArr, 0, ptr, addrArr.Length); + + if (MMALI2C.Ioctl(fd, I2C_SLAVE_FORCE, ptr) < 0) { + Marshal.FreeHGlobal(ptr); throw new MMALIOException("Failed to set I2C address."); } + Marshal.FreeHGlobal(ptr); + await this.SendRegs(fd, sensorDef, sensorDef.StopReg); MMALI2C.Close(fd); MMALLog.Logger.LogInformation("Stop streaming..."); @@ -224,102 +241,116 @@ private SensorDef ProbeSensor(string i2cDeviceName) { var fd = MMALI2C.Open(i2cDeviceName, OPEN_READ_WRITE); + var err = Marshal.GetLastWin32Error(); + if (fd == 0) { throw new MMALIOException("Couldn't open I2C device."); } + MMALLog.Logger.LogInformation($"I2C Device Name: {i2cDeviceName} | Fd err: {err}"); + foreach (var sensorDef in SensorDefs) { - MMALLog.Logger.LogDebug($"Probing sensor {sensorDef.Name} on addr {sensorDef.I2CAddr}"); + MMALLog.Logger.LogInformation($"Probing sensor {sensorDef.Name} on addr {sensorDef.I2CAddr}"); if (sensorDef.I2CIdentLength <= 2) { - var rd = this.I2CRead(fd, sensorDef.I2CAddr, sensorDef.I2CIdentReg, sensorDef.I2CIdentLength, sensorDef.I2CAddressing); - - if (rd != null && rd[0] == sensorDef.I2CIdentValue) + var rd = this.I2CRead(fd, sensorDef.I2CAddr, sensorDef.I2CIdentReg, sensorDef.I2CIdentLength, sensorDef.I2CAddressing, sensorDef.I2CIdentValue); + + if (rd != null) { + var i2cIdentValueReturned = BitConverter.ToUInt16(rd, 0); + + if (i2cIdentValueReturned != sensorDef.I2CIdentValue) + { + MMALLog.Logger.LogInformation($"Sensor probe successful but returned incorrect I2CIdentValue."); + return null; + } + MMALLog.Logger.LogInformation($"Found sensor {sensorDef.Name} at address {sensorDef.I2CAddr}."); return sensorDef; } - else - { - MMALLog.Logger.LogInformation($"Unable to probe sensor {sensorDef.Name} at address {sensorDef.I2CAddr}."); - } + + MMALLog.Logger.LogInformation($"Unable to probe sensor {sensorDef.Name} at address {sensorDef.I2CAddr}."); } } return null; } - private unsafe byte[] I2CRead(int fd, int i2cAddr, int reg, int n, int addressing) + private byte[] I2CRead(int fd, byte i2cAddr, int reg, ushort n, int addressing, ushort i2cIdentValue) { - int len = 0; - - Console.WriteLine($"I2C Addr: {i2cAddr}"); - Console.WriteLine($"Reg: {reg}"); - Console.WriteLine($"N: {n}"); - Console.WriteLine($"Addressing: {addressing}"); + MMALLog.Logger.LogInformation($"I2C Addr: {i2cAddr} | Reg: {reg} | Addressing: {addressing} | Fd: {fd}"); + var len = addressing == 1 ? 1 : 2; var buf = new byte[2] { (byte)(reg >> 8), (byte)(reg & 0xff) }; + var err = 0; + var win32Err = 0; - Console.WriteLine($"Buffer value 1 {reg >> 8}. Buffer value 2 {reg & 0xff}"); - - var ptr1 = Marshal.AllocHGlobal(2); + var ptr1 = Marshal.AllocHGlobal(len); var ptr2 = Marshal.AllocHGlobal(n); - Marshal.Copy(buf, 0, ptr1, 2); - - if (addressing == 1) - { - len = 1; - } - else - { - len = n; - } - - var msg1 = new I2CMsg(i2cAddr, 0, 2, ptr1); - var msg2 = new I2CMsg(i2cAddr, I2C_M_RD, len, ptr2); + Marshal.Copy(buf, 0, ptr1, buf.Length); - var msgArr = new I2CMsg[2] { msg1, msg2 }; - var err = 0; - var win32Err = 0; - - fixed (I2CMsg* pArray = msgArr) - { - IntPtr msgs = new IntPtr((void*)pArray); + var msg1 = new I2CMsg(i2cAddr, 0, (ushort)len, ptr1); + var msg2 = new I2CMsg(i2cAddr, I2C_M_RD, n, ptr2); - var msgSet = new I2CRdwrIoctlData(msgs, 2); + var msgPtr1 = Marshal.AllocHGlobal(Marshal.SizeOf() + len); + var msgPtr2 = Marshal.AllocHGlobal(Marshal.SizeOf() + n); - var msgSetPtr = Marshal.AllocHGlobal(Marshal.SizeOf(typeof(I2CRdwrIoctlData))); - Marshal.StructureToPtr(msgSet, msgSetPtr, true); + Marshal.StructureToPtr(msg1, msgPtr1, false); + Marshal.StructureToPtr(msg2, msgPtr2, false); - Console.WriteLine($"Fd: {fd}"); + var msgSet1 = new I2CRdwrIoctlData(msgPtr1, 1); + var msgSet2 = new I2CRdwrIoctlData(msgPtr2, 1); - err = MMALI2C.Ioctl(fd, I2C_RDWR, msgSetPtr); + var msgSetPtr1 = Marshal.AllocHGlobal(Marshal.SizeOf() + len); + var msgSetPtr2 = Marshal.AllocHGlobal(Marshal.SizeOf() + n); - win32Err = Marshal.GetLastWin32Error(); + Marshal.StructureToPtr(msgSet1, msgSetPtr1, false); + Marshal.StructureToPtr(msgSet2, msgSetPtr2, false); - Marshal.FreeHGlobal(msgSetPtr); - } + err = MMALI2C.Ioctl(fd, I2C_RDWR, msgSetPtr1); + + win32Err = Marshal.GetLastWin32Error(); + Console.WriteLine($"Read I2C err value: {err} | win32Err: {win32Err}"); + + err = MMALI2C.Ioctl(fd, I2C_RDWR, msgSetPtr2); + + win32Err = Marshal.GetLastWin32Error(); + Console.WriteLine($"Write I2C err value: {err} | win32Err: {win32Err}"); var arr = new byte[n]; Marshal.Copy(ptr2, arr, 0, n); + Marshal.DestroyStructure(msgSetPtr1, typeof(I2CRdwrIoctlData)); + Marshal.FreeHGlobal(msgSetPtr1); + + Marshal.DestroyStructure(msgSetPtr2, typeof(I2CRdwrIoctlData)); + Marshal.FreeHGlobal(msgSetPtr2); + + Marshal.DestroyStructure(msgPtr1, typeof(I2CMsg)); + Marshal.FreeHGlobal(msgPtr1); + + Marshal.DestroyStructure(msgPtr2, typeof(I2CMsg)); + Marshal.FreeHGlobal(msgPtr2); + Marshal.FreeHGlobal(ptr1); Marshal.FreeHGlobal(ptr2); - Console.WriteLine($"Return value: {err}"); - - if (err != 2) + if (err != 1) { - MMALLog.Logger.LogWarning($"Unable to read from I2C. Error {win32Err}."); + MMALLog.Logger.LogWarning($"Unable to probe sensor on address {i2cAddr}."); return null; } + var i2cIdentValueReturned = BitConverter.ToUInt16(arr, 0); + + MMALLog.Logger.LogInformation($"Probe successful on address {i2cAddr}. Sensor I2C Ident Value: {i2cIdentValue}. I2C Ident Value Returned: {i2cIdentValueReturned}."); + return arr; } @@ -343,7 +374,7 @@ private async Task SendRegs(int fd, SensorDef sensorDef, List sensorR { // Sleep...? MMALLog.Logger.LogDebug($"Delaying for {sensorRegs[i].Data}ms."); - await Task.Delay(sensorRegs[i].Data); + await Task.Delay(sensorRegs[i].Data).ConfigureAwait(false); } else { diff --git a/src/MMALSharp/Native/MMALI2C.cs b/src/MMALSharp/Native/MMALI2C.cs index 99d2a6a2..870c5139 100644 --- a/src/MMALSharp/Native/MMALI2C.cs +++ b/src/MMALSharp/Native/MMALI2C.cs @@ -7,33 +7,33 @@ namespace MMALSharp.Native #pragma warning disable SA1132 // Each field should be declared on its own line public class MMALI2C { - [DllImport("libc.so.6", EntryPoint = "open", SetLastError = true, CallingConvention = CallingConvention.Cdecl)] - public static extern int Open(string fileName, int mode); + [DllImport("libc.so.6", EntryPoint = "open", SetLastError = true)] + internal static extern int Open(string fileName, int mode); - [DllImport("libc.so.6", EntryPoint = "ioctl", SetLastError = true, CallingConvention = CallingConvention.Cdecl)] - public static extern int Ioctl(int fd, int request, IntPtr data); + [DllImport("libc.so.6", EntryPoint = "ioctl", SetLastError = true)] + internal static extern int Ioctl(int fd, int request, IntPtr data); - [DllImport("libc.so.6", EntryPoint = "read", SetLastError = true, CallingConvention = CallingConvention.Cdecl)] - public static extern int Read(int handle, IntPtr data, int length); + [DllImport("libc.so.6", EntryPoint = "read", SetLastError = true)] + internal static extern int Read(int handle, IntPtr data, int length); - [DllImport("libc.so.6", EntryPoint = "write", SetLastError = true, CallingConvention = CallingConvention.Cdecl)] - public static extern int Write(int handle, IntPtr data, int length); + [DllImport("libc.so.6", EntryPoint = "write", SetLastError = true)] + internal static extern int Write(int handle, IntPtr data, int length); - [DllImport("libc.so.6", EntryPoint = "close", SetLastError = true, CallingConvention = CallingConvention.Cdecl)] - public static extern int Close(int handle); + [DllImport("libc.so.6", EntryPoint = "close", SetLastError = true)] + internal static extern int Close(int handle); [StructLayout(LayoutKind.Sequential)] public struct I2CMsg { - private int _addr, _flags, _len; + private ushort _addr, _flags, _len; private IntPtr _buf; - - public int Addr => _addr; - public int Flags => _flags; - public int Len => _len; + + public ushort Addr => _addr; + public ushort Flags => _flags; + public ushort Len => _len; public IntPtr Buf => _buf; - public I2CMsg(int addr, int flags, int len, IntPtr buf) + public I2CMsg(byte addr, ushort flags, ushort len, IntPtr buf) { _addr = addr; _flags = flags;