Multi sensor fix (#887)
* more logging * set uart mode all at once * updated logging * pr feedback
This commit is contained in:
Peli de Halleux
GitHub
parent
85cfc86bf8
commit
abc93dd7da
@ -71,20 +71,18 @@ namespace sensors.internal {
|
||||
IICMM = control.mmap("/dev/lms_iic", IICOff.Size, 0)
|
||||
if (!IICMM) control.fail("no iic sensor")
|
||||
|
||||
forever(() => {
|
||||
detectDevices()
|
||||
pause(500)
|
||||
})
|
||||
|
||||
for (let info_ of sensorInfos) {
|
||||
let info = info_
|
||||
unsafePollForChanges(500,
|
||||
() => { return hashDevices(); },
|
||||
(prev, curr) => { detectDevices();
|
||||
});
|
||||
sensorInfos.forEach(info => {
|
||||
unsafePollForChanges(50, () => {
|
||||
if (info.sensor) return info.sensor._query()
|
||||
return 0
|
||||
}, (prev, curr) => {
|
||||
if (info.sensor) info.sensor._update(prev, curr)
|
||||
})
|
||||
}
|
||||
})
|
||||
|
||||
}
|
||||
|
||||
@ -104,12 +102,12 @@ namespace sensors.internal {
|
||||
}
|
||||
|
||||
export function readIICID(port: number) {
|
||||
let buf = output.createBuffer(IICStr.Size)
|
||||
const buf = output.createBuffer(IICStr.Size)
|
||||
buf[IICStr.Port] = port
|
||||
IICMM.ioctl(IO.IIC_READ_TYPE_INFO, buf)
|
||||
let Manufacturer = bufferToString(buf.slice(IICStr.Manufacturer, 8))
|
||||
let SensorType = bufferToString(buf.slice(IICStr.SensorType, 8))
|
||||
return Manufacturer + SensorType ;
|
||||
const manufacturer = bufferToString(buf.slice(IICStr.Manufacturer, 8))
|
||||
const sensorType = bufferToString(buf.slice(IICStr.SensorType, 8))
|
||||
return manufacturer + sensorType;
|
||||
}
|
||||
|
||||
export function getBatteryInfo(): { temp: number; current: number } {
|
||||
@ -120,65 +118,87 @@ namespace sensors.internal {
|
||||
}
|
||||
}
|
||||
|
||||
function hashDevices(): number {
|
||||
const conns = analogMM.slice(AnalogOff.InConn, DAL.NUM_INPUTS)
|
||||
let r = 0;
|
||||
for(let i = 0; i < conns.length; ++i) {
|
||||
r = (r << 8 | conns[i]);
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
let nonActivated = 0;
|
||||
function detectDevices() {
|
||||
let conns = analogMM.slice(AnalogOff.InConn, DAL.NUM_INPUTS)
|
||||
let numChanged = 0
|
||||
control.dmesg(`detect devices (${nonActivated} na)`)
|
||||
const conns = analogMM.slice(AnalogOff.InConn, DAL.NUM_INPUTS)
|
||||
let numChanged = 0;
|
||||
const uartSensors: SensorInfo[] = [];
|
||||
|
||||
for (let info of sensorInfos) {
|
||||
let newConn = conns[info.port]
|
||||
if (newConn == info.connType)
|
||||
continue
|
||||
for (const sensorInfo of sensorInfos) {
|
||||
const newConn = conns[sensorInfo.port]
|
||||
if (newConn == sensorInfo.connType) {
|
||||
// control.dmesg(`connection unchanged ${newConn} at ${sensorInfo.port}`)
|
||||
continue;
|
||||
}
|
||||
numChanged++
|
||||
info.connType = newConn
|
||||
info.devType = DAL.DEVICE_TYPE_NONE
|
||||
sensorInfo.connType = newConn
|
||||
sensorInfo.devType = DAL.DEVICE_TYPE_NONE
|
||||
if (newConn == DAL.CONN_INPUT_UART) {
|
||||
control.dmesg(`new UART connection at ${info.port}`)
|
||||
setUartMode(info.port, 0)
|
||||
let uinfo = readUartInfo(info.port, 0)
|
||||
info.devType = uinfo[TypesOff.Type]
|
||||
control.dmesg(`UART type ${info.devType}`)
|
||||
control.dmesg(`new UART connection at ${sensorInfo.port}`)
|
||||
updateUartMode(sensorInfo.port, 0);
|
||||
uartSensors.push(sensorInfo);
|
||||
} else if (newConn == DAL.CONN_NXT_IIC) {
|
||||
control.dmesg(`new IIC connection at ${info.port}`)
|
||||
info.devType = DAL.DEVICE_TYPE_IIC_UNKNOWN
|
||||
info.iicid = readIICID(info.port)
|
||||
control.dmesg(`IIC ID ${info.iicid.length}`)
|
||||
control.dmesg(`new IIC connection at ${sensorInfo.port}`)
|
||||
sensorInfo.devType = DAL.DEVICE_TYPE_IIC_UNKNOWN
|
||||
sensorInfo.iicid = readIICID(sensorInfo.port)
|
||||
control.dmesg(`IIC ID ${sensorInfo.iicid.length}`)
|
||||
} else if (newConn == DAL.CONN_INPUT_DUMB) {
|
||||
control.dmesg(`new DUMB connection at ${info.port}`)
|
||||
control.dmesg(`new DUMB connection at ${sensorInfo.port}`)
|
||||
// TODO? for now assume touch
|
||||
info.devType = DAL.DEVICE_TYPE_TOUCH
|
||||
sensorInfo.devType = DAL.DEVICE_TYPE_TOUCH
|
||||
} else if (newConn == DAL.CONN_NONE || newConn == 0) {
|
||||
control.dmesg(`disconnect at ${info.port}`)
|
||||
control.dmesg(`disconnect at port ${sensorInfo.port}`)
|
||||
} else {
|
||||
control.dmesg(`unknown connection type: ${newConn} at ${info.port}`)
|
||||
control.dmesg(`unknown connection type: ${newConn} at ${sensorInfo.port}`)
|
||||
}
|
||||
}
|
||||
|
||||
if (uartSensors.length > 0) {
|
||||
setUartModes();
|
||||
for (const sensorInfo of uartSensors) {
|
||||
let uinfo = readUartInfo(sensorInfo.port, 0)
|
||||
sensorInfo.devType = uinfo[TypesOff.Type]
|
||||
control.dmesg(`UART type ${sensorInfo.devType}`)
|
||||
}
|
||||
}
|
||||
|
||||
if (numChanged == 0 && nonActivated == 0)
|
||||
return
|
||||
|
||||
control.dmesg(`updating sensor status`)
|
||||
nonActivated = 0;
|
||||
for (let si of sensorInfos) {
|
||||
if(si.devType == DAL.DEVICE_TYPE_IIC_UNKNOWN){
|
||||
si.sensor = si.sensors.filter(s => s._IICId() == si.iicid)[0]
|
||||
if (!si.sensor) {
|
||||
control.dmesg(`sensor not found for iicid=${si.iicid} at ${si.port}`)
|
||||
for (const sensorInfo of sensorInfos) {
|
||||
if (sensorInfo.devType == DAL.DEVICE_TYPE_IIC_UNKNOWN) {
|
||||
sensorInfo.sensor = sensorInfo.sensors.filter(s => s._IICId() == sensorInfo.iicid)[0]
|
||||
if (!sensorInfo.sensor) {
|
||||
control.dmesg(`sensor not found for iicid=${sensorInfo.iicid} at ${sensorInfo.port}`)
|
||||
nonActivated++;
|
||||
} else {
|
||||
control.dmesg(`sensor connected iicid=${si.iicid} at ${si.port}`)
|
||||
si.sensor._activated()
|
||||
control.dmesg(`sensor connected iicid=${sensorInfo.iicid} at ${sensorInfo.port}`)
|
||||
sensorInfo.sensor._activated()
|
||||
}
|
||||
}else if (si.devType != DAL.DEVICE_TYPE_NONE) {
|
||||
si.sensor = si.sensors.filter(s => s._deviceType() == si.devType)[0]
|
||||
if (!si.sensor) {
|
||||
control.dmesg(`sensor not found for type=${si.devType} at ${si.port}`)
|
||||
} else if (sensorInfo.devType != DAL.DEVICE_TYPE_NONE) {
|
||||
sensorInfo.sensor = sensorInfo.sensors.filter(s => s._deviceType() == sensorInfo.devType)[0]
|
||||
if (!sensorInfo.sensor) {
|
||||
control.dmesg(`sensor not found for type=${sensorInfo.devType} at ${sensorInfo.port}`)
|
||||
nonActivated++;
|
||||
} else {
|
||||
control.dmesg(`sensor connected type=${si.devType} at ${si.port}`)
|
||||
si.sensor._activated()
|
||||
control.dmesg(`sensor connected type=${sensorInfo.devType} at ${sensorInfo.port}`)
|
||||
sensorInfo.sensor._activated()
|
||||
}
|
||||
}
|
||||
}
|
||||
control.dmesg(`detect devices done`)
|
||||
}
|
||||
|
||||
export class Sensor extends control.Component {
|
||||
@ -355,6 +375,7 @@ namespace sensors.internal {
|
||||
}
|
||||
|
||||
function uartClearChange(port: number) {
|
||||
control.dmesg(`UART clear change`);
|
||||
const UART_DATA_READY = 8
|
||||
const UART_PORT_CHANGED = 1
|
||||
while (true) {
|
||||
@ -376,20 +397,43 @@ namespace sensors.internal {
|
||||
}
|
||||
}
|
||||
|
||||
function setUartMode(port: number, mode: number) {
|
||||
const UART_PORT_CHANGED = 1
|
||||
while (true) {
|
||||
if (port < 0) return
|
||||
function setUartModes() {
|
||||
control.dmesg(`UART set modes`)
|
||||
uartMM.ioctl(IO.UART_SET_CONN, devcon)
|
||||
const ports: number[] = [];
|
||||
for (let port = 0; port < DAL.NUM_INPUTS; ++port) {
|
||||
if (devcon.getNumber(NumberFormat.Int8LE, DevConOff.Connection + port) == DAL.CONN_INPUT_UART) {
|
||||
ports.push(port);
|
||||
}
|
||||
}
|
||||
|
||||
while (ports.length) {
|
||||
const port = ports.pop();
|
||||
const status = waitNonZeroUartStatus(port)
|
||||
control.dmesg(`UART set mode ${status} at ${port}`);
|
||||
}
|
||||
}
|
||||
|
||||
function updateUartMode(port: number, mode: number) {
|
||||
control.dmesg(`UART set mode to ${mode} at ${port}`)
|
||||
devcon.setNumber(NumberFormat.Int8LE, DevConOff.Connection + port, DAL.CONN_INPUT_UART)
|
||||
devcon.setNumber(NumberFormat.Int8LE, DevConOff.Type + port, 33)
|
||||
devcon.setNumber(NumberFormat.Int8LE, DevConOff.Mode + port, mode)
|
||||
}
|
||||
|
||||
function setUartMode(port: number, mode: number) {
|
||||
const UART_PORT_CHANGED = 1
|
||||
while (true) {
|
||||
if (port < 0) return
|
||||
updateUartMode(port, mode);
|
||||
uartMM.ioctl(IO.UART_SET_CONN, devcon)
|
||||
let status = waitNonZeroUartStatus(port)
|
||||
if (status & UART_PORT_CHANGED) {
|
||||
control.dmesg(`UART clear changed at ${port}`)
|
||||
uartClearChange(port)
|
||||
} else {
|
||||
break
|
||||
control.dmesg(`UART status ${status}`);
|
||||
break;
|
||||
}
|
||||
pause(10)
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user