Skip to content
This repository has been archived by the owner on Feb 22, 2024. It is now read-only.

Commit

Permalink
#47. Probing sensor now working.
Browse files Browse the repository at this point in the history
  • Loading branch information
@techyian
techyian committed Feb 10, 2020
1 parent 56bb0b0 commit 7fb244e
Show file tree
Hide file tree
Showing 4 changed files with 123 additions and 92 deletions.
4 changes: 2 additions & 2 deletions src/MMALSharp/Config/ModeDefs/ModeDef.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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; }
Expand Down
18 changes: 9 additions & 9 deletions src/MMALSharp/Config/SensorDefs/SensorDef.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,27 +11,27 @@ public class SensorDef
public List<SensorReg> 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; }
}
}
161 changes: 96 additions & 65 deletions src/MMALSharp/MMALRawcam.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
/// <summary>
/// Gets the singleton instance of the MMAL Raw Camera. Call to initialise the camera for first use.
/// </summary>
Expand Down Expand Up @@ -54,6 +55,8 @@ private static List<SensorDef> SensorDefs

private ModeDef ModeDef { get; set; }

private int Mode { get; set; }

private string I2CDeviceName { get; set; }

private MMALRawcam()
Expand All @@ -68,16 +71,14 @@ private MMALRawcam()
/// </summary>
/// <param name="rawcamComponent">The <see cref="MMALRawcamComponent"/> component reference.</param>
/// <param name="ispComponent">The <see cref="MMALIspComponent"/> component reference.</param>
/// <param name="sensorDef">The sensor definition config object.</param>
/// <param name="modeDef">The mode definition config object.</param>
/// <param name="mode">The selected sensor mode.</param>
/// <param name="i2cDeviceName">The I2C device name.</param>
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;
}

/// <summary>
Expand All @@ -90,7 +91,7 @@ public void ConfigureRawcamPipeline(MMALRawcamComponent rawcamComponent, MMALIsp
{
var tasks = new List<Task>();

MMALLog.Logger.LogInformation("Attemping to enable rawcam components...");
MMALLog.Logger.LogInformation("Attempting to enable rawcam components...");

var sensor = this.ProbeSensor(this.I2CDeviceName);

Expand All @@ -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
{
Expand All @@ -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);
}
Expand Down Expand Up @@ -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...");
Expand All @@ -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...");
Expand All @@ -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<I2CMsg>() + len);
var msgPtr2 = Marshal.AllocHGlobal(Marshal.SizeOf<I2CMsg>() + 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<I2CRdwrIoctlData>() + len);
var msgSetPtr2 = Marshal.AllocHGlobal(Marshal.SizeOf<I2CRdwrIoctlData>() + 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;
}

Expand All @@ -343,7 +374,7 @@ private async Task SendRegs(int fd, SensorDef sensorDef, List<SensorReg> 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
{
Expand Down
32 changes: 16 additions & 16 deletions src/MMALSharp/Native/MMALI2C.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down

1 comment on commit 7fb244e

@techyian
Copy link
Owner Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Tracked against #124.

Please sign in to comment.